protected override void Draw(Settings settings) { base.Draw(settings); b2DistanceInput input = b2DistanceInput.Create(); input.proxyA.Set(m_polygonA, 0); input.proxyB.Set(m_polygonB, 0); input.transformA = m_transformA; input.transformB = m_transformB; input.useRadii = true; b2SimplexCache cache = b2SimplexCache.Create(); cache.count = 0; b2DistanceOutput output = new b2DistanceOutput(); b2Simplex.b2Distance(ref output, ref cache, ref input); m_debugDraw.DrawString(5, m_textLine, "distance = {0}", output.distance); m_textLine += 15; m_debugDraw.DrawString(5, m_textLine, "iterations = {0}", output.iterations); m_textLine += 15; { b2Color color = new b2Color(0.9f, 0.9f, 0.9f); b2Vec2[] v = new b2Vec2[b2Settings.b2_maxPolygonVertices]; for (int i = 0; i < m_polygonA.VertexCount; ++i) { v[i] = b2Math.b2Mul(m_transformA, m_polygonA.Vertices[i]); } m_debugDraw.DrawPolygon(v, m_polygonA.VertexCount, color); for (int i = 0; i < m_polygonB.VertexCount; ++i) { v[i] = b2Math.b2Mul(m_transformB, m_polygonB.Vertices[i]); } m_debugDraw.DrawPolygon(v, m_polygonB.VertexCount, color); } b2Vec2 x1 = output.pointA; b2Vec2 x2 = output.pointB; b2Color c1 = new b2Color(1.0f, 0.0f, 0.0f); m_debugDraw.DrawPoint(x1, 4.0f, c1); b2Color c2 = new b2Color(1.0f, 1.0f, 0.0f); m_debugDraw.DrawPoint(x2, 4.0f, c2); }
/// Determine if two generic shapes overlap. public static bool b2TestOverlap(b2Shape shapeA, int indexA, b2Shape shapeB, int indexB, ref b2Transform xfA, ref b2Transform xfB) { b2DistanceInput input = b2DistanceInput.Create(); input.proxyA = b2DistanceProxy.Create(shapeA, indexA); input.proxyB = b2DistanceProxy.Create(shapeB, indexB); input.transformA = xfA; input.transformB = xfB; input.useRadii = true; b2SimplexCache cache = b2SimplexCache.Create(); b2DistanceOutput output = new b2DistanceOutput(); b2Simplex.b2Distance(ref output, ref cache, ref input); // Console.WriteLine("{2} vs {3}: distance={0} after {1} iters", output.distance, output.iterations, shapeA.ShapeType, shapeB.ShapeType); return output.distance < 10.0f * b2Settings.b2_epsilon; }
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]; }
// CCD via the local separating axis method. This seeks progression // by computing the largest time at which separation is maintained. public static b2TOIOutput Compute(b2TOIInput input) { b2TOIOutput output = new b2TOIOutput(); ++b2_toiCalls; output.state = b2ImpactState.e_unknown; output.t = input.tMax; b2DistanceProxy proxyA = input.proxyA; b2DistanceProxy proxyB = input.proxyB; b2Sweep sweepA = input.sweepA; b2Sweep 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.Radius + proxyB.Radius; float target = Math.Max(b2Settings.b2_linearSlop, totalRadius - 3.0f * b2Settings.b2_linearSlop); float tolerance = 0.25f * b2Settings.b2_linearSlop; Debug.Assert(target > tolerance); float t1 = 0.0f; int k_maxIterations = 20; // TODO_ERIN b2Settings int iter = 0; // Prepare input for distance query. b2SimplexCache cache = b2SimplexCache.Create(); b2DistanceInput distanceInput = new b2DistanceInput(); 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). while (true) { b2Transform 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; b2DistanceOutput distanceOutput = new b2DistanceOutput(); b2Simplex.b2Distance(ref distanceOutput, ref cache, ref distanceInput); // If the shapes are overlapped, we give up on continuous collision. if (distanceOutput.distance <= 0.0f) { // Failure! output.state = b2ImpactState.e_overlapped; output.t = 0.0f; break; } if (distanceOutput.distance < target + tolerance) { // Victory! output.state = b2ImpactState.e_touching; output.t = t1; break; } // Initialize the separating axis. b2SeparationFunction fcn = new b2SeparationFunction(); fcn.Initialize(ref cache, proxyA, ref sweepA, proxyB, ref sweepB, t1); #if false // Dump the curve seen by the root finder { int N = 100; float dx = 1.0f / N; float xs[N+1];
// CCD via the local separating axis method. This seeks progression // by computing the largest time at which separation is maintained. public static b2TOIOutput Compute(b2TOIInput input) { b2TOIOutput output = new b2TOIOutput(); ++b2_toiCalls; output.state = b2ImpactState.e_unknown; output.t = input.tMax; b2DistanceProxy proxyA = input.proxyA.Copy(); b2DistanceProxy proxyB = input.proxyB.Copy(); b2Sweep sweepA = input.sweepA; b2Sweep 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.Radius + proxyB.Radius; float target = Math.Max(b2Settings.b2_linearSlop, totalRadius - 3.0f * b2Settings.b2_linearSlop); float tolerance = 0.25f * b2Settings.b2_linearSlop; Debug.Assert(target > tolerance); float t1 = 0.0f; int k_maxIterations = 20; // TODO_ERIN b2Settings int iter = 0; // Prepare input for distance query. b2SimplexCache cache = b2SimplexCache.Create(); b2DistanceInput distanceInput = new b2DistanceInput(); 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). while (true) { b2Transform xfA = b2Transform.Create(), xfB = b2Transform.Create(); sweepA.GetTransform(ref xfA, t1); sweepB.GetTransform(ref xfB, t1); // Get the distance between shapes. We can also use the results // to get a separating axis. distanceInput.transformA = xfA; distanceInput.transformB = xfB; b2DistanceOutput distanceOutput = new b2DistanceOutput(); b2Simplex.b2Distance(ref distanceOutput, ref cache, ref distanceInput); // If the shapes are overlapped, we give up on continuous collision. if (distanceOutput.distance <= 0.0f) { // Failure! output.state = b2ImpactState.e_overlapped; output.t = 0.0f; break; } if (distanceOutput.distance < target + tolerance) { // Victory! output.state = b2ImpactState.e_touching; output.t = t1; break; } // Initialize the separating axis. b2SeparationFunction fcn = new b2SeparationFunction(); fcn.Initialize(ref cache, ref proxyA, ref sweepA, ref proxyB, ref sweepB, t1); #if false // Dump the curve seen by the root finder { int N = 100; float dx = 1.0f / N; float xs[N + 1];