internal void ReadCache(ref SimplexCache cache, DistanceProxy proxyA, ref Transform transformA, 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];
TSVector2 wALocal = proxyA.Vertices[v.IndexA];
TSVector2 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)
{
FP metric1 = cache.Metric;
FP metric2 = GetMetric();
if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < Settings.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;
TSVector2 wALocal = proxyA.Vertices[0];
TSVector2 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;
}
}
internal void ReadCache(ref SimplexCache cache, DistanceProxy proxyA, ref Transform transformA, DistanceProxy proxyB, ref Transform transformB)
{
Debug.Assert(cache.Count <= 3);
this.Count = (int)cache.Count;
for (int i = 0; i < this.Count; i++)
{
SimplexVertex simplexVertex = this.V[i];
simplexVertex.IndexA = (int)cache.IndexA[i];
simplexVertex.IndexB = (int)cache.IndexB[i];
TSVector2 v = proxyA.Vertices[simplexVertex.IndexA];
TSVector2 v2 = proxyB.Vertices[simplexVertex.IndexB];
simplexVertex.WA = MathUtils.Mul(ref transformA, v);
simplexVertex.WB = MathUtils.Mul(ref transformB, v2);
simplexVertex.W = simplexVertex.WB - simplexVertex.WA;
simplexVertex.A = 0f;
this.V[i] = simplexVertex;
}
bool flag = this.Count > 1;
if (flag)
{
FP metric = cache.Metric;
FP metric2 = this.GetMetric();
bool flag2 = metric2 < 0.5f * metric || 2f * metric < metric2 || metric2 < Settings.Epsilon;
if (flag2)
{
this.Count = 0;
}
}
bool flag3 = this.Count == 0;
if (flag3)
{
SimplexVertex simplexVertex2 = this.V[0];
simplexVertex2.IndexA = 0;
simplexVertex2.IndexB = 0;
TSVector2 v3 = proxyA.Vertices[0];
TSVector2 v4 = proxyB.Vertices[0];
simplexVertex2.WA = MathUtils.Mul(ref transformA, v3);
simplexVertex2.WB = MathUtils.Mul(ref transformB, v4);
simplexVertex2.W = simplexVertex2.WB - simplexVertex2.WA;
simplexVertex2.A = 1f;
this.V[0] = simplexVertex2;
this.Count = 1;
}
}
public static void Set(ref SimplexCache cache, DistanceProxy proxyA, ref Sweep sweepA, DistanceProxy proxyB, ref Sweep sweepB, FP t1)
{
SeparationFunction._localPoint = TSVector2.zero;
SeparationFunction._proxyA = proxyA;
SeparationFunction._proxyB = proxyB;
int count = (int)cache.Count;
Debug.Assert(0 < count && count < 3);
SeparationFunction._sweepA = sweepA;
SeparationFunction._sweepB = sweepB;
Transform transform;
SeparationFunction._sweepA.GetTransform(out transform, t1);
Transform transform2;
SeparationFunction._sweepB.GetTransform(out transform2, t1);
bool flag = count == 1;
if (flag)
{
SeparationFunction._type = SeparationFunctionType.Points;
TSVector2 v = SeparationFunction._proxyA.Vertices[(int)cache.IndexA[0]];
TSVector2 v2 = SeparationFunction._proxyB.Vertices[(int)cache.IndexB[0]];
TSVector2 value = MathUtils.Mul(ref transform, v);
TSVector2 value2 = MathUtils.Mul(ref transform2, v2);
SeparationFunction._axis = value2 - value;
SeparationFunction._axis.Normalize();
}
else
{
bool flag2 = cache.IndexA[0] == cache.IndexA[1];
if (flag2)
{
SeparationFunction._type = SeparationFunctionType.FaceB;
TSVector2 tSVector = proxyB.Vertices[(int)cache.IndexB[0]];
TSVector2 tSVector2 = proxyB.Vertices[(int)cache.IndexB[1]];
TSVector2 tSVector3 = tSVector2 - tSVector;
SeparationFunction._axis = new TSVector2(tSVector3.y, -tSVector3.x);
SeparationFunction._axis.Normalize();
TSVector2 value3 = MathUtils.Mul(ref transform2.q, SeparationFunction._axis);
SeparationFunction._localPoint = 0.5f * (tSVector + tSVector2);
TSVector2 value4 = MathUtils.Mul(ref transform2, SeparationFunction._localPoint);
TSVector2 v3 = proxyA.Vertices[(int)cache.IndexA[0]];
TSVector2 value5 = MathUtils.Mul(ref transform, v3);
FP x = TSVector2.Dot(value5 - value4, value3);
bool flag3 = x < 0f;
if (flag3)
{
SeparationFunction._axis = -SeparationFunction._axis;
}
}
else
{
SeparationFunction._type = SeparationFunctionType.FaceA;
TSVector2 tSVector4 = SeparationFunction._proxyA.Vertices[(int)cache.IndexA[0]];
TSVector2 tSVector5 = SeparationFunction._proxyA.Vertices[(int)cache.IndexA[1]];
TSVector2 tSVector6 = tSVector5 - tSVector4;
SeparationFunction._axis = new TSVector2(tSVector6.y, -tSVector6.x);
SeparationFunction._axis.Normalize();
TSVector2 value6 = MathUtils.Mul(ref transform.q, SeparationFunction._axis);
SeparationFunction._localPoint = 0.5f * (tSVector4 + tSVector5);
TSVector2 value7 = MathUtils.Mul(ref transform, SeparationFunction._localPoint);
TSVector2 v4 = SeparationFunction._proxyB.Vertices[(int)cache.IndexB[0]];
TSVector2 value8 = MathUtils.Mul(ref transform2, v4);
FP x2 = TSVector2.Dot(value8 - value7, value6);
bool flag4 = x2 < 0f;
if (flag4)
{
SeparationFunction._axis = -SeparationFunction._axis;
}
}
}
}
public static void Set(ref SimplexCache cache, DistanceProxy proxyA, ref Sweep sweepA, DistanceProxy proxyB, ref Sweep sweepB, FP t1)
{
_localPoint = TSVector2.zero;
_proxyA = proxyA;
_proxyB = proxyB;
int count = cache.Count;
Debug.Assert(0 < count && count < 3);
_sweepA = sweepA;
_sweepB = sweepB;
Transform xfA, xfB;
_sweepA.GetTransform(out xfA, t1);
_sweepB.GetTransform(out xfB, t1);
if (count == 1)
{
_type = SeparationFunctionType.Points;
TSVector2 localPointA = _proxyA.Vertices[cache.IndexA[0]];
TSVector2 localPointB = _proxyB.Vertices[cache.IndexB[0]];
TSVector2 pointA = MathUtils.Mul(ref xfA, localPointA);
TSVector2 pointB = MathUtils.Mul(ref xfB, localPointB);
_axis = pointB - pointA;
_axis.Normalize();
}
else if (cache.IndexA[0] == cache.IndexA[1])
{
// Two points on B and one on A.
_type = SeparationFunctionType.FaceB;
TSVector2 localPointB1 = proxyB.Vertices[cache.IndexB[0]];
TSVector2 localPointB2 = proxyB.Vertices[cache.IndexB[1]];
TSVector2 a = localPointB2 - localPointB1;
_axis = new TSVector2(a.y, -a.x);
_axis.Normalize();
TSVector2 normal = MathUtils.Mul(ref xfB.q, _axis);
_localPoint = 0.5f * (localPointB1 + localPointB2);
TSVector2 pointB = MathUtils.Mul(ref xfB, _localPoint);
TSVector2 localPointA = proxyA.Vertices[cache.IndexA[0]];
TSVector2 pointA = MathUtils.Mul(ref xfA, localPointA);
FP s = TSVector2.Dot(pointA - pointB, normal);
if (s < 0.0f)
{
_axis = -_axis;
}
}
else
{
// Two points on A and one or two points on B.
_type = SeparationFunctionType.FaceA;
TSVector2 localPointA1 = _proxyA.Vertices[cache.IndexA[0]];
TSVector2 localPointA2 = _proxyA.Vertices[cache.IndexA[1]];
TSVector2 a = localPointA2 - localPointA1;
_axis = new TSVector2(a.y, -a.x);
_axis.Normalize();
TSVector2 normal = MathUtils.Mul(ref xfA.q, _axis);
_localPoint = 0.5f * (localPointA1 + localPointA2);
TSVector2 pointA = MathUtils.Mul(ref xfA, _localPoint);
TSVector2 localPointB = _proxyB.Vertices[cache.IndexB[0]];
TSVector2 pointB = MathUtils.Mul(ref xfB, localPointB);
FP s = TSVector2.Dot(pointB - pointA, normal);
if (s < 0.0f)
{
_axis = -_axis;
}
}
//FPE note: the returned value that used to be here has been removed, as it was not used.
}