public static void Initialize(ref SimplexCache cache, DistanceProxy proxyA, ref Sweep sweepA, DistanceProxy proxyB, ref Sweep sweepB, float t1, out Vector2 axis, out Vector2 localPoint, out SeparationFunctionType type) { int count = cache.Count; System.Diagnostics.Debug.Assert(0 < count && count < 3); Transform xfA, xfB; sweepA.GetTransform(out xfA, t1); sweepB.GetTransform(out xfB, t1); if (count == 1) { localPoint = Vector2.Zero; type = SeparationFunctionType.Points; Vector2 localPointA = proxyA.Vertices[cache.IndexA[0]]; Vector2 localPointB = proxyB.Vertices[cache.IndexB[0]]; Vector2 pointA = MathUtils.Mul(ref xfA, localPointA); Vector2 pointB = MathUtils.Mul(ref xfB, localPointB); axis = pointB - pointA; axis.Normalize(); } else if (cache.IndexA[0] == cache.IndexA[1]) { // Two points on B and one on A. type = SeparationFunctionType.FaceB; Vector2 localPointB1 = proxyB.Vertices[cache.IndexB[0]]; Vector2 localPointB2 = proxyB.Vertices[cache.IndexB[1]]; Vector2 a = localPointB2 - localPointB1; axis = new Vector2(a.Y, -a.X); axis.Normalize(); Vector2 normal = MathUtils.Mul(ref xfB.q, axis); localPoint = 0.5f * (localPointB1 + localPointB2); Vector2 pointB = MathUtils.Mul(ref xfB, localPoint); Vector2 localPointA = proxyA.Vertices[cache.IndexA[0]]; Vector2 pointA = MathUtils.Mul(ref xfA, localPointA); float s = Vector2.Dot(pointA - pointB, normal); if (s < 0.0f) { axis = -axis; } } else { // Two points on A and one or two points on B. type = SeparationFunctionType.FaceA; Vector2 localPointA1 = proxyA.Vertices[cache.IndexA[0]]; Vector2 localPointA2 = proxyA.Vertices[cache.IndexA[1]]; Vector2 a = localPointA2 - localPointA1; axis = new Vector2(a.Y, -a.X); axis.Normalize(); Vector2 normal = MathUtils.Mul(ref xfA.q, axis); localPoint = 0.5f * (localPointA1 + localPointA2); Vector2 pointA = MathUtils.Mul(ref xfA, localPoint); Vector2 localPointB = proxyB.Vertices[cache.IndexB[0]]; Vector2 pointB = MathUtils.Mul(ref xfB, localPointB); float s = Vector2.Dot(pointB - pointA, normal); if (s < 0.0f) { axis = -axis; } } //Velcro note: the returned value that used to be here has been removed, as it was not used. }
/// <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) //Velcro: 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(); float tMax = input.TMax; float totalRadius = input.ProxyA.Radius + input.ProxyB.Radius; float target = Math.Max(Settings.LinearSlop, totalRadius - 3.0f * Settings.LinearSlop); const float tolerance = 0.25f * Settings.LinearSlop; System.Diagnostics.Debug.Assert(target > tolerance); float 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.Initialize(ref cache, input.ProxyA, ref sweepA, input.ProxyB, ref sweepB, t1, out Vector2 axis, out Vector2 localPoint, out SeparationFunctionType type); // 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; float t2 = tMax; int pushBackIter = 0; for (;;) { // Find the deepest point at t2. Store the witness point indices. int indexA, indexB; float s2 = SeparationFunction.FindMinSeparation(out indexA, out indexB, t2, input.ProxyA, ref sweepA, input.ProxyB, ref sweepB, ref axis, ref localPoint, type); // 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. float s1 = SeparationFunction.Evaluate(indexA, indexB, t1, input.ProxyA, ref sweepA, input.ProxyB, ref sweepB, ref axis, ref localPoint, type); // 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; float a1 = t1, a2 = t2; for (;;) { // Use a mix of the secant rule and bisection. float 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) //Velcro: We only gather diagnostics when enabled { ++TOIRootIters; } float s = SeparationFunction.Evaluate(indexA, indexB, t, input.ProxyA, ref sweepA, input.ProxyB, ref sweepB, ref axis, ref localPoint, type); if (Math.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) //Velcro: We only gather diagnostics when enabled { TOIMaxRootIters = Math.Max(TOIMaxRootIters, rootIterCount); } ++pushBackIter; if (pushBackIter == Settings.MaxPolygonVertices) { break; } } ++iter; if (Settings.EnableDiagnostics) //Velcro: 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) //Velcro: We only gather diagnostics when enabled { TOIMaxIters = Math.Max(TOIMaxIters, iter); } }
public static float FindMinSeparation(out int indexA, out int indexB, float t, DistanceProxy proxyA, ref Sweep sweepA, DistanceProxy proxyB, ref Sweep sweepB, ref Vector2 axis, ref Vector2 localPoint, SeparationFunctionType type) { Transform xfA, xfB; sweepA.GetTransform(out xfA, t); sweepB.GetTransform(out xfB, t); switch (type) { case SeparationFunctionType.Points: { Vector2 axisA = MathUtils.MulT(ref xfA.q, axis); Vector2 axisB = MathUtils.MulT(ref xfB.q, -axis); indexA = proxyA.GetSupport(axisA); indexB = proxyB.GetSupport(axisB); Vector2 localPointA = proxyA.Vertices[indexA]; Vector2 localPointB = proxyB.Vertices[indexB]; Vector2 pointA = MathUtils.Mul(ref xfA, localPointA); Vector2 pointB = MathUtils.Mul(ref xfB, localPointB); float separation = Vector2.Dot(pointB - pointA, axis); return(separation); } case SeparationFunctionType.FaceA: { Vector2 normal = MathUtils.Mul(ref xfA.q, axis); Vector2 pointA = MathUtils.Mul(ref xfA, localPoint); Vector2 axisB = MathUtils.MulT(ref xfB.q, -normal); indexA = -1; indexB = proxyB.GetSupport(axisB); Vector2 localPointB = proxyB.Vertices[indexB]; Vector2 pointB = MathUtils.Mul(ref xfB, localPointB); float separation = Vector2.Dot(pointB - pointA, normal); return(separation); } case SeparationFunctionType.FaceB: { Vector2 normal = MathUtils.Mul(ref xfB.q, axis); Vector2 pointB = MathUtils.Mul(ref xfB, localPoint); Vector2 axisA = MathUtils.MulT(ref xfA.q, -normal); indexB = -1; indexA = proxyA.GetSupport(axisA); Vector2 localPointA = proxyA.Vertices[indexA]; Vector2 pointA = MathUtils.Mul(ref xfA, localPointA); float separation = Vector2.Dot(pointA - pointB, normal); return(separation); } default: System.Diagnostics.Debug.Assert(false); indexA = -1; indexB = -1; return(0.0f); } }