public void ReadCache(b2SimplexCache cache, b2DistanceProxy proxyA, b2Transform transformA, b2DistanceProxy proxyB, b2Transform transformB)
{
Debug.Assert(cache.count <= 3);
// Copy data from cache.
m_count = cache.count;
b2SimplexVertex[] vertices = { m_v1, m_v2, m_v3 };
for (int i = 0; i < m_count; ++i)
{
b2SimplexVertex v = vertices[i];
v.indexA = cache.indexA[i];
v.indexB = cache.indexB[i];
b2Vec2 wALocal = proxyA.GetVertex(v.indexA);
b2Vec2 wBLocal = proxyB.GetVertex(v.indexB);
v.wA = Utils.b2Mul(transformA, wALocal);
v.wB = Utils.b2Mul(transformB, wBLocal);
v.w = v.wB - 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 < float.Epsilon)
{
// 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;
b2Vec2 wALocal = proxyA.GetVertex(0);
b2Vec2 wBLocal = proxyB.GetVertex(0);
v.wA = Utils.b2Mul(transformA, wALocal);
v.wB = Utils.b2Mul(transformB, wBLocal);
v.w = v.wB - v.wA;
v.a = 1.0f;
m_count = 1;
}
}
// TODO_ERIN might not need to return the separation
public float Initialize(b2SimplexCache cache, b2DistanceProxy proxyA, b2Sweep sweepA, b2DistanceProxy proxyB, b2Sweep 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;
b2Transform xfA = new b2Transform();
b2Transform xfB = new b2Transform();
m_sweepA.GetTransform(ref xfA, t1);
m_sweepB.GetTransform(ref xfB, t1);
if (count == 1)
{
m_type = Type.e_points;
b2Vec2 localPointA = m_proxyA.GetVertex(cache.indexA[0]);
b2Vec2 localPointB = m_proxyB.GetVertex(cache.indexB[0]);
b2Vec2 pointA = Utils.b2Mul(xfA, localPointA);
b2Vec2 pointB = Utils.b2Mul(xfB, localPointB);
m_axis = pointB - 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 = Type.e_faceB;
b2Vec2 localPointB1 = proxyB.GetVertex(cache.indexB[0]);
b2Vec2 localPointB2 = proxyB.GetVertex(cache.indexB[1]);
m_axis = Utils.b2Cross(localPointB2 - localPointB1, 1.0f);
m_axis.Normalize();
b2Vec2 normal = Utils.b2Mul(xfB.q, m_axis);
m_localPoint = 0.5f * (localPointB1 + localPointB2);
b2Vec2 pointB = Utils.b2Mul(xfB, m_localPoint);
b2Vec2 localPointA = proxyA.GetVertex(cache.indexA[0]);
b2Vec2 pointA = Utils.b2Mul(xfA, localPointA);
float s = Utils.b2Dot(pointA - pointB, normal);
if (s < 0.0f)
{
m_axis = -m_axis;
s = -s;
}
return(s);
}
else
{
// Two points on A and one or two points on B.
m_type = Type.e_faceA;
b2Vec2 localPointA1 = m_proxyA.GetVertex(cache.indexA[0]);
b2Vec2 localPointA2 = m_proxyA.GetVertex(cache.indexA[1]);
m_axis = Utils.b2Cross(localPointA2 - localPointA1, 1.0f);
m_axis.Normalize();
b2Vec2 normal = Utils.b2Mul(xfA.q, m_axis);
m_localPoint = 0.5f * (localPointA1 + localPointA2);
b2Vec2 pointA = Utils.b2Mul(xfA, m_localPoint);
b2Vec2 localPointB = m_proxyB.GetVertex(cache.indexB[0]);
b2Vec2 pointB = Utils.b2Mul(xfB, localPointB);
float s = Utils.b2Dot(pointB - pointA, normal);
if (s < 0.0f)
{
m_axis = -m_axis;
s = -s;
}
return(s);
}
}
internal static global::System.Runtime.InteropServices.HandleRef getCPtr(b2DistanceProxy obj)
{
return((obj == null) ? new global::System.Runtime.InteropServices.HandleRef(null, global::System.IntPtr.Zero) : obj.swigCPtr);
}
