public void WriteCache(SimplexCache cache)
{
cache.Metric = GetMetric();
cache.Count = (ushort)Count;
for (int i = 0; i < Count; ++i)
{
cache.IndexA[i] = (byte)Vertices[i].IndexA;
cache.IndexB[i] = (byte)Vertices[i].IndexB;
}
}
internal void WriteCache(SimplexCache cache)
{
cache.Metric = GetMetric();
cache.Count = (UInt16)_count;
SimplexVertex[] vertices = new SimplexVertex[] { _v1, _v2, _v3 };
for (int i = 0; i < _count; ++i)
{
cache.IndexA[i] = (Byte)(vertices[i].indexA);
cache.IndexB[i] = (Byte)(vertices[i].indexB);
}
}
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;
}
}
internal 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 = new SimplexVertex[] { _v1, _v2, _v3 };
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;
}
}
public override void Step(Settings settings)
{
base.Step(settings);
DistanceInput input = new DistanceInput();
input.TransformA = _transformA;
input.TransformB = _transformB;
input.UseRadii = true;
SimplexCache cache = new SimplexCache();
cache.Count = 0;
DistanceOutput output;
Collision.Distance(out output, ref cache, ref input, _polygonA, _polygonB);
StringBuilder strBld = new StringBuilder();
strBld.AppendFormat("distance = {0}", new object[] { output.Distance });
OpenGLDebugDraw.DrawString(5, _textLine, strBld.ToString());
_textLine += 15;
strBld = new StringBuilder();
strBld.AppendFormat("iterations = {0}", new object[] { output.Iterations });
OpenGLDebugDraw.DrawString(5, _textLine, strBld.ToString());
_textLine += 15;
{
Color color = new Color(0.9f, 0.9f, 0.9f);
int i;
for (i = 0; i < _polygonA.VertexCount; ++i)
{
_dv[i] = Math.Mul(_transformA, _polygonA.Vertices[i]);
}
_debugDraw.DrawPolygon(_dv, _polygonA.VertexCount, color);
for (i = 0; i < _polygonB.VertexCount; ++i)
{
_dv[i] = Math.Mul(_transformB, _polygonB.Vertices[i]);
}
_debugDraw.DrawPolygon(_dv, _polygonB.VertexCount, color);
}
Vec2 x1 = output.PointA;
Vec2 x2 = output.PointB;
OpenGLDebugDraw.DrawPoint(x1, 4.0f, new Color(1, 0, 0));
OpenGLDebugDraw.DrawSegment(x1, x2, new Color(1, 1, 0));
OpenGLDebugDraw.DrawPoint(x2, 4.0f, new Color(1, 0, 0));
}
// CCD via the secant method.
/// <summary>
/// Compute the time when two shapes begin to touch or touch at a closer distance.
/// TOI considers the shape radii. It attempts to have the radii overlap by the tolerance.
/// Iterations terminate with the overlap is within 0.5 * tolerance. The tolerance should be
/// smaller than sum of the shape radii.
/// Warning the sweeps must have the same time interval.
/// </summary>
/// <returns>
/// The fraction between [0,1] in which the shapes first touch.
/// fraction=0 means the shapes begin touching/overlapped, and fraction=1 means the shapes don't touch.
/// </returns>
public static float TimeOfImpact(TOIInput input, Shape shapeA, Shape shapeB)
{
Sweep sweepA = input.SweepA;
Sweep sweepB = input.SweepB;
Box2DXDebug.Assert(sweepA.T0 == sweepB.T0);
Box2DXDebug.Assert(1.0f - sweepA.T0 > Common.Settings.FLT_EPSILON);
float radius = shapeA._radius + shapeB._radius;
float tolerance = input.Tolerance;
float alpha = 0.0f;
const int k_maxIterations = 1000; // TODO_ERIN b2Settings
int iter = 0;
float target = 0.0f;
// Prepare input for distance query.
SimplexCache cache = new SimplexCache();
cache.Count = 0;
DistanceInput distanceInput;
distanceInput.UseRadii = false;
for (; ;)
{
Transform xfA, xfB;
sweepA.GetTransform(out xfA, alpha);
sweepB.GetTransform(out xfB, alpha);
// Get the distance between shapes.
distanceInput.TransformA = xfA;
distanceInput.TransformB = xfB;
DistanceOutput distanceOutput;
Distance(out distanceOutput, ref cache, ref distanceInput, shapeA, shapeB);
if (distanceOutput.Distance <= 0.0f)
{
alpha = 1.0f;
break;
}
SeparationFunction fcn = new SeparationFunction();
#if ALLOWUNSAFE
unsafe
{
fcn.Initialize(&cache, shapeA, xfA, shapeB, xfB);
}
#else
fcn.Initialize(cache, shapeA, xfA, shapeB, xfB);
#endif
float separation = fcn.Evaluate(xfA, xfB);
if (separation <= 0.0f)
{
alpha = 1.0f;
break;
}
if (iter == 0)
{
// Compute a reasonable target distance to give some breathing room
// for conservative advancement. We take advantage of the shape radii
// to create additional clearance.
if (separation > radius)
{
target = Common.Math.Max(radius - tolerance, 0.75f * radius);
}
else
{
target = Common.Math.Max(separation - tolerance, 0.02f * radius);
}
}
if (separation - target < 0.5f * tolerance)
{
if (iter == 0)
{
alpha = 1.0f;
break;
}
break;
}
#if _FALSE
// Dump the curve seen by the root finder
{
const int32 N = 100;
float32 dx = 1.0f / N;
float32 xs[N + 1];
internal void Initialize(SimplexCache cache, Shape shapeA, Transform transformA, Shape shapeB, Transform transformB)
{
ShapeA = shapeA;
ShapeB = shapeB;
int count = cache.Count;
Box2DXDebug.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;
}
}
}
internal void WriteCache(SimplexCache cache)
{
cache.Metric = GetMetric();
cache.Count = (UInt16)_count;
SimplexVertex[] vertices = new SimplexVertex[] { _v1, _v2, _v3 };
for (int i = 0; i < _count; ++i)
{
cache.IndexA[i] = (Byte)(vertices[i].indexA);
cache.IndexB[i] = (Byte)(vertices[i].indexB);
}
}
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>
/// 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;
}
}
}
internal unsafe void Initialize(SimplexCache* cache,
Shape shapeA, Transform TransformA,
Shape shapeB, Transform TransformB)
{
ShapeA = shapeA;
ShapeB = shapeB;
int count = cache->Count;
Box2DXDebug.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;
}
}
}
internal unsafe void Initialize(SimplexCache* cache,
Shape shapeA, XForm transformA,
Shape shapeB, XForm transformB)
{
ShapeA = shapeA;
ShapeB = shapeB;
int count = cache->Count;
Box2DXDebug.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;
}
}
}
/// <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;
}
}
}
static void Distance(out DistanceOutput output, ref SimplexCache cache, ref DistanceInput input, Shape shapeA, Shape shapeB)
{
output = new DistanceOutput();
Transform transformA = input.TransformA;
Transform transformB = input.TransformB;
// Initialize the simplex.
Simplex simplex = new Simplex();
#if ALLOWUNSAFE
fixed(SimplexCache *sPtr = &cache)
{
simplex.ReadCache(sPtr, shapeA, transformA, shapeB, transformB);
}
#else
simplex.ReadCache(cache, shapeA, transformA, shapeB, transformB);
#endif
// Get simplex vertices as an array.
#if ALLOWUNSAFE
SimplexVertex *vertices = &simplex._v1;
#else
SimplexVertex[] vertices = new SimplexVertex[] { simplex._v1, simplex._v2, simplex._v3 };
#endif
// These store the vertices of the last simplex so that we
// can check for duplicates and prevent cycling.
#if ALLOWUNSAFE
int *lastA = stackalloc int[4], lastB = stackalloc int[4];
#else
int[] lastA = new int[4];
int[] lastB = new int[4];
#endif // ALLOWUNSAFE
int lastCount;
// Main iteration loop.
int iter = 0;
const int k_maxIterationCount = 20;
while (iter < k_maxIterationCount)
{
// Copy simplex so we can identify duplicates.
lastCount = simplex._count;
int i;
for (i = 0; i < lastCount; ++i)
{
lastA[i] = vertices[i].indexA;
lastB[i] = vertices[i].indexB;
}
switch (simplex._count)
{
case 1:
break;
case 2:
simplex.Solve2();
break;
case 3:
simplex.Solve3();
break;
default:
#if DEBUG
Box2DXDebug.Assert(false);
#endif
break;
}
// If we have 3 points, then the origin is in the corresponding triangle.
if (simplex._count == 3)
{
break;
}
// Compute closest point.
Vector2 p = simplex.GetClosestPoint();
float distanceSqr = p.sqrMagnitude;
// Ensure the search direction is numerically fit.
if (distanceSqr < Common.Settings.FLT_EPSILON_SQUARED)
{
// 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.
#if ALLOWUNSAFE
SimplexVertex *vertex = vertices + simplex._count;
vertex->indexA = shapeA.GetSupport(transformA.InverseTransformDirection(p));
vertex->wA = transformA.TransformPoint(shapeA.GetVertex(vertex->indexA));
//Vec2 wBLocal;
vertex->indexB = shapeB.GetSupport(transformB.InverseTransformDirection(-p));
vertex->wB = transformB.TransformPoint(shapeB.GetVertex(vertex->indexB));
vertex->w = vertex->wB - vertex->wA;
#else
SimplexVertex vertex = vertices[simplex._count - 1];
vertex.indexA = shapeA.GetSupport(transformA.InverseTransformDirection(p));
vertex.wA = transformA.TransformPoint(shapeA.GetVertex(vertex.indexA));
//Vec2 wBLocal;
vertex.indexB = shapeB.GetSupport(transformB.InverseTransformDirection(-p));
vertex.wB = transformB.TransformPoint(shapeB.GetVertex(vertex.indexB));
vertex.w = vertex.wB - vertex.wA;
#endif // ALLOWUNSAFE
// Iteration count is equated to the number of support point calls.
++iter;
// Check for convergence.
#if ALLOWUNSAFE
float lowerBound = Vector2.Dot(p, vertex->w);
#else
float lowerBound = Vector2.Dot(p, vertex.w);
#endif
float upperBound = distanceSqr;
const float k_relativeTolSqr = 0.01f * 0.01f; // 1:100
if (upperBound - lowerBound <= k_relativeTolSqr * upperBound)
{
// Converged!
break;
}
// Check for duplicate support points.
bool duplicate = false;
for (i = 0; i < lastCount; ++i)
{
#if ALLOWUNSAFE
if (vertex->indexA == lastA[i] && vertex->indexB == lastB[i])
#else
if (vertex.indexA == lastA[i] && vertex.indexB == lastB[i])
#endif
{
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;
}
#if ALLOWUNSAFE
fixed(DistanceOutput *doPtr = &output)
{
// Prepare output.
simplex.GetWitnessPoints(&doPtr->PointA, &doPtr->PointB);
doPtr->Distance = Vector2.Distance(doPtr->PointA, doPtr->PointB);
doPtr->Iterations = iter;
}
fixed(SimplexCache *sPtr = &cache)
{
// Cache the simplex.
simplex.WriteCache(sPtr);
}
#else
// Prepare output.
simplex.GetWitnessPoints(out output.PointA, out output.PointB);
output.Distance = Vector2.Distance(output.PointA, output.PointB);
output.Iterations = iter;
// Cache the simplex.
simplex.WriteCache(cache);
#endif
// Apply radii if requested.
if (input.UseRadii)
{
float rA = shapeA._radius;
float rB = shapeB._radius;
if (output.Distance > rA + rB && output.Distance > Common.Settings.FLT_EPSILON)
{
// Shapes are still no overlapped.
// Move the witness points to the outer surface.
output.Distance -= rA + rB;
Vector2 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.
Vector2 p = 0.5f * (output.PointA + output.PointB);
output.PointA = p;
output.PointB = p;
output.Distance = 0.0f;
}
}
}
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;
}
}
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;
}
}
}
public void WriteCache(SimplexCache cache)
{
cache.Metric = GetMetric();
cache.Count = (ushort)Count;
for (int i = 0; i < Count; ++i)
{
cache.IndexA[i] = (byte)Vertices[i].IndexA;
cache.IndexB[i] = (byte)Vertices[i].IndexB;
}
}
internal 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 = new SimplexVertex[] { _v1, _v2, _v3 };
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;
}
}
/// <summary>
/// CCD via the secant method.
/// Compute the time when two shapes begin to touch or touch at a closer distance.
/// TOI considers the shape radii. It attempts to have the radii overlap by the tolerance.
/// Iterations terminate with the overlap is within 0.5 * tolerance. The tolerance should be
/// smaller than sum of the shape radii.
/// @warning the sweeps must have the same time interval.
/// fraction=0 means the shapes begin touching/overlapped, and fraction=1 means the shapes don't touch.
/// </summary>
/// <param name="input">The input.</param>
/// <param name="shapeA">The shape A.</param>
/// <param name="shapeB">The shape B.</param>
/// <returns>
/// fraction between [0,1] in which the shapes first touch.
/// </returns>
public static float TimeOfImpact(TOIInput input)
{
++ToiCalls;
DistanceProxy proxyA = input.ProxyA;
DistanceProxy proxyB = input.ProxyB;
Sweep sweepA = input.SweepA;
Sweep sweepB = input.SweepB;
Box2DXDebug.Assert(sweepA.T0 == sweepB.T0);
Box2DXDebug.Assert(1.0f - sweepA.T0 > Settings.FLT_EPSILON);
float radius = proxyA._radius + proxyB._radius;
float tolerance = input.Tolerance;
float alpha = 0.0f;
const int k_maxIterations = 1000; // TODO_ERIN b2Settings
int iter = 0;
float target = 0.0f;
// Prepare input for distance query.
SimplexCache cache = new SimplexCache();
cache.Count = 0;
DistanceInput distanceInput = new DistanceInput();
distanceInput.proxyA = input.ProxyA;
distanceInput.proxyB = input.ProxyB;
distanceInput.UseRadii = false;
for (;;)
{
Transform xfA, xfB;
sweepA.GetTransform(out xfA, alpha);
sweepB.GetTransform(out xfB, alpha);
// Get the distance between shapes.
distanceInput.TransformA = xfA;
distanceInput.TransformB = xfB;
DistanceOutput distanceOutput;
Distance(out distanceOutput, cache, distanceInput);
if (distanceOutput.Distance <= 0.0f)
{
alpha = 1.0f;
break;
}
SeparationFunction fcn = new SeparationFunction();
fcn.Initialize(cache, proxyA, xfA, proxyB, xfB);
float separation = fcn.Evaluate(xfA, xfB);
if (separation <= 0.0f)
{
alpha = 1.0f;
break;
}
if (iter == 0)
{
// Compute a reasonable target distance to give some breathing room
// for conservative advancement. We take advantage of the shape radii
// to create additional clearance.
if (separation > radius)
{
target = Math.Max(radius - tolerance, 0.75f*radius);
}
else
{
target = Math.Max(separation - tolerance, 0.02f*radius);
}
}
if (separation - target < 0.5f*tolerance)
{
if (iter == 0)
{
alpha = 1.0f;
break;
}
break;
}
#if false
// Dump the curve seen by the root finder
{
const int N = 100;
float dx = 1.0f / N;
float xs[N+1];
static void Distance(out DistanceOutput output, ref SimplexCache cache, ref DistanceInput input, Shape shapeA, Shape shapeB)
{
output = new DistanceOutput();
Transform transformA = input.TransformA;
Transform transformB = input.TransformB;
// Initialize the simplex.
Simplex simplex = new Simplex();
#if ALLOWUNSAFE
fixed (SimplexCache* sPtr = &cache)
{
simplex.ReadCache(sPtr, shapeA, transformA, shapeB, transformB);
}
#else
simplex.ReadCache(cache, shapeA, transformA, shapeB, transformB);
#endif
// Get simplex vertices as an array.
#if ALLOWUNSAFE
SimplexVertex* vertices = &simplex._v1;
#else
SimplexVertex[] vertices = new SimplexVertex[] { simplex._v1, simplex._v2, simplex._v3 };
#endif
// These store the vertices of the last simplex so that we
// can check for duplicates and prevent cycling.
#if ALLOWUNSAFE
int* lastA = stackalloc int[4], lastB = stackalloc int[4];
#else
int[] lastA = new int[4];
int[] lastB = new int[4];
#endif // ALLOWUNSAFE
int lastCount;
// Main iteration loop.
int iter = 0;
const int k_maxIterationCount = 20;
while (iter < k_maxIterationCount)
{
// Copy simplex so we can identify duplicates.
lastCount = simplex._count;
int i;
for (i = 0; i < lastCount; ++i)
{
lastA[i] = vertices[i].indexA;
lastB[i] = vertices[i].indexB;
}
switch (simplex._count)
{
case 1:
break;
case 2:
simplex.Solve2();
break;
case 3:
simplex.Solve3();
break;
default:
#if DEBUG
Box2DXDebug.Assert(false);
#endif
break;
}
// If we have 3 points, then the origin is in the corresponding triangle.
if (simplex._count == 3)
{
break;
}
// Compute closest point.
Vector2 p = simplex.GetClosestPoint();
float distanceSqr = p.sqrMagnitude;
// Ensure the search direction is numerically fit.
if (distanceSqr < Common.Settings.FLT_EPSILON_SQUARED)
{
// 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.
#if ALLOWUNSAFE
SimplexVertex* vertex = vertices + simplex._count;
vertex->indexA = shapeA.GetSupport(transformA.InverseTransformDirection(p));
vertex->wA = transformA.TransformPoint(shapeA.GetVertex(vertex->indexA));
//Vec2 wBLocal;
vertex->indexB = shapeB.GetSupport(transformB.InverseTransformDirection(-p));
vertex->wB = transformB.TransformPoint(shapeB.GetVertex(vertex->indexB));
vertex->w = vertex->wB - vertex->wA;
#else
SimplexVertex vertex = vertices[simplex._count - 1];
vertex.indexA = shapeA.GetSupport(transformA.InverseTransformDirection(p));
vertex.wA = transformA.TransformPoint(shapeA.GetVertex(vertex.indexA));
//Vec2 wBLocal;
vertex.indexB = shapeB.GetSupport(transformB.InverseTransformDirection(-p));
vertex.wB = transformB.TransformPoint(shapeB.GetVertex(vertex.indexB));
vertex.w = vertex.wB - vertex.wA;
#endif // ALLOWUNSAFE
// Iteration count is equated to the number of support point calls.
++iter;
// Check for convergence.
#if ALLOWUNSAFE
float lowerBound = Vector2.Dot(p, vertex->w);
#else
float lowerBound = Vector2.Dot(p, vertex.w);
#endif
float upperBound = distanceSqr;
const float k_relativeTolSqr = 0.01f * 0.01f; // 1:100
if (upperBound - lowerBound <= k_relativeTolSqr * upperBound)
{
// Converged!
break;
}
// Check for duplicate support points.
bool duplicate = false;
for (i = 0; i < lastCount; ++i)
{
#if ALLOWUNSAFE
if (vertex->indexA == lastA[i] && vertex->indexB == lastB[i])
#else
if (vertex.indexA == lastA[i] && vertex.indexB == lastB[i])
#endif
{
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;
}
#if ALLOWUNSAFE
fixed (DistanceOutput* doPtr = &output)
{
// Prepare output.
simplex.GetWitnessPoints(&doPtr->PointA, &doPtr->PointB);
doPtr->Distance = Vector2.Distance(doPtr->PointA, doPtr->PointB);
doPtr->Iterations = iter;
}
fixed (SimplexCache* sPtr = &cache)
{
// Cache the simplex.
simplex.WriteCache(sPtr);
}
#else
// Prepare output.
simplex.GetWitnessPoints(out output.PointA, out output.PointB);
output.Distance = Vector2.Distance(output.PointA, output.PointB);
output.Iterations = iter;
// Cache the simplex.
simplex.WriteCache(cache);
#endif
// Apply radii if requested.
if (input.UseRadii)
{
float rA = shapeA._radius;
float rB = shapeB._radius;
if (output.Distance > rA + rB && output.Distance > Common.Settings.FLT_EPSILON)
{
// Shapes are still no overlapped.
// Move the witness points to the outer surface.
output.Distance -= rA + rB;
Vector2 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.
Vector2 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;
}
}
}
}
