public void Clone(TOIInput input) { this.ProxyA.Radius = input.ProxyA.Radius; this.ProxyA.Vertices.Clear(); this.ProxyA.Vertices.AddRange(input.ProxyA.Vertices); this.ProxyB.Radius = input.ProxyB.Radius; this.ProxyB.Vertices.Clear(); this.ProxyB.Vertices.AddRange(input.ProxyB.Vertices); this.SweepA = input.SweepA; this.SweepB = input.SweepB; this.TMax = input.TMax; }
public void Restore(TOIInput input) { input.ProxyA.Radius = this.ProxyA.Radius; input.ProxyA.Vertices.Clear(); input.ProxyA.Vertices.AddRange(this.ProxyA.Vertices); input.ProxyB.Radius = this.ProxyB.Radius; input.ProxyB.Vertices.Clear(); input.ProxyB.Vertices.AddRange(this.ProxyB.Vertices); input.SweepA = this.SweepA; input.SweepB = this.SweepB; input.TMax = this.TMax; }
public override void Step(Settings settings) { settings.pause = 1; base.Step(settings); settings.pause = 0; Sweep sweep1 = new Sweep(); sweep1.C0.Set(0.0f, 20.0f); sweep1.A0 = 0.0f; sweep1.C = sweep1.C0; sweep1.A = sweep1.A0; sweep1.T0 = 0.0f; sweep1.LocalCenter = _body1.GetLocalCenter(); Sweep sweep2 = new Sweep(); sweep2.C0.Set(9.6363468f, 28.050615f); sweep2.A0 = 1.6408679f; sweep2.C = sweep2.C0 + new Vec2(-0.075121880f, 0.27358246f); sweep2.A = sweep2.A0 - 10.434675f; sweep2.T0 = 0.0f; sweep2.LocalCenter = _body2.GetLocalCenter(); TOIInput input = new TOIInput { SweepA = sweep1, SweepB = sweep2 }; float toi = Collision.TimeOfImpact(input, _shape1, _shape2); OpenGLDebugDraw.DrawString(5, _textLine, "toi = " + toi.ToString()); _textLine += 15; XForm xf2 = new XForm(); sweep2.GetTransform(out xf2, toi); int vertexCount = _shape2.VertexCount; Vec2[] vertices = new Vec2[Box2DNet.Common.Settings.MaxPolygonVertices]; Vec2[] localVertices = _shape2.Vertices; for (int i = 0; i < vertexCount; ++i) { vertices[i] = Box2DNet.Common.Math.Mul(xf2, localVertices[i]); } _debugDraw.DrawPolygon(vertices, vertexCount, new Color(0.5f, 0.7f, 0.9f)); localVertices = _shape2.CoreVertices; for (int i = 0; i < vertexCount; ++i) { vertices[i] = Box2DNet.Common.Math.Mul(xf2, localVertices[i]); } _debugDraw.DrawPolygon(vertices, vertexCount, new Color(0.5f, 0.7f, 0.9f)); }
public float ComputeTOI(Sweep sweepA, Sweep sweepB) { TOIInput input = new TOIInput(); input.SweepA = sweepA; input.SweepB = sweepB; input.SweepRadiusA = _fixtureA.ComputeSweepRadius(sweepA.LocalCenter); input.SweepRadiusB = _fixtureB.ComputeSweepRadius(sweepB.LocalCenter); input.Tolerance = Common.Settings.LinearSlop; return(Collision.Collision.TimeOfImpact(input, _fixtureA.Shape, _fixtureB.Shape)); }
public void Distance() { TOIInput input = new TOIInput(); input.ProxyA = new DistanceProxy(_shapeA, 0); input.ProxyB = new DistanceProxy(_shapeB, 0); input.SweepA = _sweepA; input.SweepB = _sweepB; input.TMax = 1.0f; TimeOfImpact.CalculateTimeOfImpact(ref input, out _); }
public void Distance() { TOIInput input = new TOIInput(); input.ProxyA.Set(_shapeA, 0); input.ProxyB.Set(_shapeB, 0); input.SweepA = _sweepA; input.SweepB = _sweepB; input.TMax = 1.0f; TOIOutput output; TimeOfImpact.CalculateTimeOfImpact(out output, input); }
public override void Update(GameSettings settings, GameTime gameTime) { base.Update(settings, gameTime); Sweep sweepA = new Sweep(); sweepA.C0 = new Vector2(24.0f, -60.0f); sweepA.A0 = 2.95f; sweepA.C = sweepA.C0; sweepA.A = sweepA.A0; sweepA.LocalCenter = Vector2.Zero; Sweep sweepB = new Sweep(); sweepB.C0 = new Vector2(53.474274f, -50.252514f); sweepB.A0 = 513.36676f; // - 162.0f * b2_pi; sweepB.C = new Vector2(54.595478f, -51.083473f); sweepB.A = 513.62781f; // - 162.0f * b2_pi; sweepB.LocalCenter = Vector2.Zero; //sweepB.a0 -= 300.0f * b2_pi; //sweepB.a -= 300.0f * b2_pi; TOIInput input = new TOIInput(); input.ProxyA.Set(_shapeA, 0); input.ProxyB.Set(_shapeB, 0); input.SweepA = sweepA; input.SweepB = sweepB; input.TMax = 1.0f; TOIOutput output; TimeOfImpact.CalculateTimeOfImpact(out output, input); DrawString("TOI = " + output.T); DrawString(string.Format("Max TOI iters = {0:n}, Max root iters = {1:n}", TimeOfImpact.TOIMaxIters, TimeOfImpact.TOIMaxRootIters)); Vector2[] vertices = new Vector2[Settings.MaxPolygonVertices]; DebugView.BeginCustomDraw(ref GameInstance.Projection, ref GameInstance.View); Transform transformA; sweepA.GetTransform(out transformA, 0.0f); for (int i = 0; i < _shapeA.Vertices.Count; ++i) { vertices[i] = MathUtils.Mul(ref transformA, _shapeA.Vertices[i]); } DebugView.DrawPolygon(vertices, _shapeA.Vertices.Count, new Color(0.9f, 0.9f, 0.9f)); Transform transformB; sweepB.GetTransform(out transformB, 0.0f); for (int i = 0; i < _shapeB.Vertices.Count; ++i) { vertices[i] = MathUtils.Mul(ref transformB, _shapeB.Vertices[i]); } DebugView.DrawPolygon(vertices, _shapeB.Vertices.Count, new Color(0.5f, 0.9f, 0.5f)); sweepB.GetTransform(out transformB, output.T); for (int i = 0; i < _shapeB.Vertices.Count; ++i) { vertices[i] = MathUtils.Mul(ref transformB, _shapeB.Vertices[i]); } DebugView.DrawPolygon(vertices, _shapeB.Vertices.Count, new Color(0.5f, 0.7f, 0.9f)); sweepB.GetTransform(out transformB, 1.0f); for (int i = 0; i < _shapeB.Vertices.Count; ++i) { vertices[i] = MathUtils.Mul(ref transformB, _shapeB.Vertices[i]); } DebugView.DrawPolygon(vertices, _shapeB.Vertices.Count, new Color(0.9f, 0.5f, 0.5f)); DebugView.EndCustomDraw(); }
/// <summary> /// Compute the upper bound on time before two shapes penetrate. Time is represented as a fraction /// between [0,tMax]. This uses a swept separating axis and may miss some intermediate, /// non-tunneling collision. If you change the time interval, you should call this function again. /// Note: use Distance to compute the contact point and normal at the time of impact. /// </summary> /// <param name="output"></param> /// <param name="input"></param> public void GetTimeOfImpact(TOIOutput output, TOIInput input) { // CCD via the local separating axis method. This seeks progression // by computing the largest time at which separation is maintained. ++ToiCalls; output.State = TOIOutputState.Unknown; output.T = input.tMax; Distance.DistanceProxy proxyA = input.ProxyA; Distance.DistanceProxy proxyB = input.ProxyB; sweepA.Set(input.SweepA); sweepB.Set(input.SweepB); // Large rotations can make the root finder fail, so we normalize the // sweep angles. sweepA.Normalize(); sweepB.Normalize(); float tMax = input.tMax; float totalRadius = proxyA.Radius + proxyB.Radius; // djm: whats with all these constants? float target = MathUtils.Max(Settings.LINEAR_SLOP, totalRadius - 3.0f * Settings.LINEAR_SLOP); const float tolerance = 0.25f * Settings.LINEAR_SLOP; Debug.Assert(target > tolerance); float t1 = 0f; int iter = 0; cache.Count = 0; distanceInput.ProxyA = input.ProxyA; distanceInput.ProxyB = input.ProxyB; distanceInput.UseRadii = false; // The outer loop progressively attempts to compute new separating axes. // This loop terminates when an axis is repeated (no progress is made). for (; ; ) { sweepA.GetTransform(xfA, t1); sweepB.GetTransform(xfB, t1); // System.out.printf("sweepA: %f, %f, sweepB: %f, %f\n", // sweepA.c.x, sweepA.c.y, sweepB.c.x, sweepB.c.y); // Get the distance between shapes. We can also use the results // to get a separating axis distanceInput.TransformA = xfA; distanceInput.TransformB = xfB; pool.GetDistance().GetDistance(distanceOutput, cache, distanceInput); // System.out.printf("Dist: %f at points %f, %f and %f, %f. %d iterations\n", // distanceOutput.distance, distanceOutput.pointA.x, distanceOutput.pointA.y, // distanceOutput.pointB.x, distanceOutput.pointB.y, // distanceOutput.iterations); // If the shapes are overlapped, we give up on continuous collision. if (distanceOutput.Distance <= 0f) { // System.out.println("failure, overlapped"); // Failure! output.State = TOIOutputState.Overlapped; output.T = 0f; break; } if (distanceOutput.Distance < target + tolerance) { // System.out.println("touching, victory"); // Victory! output.State = TOIOutputState.Touching; output.T = t1; break; } // Initialize the separating axis. fcn.Initialize(cache, proxyA, sweepA, proxyB, sweepB, t1); // Compute the TOI on the separating axis. We do this by successively // resolving the deepest point. This loop is bounded by the number of // vertices. bool done = false; float t2 = tMax; int pushBackIter = 0; for (; ; ) { // Find the deepest point at t2. Store the witness point indices. float s2 = fcn.FindMinSeparation(indexes, t2); // System.out.printf("s2: %f\n", s2); // Is the final configuration separated? if (s2 > target + tolerance) { // Victory! // System.out.println("separated"); output.State = TOIOutputState.Separated; output.T = tMax; done = true; break; } // Has the separation reached tolerance? if (s2 > target - tolerance) { // System.out.println("advancing"); // Advance the sweeps t1 = t2; break; } // Compute the initial separation of the witness points. float s1 = fcn.Evaluate(indexes[0], indexes[1], t1); // Check for initial overlap. This might happen if the root finder // runs out of iterations. // System.out.printf("s1: %f, target: %f, tolerance: %f\n", s1, target, // tolerance); if (s1 < target - tolerance) { // System.out.println("failed?"); output.State = TOIOutputState.Failed; output.T = t1; done = true; break; } // Check for touching if (s1 <= target + tolerance) { // System.out.println("touching?"); // Victory! t1 should hold the TOI (could be 0.0). output.State = TOIOutputState.Touching; output.T = t1; done = true; break; } // Compute 1D root of: f(x) - target = 0 int rootIterCount = 0; float a1 = t1, a2 = t2; for (; ; ) { // Use a mix of the secant rule and bisection. float t; if ((rootIterCount & 1) == 1) { // Secant rule to improve convergence. t = a1 + (target - s1) * (a2 - a1) / (s2 - s1); } else { // Bisection to guarantee progress. t = 0.5f * (a1 + a2); } float s = fcn.Evaluate(indexes[0], indexes[1], t); if (MathUtils.Abs(s - target) < tolerance) { // t2 holds a tentative value for t1 t2 = t; break; } // Ensure we continue to bracket the root. if (s > target) { a1 = t; s1 = s; } else { a2 = t; s2 = s; } ++rootIterCount; ++ToiRootIters; // djm: whats with this? put in settings? if (rootIterCount == 50) { break; } } ToiMaxRootIters = MathUtils.Max(ToiMaxRootIters, rootIterCount); ++pushBackIter; if (pushBackIter == Settings.MAX_POLYGON_VERTICES) { break; } } ++iter; ++ToiIters; if (done) { // System.out.println("done"); break; } if (iter == MAX_ITERATIONS) { // System.out.println("failed, root finder stuck"); // Root finder got stuck. Semi-victory. output.State = TOIOutputState.Failed; output.T = t1; break; } } // System.out.printf("final sweeps: %f, %f, %f; %f, %f, %f", input.s) ToiMaxIters = MathUtils.Max(ToiMaxIters, iter); }
/// <summary> /// Compute the upper bound on time before two shapes penetrate. Time is represented as a fraction /// between [0,tMax]. This uses a swept separating axis and may miss some intermediate, /// non-tunneling collision. If you change the time interval, you should call this function again. /// Note: use Distance to compute the contact point and normal at the time of impact. /// </summary> /// <param name="output"></param> /// <param name="input"></param> public void GetTimeOfImpact(TOIOutput output, TOIInput input) { // CCD via the local separating axis method. This seeks progression // by computing the largest time at which separation is maintained. ++ToiCalls; output.State = TOIOutputState.Unknown; output.T = input.tMax; Distance.DistanceProxy proxyA = input.ProxyA; Distance.DistanceProxy proxyB = input.ProxyB; sweepA.Set(input.SweepA); sweepB.Set(input.SweepB); // Large rotations can make the root finder fail, so we normalize the // sweep angles. sweepA.Normalize(); sweepB.Normalize(); float tMax = input.tMax; float totalRadius = proxyA.Radius + proxyB.Radius; // djm: whats with all these constants? float target = MathUtils.Max(Settings.LINEAR_SLOP, totalRadius - 3.0f * Settings.LINEAR_SLOP); const float tolerance = 0.25f * Settings.LINEAR_SLOP; Debug.Assert(target > tolerance); float t1 = 0f; int iter = 0; cache.Count = 0; distanceInput.ProxyA = input.ProxyA; distanceInput.ProxyB = input.ProxyB; distanceInput.UseRadii = false; // The outer loop progressively attempts to compute new separating axes. // This loop terminates when an axis is repeated (no progress is made). for (; ;) { sweepA.GetTransform(xfA, t1); sweepB.GetTransform(xfB, t1); // System.out.printf("sweepA: %f, %f, sweepB: %f, %f\n", // sweepA.c.x, sweepA.c.y, sweepB.c.x, sweepB.c.y); // Get the distance between shapes. We can also use the results // to get a separating axis distanceInput.TransformA = xfA; distanceInput.TransformB = xfB; pool.GetDistance().GetDistance(distanceOutput, cache, distanceInput); // System.out.printf("Dist: %f at points %f, %f and %f, %f. %d iterations\n", // distanceOutput.distance, distanceOutput.pointA.x, distanceOutput.pointA.y, // distanceOutput.pointB.x, distanceOutput.pointB.y, // distanceOutput.iterations); // If the shapes are overlapped, we give up on continuous collision. if (distanceOutput.Distance <= 0f) { // System.out.println("failure, overlapped"); // Failure! output.State = TOIOutputState.Overlapped; output.T = 0f; break; } if (distanceOutput.Distance < target + tolerance) { // System.out.println("touching, victory"); // Victory! output.State = TOIOutputState.Touching; output.T = t1; break; } // Initialize the separating axis. fcn.Initialize(cache, proxyA, sweepA, proxyB, sweepB, t1); // Compute the TOI on the separating axis. We do this by successively // resolving the deepest point. This loop is bounded by the number of // vertices. bool done = false; float t2 = tMax; int pushBackIter = 0; for (; ;) { // Find the deepest point at t2. Store the witness point indices. float s2 = fcn.FindMinSeparation(indexes, t2); // System.out.printf("s2: %f\n", s2); // Is the final configuration separated? if (s2 > target + tolerance) { // Victory! // System.out.println("separated"); output.State = TOIOutputState.Separated; output.T = tMax; done = true; break; } // Has the separation reached tolerance? if (s2 > target - tolerance) { // System.out.println("advancing"); // Advance the sweeps t1 = t2; break; } // Compute the initial separation of the witness points. float s1 = fcn.Evaluate(indexes[0], indexes[1], t1); // Check for initial overlap. This might happen if the root finder // runs out of iterations. // System.out.printf("s1: %f, target: %f, tolerance: %f\n", s1, target, // tolerance); if (s1 < target - tolerance) { // System.out.println("failed?"); output.State = TOIOutputState.Failed; output.T = t1; done = true; break; } // Check for touching if (s1 <= target + tolerance) { // System.out.println("touching?"); // Victory! t1 should hold the TOI (could be 0.0). output.State = TOIOutputState.Touching; output.T = t1; done = true; break; } // Compute 1D root of: f(x) - target = 0 int rootIterCount = 0; float a1 = t1, a2 = t2; for (; ;) { // Use a mix of the secant rule and bisection. float t; if ((rootIterCount & 1) == 1) { // Secant rule to improve convergence. t = a1 + (target - s1) * (a2 - a1) / (s2 - s1); } else { // Bisection to guarantee progress. t = 0.5f * (a1 + a2); } float s = fcn.Evaluate(indexes[0], indexes[1], t); if (MathUtils.Abs(s - target) < tolerance) { // t2 holds a tentative value for t1 t2 = t; break; } // Ensure we continue to bracket the root. if (s > target) { a1 = t; s1 = s; } else { a2 = t; s2 = s; } ++rootIterCount; ++ToiRootIters; // djm: whats with this? put in settings? if (rootIterCount == 50) { break; } } ToiMaxRootIters = MathUtils.Max(ToiMaxRootIters, rootIterCount); ++pushBackIter; if (pushBackIter == Settings.MAX_POLYGON_VERTICES) { break; } } ++iter; ++ToiIters; if (done) { // System.out.println("done"); break; } if (iter == MAX_ITERATIONS) { // System.out.println("failed, root finder stuck"); // Root finder got stuck. Semi-victory. output.State = TOIOutputState.Failed; output.T = t1; break; } } // System.out.printf("final sweeps: %f, %f, %f; %f, %f, %f", input.s) ToiMaxIters = MathUtils.Max(ToiMaxIters, iter); }
/// <summary> /// Find TOI contacts and solve them. /// </summary> /// <param name="step">The step.</param> private void SolveTOI(ref TimeStep step) { Island.Reset(2 * Settings.MaxTOIContacts, Settings.MaxTOIContacts, 0, ContactManager); if (_stepComplete) { for (int i = 0; i < BodyList.Count; i++) { BodyList[i].Flags &= ~BodyFlags.Island; BodyList[i].Sweep.Alpha0 = 0.0f; } for (Contact c = ContactManager.ContactList; c != null; c = c.Next) { // Invalidate TOI c.Flags &= ~(ContactFlags.TOI | ContactFlags.Island); c.TOICount = 0; c.TOI = 1.0f; } } // Find TOI events and solve them. for (;;) { // Find the first TOI. Contact minContact = null; float minAlpha = 1.0f; for (Contact c = ContactManager.ContactList; c != null; c = c.Next) { // Is this contact disabled? if (c.Enabled == false) { continue; } // Prevent excessive sub-stepping. if (c.TOICount > Settings.MaxSubSteps) { continue; } float alpha; if ((c.Flags & ContactFlags.TOI) == ContactFlags.TOI) { // This contact has a valid cached TOI. alpha = c.TOI; } else { Fixture fA = c.FixtureA; Fixture fB = c.FixtureB; // Is there a sensor? if (fA.IsSensor || fB.IsSensor) { continue; } Body bA = fA.Body; Body bB = fB.Body; BodyType typeA = bA.BodyType; BodyType typeB = bB.BodyType; Debug.Assert(typeA == BodyType.Dynamic || typeB == BodyType.Dynamic); bool awakeA = bA.Awake && typeA != BodyType.Static; bool awakeB = bB.Awake && typeB != BodyType.Static; // Is at least one body awake? if (awakeA == false && awakeB == false) { continue; } bool collideA = bA.IsBullet || typeA != BodyType.Dynamic; bool collideB = bB.IsBullet || typeB != BodyType.Dynamic; // Are these two non-bullet dynamic bodies? if (collideA == false && collideB == false) { continue; } // Compute the TOI for this contact. // Put the sweeps onto the same time interval. float alpha0 = bA.Sweep.Alpha0; if (bA.Sweep.Alpha0 < bB.Sweep.Alpha0) { alpha0 = bB.Sweep.Alpha0; bA.Sweep.Advance(alpha0); } else if (bB.Sweep.Alpha0 < bA.Sweep.Alpha0) { alpha0 = bA.Sweep.Alpha0; bB.Sweep.Advance(alpha0); } Debug.Assert(alpha0 < 1.0f); // Compute the time of impact in interval [0, minTOI] TOIInput input = new TOIInput(); input.ProxyA.Set(fA.Shape, c.ChildIndexA); input.ProxyB.Set(fB.Shape, c.ChildIndexB); input.SweepA = bA.Sweep; input.SweepB = bB.Sweep; input.TMax = 1.0f; TOIOutput output; TimeOfImpact.CalculateTimeOfImpact(out output, ref input); // Beta is the fraction of the remaining portion of the . float beta = output.T; if (output.State == TOIOutputState.Touching) { alpha = Math.Min(alpha0 + (1.0f - alpha0) * beta, 1.0f); } else { alpha = 1.0f; } c.TOI = alpha; c.Flags |= ContactFlags.TOI; } if (alpha < minAlpha) { // This is the minimum TOI found so far. minContact = c; minAlpha = alpha; } } if (minContact == null || 1.0f - 10.0f * Settings.Epsilon < minAlpha) { // No more TOI events. Done! _stepComplete = true; break; } // Advance the bodies to the TOI. Fixture fA1 = minContact.FixtureA; Fixture fB1 = minContact.FixtureB; Body bA1 = fA1.Body; Body bB1 = fB1.Body; Sweep backup1 = bA1.Sweep; Sweep backup2 = bB1.Sweep; bA1.Advance(minAlpha); bB1.Advance(minAlpha); // The TOI contact likely has some new contact points. minContact.Update(ContactManager); minContact.Flags &= ~ContactFlags.TOI; ++minContact.TOICount; // Is the contact solid? if (minContact.Enabled == false || minContact.IsTouching() == false) { // Restore the sweeps. minContact.Enabled = false; bA1.Sweep = backup1; bB1.Sweep = backup2; bA1.SynchronizeTransform(); bB1.SynchronizeTransform(); continue; } bA1.Awake = true; bB1.Awake = true; // Build the island Island.Clear(); Island.Add(bA1); Island.Add(bB1); Island.Add(minContact); bA1.Flags |= BodyFlags.Island; bB1.Flags |= BodyFlags.Island; minContact.Flags |= ContactFlags.Island; // Get contacts on bodyA and bodyB. Body[] bodies = { bA1, bB1 }; for (int i = 0; i < 2; ++i) { Body body = bodies[i]; if (body.BodyType == BodyType.Dynamic) { // for (ContactEdge ce = body.ContactList; ce && Island.BodyCount < Settings.MaxTOIContacts; ce = ce.Next) for (ContactEdge ce = body.ContactList; ce != null; ce = ce.Next) { Contact contact = ce.Contact; // Has this contact already been added to the island? if ((contact.Flags & ContactFlags.Island) == ContactFlags.Island) { continue; } // Only add static, kinematic, or bullet bodies. Body other = ce.Other; if (other.BodyType == BodyType.Dynamic && body.IsBullet == false && other.IsBullet == false) { continue; } // Skip sensors. if (contact.FixtureA.IsSensor || contact.FixtureB.IsSensor) { continue; } // Tentatively advance the body to the TOI. Sweep backup = other.Sweep; if ((other.Flags & BodyFlags.Island) == 0) { other.Advance(minAlpha); } // Update the contact points contact.Update(ContactManager); // Was the contact disabled by the user? if (contact.Enabled == false) { other.Sweep = backup; other.SynchronizeTransform(); continue; } // Are there contact points? if (contact.IsTouching() == false) { other.Sweep = backup; other.SynchronizeTransform(); continue; } // Add the contact to the island contact.Flags |= ContactFlags.Island; Island.Add(contact); // Has the other body already been added to the island? if ((other.Flags & BodyFlags.Island) == BodyFlags.Island) { continue; } // Add the other body to the island. other.Flags |= BodyFlags.Island; if (other.BodyType != BodyType.Static) { other.Awake = true; } Island.Add(other); } } } TimeStep subStep; subStep.dt = (1.0f - minAlpha) * step.dt; subStep.inv_dt = 1.0f / subStep.dt; subStep.dtRatio = 1.0f; //subStep.positionIterations = 20; //subStep.velocityIterations = step.velocityIterations; //subStep.warmStarting = false; Island.SolveTOI(ref subStep); // Reset island flags and synchronize broad-phase proxies. for (int i = 0; i < Island.BodyCount; ++i) { Body body = Island.Bodies[i]; body.Flags &= ~BodyFlags.Island; if (body.BodyType != BodyType.Dynamic) { continue; } body.SynchronizeFixtures(); // Invalidate all contact TOIs on this displaced body. for (ContactEdge ce = body.ContactList; ce != null; ce = ce.Next) { ce.Contact.Flags &= ~(ContactFlags.TOI | ContactFlags.Island); } } // Commit fixture proxy movements to the broad-phase so that new contacts are created. // Also, some contacts can be destroyed. ContactManager.FindNewContacts(); if (_subStepping) { _stepComplete = false; break; } } }
/// <summary> // Advance a dynamic body to its first time of contact // and adjust the position to ensure clearance. /// </summary> /// <param name="body">The body.</param> private void SolveTOI(Body body) { // Find the minimum contact. Contact toiContact = null; float toi = 1.0f; Body toiOther = null; bool found; int count; int iter = 0; bool bullet = body.IsBullet; // Iterate until all contacts agree on the minimum TOI. We have // to iterate because the TOI algorithm may skip some intermediate // collisions when objects rotate through each other. do { count = 0; found = false; for (ContactEdge ce = body.ContactList; ce != null; ce = ce.Next) { if (ce.Contact == toiContact) { continue; } Body other = ce.Other; BodyType type = other.BodyType; // Only bullets perform TOI with dynamic bodies. if (bullet) { // Bullets only perform TOI with bodies that have their TOI resolved. if ((other.Flags & BodyFlags.Toi) == 0) { continue; } // No repeated hits on non-static bodies if (type != BodyType.Static && (ce.Contact.Flags & ContactFlags.BulletHit) != 0) { continue; } } else if (type == BodyType.Dynamic) { continue; } // Check for a disabled contact. Contact contact = ce.Contact; if (contact.Enabled == false) { continue; } // Prevent infinite looping. if (contact.TOICount > 10) { continue; } Fixture fixtureA = contact.FixtureA; Fixture fixtureB = contact.FixtureB; int indexA = contact.ChildIndexA; int indexB = contact.ChildIndexB; // Cull sensors. if (fixtureA.IsSensor || fixtureB.IsSensor) { continue; } Body bodyA = fixtureA.Body; Body bodyB = fixtureB.Body; // Compute the time of impact in interval [0, minTOI] TOIInput input = new TOIInput(); input.ProxyA.Set(fixtureA.Shape, indexA); input.ProxyB.Set(fixtureB.Shape, indexB); input.SweepA = bodyA.Sweep; input.SweepB = bodyB.Sweep; input.TMax = toi; TOIOutput output; TimeOfImpact.CalculateTimeOfImpact(out output, ref input); if (output.State == TOIOutputState.Touching && output.T < toi) { toiContact = contact; toi = output.T; toiOther = other; found = true; } ++count; } ++iter; } while (found && count > 1 && iter < 50); if (toiContact == null) { body.Advance(1.0f); return; } Sweep backup = body.Sweep; body.Advance(toi); toiContact.Update(ContactManager); if (toiContact.Enabled == false) { // Contact disabled. Backup and recurse. body.Sweep = backup; SolveTOI(body); } ++toiContact.TOICount; // Update all the valid contacts on this body and build a contact island. count = 0; for (ContactEdge ce = body.ContactList; (ce != null) && (count < Settings.MaxTOIContacts); ce = ce.Next) { Body other = ce.Other; BodyType type = other.BodyType; // Only perform correction with static bodies, so the // body won't get pushed out of the world. if (type == BodyType.Dynamic) { continue; } // Check for a disabled contact. Contact contact = ce.Contact; if (contact.Enabled == false) { continue; } Fixture fixtureA = contact.FixtureA; Fixture fixtureB = contact.FixtureB; // Cull sensors. if (fixtureA.IsSensor || fixtureB.IsSensor) { continue; } // The contact likely has some new contact points. The listener // gives the user a chance to disable the contact. if (contact != toiContact) { contact.Update(ContactManager); } // Did the user disable the contact? if (contact.Enabled == false) { // Skip this contact. continue; } if (contact.IsTouching() == false) { continue; } _toiContacts[count] = contact; ++count; } // Reduce the TOI body's overlap with the contact island. _toiSolver.Initialize(_toiContacts, count, body); const float k_toiBaumgarte = 0.75f; // bool solved = false; for (int i = 0; i < 20; ++i) { bool contactsOkay = _toiSolver.Solve(k_toiBaumgarte); if (contactsOkay) { // solved = true; break; } } if (toiOther.BodyType != BodyType.Static) { toiContact.Flags |= ContactFlags.BulletHit; } }
public override void Step(Framework.Settings settings) { base.Step(settings); Sweep sweepA = new Sweep(); sweepA.c0 = Vector2.Zero; sweepA.a0 = 0.0f; sweepA.c = sweepA.c0; sweepA.a = sweepA.a0; sweepA.localCenter = Vector2.Zero; Sweep sweepB = new Sweep(); sweepB.c0 = new Vector2(-0.20382018f, 2.1368704f); sweepB.a0 = -3.1664171f; sweepB.c = new Vector2(-0.26699525f, 2.3552670f); sweepB.a = -3.3926492f; sweepB.localCenter = Vector2.Zero; TOIInput input = new TOIInput(); input.proxyA.Set(_shapeA); input.proxyB.Set(_shapeB); input.sweepA = sweepA; input.sweepB = sweepB; input.tMax = 1.0f; TOIOutput output; XNA.TimeOfImpact.CalculateTimeOfImpact(out output, ref input); _debugDraw.DrawString(50, _textLine, "toi = {0:n}", output.t); _textLine += 15; _debugDraw.DrawString(50, _textLine, "max toi iters = {0:n}, max root iters = {1:n}", XNA.TimeOfImpact.b2_toiMaxIters, XNA.TimeOfImpact.b2_toiMaxRootIters); _textLine += 15; FixedArray8 <Vector2> vertices = new FixedArray8 <Vector2>(); Transform transformA; sweepA.GetTransform(out transformA, 0.0f); for (int i = 0; i < _shapeA._vertexCount; ++i) { vertices[i] = MathUtils.Multiply(ref transformA, _shapeA._vertices[i]); } _debugDraw.DrawPolygon(ref vertices, _shapeA._vertexCount, new Color(0.9f, 0.9f, 0.9f)); Transform transformB; sweepB.GetTransform(out transformB, 0.0f); Vector2 localPoint = new Vector2(2.0f, -0.1f); Vector2 rB = MathUtils.Multiply(ref transformB, localPoint) - sweepB.c0; float wB = sweepB.a - sweepB.a0; Vector2 vB = sweepB.c - sweepB.c0; Vector2 v = vB + MathUtils.Cross(wB, rB); for (int i = 0; i < _shapeB._vertexCount; ++i) { vertices[i] = MathUtils.Multiply(ref transformB, _shapeB._vertices[i]); } _debugDraw.DrawPolygon(ref vertices, _shapeB._vertexCount, new Color(0.5f, 0.9f, 0.5f)); sweepB.GetTransform(out transformB, output.t); for (int i = 0; i < _shapeB._vertexCount; ++i) { vertices[i] = MathUtils.Multiply(ref transformB, _shapeB._vertices[i]); } _debugDraw.DrawPolygon(ref vertices, _shapeB._vertexCount, new Color(0.5f, 0.7f, 0.9f)); sweepB.GetTransform(out transformB, 1.0f); for (int i = 0; i < _shapeB._vertexCount; ++i) { vertices[i] = MathUtils.Multiply(ref transformB, _shapeB._vertices[i]); } _debugDraw.DrawPolygon(ref vertices, _shapeB._vertexCount, new Color(0.9f, 0.5f, 0.5f)); }
public override void Update(GameSettings settings, GameTime gameTime) { base.Update(settings, gameTime); Sweep sweepA = new Sweep(); sweepA.C0 = new Vector2(24.0f, -60.0f); sweepA.A0 = 2.95f; sweepA.C = sweepA.C0; sweepA.A = sweepA.A0; sweepA.LocalCenter = Vector2.Zero; Sweep sweepB = new Sweep(); sweepB.C0 = new Vector2(53.474274f, -50.252514f); sweepB.A0 = 513.36676f; // - 162.0f * MathConstants.Pi; sweepB.C = new Vector2(54.595478f, -51.083473f); sweepB.A = 513.62781f; // - 162.0f * MathConstants.Pi; sweepB.LocalCenter = Vector2.Zero; //sweepB.a0 -= 300.0f * MathConstants.Pi; //sweepB.a -= 300.0f * MathConstants.Pi; TOIInput input = new TOIInput(); input.ProxyA = new DistanceProxy(_shapeA, 0); input.ProxyB = new DistanceProxy(_shapeB, 0); input.SweepA = sweepA; input.SweepB = sweepB; input.TMax = 1.0f; TOIOutput output; TimeOfImpact.CalculateTimeOfImpact(ref input, out output); DrawString("toi = " + output.T); DrawString($"max toi iters = {TimeOfImpact.TOIMaxIters}, max root iters = {TimeOfImpact.TOIMaxRootIters}"); Vector2[] vertices = new Vector2[Settings.MaxPolygonVertices]; DebugView.BeginCustomDraw(ref GameInstance.Projection, ref GameInstance.View); Transform transformA; sweepA.GetTransform(out transformA, 0.0f); for (int i = 0; i < _shapeA.Vertices.Count; ++i) { vertices[i] = MathUtils.Mul(ref transformA, _shapeA.Vertices[i]); } DebugView.DrawPolygon(vertices, _shapeA.Vertices.Count, new Color(0.9f, 0.9f, 0.9f)); Transform transformB; sweepB.GetTransform(out transformB, 0.0f); //b2Vec2 localPoint(2.0f, -0.1f); for (int i = 0; i < _shapeB.Vertices.Count; ++i) { vertices[i] = MathUtils.Mul(ref transformB, _shapeB.Vertices[i]); } DebugView.DrawPolygon(vertices, _shapeB.Vertices.Count, new Color(0.5f, 0.9f, 0.5f)); sweepB.GetTransform(out transformB, output.T); for (int i = 0; i < _shapeB.Vertices.Count; ++i) { vertices[i] = MathUtils.Mul(ref transformB, _shapeB.Vertices[i]); } DebugView.DrawPolygon(vertices, _shapeB.Vertices.Count, new Color(0.5f, 0.7f, 0.9f)); sweepB.GetTransform(out transformB, 1.0f); for (int i = 0; i < _shapeB.Vertices.Count; ++i) { vertices[i] = MathUtils.Mul(ref transformB, _shapeB.Vertices[i]); } DebugView.DrawPolygon(vertices, _shapeB.Vertices.Count, new Color(0.9f, 0.5f, 0.5f)); DebugView.EndCustomDraw(); #if false for (float t = 0.0f; t < 1.0f; t += 0.1f) { sweepB.GetTransform(&transformB, t); for (int i = 0; i < _shapeB.m_count; ++i) { vertices[i] = MathUtils.Mul(transformB, _shapeB.m_vertices[i]); } DebugView.DrawPolygon(vertices, _shapeB.m_count, b2Color(0.9f, 0.5f, 0.5f)); } #endif }
public override void Step(TestSettings settings) { base.Step(settings); Sweep sweepA = new Sweep(); sweepA.c0.Set(24.0f, -60.0f); sweepA.a0 = 2.95f; sweepA.c = sweepA.c0; sweepA.a = sweepA.a0; sweepA.localCenter.SetZero(); Sweep sweepB = new Sweep(); sweepB.c0.Set(53.474274f, -50.252514f); sweepB.a0 = 513.36676f; // - 162.0f * (float)Math.PI; sweepB.c.Set(54.595478f, -51.083473f); sweepB.a = 513.62781f; // - 162.0f * (float)Math.PI; sweepB.localCenter.SetZero(); //sweepB.a0 -= 300.0f * (float)Math.PI; //sweepB.a -= 300.0f * (float)Math.PI; TOIInput input = new TOIInput(); input.proxyA.Set(m_shapeA, 0); input.proxyB.Set(m_shapeB, 0); input.sweepA = sweepA; input.sweepB = sweepB; input.tMax = 1.0f; TOIOutput output; Utilities.TimeOfImpact(out output, input); m_debugDraw.DrawString("toi = {0}", output.t); m_debugDraw.DrawString("max toi iters = {0}, max root iters = {1}", Utilities._toiMaxIters, Utilities._toiMaxRootIters); Vec2[] vertices = new Vec2[Settings._maxPolygonVertices]; Transform transformA; sweepA.GetTransform(out transformA, 0.0f); for (int i = 0; i < m_shapeA.m_count; ++i) { vertices[i] = Utilities.Mul(transformA, m_shapeA.m_vertices[i]); } m_debugDraw.DrawPolygon(vertices, m_shapeA.m_count, Color.FromArgb(225, 225, 225)); Transform transformB; sweepB.GetTransform(out transformB, 0.0f); //Vec2 localPoint(2.0f, -0.1f); for (int i = 0; i < m_shapeB.m_count; ++i) { vertices[i] = Utilities.Mul(transformB, m_shapeB.m_vertices[i]); } m_debugDraw.DrawPolygon(vertices, m_shapeB.m_count, Color.FromArgb(128, 225, 128)); sweepB.GetTransform(out transformB, output.t); for (int i = 0; i < m_shapeB.m_count; ++i) { vertices[i] = Utilities.Mul(transformB, m_shapeB.m_vertices[i]); } m_debugDraw.DrawPolygon(vertices, m_shapeB.m_count, Color.FromArgb(128, 175, 225)); sweepB.GetTransform(out transformB, 1.0f); for (int i = 0; i < m_shapeB.m_count; ++i) { vertices[i] = Utilities.Mul(transformB, m_shapeB.m_vertices[i]); } m_debugDraw.DrawPolygon(vertices, m_shapeB.m_count, Color.FromArgb(225, 128, 128)); #if ZERO for (float t = 0.0f; t < 1.0f; t += 0.1f) { sweepB.GetTransform(out transformB, t); for (int i = 0; i < m_shapeB.m_count; ++i) { vertices[i] = Utilities.Mul(transformB, m_shapeB.m_vertices[i]); } m_debugDraw.DrawPolygon(vertices, m_shapeB.m_count, Color.FromArgb(225, 0.5f, 0.5f)); } #endif }
public override void Step(Framework.Settings settings) { base.Step(settings); Sweep sweepA = new Sweep(); sweepA.c0 = new Vector2(24.0f, -60.0f); sweepA.a0 = 2.95f; sweepA.c = sweepA.c0; sweepA.a = sweepA.a0; sweepA.localCenter = Vector2.Zero; Sweep sweepB = new Sweep(); sweepB.c0 = new Vector2(53.474274f, -50.252514f); sweepB.a0 = 513.36676f; sweepB.c = new Vector2(54.595478f, -51.083473f); sweepB.a = 513.62781f; sweepB.localCenter = Vector2.Zero; TOIInput input = new TOIInput(); input.proxyA.Set(_shapeA, 0); input.proxyB.Set(_shapeB, 0); input.sweepA = sweepA; input.sweepB = sweepB; input.tMax = 1.0f; TOIOutput output; XNA.TimeOfImpact.CalculateTimeOfImpact(out output, ref input); _debugDraw.DrawString(50, _textLine, "toi = {0:n}", output.t); _textLine += 15; _debugDraw.DrawString(50, _textLine, "max toi iters = {0:n}, max root iters = {1:n}", XNA.TimeOfImpact.b2_toiMaxIters, XNA.TimeOfImpact.b2_toiMaxRootIters); _textLine += 15; FixedArray8<Vector2> vertices = new FixedArray8<Vector2>(); Transform transformA; sweepA.GetTransform(out transformA, 0.0f); for (int i = 0; i < _shapeA._vertexCount; ++i) { vertices[i] = MathUtils.Multiply(ref transformA, _shapeA._vertices[i]); } _debugDraw.DrawPolygon(ref vertices, _shapeA._vertexCount, new Color(0.9f, 0.9f, 0.9f)); Transform transformB; sweepB.GetTransform(out transformB, 0.0f); Vector2 localPoint = new Vector2(2.0f, -0.1f); Vector2 rB = MathUtils.Multiply(ref transformB, localPoint) - sweepB.c0; float wB = sweepB.a - sweepB.a0; Vector2 vB = sweepB.c - sweepB.c0; Vector2 v = vB + MathUtils.Cross(wB, rB); for (int i = 0; i < _shapeB._vertexCount; ++i) { vertices[i] = MathUtils.Multiply(ref transformB, _shapeB._vertices[i]); } _debugDraw.DrawPolygon(ref vertices, _shapeB._vertexCount, new Color(0.5f, 0.9f, 0.5f)); sweepB.GetTransform(out transformB, output.t); for (int i = 0; i < _shapeB._vertexCount; ++i) { vertices[i] = MathUtils.Multiply(ref transformB, _shapeB._vertices[i]); } _debugDraw.DrawPolygon(ref vertices, _shapeB._vertexCount, new Color(0.5f, 0.7f, 0.9f)); sweepB.GetTransform(out transformB, 1.0f); for (int i = 0; i < _shapeB._vertexCount; ++i) { vertices[i] = MathUtils.Multiply(ref transformB, _shapeB._vertices[i]); } _debugDraw.DrawPolygon(ref vertices, _shapeB._vertexCount, new Color(0.9f, 0.5f, 0.5f)); }
private void SolveTOI(ref TimeStep step) { Island.Reset(2 * Settings.MaxTOIContacts, Settings.MaxTOIContacts, 0, ContactManager); if (_stepComplete) { for (var i = 0; i < BodyList.Count; i++) { BodyList[i]._flags &= ~BodyFlags.IslandFlag; BodyList[i]._sweep.Alpha0 = 0.0f; } for (var i = 0; i < ContactManager.ContactList.Count; i++) { var c = ContactManager.ContactList[i]; // Invalidate TOI c._flags &= ~ContactFlags.IslandFlag; c._flags &= ~ContactFlags.TOIFlag; c._toiCount = 0; c._toi = 1.0f; } } // Find TOI events and solve them. for (;;) { // Find the first TOI. Contact minContact = null; var minAlpha = 1.0f; for (var i = 0; i < ContactManager.ContactList.Count; i++) { var c = ContactManager.ContactList[i]; // Is this contact disabled? if (c.Enabled == false) { continue; } // Prevent excessive sub-stepping. if (c._toiCount > Settings.MaxSubSteps) { continue; } float alpha; if (c.TOIFlag) { // This contact has a valid cached TOI. alpha = c._toi; } else { var fA = c.FixtureA; var fB = c.FixtureB; // Is there a sensor? if (fA.IsSensor || fB.IsSensor) { continue; } var bA = fA.Body; var bB = fB.Body; var typeA = bA.BodyType; var typeB = bB.BodyType; Debug.Assert(typeA == BodyType.Dynamic || typeB == BodyType.Dynamic); var activeA = bA.Awake && typeA != BodyType.Static; var activeB = bB.Awake && typeB != BodyType.Static; // Is at least one body active (awake and dynamic or kinematic)? if (activeA == false && activeB == false) { continue; } var collideA = (bA.IsBullet || typeA != BodyType.Dynamic) && (fA.IgnoreCCDWith & fB.CollisionCategories) == 0 && !bA.IgnoreCCD; var collideB = (bB.IsBullet || typeB != BodyType.Dynamic) && (fB.IgnoreCCDWith & fA.CollisionCategories) == 0 && !bB.IgnoreCCD; // Are these two non-bullet dynamic bodies? if (collideA == false && collideB == false) { continue; } // Compute the TOI for this contact. // Put the sweeps onto the same time interval. var alpha0 = bA._sweep.Alpha0; if (bA._sweep.Alpha0 < bB._sweep.Alpha0) { alpha0 = bB._sweep.Alpha0; bA._sweep.Advance(alpha0); } else if (bB._sweep.Alpha0 < bA._sweep.Alpha0) { alpha0 = bA._sweep.Alpha0; bB._sweep.Advance(alpha0); } Debug.Assert(alpha0 < 1.0f); // Compute the time of impact in interval [0, minTOI] var input = new TOIInput(); input.ProxyA = new DistanceProxy(fA.Shape, c.ChildIndexA); input.ProxyB = new DistanceProxy(fB.Shape, c.ChildIndexB); input.SweepA = bA._sweep; input.SweepB = bB._sweep; input.TMax = 1.0f; TOIOutput output; TimeOfImpact.CalculateTimeOfImpact(ref input, out output); // Beta is the fraction of the remaining portion of the . var beta = output.T; if (output.State == TOIOutputState.Touching) { alpha = Mathf.Min(alpha0 + (1.0f - alpha0) * beta, 1.0f); } else { alpha = 1.0f; } c._toi = alpha; c._flags &= ~ContactFlags.TOIFlag; } if (alpha < minAlpha) { // This is the minimum TOI found so far. minContact = c; minAlpha = alpha; } } if (minContact == null || 1.0f - 10.0f * Settings.Epsilon < minAlpha) { // No more TOI events. Done! _stepComplete = true; break; } // Advance the bodies to the TOI. var fA1 = minContact.FixtureA; var fB1 = minContact.FixtureB; var bA0 = fA1.Body; var bB0 = fB1.Body; var backup1 = bA0._sweep; var backup2 = bB0._sweep; bA0.Advance(minAlpha); bB0.Advance(minAlpha); // The TOI contact likely has some new contact points. minContact.Update(ContactManager); minContact._flags &= ~ContactFlags.TOIFlag; ++minContact._toiCount; // Is the contact solid? if (minContact.Enabled == false || minContact.IsTouching == false) { // Restore the sweeps. minContact._flags &= ~ContactFlags.EnabledFlag; bA0._sweep = backup1; bB0._sweep = backup2; bA0.SynchronizeTransform(); bB0.SynchronizeTransform(); continue; } bA0.Awake = true; bB0.Awake = true; // Build the island Island.Clear(); Island.Add(bA0); Island.Add(bB0); Island.Add(minContact); bA0._flags |= BodyFlags.IslandFlag; bB0._flags |= BodyFlags.IslandFlag; minContact._flags &= ~ContactFlags.IslandFlag; // Get contacts on bodyA and bodyB. Body[] bodies = { bA0, bB0 }; for (var i = 0; i < 2; ++i) { var body = bodies[i]; if (body.BodyType == BodyType.Dynamic) { for (var ce = body.ContactList; ce != null; ce = ce.Next) { var contact = ce.Contact; if (Island.BodyCount == Island.BodyCapacity) { break; } if (Island.ContactCount == Island.ContactCapacity) { break; } // Has this contact already been added to the island? if (contact.IslandFlag) { continue; } // Only add static, kinematic, or bullet bodies. var other = ce.Other; if (other.BodyType == BodyType.Dynamic && body.IsBullet == false && other.IsBullet == false) { continue; } // Skip sensors. if (contact.FixtureA.IsSensor || contact.FixtureB.IsSensor) { continue; } // Tentatively advance the body to the TOI. var backup = other._sweep; if (!other.IsIsland) { other.Advance(minAlpha); } // Update the contact points contact.Update(ContactManager); // Was the contact disabled by the user? if (contact.Enabled == false) { other._sweep = backup; other.SynchronizeTransform(); continue; } // Are there contact points? if (contact.IsTouching == false) { other._sweep = backup; other.SynchronizeTransform(); continue; } // Add the contact to the island minContact._flags |= ContactFlags.IslandFlag; Island.Add(contact); // Has the other body already been added to the island? if (other.IsIsland) { continue; } // Add the other body to the island. other._flags |= BodyFlags.IslandFlag; if (other.BodyType != BodyType.Static) { other.Awake = true; } Island.Add(other); } } } TimeStep subStep; subStep.dt = (1.0f - minAlpha) * step.dt; subStep.inv_dt = 1.0f / subStep.dt; subStep.dtRatio = 1.0f; Island.SolveTOI(ref subStep, bA0.IslandIndex, bB0.IslandIndex); // Reset island flags and synchronize broad-phase proxies. for (var i = 0; i < Island.BodyCount; ++i) { var body = Island.Bodies[i]; body._flags &= ~BodyFlags.IslandFlag; if (body.BodyType != BodyType.Dynamic) { continue; } body.SynchronizeFixtures(); // Invalidate all contact TOIs on this displaced body. for (var ce = body.ContactList; ce != null; ce = ce.Next) { ce.Contact._flags &= ~ContactFlags.TOIFlag; ce.Contact._flags &= ~ContactFlags.IslandFlag; } } // Commit fixture proxy movements to the broad-phase so that new contacts are created. // Also, some contacts can be destroyed. ContactManager.FindNewContacts(); if (Settings.EnableSubStepping) { _stepComplete = false; break; } } }
public override void Step(Framework.Settings settings) { base.Step(settings); Sweep sweepA = new Sweep(); sweepA.c0 = new Vector2(24.0f, -60.0f); sweepA.a0 = 2.95f; sweepA.c = sweepA.c0; sweepA.a = sweepA.a0; sweepA.localCenter = Vector2.Zero; Sweep sweepB = new Sweep(); sweepB.c0 = new Vector2(53.474274f, -50.252514f); sweepB.a0 = 513.36676f; sweepB.c = new Vector2(54.595478f, -51.083473f); sweepB.a = 513.62781f; sweepB.localCenter = Vector2.Zero; TOIInput input = new TOIInput(); input.proxyA.Set(_shapeA, 0); input.proxyB.Set(_shapeB, 0); input.sweepA = sweepA; input.sweepB = sweepB; input.tMax = 1.0f; TOIOutput output; Alt.Box2D.TimeOfImpact.CalculateTimeOfImpact(out output, ref input); _debugDraw.DrawString(50, _textLine, "toi = {0:n}", output.t); _textLine += 15; _debugDraw.DrawString(50, _textLine, "max toi iters = {0:n}, max root iters = {1:n}", Alt.Box2D.TimeOfImpact.b2_toiMaxIters, Alt.Box2D.TimeOfImpact.b2_toiMaxRootIters); _textLine += 15; FixedArray8 <Vector2> vertices = new FixedArray8 <Vector2>(); Alt.Box2D.Transform transformA; sweepA.GetTransform(out transformA, 0.0f); for (int i = 0; i < _shapeA._vertexCount; ++i) { vertices[i] = MathUtils.Multiply(ref transformA, _shapeA._vertices[i]); } _debugDraw.DrawPolygon(ref vertices, _shapeA._vertexCount, new ColorR(0.9, 0.9, 0.9)); Alt.Box2D.Transform transformB; sweepB.GetTransform(out transformB, 0.0f); //NoNeed Vector2 localPoint = new Vector2(2.0f, -0.1f); //NoNeed Vector2 rB = MathUtils.Multiply(ref transformB, localPoint) - sweepB.c0; //NoNeed double wB = sweepB.a - sweepB.a0; //NoNeed Vector2 vB = sweepB.c - sweepB.c0; //Vector2 v = vB + MathUtils.Cross(wB, rB); for (int i = 0; i < _shapeB._vertexCount; ++i) { vertices[i] = MathUtils.Multiply(ref transformB, _shapeB._vertices[i]); } _debugDraw.DrawPolygon(ref vertices, _shapeB._vertexCount, new ColorR(0.5, 0.9, 0.5)); sweepB.GetTransform(out transformB, output.t); for (int i = 0; i < _shapeB._vertexCount; ++i) { vertices[i] = MathUtils.Multiply(ref transformB, _shapeB._vertices[i]); } _debugDraw.DrawPolygon(ref vertices, _shapeB._vertexCount, new ColorR(0.5f, 0.7f, 0.9f)); sweepB.GetTransform(out transformB, 1.0f); for (int i = 0; i < _shapeB._vertexCount; ++i) { vertices[i] = MathUtils.Multiply(ref transformB, _shapeB._vertices[i]); } _debugDraw.DrawPolygon(ref vertices, _shapeB._vertexCount, new ColorR(0.9, 0.5, 0.5)); }