/// <summary>
/// Writes the cache using the specified cache
/// </summary>
/// <param name="cache">The cache</param>
internal void WriteCache(ref SimplexCache cache)
{
cache.Metric = GetMetric();
cache.Count = (ushort)Count;
for (int i = 0; i < Count; ++i)
{
cache.IndexA[i] = (byte)V[i].IndexA;
cache.IndexB[i] = (byte)V[i].IndexB;
}
}
/// <summary>
/// Reads the cache using the specified cache
/// </summary>
/// <param name="cache">The cache</param>
/// <param name="proxyA">The proxy</param>
/// <param name="transformA">The transform</param>
/// <param name="proxyB">The proxy</param>
/// <param name="transformB">The transform</param>
internal void ReadCache(ref SimplexCache cache, ref DistanceProxy proxyA, ref Transform transformA,
ref DistanceProxy proxyB, ref Transform transformB)
{
Debug.Assert(cache.Count <= 3);
// Copy data from cache.
Count = cache.Count;
for (int i = 0; i < Count; ++i)
{
SimplexVertex v = V[i];
v.IndexA = cache.IndexA[i];
v.IndexB = cache.IndexB[i];
Vector2 wALocal = proxyA.Vertices[v.IndexA];
Vector2 wBLocal = proxyB.Vertices[v.IndexB];
v.Wa = MathUtils.Mul(ref transformA, wALocal);
v.Wb = MathUtils.Mul(ref transformB, wBLocal);
v.W = v.Wb - v.Wa;
v.A = 0.0f;
V[i] = v;
}
// 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 < MathConstants.Epsilon)
{
// Reset the simplex.
Count = 0;
}
}
// If the cache is empty or invalid ...
if (Count == 0)
{
SimplexVertex v = V[0];
v.IndexA = 0;
v.IndexB = 0;
Vector2 wALocal = proxyA.Vertices[0];
Vector2 wBLocal = proxyB.Vertices[0];
v.Wa = MathUtils.Mul(ref transformA, wALocal);
v.Wb = MathUtils.Mul(ref transformB, wBLocal);
v.W = v.Wb - v.Wa;
v.A = 1.0f;
V[0] = v;
Count = 1;
}
}
