// TODO_ERIN might not need to return the separation float Initialize(SimplexCache cache, DistanceProxy proxyA, Sweep sweepA, DistanceProxy proxyB, Sweep sweepB, float t1) { m_proxyA = proxyA; m_proxyB = proxyB; int count = cache.count; Utilities.Assert(0 < count && count < 3); m_sweepA = sweepA; m_sweepB = sweepB; Transform xfA, xfB; m_sweepA.GetTransform(out xfA, t1); m_sweepB.GetTransform(out xfB, t1); if (count == 1) { m_type = SeparationType.e_points; Vec2 localPointA = m_proxyA.GetVertex(cache.indexA[0]); Vec2 localPointB = m_proxyB.GetVertex(cache.indexB[0]); Vec2 pointA = Utilities.Mul(xfA, localPointA); Vec2 pointB = Utilities.Mul(xfB, localPointB); m_axis = pointB - pointA; float s = m_axis.Normalize(); return(s); } else if (cache.indexA[0] == cache.indexA[1]) { // Two points on B and one on A. m_type = SeparationType.e_faceB; Vec2 localPointB1 = proxyB.GetVertex(cache.indexB[0]); Vec2 localPointB2 = proxyB.GetVertex(cache.indexB[1]); m_axis = Utilities.Cross(localPointB2 - localPointB1, 1.0f); m_axis.Normalize(); Vec2 normal = Utilities.Mul(xfB.q, m_axis); m_localPoint = 0.5f * (localPointB1 + localPointB2); Vec2 pointB = Utilities.Mul(xfB, m_localPoint); Vec2 localPointA = proxyA.GetVertex(cache.indexA[0]); Vec2 pointA = Utilities.Mul(xfA, localPointA); float s = Utilities.Dot(pointA - pointB, normal); if (s < 0.0f) { m_axis = -m_axis; s = -s; } return(s); } else { // Two points on A and one or two points on B. m_type = SeparationType.e_faceA; Vec2 localPointA1 = m_proxyA.GetVertex(cache.indexA[0]); Vec2 localPointA2 = m_proxyA.GetVertex(cache.indexA[1]); m_axis = Utilities.Cross(localPointA2 - localPointA1, 1.0f); m_axis.Normalize(); Vec2 normal = Utilities.Mul(xfA.q, m_axis); m_localPoint = 0.5f * (localPointA1 + localPointA2); Vec2 pointA = Utilities.Mul(xfA, m_localPoint); Vec2 localPointB = m_proxyB.GetVertex(cache.indexB[0]); Vec2 pointB = Utilities.Mul(xfB, localPointB); float s = Utilities.Dot(pointB - pointA, normal); if (s < 0.0f) { m_axis = -m_axis; s = -s; } return(s); } }
public SeparationFunction(ref SimplexCache cache, ref DistanceProxy proxyA, ref Sweep sweepA, ref DistanceProxy proxyB, ref Sweep sweepB, float t1) { _localPoint = Vector2.zero; _proxyA = proxyA; _proxyB = proxyB; int count = cache.count; //Debug.Assert(0 < count && count < 3); _sweepA = sweepA; _sweepB = sweepB; Transform xfA, xfB; _sweepA.GetTransform(out xfA, t1); _sweepB.GetTransform(out xfB, t1); if (count == 1) { _type = SeparationFunctionType.Points; Vector2 localPointA = _proxyA.GetVertex(cache.indexA[0]); Vector2 localPointB = _proxyB.GetVertex(cache.indexB[0]); Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA); Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB); _axis = pointB - pointA; _axis.Normalize(); return; } else if (cache.indexA[0] == cache.indexA[1]) { // Two points on B and one on A. _type = SeparationFunctionType.FaceB; Vector2 localPointB1 = proxyB.GetVertex(cache.indexB[0]); Vector2 localPointB2 = proxyB.GetVertex(cache.indexB[1]); _axis = MathUtils.Cross(localPointB2 - localPointB1, 1.0f); _axis.Normalize(); Vector2 normal = MathUtils.Multiply(ref xfB.R, _axis); _localPoint = 0.5f * (localPointB1 + localPointB2); Vector2 pointB = MathUtils.Multiply(ref xfB, _localPoint); Vector2 localPointA = proxyA.GetVertex(cache.indexA[0]); Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA); float s = Vector2.Dot(pointA - pointB, normal); if (s < 0.0f) { _axis = -_axis; s = -s; } return; } else { // Two points on A and one or two points on B. _type = SeparationFunctionType.FaceA; Vector2 localPointA1 = _proxyA.GetVertex(cache.indexA[0]); Vector2 localPointA2 = _proxyA.GetVertex(cache.indexA[1]); _axis = MathUtils.Cross(localPointA2 - localPointA1, 1.0f); _axis.Normalize(); Vector2 normal = MathUtils.Multiply(ref xfA.R, _axis); _localPoint = 0.5f * (localPointA1 + localPointA2); Vector2 pointA = MathUtils.Multiply(ref xfA, _localPoint); Vector2 localPointB = _proxyB.GetVertex(cache.indexB[0]); Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB); float s = Vector2.Dot(pointB - pointA, normal); if (s < 0.0f) { _axis = -_axis; s = -s; } return; } }
// CCD via the local separating axis method. This seeks progression // by computing the largest time at which separation is maintained. /// 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 b2Distance to compute the contact point and normal at the time of impact. public static void CalculateTimeOfImpact(out TOIOutput output, ref TOIInput input) { ++b2_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.b2_linearSlop, totalRadius - 3.0f * Settings.b2_linearSlop); float tolerance = 0.25f * Settings.b2_linearSlop; //Debug.Assert(target > tolerance); float t1 = 0.0f; int k_maxIterations = 20; int iter = 0; // Prepare input for distance query. SimplexCache cache; DistanceInput 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; Distance.ComputeDistance(out distanceOutput, out cache, ref 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 fcn = new SeparationFunction(ref cache, ref input.proxyA, ref sweepA, ref 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; float t2 = tMax; int pushBackIter = 0; for (;;) { // Find the deepest point at t2. Store the witness point indices. int indexA, indexB; float s2 = fcn.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. float s1 = fcn.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; 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); } float s = fcn.Evaluate(indexA, indexB, t); 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; } ++rootIterCount; ++b2_toiRootIters; if (rootIterCount == 50) { break; } } b2_toiMaxRootIters = Math.Max(b2_toiMaxRootIters, rootIterCount); ++pushBackIter; if (pushBackIter == Settings.b2_maxPolygonVertices) { break; } } ++iter; ++b2_toiIters; if (done) { break; } if (iter == k_maxIterations) { // Root finder got stuck. Semi-victory. output.State = TOIOutputState.Failed; output.t = t1; break; } } b2_toiMaxIters = Math.Max(b2_toiMaxIters, iter); }
public override void Step(TestSettings settings) { base.Step(settings); Sweep sweepA = new Sweep(); sweepA.c0.Set(24.0f, -60.0f); sweepA.a0 = 2.95f; sweepA.c = sweepA.c0; sweepA.a = sweepA.a0; sweepA.localCenter.SetZero(); Sweep sweepB = new Sweep(); sweepB.c0.Set(53.474274f, -50.252514f); sweepB.a0 = 513.36676f; // - 162.0f * (float)Math.PI; sweepB.c.Set(54.595478f, -51.083473f); sweepB.a = 513.62781f; // - 162.0f * (float)Math.PI; sweepB.localCenter.SetZero(); //sweepB.a0 -= 300.0f * (float)Math.PI; //sweepB.a -= 300.0f * (float)Math.PI; TOIInput input = new TOIInput(); input.proxyA.Set(m_shapeA, 0); input.proxyB.Set(m_shapeB, 0); input.sweepA = sweepA; input.sweepB = sweepB; input.tMax = 1.0f; TOIOutput output; Utilities.TimeOfImpact(out output, input); m_debugDraw.DrawString("toi = {0}", output.t); m_debugDraw.DrawString("max toi iters = {0}, max root iters = {1}", Utilities._toiMaxIters, Utilities._toiMaxRootIters); Vec2[] vertices = new Vec2[Settings._maxPolygonVertices]; Transform transformA; sweepA.GetTransform(out transformA, 0.0f); for (int i = 0; i < m_shapeA.m_count; ++i) { vertices[i] = Utilities.Mul(transformA, m_shapeA.m_vertices[i]); } m_debugDraw.DrawPolygon(vertices, m_shapeA.m_count, Color.FromArgb(225, 225, 225)); Transform transformB; sweepB.GetTransform(out transformB, 0.0f); //Vec2 localPoint(2.0f, -0.1f); for (int i = 0; i < m_shapeB.m_count; ++i) { vertices[i] = Utilities.Mul(transformB, m_shapeB.m_vertices[i]); } m_debugDraw.DrawPolygon(vertices, m_shapeB.m_count, Color.FromArgb(128, 225, 128)); sweepB.GetTransform(out transformB, output.t); for (int i = 0; i < m_shapeB.m_count; ++i) { vertices[i] = Utilities.Mul(transformB, m_shapeB.m_vertices[i]); } m_debugDraw.DrawPolygon(vertices, m_shapeB.m_count, Color.FromArgb(128, 175, 225)); sweepB.GetTransform(out transformB, 1.0f); for (int i = 0; i < m_shapeB.m_count; ++i) { vertices[i] = Utilities.Mul(transformB, m_shapeB.m_vertices[i]); } m_debugDraw.DrawPolygon(vertices, m_shapeB.m_count, Color.FromArgb(225, 128, 128)); #if ZERO for (float t = 0.0f; t < 1.0f; t += 0.1f) { sweepB.GetTransform(out transformB, t); for (int i = 0; i < m_shapeB.m_count; ++i) { vertices[i] = Utilities.Mul(transformB, m_shapeB.m_vertices[i]); } m_debugDraw.DrawPolygon(vertices, m_shapeB.m_count, Color.FromArgb(225, 0.5f, 0.5f)); } #endif }
/// 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. // CCD via the local separating axis method. This seeks progression // by computing the largest time at which separation is maintained. public static void TimeOfImpact(out TOIOutput output, TOIInput input) { Timer timer = new Timer(); ++_toiCalls; output.state = TOIOutput.State.e_unknown; output.t = input.tMax; DistanceProxy proxyA = input.proxyA; DistanceProxy proxyB = input.proxyB; 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 = proxyA.m_radius + proxyB.m_radius; float target = Math.Max(Settings._linearSlop, totalRadius - 3.0f * Settings._linearSlop); float tolerance = 0.25f * Settings._linearSlop; Utilities.Assert(target > tolerance); float t1 = 0.0f; const int k_maxIterations = 20; // TODO_ERIN Settings int iter = 0; // Prepare input for distance query. SimplexCache cache = new SimplexCache(); cache.count = 0; DistanceInput 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; Utilities.Distance(out distanceOutput, cache, distanceInput); // If the shapes are overlapped, we give up on continuous collision. if (distanceOutput.distance <= 0.0f) { // Failure! output.state = TOIOutput.State.e_overlapped; output.t = 0.0f; break; } if (distanceOutput.distance < target + tolerance) { // Victory! output.state = TOIOutput.State.e_touching; output.t = t1; break; } // Initialize the separating axis. throw new NotImplementedException(); // SeparationFunction fcn; // fcn.Initialize(&cache, proxyA, sweepA, proxyB, sweepB, t1); //#if ZERO // // Dump the curve seen by the root finder // { // const int N = 100; // float dx = 1.0f / N; // float xs[N+1]; // float fs[N+1]; // float x = 0.0f; // for (int i = 0; i <= N; ++i) // { // sweepA.GetTransform(out xfA, x); // sweepB.GetTransform(out xfB, x); // float f = fcn.Evaluate(xfA, xfB) - target; // printf("%g %g\n", x, f); // xs[i] = x; // fs[i] = f; // x += dx; // } // } //#endif // // 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 = fcn.FindMinSeparation(&indexA, &indexB, t2); // // Is the final configuration separated? // if (s2 > target + tolerance) // { // // Victory! // output.state = TOIOutput.State.e_separated; // 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 = fcn.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 = TOIOutput.State.e_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 = TOIOutput.State.e_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) // { // // Secant rule to improve convergence. // t = a1 + (target - s1) * (a2 - a1) / (s2 - s1); // } // else // { // // Bisection to guarantee progress. // t = 0.5f * (a1 + a2); // } // ++rootIterCount; // ++_toiRootIters; // float s = fcn.Evaluate(indexA, indexB, t); // 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; // } // } // _toiMaxRootIters = Math.Max(_toiMaxRootIters, rootIterCount); // ++pushBackIter; // if (pushBackIter == Settings._maxPolygonVertices) // { // break; // } // } // ++iter; // ++_toiIters; // if (done) // { // break; // } // if (iter == k_maxIterations) // { // // Root finder got stuck. Semi-victory. // output.state = TOIOutput.State.e_failed; // output.t = t1; // break; // } } _toiMaxIters = Math.Max(_toiMaxIters, iter); float time = timer.GetMilliseconds(); _toiMaxTime = Math.Max(_toiMaxTime, time); _toiTime += time; }
// TODO_ERIN might not need to return the separation float Initialize(SimplexCache cache, DistanceProxy proxyA, Sweep sweepA, DistanceProxy proxyB, Sweep sweepB, float t1) { m_proxyA = proxyA; m_proxyB = proxyB; int count = cache.count; Utilities.Assert(0 < count && count < 3); m_sweepA = sweepA; m_sweepB = sweepB; Transform xfA, xfB; m_sweepA.GetTransform(out xfA, t1); m_sweepB.GetTransform(out xfB, t1); if (count == 1) { m_type = SeparationType.e_points; Vec2 localPointA = m_proxyA.GetVertex(cache.indexA[0]); Vec2 localPointB = m_proxyB.GetVertex(cache.indexB[0]); Vec2 pointA = Utilities.Mul(xfA, localPointA); Vec2 pointB = Utilities.Mul(xfB, localPointB); m_axis = pointB - pointA; float s = m_axis.Normalize(); return s; } else if (cache.indexA[0] == cache.indexA[1]) { // Two points on B and one on A. m_type = SeparationType.e_faceB; Vec2 localPointB1 = proxyB.GetVertex(cache.indexB[0]); Vec2 localPointB2 = proxyB.GetVertex(cache.indexB[1]); m_axis = Utilities.Cross(localPointB2 - localPointB1, 1.0f); m_axis.Normalize(); Vec2 normal = Utilities.Mul(xfB.q, m_axis); m_localPoint = 0.5f * (localPointB1 + localPointB2); Vec2 pointB = Utilities.Mul(xfB, m_localPoint); Vec2 localPointA = proxyA.GetVertex(cache.indexA[0]); Vec2 pointA = Utilities.Mul(xfA, localPointA); float s = Utilities.Dot(pointA - pointB, normal); if (s < 0.0f) { m_axis = -m_axis; s = -s; } return s; } else { // Two points on A and one or two points on B. m_type = SeparationType.e_faceA; Vec2 localPointA1 = m_proxyA.GetVertex(cache.indexA[0]); Vec2 localPointA2 = m_proxyA.GetVertex(cache.indexA[1]); m_axis = Utilities.Cross(localPointA2 - localPointA1, 1.0f); m_axis.Normalize(); Vec2 normal = Utilities.Mul(xfA.q, m_axis); m_localPoint = 0.5f * (localPointA1 + localPointA2); Vec2 pointA = Utilities.Mul(xfA, m_localPoint); Vec2 localPointB = m_proxyB.GetVertex(cache.indexB[0]); Vec2 pointB = Utilities.Mul(xfB, localPointB); float s = Utilities.Dot(pointB - pointA, normal); if (s < 0.0f) { m_axis = -m_axis; s = -s; } return s; } }