public static void ReadCache(
out Simplex simplex,
SimplexCache cache,
WorldTransform xfA,
DistanceProxy proxyA,
WorldTransform xfB,
DistanceProxy proxyB)
{
System.Diagnostics.Debug.Assert(cache.Count <= 3);
// Copy data from cache.
simplex = new Simplex {
Count = cache.Count
};
for (var i = 0; i < simplex.Count; ++i)
{
SimplexVertex v;
v.IndexA = cache.IndexA[i];
v.IndexB = cache.IndexB[i];
v.VertexA = xfA.ToGlobal(proxyA.Vertices[v.IndexA]);
v.VertexB = xfB.ToGlobal(proxyB.Vertices[v.IndexB]);
// ReSharper disable RedundantCast Necessary for FarPhysics.
v.VertexDelta = (LocalPoint)(v.VertexB - v.VertexA);
// ReSharper restore RedundantCast
v.Alpha = 0.0f;
simplex.Vertices[i] = v;
}
// Compute the new simplex metric, if it is substantially different than
// old metric then flush the simplex.
if (simplex.Count > 1)
{
var metric1 = cache.Metric;
var metric2 = simplex.GetMetric();
if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < float.Epsilon)
{
// Reset the simplex, triggers computation below.
simplex.Count = 0;
}
}
// If the cache is empty or invalid ...
if (simplex.Count == 0)
{
SimplexVertex v;
v.IndexA = 0;
v.IndexB = 0;
v.VertexA = xfA.ToGlobal(proxyA.Vertices[0]);
v.VertexB = xfB.ToGlobal(proxyB.Vertices[0]);
// ReSharper disable RedundantCast Necessary for FarPhysics.
v.VertexDelta = (LocalPoint)(v.VertexB - v.VertexA);
// ReSharper restore RedundantCast
v.Alpha = 1.0f;
simplex.Vertices.Item1 = v;
simplex.Count = 1;
}
}
