public static void ComputeDistance(out DistanceOutput output,
out SimplexCache cache,
ref DistanceInput input)
{
cache = new SimplexCache();
++b2_gjkCalls;
// Initialize the simplex.
Simplex simplex = new Simplex();
simplex.ReadCache(ref cache, ref input.proxyA, ref input.transformA, ref input.proxyB, ref input.transformB);
// Get simplex vertices as an array.
const int k_maxIters = 20;
// These store the vertices of the last simplex so that we
// can check for duplicates and prevent cycling.
FixedArray3<int> saveA = new FixedArray3<int>();
FixedArray3<int> saveB = new FixedArray3<int>();
int saveCount = 0;
Vector2 closestPoint = simplex.GetClosestPoint();
float distanceSqr1 = closestPoint.LengthSquared();
float distanceSqr2 = distanceSqr1;
// Main iteration loop.
int iter = 0;
while (iter < k_maxIters)
{
// Copy simplex so we can identify duplicates.
saveCount = simplex._count;
for (int i = 0; i < saveCount; ++i)
{
saveA[i] = simplex._v[i].indexA;
saveB[i] = simplex._v[i].indexB;
}
switch (simplex._count)
{
case 1:
break;
case 2:
simplex.Solve2();
break;
case 3:
simplex.Solve3();
break;
default:
Debug.Assert(false);
break;
}
// If we have 3 points, then the origin is in the corresponding triangle.
if (simplex._count == 3)
{
break;
}
// Compute closest point.
Vector2 p = simplex.GetClosestPoint();
distanceSqr2 = p.LengthSquared();
// Ensure progress
if (distanceSqr2 >= distanceSqr1)
{
//break;
}
distanceSqr1 = distanceSqr2;
// Get search direction.
Vector2 d = simplex.GetSearchDirection();
// Ensure the search direction is numerically fit.
if (d.LengthSquared() < Settings.b2_epsilon * Settings.b2_epsilon)
{
// 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 = simplex._v[simplex._count];
vertex.indexA = input.proxyA.GetSupport(MathUtils.MultiplyT(ref input.transformA.R, -d));
vertex.wA = MathUtils.Multiply(ref input.transformA, input.proxyA.GetVertex(vertex.indexA));
vertex.indexB = input.proxyB.GetSupport(MathUtils.MultiplyT(ref input.transformB.R, d));
vertex.wB = MathUtils.Multiply(ref input.transformB, input.proxyB.GetVertex(vertex.indexB));
vertex.w = vertex.wB - vertex.wA;
simplex._v[simplex._count] = vertex;
// Iteration count is equated to the number of support point calls.
++iter;
++b2_gjkIters;
// Check for duplicate support points. This is the main termination criteria.
bool duplicate = false;
for (int i = 0; i < saveCount; ++i)
{
if (vertex.indexA == saveA[i] && vertex.indexB == saveB[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;
}
b2_gjkMaxIters = Math.Max(b2_gjkMaxIters, iter);
// Prepare output.
simplex.GetWitnessPoints(out output.pointA, out output.pointB);
output.distance = (output.pointA - output.pointB).Length();
output.iterations = iter;
// Cache the simplex.
simplex.WriteCache(ref cache);
// Apply radii if requested.
if (input.useRadii)
{
float rA = input.proxyA._radius;
float rB = input.proxyB._radius;
if (output.distance > rA + rB && output.distance > Settings.b2_epsilon)
{
// Shapes are still no overlapped.
// Move the witness points to the outer surface.
output.distance -= rA + rB;
Vector2 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.
Vector2 p = 0.5f * (output.pointA + output.pointB);
output.pointA = p;
output.pointB = p;
output.distance = 0.0f;
}
}
}
public static void ComputeDistance(out DistanceOutput output,
out SimplexCache cache,
ref DistanceInput input)
{
cache = new SimplexCache();
++b2_gjkCalls;
// Initialize the simplex.
Simplex simplex = new Simplex();
simplex.ReadCache(ref cache, ref input.proxyA, ref input.transformA, ref input.proxyB, ref input.transformB);
// Get simplex vertices as an array.
int k_maxIters = 20;
// These store the vertices of the last simplex so that we
// can check for duplicates and prevent cycling.
FixedArray3 <int> saveA = new FixedArray3 <int>();
FixedArray3 <int> saveB = new FixedArray3 <int>();
int saveCount = 0;
Vector2 closestPoint = simplex.GetClosestPoint();
float distanceSqr1 = closestPoint.LengthSquared();
float distanceSqr2 = distanceSqr1;
// Main iteration loop.
int iter = 0;
while (iter < k_maxIters)
{
// Copy simplex so we can identify duplicates.
saveCount = simplex._count;
for (int i = 0; i < saveCount; ++i)
{
saveA[i] = simplex._v[i].indexA;
saveB[i] = simplex._v[i].indexB;
}
switch (simplex._count)
{
case 1:
break;
case 2:
simplex.Solve2();
break;
case 3:
simplex.Solve3();
break;
default:
Debug.Assert(false);
break;
}
// If we have 3 points, then the origin is in the corresponding triangle.
if (simplex._count == 3)
{
break;
}
// Compute closest point.
Vector2 p = simplex.GetClosestPoint();
distanceSqr2 = p.LengthSquared();
// Ensure progress
if (distanceSqr2 >= distanceSqr1)
{
//break;
}
distanceSqr1 = distanceSqr2;
// Get search direction.
Vector2 d = simplex.GetSearchDirection();
// Ensure the search direction is numerically fit.
if (d.LengthSquared() < Settings.b2_epsilon * Settings.b2_epsilon)
{
// 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 = simplex._v[simplex._count];
vertex.indexA = input.proxyA.GetSupport(MathUtils.MultiplyT(ref input.transformA.R, -d));
vertex.wA = MathUtils.Multiply(ref input.transformA, input.proxyA.GetVertex(vertex.indexA));
vertex.indexB = input.proxyB.GetSupport(MathUtils.MultiplyT(ref input.transformB.R, d));
vertex.wB = MathUtils.Multiply(ref input.transformB, input.proxyB.GetVertex(vertex.indexB));
vertex.w = vertex.wB - vertex.wA;
simplex._v[simplex._count] = vertex;
// Iteration count is equated to the number of support point calls.
++iter;
++b2_gjkIters;
// Check for duplicate support points. This is the main termination criteria.
bool duplicate = false;
for (int i = 0; i < saveCount; ++i)
{
if (vertex.indexA == saveA[i] && vertex.indexB == saveB[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;
}
b2_gjkMaxIters = Math.Max(b2_gjkMaxIters, iter);
// Prepare output.
simplex.GetWitnessPoints(out output.pointA, out output.pointB);
output.distance = (output.pointA - output.pointB).Length();
output.iterations = iter;
// Cache the simplex.
simplex.WriteCache(ref cache);
// Apply radii if requested.
if (input.useRadii)
{
float rA = input.proxyA._radius;
float rB = input.proxyB._radius;
if (output.distance > rA + rB && output.distance > Settings.b2_epsilon)
{
// Shapes are still no overlapped.
// Move the witness points to the outer surface.
output.distance -= rA + rB;
Vector2 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.
Vector2 p = 0.5f * (output.pointA + output.pointB);
output.pointA = p;
output.pointB = p;
output.distance = 0.0f;
}
}
}