// Solve a line segment using barycentric coordinates.
//
// p = a1 * w1 + a2 * w2
// a1 + a2 = 1
//
// The vector from the origin to the closest point on the line is
// perpendicular to the line.
// e12 = w2 - w1
// dot(p, e) = 0
// a1 * dot(w1, e) + a2 * dot(w2, e) = 0
//
// 2-by-2 linear system
// [1 1 ][a1] = [1]
// [w1.e12 w2.e12][a2] = [0]
//
// Define
// d12_1 = dot(w2, e12)
// d12_2 = -dot(w1, e12)
// d12 = d12_1 + d12_2
//
// Solution
// a1 = d12_1 / d12
// a2 = d12_2 / d12
internal void Solve2()
{
FPVector2 w1 = V[0].W;
FPVector2 w2 = V[1].W;
FPVector2 e12 = w2 - w1;
// w1 region
FP d12_2 = -FPVector2.Dot(w1, e12);
if (d12_2 <= 0.0f)
{
// a2 <= 0, so we clamp it to 0
SimplexVertex v0 = V[0];
v0.A = 1.0f;
V[0] = v0;
Count = 1;
return;
}
// w2 region
FP d12_1 = FPVector2.Dot(w2, e12);
if (d12_1 <= 0.0f)
{
// a1 <= 0, so we clamp it to 0
SimplexVertex v1 = V[1];
v1.A = 1.0f;
V[1] = v1;
Count = 1;
V[0] = V[1];
return;
}
// Must be in e12 region.
FP inv_d12 = 1.0f / (d12_1 + d12_2);
SimplexVertex v0_2 = V[0];
SimplexVertex v1_2 = V[1];
v0_2.A = d12_1 * inv_d12;
v1_2.A = d12_2 * inv_d12;
V[0] = v0_2;
V[1] = v1_2;
Count = 2;
}
public static void ComputeDistance(out DistanceOutput output, out SimplexCache cache, DistanceInput input)
{
cache = new SimplexCache();
if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled
{
++GJKCalls;
}
// Initialize the simplex.
Simplex simplex = new Simplex();
simplex.ReadCache(ref cache, input.ProxyA, ref input.TransformA, input.ProxyB, ref input.TransformB);
// 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>();
//FP distanceSqr1 = Settings.MaxFP;
// Main iteration loop.
int iter = 0;
while (iter < Settings.MaxGJKIterations)
{
// Copy simplex so we can identify duplicates.
int 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;
}
//FPE: This code was not used anyway.
// Compute closest point.
//Vector2 p = simplex.GetClosestPoint();
//FP distanceSqr2 = p.LengthSquared();
// Ensure progress
//if (distanceSqr2 >= distanceSqr1)
//{
//break;
//}
//distanceSqr1 = distanceSqr2;
// Get search direction.
FPVector2 d = simplex.GetSearchDirection();
// Ensure the search direction is numerically fit.
if (d.LengthSquared() < Settings.EpsilonSqr)
{
// 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 = input.ProxyA.GetSupport(MathUtils.MulT(input.TransformA.q, -d));
vertex.WA = MathUtils.Mul(ref input.TransformA, input.ProxyA.Vertices[vertex.IndexA]);
vertex.IndexB = input.ProxyB.GetSupport(MathUtils.MulT(input.TransformB.q, d));
vertex.WB = MathUtils.Mul(ref input.TransformB, input.ProxyB.Vertices[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;
if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled
{
++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;
}
if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled
{
GJKMaxIters = Math.Max(GJKMaxIters, iter);
}
// Prepare output.
simplex.GetWitnessPoints(out output.PointA, out output.PointB);
output.Distance = (output.PointA - output.PointB).magnitude;
output.Iterations = iter;
// Cache the simplex.
simplex.WriteCache(ref cache);
// Apply radii if requested.
if (input.UseRadii)
{
FP rA = input.ProxyA.Radius;
FP rB = input.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;
FPVector2 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.
FPVector2 p = 0.5f * (output.PointA + output.PointB);
output.PointA = p;
output.PointB = p;
output.Distance = 0.0f;
}
}
}
// Possible regions:
// - points[2]
// - edge points[0]-points[2]
// - edge points[1]-points[2]
// - inside the triangle
internal void Solve3()
{
FPVector2 w1 = V[0].W;
FPVector2 w2 = V[1].W;
FPVector2 w3 = V[2].W;
// Edge12
// [1 1 ][a1] = [1]
// [w1.e12 w2.e12][a2] = [0]
// a3 = 0
FPVector2 e12 = w2 - w1;
FP w1e12 = FPVector2.Dot(w1, e12);
FP w2e12 = FPVector2.Dot(w2, e12);
FP d12_1 = w2e12;
FP d12_2 = -w1e12;
// Edge13
// [1 1 ][a1] = [1]
// [w1.e13 w3.e13][a3] = [0]
// a2 = 0
FPVector2 e13 = w3 - w1;
FP w1e13 = FPVector2.Dot(w1, e13);
FP w3e13 = FPVector2.Dot(w3, e13);
FP d13_1 = w3e13;
FP d13_2 = -w1e13;
// Edge23
// [1 1 ][a2] = [1]
// [w2.e23 w3.e23][a3] = [0]
// a1 = 0
FPVector2 e23 = w3 - w2;
FP w2e23 = FPVector2.Dot(w2, e23);
FP w3e23 = FPVector2.Dot(w3, e23);
FP d23_1 = w3e23;
FP d23_2 = -w2e23;
// Triangle123
FP n123 = MathUtils.Cross(e12, e13);
FP d123_1 = n123 * MathUtils.Cross(w2, w3);
FP d123_2 = n123 * MathUtils.Cross(w3, w1);
FP d123_3 = n123 * MathUtils.Cross(w1, w2);
// w1 region
if (d12_2 <= 0.0f && d13_2 <= 0.0f)
{
SimplexVertex v0_1 = V[0];
v0_1.A = 1.0f;
V[0] = v0_1;
Count = 1;
return;
}
// e12
if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f)
{
FP inv_d12 = 1.0f / (d12_1 + d12_2);
SimplexVertex v0_2 = V[0];
SimplexVertex v1_2 = V[1];
v0_2.A = d12_1 * inv_d12;
v1_2.A = d12_2 * inv_d12;
V[0] = v0_2;
V[1] = v1_2;
Count = 2;
return;
}
// e13
if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f)
{
FP inv_d13 = 1.0f / (d13_1 + d13_2);
SimplexVertex v0_3 = V[0];
SimplexVertex v2_3 = V[2];
v0_3.A = d13_1 * inv_d13;
v2_3.A = d13_2 * inv_d13;
V[0] = v0_3;
V[2] = v2_3;
Count = 2;
V[1] = V[2];
return;
}
// w2 region
if (d12_1 <= 0.0f && d23_2 <= 0.0f)
{
SimplexVertex v1_4 = V[1];
v1_4.A = 1.0f;
V[1] = v1_4;
Count = 1;
V[0] = V[1];
return;
}
// w3 region
if (d13_1 <= 0.0f && d23_1 <= 0.0f)
{
SimplexVertex v2_5 = V[2];
v2_5.A = 1.0f;
V[2] = v2_5;
Count = 1;
V[0] = V[2];
return;
}
// e23
if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f)
{
FP inv_d23 = 1.0f / (d23_1 + d23_2);
SimplexVertex v1_6 = V[1];
SimplexVertex v2_6 = V[2];
v1_6.A = d23_1 * inv_d23;
v2_6.A = d23_2 * inv_d23;
V[1] = v1_6;
V[2] = v2_6;
Count = 2;
V[0] = V[2];
return;
}
// Must be in triangle123
FP inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3);
SimplexVertex v0_7 = V[0];
SimplexVertex v1_7 = V[1];
SimplexVertex v2_7 = V[2];
v0_7.A = d123_1 * inv_d123;
v1_7.A = d123_2 * inv_d123;
v2_7.A = d123_3 * inv_d123;
V[0] = v0_7;
V[1] = v1_7;
V[2] = v2_7;
Count = 3;
}
