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); }
public static bool TestOverlap(b2Shape shape1, b2Transform transform1, b2Shape shape2, b2Transform transform2) { b2DistanceInput input = new b2DistanceInput(); input.proxyA = new b2DistanceProxy(); input.proxyA.Set(shape1); input.proxyB = new b2DistanceProxy(); input.proxyB.Set(shape2); input.transformA = transform1; input.transformB = transform2; input.useRadii = true; b2SimplexCache simplexCache = new b2SimplexCache(); simplexCache.count = 0; b2DistanceOutput output = new b2DistanceOutput(); b2Distance.Distance(output, simplexCache, input); //return output.distance < 10.0f * float.MinValue; //改 return output.distance < 1e-10f; }
public static void b2Distance(b2DistanceOutput output, b2SimplexCache cache, b2DistanceInput input) { Box2dPINVOKE.b2Distance__SWIG_1(b2DistanceOutput.getCPtr(output), b2SimplexCache.getCPtr(cache), b2DistanceInput.getCPtr(input)); }
internal static global::System.Runtime.InteropServices.HandleRef getCPtr(b2DistanceInput obj) { return((obj == null) ? new global::System.Runtime.InteropServices.HandleRef(null, global::System.IntPtr.Zero) : obj.swigCPtr); }
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); }