public int indexB; // wB index
public void CopyFrom(b2SimplexVertex other)
{
wA = other.wA;
wB = other.wB;
w = other.w;
a = other.a;
indexA = other.indexA;
indexB = other.indexB;
}
public void Set(b2SimplexVertex other)
{
wA.SetV(other.wA);
wB.SetV(other.wB);
w.SetV(other.w);
a = other.a;
indexA = other.indexA;
indexB = other.indexB;
}
static b2Simplex()
{
m_vertices = new b2SimplexVertex[3];
m_vertices[0] = new b2SimplexVertex();
m_vertices[1] = new b2SimplexVertex();
m_vertices[2] = new b2SimplexVertex();
m_vertices_0 = m_vertices[0];
m_vertices_1 = m_vertices[1];
m_vertices_2 = m_vertices[2];
}
public void ReadCache(ref b2SimplexCache cache,
ref b2DistanceProxy proxyA, ref b2Transform transformA,
ref b2DistanceProxy proxyB, ref b2Transform transformB)
{
Debug.Assert(cache.count <= 3);
// Copy data from cache.
m_count = (int)cache.count;
m_vertices = new b2SimplexVertex[3];
for (int i = 0; i < m_count; ++i)
{
b2SimplexVertex v;
v.indexA = (int)cache.indexA[i];
v.indexB = (int)cache.indexB[i];
b2Vec2 wALocal = proxyA.GetVertex(v.indexA);
b2Vec2 wBLocal = proxyB.GetVertex(v.indexB);
v.wA = b2Math.b2Mul(transformA, wALocal);
v.wB = b2Math.b2Mul(transformB, wBLocal);
v.w = v.wB - v.wA;
v.a = 0.0f;
m_vertices[i] = v;
}
// Compute the new simplex metric, if it is substantially different than
// old metric then flush the simplex.
if (m_count > 1)
{
float metric1 = cache.metric;
float metric2 = GetMetric();
if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < b2Settings.b2_epsilon)
{
// Reset the simplex.
m_count = 0;
}
}
// If the cache is empty or invalid ...
if (m_count == 0)
{
b2SimplexVertex v;
v.indexA = 0;
v.indexB = 0;
b2Vec2 wALocal = proxyA.GetVertex(0);
b2Vec2 wBLocal = proxyB.GetVertex(0);
v.wA = b2Math.b2Mul(transformA, wALocal);
v.wB = b2Math.b2Mul(transformB, wBLocal);
v.w = v.wB - v.wA;
v.a = 0.0f;
m_count = 1;
m_vertices[0] = v;
}
}
public void ReadCache(b2SimplexCache cache,
b2DistanceProxy proxyA, b2Transform transformA,
b2DistanceProxy proxyB, b2Transform transformB)
{
b2Settings.b2Assert(0 <= cache.count && cache.count <= 3);
b2Vec2 wALocal;
b2Vec2 wBLocal;
// Copy data from cache.
m_count = (int)cache.count;
b2SimplexVertex[] vertices = m_vertices;
for (int i = 0; i < m_count; i++)
{
b2SimplexVertex v = vertices[i];
v.indexA = cache.indexA[i];
v.indexB = cache.indexB[i];
wALocal = proxyA.GetVertex(v.indexA);
wBLocal = proxyB.GetVertex(v.indexB);
v.wA = b2Math.MulX(transformA, wALocal);
v.wB = b2Math.MulX(transformB, wBLocal);
v.w = b2Math.SubtractVV(v.wB, v.wA);
v.a = 0;
}
// Compute the new simplex metric, if it substantially different than
// old metric then flush the simplex
if (m_count > 1)
{
float metric1 = cache.metric;
float metric2 = GetMetric();
if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < float.MinValue)
{
// Reset the simplex
m_count = 0;
}
}
// If the cache is empty or invalid
if (m_count == 0)
{
b2SimplexVertex v = vertices[0];
v.indexA = 0;
v.indexB = 0;
wALocal = proxyA.GetVertex(0);
wBLocal = proxyB.GetVertex(0);
v.wA = b2Math.MulX(transformA, wALocal);
v.wB = b2Math.MulX(transformB, wBLocal);
v.w = b2Math.SubtractVV(v.wB, v.wA);
m_count = 1;
}
}
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 int indexB; // wB index
public void CopyFrom(b2SimplexVertex other)
{
wA = other.wA;
wB = other.wB;
w = other.w;
a = other.a;
indexA = other.indexA;
indexB = other.indexB;
}
static b2Simplex()
{
m_vertices = new b2SimplexVertex[3];
m_vertices[0] = new b2SimplexVertex();
m_vertices[1] = new b2SimplexVertex();
m_vertices[2] = new b2SimplexVertex();
m_vertices_0 = m_vertices[0];
m_vertices_1 = m_vertices[1];
m_vertices_2 = m_vertices[2];
}
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];
}
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;
}
}
}
private void ReadCache(ref b2SimplexCache cache,
b2DistanceProxy proxyA, ref b2Transform transformA,
b2DistanceProxy proxyB, ref b2Transform transformB)
{
Debug.Assert(cache.count <= 3);
// Copy data from cache.
m_count = (int)cache.count;
for (int i = 0; i < m_count; ++i)
{
b2SimplexVertex v = m_vertices[i];
v.indexA = (int)cache.indexA[i];
v.indexB = (int)cache.indexB[i];
b2Vec2 wALocal = proxyA.m_vertices[v.indexA];
b2Vec2 wBLocal = proxyB.m_vertices[v.indexB];
v.wA.x = (transformA.q.c * wALocal.x - transformA.q.s * wALocal.y) + transformA.p.x;
v.wA.y = (transformA.q.s * wALocal.x + transformA.q.c * wALocal.y) + transformA.p.y;
v.wB.x = (transformB.q.c * wBLocal.x - transformB.q.s * wBLocal.y) + transformB.p.x;
v.wB.y = (transformB.q.s * wBLocal.x + transformB.q.c * wBLocal.y) + transformB.p.y;
v.w.x = v.wB.x - v.wA.x;
v.w.y = v.wB.y - v.wA.y;
v.a = 0.0f;
}
// Compute the new simplex metric, if it is substantially different than
// old metric then flush the simplex.
if (m_count > 1)
{
float metric1 = cache.metric;
float metric2 = GetMetric();
if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < b2Settings.b2_epsilon)
{
// Reset the simplex.
m_count = 0;
}
}
// If the cache is empty or invalid ...
if (m_count == 0)
{
b2SimplexVertex v = m_vertices_0;
v.indexA = 0;
v.indexB = 0;
b2Vec2 wALocal = proxyA.m_vertices[0];
b2Vec2 wBLocal = proxyB.m_vertices[0];
v.wA.x = (transformA.q.c * wALocal.x - transformA.q.s * wALocal.y) + transformA.p.x;
v.wA.y = (transformA.q.s * wALocal.x + transformA.q.c * wALocal.y) + transformA.p.y;
v.wB.x = (transformB.q.c * wBLocal.x - transformB.q.s * wBLocal.y) + transformB.p.x;
v.wB.y = (transformB.q.s * wBLocal.x + transformB.q.c * wBLocal.y) + transformB.p.y;
v.w.x = v.wB.x - v.wA.x;
v.w.y = v.wB.y - v.wA.y;
v.a = 0.0f;
m_count = 1;
}
}
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];
}
public void ReadCache(ref b2SimplexCache cache,
ref b2DistanceProxy proxyA, ref b2Transform transformA,
ref b2DistanceProxy proxyB, ref b2Transform transformB)
{
Debug.Assert(cache.count <= 3);
// Copy data from cache.
m_count = (int)cache.count;
m_vertices = new b2SimplexVertex[3];
for (int i = 0; i < m_count; ++i)
{
b2SimplexVertex v;
v.indexA = (int)cache.indexA[i];
v.indexB = (int)cache.indexB[i];
b2Vec2 wALocal = proxyA.GetVertex(v.indexA);
b2Vec2 wBLocal = proxyB.GetVertex(v.indexB);
v.wA = b2Math.b2Mul(transformA, wALocal);
v.wB = b2Math.b2Mul(transformB, wBLocal);
v.w = v.wB - v.wA;
v.a = 0.0f;
m_vertices[i] = v;
}
// Compute the new simplex metric, if it is substantially different than
// old metric then flush the simplex.
if (m_count > 1)
{
float metric1 = cache.metric;
float metric2 = GetMetric();
if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < b2Settings.b2_epsilon)
{
// Reset the simplex.
m_count = 0;
}
}
// If the cache is empty or invalid ...
if (m_count == 0)
{
b2SimplexVertex v;
v.indexA = 0;
v.indexB = 0;
b2Vec2 wALocal = proxyA.GetVertex(0);
b2Vec2 wBLocal = proxyB.GetVertex(0);
v.wA = b2Math.b2Mul(transformA, wALocal);
v.wB = b2Math.b2Mul(transformB, wBLocal);
v.w = v.wB - v.wA;
v.a = 0.0f;
m_count = 1;
m_vertices[0] = v;
}
}
