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); } }