public static b2DistanceInput Create() { b2DistanceInput result = new b2DistanceInput(); result.proxyA = b2DistanceProxy.Create(); result.proxyB = b2DistanceProxy.Create(); result.transformA = b2Transform.Identity; result.transformB = b2Transform.Identity; result.useRadii = false; return (result); }
public static b2DistanceInput Create() { b2DistanceInput result = new b2DistanceInput(); result.proxyA = b2DistanceProxy.Create(); result.proxyB = b2DistanceProxy.Create(); result.transformA = b2Transform.Identity; result.transformB = b2Transform.Identity; result.useRadii = false; return(result); }
/// 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 void SolveTOI(b2TimeStep subStep, int toiIndexA, int toiIndexB) { Debug.Assert(toiIndexA < m_bodyCount); Debug.Assert(toiIndexB < m_bodyCount); // Initialize the body state. for (int i = 0; i < m_bodyCount; ++i) { b2Body b = m_bodies[i]; m_positions[i].c = b.Sweep.c; m_positions[i].a = b.Sweep.a; m_velocities[i].v = b.LinearVelocity; m_velocities[i].w = b.AngularVelocity; } b2ContactSolverDef contactSolverDef; contactSolverDef.contacts = m_contacts; contactSolverDef.count = m_contactCount; contactSolverDef.step = subStep; contactSolverDef.positions = m_positions; contactSolverDef.velocities = m_velocities; b2ContactSolver contactSolver = new b2ContactSolver(contactSolverDef); // Solve position constraints. for (int i = 0; i < subStep.positionIterations; ++i) { bool contactsOkay = contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB); if (contactsOkay) { break; } } #if false // Is the new position really safe? for (int i = 0; i < m_contactCount; ++i) { b2Contact c = m_contacts[i]; b2Fixture fA = c.GetFixtureA(); b2Fixture fB = c.GetFixtureB(); b2Body bA = fA.Body; b2Body bB = fB.Body; int indexA = c.GetChildIndexA(); int indexB = c.GetChildIndexB(); b2DistanceInput input = new b2DistanceInput(); input.proxyA.Set(fA.Shape, indexA); input.proxyB.Set(fB.Shape, indexB); input.transformA = bA.Transform; input.transformB = bB.Transform; input.useRadii = false; b2DistanceOutput output; b2SimplexCache cache = new b2SimplexCache(); cache.count = 0; output = b2Distance(cache, input); if (output.distance == 0 || cache.count == 3) { cache.count += 0; } } #endif // Leap of faith to new safe state. m_bodies[toiIndexA].Sweep.c0 = m_positions[toiIndexA].c; m_bodies[toiIndexA].Sweep.a0 = m_positions[toiIndexA].a; m_bodies[toiIndexB].Sweep.c0 = m_positions[toiIndexB].c; m_bodies[toiIndexB].Sweep.a0 = m_positions[toiIndexB].a; // No warm starting is needed for TOI events because warm // starting impulses were applied in the discrete solver. contactSolver.InitializeVelocityConstraints(); // Solve velocity constraints. for (int i = 0; i < subStep.velocityIterations; ++i) { contactSolver.SolveVelocityConstraints(); } // Don't store the TOI contact forces for warm starting // because they can be quite large. float h = subStep.dt; // Integrate positions for (int i = 0; i < m_bodyCount; ++i) { b2Vec2 c = m_positions[i].c; float a = m_positions[i].a; b2Vec2 v = m_velocities[i].v; float w = m_velocities[i].w; // Check for large velocities b2Vec2 translation = h * v; if (b2Math.b2Dot(translation, translation) > b2Settings.b2_maxTranslationSquared) { float ratio = b2Settings.b2_maxTranslation / translation.Length(); v *= ratio; } float rotation = h * w; if (rotation * rotation > b2Settings.b2_maxRotationSquared) { float ratio = b2Settings.b2_maxRotation / Math.Abs(rotation); w *= ratio; } // Integrate c += h * v; a += h * w; m_positions[i].c = c; m_positions[i].a = a; m_velocities[i].v = v; m_velocities[i].w = w; // Sync bodies b2Body body = m_bodies[i]; body.Sweep.c = c; body.Sweep.a = a; body.LinearVelocity = v; body.AngularVelocity = w; body.SynchronizeTransform(); } Report(contactSolver.m_velocityConstraints); }
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];
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.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];