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); }