public float ComputeTOI(Sweep sweepA, Sweep sweepB)
{
TOIInput input = new TOIInput();
input.SweepA = sweepA;
input.SweepB = sweepB;
input.SweepRadiusA = _fixtureA.ComputeSweepRadius(sweepA.LocalCenter);
input.SweepRadiusB = _fixtureB.ComputeSweepRadius(sweepB.LocalCenter);
input.Tolerance = Common.Settings.LinearSlop;
return Collision.Collision.TimeOfImpact(input, _fixtureA.Shape, _fixtureB.Shape);
}
// CCD via the secant method.
/// <summary>
/// Compute the time when two shapes begin to touch or touch at a closer distance.
/// TOI considers the shape radii. It attempts to have the radii overlap by the tolerance.
/// Iterations terminate with the overlap is within 0.5 * tolerance. The tolerance should be
/// smaller than sum of the shape radii.
/// Warning the sweeps must have the same time interval.
/// </summary>
/// <returns>
/// The fraction between [0,1] in which the shapes first touch.
/// fraction=0 means the shapes begin touching/overlapped, and fraction=1 means the shapes don't touch.
/// </returns>
public static float TimeOfImpact(TOIInput input, Shape shapeA, Shape shapeB)
{
Sweep sweepA = input.SweepA;
Sweep sweepB = input.SweepB;
Box2DXDebug.Assert(sweepA.T0 == sweepB.T0);
Box2DXDebug.Assert(1.0f - sweepA.T0 > Common.Settings.FLT_EPSILON);
float radius = shapeA._radius + shapeB._radius;
float tolerance = input.Tolerance;
float alpha = 0.0f;
const int k_maxIterations = 1000; // TODO_ERIN b2Settings
int iter = 0;
float target = 0.0f;
// Prepare input for distance query.
SimplexCache cache = new SimplexCache();
cache.Count = 0;
DistanceInput distanceInput;
distanceInput.UseRadii = false;
for (; ;)
{
Transform xfA, xfB;
sweepA.GetTransform(out xfA, alpha);
sweepB.GetTransform(out xfB, alpha);
// Get the distance between shapes.
distanceInput.TransformA = xfA;
distanceInput.TransformB = xfB;
DistanceOutput distanceOutput;
Distance(out distanceOutput, ref cache, ref distanceInput, shapeA, shapeB);
if (distanceOutput.Distance <= 0.0f)
{
alpha = 1.0f;
break;
}
SeparationFunction fcn = new SeparationFunction();
#if ALLOWUNSAFE
unsafe
{
fcn.Initialize(&cache, shapeA, xfA, shapeB, xfB);
}
#else
fcn.Initialize(cache, shapeA, xfA, shapeB, xfB);
#endif
float separation = fcn.Evaluate(xfA, xfB);
if (separation <= 0.0f)
{
alpha = 1.0f;
break;
}
if (iter == 0)
{
// Compute a reasonable target distance to give some breathing room
// for conservative advancement. We take advantage of the shape radii
// to create additional clearance.
if (separation > radius)
{
target = Common.Math.Max(radius - tolerance, 0.75f * radius);
}
else
{
target = Common.Math.Max(separation - tolerance, 0.02f * radius);
}
}
if (separation - target < 0.5f * tolerance)
{
if (iter == 0)
{
alpha = 1.0f;
break;
}
break;
}
#if _FALSE
// Dump the curve seen by the root finder
{
const int32 N = 100;
float32 dx = 1.0f / N;
float32 xs[N + 1];
/// <summary>
/// CCD via the secant method.
/// Compute the time when two shapes begin to touch or touch at a closer distance.
/// TOI considers the shape radii. It attempts to have the radii overlap by the tolerance.
/// Iterations terminate with the overlap is within 0.5 * tolerance. The tolerance should be
/// smaller than sum of the shape radii.
/// @warning the sweeps must have the same time interval.
/// fraction=0 means the shapes begin touching/overlapped, and fraction=1 means the shapes don't touch.
/// </summary>
/// <param name="input">The input.</param>
/// <param name="shapeA">The shape A.</param>
/// <param name="shapeB">The shape B.</param>
/// <returns>
/// fraction between [0,1] in which the shapes first touch.
/// </returns>
public static float TimeOfImpact(TOIInput input)
{
++ToiCalls;
DistanceProxy proxyA = input.ProxyA;
DistanceProxy proxyB = input.ProxyB;
Sweep sweepA = input.SweepA;
Sweep sweepB = input.SweepB;
Box2DXDebug.Assert(sweepA.T0 == sweepB.T0);
Box2DXDebug.Assert(1.0f - sweepA.T0 > Settings.FLT_EPSILON);
float radius = proxyA._radius + proxyB._radius;
float tolerance = input.Tolerance;
float alpha = 0.0f;
const int k_maxIterations = 1000; // TODO_ERIN b2Settings
int iter = 0;
float target = 0.0f;
// Prepare input for distance query.
SimplexCache cache = new SimplexCache();
cache.Count = 0;
DistanceInput distanceInput = new DistanceInput();
distanceInput.proxyA = input.ProxyA;
distanceInput.proxyB = input.ProxyB;
distanceInput.UseRadii = false;
for (;;)
{
Transform xfA, xfB;
sweepA.GetTransform(out xfA, alpha);
sweepB.GetTransform(out xfB, alpha);
// Get the distance between shapes.
distanceInput.TransformA = xfA;
distanceInput.TransformB = xfB;
DistanceOutput distanceOutput;
Distance(out distanceOutput, cache, distanceInput);
if (distanceOutput.Distance <= 0.0f)
{
alpha = 1.0f;
break;
}
SeparationFunction fcn = new SeparationFunction();
fcn.Initialize(cache, proxyA, xfA, proxyB, xfB);
float separation = fcn.Evaluate(xfA, xfB);
if (separation <= 0.0f)
{
alpha = 1.0f;
break;
}
if (iter == 0)
{
// Compute a reasonable target distance to give some breathing room
// for conservative advancement. We take advantage of the shape radii
// to create additional clearance.
if (separation > radius)
{
target = Math.Max(radius - tolerance, 0.75f*radius);
}
else
{
target = Math.Max(separation - tolerance, 0.02f*radius);
}
}
if (separation - target < 0.5f*tolerance)
{
if (iter == 0)
{
alpha = 1.0f;
break;
}
break;
}
#if false
// Dump the curve seen by the root finder
{
const int N = 100;
float dx = 1.0f / N;
float xs[N+1];