protected override void Draw(Settings settings)
{
base.Draw(settings);
b2DistanceInput input = b2DistanceInput.Create();
input.proxyA.Set(m_polygonA, 0);
input.proxyB.Set(m_polygonB, 0);
input.transformA = m_transformA;
input.transformB = m_transformB;
input.useRadii = true;
b2SimplexCache cache = b2SimplexCache.Create();
cache.count = 0;
b2DistanceOutput output = new b2DistanceOutput();
b2Simplex.b2Distance(ref output, ref cache, ref input);
m_debugDraw.DrawString(5, m_textLine, "distance = {0}", output.distance);
m_textLine += 15;
m_debugDraw.DrawString(5, m_textLine, "iterations = {0}", output.iterations);
m_textLine += 15;
{
b2Color color = new b2Color(0.9f, 0.9f, 0.9f);
b2Vec2[] v = new b2Vec2[b2Settings.b2_maxPolygonVertices];
for (int i = 0; i < m_polygonA.VertexCount; ++i)
{
v[i] = b2Math.b2Mul(m_transformA, m_polygonA.Vertices[i]);
}
m_debugDraw.DrawPolygon(v, m_polygonA.VertexCount, color);
for (int i = 0; i < m_polygonB.VertexCount; ++i)
{
v[i] = b2Math.b2Mul(m_transformB, m_polygonB.Vertices[i]);
}
m_debugDraw.DrawPolygon(v, m_polygonB.VertexCount, color);
}
b2Vec2 x1 = output.pointA;
b2Vec2 x2 = output.pointB;
b2Color c1 = new b2Color(1.0f, 0.0f, 0.0f);
m_debugDraw.DrawPoint(x1, 4.0f, c1);
b2Color c2 = new b2Color(1.0f, 1.0f, 0.0f);
m_debugDraw.DrawPoint(x2, 4.0f, c2);
}
/// Determine if two generic shapes overlap.
public static bool b2TestOverlap(b2Shape shapeA, int indexA,
b2Shape shapeB, int indexB,
ref b2Transform xfA, ref b2Transform xfB)
{
b2DistanceInput input = b2DistanceInput.Create();
input.proxyA = b2DistanceProxy.Create(shapeA, indexA);
input.proxyB = b2DistanceProxy.Create(shapeB, indexB);
input.transformA = xfA;
input.transformB = xfB;
input.useRadii = true;
b2SimplexCache cache = b2SimplexCache.Create();
b2DistanceOutput output = new b2DistanceOutput();
b2Simplex.b2Distance(ref output, ref cache, ref input);
// Console.WriteLine("{2} vs {3}: distance={0} after {1} iters", output.distance, output.iterations, shapeA.ShapeType, shapeB.ShapeType);
return output.distance < 10.0f * b2Settings.b2_epsilon;
}
public static void Distance(b2DistanceOutput output, b2SimplexCache cache, b2DistanceInput input)
{
++b2_gjkCalls;
b2DistanceProxy proxyA = input.proxyA;
b2DistanceProxy proxyB = input.proxyB;
b2Transform transformA = input.transformA;
b2Transform transformB = input.transformB;
// Initialize the simplex
b2Simplex simplex = s_simplex;
simplex.ReadCache(cache, proxyA, transformA, proxyB, transformB);
// Get simplex vertices as an vector.
b2SimplexVertex[] vertices = simplex.m_vertices;
const int k_maxIters = 20;
// These store the vertices of the last simplex so that we
// can check for duplicates and preven cycling
int[] saveA = s_saveA;
int[] saveB = s_saveB;
int saveCount = 0;
b2Vec2 closestPoint = simplex.GetClosestPoint();
float distanceSqr1 = closestPoint.LengthSquared();
float distanceSqr2 = distanceSqr1;
int i;
b2Vec2 p;
// Main iteration loop
int iter = 0;
while (iter < k_maxIters)
{
// Copy the simplex so that we can identify duplicates
saveCount = simplex.m_count;
for (i = 0; i < saveCount; i++)
{
saveA[i] = vertices[i].indexA;
saveB[i] = vertices[i].indexB;
}
/*switch(simplex.m_count)
* {
* case 1:
* break;
* case 2:
* simplex.Solve2();
* break;
* case 3:
* simplex.Solve3();
* break;
* default:
* b2Settings.b2Assert(false);
* }*/
if (simplex.m_count == 1)
{
}
else if (simplex.m_count == 2)
{
simplex.Solve2();
}
else if (simplex.m_count == 3)
{
simplex.Solve3();
}
else
{
b2Settings.b2Assert(false);
}
// If we have 3 points, then the origin is in the corresponding triangle.
if (simplex.m_count == 3)
{
break;
}
// Compute the closest point.
p = simplex.GetClosestPoint();
distanceSqr2 = p.LengthSquared();
// Ensure progress
if (distanceSqr2 > distanceSqr1)
{
//break;
}
distanceSqr1 = distanceSqr2;
// Get search direction.
b2Vec2 d = simplex.GetSearchDirection();
// Ensure the search direction is numerically fit.
if (d.LengthSquared() < 0.0f /*Number.MinValue * Number.MinValue*/)
{
// 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 very 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
b2SimplexVertex vertex = vertices[simplex.m_count];
vertex.indexA = (int)proxyA.GetSupport(b2Math.MulTMV(transformA.R, d.GetNegative()));
vertex.wA = b2Math.MulX(transformA, proxyA.GetVertex(vertex.indexA));
vertex.indexB = (int)proxyB.GetSupport(b2Math.MulTMV(transformB.R, d));
vertex.wB = b2Math.MulX(transformB, proxyB.GetVertex(vertex.indexB));
vertex.w = b2Math.SubtractVV(vertex.wB, vertex.wA);
// 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 (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 exist to avoid cycling
if (duplicate)
{
break;
}
// New vertex is ok and needed.
++simplex.m_count;
}
b2_gjkMaxIters = (int)b2Math.Max(b2_gjkMaxIters, iter);
// Prepare output
simplex.GetWitnessPoints(output.pointA, output.pointB);
output.distance = b2Math.SubtractVV(output.pointA, output.pointB).Length();
output.iterations = iter;
// Cache the simplex
simplex.WriteCache(cache);
// Apply radii if requested.
if (input.useRadii)
{
float rA = proxyA.m_radius;
float rB = proxyB.m_radius;
if (output.distance > rA + rB && output.distance > float.MinValue)
{
// Shapes are still not overlapped.
// Move the witness points to the outer surface.
output.distance -= rA + rB;
b2Vec2 normal = b2Math.SubtractVV(output.pointB, output.pointA);
normal.Normalize();
output.pointA.x += rA * normal.x;
output.pointA.y += rA * normal.y;
output.pointB.x -= rB * normal.x;
output.pointB.y -= rB * normal.y;
}
else
{
// Shapes are overlapped when radii are considered.
// Move the witness points to the middle.
p = new b2Vec2();
p.x = 0.5f * (output.pointA.x + output.pointB.x);
p.y = 0.5f * (output.pointA.y + output.pointB.y);
output.pointA.x = output.pointB.x = p.x;
output.pointA.y = output.pointB.y = p.y;
output.distance = 0.0f;
}
}
}
public static void b2Distance(out b2DistanceOutput output, ref b2SimplexCache cache, ref b2DistanceInput input)
{
++b2DistanceProxy.b2_gjkCalls;
b2DistanceProxy proxyA = input.proxyA;
b2DistanceProxy proxyB = input.proxyB;
// Initialize the simplex.
b2Simplex simplex = new b2Simplex();
simplex.ReadCache(ref cache, proxyA, ref input.transformA, proxyB, ref input.transformB);
// Get simplex vertices as an array.
b2SimplexVertex[] vertices = b2Simplex.m_vertices;
int k_maxIters = 20;
// These store the vertices of the last simplex so that we
// can check for duplicates and prevent cycling.
int[] saveA = _saveA;
int[] saveB = _saveB;
int saveCount = 0;
b2Vec2 closestPoint;
simplex.GetClosestPoint(out closestPoint);
float distanceSqr1 = closestPoint.LengthSquared;
float distanceSqr2 = distanceSqr1;
// Console.WriteLine("Closest Point={0},{1}, distance={2}", closestPoint.x, closestPoint.y, distanceSqr1);
// Main iteration loop.
#region Main Iteration Loop
int iter = 0;
while (iter < k_maxIters)
{
// Copy simplex so we can identify duplicates.
saveCount = simplex.m_count;
for (int i = 0; i < saveCount; ++i)
{
saveA[i] = vertices[i].indexA;
saveB[i] = vertices[i].indexB;
}
switch (simplex.m_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.m_count == 3)
{
break;
}
// Compute closest point.
b2Vec2 p;
simplex.GetClosestPoint(out p);
distanceSqr2 = p.x * p.x + p.y * p.y;
// Ensure progress
if (distanceSqr2 >= distanceSqr1)
{
//break;
}
distanceSqr1 = distanceSqr2;
// Get search direction.
b2Vec2 d;
simplex.GetSearchDirection(out d);
// Ensure the search direction is numerically fit.
if ((d.x * d.x + d.y * d.y) < b2Settings.b2_epsilon * b2Settings.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.
b2SimplexVertex vertex = vertices[simplex.m_count];
var q = input.transformA.q;
b2Vec2 b;
b.x = q.c * -d.x + q.s * -d.y;
b.y = -q.s * -d.x + q.c * -d.y;
vertex.indexA = proxyA.GetSupport(ref b);
var vA = proxyA.m_vertices[vertex.indexA];
vertex.wA.x = (q.c * vA.x - q.s * vA.y) + input.transformA.p.x;
vertex.wA.y = (q.s * vA.x + q.c * vA.y) + input.transformA.p.y;
// b2Vec2 wBLocal = new b2Vec2();
q = input.transformB.q;
b.x = q.c * d.x + q.s * d.y;
b.y = -q.s * d.x + q.c * d.y;
vertex.indexB = proxyB.GetSupport(ref b);
var vB = proxyB.m_vertices[vertex.indexB];
vertex.wB.x = (input.transformB.q.c * vB.x - input.transformB.q.s * vB.y) + input.transformB.p.x;
vertex.wB.y = (input.transformB.q.s * vB.x + input.transformB.q.c * vB.y) + input.transformB.p.y;
vertex.w.x = vertex.wB.x - vertex.wA.x;
vertex.w.y = vertex.wB.y - vertex.wA.y;
// Iteration count is equated to the number of support point calls.
++iter;
++b2DistanceProxy.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.m_count;
}
#endregion
b2DistanceProxy.b2_gjkMaxIters = Math.Max(b2DistanceProxy.b2_gjkMaxIters, iter);
// Prepare output.
simplex.GetWitnessPoints(out output.pointA, out output.pointB);
output.distance = b2Math.b2Distance(ref output.pointA, ref output.pointB);
output.iterations = iter;
// Cache the simplex.
simplex.WriteCache(ref cache);
// Apply radii if requested.
if (input.useRadii)
{
float rA = proxyA.Radius;
float rB = proxyB.Radius;
if (output.distance > rA + rB && output.distance > b2Settings.b2_epsilon)
{
// Shapes are still not overlapped.
// Move the witness points to the outer surface.
output.distance -= rA + rB;
b2Vec2 normal;
normal.x = output.pointB.x - output.pointA.x;
normal.y = output.pointB.y - output.pointA.y;
normal.Normalize();
output.pointA.x += rA * normal.x;
output.pointA += rA * normal;
output.pointB.x -= rB * normal.x;
output.pointB.y -= rB * normal.y;
}
else
{
// Shapes are overlapped when radii are considered.
// Move the witness points to the middle.
b2Vec2 p;
p.x = 0.5f * (output.pointA.x + output.pointB.x);
p.y = 0.5f * (output.pointA.y + output.pointB.y);
output.pointA = p;
output.pointB = p;
output.distance = 0.0f;
}
}
}
public static void b2Distance(ref b2DistanceOutput output, ref b2SimplexCache cache, ref b2DistanceInput input)
{
++b2DistanceProxy.b2_gjkCalls;
b2DistanceProxy proxyA = input.proxyA.Copy();
b2DistanceProxy proxyB = input.proxyB.Copy();
b2Transform transformA = input.transformA;
b2Transform transformB = input.transformB;
// Initialize the simplex.
b2Simplex simplex = new b2Simplex();
simplex.ReadCache(ref cache, ref proxyA, ref transformA, ref proxyB, ref transformB);
// Get simplex vertices as an array.
b2SimplexVertex[] vertices = new b2SimplexVertex[] { simplex.m_vertices[0], simplex.m_vertices[1], simplex.m_vertices[2] };
int k_maxIters = 20;
// These store the vertices of the last simplex so that we
// can check for duplicates and prevent cycling.
int[] saveA = new int[3];
int[] saveB = new int[3];
int saveCount = 0;
b2Vec2 closestPoint = simplex.GetClosestPoint();
float distanceSqr1 = closestPoint.LengthSquared();
float distanceSqr2 = distanceSqr1;
// Console.WriteLine("Closest Point={0},{1}, distance={2}", closestPoint.x, closestPoint.y, distanceSqr1);
// Main iteration loop.
#region Main Iteration Loop
int iter = 0;
while (iter < k_maxIters)
{
// Copy simplex so we can identify duplicates.
saveCount = simplex.m_count;
for (int i = 0; i < saveCount; ++i)
{
saveA[i] = vertices[i].indexA;
saveB[i] = vertices[i].indexB;
}
switch (simplex.m_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.m_count == 3)
{
break;
}
// Compute closest point.
b2Vec2 p = simplex.GetClosestPoint();
distanceSqr2 = p.LengthSquared();
// Ensure progress
if (distanceSqr2 >= distanceSqr1)
{
//break;
}
distanceSqr1 = distanceSqr2;
// Get search direction.
b2Vec2 d = simplex.GetSearchDirection();
// Ensure the search direction is numerically fit.
if (d.LengthSquared() < b2Settings.b2_epsilon * b2Settings.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.
b2SimplexVertex vertex = vertices[simplex.m_count];
vertex.indexA = proxyA.GetSupport(b2Math.b2MulT(transformA.q, -d));
vertex.wA = b2Math.b2Mul(transformA, proxyA.GetVertex(vertex.indexA));
// b2Vec2 wBLocal = new b2Vec2();
vertex.indexB = proxyB.GetSupport(b2Math.b2MulT(transformB.q, d));
vertex.wB = b2Math.b2Mul(transformB, proxyB.GetVertex(vertex.indexB));
vertex.w = vertex.wB - vertex.wA;
vertices[simplex.m_count] = vertex;
// Iteration count is equated to the number of support point calls.
++iter;
++b2DistanceProxy.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.m_count;
}
#endregion
b2DistanceProxy.b2_gjkMaxIters = Math.Max(b2DistanceProxy.b2_gjkMaxIters, iter);
// Prepare output.
simplex.GetWitnessPoints(ref output.pointA, ref output.pointB);
output.distance = b2Math.b2Distance(output.pointA, output.pointB);
output.iterations = iter;
// Cache the simplex.
simplex.WriteCache(ref cache);
// Apply radii if requested.
if (input.useRadii)
{
float rA = proxyA.Radius;
float rB = proxyB.Radius;
if (output.distance > rA + rB && output.distance > b2Settings.b2_epsilon)
{
// Shapes are still not overlapped.
// Move the witness points to the outer surface.
output.distance -= rA + rB;
b2Vec2 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.
b2Vec2 p = 0.5f * (output.pointA + output.pointB);
output.pointA = p;
output.pointB = p;
output.distance = 0.0f;
}
}
// Copy back the vertex changes because they are structs, but in C++ land they are reference values
simplex.m_vertices[0] = vertices[0];
simplex.m_vertices[1] = vertices[1];
simplex.m_vertices[2] = vertices[2];
}
// CCD via the local separating axis method. This seeks progression
// by computing the largest time at which separation is maintained.
public static b2TOIOutput Compute(b2TOIInput input)
{
b2TOIOutput output = new b2TOIOutput();
++b2_toiCalls;
output.state = b2ImpactState.e_unknown;
output.t = input.tMax;
b2DistanceProxy proxyA = input.proxyA;
b2DistanceProxy proxyB = input.proxyB;
b2Sweep sweepA = input.sweepA;
b2Sweep 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 = proxyA.Radius + proxyB.Radius;
float target = Math.Max(b2Settings.b2_linearSlop, totalRadius - 3.0f * b2Settings.b2_linearSlop);
float tolerance = 0.25f * b2Settings.b2_linearSlop;
Debug.Assert(target > tolerance);
float t1 = 0.0f;
int k_maxIterations = 20; // TODO_ERIN b2Settings
int iter = 0;
// Prepare input for distance query.
b2SimplexCache cache = b2SimplexCache.Create();
b2DistanceInput distanceInput = new b2DistanceInput();
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).
while (true)
{
b2Transform 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;
b2DistanceOutput distanceOutput = new b2DistanceOutput();
b2Simplex.b2Distance(ref distanceOutput, ref cache, ref distanceInput);
// If the shapes are overlapped, we give up on continuous collision.
if (distanceOutput.distance <= 0.0f)
{
// Failure!
output.state = b2ImpactState.e_overlapped;
output.t = 0.0f;
break;
}
if (distanceOutput.distance < target + tolerance)
{
// Victory!
output.state = b2ImpactState.e_touching;
output.t = t1;
break;
}
// Initialize the separating axis.
b2SeparationFunction fcn = new b2SeparationFunction();
fcn.Initialize(ref cache, proxyA, ref sweepA, proxyB, ref sweepB, t1);
#if false
// Dump the curve seen by the root finder
{
int N = 100;
float dx = 1.0f / N;
float xs[N+1];
public static void b2Distance(out b2DistanceOutput output, ref b2SimplexCache cache, ref b2DistanceInput input)
{
++b2DistanceProxy.b2_gjkCalls;
b2DistanceProxy proxyA = input.proxyA;
b2DistanceProxy proxyB = input.proxyB;
// Initialize the simplex.
b2Simplex simplex = new b2Simplex();
simplex.ReadCache(ref cache, proxyA, ref input.transformA, proxyB, ref input.transformB);
// Get simplex vertices as an array.
b2SimplexVertex[] vertices = b2Simplex.m_vertices;
int k_maxIters = 20;
// These store the vertices of the last simplex so that we
// can check for duplicates and prevent cycling.
int[] saveA = _saveA;
int[] saveB = _saveB;
int saveCount = 0;
b2Vec2 closestPoint;
simplex.GetClosestPoint(out closestPoint);
float distanceSqr1 = closestPoint.LengthSquared;
float distanceSqr2 = distanceSqr1;
// Console.WriteLine("Closest Point={0},{1}, distance={2}", closestPoint.x, closestPoint.y, distanceSqr1);
// Main iteration loop.
#region Main Iteration Loop
int iter = 0;
while (iter < k_maxIters)
{
// Copy simplex so we can identify duplicates.
saveCount = simplex.m_count;
for (int i = 0; i < saveCount; ++i)
{
saveA[i] = vertices[i].indexA;
saveB[i] = vertices[i].indexB;
}
switch (simplex.m_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.m_count == 3)
{
break;
}
// Compute closest point.
b2Vec2 p;
simplex.GetClosestPoint(out p);
distanceSqr2 = p.x * p.x + p.y * p.y;
// Ensure progress
if (distanceSqr2 >= distanceSqr1)
{
//break;
}
distanceSqr1 = distanceSqr2;
// Get search direction.
b2Vec2 d;
simplex.GetSearchDirection(out d);
// Ensure the search direction is numerically fit.
if ((d.x * d.x + d.y * d.y) < b2Settings.b2_epsilon * b2Settings.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.
b2SimplexVertex vertex = vertices[simplex.m_count];
var q = input.transformA.q;
b2Vec2 b;
b.x = q.c * -d.x + q.s * -d.y;
b.y = -q.s * -d.x + q.c * -d.y;
vertex.indexA = proxyA.GetSupport(ref b);
var vA = proxyA.m_vertices[vertex.indexA];
vertex.wA.x = (q.c * vA.x - q.s * vA.y) + input.transformA.p.x;
vertex.wA.y = (q.s * vA.x + q.c * vA.y) + input.transformA.p.y;
// b2Vec2 wBLocal = new b2Vec2();
q = input.transformB.q;
b.x = q.c * d.x + q.s * d.y;
b.y = -q.s * d.x + q.c * d.y;
vertex.indexB = proxyB.GetSupport(ref b);
var vB = proxyB.m_vertices[vertex.indexB];
vertex.wB.x = (input.transformB.q.c * vB.x - input.transformB.q.s * vB.y) + input.transformB.p.x;
vertex.wB.y = (input.transformB.q.s * vB.x + input.transformB.q.c * vB.y) + input.transformB.p.y;
vertex.w.x = vertex.wB.x - vertex.wA.x;
vertex.w.y = vertex.wB.y - vertex.wA.y;
// Iteration count is equated to the number of support point calls.
++iter;
++b2DistanceProxy.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.m_count;
}
#endregion
b2DistanceProxy.b2_gjkMaxIters = Math.Max(b2DistanceProxy.b2_gjkMaxIters, iter);
// Prepare output.
simplex.GetWitnessPoints(out output.pointA, out output.pointB);
output.distance = b2Math.b2Distance(ref output.pointA, ref output.pointB);
output.iterations = iter;
// Cache the simplex.
simplex.WriteCache(ref cache);
// Apply radii if requested.
if (input.useRadii)
{
float rA = proxyA.Radius;
float rB = proxyB.Radius;
if (output.distance > rA + rB && output.distance > b2Settings.b2_epsilon)
{
// Shapes are still not overlapped.
// Move the witness points to the outer surface.
output.distance -= rA + rB;
b2Vec2 normal;
normal.x = output.pointB.x - output.pointA.x;
normal.y = output.pointB.y - output.pointA.y;
normal.Normalize();
output.pointA.x += rA * normal.x;
output.pointA += rA * normal;
output.pointB.x -= rB * normal.x;
output.pointB.y -= rB * normal.y;
}
else
{
// Shapes are overlapped when radii are considered.
// Move the witness points to the middle.
b2Vec2 p;
p.x = 0.5f * (output.pointA.x + output.pointB.x);
p.y = 0.5f * (output.pointA.y + output.pointB.y);
output.pointA = p;
output.pointB = p;
output.distance = 0.0f;
}
}
}
public static void b2Distance(ref b2DistanceOutput output, ref b2SimplexCache cache, ref b2DistanceInput input)
{
++b2DistanceProxy.b2_gjkCalls;
b2DistanceProxy proxyA = input.proxyA.Copy();
b2DistanceProxy proxyB = input.proxyB.Copy();
b2Transform transformA = input.transformA;
b2Transform transformB = input.transformB;
// Initialize the simplex.
b2Simplex simplex = new b2Simplex();
simplex.ReadCache(ref cache, ref proxyA, ref transformA, ref proxyB, ref transformB);
// Get simplex vertices as an array.
b2SimplexVertex[] vertices = new b2SimplexVertex[] { simplex.m_vertices[0], simplex.m_vertices[1], simplex.m_vertices[2] };
int k_maxIters = 20;
// These store the vertices of the last simplex so that we
// can check for duplicates and prevent cycling.
int[] saveA = new int[3];
int[] saveB = new int[3];
int saveCount = 0;
b2Vec2 closestPoint = simplex.GetClosestPoint();
float distanceSqr1 = closestPoint.LengthSquared();
float distanceSqr2 = distanceSqr1;
// Console.WriteLine("Closest Point={0},{1}, distance={2}", closestPoint.x, closestPoint.y, distanceSqr1);
// Main iteration loop.
#region Main Iteration Loop
int iter = 0;
while (iter < k_maxIters)
{
// Copy simplex so we can identify duplicates.
saveCount = simplex.m_count;
for (int i = 0; i < saveCount; ++i)
{
saveA[i] = vertices[i].indexA;
saveB[i] = vertices[i].indexB;
}
switch (simplex.m_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.m_count == 3)
{
break;
}
// Compute closest point.
b2Vec2 p = simplex.GetClosestPoint();
distanceSqr2 = p.LengthSquared();
// Ensure progress
if (distanceSqr2 >= distanceSqr1)
{
//break;
}
distanceSqr1 = distanceSqr2;
// Get search direction.
b2Vec2 d = simplex.GetSearchDirection();
// Ensure the search direction is numerically fit.
if (d.LengthSquared() < b2Settings.b2_epsilon * b2Settings.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.
b2SimplexVertex vertex = vertices[simplex.m_count];
vertex.indexA = proxyA.GetSupport(b2Math.b2MulT(transformA.q, -d));
vertex.wA = b2Math.b2Mul(transformA, proxyA.GetVertex(vertex.indexA));
// b2Vec2 wBLocal = new b2Vec2();
vertex.indexB = proxyB.GetSupport(b2Math.b2MulT(transformB.q, d));
vertex.wB = b2Math.b2Mul(transformB, proxyB.GetVertex(vertex.indexB));
vertex.w = vertex.wB - vertex.wA;
vertices[simplex.m_count] = vertex;
// Iteration count is equated to the number of support point calls.
++iter;
++b2DistanceProxy.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.m_count;
}
#endregion
b2DistanceProxy.b2_gjkMaxIters = Math.Max(b2DistanceProxy.b2_gjkMaxIters, iter);
// Prepare output.
simplex.GetWitnessPoints(ref output.pointA, ref output.pointB);
output.distance = b2Math.b2Distance(output.pointA, output.pointB);
output.iterations = iter;
// Cache the simplex.
simplex.WriteCache(ref cache);
// Apply radii if requested.
if (input.useRadii)
{
float rA = proxyA.Radius;
float rB = proxyB.Radius;
if (output.distance > rA + rB && output.distance > b2Settings.b2_epsilon)
{
// Shapes are still not overlapped.
// Move the witness points to the outer surface.
output.distance -= rA + rB;
b2Vec2 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.
b2Vec2 p = 0.5f * (output.pointA + output.pointB);
output.pointA = p;
output.pointB = p;
output.distance = 0.0f;
}
}
// Copy back the vertex changes because they are structs, but in C++ land they are reference values
simplex.m_vertices[0] = vertices[0];
simplex.m_vertices[1] = vertices[1];
simplex.m_vertices[2] = vertices[2];
}
// CCD via the local separating axis method. This seeks progression
// by computing the largest time at which separation is maintained.
public static b2TOIOutput Compute(b2TOIInput input)
{
b2TOIOutput output = new b2TOIOutput();
++b2_toiCalls;
output.state = b2ImpactState.e_unknown;
output.t = input.tMax;
b2DistanceProxy proxyA = input.proxyA.Copy();
b2DistanceProxy proxyB = input.proxyB.Copy();
b2Sweep sweepA = input.sweepA;
b2Sweep 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 = proxyA.Radius + proxyB.Radius;
float target = Math.Max(b2Settings.b2_linearSlop, totalRadius - 3.0f * b2Settings.b2_linearSlop);
float tolerance = 0.25f * b2Settings.b2_linearSlop;
Debug.Assert(target > tolerance);
float t1 = 0.0f;
int k_maxIterations = 20; // TODO_ERIN b2Settings
int iter = 0;
// Prepare input for distance query.
b2SimplexCache cache = b2SimplexCache.Create();
b2DistanceInput distanceInput = new b2DistanceInput();
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).
while (true)
{
b2Transform xfA = b2Transform.Create(), xfB = b2Transform.Create();
sweepA.GetTransform(ref xfA, t1);
sweepB.GetTransform(ref xfB, t1);
// Get the distance between shapes. We can also use the results
// to get a separating axis.
distanceInput.transformA = xfA;
distanceInput.transformB = xfB;
b2DistanceOutput distanceOutput = new b2DistanceOutput();
b2Simplex.b2Distance(ref distanceOutput, ref cache, ref distanceInput);
// If the shapes are overlapped, we give up on continuous collision.
if (distanceOutput.distance <= 0.0f)
{
// Failure!
output.state = b2ImpactState.e_overlapped;
output.t = 0.0f;
break;
}
if (distanceOutput.distance < target + tolerance)
{
// Victory!
output.state = b2ImpactState.e_touching;
output.t = t1;
break;
}
// Initialize the separating axis.
b2SeparationFunction fcn = new b2SeparationFunction();
fcn.Initialize(ref cache, ref proxyA, ref sweepA, ref proxyB, ref sweepB, t1);
#if false
// Dump the curve seen by the root finder
{
int N = 100;
float dx = 1.0f / N;
float xs[N + 1];
