/// <summary>
/// Reads the cache using the specified cache
/// </summary>
/// <param name="cache">The cache</param>
/// <param name="shapeA">The shape</param>
/// <param name="transformA">The transform</param>
/// <param name="shapeB">The shape</param>
/// <param name="transformB">The transform</param>
internal unsafe void ReadCache(SimplexCache *cache, Shape shapeA, XForm transformA, Shape shapeB,
XForm 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];
Vec2 wALocal = shapeA.GetVertex(v->IndexA);
Vec2 wBLocal = shapeB.GetVertex(v->IndexB);
v->Wa = Math.Mul(transformA, wALocal);
v->Wb = Math.Mul(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 (Count > 1)
{
float metric1 = cache->Metric;
float metric2 = GetMetric();
if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < Settings.FltEpsilon)
{
// 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;
Vec2 wALocal = shapeA.GetVertex(0);
Vec2 wBLocal = shapeB.GetVertex(0);
v->Wa = Math.Mul(transformA, wALocal);
v->Wb = Math.Mul(transformB, wBLocal);
v->W = v->Wb - v->Wa;
Count = 1;
}
}
}
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;
}
}
}
internal unsafe void WriteCache(SimplexCache *cache)
{
cache->Metric = GetMetric();
cache->Count = (UInt16)_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)
{
cache->IndexA[i] = (Byte)(vertices[i]->indexA);
cache->IndexB[i] = (Byte)(vertices[i]->indexB);
}
}
}
/// <summary>
/// Writes the cache using the specified cache
/// </summary>
/// <param name="cache">The cache</param>
internal unsafe void WriteCache(SimplexCache *cache)
{
cache->Metric = GetMetric();
cache->Count = (ushort)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)
{
cache->IndexA[i] = (byte)vertices[i]->IndexA;
cache->IndexB[i] = (byte)vertices[i]->IndexB;
}
}
}