public static void ComputeDistance(out DistanceOutput output, out SimplexCache cache, DistanceInput input)
{
cache = default(SimplexCache);
Simplex simplex = default(Simplex);
simplex.ReadCache(ref cache, input.ProxyA, ref input.TransformA, input.ProxyB, ref input.TransformB);
FixedArray3 <int> fixedArray = default(FixedArray3 <int>);
FixedArray3 <int> fixedArray2 = default(FixedArray3 <int>);
int i = 0;
while (i < 20)
{
int count = simplex.Count;
for (int j = 0; j < count; j++)
{
fixedArray[j] = simplex.V[j].IndexA;
fixedArray2[j] = simplex.V[j].IndexB;
}
switch (simplex.Count)
{
case 1:
break;
case 2:
simplex.Solve2();
break;
case 3:
simplex.Solve3();
break;
default:
Debug.Assert(false);
break;
}
bool flag = simplex.Count == 3;
if (flag)
{
break;
}
TSVector2 searchDirection = simplex.GetSearchDirection();
bool flag2 = searchDirection.LengthSquared() < Settings.EpsilonSqr;
if (flag2)
{
break;
}
SimplexVertex simplexVertex = simplex.V[simplex.Count];
simplexVertex.IndexA = input.ProxyA.GetSupport(MathUtils.MulT(input.TransformA.q, -searchDirection));
simplexVertex.WA = MathUtils.Mul(ref input.TransformA, input.ProxyA.Vertices[simplexVertex.IndexA]);
simplexVertex.IndexB = input.ProxyB.GetSupport(MathUtils.MulT(input.TransformB.q, searchDirection));
simplexVertex.WB = MathUtils.Mul(ref input.TransformB, input.ProxyB.Vertices[simplexVertex.IndexB]);
simplexVertex.W = simplexVertex.WB - simplexVertex.WA;
simplex.V[simplex.Count] = simplexVertex;
i++;
bool flag3 = false;
for (int k = 0; k < count; k++)
{
bool flag4 = simplexVertex.IndexA == fixedArray[k] && simplexVertex.IndexB == fixedArray2[k];
if (flag4)
{
flag3 = true;
break;
}
}
bool flag5 = flag3;
if (flag5)
{
break;
}
simplex.Count++;
}
simplex.GetWitnessPoints(out output.PointA, out output.PointB);
output.Distance = (output.PointA - output.PointB).magnitude;
output.Iterations = i;
simplex.WriteCache(ref cache);
bool useRadii = input.UseRadii;
if (useRadii)
{
FP radius = input.ProxyA.Radius;
FP radius2 = input.ProxyB.Radius;
bool flag6 = output.Distance > radius + radius2 && output.Distance > Settings.Epsilon;
if (flag6)
{
output.Distance -= radius + radius2;
TSVector2 value = output.PointB - output.PointA;
value.Normalize();
output.PointA += radius * value;
output.PointB -= radius2 * value;
}
else
{
TSVector2 tSVector = 0.5f * (output.PointA + output.PointB);
output.PointA = tSVector;
output.PointB = tSVector;
output.Distance = 0f;
}
}
}
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.
TSVector2 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;
TSVector2 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.
TSVector2 p = 0.5f * (output.PointA + output.PointB);
output.PointA = p;
output.PointB = p;
output.Distance = 0.0f;
}
}
}
public static void CalculateTimeOfImpact(out TOIOutput output, TOIInput input)
{
output = default(TOIOutput);
output.State = TOIOutputState.Unknown;
output.T = input.TMax;
Sweep sweepA = input.SweepA;
Sweep sweepB = input.SweepB;
sweepA.Normalize();
sweepB.Normalize();
FP tMax = input.TMax;
FP x = input.ProxyA.Radius + input.ProxyB.Radius;
FP fP = TSMath.Max(Settings.LinearSlop, x - 3f * Settings.LinearSlop);
FP y = 0.25f * Settings.LinearSlop;
Debug.Assert(fP > y);
FP fP2 = 0f;
int num = 0;
TimeOfImpact._distanceInput = (TimeOfImpact._distanceInput ?? new DistanceInput());
TimeOfImpact._distanceInput.ProxyA = input.ProxyA;
TimeOfImpact._distanceInput.ProxyB = input.ProxyB;
TimeOfImpact._distanceInput.UseRadii = false;
while (true)
{
Transform transformA;
sweepA.GetTransform(out transformA, fP2);
Transform transformB;
sweepB.GetTransform(out transformB, fP2);
TimeOfImpact._distanceInput.TransformA = transformA;
TimeOfImpact._distanceInput.TransformB = transformB;
DistanceOutput distanceOutput;
SimplexCache simplexCache;
Distance.ComputeDistance(out distanceOutput, out simplexCache, TimeOfImpact._distanceInput);
bool flag = distanceOutput.Distance <= 0f;
if (flag)
{
break;
}
bool flag2 = distanceOutput.Distance < fP + y;
if (flag2)
{
goto Block_3;
}
SeparationFunction.Set(ref simplexCache, input.ProxyA, ref sweepA, input.ProxyB, ref sweepB, fP2);
bool flag3 = false;
FP fP3 = tMax;
int num2 = 0;
while (true)
{
int indexA;
int indexB;
FP x2 = SeparationFunction.FindMinSeparation(out indexA, out indexB, fP3);
bool flag4 = x2 > fP + y;
if (flag4)
{
goto Block_4;
}
bool flag5 = x2 > fP - y;
if (flag5)
{
goto Block_5;
}
FP fP4 = SeparationFunction.Evaluate(indexA, indexB, fP2);
bool flag6 = fP4 < fP - y;
if (flag6)
{
goto Block_6;
}
bool flag7 = fP4 <= fP + y;
if (flag7)
{
goto Block_7;
}
int num3 = 0;
FP fP5 = fP2;
FP fP6 = fP3;
FP fP7;
bool flag11;
do
{
bool flag8 = (num3 & 1) != 0;
if (flag8)
{
fP7 = fP5 + (fP - fP4) * (fP6 - fP5) / (x2 - fP4);
}
else
{
fP7 = 0.5f * (fP5 + fP6);
}
num3++;
FP fP8 = SeparationFunction.Evaluate(indexA, indexB, fP7);
bool flag9 = FP.Abs(fP8 - fP) < y;
if (flag9)
{
goto Block_9;
}
bool flag10 = fP8 > fP;
if (flag10)
{
fP5 = fP7;
fP4 = fP8;
}
else
{
fP6 = fP7;
x2 = fP8;
}
flag11 = (num3 == 50);
}while (!flag11);
IL_373:
num2++;
bool flag12 = num2 == Settings.MaxPolygonVertices;
if (flag12)
{
break;
}
continue;
Block_9:
fP3 = fP7;
goto IL_373;
}
IL_396:
num++;
bool flag13 = flag3;
if (flag13)
{
goto Block_13;
}
bool flag14 = num == 20;
if (flag14)
{
goto Block_14;
}
continue;
Block_4:
output.State = TOIOutputState.Seperated;
output.T = tMax;
flag3 = true;
goto IL_396;
Block_5:
fP2 = fP3;
goto IL_396;
Block_6:
output.State = TOIOutputState.Failed;
output.T = fP2;
flag3 = true;
goto IL_396;
Block_7:
output.State = TOIOutputState.Touching;
output.T = fP2;
flag3 = true;
goto IL_396;
}
output.State = TOIOutputState.Overlapped;
output.T = 0f;
return;
Block_3:
output.State = TOIOutputState.Touching;
output.T = fP2;
Block_13:
return;
Block_14:
output.State = TOIOutputState.Failed;
output.T = fP2;
}
/// <summary>
/// Compute the upper bound on time before two shapes penetrate. Time is represented as
/// a fraction between [0,tMax]. This uses a swept separating axis and may miss some intermediate,
/// non-tunneling collision. If you change the time interval, you should call this function
/// again.
/// Note: use Distance() to compute the contact point and normal at the time of impact.
/// </summary>
/// <param name="output">The output.</param>
/// <param name="input">The input.</param>
public static void CalculateTimeOfImpact(out TOIOutput output, TOIInput input)
{
if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled
{
++TOICalls;
}
output = new TOIOutput();
output.State = TOIOutputState.Unknown;
output.T = input.TMax;
Sweep sweepA = input.SweepA;
Sweep sweepB = input.SweepB;
// Large rotations can make the root finder fail, so we normalize the
// sweep angles.
sweepA.Normalize();
sweepB.Normalize();
FP tMax = input.TMax;
FP totalRadius = input.ProxyA.Radius + input.ProxyB.Radius;
FP target = TrueSync.TSMath.Max(Settings.LinearSlop, totalRadius - 3.0f * Settings.LinearSlop);
FP tolerance = 0.25f * Settings.LinearSlop;
Debug.Assert(target > tolerance);
FP t1 = 0.0f;
const int k_maxIterations = 20;
int iter = 0;
// Prepare input for distance query.
_distanceInput = _distanceInput ?? new DistanceInput();
_distanceInput.ProxyA = input.ProxyA;
_distanceInput.ProxyB = input.ProxyB;
_distanceInput.UseRadii = false;
// The outer loop progressively attempts to compute new separating axes.
// This loop terminates when an axis is repeated (no progress is made).
for (; ;)
{
Transform xfA, xfB;
sweepA.GetTransform(out xfA, t1);
sweepB.GetTransform(out xfB, t1);
// Get the distance between shapes. We can also use the results
// to get a separating axis.
_distanceInput.TransformA = xfA;
_distanceInput.TransformB = xfB;
DistanceOutput distanceOutput;
SimplexCache cache;
Distance.ComputeDistance(out distanceOutput, out cache, _distanceInput);
// If the shapes are overlapped, we give up on continuous collision.
if (distanceOutput.Distance <= 0.0f)
{
// Failure!
output.State = TOIOutputState.Overlapped;
output.T = 0.0f;
break;
}
if (distanceOutput.Distance < target + tolerance)
{
// Victory!
output.State = TOIOutputState.Touching;
output.T = t1;
break;
}
SeparationFunction.Set(ref cache, input.ProxyA, ref sweepA, input.ProxyB, ref sweepB, t1);
// Compute the TOI on the separating axis. We do this by successively
// resolving the deepest point. This loop is bounded by the number of vertices.
bool done = false;
FP t2 = tMax;
int pushBackIter = 0;
for (; ;)
{
// Find the deepest point at t2. Store the witness point indices.
int indexA, indexB;
FP s2 = SeparationFunction.FindMinSeparation(out indexA, out indexB, t2);
// Is the final configuration separated?
if (s2 > target + tolerance)
{
// Victory!
output.State = TOIOutputState.Seperated;
output.T = tMax;
done = true;
break;
}
// Has the separation reached tolerance?
if (s2 > target - tolerance)
{
// Advance the sweeps
t1 = t2;
break;
}
// Compute the initial separation of the witness points.
FP s1 = SeparationFunction.Evaluate(indexA, indexB, t1);
// Check for initial overlap. This might happen if the root finder
// runs out of iterations.
if (s1 < target - tolerance)
{
output.State = TOIOutputState.Failed;
output.T = t1;
done = true;
break;
}
// Check for touching
if (s1 <= target + tolerance)
{
// Victory! t1 should hold the TOI (could be 0.0).
output.State = TOIOutputState.Touching;
output.T = t1;
done = true;
break;
}
// Compute 1D root of: f(x) - target = 0
int rootIterCount = 0;
FP a1 = t1, a2 = t2;
for (; ;)
{
// Use a mix of the secant rule and bisection.
FP t;
if ((rootIterCount & 1) != 0)
{
// Secant rule to improve convergence.
t = a1 + (target - s1) * (a2 - a1) / (s2 - s1);
}
else
{
// Bisection to guarantee progress.
t = 0.5f * (a1 + a2);
}
++rootIterCount;
if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled
{
++TOIRootIters;
}
FP s = SeparationFunction.Evaluate(indexA, indexB, t);
if (FP.Abs(s - target) < tolerance)
{
// t2 holds a tentative value for t1
t2 = t;
break;
}
// Ensure we continue to bracket the root.
if (s > target)
{
a1 = t;
s1 = s;
}
else
{
a2 = t;
s2 = s;
}
if (rootIterCount == 50)
{
break;
}
}
if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled
{
TOIMaxRootIters = Math.Max(TOIMaxRootIters, rootIterCount);
}
++pushBackIter;
if (pushBackIter == Settings.MaxPolygonVertices)
{
break;
}
}
++iter;
if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled
{
++TOIIters;
}
if (done)
{
break;
}
if (iter == k_maxIterations)
{
// Root finder got stuck. Semi-victory.
output.State = TOIOutputState.Failed;
output.T = t1;
break;
}
}
if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled
{
TOIMaxIters = Math.Max(TOIMaxIters, iter);
}
}
