internal unsafe void ReadCache(SimplexCache *cache, Shape shapeA, Transform TransformA, Shape shapeB, Transform TransformB)
{
Box2DXDebug.Assert(0 <= cache->Count && cache->Count <= 3);
// Copy data from cache.
_count = cache->Count;
SimplexVertex **vertices = stackalloc SimplexVertex *[3];
fixed(SimplexVertex *v1Ptr = &_v1, v2Ptr = &_v2, v3Ptr = &_v3)
{
vertices[0] = v1Ptr;
vertices[1] = v2Ptr;
vertices[2] = v3Ptr;
for (int i = 0; i < _count; ++i)
{
SimplexVertex *v = vertices[i];
v->indexA = cache->IndexA[i];
v->indexB = cache->IndexB[i];
Vector2 wALocal = shapeA.GetVertex(v->indexA);
Vector2 wBLocal = shapeB.GetVertex(v->indexB);
v->wA = TransformA.TransformPoint(wALocal);
v->wB = TransformB.TransformPoint(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 (_count > 1)
{
float metric1 = cache->Metric;
float metric2 = GetMetric();
if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < Common.Settings.FLT_EPSILON)
{
// Reset the simplex.
_count = 0;
}
}
// If the cache is empty or invalid ...
if (_count == 0)
{
SimplexVertex *v = vertices[0];
v->indexA = 0;
v->indexB = 0;
Vector2 wALocal = shapeA.GetVertex(0);
Vector2 wBLocal = shapeB.GetVertex(0);
v->wA = TransformA.TransformPoint(wALocal);
v->wB = TransformB.TransformPoint(wBLocal);
v->w = v->wB - v->wA;
_count = 1;
}
}
}