internal void ReadCache(ref SimplexCache cache,
Shape shapeA, ref XForm transformA,
Shape shapeB, ref XForm transformB)
{
Debug.Assert(0 <= cache.count && 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 = shapeA.GetVertex(v.indexA);
Vector2 wBLocal = shapeB.GetVertex(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.b2_FLT_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 = shapeA.GetVertex(0);
Vector2 wBLocal = shapeB.GetVertex(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 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 static void ComputeDistance(out DistanceOutput output,
out SimplexCache cache,
ref DistanceInput input,
Shape shapeA,
Shape shapeB)
{
cache = new SimplexCache();
++b2_gjkCalls;
XForm transformA = input.transformA;
XForm transformB = input.transformB;
// Initialize the simplex.
Simplex simplex = new Simplex();
simplex.ReadCache(ref cache, shapeA, ref transformA, shapeB, ref transformB);
// Get simplex vertices as an array.
int k_maxIters = 20;
// These store the vertices of the last simplex so that we
// can check for duplicates and prevent cycling.
FixedArray3<int> saveA = new FixedArray3<int>();
FixedArray3<int> saveB = new FixedArray3<int>();
int saveCount = 0;
Vector2 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] = simplex._v[i].indexA;
saveB[i] = simplex._v[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.
Vector2 p = simplex.GetClosestPoint();
distanceSqr2 = p.LengthSquared();
// Ensure progress
if (distanceSqr2 >= distanceSqr1)
{
//break;
}
distanceSqr1 = distanceSqr2;
// Get search direction.
Vector2 d = simplex.GetSearchDirection();
// Ensure the search direction is numerically fit.
if (d.LengthSquared() < Settings.b2_FLT_EPSILON * Settings.b2_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 = simplex._v[simplex._count];
vertex.indexA = shapeA.GetSupport(MathUtils.MultiplyT(ref transformA.R, -d));
vertex.wA = MathUtils.Multiply(ref transformA, shapeA.GetVertex(vertex.indexA));
//Vector2 wBLocal;
vertex.indexB = shapeB.GetSupport(MathUtils.MultiplyT(ref transformB.R, d));
vertex.wB = MathUtils.Multiply(ref transformB, shapeB.GetVertex(vertex.indexB));
vertex.w = vertex.wB - vertex.wA;
simplex._v[simplex._count] = vertex;
// Iteration count is equated to the number of support point calls.
++iter;
++b2_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;
}
b2_gjkMaxIters = Math.Max(b2_gjkMaxIters, iter);
// Prepare output.
simplex.GetWitnessPoints(out output.pointA, out output.pointB);
output.distance = (output.pointA - output.pointB).Length();
output.iterations = iter;
// Cache the simplex.
simplex.WriteCache(ref cache);
// Apply radii if requested.
if (input.useRadii)
{
float rA = shapeA._radius;
float rB = shapeB._radius;
if (output.distance > rA + rB && output.distance > Settings.b2_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;
}
}
}
internal void ReadCache( ref SimplexCache cache,
Shape shapeA, ref XForm transformA,
Shape shapeB, ref XForm transformB)
{
Debug.Assert(0 <= cache.count && 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 = shapeA.GetVertex(v.indexA);
Vector2 wBLocal = shapeB.GetVertex(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.b2_FLT_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 = shapeA.GetVertex(0);
Vector2 wBLocal = shapeB.GetVertex(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 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 static void ComputeDistance(out DistanceOutput output,
out SimplexCache cache,
ref DistanceInput input,
Shape shapeA,
Shape shapeB)
{
cache = new SimplexCache();
++b2_gjkCalls;
XForm transformA = input.transformA;
XForm transformB = input.transformB;
// Initialize the simplex.
Simplex simplex = new Simplex();
simplex.ReadCache(ref cache, shapeA, ref transformA, shapeB, ref transformB);
// Get simplex vertices as an array.
int k_maxIters = 20;
// These store the vertices of the last simplex so that we
// can check for duplicates and prevent cycling.
FixedArray3 <int> saveA = new FixedArray3 <int>();
FixedArray3 <int> saveB = new FixedArray3 <int>();
int saveCount = 0;
Vector2 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] = simplex._v[i].indexA;
saveB[i] = simplex._v[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.
Vector2 p = simplex.GetClosestPoint();
distanceSqr2 = p.LengthSquared();
// Ensure progress
if (distanceSqr2 >= distanceSqr1)
{
//break;
}
distanceSqr1 = distanceSqr2;
// Get search direction.
Vector2 d = simplex.GetSearchDirection();
// Ensure the search direction is numerically fit.
if (d.LengthSquared() < Settings.b2_FLT_EPSILON * Settings.b2_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 = simplex._v[simplex._count];
vertex.indexA = shapeA.GetSupport(MathUtils.MultiplyT(ref transformA.R, -d));
vertex.wA = MathUtils.Multiply(ref transformA, shapeA.GetVertex(vertex.indexA));
//Vector2 wBLocal;
vertex.indexB = shapeB.GetSupport(MathUtils.MultiplyT(ref transformB.R, d));
vertex.wB = MathUtils.Multiply(ref transformB, shapeB.GetVertex(vertex.indexB));
vertex.w = vertex.wB - vertex.wA;
simplex._v[simplex._count] = vertex;
// Iteration count is equated to the number of support point calls.
++iter;
++b2_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;
}
b2_gjkMaxIters = Math.Max(b2_gjkMaxIters, iter);
// Prepare output.
simplex.GetWitnessPoints(out output.pointA, out output.pointB);
output.distance = (output.pointA - output.pointB).Length();
output.iterations = iter;
// Cache the simplex.
simplex.WriteCache(ref cache);
// Apply radii if requested.
if (input.useRadii)
{
float rA = shapeA._radius;
float rB = shapeB._radius;
if (output.distance > rA + rB && output.distance > Settings.b2_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;
}
}
}