/// <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;
}
}
}
internal unsafe void Initialize(SimplexCache *cache,
Shape shapeA, XForm transformA,
Shape shapeB, XForm transformB)
{
ShapeA = shapeA;
ShapeB = shapeB;
int count = cache->Count;
Box2DNetDebug.Assert(0 < count && count < 3);
if (count == 1)
{
FaceType = Type.Points;
Vec2 localPointA = ShapeA.GetVertex(cache->IndexA[0]);
Vec2 localPointB = ShapeB.GetVertex(cache->IndexB[0]);
Vec2 pointA = Common.Math.Mul(transformA, localPointA);
Vec2 pointB = Common.Math.Mul(transformB, localPointB);
Axis = pointB - pointA;
Axis.Normalize();
}
else if (cache->IndexB[0] == cache->IndexB[1])
{
// Two points on A and one on B
FaceType = Type.FaceA;
Vec2 localPointA1 = ShapeA.GetVertex(cache->IndexA[0]);
Vec2 localPointA2 = ShapeA.GetVertex(cache->IndexA[1]);
Vec2 localPointB = ShapeB.GetVertex(cache->IndexB[0]);
LocalPoint = 0.5f * (localPointA1 + localPointA2);
Axis = Vec2.Cross(localPointA2 - localPointA1, 1.0f);
Axis.Normalize();
Vec2 normal = Common.Math.Mul(transformA.R, Axis);
Vec2 pointA = Common.Math.Mul(transformA, LocalPoint);
Vec2 pointB = Common.Math.Mul(transformB, localPointB);
float s = Vec2.Dot(pointB - pointA, normal);
if (s < 0.0f)
{
Axis = -Axis;
}
}
else
{
// Two points on B and one or two points on A.
// We ignore the second point on A.
FaceType = Type.FaceB;
Vec2 localPointA = shapeA.GetVertex(cache->IndexA[0]);
Vec2 localPointB1 = shapeB.GetVertex(cache->IndexB[0]);
Vec2 localPointB2 = shapeB.GetVertex(cache->IndexB[1]);
LocalPoint = 0.5f * (localPointB1 + localPointB2);
Axis = Vec2.Cross(localPointB2 - localPointB1, 1.0f);
Axis.Normalize();
Vec2 normal = Common.Math.Mul(transformB.R, Axis);
Vec2 pointB = Common.Math.Mul(transformB, LocalPoint);
Vec2 pointA = Common.Math.Mul(transformA, localPointA);
float s = Vec2.Dot(pointA - pointB, normal);
if (s < 0.0f)
{
Axis = -Axis;
}
}
}
internal unsafe void Initialize(SimplexCache *cache,
Shape shapeA, Transform TransformA,
Shape shapeB, Transform TransformB)
{
ShapeA = shapeA;
ShapeB = shapeB;
int count = cache->Count;
Box2DNetDebug.Assert(0 < count && count < 3);
if (count == 1)
{
FaceType = Type.Points;
Vector2 localPointA = ShapeA.GetVertex(cache->IndexA[0]);
Vector2 localPointB = ShapeB.GetVertex(cache->IndexB[0]);
Vector2 pointA = TransformA.TransformPoint(localPointA);
Vector2 pointB = TransformB.TransformPoint(localPointB);
Axis = pointB - pointA;
Axis.Normalize();
}
else if (cache->IndexB[0] == cache->IndexB[1])
{
// Two points on A and one on B
FaceType = Type.FaceA;
Vector2 localPointA1 = ShapeA.GetVertex(cache->IndexA[0]);
Vector2 localPointA2 = ShapeA.GetVertex(cache->IndexA[1]);
Vector2 localPointB = ShapeB.GetVertex(cache->IndexB[0]);
LocalPoint = 0.5f * (localPointA1 + localPointA2);
Axis = (localPointA2 - localPointA1).CrossScalarPostMultiply(1.0f);
Axis.Normalize();
Vector2 normal = TransformA.TransformDirection(Axis);
Vector2 pointA = TransformA.TransformPoint(LocalPoint);
Vector2 pointB = TransformB.TransformPoint(localPointB);
float s = Vector2.Dot(pointB - pointA, normal);
if (s < 0.0f)
{
Axis = -Axis;
}
}
else
{
// Two points on B and one or two points on A.
// We ignore the second point on A.
FaceType = Type.FaceB;
Vector2 localPointA = shapeA.GetVertex(cache->IndexA[0]);
Vector2 localPointB1 = shapeB.GetVertex(cache->IndexB[0]);
Vector2 localPointB2 = shapeB.GetVertex(cache->IndexB[1]);
LocalPoint = 0.5f * (localPointB1 + localPointB2);
Axis = (localPointB2 - localPointB1).CrossScalarPostMultiply(1.0f);
Axis.Normalize();
Vector2 normal = TransformB.TransformDirection(Axis);
Vector2 pointB = TransformB.TransformPoint(LocalPoint);
Vector2 pointA = TransformA.TransformPoint(localPointA);
float s = Vector2.Dot(pointA - pointB, normal);
if (s < 0.0f)
{
Axis = -Axis;
}
}
}
