// CCD via the local separating axis method. This seeks progression
// by computing the largest time at which separation is maintained.
/// <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 b2Distance to compute the contact point and normal at the time of impact.
/// </summary>
/// <param name="output"></param>
/// <param name="input"></param>
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;
float tMax = input.tMax;
float target = Settings.b2_linearSlop;
float tolerance = 0.25f * Settings.b2_linearSlop;
Debug.Assert(target > tolerance);
float t1 = 0.0f;
const int k_maxIterations = 1000;
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;
}
SeparationFunction fcn = new SeparationFunction(ref cache, ref input.proxyA, ref sweepA, ref input.proxyB, ref sweepB);
// 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;
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;
}
// Is the final configuration touching?
if (s2 > target - tolerance)
{
// Victory!
output.State = TOIOutputState.Touching;
output.t = t2;
done = true;
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);
}
++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);
}
public SeparationFunction(ref SimplexCache cache,
ref DistanceProxy proxyA, ref Sweep sweepA,
ref DistanceProxy proxyB, ref Sweep sweepB)
{
_localPoint = Vector2.Zero;
_proxyA = proxyA;
_proxyB = proxyB;
int count = cache.count;
Debug.Assert(0 < count && count < 3);
_sweepA = sweepA;
_sweepB = sweepB;
Transform xfA, xfB;
_sweepA.GetTransform(out xfA, 0.0f);
_sweepB.GetTransform(out xfB, 0.0f);
if (count == 1)
{
_type = SeparationFunctionType.Points;
Vector2 localPointA = _proxyA.GetVertex(cache.indexA[0]);
Vector2 localPointB = _proxyB.GetVertex(cache.indexB[0]);
Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA);
Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB);
_axis = pointB - pointA;
_axis.Normalize();
return;
}
else if (cache.indexA[0] == cache.indexA[1])
{
// Two points on B and one on A.
_type = SeparationFunctionType.FaceB;
Vector2 localPointB1 = proxyB.GetVertex(cache.indexB[0]);
Vector2 localPointB2 = proxyB.GetVertex(cache.indexB[1]);
_axis = MathUtils.Cross(localPointB2 - localPointB1, 1.0f);
_axis.Normalize();
Vector2 normal = MathUtils.Multiply(ref xfB.R, _axis);
_localPoint = 0.5f * (localPointB1 + localPointB2);
Vector2 pointB = MathUtils.Multiply(ref xfB, _localPoint);
Vector2 localPointA = proxyA.GetVertex(cache.indexA[0]);
Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA);
float s = Vector2.Dot(pointA - pointB, normal);
if (s < 0.0f)
{
_axis = -_axis;
s = -s;
}
return;
}
else
{
// Two points on A and one or two points on B.
_type = SeparationFunctionType.FaceA;
Vector2 localPointA1 = _proxyA.GetVertex(cache.indexA[0]);
Vector2 localPointA2 = _proxyA.GetVertex(cache.indexA[1]);
_axis = MathUtils.Cross(localPointA2 - localPointA1, 1.0f);
_axis.Normalize();
Vector2 normal = MathUtils.Multiply(ref xfA.R, _axis);
_localPoint = 0.5f * (localPointA1 + localPointA2);
Vector2 pointA = MathUtils.Multiply(ref xfA, _localPoint);
Vector2 localPointB = _proxyB.GetVertex(cache.indexB[0]);
Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB);
float s = Vector2.Dot(pointB - pointA, normal);
if (s < 0.0f)
{
_axis = -_axis;
s = -s;
}
return;
}
}
// 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;
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)
{
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;
}
}
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;
// 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());
input.proxyB.Set(fixtureB.GetShape());
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;
found = true;
}
++count;
}
++iter;
} while (found && count > 1 && iter < 50);
if (toiContact == null)
{
return;
}
// Advance the body to its safe time.
Sweep backup = body._sweep;
body.Advance(toi);
++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.Static)
{
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;
contact.Update(_contactManager.ContactListener);
// Did the user disable the contact?
if (contact.IsEnabled() == false)
{
if (contact == toiContact)
{
// Restore the body's sweep.
body._sweep = backup;
body.SynchronizeTransform();
// Recurse because the TOI has been invalidated.
SolveTOI(body);
return;
}
// 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;
}
}
}