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 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.
}
public SeparationFunction(ref SimplexCache cache,
Shape shapeA, ref XForm transformA,
Shape shapeB, ref XForm transformB)
{
_localPoint = Vector2.Zero;
_shapeA = shapeA;
_shapeB = shapeB;
int count = cache.count;
Debug.Assert(0 < count && count < 3);
if (count == 1)
{
_type = SeparationFunctionType.Points;
Vector2 localPointA = _shapeA.GetVertex(cache.indexA[0]);
Vector2 localPointB = _shapeB.GetVertex(cache.indexB[0]);
Vector2 pointA = MathUtils.Multiply(ref transformA, localPointA);
Vector2 pointB = MathUtils.Multiply(ref transformB, localPointB);
_axis = pointB - pointA;
_axis.Normalize();
}
else if (cache.indexB[0] == cache.indexB[1])
{
// Two points on A and one on B
_type = SeparationFunctionType.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 = MathUtils.Cross(localPointA2 - localPointA1, 1.0f);
_axis.Normalize();
Vector2 normal = MathUtils.Multiply(ref transformA.R, _axis);
Vector2 pointA = MathUtils.Multiply(ref transformA, _localPoint);
Vector2 pointB = MathUtils.Multiply(ref transformB, localPointB);
float s = Vector2.Dot(pointB - pointA, normal);
if (s < 0.0f)
{
_axis = -_axis;
}
}
else if (cache.indexA[0] == cache.indexA[1])
{
// Two points on B and one on A.
_type = SeparationFunctionType.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 = MathUtils.Cross(localPointB2 - localPointB1, 1.0f);
_axis.Normalize();
Vector2 normal = MathUtils.Multiply(ref transformB.R, _axis);
Vector2 pointB = MathUtils.Multiply(ref transformB, _localPoint);
Vector2 pointA = MathUtils.Multiply(ref transformA, localPointA);
float s = Vector2.Dot(pointA - pointB, normal);
if (s < 0.0f)
{
_axis = -_axis;
}
}
else
{
// Two points on B and two points on A.
// The faces are parallel.
Vector2 localPointA1 = _shapeA.GetVertex(cache.indexA[0]);
Vector2 localPointA2 = _shapeA.GetVertex(cache.indexA[1]);
Vector2 localPointB1 = _shapeB.GetVertex(cache.indexB[0]);
Vector2 localPointB2 = _shapeB.GetVertex(cache.indexB[1]);
Vector2 pA = MathUtils.Multiply(ref transformA, localPointA1);
Vector2 dA = MathUtils.Multiply(ref transformA.R, localPointA2 - localPointA1);
Vector2 pB = MathUtils.Multiply(ref transformB, localPointB1);
Vector2 dB = MathUtils.Multiply(ref transformB.R, localPointB2 - localPointB1);
float a = Vector2.Dot(dA, dA);
float e = Vector2.Dot(dB, dB);
Vector2 r = pA - pB;
float c = Vector2.Dot(dA, r);
float f = Vector2.Dot(dB, r);
float b = Vector2.Dot(dA, dB);
float denom = a * e - b * b;
float s = 0.0f;
if (denom != 0.0f)
{
s = MathUtils.Clamp((b * f - c * e) / denom, 0.0f, 1.0f);
}
float t = (b * s + f) / e;
if (t < 0.0f)
{
t = 0.0f;
s = MathUtils.Clamp(-c / a, 0.0f, 1.0f);
}
else if (t > 1.0f)
{
t = 1.0f;
s = MathUtils.Clamp((b - c) / a, 0.0f, 1.0f);
}
Vector2 localPointA = localPointA1 + s * (localPointA2 - localPointA1);
Vector2 localPointB = localPointB1 + t * (localPointB2 - localPointB1);
if (s == 0.0f || s == 1.0f)
{
_type = SeparationFunctionType.FaceB;
_axis = MathUtils.Cross(localPointB2 - localPointB1, 1.0f);
_axis.Normalize();
_localPoint = localPointB;
Vector2 normal = MathUtils.Multiply(ref transformB.R, _axis);
Vector2 pointA = MathUtils.Multiply(ref transformA, localPointA);
Vector2 pointB = MathUtils.Multiply(ref transformB, localPointB);
float sgn = Vector2.Dot(pointA - pointB, normal);
if (sgn < 0.0f)
{
_axis = -_axis;
}
}
else
{
_type = SeparationFunctionType.FaceA;
_axis = MathUtils.Cross(localPointA2 - localPointA1, 1.0f);
_axis.Normalize();
_localPoint = localPointA;
Vector2 normal = MathUtils.Multiply(ref transformA.R, _axis);
Vector2 pointA = MathUtils.Multiply(ref transformA, localPointA);
Vector2 pointB = MathUtils.Multiply(ref transformB, localPointB);
float sgn = Vector2.Dot(pointB - pointA, normal);
if (sgn < 0.0f)
{
_axis = -_axis;
}
}
}
}
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]);
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.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]);
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.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;
}
}
