public static void Distance(b2DistanceOutput output, b2SimplexCache cache, b2DistanceInput input) { ++b2_gjkCalls; b2DistanceProxy proxyA = input.proxyA; b2DistanceProxy proxyB = input.proxyB; b2Transform transformA = input.transformA; b2Transform transformB = input.transformB; // Initialize the simplex b2Simplex simplex = s_simplex; simplex.ReadCache(cache, proxyA, transformA, proxyB, transformB); // Get simplex vertices as an vector. b2SimplexVertex[] vertices = simplex.m_vertices; const int k_maxIters = 20; // These store the vertices of the last simplex so that we // can check for duplicates and preven cycling int[] saveA = s_saveA; int[] saveB = s_saveB; int saveCount = 0; b2Vec2 closestPoint = simplex.GetClosestPoint(); float distanceSqr1 = closestPoint.LengthSquared(); float distanceSqr2 = distanceSqr1; int i; b2Vec2 p; // Main iteration loop int iter = 0; while (iter < k_maxIters) { // Copy the simplex so that we can identify duplicates saveCount = simplex.m_count; for (i = 0; i < saveCount; i++) { saveA[i] = vertices[i].indexA; saveB[i] = vertices[i].indexB; } /*switch(simplex.m_count) * { * case 1: * break; * case 2: * simplex.Solve2(); * break; * case 3: * simplex.Solve3(); * break; * default: * b2Settings.b2Assert(false); * }*/ if (simplex.m_count == 1) { } else if (simplex.m_count == 2) { simplex.Solve2(); } else if (simplex.m_count == 3) { simplex.Solve3(); } else { b2Settings.b2Assert(false); } // If we have 3 points, then the origin is in the corresponding triangle. if (simplex.m_count == 3) { break; } // Compute the closest point. p = simplex.GetClosestPoint(); distanceSqr2 = p.LengthSquared(); // Ensure progress if (distanceSqr2 > distanceSqr1) { //break; } distanceSqr1 = distanceSqr2; // Get search direction. b2Vec2 d = simplex.GetSearchDirection(); // Ensure the search direction is numerically fit. if (d.LengthSquared() < 0.0f /*Number.MinValue * Number.MinValue*/) { // 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 very 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 b2SimplexVertex vertex = vertices[simplex.m_count]; vertex.indexA = (int)proxyA.GetSupport(b2Math.MulTMV(transformA.R, d.GetNegative())); vertex.wA = b2Math.MulX(transformA, proxyA.GetVertex(vertex.indexA)); vertex.indexB = (int)proxyB.GetSupport(b2Math.MulTMV(transformB.R, d)); vertex.wB = b2Math.MulX(transformB, proxyB.GetVertex(vertex.indexB)); vertex.w = b2Math.SubtractVV(vertex.wB, vertex.wA); // Iteration count is equated to the number of support point calls. ++iter; ++b2_gjkIters; // Check for duplicate support points. This is the main termination criteria. bool duplicate = false; for (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 exist to avoid cycling if (duplicate) { break; } // New vertex is ok and needed. ++simplex.m_count; } b2_gjkMaxIters = (int)b2Math.Max(b2_gjkMaxIters, iter); // Prepare output simplex.GetWitnessPoints(output.pointA, output.pointB); output.distance = b2Math.SubtractVV(output.pointA, output.pointB).Length(); output.iterations = iter; // Cache the simplex simplex.WriteCache(cache); // Apply radii if requested. if (input.useRadii) { float rA = proxyA.m_radius; float rB = proxyB.m_radius; if (output.distance > rA + rB && output.distance > float.MinValue) { // Shapes are still not overlapped. // Move the witness points to the outer surface. output.distance -= rA + rB; b2Vec2 normal = b2Math.SubtractVV(output.pointB, output.pointA); normal.Normalize(); output.pointA.x += rA * normal.x; output.pointA.y += rA * normal.y; output.pointB.x -= rB * normal.x; output.pointB.y -= rB * normal.y; } else { // Shapes are overlapped when radii are considered. // Move the witness points to the middle. p = new b2Vec2(); p.x = 0.5f * (output.pointA.x + output.pointB.x); p.y = 0.5f * (output.pointA.y + output.pointB.y); output.pointA.x = output.pointB.x = p.x; output.pointA.y = output.pointB.y = p.y; output.distance = 0.0f; } } }
public static void b2Distance(out b2DistanceOutput output, ref b2SimplexCache cache, ref b2DistanceInput input) { ++b2DistanceProxy.b2_gjkCalls; b2DistanceProxy proxyA = input.proxyA; b2DistanceProxy proxyB = input.proxyB; // Initialize the simplex. b2Simplex simplex = new b2Simplex(); simplex.ReadCache(ref cache, proxyA, ref input.transformA, proxyB, ref input.transformB); // Get simplex vertices as an array. b2SimplexVertex[] vertices = b2Simplex.m_vertices; int k_maxIters = 20; // These store the vertices of the last simplex so that we // can check for duplicates and prevent cycling. int[] saveA = _saveA; int[] saveB = _saveB; int saveCount = 0; b2Vec2 closestPoint; simplex.GetClosestPoint(out closestPoint); float distanceSqr1 = closestPoint.LengthSquared; float distanceSqr2 = distanceSqr1; // Console.WriteLine("Closest Point={0},{1}, distance={2}", closestPoint.x, closestPoint.y, distanceSqr1); // Main iteration loop. #region Main Iteration Loop int iter = 0; while (iter < k_maxIters) { // Copy simplex so we can identify duplicates. saveCount = simplex.m_count; for (int i = 0; i < saveCount; ++i) { saveA[i] = vertices[i].indexA; saveB[i] = vertices[i].indexB; } switch (simplex.m_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.m_count == 3) { break; } // Compute closest point. b2Vec2 p; simplex.GetClosestPoint(out p); distanceSqr2 = p.x * p.x + p.y * p.y; // Ensure progress if (distanceSqr2 >= distanceSqr1) { //break; } distanceSqr1 = distanceSqr2; // Get search direction. b2Vec2 d; simplex.GetSearchDirection(out d); // Ensure the search direction is numerically fit. if ((d.x * d.x + d.y * d.y) < b2Settings.b2_epsilon * b2Settings.b2_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. b2SimplexVertex vertex = vertices[simplex.m_count]; var q = input.transformA.q; b2Vec2 b; b.x = q.c * -d.x + q.s * -d.y; b.y = -q.s * -d.x + q.c * -d.y; vertex.indexA = proxyA.GetSupport(ref b); var vA = proxyA.m_vertices[vertex.indexA]; vertex.wA.x = (q.c * vA.x - q.s * vA.y) + input.transformA.p.x; vertex.wA.y = (q.s * vA.x + q.c * vA.y) + input.transformA.p.y; // b2Vec2 wBLocal = new b2Vec2(); q = input.transformB.q; b.x = q.c * d.x + q.s * d.y; b.y = -q.s * d.x + q.c * d.y; vertex.indexB = proxyB.GetSupport(ref b); var vB = proxyB.m_vertices[vertex.indexB]; vertex.wB.x = (input.transformB.q.c * vB.x - input.transformB.q.s * vB.y) + input.transformB.p.x; vertex.wB.y = (input.transformB.q.s * vB.x + input.transformB.q.c * vB.y) + input.transformB.p.y; vertex.w.x = vertex.wB.x - vertex.wA.x; vertex.w.y = vertex.wB.y - vertex.wA.y; // Iteration count is equated to the number of support point calls. ++iter; ++b2DistanceProxy.b2_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.m_count; } #endregion b2DistanceProxy.b2_gjkMaxIters = Math.Max(b2DistanceProxy.b2_gjkMaxIters, iter); // Prepare output. simplex.GetWitnessPoints(out output.pointA, out output.pointB); output.distance = b2Math.b2Distance(ref output.pointA, ref output.pointB); output.iterations = iter; // Cache the simplex. simplex.WriteCache(ref cache); // Apply radii if requested. if (input.useRadii) { float rA = proxyA.Radius; float rB = proxyB.Radius; if (output.distance > rA + rB && output.distance > b2Settings.b2_epsilon) { // Shapes are still not overlapped. // Move the witness points to the outer surface. output.distance -= rA + rB; b2Vec2 normal; normal.x = output.pointB.x - output.pointA.x; normal.y = output.pointB.y - output.pointA.y; normal.Normalize(); output.pointA.x += rA * normal.x; output.pointA += rA * normal; output.pointB.x -= rB * normal.x; output.pointB.y -= rB * normal.y; } else { // Shapes are overlapped when radii are considered. // Move the witness points to the middle. b2Vec2 p; p.x = 0.5f * (output.pointA.x + output.pointB.x); p.y = 0.5f * (output.pointA.y + output.pointB.y); output.pointA = p; output.pointB = p; output.distance = 0.0f; } } }
public static void b2Distance(ref b2DistanceOutput output, ref b2SimplexCache cache, ref b2DistanceInput input) { ++b2DistanceProxy.b2_gjkCalls; b2DistanceProxy proxyA = input.proxyA.Copy(); b2DistanceProxy proxyB = input.proxyB.Copy(); b2Transform transformA = input.transformA; b2Transform transformB = input.transformB; // Initialize the simplex. b2Simplex simplex = new b2Simplex(); simplex.ReadCache(ref cache, ref proxyA, ref transformA, ref proxyB, ref transformB); // Get simplex vertices as an array. b2SimplexVertex[] vertices = new b2SimplexVertex[] { simplex.m_vertices[0], simplex.m_vertices[1], simplex.m_vertices[2] }; 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]; int[] saveB = new int[3]; int saveCount = 0; b2Vec2 closestPoint = simplex.GetClosestPoint(); float distanceSqr1 = closestPoint.LengthSquared(); float distanceSqr2 = distanceSqr1; // Console.WriteLine("Closest Point={0},{1}, distance={2}", closestPoint.x, closestPoint.y, distanceSqr1); // Main iteration loop. #region Main Iteration Loop int iter = 0; while (iter < k_maxIters) { // Copy simplex so we can identify duplicates. saveCount = simplex.m_count; for (int i = 0; i < saveCount; ++i) { saveA[i] = vertices[i].indexA; saveB[i] = vertices[i].indexB; } switch (simplex.m_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.m_count == 3) { break; } // Compute closest point. b2Vec2 p = simplex.GetClosestPoint(); distanceSqr2 = p.LengthSquared(); // Ensure progress if (distanceSqr2 >= distanceSqr1) { //break; } distanceSqr1 = distanceSqr2; // Get search direction. b2Vec2 d = simplex.GetSearchDirection(); // Ensure the search direction is numerically fit. if (d.LengthSquared() < b2Settings.b2_epsilon * b2Settings.b2_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. b2SimplexVertex vertex = vertices[simplex.m_count]; vertex.indexA = proxyA.GetSupport(b2Math.b2MulT(transformA.q, -d)); vertex.wA = b2Math.b2Mul(transformA, proxyA.GetVertex(vertex.indexA)); // b2Vec2 wBLocal = new b2Vec2(); vertex.indexB = proxyB.GetSupport(b2Math.b2MulT(transformB.q, d)); vertex.wB = b2Math.b2Mul(transformB, proxyB.GetVertex(vertex.indexB)); vertex.w = vertex.wB - vertex.wA; vertices[simplex.m_count] = vertex; // Iteration count is equated to the number of support point calls. ++iter; ++b2DistanceProxy.b2_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.m_count; } #endregion b2DistanceProxy.b2_gjkMaxIters = Math.Max(b2DistanceProxy.b2_gjkMaxIters, iter); // Prepare output. simplex.GetWitnessPoints(ref output.pointA, ref output.pointB); output.distance = b2Math.b2Distance(output.pointA, output.pointB); output.iterations = iter; // Cache the simplex. simplex.WriteCache(ref cache); // Apply radii if requested. if (input.useRadii) { float rA = proxyA.Radius; float rB = proxyB.Radius; if (output.distance > rA + rB && output.distance > b2Settings.b2_epsilon) { // Shapes are still not overlapped. // Move the witness points to the outer surface. output.distance -= rA + rB; b2Vec2 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. b2Vec2 p = 0.5f * (output.pointA + output.pointB); output.pointA = p; output.pointB = p; output.distance = 0.0f; } } // Copy back the vertex changes because they are structs, but in C++ land they are reference values simplex.m_vertices[0] = vertices[0]; simplex.m_vertices[1] = vertices[1]; simplex.m_vertices[2] = vertices[2]; }