/// 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. // CCD via the local separating axis method. This seeks progression // by computing the largest time at which separation is maintained. public static void TimeOfImpact(out TOIOutput output, TOIInput input){ Timer timer = new Timer(); ++_toiCalls; output.state = TOIOutput.State.e_unknown; output.t = input.tMax; DistanceProxy proxyA = input.proxyA; DistanceProxy proxyB = input.proxyB; 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 = proxyA.m_radius + proxyB.m_radius; float target = Math.Max(Settings._linearSlop, totalRadius - 3.0f * Settings._linearSlop); float tolerance = 0.25f * Settings._linearSlop; Utilities.Assert(target > tolerance); float t1 = 0.0f; const int k_maxIterations = 20; // TODO_ERIN Settings int iter = 0; // Prepare input for distance query. SimplexCache cache = new SimplexCache(); cache.count = 0; 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; Utilities.Distance(out distanceOutput, cache, distanceInput); // If the shapes are overlapped, we give up on continuous collision. if (distanceOutput.distance <= 0.0f) { // Failure! output.state = TOIOutput.State.e_overlapped; output.t = 0.0f; break; } if (distanceOutput.distance < target + tolerance) { // Victory! output.state = TOIOutput.State.e_touching; output.t = t1; break; } // Initialize the separating axis. throw new NotImplementedException(); // SeparationFunction fcn; // fcn.Initialize(&cache, proxyA, sweepA, proxyB, sweepB, t1); //#if ZERO // // Dump the curve seen by the root finder // { // const int N = 100; // float dx = 1.0f / N; // float xs[N+1]; // float fs[N+1]; // float x = 0.0f; // for (int i = 0; i <= N; ++i) // { // sweepA.GetTransform(out xfA, x); // sweepB.GetTransform(out xfB, x); // float f = fcn.Evaluate(xfA, xfB) - target; // printf("%g %g\n", x, f); // xs[i] = x; // fs[i] = f; // x += dx; // } // } //#endif // // 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(&indexA, &indexB, t2); // // Is the final configuration separated? // if (s2 > target + tolerance) // { // // Victory! // output.state = TOIOutput.State.e_separated; // 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 = TOIOutput.State.e_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 = TOIOutput.State.e_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) // { // // Secant rule to improve convergence. // t = a1 + (target - s1) * (a2 - a1) / (s2 - s1); // } // else // { // // Bisection to guarantee progress. // t = 0.5f * (a1 + a2); // } // ++rootIterCount; // ++_toiRootIters; // 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; // } // 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 = TOIOutput.State.e_failed; // output.t = t1; // break; // } } _toiMaxIters = Math.Max(_toiMaxIters, iter); float time = timer.GetMilliseconds(); _toiMaxTime = Math.Max(_toiMaxTime, time); _toiTime += time; }
// CCD via the local separating axis method. This seeks progression // by computing the largest time at which separation is maintained. /// 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 b2Distance to compute the contact point and normal at the time of impact. public static void CalculateTimeOfImpact(out TOIOutput output, ref TOIInput input) { ++b2_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.b2_linearSlop, totalRadius - 3.0f * Settings.b2_linearSlop); float tolerance = 0.25f * Settings.b2_linearSlop; //Debug.Assert(target > tolerance); float t1 = 0.0f; 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; ++b2_toiRootIters; if (rootIterCount == 50) { break; } } b2_toiMaxRootIters = Math.Max(b2_toiMaxRootIters, rootIterCount); ++pushBackIter; if (pushBackIter == Settings.b2_maxPolygonVertices) { break; } } ++iter; ++b2_toiIters; if (done) { break; } if (iter == k_maxIterations) { // Root finder got stuck. Semi-victory. output.State = TOIOutputState.Failed; output.t = t1; break; } } b2_toiMaxIters = Math.Max(b2_toiMaxIters, iter); }
private void SolveTOI(TimeStep step){ Island island = new Island(m_contactManager.m_contactListener); if (m_stepComplete) { foreach (Body b in m_bodyList) { b.m_flags &= ~Body.BodyFlags.e_islandFlag; b.m_sweep.alpha0 = 0.0f; } foreach (Contact c in m_contactManager.m_contactList) { // Invalidate TOI c.m_flags &= ~(ContactFlags.e_toiFlag | ContactFlags.e_islandFlag); c.m_toiCount = 0; c.m_toi = 1.0f; } } Fixture fA = null; Fixture fB = null; Body bA = null; Body bB = null; // Find TOI events and solve them. for (;;) { // Find the first TOI. Contact minContact = null; float minAlpha = 1.0f; foreach (Contact c in m_contactManager.m_contactList) { // Is this contact disabled? if (c.IsEnabled() == false) { continue; } // Prevent excessive sub-stepping. if (c.m_toiCount > Settings._maxSubSteps) { continue; } float alpha = 1.0f; if (c.m_flags.HasFlag(ContactFlags.e_toiFlag)) { // This contact has a valid cached TOI. alpha = c.m_toi; } else { fA = c.FixtureA; fB = c.FixtureB; // Is there a sensor? if (fA.IsSensor || fB.IsSensor) { continue; } bA = fA.GetBody(); bB = fB.GetBody(); BodyType typeA = bA.m_type; BodyType typeB = bB.m_type; Utilities.Assert(typeA == BodyType._dynamicBody || typeB == BodyType._dynamicBody); bool activeA = bA.IsAwake() && typeA != BodyType._staticBody; bool activeB = bB.IsAwake() && typeB != BodyType._staticBody; // Is at least one body active (awake and dynamic or kinematic)? if (activeA == false && activeB == false) { continue; } bool collideA = bA.IsBullet() || typeA != BodyType._dynamicBody; bool collideB = bB.IsBullet() || typeB != BodyType._dynamicBody; // 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.m_sweep.alpha0; if (bA.m_sweep.alpha0 < bB.m_sweep.alpha0) { alpha0 = bB.m_sweep.alpha0; bA.m_sweep.Advance(alpha0); } else if (bB.m_sweep.alpha0 < bA.m_sweep.alpha0) { alpha0 = bA.m_sweep.alpha0; bB.m_sweep.Advance(alpha0); } Utilities.Assert(alpha0 < 1.0f); int indexA = c.GetChildIndexA(); int indexB = c.GetChildIndexB(); // Compute the time of impact in interval [0, minTOI] TOIInput input = new TOIInput(); input.proxyA.Set(fA.GetShape(), indexA); input.proxyB.Set(fB.GetShape(), indexB); input.sweepA = bA.m_sweep; input.sweepB = bB.m_sweep; input.tMax = 1.0f; TOIOutput output; Utilities.TimeOfImpact(out output, input); // Beta is the fraction of the remaining portion of the . float beta = output.t; if (output.state == TOIOutput.State.e_touching) { alpha = Math.Min(alpha0 + (1.0f - alpha0) * beta, 1.0f); } else { alpha = 1.0f; } c.m_toi = alpha; c.m_flags |= ContactFlags.e_toiFlag; } if (alpha < minAlpha) { // This is the minimum TOI found so far. minContact = c; minAlpha = alpha; } } if (minContact == null || 1.0f - 10.0f * Single.Epsilon < minAlpha) { // No more TOI events. Done! m_stepComplete = true; break; } // Advance the bodies to the TOI. fA = minContact.FixtureA; fB = minContact.FixtureB; bA = fA.GetBody(); bB = fB.GetBody(); Sweep backup1 = bA.m_sweep; Sweep backup2 = bB.m_sweep; bA.Advance(minAlpha); bB.Advance(minAlpha); // The TOI contact likely has some new contact points. minContact.Update(m_contactManager.m_contactListener); minContact.m_flags &= ~ContactFlags.e_toiFlag; ++minContact.m_toiCount; // Is the contact solid? if (minContact.IsEnabled() == false || minContact.IsTouching() == false) { // Restore the sweeps. minContact.SetEnabled(false); bA.m_sweep = backup1; bB.m_sweep = backup2; bA.SynchronizeTransform(); bB.SynchronizeTransform(); continue; } bA.SetAwake(true); bB.SetAwake(true); // Build the island island.Clear(); island.Add(bA); island.Add(bB); island.Add(minContact); bA.m_flags |= Body.BodyFlags.e_islandFlag; bB.m_flags |= Body.BodyFlags.e_islandFlag; minContact.m_flags |= ContactFlags.e_islandFlag; // Get contacts on bodyA and bodyB. Body[] bodies = {bA, bB}; for (int i = 0; i < 2; ++i) { Body body = bodies[i]; if (body.m_type == BodyType._dynamicBody) { foreach (ContactEdge ce in body.m_contactList) { throw new NotImplementedException(); //if (island.m_bodies.Count() == island.m_bodyCapacity) //{ // break; //} //if (island.m_bodies.Count() == island.m_contactCapacity) //{ // break; //} //Contact* contact = ce.contact; //// Has this contact already been added to the island? //if (contact.m_flags & ContactFlags.e_islandFlag) //{ // continue; //} //// Only add static, kinematic, or bullet bodies. //Body* other = ce.other; //if (other.m_type == _dynamicBody && // body.IsBullet() == false && other.IsBullet() == false) //{ // continue; //} //// Skip sensors. //bool sensorA = contact.m_fixtureA.m_isSensor; //bool sensorB = contact.m_fixtureB.m_isSensor; //if (sensorA || sensorB) //{ // continue; //} //// Tentatively advance the body to the TOI. //Sweep backup = other.m_sweep; //if ((other.m_flags & Body.BodyFlags.e_islandFlag) == 0) //{ // other.Advance(minAlpha); //} //// Update the contact points //contact.Update(m_contactManager.m_contactListener); //// Was the contact disabled by the user? //if (contact.IsEnabled() == false) //{ // other.m_sweep = backup; // other.SynchronizeTransform(); // continue; //} //// Are there contact points? //if (contact.IsTouching() == false) //{ // other.m_sweep = backup; // other.SynchronizeTransform(); // continue; //} //// Add the contact to the island //contact.m_flags |= ContactFlags.e_islandFlag; //island.Add(contact); //// Has the other body already been added to the island? //if (other.m_flags & Body.BodyFlags.e_islandFlag) //{ // continue; //} //// Add the other body to the island. //other.m_flags |= Body.BodyFlags.e_islandFlag; //if (other.m_type != _staticBody) //{ // other.SetAwake(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(subStep, bA.m_islandIndex, bB.m_islandIndex); // Reset island flags and synchronize broad-phase proxies. for (int i = 0; i < island.m_bodies.Count(); ++i) { throw new NotImplementedException(); //Body* body = island.m_bodies[i]; //body.m_flags &= ~Body.BodyFlags.e_islandFlag; //if (body.m_type != _dynamicBody) //{ // continue; //} //body.SynchronizeFixtures(); //// Invalidate all contact TOIs on this displaced body. //for (ContactEdge* ce = body.m_contactList; ce; ce = ce.next) //{ // ce.contact.m_flags &= ~(ContactFlags.e_toiFlag | ContactFlags.e_islandFlag); //} } // Commit fixture proxy movements to the broad-phase so that new contacts are created. // Also, some contacts can be destroyed. m_contactManager.FindNewContacts(); if (m_subStepping) { m_stepComplete = false; break; } } }
// Advance a dynamic body to its first time of contact // and adjust the position to ensure clearance. 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.GetType(); // Only bullets perform TOI with dynamic bodies. if (bullet == true) { // 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.IsEnabled() == false) { continue; } // Prevent infinite looping. if (contact._toiCount > 10) { continue; } Fixture fixtureA = contact._fixtureA; Fixture fixtureB = contact._fixtureB; int indexA = contact._indexA; int indexB = contact._indexB; // 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.GetShape(), indexA); input.proxyB.Set(fixtureB.GetShape(), 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.ContactListener); if (toiContact.IsEnabled() == 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.b2_maxTOIContacts); ce = ce.Next) { Body other = ce.Other; BodyType type = other.GetType(); // 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.IsEnabled() == 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.ContactListener); } // Did the user disable the contact? if (contact.IsEnabled() == 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); 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.GetType() != BodyType.Static) { toiContact._flags |= ContactFlags.BulletHit; } }
private void SolveTOI(TimeStep step) { Island island = new Island(m_contactManager.m_contactListener); if (m_stepComplete) { foreach (Body b in m_bodyList) { b.m_flags &= ~Body.BodyFlags.e_islandFlag; b.m_sweep.alpha0 = 0.0f; } foreach (Contact c in m_contactManager.m_contactList) { // Invalidate TOI c.m_flags &= ~(ContactFlags.e_toiFlag | ContactFlags.e_islandFlag); c.m_toiCount = 0; c.m_toi = 1.0f; } } Fixture fA = null; Fixture fB = null; Body bA = null; Body bB = null; // Find TOI events and solve them. for (;;) { // Find the first TOI. Contact minContact = null; float minAlpha = 1.0f; foreach (Contact c in m_contactManager.m_contactList) { // Is this contact disabled? if (c.IsEnabled() == false) { continue; } // Prevent excessive sub-stepping. if (c.m_toiCount > Settings._maxSubSteps) { continue; } float alpha = 1.0f; if (c.m_flags.HasFlag(ContactFlags.e_toiFlag)) { // This contact has a valid cached TOI. alpha = c.m_toi; } else { fA = c.FixtureA; fB = c.FixtureB; // Is there a sensor? if (fA.IsSensor || fB.IsSensor) { continue; } bA = fA.GetBody(); bB = fB.GetBody(); BodyType typeA = bA.m_type; BodyType typeB = bB.m_type; Utilities.Assert(typeA == BodyType._dynamicBody || typeB == BodyType._dynamicBody); bool activeA = bA.IsAwake() && typeA != BodyType._staticBody; bool activeB = bB.IsAwake() && typeB != BodyType._staticBody; // Is at least one body active (awake and dynamic or kinematic)? if (activeA == false && activeB == false) { continue; } bool collideA = bA.IsBullet() || typeA != BodyType._dynamicBody; bool collideB = bB.IsBullet() || typeB != BodyType._dynamicBody; // 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.m_sweep.alpha0; if (bA.m_sweep.alpha0 < bB.m_sweep.alpha0) { alpha0 = bB.m_sweep.alpha0; bA.m_sweep.Advance(alpha0); } else if (bB.m_sweep.alpha0 < bA.m_sweep.alpha0) { alpha0 = bA.m_sweep.alpha0; bB.m_sweep.Advance(alpha0); } Utilities.Assert(alpha0 < 1.0f); int indexA = c.GetChildIndexA(); int indexB = c.GetChildIndexB(); // Compute the time of impact in interval [0, minTOI] TOIInput input = new TOIInput(); input.proxyA.Set(fA.GetShape(), indexA); input.proxyB.Set(fB.GetShape(), indexB); input.sweepA = bA.m_sweep; input.sweepB = bB.m_sweep; input.tMax = 1.0f; TOIOutput output; Utilities.TimeOfImpact(out output, input); // Beta is the fraction of the remaining portion of the . float beta = output.t; if (output.state == TOIOutput.State.e_touching) { alpha = Math.Min(alpha0 + (1.0f - alpha0) * beta, 1.0f); } else { alpha = 1.0f; } c.m_toi = alpha; c.m_flags |= ContactFlags.e_toiFlag; } if (alpha < minAlpha) { // This is the minimum TOI found so far. minContact = c; minAlpha = alpha; } } if (minContact == null || 1.0f - 10.0f * Single.Epsilon < minAlpha) { // No more TOI events. Done! m_stepComplete = true; break; } // Advance the bodies to the TOI. fA = minContact.FixtureA; fB = minContact.FixtureB; bA = fA.GetBody(); bB = fB.GetBody(); Sweep backup1 = bA.m_sweep; Sweep backup2 = bB.m_sweep; bA.Advance(minAlpha); bB.Advance(minAlpha); // The TOI contact likely has some new contact points. minContact.Update(m_contactManager.m_contactListener); minContact.m_flags &= ~ContactFlags.e_toiFlag; ++minContact.m_toiCount; // Is the contact solid? if (minContact.IsEnabled() == false || minContact.IsTouching() == false) { // Restore the sweeps. minContact.SetEnabled(false); bA.m_sweep = backup1; bB.m_sweep = backup2; bA.SynchronizeTransform(); bB.SynchronizeTransform(); continue; } bA.SetAwake(true); bB.SetAwake(true); // Build the island island.Clear(); island.Add(bA); island.Add(bB); island.Add(minContact); bA.m_flags |= Body.BodyFlags.e_islandFlag; bB.m_flags |= Body.BodyFlags.e_islandFlag; minContact.m_flags |= ContactFlags.e_islandFlag; // Get contacts on bodyA and bodyB. Body[] bodies = { bA, bB }; for (int i = 0; i < 2; ++i) { Body body = bodies[i]; if (body.m_type == BodyType._dynamicBody) { foreach (ContactEdge ce in body.m_contactList) { throw new NotImplementedException(); //if (island.m_bodies.Count() == island.m_bodyCapacity) //{ // break; //} //if (island.m_bodies.Count() == island.m_contactCapacity) //{ // break; //} //Contact* contact = ce.contact; //// Has this contact already been added to the island? //if (contact.m_flags & ContactFlags.e_islandFlag) //{ // continue; //} //// Only add static, kinematic, or bullet bodies. //Body* other = ce.other; //if (other.m_type == _dynamicBody && // body.IsBullet() == false && other.IsBullet() == false) //{ // continue; //} //// Skip sensors. //bool sensorA = contact.m_fixtureA.m_isSensor; //bool sensorB = contact.m_fixtureB.m_isSensor; //if (sensorA || sensorB) //{ // continue; //} //// Tentatively advance the body to the TOI. //Sweep backup = other.m_sweep; //if ((other.m_flags & Body.BodyFlags.e_islandFlag) == 0) //{ // other.Advance(minAlpha); //} //// Update the contact points //contact.Update(m_contactManager.m_contactListener); //// Was the contact disabled by the user? //if (contact.IsEnabled() == false) //{ // other.m_sweep = backup; // other.SynchronizeTransform(); // continue; //} //// Are there contact points? //if (contact.IsTouching() == false) //{ // other.m_sweep = backup; // other.SynchronizeTransform(); // continue; //} //// Add the contact to the island //contact.m_flags |= ContactFlags.e_islandFlag; //island.Add(contact); //// Has the other body already been added to the island? //if (other.m_flags & Body.BodyFlags.e_islandFlag) //{ // continue; //} //// Add the other body to the island. //other.m_flags |= Body.BodyFlags.e_islandFlag; //if (other.m_type != _staticBody) //{ // other.SetAwake(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(subStep, bA.m_islandIndex, bB.m_islandIndex); // Reset island flags and synchronize broad-phase proxies. for (int i = 0; i < island.m_bodies.Count(); ++i) { throw new NotImplementedException(); //Body* body = island.m_bodies[i]; //body.m_flags &= ~Body.BodyFlags.e_islandFlag; //if (body.m_type != _dynamicBody) //{ // continue; //} //body.SynchronizeFixtures(); //// Invalidate all contact TOIs on this displaced body. //for (ContactEdge* ce = body.m_contactList; ce; ce = ce.next) //{ // ce.contact.m_flags &= ~(ContactFlags.e_toiFlag | ContactFlags.e_islandFlag); //} } // Commit fixture proxy movements to the broad-phase so that new contacts are created. // Also, some contacts can be destroyed. m_contactManager.FindNewContacts(); if (m_subStepping) { m_stepComplete = false; break; } } }
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 }
/// 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. // CCD via the local separating axis method. This seeks progression // by computing the largest time at which separation is maintained. public static void TimeOfImpact(out TOIOutput output, TOIInput input) { Timer timer = new Timer(); ++_toiCalls; output.state = TOIOutput.State.e_unknown; output.t = input.tMax; DistanceProxy proxyA = input.proxyA; DistanceProxy proxyB = input.proxyB; 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 = proxyA.m_radius + proxyB.m_radius; float target = Math.Max(Settings._linearSlop, totalRadius - 3.0f * Settings._linearSlop); float tolerance = 0.25f * Settings._linearSlop; Utilities.Assert(target > tolerance); float t1 = 0.0f; const int k_maxIterations = 20; // TODO_ERIN Settings int iter = 0; // Prepare input for distance query. SimplexCache cache = new SimplexCache(); cache.count = 0; 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; Utilities.Distance(out distanceOutput, cache, distanceInput); // If the shapes are overlapped, we give up on continuous collision. if (distanceOutput.distance <= 0.0f) { // Failure! output.state = TOIOutput.State.e_overlapped; output.t = 0.0f; break; } if (distanceOutput.distance < target + tolerance) { // Victory! output.state = TOIOutput.State.e_touching; output.t = t1; break; } // Initialize the separating axis. throw new NotImplementedException(); // SeparationFunction fcn; // fcn.Initialize(&cache, proxyA, sweepA, proxyB, sweepB, t1); //#if ZERO // // Dump the curve seen by the root finder // { // const int N = 100; // float dx = 1.0f / N; // float xs[N+1]; // float fs[N+1]; // float x = 0.0f; // for (int i = 0; i <= N; ++i) // { // sweepA.GetTransform(out xfA, x); // sweepB.GetTransform(out xfB, x); // float f = fcn.Evaluate(xfA, xfB) - target; // printf("%g %g\n", x, f); // xs[i] = x; // fs[i] = f; // x += dx; // } // } //#endif // // 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(&indexA, &indexB, t2); // // Is the final configuration separated? // if (s2 > target + tolerance) // { // // Victory! // output.state = TOIOutput.State.e_separated; // 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 = TOIOutput.State.e_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 = TOIOutput.State.e_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) // { // // Secant rule to improve convergence. // t = a1 + (target - s1) * (a2 - a1) / (s2 - s1); // } // else // { // // Bisection to guarantee progress. // t = 0.5f * (a1 + a2); // } // ++rootIterCount; // ++_toiRootIters; // 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; // } // 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 = TOIOutput.State.e_failed; // output.t = t1; // break; // } } _toiMaxIters = Math.Max(_toiMaxIters, iter); float time = timer.GetMilliseconds(); _toiMaxTime = Math.Max(_toiMaxTime, time); _toiTime += time; }