/// <summary>
/// Compute the closest points between two shapes. Supports any combination of:
/// CircleShape, PolygonShape, EdgeShape. The simplex cache is input/output.
/// On the first call set SimplexCache.Count to zero.
/// </summary>
public static unsafe void Distance(out DistanceOutput output, ref SimplexCache cache, ref DistanceInput input,
Shape shapeA, Shape shapeB)
{
output = new DistanceOutput();
XForm transformA = input.TransformA;
XForm transformB = input.TransformB;
// Initialize the simplex.
Simplex simplex = new Simplex();
fixed(SimplexCache *sPtr = &cache)
{
simplex.ReadCache(sPtr, shapeA, transformA, shapeB, transformB);
}
// Get simplex vertices as an array.
SimplexVertex *vertices = &simplex.V1;
// These store the vertices of the last simplex so that we
// can check for duplicates and prevent cycling.
int *lastA = stackalloc int[4], lastB = stackalloc int[4];
int lastCount;
// Main iteration loop.
int iter = 0;
const int kMaxIterationCount = 20;
while (iter < kMaxIterationCount)
{
// Copy simplex so we can identify duplicates.
lastCount = simplex.Count;
int i;
for (i = 0; i < lastCount; ++i)
{
lastA[i] = vertices[i].IndexA;
lastB[i] = vertices[i].IndexB;
}
switch (simplex.Count)
{
case 1:
break;
case 2:
simplex.Solve2();
break;
case 3:
simplex.Solve3();
break;
default:
#if DEBUG
Box2DxDebug.Assert(false);
#endif
break;
}
// If we have 3 points, then the origin is in the corresponding triangle.
if (simplex.Count == 3)
{
break;
}
// Compute closest point.
Vec2 p = simplex.GetClosestPoint();
float distanceSqr = p.LengthSquared();
// Ensure the search direction is numerically fit.
if (distanceSqr < Settings.FltEpsilonSquared)
{
// The origin is probably contained by a line segment
// or triangle. Thus the shapes are overlapped.
// We can't return zero here even though there may be overlap.
// In case the simplex is a point, segment, or triangle it is difficult
// to determine if the origin is contained in the CSO or very close to it.
break;
}
// Compute a tentative new simplex vertex using support points.
SimplexVertex *vertex = vertices + simplex.Count;
vertex->IndexA = shapeA.GetSupport(Math.MulT(transformA.R, p));
vertex->Wa = Math.Mul(transformA, shapeA.GetVertex(vertex->IndexA));
//Vec2 wBLocal;
vertex->IndexB = shapeB.GetSupport(Math.MulT(transformB.R, -p));
vertex->Wb = Math.Mul(transformB, shapeB.GetVertex(vertex->IndexB));
vertex->W = vertex->Wb - vertex->Wa;
// Iteration count is equated to the number of support point calls.
++iter;
// Check for convergence.
float lowerBound = Vec2.Dot(p, vertex->W);
float upperBound = distanceSqr;
const float kRelativeTolSqr = 0.01f * 0.01f; // 1:100
if (upperBound - lowerBound <= kRelativeTolSqr * upperBound)
{
// Converged!
break;
}
// Check for duplicate support points.
bool duplicate = false;
for (i = 0; i < lastCount; ++i)
{
if (vertex->IndexA == lastA[i] && vertex->IndexB == lastB[i])
{
duplicate = true;
break;
}
}
// If we found a duplicate support point we must exit to avoid cycling.
if (duplicate)
{
break;
}
// New vertex is ok and needed.
++simplex.Count;
}
fixed(DistanceOutput *doPtr = &output)
{
// Prepare output.
simplex.GetWitnessPoints(&doPtr->PointA, &doPtr->PointB);
doPtr->Distance = Vec2.Distance(doPtr->PointA, doPtr->PointB);
doPtr->Iterations = iter;
}
fixed(SimplexCache *sPtr = &cache)
{
// Cache the simplex.
simplex.WriteCache(sPtr);
}
// Apply radii if requested.
if (input.UseRadii)
{
float rA = shapeA.Radius;
float rB = shapeB.Radius;
if (output.Distance > rA + rB && output.Distance > Settings.FltEpsilon)
{
// Shapes are still no overlapped.
// Move the witness points to the outer surface.
output.Distance -= rA + rB;
Vec2 normal = output.PointB - output.PointA;
normal.Normalize();
output.PointA += rA * normal;
output.PointB -= rB * normal;
}
else
{
// Shapes are overlapped when radii are considered.
// Move the witness points to the middle.
Vec2 p = 0.5f * (output.PointA + output.PointB);
output.PointA = p;
output.PointB = p;
output.Distance = 0.0f;
}
}
}
// 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 > Settings.FltEpsilon);
float radius = shapeA.Radius + shapeB.Radius;
float tolerance = input.Tolerance;
float alpha = 0.0f;
const int kMaxIterations = 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 (;;)
{
XForm 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();
unsafe
{
fcn.Initialize(&cache, shapeA, xfA, shapeB, 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;
}
#if _FALSE
// Dump the curve seen by the root finder
{
const int32 N = 100;
float32 dx = 1.0f / N;
float32 xs[N + 1];
