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