public override void Step(Settings settings) { base.Step(settings); DistanceInput input = new DistanceInput(); input.TransformA = _transformA; input.TransformB = _transformB; input.UseRadii = true; SimplexCache cache = new SimplexCache(); cache.Count = 0; DistanceOutput output; Collision.Distance(out output, ref cache, ref input, _polygonA, _polygonB); StringBuilder strBld = new StringBuilder(); strBld.AppendFormat("distance = {0}", new object[] { output.Distance }); OpenGLDebugDraw.DrawString(5, _textLine, strBld.ToString()); _textLine += 15; strBld = new StringBuilder(); strBld.AppendFormat("iterations = {0}", new object[] { output.Iterations }); OpenGLDebugDraw.DrawString(5, _textLine, strBld.ToString()); _textLine += 15; { Color color = new Color(0.9f, 0.9f, 0.9f); int i; for (i = 0; i < _polygonA.VertexCount; ++i) { _dv[i] = Math.Mul(_transformA, _polygonA.Vertices[i]); } _debugDraw.DrawPolygon(_dv, _polygonA.VertexCount, color); for (i = 0; i < _polygonB.VertexCount; ++i) { _dv[i] = Math.Mul(_transformB, _polygonB.Vertices[i]); } _debugDraw.DrawPolygon(_dv, _polygonB.VertexCount, color); } Vec2 x1 = output.PointA; Vec2 x2 = output.PointB; OpenGLDebugDraw.DrawPoint(x1, 4.0f, new Color(1, 0, 0)); OpenGLDebugDraw.DrawSegment(x1, x2, new Color(1, 1, 0)); OpenGLDebugDraw.DrawPoint(x2, 4.0f, new Color(1, 0, 0)); }
static void Distance(out DistanceOutput output, ref SimplexCache cache, ref DistanceInput input, Shape shapeA, Shape shapeB) { output = new DistanceOutput(); Transform transformA = input.TransformA; Transform transformB = input.TransformB; // Initialize the simplex. Simplex simplex = new Simplex(); #if ALLOWUNSAFE fixed (SimplexCache* sPtr = &cache) { simplex.ReadCache(sPtr, shapeA, transformA, shapeB, transformB); } #else simplex.ReadCache(cache, shapeA, transformA, shapeB, transformB); #endif // Get simplex vertices as an array. #if ALLOWUNSAFE SimplexVertex* vertices = &simplex._v1; #else SimplexVertex[] vertices = new SimplexVertex[] { simplex._v1, simplex._v2, simplex._v3 }; #endif // These store the vertices of the last simplex so that we // can check for duplicates and prevent cycling. #if ALLOWUNSAFE int* lastA = stackalloc int[4], lastB = stackalloc int[4]; #else int[] lastA = new int[4]; int[] lastB = new int[4]; #endif // ALLOWUNSAFE int lastCount; // Main iteration loop. int iter = 0; const int k_maxIterationCount = 20; while (iter < k_maxIterationCount) { // Copy simplex so we can identify duplicates. lastCount = simplex._count; int i; for (i = 0; i < lastCount; ++i) { lastA[i] = vertices[i].indexA; lastB[i] = vertices[i].indexB; } switch (simplex._count) { case 1: break; case 2: simplex.Solve2(); break; case 3: simplex.Solve3(); break; default: #if DEBUG Box2DXDebug.Assert(false); #endif break; } // If we have 3 points, then the origin is in the corresponding triangle. if (simplex._count == 3) { break; } // Compute closest point. Vector2 p = simplex.GetClosestPoint(); float distanceSqr = p.sqrMagnitude; // Ensure the search direction is numerically fit. if (distanceSqr < Common.Settings.FLT_EPSILON_SQUARED) { // 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. #if ALLOWUNSAFE SimplexVertex* vertex = vertices + simplex._count; vertex->indexA = shapeA.GetSupport(transformA.InverseTransformDirection(p)); vertex->wA = transformA.TransformPoint(shapeA.GetVertex(vertex->indexA)); //Vec2 wBLocal; vertex->indexB = shapeB.GetSupport(transformB.InverseTransformDirection(-p)); vertex->wB = transformB.TransformPoint(shapeB.GetVertex(vertex->indexB)); vertex->w = vertex->wB - vertex->wA; #else SimplexVertex vertex = vertices[simplex._count - 1]; vertex.indexA = shapeA.GetSupport(transformA.InverseTransformDirection(p)); vertex.wA = transformA.TransformPoint(shapeA.GetVertex(vertex.indexA)); //Vec2 wBLocal; vertex.indexB = shapeB.GetSupport(transformB.InverseTransformDirection(-p)); vertex.wB = transformB.TransformPoint(shapeB.GetVertex(vertex.indexB)); vertex.w = vertex.wB - vertex.wA; #endif // ALLOWUNSAFE // Iteration count is equated to the number of support point calls. ++iter; // Check for convergence. #if ALLOWUNSAFE float lowerBound = Vector2.Dot(p, vertex->w); #else float lowerBound = Vector2.Dot(p, vertex.w); #endif float upperBound = distanceSqr; const float k_relativeTolSqr = 0.01f * 0.01f; // 1:100 if (upperBound - lowerBound <= k_relativeTolSqr * upperBound) { // Converged! break; } // Check for duplicate support points. bool duplicate = false; for (i = 0; i < lastCount; ++i) { #if ALLOWUNSAFE if (vertex->indexA == lastA[i] && vertex->indexB == lastB[i]) #else if (vertex.indexA == lastA[i] && vertex.indexB == lastB[i]) #endif { 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 ALLOWUNSAFE fixed (DistanceOutput* doPtr = &output) { // Prepare output. simplex.GetWitnessPoints(&doPtr->PointA, &doPtr->PointB); doPtr->Distance = Vector2.Distance(doPtr->PointA, doPtr->PointB); doPtr->Iterations = iter; } fixed (SimplexCache* sPtr = &cache) { // Cache the simplex. simplex.WriteCache(sPtr); } #else // Prepare output. simplex.GetWitnessPoints(out output.PointA, out output.PointB); output.Distance = Vector2.Distance(output.PointA, output.PointB); output.Iterations = iter; // Cache the simplex. simplex.WriteCache(cache); #endif // Apply radii if requested. if (input.UseRadii) { float rA = shapeA._radius; float rB = shapeB._radius; if (output.Distance > rA + rB && output.Distance > Common.Settings.FLT_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; } } }
static void Distance(out DistanceOutput output, ref SimplexCache cache, ref DistanceInput input, Shape shapeA, Shape shapeB) { output = new DistanceOutput(); Transform transformA = input.TransformA; Transform transformB = input.TransformB; // Initialize the simplex. Simplex simplex = new Simplex(); #if ALLOWUNSAFE fixed(SimplexCache *sPtr = &cache) { simplex.ReadCache(sPtr, shapeA, transformA, shapeB, transformB); } #else simplex.ReadCache(cache, shapeA, transformA, shapeB, transformB); #endif // Get simplex vertices as an array. #if ALLOWUNSAFE SimplexVertex *vertices = &simplex._v1; #else SimplexVertex[] vertices = new SimplexVertex[] { simplex._v1, simplex._v2, simplex._v3 }; #endif // These store the vertices of the last simplex so that we // can check for duplicates and prevent cycling. #if ALLOWUNSAFE int *lastA = stackalloc int[4], lastB = stackalloc int[4]; #else int[] lastA = new int[4]; int[] lastB = new int[4]; #endif // ALLOWUNSAFE int lastCount; // Main iteration loop. int iter = 0; const int k_maxIterationCount = 20; while (iter < k_maxIterationCount) { // Copy simplex so we can identify duplicates. lastCount = simplex._count; int i; for (i = 0; i < lastCount; ++i) { lastA[i] = vertices[i].indexA; lastB[i] = vertices[i].indexB; } switch (simplex._count) { case 1: break; case 2: simplex.Solve2(); break; case 3: simplex.Solve3(); break; default: #if DEBUG Box2DXDebug.Assert(false); #endif break; } // If we have 3 points, then the origin is in the corresponding triangle. if (simplex._count == 3) { break; } // Compute closest point. Vector2 p = simplex.GetClosestPoint(); float distanceSqr = p.sqrMagnitude; // Ensure the search direction is numerically fit. if (distanceSqr < Common.Settings.FLT_EPSILON_SQUARED) { // 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. #if ALLOWUNSAFE SimplexVertex *vertex = vertices + simplex._count; vertex->indexA = shapeA.GetSupport(transformA.InverseTransformDirection(p)); vertex->wA = transformA.TransformPoint(shapeA.GetVertex(vertex->indexA)); //Vec2 wBLocal; vertex->indexB = shapeB.GetSupport(transformB.InverseTransformDirection(-p)); vertex->wB = transformB.TransformPoint(shapeB.GetVertex(vertex->indexB)); vertex->w = vertex->wB - vertex->wA; #else SimplexVertex vertex = vertices[simplex._count - 1]; vertex.indexA = shapeA.GetSupport(transformA.InverseTransformDirection(p)); vertex.wA = transformA.TransformPoint(shapeA.GetVertex(vertex.indexA)); //Vec2 wBLocal; vertex.indexB = shapeB.GetSupport(transformB.InverseTransformDirection(-p)); vertex.wB = transformB.TransformPoint(shapeB.GetVertex(vertex.indexB)); vertex.w = vertex.wB - vertex.wA; #endif // ALLOWUNSAFE // Iteration count is equated to the number of support point calls. ++iter; // Check for convergence. #if ALLOWUNSAFE float lowerBound = Vector2.Dot(p, vertex->w); #else float lowerBound = Vector2.Dot(p, vertex.w); #endif float upperBound = distanceSqr; const float k_relativeTolSqr = 0.01f * 0.01f; // 1:100 if (upperBound - lowerBound <= k_relativeTolSqr * upperBound) { // Converged! break; } // Check for duplicate support points. bool duplicate = false; for (i = 0; i < lastCount; ++i) { #if ALLOWUNSAFE if (vertex->indexA == lastA[i] && vertex->indexB == lastB[i]) #else if (vertex.indexA == lastA[i] && vertex.indexB == lastB[i]) #endif { 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 ALLOWUNSAFE fixed(DistanceOutput *doPtr = &output) { // Prepare output. simplex.GetWitnessPoints(&doPtr->PointA, &doPtr->PointB); doPtr->Distance = Vector2.Distance(doPtr->PointA, doPtr->PointB); doPtr->Iterations = iter; } fixed(SimplexCache *sPtr = &cache) { // Cache the simplex. simplex.WriteCache(sPtr); } #else // Prepare output. simplex.GetWitnessPoints(out output.PointA, out output.PointB); output.Distance = Vector2.Distance(output.PointA, output.PointB); output.Iterations = iter; // Cache the simplex. simplex.WriteCache(cache); #endif // Apply radii if requested. if (input.UseRadii) { float rA = shapeA._radius; float rB = shapeB._radius; if (output.Distance > rA + rB && output.Distance > Common.Settings.FLT_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; } } }
/// <summary> /// CCD via the secant method. /// Compute the time when two shapes begin to touch or touch at a closer distance. /// TOI considers the shape radii. It attempts to have the radii overlap by the tolerance. /// Iterations terminate with the overlap is within 0.5 * tolerance. The tolerance should be /// smaller than sum of the shape radii. /// @warning the sweeps must have the same time interval. /// fraction=0 means the shapes begin touching/overlapped, and fraction=1 means the shapes don't touch. /// </summary> /// <param name="input">The input.</param> /// <param name="shapeA">The shape A.</param> /// <param name="shapeB">The shape B.</param> /// <returns> /// fraction between [0,1] in which the shapes first touch. /// </returns> public static float TimeOfImpact(TOIInput input) { ++ToiCalls; DistanceProxy proxyA = input.ProxyA; DistanceProxy proxyB = input.ProxyB; Sweep sweepA = input.SweepA; Sweep sweepB = input.SweepB; Box2DXDebug.Assert(sweepA.T0 == sweepB.T0); Box2DXDebug.Assert(1.0f - sweepA.T0 > Settings.FLT_EPSILON); float radius = proxyA._radius + proxyB._radius; float tolerance = input.Tolerance; float alpha = 0.0f; const int k_maxIterations = 1000; // TODO_ERIN b2Settings int iter = 0; float target = 0.0f; // Prepare input for distance query. SimplexCache cache = new SimplexCache(); cache.Count = 0; DistanceInput distanceInput = new DistanceInput(); distanceInput.proxyA = input.ProxyA; distanceInput.proxyB = input.ProxyB; distanceInput.UseRadii = false; for (;;) { Transform xfA, xfB; sweepA.GetTransform(out xfA, alpha); sweepB.GetTransform(out xfB, alpha); // Get the distance between shapes. distanceInput.TransformA = xfA; distanceInput.TransformB = xfB; DistanceOutput distanceOutput; Distance(out distanceOutput, cache, distanceInput); if (distanceOutput.Distance <= 0.0f) { alpha = 1.0f; break; } SeparationFunction fcn = new SeparationFunction(); fcn.Initialize(cache, proxyA, xfA, proxyB, xfB); float separation = fcn.Evaluate(xfA, xfB); if (separation <= 0.0f) { alpha = 1.0f; break; } if (iter == 0) { // Compute a reasonable target distance to give some breathing room // for conservative advancement. We take advantage of the shape radii // to create additional clearance. if (separation > radius) { target = Math.Max(radius - tolerance, 0.75f*radius); } else { target = Math.Max(separation - tolerance, 0.02f*radius); } } if (separation - target < 0.5f*tolerance) { if (iter == 0) { alpha = 1.0f; break; } break; } #if false // Dump the curve seen by the root finder { const int N = 100; float dx = 1.0f / N; float xs[N+1];
/// <summary> /// Compute the closest points between two shapes. Supports any combination of: /// b2CircleShape, b2PolygonShape, b2EdgeShape. The simplex cache is input/output. /// On the first call set b2SimplexCache.count to zero. /// </summary> public static void Distance(out DistanceOutput output, SimplexCache cache, DistanceInput input) { ++GjkCalls; DistanceProxy proxyA = input.proxyA; DistanceProxy proxyB = input.proxyB; Transform transformA = input.TransformA; Transform transformB = input.TransformB; // Initialize the simplex. Simplex simplex = new Simplex(); simplex.ReadCache(cache, proxyA, ref transformA, proxyB, ref transformB); // Get simplex vertices as an array. SimplexVertex[] vertices = simplex.Vertices; const int k_maxIters = 20; // These store the vertices of the last simplex so that we // can check for duplicates and prevent cycling. int[] saveA = new int[3], saveB = new int[3]; int saveCount = 0; Vec2 closestPoint = simplex.GetClosestPoint(); float distanceSqr1 = closestPoint.LengthSquared(); float distanceSqr2 = distanceSqr1; // Main iteration loop. int iter = 0; while (iter < k_maxIters) { // Copy simplex so we can identify duplicates. saveCount = simplex.Count; for (int i = 0; i < saveCount; ++i) { saveA[i] = vertices[i].IndexA; saveB[i] = vertices[i].IndexB; } switch (simplex.Count) { case 1: break; case 2: simplex.Solve2(); break; case 3: simplex.Solve3(); break; default: Box2DXDebug.Assert(false); break; } // If we have 3 points, then the origin is in the corresponding triangle. if (simplex.Count == 3) { break; } // Compute closest point. Vec2 p = simplex.GetClosestPoint(); float distanceSqr = p.LengthSquared(); // Ensure progress if (distanceSqr2 >= distanceSqr1) { //break; } distanceSqr1 = distanceSqr2; // Get search direction. Vec2 d = simplex.GetSearchDirection(); // Ensure the search direction is numerically fit. if (d.LengthSquared() < Settings.FLT_EPSILON * Settings.FLT_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 = vertices[simplex.Count]; vertex.IndexA = proxyA.GetSupport(Math.MulT(transformA.R, -d)); vertex.WA = Math.Mul(transformA, proxyA.GetVertex(vertex.IndexA)); vertex.IndexB = proxyB.GetSupport(Math.MulT(transformB.R, d)); vertex.WB = Math.Mul(transformB, proxyB.GetVertex(vertex.IndexB)); vertex.W = vertex.WB - vertex.WA; // Iteration count is equated to the number of support point calls. ++iter; ++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; } GjkMaxIters = Math.Max(GjkMaxIters, iter); // Prepare output. simplex.GetWitnessPoints(out output.PointA, out output.PointB); output.Distance = Vec2.Distance(output.PointA, output.PointB); output.Iterations = iter; // Cache the simplex. simplex.WriteCache(cache); // Apply radii if requested. if (input.UseRadii) { float rA = proxyA._radius; float rB = proxyB._radius; if (output.Distance > rA + rB && output.Distance > Settings.FLT_EPSILON) { // Shapes are still no overlapped. // Move the witness points to the outer surface. output.Distance -= rA + rB; Vec2 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. Vec2 p = 0.5f * (output.PointA + output.PointB); output.PointA = p; output.PointB = p; output.Distance = 0.0f; } } }
/// <summary> /// CCD via the secant method. /// Compute the time when two shapes begin to touch or touch at a closer distance. /// TOI considers the shape radii. It attempts to have the radii overlap by the tolerance. /// Iterations terminate with the overlap is within 0.5 * tolerance. The tolerance should be /// smaller than sum of the shape radii. /// @warning the sweeps must have the same time interval. /// fraction=0 means the shapes begin touching/overlapped, and fraction=1 means the shapes don't touch. /// </summary> /// <param name="input">The input.</param> /// <param name="shapeA">The shape A.</param> /// <param name="shapeB">The shape B.</param> /// <returns> /// fraction between [0,1] in which the shapes first touch. /// </returns> public static float TimeOfImpact(TOIInput input) { ++ToiCalls; DistanceProxy proxyA = input.ProxyA; DistanceProxy proxyB = input.ProxyB; Sweep sweepA = input.SweepA; Sweep sweepB = input.SweepB; Box2DXDebug.Assert(sweepA.T0 == sweepB.T0); Box2DXDebug.Assert(1.0f - sweepA.T0 > Settings.FLT_EPSILON); float radius = proxyA._radius + proxyB._radius; float tolerance = input.Tolerance; float alpha = 0.0f; const int k_maxIterations = 1000; // TODO_ERIN b2Settings int iter = 0; float target = 0.0f; // Prepare input for distance query. SimplexCache cache = new SimplexCache(); cache.Count = 0; DistanceInput distanceInput = new DistanceInput(); distanceInput.proxyA = input.ProxyA; distanceInput.proxyB = input.ProxyB; distanceInput.UseRadii = false; for (;;) { Transform xfA, xfB; sweepA.GetTransform(out xfA, alpha); sweepB.GetTransform(out xfB, alpha); // Get the distance between shapes. distanceInput.TransformA = xfA; distanceInput.TransformB = xfB; DistanceOutput distanceOutput; Distance(out distanceOutput, cache, distanceInput); if (distanceOutput.Distance <= 0.0f) { alpha = 1.0f; break; } SeparationFunction fcn = new SeparationFunction(); fcn.Initialize(cache, proxyA, xfA, proxyB, xfB); float separation = fcn.Evaluate(xfA, xfB); if (separation <= 0.0f) { alpha = 1.0f; break; } if (iter == 0) { // Compute a reasonable target distance to give some breathing room // for conservative advancement. We take advantage of the shape radii // to create additional clearance. if (separation > radius) { target = Math.Max(radius - tolerance, 0.75f * radius); } else { target = Math.Max(separation - tolerance, 0.02f * radius); } } if (separation - target < 0.5f * tolerance) { if (iter == 0) { alpha = 1.0f; break; } break; } #if false // Dump the curve seen by the root finder { const int N = 100; float dx = 1.0f / N; float xs[N + 1];