public void CalculateAndExecuteBehaviors()
{
if (Movers.Count >= MinimumGroupSize)
{
for (i = 0; i < Movers.Count; i++)
{
Move mover = Movers [i];
GroupPosition += mover.Body.Position;
}
GroupPosition /= Movers.Count;
biggestSqrDistance = 0;
for (i = 0; i < Movers.Count; i++)
{
currentSqrDistance = Movers [i].Body.Position.SqrDistance(GroupPosition.x, GroupPosition.y);
if (currentSqrDistance > biggestSqrDistance)
{
currentDistance = FixedMath.Sqrt(currentSqrDistance);
/*
* DistDif = currentDistance - Radius;
* if (DistDif > MaximumDistDif * MoversCount / 128) {
* ExecuteGroupIndividualMove ();
* return;
* }*/
biggestSqrDistance = currentSqrDistance;
Radius = currentDistance;
}
}
if (GroupPosition.SqrDistance(Destination.x, Destination.y) < (biggestSqrDistance * 5 / 4))
{
ExecuteGroupIndividualMove();
return;
}
GroupDirection = Destination - GroupPosition;
for (i = 0; i < Movers.Count; i++)
{
Move mover = Movers [i];
mover.Destination = mover.Body.Position + GroupDirection;
mover.IsFormationMoving = true;
mover.closingDistanceMultiplier = FixedMath.One * 2 / 5;
mover.StartMove();
}
}
else
{
for (i = 0; i < Movers.Count; i++)
{
Move mover = Movers [i];
mover.closingDistanceMultiplier = FixedMath.One / 4;
mover.Destination = Destination;
mover.IsFormationMoving = false;
mover.StartMove();
}
}
}
protected override BehaviourNodeStatus OnExecute(AIAgent agent)
{
var speed = agent.SceneObject.GetAttributeValue(Logic.AttributeType.Speed);
long moveDistance = speed.Mul(FixedMath.One / 15);
while (moveDistance > 0 && _pathIndex < _path.Count)
{
long nextCornerDistance = Vector3d.SqrDistance(_path[_pathIndex],
agent.SceneObject.Position);
if (nextCornerDistance < moveDistance.Mul(moveDistance))
{
agent.SceneObject.Position = _path[_pathIndex];
moveDistance -= FixedMath.Sqrt(nextCornerDistance);
_pathIndex++;
if (_pathIndex == _path.Count)
{
break;
}
else
{
Vector3d moveDirection = (_path[_pathIndex] - agent.SceneObject.Position).Normalize();
SceneObject.Forward = moveDirection;
}
}
else
{
agent.SceneObject.Position += agent.SceneObject.Forward * moveDistance;
break;
}
}
return(BehaviourNodeStatus.Running);
}
public static FixedQuaternion LookRotation(Vector3d forward, Vector3d up)
{
Vector3d vector = forward;
Vector3d vector2 = Vector3d.Normalize(Vector3d.Cross(up, vector));
Vector3d vector3 = Vector3d.Cross(vector, vector2);
var m00 = vector2.x;
var m01 = vector2.y;
var m02 = vector2.z;
var m10 = vector3.x;
var m11 = vector3.y;
var m12 = vector3.z;
var m20 = vector.x;
var m21 = vector.y;
var m22 = vector.z;
var num8 = (m00 + m11) + m22;
var quaternion = FixedQuaternion.identity;
if (num8 > 0)
{
var num = FixedMath.Sqrt(num8 + FixedMath.One);
quaternion.w = num / 2;
num = FixedMath.Half.Div(num);
quaternion.x = (m12 - m21).Mul(num);
quaternion.y = (m20 - m02).Mul(num);
quaternion.z = (m01 - m10).Mul(num);
return(quaternion);
}
if ((m00 >= m11) && (m00 >= m22))
{
var num7 = FixedMath.Sqrt(((FixedMath.One + m00) - m11) - m22);
var num4 = FixedMath.Half.Div(num7);
quaternion.x = num7 / 2;
quaternion.y = (m01 + m10).Mul(num4);
quaternion.z = (m02 + m20).Mul(num4);
quaternion.w = (m12 - m21).Mul(num4);
return(quaternion);
}
if (m11 > m22)
{
var num6 = FixedMath.Sqrt(((FixedMath.One + m11) - m00) - m22);
var num3 = FixedMath.Half.Div(num6);
quaternion.x = (m10 + m01).Mul(num3);
quaternion.y = num6 / 2;
quaternion.z = (m21 + m12).Mul(num3);
quaternion.w = (m20 - m02).Mul(num3);
return(quaternion);
}
var num5 = FixedMath.Sqrt(((FixedMath.One + m22) - m00) - m11);
var num2 = FixedMath.Half.Div(num5);
quaternion.x = (m20 + m02).Mul(num2);
quaternion.y = (m21 + m12).Mul(num2);
quaternion.z = num5 / 2;
quaternion.w = (m01 - m10).Mul(num2);
return(quaternion);
}
public PhysicsOutput ObbTest(AABB aabb, long angle)
{
var halfw = aabb.Width / 2;
var halfh = aabb.Height / 2;
if (angle == 0)
{
return(ObbTest(aabb, 0, aabb));
}
var radius = FixedMath.Sqrt((halfw).Mul(halfw) + halfh.Mul(halfh));
var outterAABB = new AABB(aabb.Center, radius * 2, radius * 2);
return(ObbTest(aabb, angle, outterAABB));
}
public void CaculateAlignment()
{
var rowCount = FixedMath.Sqrt(_npcs.Count * FixedMath.One).CeilToInt();
int x, y;
GridService.GetCoordinate(_commander.Position, out x, out y);
int radius = 1;
int index = 0;
while (true)
{
for (int i = radius; i >= -radius; i--)
{
for (int j = radius; j >= -radius; j--)
{
if (i != radius && i != -radius)
{
if (j == radius || j == -radius)
{
if (GridService.IsEmpty(j + x, i + y))
{
_npcs[index].AlignmentX = j;
_npcs[index].AlignmentY = i;
index++;
if (index == _npcs.Count)
{
return;
}
}
}
}
else
{
if (GridService.IsEmpty(j + x, i + y))
{
_npcs[index].AlignmentX = j;
_npcs[index].AlignmentY = i;
index++;
if (index == _npcs.Count)
{
return;
}
}
}
}
}
radius++;
}
}
public override void OnAdd()
{
base.OnAdd();
SceneObject.TransformComp.EventGroup.ListenEvent((int)TransformComponent.Event.OnPositionChange, OnPositionChanged);
bs = LogicCore.SP.SceneManager.CurrentScene as BattleScene;
fp = new FixtureProxy();
fp.AABB = AABB;
fp.Fixture = new Transform2d()
{
p = AABB.Center, angle = Rotation
};
var halfw = AABB.Width / 2;
var halfh = AABB.Height / 2;
var radius = FixedMath.Sqrt((halfw).Mul(halfw) + halfh.Mul(halfh));
var outteraabb = new AABB(AABB.Center, radius * 2, radius * 2);
// fpId = bs.PhysicsTree.AddProxy(ref outteraabb, fp);
previousPosi = SceneObject.TransformComp.Position;
}
protected override void Execute(List <LogicEntity> entities)
{
foreach (LogicEntity e in entities)
{
e.ReplaceGravity((2 * e.maxJump.value).Div(e.timeToApex.value.Squared()));
e.isFalling = true;
e.ReplaceMaxJumpVelocity(e.gravity.value.Mul(e.timeToApex.value));
if (e.hasMinJump)
{
e.ReplaceMinJumpVelocity(FixedMath.Sqrt(2 * (e.gravity.value.Abs()).Mul(e.minJump.value)));
}
if (e.hasBounceHeight)
{
e.ReplaceBounceVelocity(FixedMath.Sqrt(2 * (e.gravity.value.Abs()).Mul(e.bounceHeight.value)));
}
}
}
public override void Execute(SceneObject sender, SceneObject reciever, object data)
{
var _battleScene = LogicCore.SP.SceneManager.CurrentScene as BattleScene;
long fw = FixedMath.Create(Width) / 100;
long fh = FixedMath.Create(Height) / 100;
long fr = FixedMath.Sqrt((fw / 2).Mul(fw / 2) + fh.Mul(fh));
var center = new Vector2d(0, fh / 2);
var q = FixedQuaternion.LookRotation(sender.Forward, Vector3d.up);
Utility.FixedRect rect = new Utility.FixedRect(center, fw, fh);
Utility.List.Clear();
_battleScene.FixedQuadTree.Query(sender as IFixedAgent, fr, Utility.List);
for (int i = 0; i < Utility.List.Count; i++)
{
if (IsTarget(Utility.List[i]))
{
if (Utility.PositionIsInRect(rect, sender.Position, q, Utility.List[i].Position))
{
EventManager.TriggerEvent(EventId, new RuntimeData(sender, Utility.List[i] as SceneObject, null));
}
}
}
base.Execute(sender, reciever, data);
}
private Vector3d GetAvoidDirectionVector(Vector3d selfPos, Vector3d currentVelocity, Vector3d otherPos, Vector3d otherVelocity, long combinedRadius)
{
long a = ((currentVelocity.x - otherVelocity.x).Mul(currentVelocity.x - otherVelocity.x)) +
((currentVelocity.z - otherVelocity.z).Mul(currentVelocity.z - otherVelocity.z));
long b = (2 * (selfPos.x - otherPos.x).Mul(currentVelocity.x - otherVelocity.x)) +
(2 * (selfPos.z - otherPos.z).Mul(currentVelocity.z - otherVelocity.z));
long c = ((selfPos.x - otherPos.x).Mul(selfPos.x - otherPos.x)) +
((selfPos.z - otherPos.z).Mul(selfPos.z - otherPos.z)) -
(combinedRadius.Mul(combinedRadius));
long d = (b.Mul(b)) - (4 * a.Mul(c));
if (d <= 0 || a == 0)
{
// if there are not 2 intersection points, then skip
return(Vector3d.zero);
}
// compute "heavy" calculations only once
long dSqrt = FixedMath.Sqrt(d);
long doubleA = 2 * a;
// compute roots, which in this case are actually time values informing of when the collision starts and ends
long t1 = (-b + dSqrt).Div(doubleA);
long t2 = (-b - dSqrt).Div(doubleA);
if (t1 < 0 && t2 < 0)
{
// if both times are negative, the collision is behind us (compared to velocity direction)
return(Vector3d.zero);
}
// find the lowest non-negative time, since this will be where the collision time interval starts
long time = 0;
if (t1 < 0)
{
time = t2;
}
else if (t2 < 0)
{
time = t1;
}
else
{
time = Math.Min(t1, t2);
}
// the collision time we want is actually 25 % within the collision
time += Math.Abs(t2 - t1) / 4;
// compute actual collision positions
Vector3d selfCollisionPos = selfPos + (currentVelocity * (time));
Vector3d otherCollisionPos = otherPos + (otherVelocity * (time));
_selfCollisionPos = selfCollisionPos;
//var selfgo = GameObject.CreatePrimitive(PrimitiveType.Sphere);
//var othergo = GameObject.CreatePrimitive(PrimitiveType.Sphere);
//selfgo.name = "self";
//othergo.name = "other";
//selfgo.transform.position = selfCollisionPos.ToVector3();
//othergo.transform.position = otherCollisionPos.ToVector3();
//Debug.LogError("pause " + self.name);
//Debug.LogError(self.name+" "+selfPos+" "+otherPos+" "+selfCollisionPos+" "+otherCollisionPos);
// return an avoid vector from the other's collision position to this unit's collision position
return(selfCollisionPos - otherCollisionPos);
}
public void Initialize(LSBody b1, LSBody b2)
{
IsValid = true;
if (!IsValid)
{
return;
}
if (b1.ID < b2.ID)
{
Body1 = b1;
Body2 = b2;
}
else
{
Body1 = b2;
Body2 = b1;
}
_ranIndex = -1;
_isColliding = false;
DistX = 0;
DistY = 0;
PenetrationX = 0;
PenetrationY = 0;
FastCollideDistance = b1.Radius + b2.Radius;
FastCollideDistance *= FastCollideDistance;
LeCollisionType = CollisionType.None;
if (Body1.Shape == ColliderType.None || Body2.Shape == ColliderType.None)
{
}
else if (Body1.Shape == ColliderType.Circle)
{
if (Body2.Shape == ColliderType.Circle)
{
LeCollisionType = CollisionType.Circle_Circle;
}
else if (Body2.Shape == ColliderType.AABox)
{
LeCollisionType = CollisionType.Circle_AABox;
}
else if (Body2.Shape == ColliderType.Polygon)
{
LeCollisionType = CollisionType.Circle_Polygon;
}
}
else if (Body1.Shape == ColliderType.AABox)
{
if (Body2.Shape == ColliderType.Circle)
{
LeCollisionType = CollisionType.Circle_AABox;
}
else if (Body2.Shape == ColliderType.AABox)
{
LeCollisionType = CollisionType.AABox_AABox;
}
else if (Body2.Shape == ColliderType.Polygon)
{
LeCollisionType = CollisionType.AABox_Polygon;
}
}
else if (Body1.Shape == ColliderType.Polygon)
{
if (Body2.Shape == ColliderType.Circle)
{
LeCollisionType = CollisionType.Circle_Polygon;
}
else if (Body2.Shape == ColliderType.AABox)
{
LeCollisionType = CollisionType.AABox_Polygon;
}
else if (Body2.Shape == ColliderType.Polygon)
{
LeCollisionType = CollisionType.Polygon_Polygon;
}
}
DoPhysics = ((Body1.IsTrigger || Body2.IsTrigger) == false);
if (DoPhysics)
{
}
//TODO: Space out checks when culled
//TODO: The time between collision checks might cause goofy behavior
//Maybe use a distance or velocity heuristic for culling instead of time since last collision
//It wouldn't be able to replace partitions because of raycasts and fast-moving objects
//Let's see if this works well or if something better is needed.
if (Body1.PreventCulling || Body2.PreventCulling)
{
//Never cull
CullCounter = -1;
}
else
{
//Immediately check collision
CullCounter = 0;
//If collision distance is too large, don't cull based on distance
PreventDistanceCull = FastCollideDistance > PhysicsManager.CullFastDistanceMax;
LastCollidedFrame = LockstepManager.FrameCount;
FastDistanceOffset = FixedMath.Sqrt(FastCollideDistance >> FixedMath.SHIFT_AMOUNT) + FixedMath.One * 2;
FastDistanceOffset *= FastDistanceOffset;
}
Active = true;
_Version++;
}
private void DistributeCollision()
{
if (!DoPhysics)
{
return;
}
switch (LeCollisionType)
{
case CollisionType.Circle_Circle:
DistX = Body1._position.x - Body2._position.x;
DistY = Body1._position.y - Body2._position.y;
dist = FixedMath.Sqrt((DistX * DistX + DistY * DistY) >> FixedMath.SHIFT_AMOUNT);
if (dist == 0)
{
//If objects are on the same position, give them push in random direction
const long randomMax = FixedMath.One / 32;
Body1._position.x += LSUtility.GetRandomLong(randomMax) - randomMax / 2;
Body1._position.y += LSUtility.GetRandomLong(randomMax) - randomMax / 2;
Body1.PositionChanged = true;
Body2._position.x += LSUtility.GetRandomLong(randomMax) - randomMax / 2;
Body2._position.y += LSUtility.GetRandomLong(randomMax) - randomMax / 2;
Body2.PositionChanged = true;
return;
}
depth = (Body1.Radius + Body2.Radius - dist);
if (depth <= 0)
{
return;
}
DistX = (DistX * depth / dist);
DistY = (DistY * depth / dist);
//Resolving collision
if (Body1.Immovable || (Body2.Immovable == false && Body1.Priority > Body2.Priority))
{
DistX *= -1;
DistY *= -1;
DistributeCircle_CirclePriority(Body1, Body2);
}
else if (Body2.Immovable || Body2.Priority > Body1.Priority)
{
DistributeCircle_CirclePriority(Body2, Body1);
}
else
{
DistX /= 2;
DistY /= 2;
DistributeCircle(Body1);
DistX *= -1;
DistY *= -1;
DistributeCircle(Body2);
}
break;
case CollisionType.Circle_AABox:
if (Body1.Shape == ColliderType.AABox)
{
DistributeCircle_Box(Body1, Body2);
}
else
{
DistributeCircle_Box(Body2, Body1);
}
break;
case CollisionType.Circle_Polygon:
if (Body1.Shape == ColliderType.Circle)
{
this.DistributeCircle_Poly(Body1, Body2);
}
else
{
this.DistributeCircle_Poly(Body2, Body1);
}
break;
}
}
void SearchBoxClosestXZ(int boxi, Vector3d p, ref long closestDist, NNConstraint constraint, ref NNInfo nnInfo)
{
BBTreeBox box = arr[boxi];
if (box.node != null)
{
//Leaf node
//Update the NNInfo
#if ASTARDEBUG
Debug.DrawLine((Vector3)box.node.GetVertex(1) + Vector3.up * 0.2f, (Vector3)box.node.GetVertex(2) + Vector3.up * 0.2f, Color.red);
Debug.DrawLine((Vector3)box.node.GetVertex(0) + Vector3.up * 0.2f, (Vector3)box.node.GetVertex(1) + Vector3.up * 0.2f, Color.red);
Debug.DrawLine((Vector3)box.node.GetVertex(2) + Vector3.up * 0.2f, (Vector3)box.node.GetVertex(0) + Vector3.up * 0.2f, Color.red);
#endif
Vector3d closest = box.node.ClosestPointOnNodeXZ(p);
if (constraint == null || constraint.Suitable(box.node))
{
// XZ distance
long dist = (closest.x - p.x).Mul(closest.x - p.x) + (closest.z - p.z).Mul(closest.z - p.z);
long pre = closestDist;
float pre_f = dist.ToFloat();
float clo_f = closestDist.ToFloat();
if (nnInfo.constrainedNode == null)
{
nnInfo.constrainedNode = box.node;
nnInfo.constFixedClampedPosition = closest;
closestDist = FixedMath.Sqrt(dist);
}
else if (dist < closestDist.Mul(closestDist))
{
nnInfo.constrainedNode = box.node;
nnInfo.constFixedClampedPosition = closest;
closestDist = FixedMath.Sqrt(dist);
}
}
#if ASTARDEBUG
Debug.DrawLine((Vector3)box.node.GetVertex(0), (Vector3)box.node.GetVertex(1), Color.blue);
Debug.DrawLine((Vector3)box.node.GetVertex(1), (Vector3)box.node.GetVertex(2), Color.blue);
Debug.DrawLine((Vector3)box.node.GetVertex(2), (Vector3)box.node.GetVertex(0), Color.blue);
#endif
}
else
{
#if ASTARDEBUG
Debug.DrawLine(new Vector3(box.rect.xMin, 0, box.rect.yMin), new Vector3(box.rect.xMax, 0, box.rect.yMin), Color.white);
Debug.DrawLine(new Vector3(box.rect.xMin, 0, box.rect.yMax), new Vector3(box.rect.xMax, 0, box.rect.yMax), Color.white);
Debug.DrawLine(new Vector3(box.rect.xMin, 0, box.rect.yMin), new Vector3(box.rect.xMin, 0, box.rect.yMax), Color.white);
Debug.DrawLine(new Vector3(box.rect.xMax, 0, box.rect.yMin), new Vector3(box.rect.xMax, 0, box.rect.yMax), Color.white);
#endif
//Search children
if (RectIntersectsCircle(arr[box.left].rect, p, closestDist))
{
SearchBoxClosestXZ(box.left, p, ref closestDist, constraint, ref nnInfo);
}
if (RectIntersectsCircle(arr[box.right].rect, p, closestDist))
{
SearchBoxClosestXZ(box.right, p, ref closestDist, constraint, ref nnInfo);
}
}
}
//f(x) that the table will cache
public override long F(long x)
{
return(FixedMath.Sqrt(x));
}
Vector2d linearPrograming(List <FixedLine> lines)
{
_fixedVecocity = _self.Velocity.ToVector2d();
_fixedPrefVelocity = _fixedVecocity.Normalize() * _self.Speed;
Vector2d newV = _fixedPrefVelocity;
// return curVelocity todo: 3D Linear Programming
for (int i = 0; i < lines.Count; ++i)
{
var line1 = lines[i];
var d = line1.direction;
var p = line1.point;
if (Vector2d.det(d, p - _fixedVecocity) > 0)
{
// ||d||^2 * t^2 + 2(dot(d, p)) * t + (||p||^2 - r^2) >= 0 available area in max speed
// delta = b^2 - 4ac, ||d|| = 1
var dp = Vector2d.Dot(p, d);
var delta = 4 * dp.Mul(dp) - 4 * (p.SqrMagnitude() - _self.Speed.Mul(_self.Speed));
if (delta < 0)
{
newV = _fixedVecocity;
break;
}
delta /= 4;
// m1 = (-b - sqrt(delta)) / ||d||^2
// m2 = (-b + sqrt(delta)) / ||d||^2
// m1 <= m2
var m1 = -dp - FixedMath.Sqrt(delta);
var m2 = -dp + FixedMath.Sqrt(delta);
// Cramer's rule
// p = p1 + t1d1 = p2 + t2d2
for (var j = 0; j < i; ++j)
{
var line2 = lines[j];
// t1 = det(d2, p1 - p2) / det(d1, d2)
var t1y = Vector2d.det(line1.direction, line2.direction);
var t1x = Vector2d.det(line2.direction, p - line2.point);
// parallel
if (Math.Abs(t1y) < 100)
{
// on the other side of available area
if (t1x < 0)
{
newV = _fixedVecocity;
return(newV);
}
continue;
}
var t = t1y.Div(t1x);
// right side of line1, mod m1
if (t1y < 0)
{
m1 = Math.Max(m1, t);
}
else
{
m2 = Math.Min(m2, t);
}
// Debug.LogError("fixed " + m1.ToFloat()+" "+m2.ToFloat());
if (m1 > m2)
{
newV = _fixedVecocity;
return(newV);
}
}
var tPref = Vector2d.Dot(d, _fixedPrefVelocity - p);
if (tPref > m2)
{
newV = p + m2 * d;
}
else if (tPref < m1)
{
newV = p + m1 * d;
}
else
{
newV = p + tPref * d;
}
_fixedVecocity = newV;
}
}
return(newV);
}
void computeNewFixedVelocity()
{
_fixedLines.Clear();
for (var i = 0; i < neighborsList.Count; ++i)
{
var neighbor = neighborsList[i];
var relativePos = neighbor.Position.ToVector2d() - _self.Position.ToVector2d();
var mRelativePos = relativePos.Magnitude();
//flag
var relativeVel = _self.Velocity.ToVector2d() - neighbor.Velocity.ToVector2d();
var comRadius = neighbor.Radius + _self.Radius;
FixedLine line = new FixedLine();
Vector2d u = Vector2d.zero;
if (comRadius < mRelativePos)
{
Vector2d vec = relativeVel - relativePos / FixedMath.Create(6);
var mVec = vec.Magnitude();
var dotProduct = Vector2d.Dot(vec, relativePos);
if (dotProduct < 0 && dotProduct.Mul(dotProduct) > comRadius.Mul(comRadius) * mVec.Mul(mVec))
{
Vector2d normVec = vec.Normalize();
line.direction = new Vector2d(normVec.y, -normVec.x);
u = normVec * (comRadius.Div(FixedMath.Create(6)) - mVec);
}
else
{
long mEdge = FixedMath.Sqrt(mRelativePos.Mul(mRelativePos) - comRadius.Mul(comRadius));
Vector2d edge = Vector2d.zero;
if (Vector2d.det(relativePos, vec) > 0)
{
edge = Vector2d.rotate(relativePos, mEdge.Div(mRelativePos), comRadius.Div(mRelativePos));
}
else
{
edge = Vector2d.rotate(relativePos, mEdge.Div(mRelativePos), -comRadius.Div(mRelativePos));
edge *= -1;
}
line.direction = edge.Normalize();
// project u' = v * dot(u, v) / v^2
// v = ||line.direction|| = 1
u = Vector2d.Dot(relativeVel, line.direction) * line.direction - relativeVel;
}
}
// tricks
else
{
Vector2d vec = relativeVel - relativePos / LockFrameMgr.FixedFrameTime;
Vector2d normVec = vec.Normalize();
line.direction = new Vector2d(normVec.y, -normVec.x);
u = (comRadius.Div(LockFrameMgr.FixedFrameTime) - vec.Magnitude()) * normVec;
}
line.point = _fixedVecocity + u / 2;
_fixedLines.Add(line);
}
_fixedVecocity = linearPrograming(_fixedLines);
}
// Use this for initialization
void Start()
{
return;
func = callBack;
//int count = 0;
//for (int i = 0; i < 100; i++)
//{
// var g = GameObject.CreatePrimitive(PrimitiveType.Cube);
// g.transform.position = new Vector3(Random.Range(0, 100), 0, Random.Range(0, 100));
//}
System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();
//sw.Start();
//for (int i = 0; i < 100; i++)
//{
// Ray r = new Ray();
// r.origin = new Vector3(Random.Range(0, 50),0, Random.Range(0, 50));
// r.direction = new Vector3(Random.Range(0, 1f), 0, Random.Range(0, 1f));
// r.direction = r.direction.normalized;
// var hit = Physics.Raycast(r, 100, -1);
// // Debug.DrawLine(r.origin, r.direction*100+r.origin, Color.red, 10);
// if (hit) count++;
//}
// Debug.Log(sw.ElapsedMilliseconds);
//return;
Vector2d[] vs = new Vector2d[4];
for (int i = 0; i < 100; i++)
{
var center = new Vector2d(Random.Range(0, 100), Random.Range(0, 100));
var aabb = new AABB(center, FixedMath.One * 2, FixedMath.One * 2);
FixtureProxy fp = new FixtureProxy();
long angle = FixedMath.One * (int)(Random.Range(0, 359));
Vector2d a = -aabb.Extents;
Vector2d b = a + new Vector2d(0, aabb.Height);
Vector2d c = aabb.Extents;
Vector2d d = a + new Vector2d(aabb.Width, 0);
var halfw = aabb.Width / 2;
var halfh = aabb.Height / 2;
var radius = FixedMath.Sqrt((halfw).Mul(halfw) + halfh.Mul(halfh));
a = RotatePosi(a, angle);
b = RotatePosi(b, angle);
c = RotatePosi(c, angle);
d = RotatePosi(d, angle);
vs[0] = a;
vs[1] = b;
vs[2] = c;
vs[3] = d;
Vector2d min = Vector2d.Min(vs) + center;
Vector2d max = Vector2d.Max(vs) + center;
DLog.Log(radius.ToFloat().ToString());
var outteraabb = new AABB(center, radius * 2, radius * 2);
fp.AABB = aabb;
fp.Fixture = new Transform2d(ref center, ref angle);
int id = tree.AddProxy(ref outteraabb, fp);
tree.MoveProxy(id, ref outteraabb, Vector2d.zero);
DrawFixtureProxy(fp);
DrawAABB(outteraabb);
}
//var bcs = GameObject.FindObjectsOfType<BoxCollider>();
//for (int i = 0; i < bcs.Length; i++)
//{
// var bc = bcs[i];
// var aabb = new AABB(new Vector2d(bc.transform.position), FixedMath.One, FixedMath.One);
// FixtureProxy fp = new FixtureProxy();
// fp.AABB = aabb;
// long angle = FixedMath.One * (int)(bc.transform.eulerAngles.y);
// Vector2d p = new Vector2d(bc.transform.position);
// fp.Fixture = new Transform2d(ref p, ref angle);
// tree.AddProxy(ref aabb, fp);
// DrawFixtureProxy(fp);
//}
// sw.Reset();
sw.Start();
for (int i = 0; i < 1; i++)
{
RayCastInput input = new RayCastInput();
input.Point1 = new Vector2d(Random.Range(0, 50) * FixedMath.One, Random.Range(0, 50) * FixedMath.One);
input.Point2 = new Vector2d(Random.Range(50, 100) * FixedMath.One, Random.Range(50, 100) * FixedMath.One);
NewSphere(input.Point1.ToVector3(), "start");
NewSphere(input.Point2.ToVector3(), "end");
input.MaxFraction = FixedMath.One;
DrawLine(input);
tree.RayCast(callBack, ref input);
}
Debug.Log(sw.ElapsedMilliseconds);
}
