// CCD via the local separating axis method. This seeks progression
// by computing the largest time at which separation is maintained.
public static bool TimeOfImpact(
DistanceProxy proxyA,
DistanceProxy proxyB,
Sweep sweepA,
Sweep sweepB,
float tMax,
out float t)
{
// Large rotations can make the root finder fail, so we normalize the
// sweep angles.
sweepA.Normalize();
sweepB.Normalize();
var totalRadius = proxyA.Radius + proxyB.Radius;
var target = System.Math.Max(Settings.LinearSlop, totalRadius - 3.0f * Settings.LinearSlop);
const float tolerance = 0.25f * Settings.LinearSlop;
System.Diagnostics.Debug.Assert(target > tolerance);
// Prepare input for distance query.
var cache = new SimplexCache {
Count = 0
};
// The outer loop progressively attempts to compute new separating axes.
// This loop terminates when an axis is repeated (no progress is made).
var t1 = 0.0f;
for (var iter = 0; iter < 20; ++iter)
{
WorldTransform 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.
var distance = Distance(ref cache, proxyA, xfA, proxyB, xfB);
// If the shapes are overlapped, we give up on continuous collision.
if (distance <= 0.0f)
{
// Failure!
t = 0.0f;
return(false);
}
if (distance < target + tolerance)
{
// Victory!
t = t1;
return(true);
}
// Initialize the separating axis.
SeparationFunction fcn;
SeparationFunction.Initialize(out fcn, cache, proxyA, proxyB, sweepA, 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.
var t2 = tMax;
for (var pushBackIter = 0; pushBackIter < Settings.MaxPolygonVertices; ++pushBackIter)
{
// Find the deepest point at t2. Store the witness point indices.
int indexA, indexB;
var s2 = fcn.FindMinSeparation(out indexA, out indexB, t2);
// Is the final configuration separated?
if (s2 > target + tolerance)
{
// Victory!
t = tMax;
return(false);
}
// Has the separation reached tolerance?
if (s2 > target - tolerance)
{
// Advance the sweeps
t1 = t2;
break;
}
// Compute the initial separation of the witness points.
var 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)
{
t = t1;
return(false);
}
// Check for touching
if (s1 <= target + tolerance)
{
// Victory! t1 should hold the TOI (could be 0.0).
t = t1;
return(true);
}
// Compute 1D root of: f(x) - target = 0
float a1 = t1, a2 = t2;
for (var rootIterCount = 0; rootIterCount < 50; ++rootIterCount)
{
// Use a mix of the secant rule and bisection.
float u;
if ((rootIterCount & 1) != 0)
{
// Secant rule to improve convergence.
u = a1 + (target - s1) * (a2 - a1) / (s2 - s1);
}
else
{
// Bisection to guarantee progress.
u = 0.5f * (a1 + a2);
}
var s = fcn.Evaluate(indexA, indexB, u);
if (System.Math.Abs(s - target) < tolerance)
{
// t2 holds a tentative value for t1
t2 = u;
break;
}
// Ensure we continue to bracket the root.
if (s > target)
{
a1 = u;
s1 = s;
}
else
{
a2 = u;
s2 = s;
}
}
}
}
// Root finder got stuck. Semi-victory.
t = t1;
return(false);
}
/// <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>
/// 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);
}