// Find islands, integrate and solveraints, solve positionraints public void Solve(b2TimeStep step) { m_profile.solveInit = 0.0f; m_profile.solveVelocity = 0.0f; m_profile.solvePosition = 0.0f; // Size the island for the worst case. b2Island island = new b2Island(m_bodyCount, m_contactManager.ContactCount, m_jointCount, m_contactManager.ContactListener); // Clear all the island flags. for (b2Body b = m_bodyList; b != null; b = b.Next) { b.BodyFlags &= ~b2BodyFlags.e_islandFlag; } for (b2Contact c = m_contactManager.ContactList; c != null; c = c.Next) { c.ContactFlags &= ~b2ContactFlags.e_islandFlag; } for (b2Joint j = m_jointList; j; j = j.Next) { j.m_islandFlag = false; } // Build and simulate all awake islands. int stackSize = m_bodyCount; b2Body[] stack = new b2Body[stackSize]; for (b2Body seed = m_bodyList; seed != null; seed = seed.Next) { if (seed.BodyFlags & b2BodyFlags.e_islandFlag) { continue; } if (seed.IsAwake() == false || seed.IsActive() == false) { continue; } // The seed can be dynamic or kinematic. if (seed.BodyType == b2BodyType.b2_staticBody) { continue; } // Reset island and stack. island.Clear(); int stackCount = 0; stack[stackCount++] = seed; seed.BodyFlags |= b2BodyFlags.e_islandFlag; // Perform a depth first search (DFS) on theraint graph. while (stackCount > 0) { // Grab the next body off the stack and add it to the island. b2Body b = stack[--stackCount]; island.Add(b); // Make sure the body is awake. b.SetAwake(true); // To keep islands as small as possible, we don't // propagate islands across static bodies. if (b.BodyType == b2BodyType.b2_staticBody) { continue; } // Search all contacts connected to this body. for (b2ContactEdge ce = b.ContactList; ce != null; ce = ce.next) { b2Contact contact = ce.contact; // Has this contact already been added to an island? if (contact.ContactFlags & b2ContactFlags.e_islandFlag) { continue; } // Is this contact solid and touching? if (contact.IsEnabled() == false || contact.IsTouching() == false) { continue; } // Skip sensors. bool sensorA = contact.m_fixtureA.m_isSensor; bool sensorB = contact.m_fixtureB.m_isSensor; if (sensorA || sensorB) { continue; } island.Add(contact); contact.ContactFlags |= b2ContactType.e_islandFlag; b2Body other = ce.other; // Was the other body already added to this island? if ((other.BodyFlags & b2BodyFlags.e_islandFlag) > 0) { continue; } stack[stackCount++] = other; other.BodyFlags |= b2BodyFlags.e_islandFlag; } // Search all joints connect to this body. for (b2JointEdge je = b.JointList; je; je = je.next) { if (je.joint.IslandFlag == true) { continue; } b2Body other = je.other; // Don't simulate joints connected to inactive bodies. if (other.IsActive() == false) { continue; } island.Add(je.joint); je.joint.m_islandFlag = true; if ((other.BodyFlags & b2BodyFlags.e_islandFlag) > 0) { continue; } stack[stackCount++] = other; other.BodyFlags |= b2BodyFlags.e_islandFlag; } } b2Profile profile = island.Solve(step, m_gravity, m_allowSleep); m_profile.solveInit += profile.solveInit; m_profile.solveVelocity += profile.solveVelocity; m_profile.solvePosition += profile.solvePosition; // Post solve cleanup. for (int i = 0; i < island.m_bodyCount; ++i) { // Allow static bodies to participate in other islands. b2Body b = island.m_bodies[i]; if (b.BodyType == b2BodyType.b2_staticBody) { b.BodyFlags &= ~b2BodyFlags.e_islandFlag; } } } { b2Timer timer; // Synchronize fixtures, check for out of range bodies. for (b2Body b = m_bodyList; b != null; b = b.Next) { // If a body was not in an island then it did not move. if ((b.BodyFlags & b2BodyType.e_islandFlag) == 0) { continue; } if (b.GetBodyType() == b2BodyType.b2_staticBody) { continue; } // Update fixtures (for broad-phase). b.SynchronizeFixtures(); } // Look for new contacts. m_contactManager.FindNewContacts(); m_profile.broadphase = timer.GetMilliseconds(); } }
// Find TOI contacts and solve them. public void SolveTOI(b2TimeStep step) { b2Island island = new b2Island(2 * b2Settings.b2_maxTOIContacts, b2Settings.b2_maxTOIContacts, 0, m_contactManager.ContactListener); if (m_stepComplete) { for (b2Body b = m_bodyList; b; b = b.Next) { b.BodyFlags &= ~b2Body.e_islandFlag; b.m_sweep.alpha0 = 0.0f; } for (b2Contact c = m_contactManager.ContactList; c; c = c.Next) { // Invalidate TOI c.ContactFlags &= ~(b2ContactType.e_toiFlag | b2ContactType.e_islandFlag); c.m_toiCount = 0; c.m_toi = 1.0f; } } // Find TOI events and solve them. for (; ;) { // Find the first TOI. b2Contact minContact = null; float minAlpha = 1.0f; for (b2Contact c = m_contactManager.ContactList; c != null; c = c.Next) { // Is this contact disabled? if (c.IsEnabled() == false) { continue; } // Prevent excessive sub-stepping. if (c.m_toiCount > b2Settings.b2_maxSubSteps) { continue; } float alpha = 1.0f; if (c.ContactFlags.HasFlag(b2ContactFlags.e_toiFlag)) { // This contact has a valid cached TOI. alpha = c.m_toi; } else { b2Fixture fA = c.GetFixtureA(); b2Fixture fB = c.GetFixtureB(); // Is there a sensor? if (fA.IsSensor || fB.IsSensor) { continue; } b2Body bA = fA.Body; b2Body bB = fB.Body; b2BodyType typeA = bA.BodyType; b2BodyType typeB = bB.BodyType; bool activeA = bA.IsAwake() && typeA != b2BodyType.b2_staticBody; bool activeB = bB.IsAwake() && typeB != b2BodyType.b2_staticBody; // Is at least one body active (awake and dynamic or kinematic)? if (activeA == false && activeB == false) { continue; } bool collideA = bA.IsBullet() || typeA != b2BodyType.b2_dynamicBody; bool collideB = bB.IsBullet() || typeB != b2BodyType.b2_dynamicBody; // Are these two non-bullet dynamic bodies? if (collideA == false && collideB == false) { continue; } // Compute the TOI for this contact. // Put the sweeps onto the same time interval. float alpha0 = bA.Sweep.alpha0; if (bA.Sweep.alpha0 < bB.Sweep.alpha0) { alpha0 = bB.Sweep.alpha0; bA.Sweep.Advance(alpha0); } else if (bB.Sweep.alpha0 < bA.Sweep.alpha0) { alpha0 = bA.Sweep.alpha0; bB.Sweep.Advance(alpha0); } int indexA = c.GetChildIndexA(); int indexB = c.GetChildIndexB(); // Compute the time of impact in interval [0, minTOI] b2TOIInput input = new b2TOIInput(); input.proxyA.Set(fA.Shape, indexA); input.proxyB.Set(fB.Shape, indexB); input.sweepA = bA.Sweep; input.sweepB = bB.Sweep; input.tMax = 1.0f; b2TOIOutput output = b2TimeOfImpact(input); // Beta is the fraction of the remaining portion of the . float beta = output.t; if (output.state == b2TOIOutputType.e_touching) { alpha = b2Math.b2Min(alpha0 + (1.0f - alpha0) * beta, 1.0f); } else { alpha = 1.0f; } c.m_toi = alpha; c.ContactFlags |= b2ContactFlags.e_toiFlag; } if (alpha < minAlpha) { // This is the minimum TOI found so far. minContact = c; minAlpha = alpha; } } if (minContact == null || 1.0f - 10.0f * b2Settings.b2_epsilon < minAlpha) { // No more TOI events. Done! m_stepComplete = true; break; } { // Advance the bodies to the TOI. b2Fixture fA = minContact.GetFixtureA(); b2Fixture fB = minContact.GetFixtureB(); b2Body bA = fA.Body; b2Body bB = fB.Body; b2Sweep backup1 = bA.Sweep; b2Sweep backup2 = bB.Sweep; bA.Advance(minAlpha); bB.Advance(minAlpha); // The TOI contact likely has some new contact points. minContact.Update(m_contactManager.ContactListener); minContact.ContactFlags &= ~b2ContactFlags.e_toiFlag; ++minContact.m_toiCount; // Is the contact solid? if (minContact.IsEnabled() == false || minContact.IsTouching() == false) { // Restore the sweeps. minContact.SetEnabled(false); bA.Sweep = backup1; bB.Sweep = backup2; bA.SynchronizeTransform(); bB.SynchronizeTransform(); continue; } bA.SetAwake(true); bB.SetAwake(true); // Build the island island.Clear(); island.Add(bA); island.Add(bB); island.Add(minContact); bA.BodyFlags |= b2BodyFlags.e_islandFlag; bB.BodyFlags |= b2BodyFlags.e_islandFlag; minContact.ContentType |= b2ContactFlags.e_islandFlag; // Get contacts on bodyA and bodyB. b2Body[] bodies = new b2Body[] { bA, bB }; for (int i = 0; i < 2; ++i) { b2Body body = bodies[i]; if (body.BodyType == b2BodyType.b2_dynamicBody) { for (b2ContactEdge ce = body.ContactList; ce != null; ce = ce.next) { if (island.BodyCount == island.BodyCapacity) { break; } if (island.ContactCount == island.ContactCapacity) { break; } b2Contact contact = ce.contact; // Has this contact already been added to the island? if (contact.ContactType & b2ContactType.e_islandFlag) { continue; } // Only add static, kinematic, or bullet bodies. b2Body other = ce.other; if (other.BodyType == b2BodyType.b2_dynamicBody && body.IsBullet() == false && other.IsBullet() == false) { continue; } // Skip sensors. bool sensorA = contact.m_fixtureA.m_isSensor; bool sensorB = contact.m_fixtureB.m_isSensor; if (sensorA || sensorB) { continue; } // Tentatively advance the body to the TOI. b2Sweep backup = other.Sweep; if (other.BodyFlags.HasFlag(b2BodyFlags.e_islandFlag)) { other.Advance(minAlpha); } // Update the contact points contact.Update(m_contactManager.ContactListener); // Was the contact disabled by the user? if (contact.IsEnabled() == false) { other.Sweep = backup; other.SynchronizeTransform(); continue; } // Are there contact points? if (contact.IsTouching() == false) { other.Sweep = backup; other.SynchronizeTransform(); continue; } // Add the contact to the island contact.ContactFlags |= b2ContactFlags.e_islandFlag; island.Add(contact); // Has the other body already been added to the island? if (other.BodyFlags.HasFlag(b2BodyFlags.e_islandFlag)) { continue; } // Add the other body to the island. other.BodyFlags |= b2BodyFlags.e_islandFlag; if (other.BodyType != b2BodyType.b2_staticBody) { other.SetAwake(true); } island.Add(other); } } } b2TimeStep subStep; subStep.dt = (1.0f - minAlpha) * step.dt; subStep.inv_dt = 1.0f / subStep.dt; subStep.dtRatio = 1.0f; subStep.positionIterations = 20; subStep.velocityIterations = step.velocityIterations; subStep.warmStarting = false; island.SolveTOI(subStep, bA.m_islandIndex, bB.m_islandIndex); // Reset island flags and synchronize broad-phase proxies. for (int i = 0; i < island.m_bodyCount; ++i) { b2Body body = island.m_bodies[i]; body.BodyFlags &= ~b2BodyFlags.e_islandFlag; if (body.BodyType != b2BodyType.b2_dynamicBody) { continue; } body.SynchronizeFixtures(); // Invalidate all contact TOIs on this displaced body. for (b2ContactEdge ce = body.ContactList; ce != null; ce = ce.next) { ce.Contact.ContactFlags &= ~(b2ContactFlags.e_toiFlag | b2ContactFlags.e_islandFlag); } } // Commit fixture proxy movements to the broad-phase so that new contacts are created. // Also, some contacts can be destroyed. m_contactManager.FindNewContacts(); if (m_subStepping) { m_stepComplete = false; break; } } } }
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); }
private void SolveTOI(b2TimeStep step);
public void SolveTOI(b2TimeStep subStep) { int i; int j; m_contactSolver.Initialize(subStep, m_contacts, m_contactCount, m_allocator); b2ContactSolver contactSolver = m_contactSolver; // No warm starting is needed for TOI events because warm // starting impulses were applied in the discrete solver. // Warm starting for joints is off for now, but we need to // call this function to compute Jacobians. for (i = 0; i < m_jointCount; ++i) { m_joints[i].InitVelocityConstraints(subStep); } // Solve velocity constraints. for (i = 0; i < subStep.velocityIterations; ++i) { contactSolver.SolveVelocityConstraints(); for (j = 0; j < m_jointCount; ++j) { m_joints[j].SolveVelocityConstraints(subStep); } } // Don't store the TOI contact forces for warm starting // because they can be quite large. // Integrate positions. for (i = 0; i < m_bodyCount; ++i) { b2Body b = m_bodies[i]; if (b.GetType() == b2Body.b2_staticBody) { continue; } // Check for large velocities. // b2Vec2 translation = subStep.dt * b.m_linearVelocity; float translationX = subStep.dt * b.m_linearVelocity.x; float translationY = subStep.dt * b.m_linearVelocity.y; //if (b2Dot(translation, translation) > b2_maxTranslationSquared) if ((translationX * translationX + translationY * translationY) > b2Settings.b2_maxTranslationSquared) { b.m_linearVelocity.Normalize(); b.m_linearVelocity.x *= b2Settings.b2_maxTranslation * subStep.inv_dt; b.m_linearVelocity.y *= b2Settings.b2_maxTranslation * subStep.inv_dt; } float rotation = subStep.dt * b.m_angularVelocity; if (rotation * rotation > b2Settings.b2_maxRotationSquared) { if (b.m_angularVelocity < 0.0f) { b.m_angularVelocity = -b2Settings.b2_maxRotation * subStep.inv_dt; } else { b.m_angularVelocity = b2Settings.b2_maxRotation * subStep.inv_dt; } } // Store positions for continuous collision. b.m_sweep.c0.SetV(b.m_sweep.c); b.m_sweep.a0 = b.m_sweep.a; // Integrate b.m_sweep.c.x += subStep.dt * b.m_linearVelocity.x; b.m_sweep.c.y += subStep.dt * b.m_linearVelocity.y; b.m_sweep.a += subStep.dt * b.m_angularVelocity; // Compute new transform b.SynchronizeTransform(); // Note: shapes are synchronized later. } // Solve position constraints. float k_toiBaumgarte = 0.75f; for (i = 0; i < subStep.positionIterations; ++i) { bool contactsOkay = contactSolver.SolvePositionConstraints(k_toiBaumgarte); bool jointsOkay = true; for (j = 0; j < m_jointCount; ++j) { bool jointOkay = m_joints[j].SolvePositionConstraints(b2Settings.b2_contactBaumgarte); jointsOkay = jointsOkay && jointOkay; } if (contactsOkay && jointsOkay) { break; } } Report(contactSolver.m_constraints); }
public void Solve(ref 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 = b.Sweep.c; float a = b.Sweep.a; b2Vec2 v = b.LinearVelocity; float w = b.AngularVelocity; // Store positions for continuous collision. b.Sweep.c0 = b.Sweep.c; b.Sweep.a0 = b.Sweep.a; if (b.BodyType == b2BodyType.b2_dynamicBody) { // Integrate velocities. v += h * (b.GravityScale * gravity + b.InvertedMass * b.Force); w += h * b.InvertedI * b.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 // Taylor expansion: // v2 = (1.0f - c * dt) * v1 v *= b2Math.b2Clamp(1.0f - h * b.LinearDamping, 0.0f, 1.0f); w *= b2Math.b2Clamp(1.0f - h * b.AngularDamping, 0.0f, 1.0f); } 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; 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 (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; } // 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 i2 = 0; i2 < m_jointCount; ++i2) { bool jointOkay = m_joints[i2].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.Sweep.c = m_positions[i].c; body.Sweep.a = m_positions[i].a; body.LinearVelocity = m_velocities[i].v; body.AngularVelocity = m_velocities[i].w; body.SynchronizeTransform(); } profile.solvePosition = timer.GetMilliseconds(); Report(contactSolver.m_velocityConstraints); if (allowSleep) { float minSleepTime = b2Settings.b2_maxFloat; float linTolSqr = b2Settings.b2_linearSleepTolerance * b2Settings.b2_linearSleepTolerance; float angTolSqr = b2Settings.b2_angularSleepTolerance * b2Settings.b2_angularSleepTolerance; for (int i = 0; i < m_bodyCount; ++i) { b2Body b = m_bodies[i]; if (b.BodyType == b2BodyType.b2_staticBody) { continue; } if (!(b.BodyFlags.HasFlag(b2BodyFlags.e_autoSleepFlag)) || b.AngularVelocity * b.AngularVelocity > angTolSqr || b2Math.b2Dot(b.LinearVelocity, b.LinearVelocity) > linTolSqr) { b.SleepTime = 0.0f; minSleepTime = 0.0f; } else { b.SleepTime += h; minSleepTime = Math.Min(minSleepTime, b.SleepTime); } } if (minSleepTime >= b2Settings.b2_timeToSleep && positionSolved) { for (int i = 0; i < m_bodyCount; ++i) { b2Body b = m_bodies[i]; b.SetAwake(false); } } } }
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 void Solve(b2TimeStep step, b2Vec2 gravity, bool allowSleep) { int i; int j; b2Body b; b2Joint joint; // Integrate velocities and apply damping. for (i = 0; i < m_bodyCount; ++i) { b = m_bodies[i]; if (b.GetType() != b2Body.b2_dynamicBody) { continue; } //=====================add by kingBook 2015/10/26 17:25============== b2Vec2 l_gravity; if (b.m_customGravity != null) { l_gravity = b.m_customGravity; } else { l_gravity = gravity; } //==============================added================================ // Integrate velocities. //b.m_linearVelocity += step.dt * (gravity + b.m_invMass * b.m_force); b.m_linearVelocity.x += step.dt * (l_gravity.x + b.m_invMass * b.m_force.x); b.m_linearVelocity.y += step.dt * (l_gravity.y + b.m_invMass * b.m_force.y); b.m_angularVelocity += step.dt * 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 // Taylor expansion: // v2 = (1.0f - c * dt) * v1 b.m_linearVelocity.Multiply(b2Math.Clamp(1.0f - step.dt * b.m_linearDamping, 0.0f, 1.0f)); b.m_angularVelocity *= b2Math.Clamp(1.0f - step.dt * b.m_angularDamping, 0.0f, 1.0f); } m_contactSolver.Initialize(step, m_contacts, m_contactCount, m_allocator); b2ContactSolver contactSolver = m_contactSolver; // Initialize velocity constraints. contactSolver.InitVelocityConstraints(step); for (i = 0; i < m_jointCount; ++i) { joint = m_joints[i]; joint.InitVelocityConstraints(step); } // Solve velocity constraints. for (i = 0; i < step.velocityIterations; ++i) { for (j = 0; j < m_jointCount; ++j) { joint = m_joints[j]; joint.SolveVelocityConstraints(step); } contactSolver.SolveVelocityConstraints(); } // Post-solve (store impulses for warm starting). for (i = 0; i < m_jointCount; ++i) { joint = m_joints[i]; joint.FinalizeVelocityConstraints(); } contactSolver.FinalizeVelocityConstraints(); // Integrate positions. for (i = 0; i < m_bodyCount; ++i) { b = m_bodies[i]; if (b.GetType() == b2Body.b2_staticBody) { continue; } // Check for large velocities. // b2Vec2 translation = step.dt * b.m_linearVelocity; float translationX = step.dt * b.m_linearVelocity.x; float translationY = step.dt * b.m_linearVelocity.y; //if (b2Dot(translation, translation) > b2_maxTranslationSquared) if ((translationX * translationX + translationY * translationY) > b2Settings.b2_maxTranslationSquared) { b.m_linearVelocity.Normalize(); b.m_linearVelocity.x *= b2Settings.b2_maxTranslation * step.inv_dt; b.m_linearVelocity.y *= b2Settings.b2_maxTranslation * step.inv_dt; } float rotation = step.dt * b.m_angularVelocity; if (rotation * rotation > b2Settings.b2_maxRotationSquared) { if (b.m_angularVelocity < 0.0f) { b.m_angularVelocity = -b2Settings.b2_maxRotation * step.inv_dt; } else { b.m_angularVelocity = b2Settings.b2_maxRotation * step.inv_dt; } } // Store positions for continuous collision. b.m_sweep.c0.SetV(b.m_sweep.c); b.m_sweep.a0 = b.m_sweep.a; // Integrate //b.m_sweep.c += step.dt * b.m_linearVelocity; //--------------修改start kingBook--------------- if (b.m_allowMovement) { b.m_sweep.c.x += step.dt * b.m_linearVelocity.x; b.m_sweep.c.y += step.dt * b.m_linearVelocity.y; } //--------------修改 end------------------------- b.m_sweep.a += step.dt * b.m_angularVelocity; // Compute new transform b.SynchronizeTransform(); // Note: shapes are synchronized later. } // Iterate over constraints. for (i = 0; i < step.positionIterations; ++i) { bool contactsOkay = contactSolver.SolvePositionConstraints(b2Settings.b2_contactBaumgarte); bool jointsOkay = true; for (j = 0; j < m_jointCount; ++j) { joint = m_joints[j]; bool jointOkay = joint.SolvePositionConstraints(b2Settings.b2_contactBaumgarte); jointsOkay = jointsOkay && jointOkay; } if (contactsOkay && jointsOkay) { break; } } Report(contactSolver.m_constraints); if (allowSleep) { float minSleepTime = float.MaxValue; float linTolSqr = b2Settings.b2_linearSleepTolerance * b2Settings.b2_linearSleepTolerance; float angTolSqr = b2Settings.b2_angularSleepTolerance * b2Settings.b2_angularSleepTolerance; for (i = 0; i < m_bodyCount; ++i) { b = m_bodies[i]; if (b.GetType() == b2Body.b2_staticBody) { continue; } if ((b.m_flags & b2Body.e_allowSleepFlag) == 0) { b.m_sleepTime = 0.0f; minSleepTime = 0.0f; } if ((b.m_flags & b2Body.e_allowSleepFlag) == 0 || b.m_angularVelocity * b.m_angularVelocity > angTolSqr || b2Math.Dot(b.m_linearVelocity, b.m_linearVelocity) > linTolSqr) { b.m_sleepTime = 0.0f; minSleepTime = 0.0f; } else { b.m_sleepTime += step.dt; minSleepTime = b2Math.Min(minSleepTime, b.m_sleepTime); } } if (minSleepTime >= b2Settings.b2_timeToSleep) { for (i = 0; i < m_bodyCount; ++i) { b = m_bodies[i]; b.SetAwake(false); } } } }
public void Solve(b2TimeStep step, b2Vec2 gravity, bool allowSleep) #endif { #if PROFILING b2Timer timer = new b2Timer(); #endif 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 = b.Sweep.c; float a = b.Sweep.a; b2Vec2 v = b.LinearVelocity; float w = b.AngularVelocity; // Store positions for continuous collision. b.Sweep.c0 = b.Sweep.c; b.Sweep.a0 = b.Sweep.a; if (b.BodyType == b2BodyType.b2_dynamicBody) { // Integrate velocities. v += h * (b.GravityScale * gravity + b.InvertedMass * b.Force); w += h * b.InvertedI * b.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 // Taylor expansion: // v2 = (1.0f - c * dt) * v1 v *= b2Math.b2Clamp(1.0f - h * b.LinearDamping, 0.0f, 1.0f); w *= b2Math.b2Clamp(1.0f - h * b.AngularDamping, 0.0f, 1.0f); } m_positions[i].c = c; m_positions[i].a = a; m_velocities[i].v = v; m_velocities[i].w = w; } #if PROFILING timer.Reset(); #endif // Solver data b2SolverData solverData = new b2SolverData(); solverData.step = step; solverData.positions = m_positions; solverData.velocities = m_velocities; // Initialize velocity constraints. b2ContactSolverDef contactSolverDef; 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); } #if PROFILING profile.solveInit = timer.GetMilliseconds(); #endif // Solve velocity constraints #if PROFILING timer.Reset(); #endif 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(); #if PROFILING profile.solveVelocity = timer.GetMilliseconds(); #endif // 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 (translation.LengthSquared /* 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; } // Solve position constraints #if PROFILING timer.Reset(); #endif bool positionSolved = false; for (int i = 0; i < step.positionIterations; ++i) { bool contactsOkay = contactSolver.SolvePositionConstraints(); bool jointsOkay = true; for (int i2 = 0; i2 < m_jointCount; ++i2) { bool jointOkay = m_joints[i2].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.Sweep.c = m_positions[i].c; body.Sweep.a = m_positions[i].a; body.LinearVelocity = m_velocities[i].v; body.AngularVelocity = m_velocities[i].w; body.SynchronizeTransform(); } #if PROFILING profile.solvePosition = timer.GetMilliseconds(); #endif Report(contactSolver.m_velocityConstraints); if (allowSleep) { float minSleepTime = b2Settings.b2_maxFloat; float linTolSqr = b2Settings.b2_linearSleepTolerance * b2Settings.b2_linearSleepTolerance; float angTolSqr = b2Settings.b2_angularSleepTolerance * b2Settings.b2_angularSleepTolerance; for (int i = 0; i < m_bodyCount; ++i) { b2Body b = m_bodies[i]; if (b.BodyType == b2BodyType.b2_staticBody) { continue; } if (!(b.BodyFlags.HasFlag(b2BodyFlags.e_autoSleepFlag)) || b.AngularVelocity * b.AngularVelocity > angTolSqr || b2Math.b2Dot(b.LinearVelocity, b.LinearVelocity) > linTolSqr) { b.SleepTime = 0.0f; minSleepTime = 0.0f; } else { b.SleepTime += h; minSleepTime = Math.Min(minSleepTime, b.SleepTime); } } if (minSleepTime >= b2Settings.b2_timeToSleep && positionSolved) { for (int i = 0; i < m_bodyCount; ++i) { b2Body b = m_bodies[i]; b.SetAwake(false); } } } }
public void Solve(ref b2Profile profile, b2TimeStep step, b2Vec2 gravity, bool allowSleep)