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)
{
var v = V[i];
v.IndexA = cache.IndexA[i];
v.IndexB = cache.IndexB[i];
var wALocal = proxyA.vertices[v.IndexA];
var 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 < Settings.Epsilon)
{
// Reset the simplex.
Count = 0;
}
}
// If the cache is empty or invalid ...
if (Count == 0)
{
var v = V[0];
v.IndexA = 0;
v.IndexB = 0;
var wALocal = proxyA.vertices[0];
var 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;
}
}
public static void Set(ref SimplexCache cache,
DistanceProxy proxyA, ref Sweep sweepA,
DistanceProxy proxyB, ref Sweep sweepB,
float t1)
{
_localPoint = Vector2.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;
Vector2 localPointA = _proxyA.Vertices[cache.IndexA[0]];
Vector2 localPointB = _proxyB.Vertices[cache.IndexB[0]];
Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA);
Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB);
_axis = pointB - pointA;
_axis.Normalize();
return;
}
else if (cache.IndexA[0] == cache.IndexA[1])
{
// Two points on B and one on A.
_type = SeparationFunctionType.FaceB;
Vector2 localPointB1 = proxyB.Vertices[cache.IndexB[0]];
Vector2 localPointB2 = proxyB.Vertices[cache.IndexB[1]];
Vector2 a = localPointB2 - localPointB1;
_axis = new Vector2(a.Y, -a.X);
_axis.Normalize();
Vector2 normal = MathUtils.Multiply(ref xfB.R, _axis);
_localPoint = 0.5f * (localPointB1 + localPointB2);
Vector2 pointB = MathUtils.Multiply(ref xfB, _localPoint);
Vector2 localPointA = proxyA.Vertices[cache.IndexA[0]];
Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA);
float s = Vector2.Dot(pointA - pointB, normal);
if (s < 0.0f)
{
_axis = -_axis;
s = -s;
}
return;
}
else
{
// Two points on A and one or two points on B.
_type = SeparationFunctionType.FaceA;
Vector2 localPointA1 = _proxyA.Vertices[cache.IndexA[0]];
Vector2 localPointA2 = _proxyA.Vertices[cache.IndexA[1]];
Vector2 a = localPointA2 - localPointA1;
_axis = new Vector2(a.Y, -a.X);
_axis.Normalize();
Vector2 normal = MathUtils.Multiply(ref xfA.R, _axis);
_localPoint = 0.5f * (localPointA1 + localPointA2);
Vector2 pointA = MathUtils.Multiply(ref xfA, _localPoint);
Vector2 localPointB = _proxyB.Vertices[cache.IndexB[0]];
Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB);
float s = Vector2.Dot(pointB - pointA, normal);
if (s < 0.0f)
{
_axis = -_axis;
s = -s;
}
return;
}
}
public SeparationFunction(ref SimplexCache cache,
ref DistanceProxy proxyA, ref Sweep sweepA,
ref DistanceProxy proxyB, ref Sweep sweepB,
float t1)
{
_localPoint = Vector2.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;
Vector2 localPointA = _proxyA.GetVertex(cache.IndexA[0]);
Vector2 localPointB = _proxyB.GetVertex(cache.IndexB[0]);
Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA);
Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB);
_axis = pointB - pointA;
_axis.Normalize();
return;
}
else if (cache.IndexA[0] == cache.IndexA[1])
{
// Two points on B and one on A.
_type = SeparationFunctionType.FaceB;
Vector2 localPointB1 = proxyB.GetVertex(cache.IndexB[0]);
Vector2 localPointB2 = proxyB.GetVertex(cache.IndexB[1]);
_axis = MathUtils.Cross(localPointB2 - localPointB1, 1.0f);
_axis.Normalize();
Vector2 normal = MathUtils.Multiply(ref xfB.R, _axis);
_localPoint = 0.5f * (localPointB1 + localPointB2);
Vector2 pointB = MathUtils.Multiply(ref xfB, _localPoint);
Vector2 localPointA = proxyA.GetVertex(cache.IndexA[0]);
Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA);
float s = Vector2.Dot(pointA - pointB, normal);
if (s < 0.0f)
{
_axis = -_axis;
s = -s;
}
return;
}
else
{
// Two points on A and one or two points on B.
_type = SeparationFunctionType.FaceA;
Vector2 localPointA1 = _proxyA.GetVertex(cache.IndexA[0]);
Vector2 localPointA2 = _proxyA.GetVertex(cache.IndexA[1]);
_axis = MathUtils.Cross(localPointA2 - localPointA1, 1.0f);
_axis.Normalize();
Vector2 normal = MathUtils.Multiply(ref xfA.R, _axis);
_localPoint = 0.5f * (localPointA1 + localPointA2);
Vector2 pointA = MathUtils.Multiply(ref xfA, _localPoint);
Vector2 localPointB = _proxyB.GetVertex(cache.IndexB[0]);
Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB);
float s = Vector2.Dot(pointB - pointA, normal);
if (s < 0.0f)
{
_axis = -_axis;
s = -s;
}
return;
}
}
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];
Vector2 wALocal = proxyA.Vertices[v.IndexA];
Vector2 wBLocal = proxyB.Vertices[v.IndexB];
v.WA = MathUtils.Multiply(ref transformA, wALocal);
v.WB = MathUtils.Multiply(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 < 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;
Vector2 wALocal = proxyA.Vertices[0];
Vector2 wBLocal = proxyB.Vertices[0];
v.WA = MathUtils.Multiply(ref transformA, wALocal);
v.WB = MathUtils.Multiply(ref transformB, wBLocal);
v.W = v.WB - v.WA;
V[0] = v;
Count = 1;
}
}
public static void Set(ref SimplexCache cache, ref DistanceProxy proxyA, ref Sweep sweepA, ref DistanceProxy proxyB, ref Sweep sweepB, float t1)
{
_localPoint = Vector2.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;
Vector2 localPointA = _proxyA.Vertices[cache.IndexA[0]];
Vector2 localPointB = _proxyB.Vertices[cache.IndexB[0]];
Vector2 pointA = Transform.Multiply(ref localPointA, ref xfA);
Vector2 pointB = Transform.Multiply(ref localPointB, ref xfB);
_axis = pointB - pointA;
_axis.Normalize();
}
else if (cache.IndexA[0] == cache.IndexA[1])
{
// Two points on B and one on A.
_type = SeparationFunctionType.FaceB;
Vector2 localPointB1 = proxyB.Vertices[cache.IndexB[0]];
Vector2 localPointB2 = proxyB.Vertices[cache.IndexB[1]];
Vector2 a = localPointB2 - localPointB1;
_axis = new Vector2(a.Y, -a.X);
_axis.Normalize();
Vector2 normal = Complex.Multiply(ref _axis, ref xfB.q);
_localPoint = 0.5f * (localPointB1 + localPointB2);
Vector2 pointB = Transform.Multiply(ref _localPoint, ref xfB);
Vector2 localPointA = proxyA.Vertices[cache.IndexA[0]];
Vector2 pointA = Transform.Multiply(ref localPointA, ref xfA);
float s = Vector2.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;
Vector2 localPointA1 = _proxyA.Vertices[cache.IndexA[0]];
Vector2 localPointA2 = _proxyA.Vertices[cache.IndexA[1]];
Vector2 a = localPointA2 - localPointA1;
_axis = new Vector2(a.Y, -a.X);
_axis.Normalize();
Vector2 normal = Complex.Multiply(ref _axis, ref xfA.q);
_localPoint = 0.5f * (localPointA1 + localPointA2);
Vector2 pointA = Transform.Multiply(ref _localPoint, ref xfA);
Vector2 localPointB = _proxyB.Vertices[cache.IndexB[0]];
Vector2 pointB = Transform.Multiply(ref localPointB, ref xfB);
float s = Vector2.Dot(pointB - pointA, normal);
if (s < 0.0f)
{
_axis = -_axis;
}
}
}
public static void Set(ref SimplexCache cache, DistanceProxy proxyA, ref Sweep sweepA, DistanceProxy proxyB, ref Sweep sweepB, float t1)
{
_localPoint = Vector2.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;
Vector2 localPointA = _proxyA.Vertices[cache.IndexA[0]];
Vector2 localPointB = _proxyB.Vertices[cache.IndexB[0]];
Vector2 pointA = MathUtils.Mul(ref xfA, localPointA);
Vector2 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;
Vector2 localPointB1 = proxyB.Vertices[cache.IndexB[0]];
Vector2 localPointB2 = proxyB.Vertices[cache.IndexB[1]];
Vector2 a = localPointB2 - localPointB1;
_axis = new Vector2(a.Y, -a.X);
_axis.Normalize();
Vector2 normal = MathUtils.Mul(ref xfB.q, _axis);
_localPoint = 0.5f * (localPointB1 + localPointB2);
Vector2 pointB = MathUtils.Mul(ref xfB, _localPoint);
Vector2 localPointA = proxyA.Vertices[cache.IndexA[0]];
Vector2 pointA = MathUtils.Mul(ref xfA, localPointA);
float s = Vector2.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;
Vector2 localPointA1 = _proxyA.Vertices[cache.IndexA[0]];
Vector2 localPointA2 = _proxyA.Vertices[cache.IndexA[1]];
Vector2 a = localPointA2 - localPointA1;
_axis = new Vector2(a.Y, -a.X);
_axis.Normalize();
Vector2 normal = MathUtils.Mul(ref xfA.q, _axis);
_localPoint = 0.5f * (localPointA1 + localPointA2);
Vector2 pointA = MathUtils.Mul(ref xfA, _localPoint);
Vector2 localPointB = _proxyB.Vertices[cache.IndexB[0]];
Vector2 pointB = MathUtils.Mul(ref xfB, localPointB);
float s = Vector2.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.
}