/// <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">The output.</param> /// <param name="input">The input.</param> public static void CalculateTimeOfImpact(out TOIOutput output, TOIInput input) { ++TOICalls; output = new TOIOutput(); output.State = TOIOutputState.Unknown; output.T = input.TMax; Sweep sweepA = input.SweepA; Sweep sweepB = 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 = input.ProxyA.Radius + input.ProxyB.Radius; float target = Math.Max(Settings.LinearSlop, totalRadius - 3.0f * Settings.LinearSlop); const float tolerance = 0.25f * Settings.LinearSlop; Debug.Assert(target > tolerance); float t1 = 0.0f; const int k_maxIterations = 20; int iter = 0; // Prepare input for distance query. SimplexCache cache; _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 (; ; ) { Transform xfA, xfB; sweepA.GetTransform(out xfA, t1); sweepB.GetTransform(out xfB, t1); // Get the distance between shapes. We can also use the results // to get a separating axis. _distanceInput.TransformA = xfA; _distanceInput.TransformB = xfB; DistanceOutput distanceOutput; Distance.ComputeDistance(out distanceOutput, out cache, _distanceInput); // If the shapes are overlapped, we give up on continuous collision. if (distanceOutput.Distance <= 0.0f) { // Failure! output.State = TOIOutputState.Overlapped; output.T = 0.0f; break; } if (distanceOutput.Distance < target + tolerance) { // Victory! output.State = TOIOutputState.Touching; output.T = t1; break; } SeparationFunction.Set(ref cache, input.ProxyA, ref sweepA, input.ProxyB, ref 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. int indexA, indexB; float s2 = SeparationFunction.FindMinSeparation(out indexA, out indexB, t2); // Is the final configuration separated? if (s2 > target + tolerance) { // Victory! output.State = TOIOutputState.Seperated; output.T = tMax; done = true; break; } // Has the separation reached tolerance? if (s2 > target - tolerance) { // Advance the sweeps t1 = t2; break; } // Compute the initial separation of the witness points. float s1 = SeparationFunction.Evaluate(indexA, indexB, t1); // Check for initial overlap. This might happen if the root finder // runs out of iterations. if (s1 < target - tolerance) { output.State = TOIOutputState.Failed; output.T = t1; done = true; break; } // Check for touching if (s1 <= target + tolerance) { // 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) != 0) { // 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 = SeparationFunction.Evaluate(indexA, indexB, t); if (Math.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; if (rootIterCount == 50) { break; } } TOIMaxRootIters = Math.Max(TOIMaxRootIters, rootIterCount); ++pushBackIter; if (pushBackIter == Settings.MaxPolygonVertices) { break; } } ++iter; ++TOIIters; if (done) { break; } if (iter == k_maxIterations) { // Root finder got stuck. Semi-victory. output.State = TOIOutputState.Failed; output.T = t1; break; } } TOIMaxIters = Math.Max(TOIMaxIters, iter); }
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> /// 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> /// 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">The output.</param> /// <param name="input">The input.</param> public static void CalculateTimeOfImpact(out TOIOutput output, ref TOIInput input) { ++TOICalls; output = new TOIOutput(); output.State = TOIOutputState.Unknown; output.T = input.TMax; Sweep sweepA = input.SweepA; Sweep sweepB = 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 = input.ProxyA.Radius + input.ProxyB.Radius; float target = Math.Max(Settings.LinearSlop, totalRadius - 3.0f * Settings.LinearSlop); const float tolerance = 0.25f * Settings.LinearSlop; Debug.Assert(target > tolerance); float t1 = 0.0f; const int k_maxIterations = 20; int iter = 0; // Prepare input for distance query. SimplexCache cache; DistanceInput distanceInput; 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 (;;) { Transform xfA, xfB; sweepA.GetTransform(out xfA, t1); sweepB.GetTransform(out xfB, t1); // Get the distance between shapes. We can also use the results // to get a separating axis. distanceInput.TransformA = xfA; distanceInput.TransformB = xfB; DistanceOutput distanceOutput; Distance.ComputeDistance(out distanceOutput, out cache, ref distanceInput); // If the shapes are overlapped, we give up on continuous collision. if (distanceOutput.Distance <= 0.0f) { // Failure! output.State = TOIOutputState.Overlapped; output.T = 0.0f; break; } if (distanceOutput.Distance < target + tolerance) { // Victory! output.State = TOIOutputState.Touching; output.T = t1; break; } SeparationFunction fcn = new SeparationFunction(ref cache, ref input.ProxyA, ref sweepA, ref input.ProxyB, ref 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. int indexA, indexB; float s2 = fcn.FindMinSeparation(out indexA, out indexB, t2); // Is the final configuration separated? if (s2 > target + tolerance) { // Victory! output.State = TOIOutputState.Seperated; output.T = tMax; done = true; break; } // Has the separation reached tolerance? if (s2 > target - tolerance) { // Advance the sweeps t1 = t2; break; } // Compute the initial separation of the witness points. float s1 = fcn.Evaluate(indexA, indexB, t1); // Check for initial overlap. This might happen if the root finder // runs out of iterations. if (s1 < target - tolerance) { output.State = TOIOutputState.Failed; output.T = t1; done = true; break; } // Check for touching if (s1 <= target + tolerance) { // 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) != 0) { // 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(indexA, indexB, t); if (Math.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; if (rootIterCount == 50) { break; } } TOIMaxRootIters = Math.Max(TOIMaxRootIters, rootIterCount); ++pushBackIter; if (pushBackIter == Settings.MaxPolygonVertices) { break; } } ++iter; ++TOIIters; if (done) { break; } if (iter == k_maxIterations) { // Root finder got stuck. Semi-victory. output.State = TOIOutputState.Failed; output.T = t1; break; } } TOIMaxIters = Math.Max(TOIMaxIters, iter); }
/// <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; } }