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