internal void WriteCache(ref SimplexCache cache)
{
cache.Metric = GetMetric();
cache.Count = (ushort)Count;
for (int i = 0; i < Count; ++i)
{
cache.IndexA[i] = (byte)V[i].IndexA;
cache.IndexB[i] = (byte)V[i].IndexB;
}
}
internal void ReadCache(ref SimplexCache cache, DistanceProxy proxyA, ref Transform transformA, DistanceProxy proxyB, ref Transform transformB)
{
System.Diagnostics.Debug.Assert(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 = proxyA.Vertices[v.IndexA];
Vector2 wBLocal = proxyB.Vertices[v.IndexB];
v.WA = MathUtils.Mul(ref transformA, wALocal);
v.WB = MathUtils.Mul(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.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 = proxyA.Vertices[0];
Vector2 wBLocal = proxyB.Vertices[0];
v.WA = MathUtils.Mul(ref transformA, wALocal);
v.WB = MathUtils.Mul(ref transformB, wBLocal);
v.W = v.WB - v.WA;
v.A = 1.0f;
V[0] = v;
Count = 1;
}
}
public static void ComputeDistance(out DistanceOutput output, out SimplexCache cache, DistanceInput input)
{
cache = new SimplexCache();
if (Settings.EnableDiagnostics) //Velcro: 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>();
//Velcro: This code was not used anyway.
//float distanceSqr1 = Settings.MaxFloat;
// 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:
System.Diagnostics.Debug.Assert(false);
break;
}
// If we have 3 points, then the origin is in the corresponding triangle.
if (simplex.Count == 3)
{
break;
}
// Get search direction.
Vector2 d = simplex.GetSearchDirection();
// 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;
}
// 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) //Velcro: 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) //Velcro: 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).Length();
output.Iterations = iter;
// Cache the simplex.
simplex.WriteCache(ref cache);
// Apply radii if requested.
if (input.UseRadii)
{
float rA = input.ProxyA.Radius;
float 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;
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;
}
}
}