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.m_sweep.c; m_positions[i].a = b.m_sweep.a; m_velocities[i].v = b.m_linearVelocity; m_velocities[i].w = b.m_angularVelocity; } b2ContactSolverDef contactSolverDef = new b2ContactSolverDef(); 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 (int32 i = 0; i < m_contactCount; ++i) // { // b2Contact* c = m_contacts[i]; // b2Fixture* fA = c->GetFixtureA(); // b2Fixture* fB = c->GetFixtureB(); // // b2Body* bA = fA->GetBody(); // b2Body* bB = fB->GetBody(); // // int32 indexA = c->GetChildIndexA(); // int32 indexB = c->GetChildIndexB(); // // b2DistanceInput input; // input.proxyA.Set(fA->GetShape(), indexA); // input.proxyB.Set(fB->GetShape(), indexB); // input.transformA = bA->GetTransform(); // input.transformB = bB->GetTransform(); // input.useRadii = false; // // b2DistanceOutput output; // b2SimplexCache cache; // cache.count = 0; // b2Distance(&output, &cache, &input); // // if (output.distance == 0 || cache.count == 3) // { // cache.count += 0; // } // } #endif // Leap of faith to new safe state. m_bodies[toiIndexA].m_sweep.c0 = m_positions[toiIndexA].c; m_bodies[toiIndexA].m_sweep.a0 = m_positions[toiIndexA].a; m_bodies[toiIndexB].m_sweep.c0 = m_positions[toiIndexB].c; m_bodies[toiIndexB].m_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 (Utils.b2Dot(translation, translation) > (Settings.b2_maxTranslationSquared)) { float ratio = Settings.b2_maxTranslation / translation.Length(); v *= ratio; } float rotation = h * w; if (rotation * rotation > Settings.b2_maxRotationSquared) { float ratio = (0.5f * Settings.b2_pi) / Utils.b2Abs(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.m_sweep.c = c; body.m_sweep.a = a; body.m_linearVelocity = v; body.m_angularVelocity = w; body.SynchronizeTransform(); } Report(contactSolver.m_velocityConstraints); }
public b2ContactSolver(b2ContactSolverDef def) { m_step = def.step; m_count = def.count; m_positionConstraints = Arrays.InitializeWithDefaultInstances <b2ContactPositionConstraint>(m_count); m_velocityConstraints = Arrays.InitializeWithDefaultInstances <b2ContactVelocityConstraint>(m_count); m_positions = def.positions; m_velocities = def.velocities; m_contacts = def.contacts; // Initialize position independent portions of the constraints. for (int i = 0; i < m_count; ++i) { b2Contact contact = m_contacts[i]; b2Fixture fixtureA = contact.m_fixtureA; b2Fixture fixtureB = contact.m_fixtureB; b2Shape shapeA = fixtureA.GetShape(); b2Shape shapeB = fixtureB.GetShape(); float radiusA = shapeA.m_radius; float radiusB = shapeB.m_radius; b2Body bodyA = fixtureA.GetBody(); b2Body bodyB = fixtureB.GetBody(); b2Manifold manifold = contact.GetManifold(); int pointCount = manifold.pointCount; Debug.Assert(pointCount > 0); b2ContactVelocityConstraint vc = m_velocityConstraints[i]; vc.friction = contact.m_friction; vc.restitution = contact.m_restitution; vc.tangentSpeed = contact.m_tangentSpeed; vc.indexA = bodyA.m_islandIndex; vc.indexB = bodyB.m_islandIndex; vc.invMassA = bodyA.m_invMass; vc.invMassB = bodyB.m_invMass; vc.invIA = bodyA.m_invI; vc.invIB = bodyB.m_invI; vc.contactIndex = i; vc.pointCount = pointCount; vc.K.SetZero(); vc.normalMass.SetZero(); b2ContactPositionConstraint pc = m_positionConstraints[i]; pc.indexA = bodyA.m_islandIndex; pc.indexB = bodyB.m_islandIndex; pc.invMassA = bodyA.m_invMass; pc.invMassB = bodyB.m_invMass; pc.localCenterA = bodyA.m_sweep.localCenter; pc.localCenterB = bodyB.m_sweep.localCenter; pc.invIA = bodyA.m_invI; pc.invIB = bodyB.m_invI; pc.localNormal = manifold.localNormal; pc.localPoint = manifold.localPoint; pc.pointCount = pointCount; pc.radiusA = radiusA; pc.radiusB = radiusB; pc.type = manifold.type; for (int j = 0; j < pointCount; ++j) { b2ManifoldPoint cp = manifold.points[j]; b2VelocityConstraintPoint vcp = vc.points[j]; if (m_step.warmStarting) { vcp.normalImpulse = m_step.dtRatio * cp.normalImpulse; vcp.tangentImpulse = m_step.dtRatio * cp.tangentImpulse; } else { vcp.normalImpulse = 0.0f; vcp.tangentImpulse = 0.0f; } vcp.rA.SetZero(); vcp.rB.SetZero(); vcp.normalMass = 0.0f; vcp.tangentMass = 0.0f; vcp.velocityBias = 0.0f; pc.localPoints[j] = cp.localPoint; } } }
public void Solve(b2Profile profile, b2TimeStep step, b2Vec2 gravity, bool allowSleep) { b2Timer timer = new b2Timer(); float h = step.dt; // Integrate velocities and apply damping. Initialize the body state. for (int i = 0; i < m_bodyCount; ++i) { b2Body b = m_bodies[i]; b2Vec2 c = new b2Vec2(b.m_sweep.c); float a = b.m_sweep.a; b2Vec2 v = new b2Vec2(b.m_linearVelocity); float w = b.m_angularVelocity; // Store positions for continuous collision. b.m_sweep.c0 = b.m_sweep.c; b.m_sweep.a0 = b.m_sweep.a; if (b.m_type == BodyType.b2_dynamicBody) { // Integrate velocities. v += h * (b.m_gravityScale * gravity + b.m_invMass * b.m_force); w += h * b.m_invI * b.m_torque; // Apply damping. // ODE: dv/dt + c * v = 0 // Solution: v(t) = v0 * exp(-c * t) // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt) // v2 = exp(-c * dt) * v1 // Pade approximation: // v2 = v1 * 1 / (1 + c * dt) v *= 1.0f / (1.0f + h * b.m_linearDamping); w *= 1.0f / (1.0f + h * b.m_angularDamping); } m_positions[i].c = c; m_positions[i].a = a; m_velocities[i].v = v; m_velocities[i].w = w; } timer.Reset(); // Solver data b2SolverData solverData = new b2SolverData(); solverData.step = step; solverData.positions = m_positions; solverData.velocities = m_velocities; // Initialize velocity constraints. b2ContactSolverDef contactSolverDef = new b2ContactSolverDef(); contactSolverDef.step = step; contactSolverDef.contacts = m_contacts; contactSolverDef.count = m_contactCount; contactSolverDef.positions = m_positions; contactSolverDef.velocities = m_velocities; b2ContactSolver contactSolver = new b2ContactSolver(contactSolverDef); contactSolver.InitializeVelocityConstraints(); if (step.warmStarting) { contactSolver.WarmStart(); } for (int i = 0; i < m_jointCount; ++i) { m_joints[i].InitVelocityConstraints(solverData); } profile.solveInit = timer.GetMilliseconds(); // Solve velocity constraints timer.Reset(); for (int i = 0; i < step.velocityIterations; ++i) { for (int j = 0; j < m_jointCount; ++j) { m_joints[j].SolveVelocityConstraints(solverData); } contactSolver.SolveVelocityConstraints(); } // Store impulses for warm starting contactSolver.StoreImpulses(); profile.solveVelocity = timer.GetMilliseconds(); // 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 (Utils.b2Dot(translation, translation) > (Settings.b2_maxTranslation * Settings.b2_maxTranslation)) { float ratio = Settings.b2_maxTranslation / translation.Length(); v *= ratio; } float rotation = h * w; if (rotation * rotation > Settings.b2_maxRotationSquared) { float ratio = (0.5f * Settings.b2_pi) / Utils.b2Abs(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; } // Solve position constraints timer.Reset(); bool positionSolved = false; for (int i = 0; i < step.positionIterations; ++i) { bool contactsOkay = contactSolver.SolvePositionConstraints(); bool jointsOkay = true; for (int j = 0; j < m_jointCount; ++j) { bool jointOkay = m_joints[j].SolvePositionConstraints(solverData); jointsOkay = jointsOkay && jointOkay; } if (contactsOkay && jointsOkay) { // Exit early if the position errors are small. positionSolved = true; break; } } // Copy state buffers back to the bodies for (int i = 0; i < m_bodyCount; ++i) { b2Body body = m_bodies[i]; body.m_sweep.c = m_positions[i].c; body.m_sweep.a = m_positions[i].a; body.m_linearVelocity = m_velocities[i].v; body.m_angularVelocity = m_velocities[i].w; body.SynchronizeTransform(); } profile.solvePosition = timer.GetMilliseconds(); Report(contactSolver.m_velocityConstraints); if (allowSleep) { float minSleepTime = float.MaxValue; const float linTolSqr = Settings.b2_linearSleepTolerance * Settings.b2_linearSleepTolerance; float angTolSqr = Settings.b2_angularSlop * Settings.b2_angularSlop; for (int i = 0; i < m_bodyCount; ++i) { b2Body b = m_bodies[i]; if (b.GetType() == BodyType.b2_staticBody) { continue; } if ((b.m_flags & b2Body.BodyFlags.e_autoSleepFlag) == 0 || b.m_angularVelocity * b.m_angularVelocity > angTolSqr || Utils.b2Dot(b.m_linearVelocity, b.m_linearVelocity) > linTolSqr) { b.m_sleepTime = 0.0f; minSleepTime = 0.0f; } else { b.m_sleepTime += h; minSleepTime = Utils.b2Min(minSleepTime, b.m_sleepTime); } } if (minSleepTime >= Settings.b2_timeToSleep && positionSolved) { for (int i = 0; i < m_bodyCount; ++i) { b2Body b = m_bodies[i]; b.SetAwake(false); } } } }