// 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 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;
}
}
