// TODO_ERIN might not need to return the separation
public float initialize(SimplexCache cache, DistanceProxy proxyA, Sweep sweepA,
DistanceProxy proxyB, Sweep sweepB, float t1)
{
m_proxyA = proxyA;
m_proxyB = proxyB;
int count = cache.count;
Debug.Assert(0 < count && count < 3);
m_sweepA = sweepA;
m_sweepB = sweepB;
m_sweepA.getTransform(xfa, t1);
m_sweepB.getTransform(xfb, t1);
// log.debug("initializing separation.\n" +
// "cache: "+cache.count+"-"+cache.metric+"-"+cache.indexA+"-"+cache.indexB+"\n"
// "distance: "+proxyA.
if (count == 1)
{
m_type = TOIType.POINTS;
/*
* Vec2 localPointA = m_proxyA.GetVertex(cache.indexA[0]); Vec2 localPointB =
* m_proxyB.GetVertex(cache.indexB[0]); Vec2 pointA = Mul(transformA, localPointA); Vec2
* pointB = Mul(transformB, localPointB); m_axis = pointB - pointA; m_axis.Normalize();
*/
localPointA.set(m_proxyA.getVertex(cache.indexA[0]));
localPointB.set(m_proxyB.getVertex(cache.indexB[0]));
Transform.mulToOutUnsafe(xfa, localPointA, ref pointA);
Transform.mulToOutUnsafe(xfb, localPointB, ref pointB);
m_axis.set(pointB);
m_axis.subLocal(pointA);
float s = m_axis.normalize();
return s;
}
else if (cache.indexA[0] == cache.indexA[1])
{
// Two points on B and one on A.
m_type = TOIType.FACE_B;
localPointB1.set(m_proxyB.getVertex(cache.indexB[0]));
localPointB2.set(m_proxyB.getVertex(cache.indexB[1]));
temp.set(localPointB2);
temp.subLocal(localPointB1);
Vec2.crossToOutUnsafe(temp, 1f, ref m_axis);
m_axis.normalize();
Rot.mulToOutUnsafe(xfb.q, m_axis, ref normal);
m_localPoint.set(localPointB1);
m_localPoint.addLocal(localPointB2);
m_localPoint.mulLocal(.5f);
Transform.mulToOutUnsafe(xfb, m_localPoint, ref pointB);
localPointA.set(proxyA.getVertex(cache.indexA[0]));
Transform.mulToOutUnsafe(xfa, localPointA, ref pointA);
temp.set(pointA);
temp.subLocal(pointB);
float s = Vec2.dot(temp, normal);
if (s < 0.0f)
{
m_axis.negateLocal();
s = -s;
}
return s;
}
else
{
// Two points on A and one or two points on B.
m_type = TOIType.FACE_A;
localPointA1.set(m_proxyA.getVertex(cache.indexA[0]));
localPointA2.set(m_proxyA.getVertex(cache.indexA[1]));
temp.set(localPointA2);
temp.subLocal(localPointA1);
Vec2.crossToOutUnsafe(temp, 1.0f, ref m_axis);
m_axis.normalize();
Rot.mulToOutUnsafe(xfa.q, m_axis, ref normal);
m_localPoint.set(localPointA1);
m_localPoint.addLocal(localPointA2);
m_localPoint.mulLocal(.5f);
Transform.mulToOutUnsafe(xfa, m_localPoint, ref pointA);
localPointB.set(m_proxyB.getVertex(cache.indexB[0]));
Transform.mulToOutUnsafe(xfb, localPointB, ref pointB);
temp.set(pointB);
temp.subLocal(pointA);
float s = Vec2.dot(temp, normal);
if (s < 0.0f)
{
m_axis.negateLocal();
s = -s;
}
return s;
}
}
public void set(SimplexCache sc)
{
Array.Copy(sc.indexA, 0, indexA, 0, indexA.Length);
Array.Copy(sc.indexB, 0, indexB, 0, indexB.Length);
metric = sc.metric;
count = sc.count;
}
public void writeCache(SimplexCache cache)
{
cache.metric = getMetric();
cache.count = m_count;
for (int i = 0; i < m_count; ++i)
{
cache.indexA[i] = (vertices[i].indexA);
cache.indexB[i] = (vertices[i].indexB);
}
}
/**
* Compute the closest points between two shapes. Supports any combination of: CircleShape and
* PolygonShape. The simplex cache is input/output. On the first call set SimplexCache.count to
* zero.
*
* @param output
* @param cache
* @param input
*/
public void distance(DistanceOutput output, SimplexCache cache, DistanceInput input)
{
GJK_CALLS++;
DistanceProxy proxyA = input.proxyA;
DistanceProxy proxyB = input.proxyB;
Transform transformA = input.transformA;
Transform transformB = input.transformB;
// Initialize the simplex.
simplex.readCache(cache, proxyA, transformA, proxyB, transformB);
// Get simplex vertices as an array.
SimplexVertex[] vertices = simplex.vertices;
// These store the vertices of the last simplex so that we
// can check for duplicates and prevent cycling.
// (pooled above)
int saveCount = 0;
simplex.getClosestPoint(ref closestPoint);
float distanceSqr1 = closestPoint.lengthSquared();
float distanceSqr2 = distanceSqr1;
// Main iteration loop
int iter = 0;
while (iter < MAX_ITERS)
{
// 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.
simplex.getClosestPoint(ref closestPoint);
distanceSqr2 = closestPoint.lengthSquared();
// ensure progress
if (distanceSqr2 >= distanceSqr1)
{
// break;
}
distanceSqr1 = distanceSqr2;
// get search direction;
simplex.getSearchDirection(ref d);
// Ensure the search direction is numerically fit.
if (d.lengthSquared() < Settings.EPSILON*Settings.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;
}
/*
* SimplexVertex* vertex = vertices + simplex.m_count; vertex.indexA =
* proxyA.GetSupport(MulT(transformA.R, -d)); vertex.wA = Mul(transformA,
* proxyA.GetVertex(vertex.indexA)); Vec2 wBLocal; vertex.indexB =
* proxyB.GetSupport(MulT(transformB.R, d)); vertex.wB = Mul(transformB,
* proxyB.GetVertex(vertex.indexB)); vertex.w = vertex.wB - vertex.wA;
*/
// Compute a tentative new simplex vertex using support points.
SimplexVertex vertex = vertices[simplex.m_count];
d.negateLocal();
Rot.mulTransUnsafe(transformA.q, d, ref temp);
vertex.indexA = proxyA.getSupport(temp);
Transform.mulToOutUnsafe(transformA, proxyA.getVertex(vertex.indexA), ref vertex.wA);
// Vec2 wBLocal;
d.negateLocal();
Rot.mulTransUnsafe(transformB.q, d, ref temp);
vertex.indexB = proxyB.getSupport(temp);
Transform.mulToOutUnsafe(transformB, proxyB.getVertex(vertex.indexB), ref vertex.wB);
vertex.w.set(vertex.wB);
vertex.w.subLocal(vertex.wA);
// Iteration count is equated to the number of support point calls.
++iter;
++GJK_ITERS;
// 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;
}
GJK_MAX_ITERS = MathUtils.max(GJK_MAX_ITERS, iter);
// Prepare output.
simplex.getWitnessPoints(output.pointA, output.pointB);
output.distance = MathUtils.distance(output.pointA, output.pointB);
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 > Settings.EPSILON)
{
// Shapes are still no overlapped.
// Move the witness points to the outer surface.
output.distance -= rA + rB;
normal.set(output.pointB);
normal.subLocal(output.pointA);
normal.normalize();
temp.set(normal);
temp.mulLocal(rA);
output.pointA.addLocal(temp);
temp.set(normal);
temp.mulLocal(rB);
output.pointB.subLocal(temp);
}
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.addLocal(output.pointB);
output.pointA.mulLocal(.5f);
output.pointB.set(output.pointA);
output.distance = 0.0f;
}
}
}
public void readCache(SimplexCache cache, DistanceProxy proxyA, Transform transformA,
DistanceProxy proxyB, Transform transformB)
{
Debug.Assert(cache.count <= 3);
// Copy data from cache.
m_count = cache.count;
for (int i = 0; i < m_count; ++i)
{
SimplexVertex v = vertices[i];
v.indexA = cache.indexA[i];
v.indexB = cache.indexB[i];
Vec2 wALocal = proxyA.getVertex(v.indexA);
Vec2 wBLocal = proxyB.getVertex(v.indexB);
Transform.mulToOutUnsafe(transformA, wALocal, ref v.wA);
Transform.mulToOutUnsafe(transformB, wBLocal, ref v.wB);
v.w.set(v.wB);
v.w.subLocal(v.wA);
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 < Settings.EPSILON)
{
// Reset the simplex.
m_count = 0;
}
}
// If the cache is empty or invalid ...
if (m_count == 0)
{
SimplexVertex v = vertices[0];
v.indexA = 0;
v.indexB = 0;
Vec2 wALocal = proxyA.getVertex(0);
Vec2 wBLocal = proxyB.getVertex(0);
Transform.mulToOutUnsafe(transformA, wALocal, ref v.wA);
Transform.mulToOutUnsafe(transformB, wBLocal, ref v.wB);
v.w.set(v.wB);
v.w.subLocal(v.wA);
m_count = 1;
}
}
