public void ReadCache(SimplexCache cache,
DistanceProxy shapeA, ref Transform transformA,
DistanceProxy shapeB, ref Transform transformB)
{
Box2DXDebug.Assert(0 <= cache.Count && cache.Count <= 3);
// Copy data from cache.
Count = cache.Count;
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.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;
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;
}
}
public void ReadCache(SimplexCache cache, DistanceProxy proxyA, Transform transformA, DistanceProxy proxyB, Transform transformB)
{
Debug.Assert(cache.Count <= 3);
// Copy data from cache.
Count = cache.Count;
for (int i = 0; i < Count; ++i)
{
SimplexVertex v = Vertices[i];
v.IndexA = cache.IndexA[i];
v.IndexB = cache.IndexB[i];
Vec2 wALocal = proxyA.GetVertex(v.IndexA);
Vec2 wBLocal = proxyB.GetVertex(v.IndexB);
Transform.MulToOutUnsafe(transformA, wALocal, v.WA);
Transform.MulToOutUnsafe(transformB, wBLocal, v.WB);
v.W.Set(v.WB).SubLocal(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 = Metric;
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 = Vertices[0];
v.IndexA = 0;
v.IndexB = 0;
Vec2 wALocal = proxyA.GetVertex(0);
Vec2 wBLocal = proxyB.GetVertex(0);
Transform.MulToOutUnsafe(transformA, wALocal, v.WA);
Transform.MulToOutUnsafe(transformB, wBLocal, v.WB);
v.W.Set(v.WB).SubLocal(v.WA);
Count = 1;
}
}
void ReadCache(SimplexCache cache, DistanceProxy proxyA, XForm transformA, DistanceProxy proxyB, XForm transformB)
{
// Copy data from cache.
m_count = cache.count;
SimplexVertex[] vertices = m_v1;
for (int i = 0; i < m_count; ++i)
{
SimplexVertex v = vertices[i];
v.indexA = cache.indexA[i];
v.indexB = cache.indexB[i];
Vec2 wALocal = proxyA.GetVertex(v.indexA);
Vec2 wBLocal = proxyB.GetVertex(v.indexB);
v.wA = MathB2.Mul(transformA, wALocal);
v.wB = MathB2.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 (m_count > 1)
{
float metric1 = cache.metric;
float metric2 = GetMetric();
if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < Settings.FLT_EPSILON)
{
// Reset the simplex.
m_count = 0;
}
}
// If the cache is empty or invalid ...
if (m_count == 0)
{
SimplexVertex v = vertices[0];
v.indexA = 0;
v.indexB = 0;
Vec2 wALocal = proxyA.GetVertex(0);
Vec2 wBLocal = proxyB.GetVertex(0);
v.wA = MathB2.Mul(transformA, wALocal);
v.wB = MathB2.Mul(transformB, wBLocal);
v.w = v.wB - v.wA;
v.a = 1.0f;
m_count = 1;
}
}
public void Initialize(SimplexCache cache,
DistanceProxy proxyA, Transform transformA,
DistanceProxy proxyB, Transform transformB)
{
_proxyA = proxyA;
_proxyB = proxyB;
int count = cache.Count;
Box2DXDebug.Assert(0 < count && count < 3);
if (count == 1)
{
_type = Type.Points;
Vec2 localPointA = _proxyA.GetVertex(cache.IndexA[0]);
Vec2 localPointB = _proxyB.GetVertex(cache.IndexB[0]);
Vec2 pointA = Math.Mul(transformA, localPointA);
Vec2 pointB = 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
_type = Type.FaceA;
Vec2 localPointA1 = _proxyA.GetVertex(cache.IndexA[0]);
Vec2 localPointA2 = _proxyA.GetVertex(cache.IndexA[1]);
Vec2 localPointB = _proxyB.GetVertex(cache.IndexB[0]);
_localPoint = 0.5f*(localPointA1 + localPointA2);
_axis = Vec2.Cross(localPointA2 - localPointA1, 1.0f);
_axis.Normalize();
Vec2 normal = Math.Mul(transformA.R, _axis);
Vec2 pointA = Math.Mul(transformA, _localPoint);
Vec2 pointB = Math.Mul(transformB, localPointB);
float s = Vec2.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 = Type.FaceB;
Vec2 localPointA = proxyA.GetVertex(cache.IndexA[0]);
Vec2 localPointB1 = proxyB.GetVertex(cache.IndexB[0]);
Vec2 localPointB2 = proxyB.GetVertex(cache.IndexB[1]);
_localPoint = 0.5f*(localPointB1 + localPointB2);
_axis = Vec2.Cross(localPointB2 - localPointB1, 1.0f);
_axis.Normalize();
Vec2 normal = Math.Mul(transformB.R, _axis);
Vec2 pointB = Math.Mul(transformB, _localPoint);
Vec2 pointA = Math.Mul(transformA, localPointA);
float s = Vec2.Dot(pointA - pointB, normal);
if (s < 0.0f)
{
_axis = -_axis;
}
}
else
{
// Two points on B and two points on A.
// The faces are parallel.
Vec2 localPointA1 = _proxyA.GetVertex(cache.IndexA[0]);
Vec2 localPointA2 = _proxyA.GetVertex(cache.IndexA[1]);
Vec2 localPointB1 = _proxyB.GetVertex(cache.IndexB[0]);
Vec2 localPointB2 = _proxyB.GetVertex(cache.IndexB[1]);
Vec2 pA = Math.Mul(transformA, localPointA1);
Vec2 dA = Math.Mul(transformA.R, localPointA2 - localPointA1);
Vec2 pB = Math.Mul(transformB, localPointB1);
Vec2 dB = Math.Mul(transformB.R, localPointB2 - localPointB1);
float a = Vec2.Dot(dA, dA);
float e = Vec2.Dot(dB, dB);
Vec2 r = pA - pB;
float c = Vec2.Dot(dA, r);
float f = Vec2.Dot(dB, r);
float b = Vec2.Dot(dA, dB);
float denom = a*e - b*b;
float s = 0.0f;
if (denom != 0.0f)
{
s = Math.Clamp((b*f - c*e)/denom, 0.0f, 1.0f);
}
float t = (b*s + f)/e;
if (t < 0.0f)
{
t = 0.0f;
s = Math.Clamp(-c/a, 0.0f, 1.0f);
}
else if (t > 1.0f)
{
t = 1.0f;
s = Math.Clamp((b - c)/a, 0.0f, 1.0f);
}
Vec2 localPointA = localPointA1 + s*(localPointA2 - localPointA1);
Vec2 localPointB = localPointB1 + t*(localPointB2 - localPointB1);
if (s == 0.0f || s == 1.0f)
{
_type = Type.FaceB;
_axis = Vec2.Cross(localPointB2 - localPointB1, 1.0f);
_axis.Normalize();
_localPoint = localPointB;
Vec2 normal = Math.Mul(transformB.R, _axis);
Vec2 pointA = Math.Mul(transformA, localPointA);
Vec2 pointB = Math.Mul(transformB, localPointB);
float sgn = Vec2.Dot(pointA - pointB, normal);
if (sgn < 0.0f)
{
_axis = -_axis;
}
}
else
{
_type = Type.FaceA;
_axis = Vec2.Cross(localPointA2 - localPointA1, 1.0f);
_axis.Normalize();
_localPoint = localPointA;
Vec2 normal = Math.Mul(transformA.R, _axis);
Vec2 pointA = Math.Mul(transformA, localPointA);
Vec2 pointB = Math.Mul(transformB, localPointB);
float sgn = Vec2.Dot(pointB - pointA, normal);
if (sgn < 0.0f)
{
_axis = -_axis;
}
}
}
}
public void ReadCache(SimplexCache cache,
DistanceProxy shapeA, ref Transform transformA,
DistanceProxy shapeB, ref Transform transformB)
{
Box2DXDebug.Assert(0 <= cache.Count && cache.Count <= 3);
// Copy data from cache.
Count = cache.Count;
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.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;
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;
}
}
public void ReadCache(SimplexCache cache, DistanceProxy proxyA, Transform transformA, DistanceProxy proxyB, Transform transformB)
{
Debug.Assert(cache.Count <= 3);
// Copy data from cache.
Count = cache.Count;
for (int i = 0; i < Count; ++i)
{
SimplexVertex v = Vertices[i];
v.IndexA = cache.IndexA[i];
v.IndexB = cache.IndexB[i];
Vec2 wALocal = proxyA.GetVertex(v.IndexA);
Vec2 wBLocal = proxyB.GetVertex(v.IndexB);
Transform.MulToOutUnsafe(transformA, wALocal, v.WA);
Transform.MulToOutUnsafe(transformB, wBLocal, v.WB);
v.W.Set(v.WB).SubLocal(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 = Metric;
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 = Vertices[0];
v.IndexA = 0;
v.IndexB = 0;
Vec2 wALocal = proxyA.GetVertex(0);
Vec2 wBLocal = proxyB.GetVertex(0);
Transform.MulToOutUnsafe(transformA, wALocal, v.WA);
Transform.MulToOutUnsafe(transformB, wBLocal, v.WB);
v.W.Set(v.WB).SubLocal(v.WA);
Count = 1;
}
}
/// <summary>
/// Compute the closest points between two shapes. Supports any combination of: CircleShape and
/// PolygonShape. The simplex cache is input/output. On the first call set SimplexCache.count to
/// zero.
/// </summary>
/// <param name="output"></param>
/// <param name="cache"></param>
/// <param name="input"></param>
public void GetDistance(DistanceOutput output, SimplexCache cache, DistanceInput input)
{
GJK_CALLS++;
DistanceProxy proxyA = input.ProxyA;
DistanceProxy proxyB = input.ProxyB;
Transform transformA = input.TransformA;
Transform transformB = input.TransformB;
// Initialize the simplex.
simplex.ReadCache(cache, proxyA, transformA, proxyB, transformB);
// Get simplex vertices as an array.
SimplexVertex[] vertices = simplex.Vertices;
// These store the vertices of the last simplex so that we
// can check for duplicates and prevent cycling.
// (pooled above)
simplex.GetClosestPoint(closestPoint);
float distanceSqr1 = closestPoint.LengthSquared();
// Main iteration loop
int iter = 0;
while (iter < GJK_MAX_ITERS)
{
// Copy simplex so we can identify duplicates.
int saveCount = simplex.Count;
for (int i = 0; i < saveCount; i++)
{
saveA[i] = vertices[i].IndexA;
saveB[i] = vertices[i].IndexB;
}
switch (simplex.Count)
{
case 1:
break;
case 2:
simplex.Solve2();
break;
case 3:
simplex.Solve3();
break;
default:
Debug.Assert(false);
break;
}
// If we have 3 points, then the origin is in the corresponding triangle.
if (simplex.Count == 3)
{
break;
}
// Compute closest point.
simplex.GetClosestPoint(closestPoint);
float distanceSqr2 = closestPoint.LengthSquared();
// ensure progress
if (distanceSqr2 >= distanceSqr1)
{
// break;
}
distanceSqr1 = distanceSqr2;
// get search direction;
simplex.GetSearchDirection(d);
// Ensure the search direction is numerically fit.
if (d.LengthSquared() < Settings.EPSILON * Settings.EPSILON)
{
// The origin is probably contained by a line segment
// or triangle. Thus the shapes are overlapped.
// We can't return zero here even though there may be overlap.
// In case the simplex is a point, segment, or triangle it is difficult
// to determine if the origin is contained in the CSO or very close to it.
break;
}
/*
* SimplexVertex* vertex = vertices + simplex.m_count; vertex.indexA =
* proxyA.GetSupport(MulT(transformA.R, -d)); vertex.wA = Mul(transformA,
* proxyA.GetVertex(vertex.indexA)); Vec2 wBLocal; vertex.indexB =
* proxyB.GetSupport(MulT(transformB.R, d)); vertex.wB = Mul(transformB,
* proxyB.GetVertex(vertex.indexB)); vertex.w = vertex.wB - vertex.wA;
*/
// Compute a tentative new simplex vertex using support points.
SimplexVertex vertex = vertices[simplex.Count];
Rot.MulTransUnsafe(transformA.Q, d.NegateLocal(), temp);
vertex.IndexA = proxyA.GetSupport(temp);
Transform.MulToOutUnsafe(transformA, proxyA.GetVertex(vertex.IndexA), vertex.WA);
// Vec2 wBLocal;
Rot.MulTransUnsafe(transformB.Q, d.NegateLocal(), temp);
vertex.IndexB = proxyB.GetSupport(temp);
Transform.MulToOutUnsafe(transformB, proxyB.GetVertex(vertex.IndexB), vertex.WB);
vertex.W.Set(vertex.WB).SubLocal(vertex.WA);
// Iteration count is equated to the number of support point calls.
++iter;
++GJK_ITERS;
// Check for duplicate support points. This is the main termination criteria.
bool duplicate = false;
for (int i = 0; i < saveCount; ++i)
{
if (vertex.IndexA == saveA[i] && vertex.IndexB == saveB[i])
{
duplicate = true;
break;
}
}
// If we found a duplicate support point we must exit to avoid cycling.
if (duplicate)
{
break;
}
// New vertex is ok and needed.
++simplex.Count;
}
GJK_MAX_ITERS = MathUtils.Max(GJK_MAX_ITERS, iter);
// Prepare output.
simplex.GetWitnessPoints(output.PointA, output.PointB);
output.Distance = MathUtils.Distance(output.PointA, output.PointB);
output.Iterations = iter;
// Cache the simplex.
simplex.WriteCache(cache);
// Apply radii if requested.
if (input.UseRadii)
{
float rA = proxyA.Radius;
float rB = proxyB.Radius;
if (output.Distance > rA + rB && output.Distance > Settings.EPSILON)
{
// Shapes are still no overlapped.
// Move the witness points to the outer surface.
output.Distance -= (rA + rB);
normal.Set(output.PointB).SubLocal(output.PointA);
normal.Normalize();
temp.Set(normal).MulLocal(rA);
output.PointA.AddLocal(temp);
temp.Set(normal).MulLocal(rB);
output.PointB.SubLocal(temp);
}
else
{
// Shapes are overlapped when radii are considered.
// Move the witness points to the middle.
// Vec2 p = 0.5f * (output.pointA + output.pointB);
output.PointA.AddLocal(output.PointB).MulLocal(.5f);
output.PointB.Set(output.PointA);
output.Distance = 0.0f;
}
}
}
/// <summary>
/// Compute the closest points between two shapes. Supports any combination of:
/// b2CircleShape, b2PolygonShape, b2EdgeShape. The simplex cache is input/output.
/// On the first call set b2SimplexCache.count to zero.
/// </summary>
public static void Distance(out DistanceOutput output,
SimplexCache cache,
DistanceInput input)
{
++GjkCalls;
DistanceProxy proxyA = input.proxyA;
DistanceProxy proxyB = input.proxyB;
Transform transformA = input.TransformA;
Transform transformB = input.TransformB;
// Initialize the simplex.
Simplex simplex = new Simplex();
simplex.ReadCache(cache, proxyA, ref transformA, proxyB, ref transformB);
// Get simplex vertices as an array.
SimplexVertex[] vertices = simplex.Vertices;
const int k_maxIters = 20;
// These store the vertices of the last simplex so that we
// can check for duplicates and prevent cycling.
int[] saveA = new int[3], saveB = new int[3];
int saveCount = 0;
Vec2 closestPoint = simplex.GetClosestPoint();
float distanceSqr1 = closestPoint.LengthSquared();
float distanceSqr2 = distanceSqr1;
// Main iteration loop.
int iter = 0;
while (iter < k_maxIters)
{
// Copy simplex so we can identify duplicates.
saveCount = simplex.Count;
for (int i = 0; i < saveCount; ++i)
{
saveA[i] = vertices[i].IndexA;
saveB[i] = vertices[i].IndexB;
}
switch (simplex.Count)
{
case 1:
break;
case 2:
simplex.Solve2();
break;
case 3:
simplex.Solve3();
break;
default:
Box2DXDebug.Assert(false);
break;
}
// If we have 3 points, then the origin is in the corresponding triangle.
if (simplex.Count == 3)
{
break;
}
// Compute closest point.
Vec2 p = simplex.GetClosestPoint();
float distanceSqr = p.LengthSquared();
// Ensure progress
if (distanceSqr2 >= distanceSqr1)
{
//break;
}
distanceSqr1 = distanceSqr2;
// Get search direction.
Vec2 d = simplex.GetSearchDirection();
// Ensure the search direction is numerically fit.
if (d.LengthSquared() < Settings.FLT_EPSILON * Settings.FLT_EPSILON)
{
// The origin is probably contained by a line segment
// or triangle. Thus the shapes are overlapped.
// We can't return zero here even though there may be overlap.
// In case the simplex is a point, segment, or triangle it is difficult
// to determine if the origin is contained in the CSO or very close to it.
break;
}
// Compute a tentative new simplex vertex using support points.
SimplexVertex vertex = vertices[simplex.Count];
vertex.IndexA = proxyA.GetSupport(Math.MulT(transformA.R, -d));
vertex.WA = Math.Mul(transformA, proxyA.GetVertex(vertex.IndexA));
vertex.IndexB = proxyB.GetSupport(Math.MulT(transformB.R, d));
vertex.WB = Math.Mul(transformB, proxyB.GetVertex(vertex.IndexB));
vertex.W = vertex.WB - vertex.WA;
// Iteration count is equated to the number of support point calls.
++iter;
++GjkIters;
// Check for duplicate support points. This is the main termination criteria.
bool duplicate = false;
for (int i = 0; i < saveCount; ++i)
{
if (vertex.IndexA == saveA[i] && vertex.IndexB == saveB[i])
{
duplicate = true;
break;
}
}
// If we found a duplicate support point we must exit to avoid cycling.
if (duplicate)
{
break;
}
// New vertex is ok and needed.
++simplex.Count;
}
GjkMaxIters = Math.Max(GjkMaxIters, iter);
// Prepare output.
simplex.GetWitnessPoints(out output.PointA, out output.PointB);
output.Distance = Vec2.Distance(output.PointA, output.PointB);
output.Iterations = iter;
// Cache the simplex.
simplex.WriteCache(cache);
// Apply radii if requested.
if (input.UseRadii)
{
float rA = proxyA._radius;
float rB = proxyB._radius;
if (output.Distance > rA + rB && output.Distance > Settings.FLT_EPSILON)
{
// Shapes are still no overlapped.
// Move the witness points to the outer surface.
output.Distance -= rA + rB;
Vec2 normal = output.PointB - output.PointA;
normal.Normalize();
output.PointA += rA * normal;
output.PointB -= rB * normal;
}
else
{
// Shapes are overlapped when radii are considered.
// Move the witness points to the middle.
Vec2 p = 0.5f * (output.PointA + output.PointB);
output.PointA = p;
output.PointB = p;
output.Distance = 0.0f;
}
}
}
public void Initialize(SimplexCache cache,
DistanceProxy proxyA, Transform transformA,
DistanceProxy proxyB, Transform transformB)
{
_proxyA = proxyA;
_proxyB = proxyB;
int count = cache.Count;
Box2DXDebug.Assert(0 < count && count < 3);
if (count == 1)
{
_type = Type.Points;
Vec2 localPointA = _proxyA.GetVertex(cache.IndexA[0]);
Vec2 localPointB = _proxyB.GetVertex(cache.IndexB[0]);
Vec2 pointA = Math.Mul(transformA, localPointA);
Vec2 pointB = 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
_type = Type.FaceA;
Vec2 localPointA1 = _proxyA.GetVertex(cache.IndexA[0]);
Vec2 localPointA2 = _proxyA.GetVertex(cache.IndexA[1]);
Vec2 localPointB = _proxyB.GetVertex(cache.IndexB[0]);
_localPoint = 0.5f * (localPointA1 + localPointA2);
_axis = Vec2.Cross(localPointA2 - localPointA1, 1.0f);
_axis.Normalize();
Vec2 normal = Math.Mul(transformA.R, _axis);
Vec2 pointA = Math.Mul(transformA, _localPoint);
Vec2 pointB = Math.Mul(transformB, localPointB);
float s = Vec2.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 = Type.FaceB;
Vec2 localPointA = proxyA.GetVertex(cache.IndexA[0]);
Vec2 localPointB1 = proxyB.GetVertex(cache.IndexB[0]);
Vec2 localPointB2 = proxyB.GetVertex(cache.IndexB[1]);
_localPoint = 0.5f * (localPointB1 + localPointB2);
_axis = Vec2.Cross(localPointB2 - localPointB1, 1.0f);
_axis.Normalize();
Vec2 normal = Math.Mul(transformB.R, _axis);
Vec2 pointB = Math.Mul(transformB, _localPoint);
Vec2 pointA = Math.Mul(transformA, localPointA);
float s = Vec2.Dot(pointA - pointB, normal);
if (s < 0.0f)
{
_axis = -_axis;
}
}
else
{
// Two points on B and two points on A.
// The faces are parallel.
Vec2 localPointA1 = _proxyA.GetVertex(cache.IndexA[0]);
Vec2 localPointA2 = _proxyA.GetVertex(cache.IndexA[1]);
Vec2 localPointB1 = _proxyB.GetVertex(cache.IndexB[0]);
Vec2 localPointB2 = _proxyB.GetVertex(cache.IndexB[1]);
Vec2 pA = Math.Mul(transformA, localPointA1);
Vec2 dA = Math.Mul(transformA.R, localPointA2 - localPointA1);
Vec2 pB = Math.Mul(transformB, localPointB1);
Vec2 dB = Math.Mul(transformB.R, localPointB2 - localPointB1);
float a = Vec2.Dot(dA, dA);
float e = Vec2.Dot(dB, dB);
Vec2 r = pA - pB;
float c = Vec2.Dot(dA, r);
float f = Vec2.Dot(dB, r);
float b = Vec2.Dot(dA, dB);
float denom = a * e - b * b;
float s = 0.0f;
if (denom != 0.0f)
{
s = Math.Clamp((b * f - c * e) / denom, 0.0f, 1.0f);
}
float t = (b * s + f) / e;
if (t < 0.0f)
{
t = 0.0f;
s = Math.Clamp(-c / a, 0.0f, 1.0f);
}
else if (t > 1.0f)
{
t = 1.0f;
s = Math.Clamp((b - c) / a, 0.0f, 1.0f);
}
Vec2 localPointA = localPointA1 + s * (localPointA2 - localPointA1);
Vec2 localPointB = localPointB1 + t * (localPointB2 - localPointB1);
if (s == 0.0f || s == 1.0f)
{
_type = Type.FaceB;
_axis = Vec2.Cross(localPointB2 - localPointB1, 1.0f);
_axis.Normalize();
_localPoint = localPointB;
Vec2 normal = Math.Mul(transformB.R, _axis);
Vec2 pointA = Math.Mul(transformA, localPointA);
Vec2 pointB = Math.Mul(transformB, localPointB);
float sgn = Vec2.Dot(pointA - pointB, normal);
if (sgn < 0.0f)
{
_axis = -_axis;
}
}
else
{
_type = Type.FaceA;
_axis = Vec2.Cross(localPointA2 - localPointA1, 1.0f);
_axis.Normalize();
_localPoint = localPointA;
Vec2 normal = Math.Mul(transformA.R, _axis);
Vec2 pointA = Math.Mul(transformA, localPointA);
Vec2 pointB = Math.Mul(transformB, localPointB);
float sgn = Vec2.Dot(pointB - pointA, normal);
if (sgn < 0.0f)
{
_axis = -_axis;
}
}
}
}