public void WriteCache(ref b2SimplexCache cache) { cache.metric = GetMetric(); cache.count = (ushort)m_count; b2SimplexVertex[] vertices = { m_v1, m_v2, m_v3 }; for (int i = 0; i < m_count; ++i) { cache.indexA[i] = (byte)(vertices[i].indexA); cache.indexB[i] = (byte)(vertices[i].indexB); } }
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 void ReadCache(b2SimplexCache cache, b2DistanceProxy proxyA, b2Transform transformA, b2DistanceProxy proxyB, b2Transform transformB) { Debug.Assert(cache.count <= 3); // Copy data from cache. m_count = cache.count; b2SimplexVertex[] vertices = { m_v1, m_v2, m_v3 }; for (int i = 0; i < m_count; ++i) { b2SimplexVertex v = vertices[i]; v.indexA = cache.indexA[i]; v.indexB = cache.indexB[i]; b2Vec2 wALocal = proxyA.GetVertex(v.indexA); b2Vec2 wBLocal = proxyB.GetVertex(v.indexB); v.wA = Utils.b2Mul(transformA, wALocal); v.wB = Utils.b2Mul(transformB, wBLocal); v.w = v.wB - v.wA; v.a = 0.0f; } // Compute the new simplex metric, if it is substantially different than // old metric then flush the simplex. if (m_count > 1) { float metric1 = cache.metric; float metric2 = GetMetric(); if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < float.Epsilon) { // Reset the simplex. m_count = 0; } } // If the cache is empty or invalid ... if (m_count == 0) { b2SimplexVertex v = vertices[0]; v.indexA = 0; v.indexB = 0; b2Vec2 wALocal = proxyA.GetVertex(0); b2Vec2 wBLocal = proxyB.GetVertex(0); v.wA = Utils.b2Mul(transformA, wALocal); v.wB = Utils.b2Mul(transformB, wBLocal); v.w = v.wB - v.wA; v.a = 1.0f; m_count = 1; } }
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)); }
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); }
// TODO_ERIN might not need to return the separation public float Initialize(b2SimplexCache cache, b2DistanceProxy proxyA, b2Sweep sweepA, b2DistanceProxy proxyB, b2Sweep sweepB, float t1) { m_proxyA = proxyA; m_proxyB = proxyB; int count = cache.count; Debug.Assert(0 < count && count < 3); m_sweepA = sweepA; m_sweepB = sweepB; b2Transform xfA = new b2Transform(); b2Transform xfB = new b2Transform(); m_sweepA.GetTransform(ref xfA, t1); m_sweepB.GetTransform(ref xfB, t1); if (count == 1) { m_type = Type.e_points; b2Vec2 localPointA = m_proxyA.GetVertex(cache.indexA[0]); b2Vec2 localPointB = m_proxyB.GetVertex(cache.indexB[0]); b2Vec2 pointA = Utils.b2Mul(xfA, localPointA); b2Vec2 pointB = Utils.b2Mul(xfB, localPointB); m_axis = pointB - pointA; float s = m_axis.Normalize(); return(s); } else if (cache.indexA[0] == cache.indexA[1]) { // Two points on B and one on A. m_type = Type.e_faceB; b2Vec2 localPointB1 = proxyB.GetVertex(cache.indexB[0]); b2Vec2 localPointB2 = proxyB.GetVertex(cache.indexB[1]); m_axis = Utils.b2Cross(localPointB2 - localPointB1, 1.0f); m_axis.Normalize(); b2Vec2 normal = Utils.b2Mul(xfB.q, m_axis); m_localPoint = 0.5f * (localPointB1 + localPointB2); b2Vec2 pointB = Utils.b2Mul(xfB, m_localPoint); b2Vec2 localPointA = proxyA.GetVertex(cache.indexA[0]); b2Vec2 pointA = Utils.b2Mul(xfA, localPointA); float s = Utils.b2Dot(pointA - pointB, normal); if (s < 0.0f) { m_axis = -m_axis; s = -s; } return(s); } else { // Two points on A and one or two points on B. m_type = Type.e_faceA; b2Vec2 localPointA1 = m_proxyA.GetVertex(cache.indexA[0]); b2Vec2 localPointA2 = m_proxyA.GetVertex(cache.indexA[1]); m_axis = Utils.b2Cross(localPointA2 - localPointA1, 1.0f); m_axis.Normalize(); b2Vec2 normal = Utils.b2Mul(xfA.q, m_axis); m_localPoint = 0.5f * (localPointA1 + localPointA2); b2Vec2 pointA = Utils.b2Mul(xfA, m_localPoint); b2Vec2 localPointB = m_proxyB.GetVertex(cache.indexB[0]); b2Vec2 pointB = Utils.b2Mul(xfB, localPointB); float s = Utils.b2Dot(pointB - pointA, normal); if (s < 0.0f) { m_axis = -m_axis; s = -s; } return(s); } }
internal static global::System.Runtime.InteropServices.HandleRef getCPtr(b2SimplexCache obj) { return((obj == null) ? new global::System.Runtime.InteropServices.HandleRef(null, global::System.IntPtr.Zero) : obj.swigCPtr); }