void Island::SolveTOI(const PTimeStep& subStep, s32 toiIndexA, s32 toiIndexB) { assert(toiIndexA < m_bodyCount); assert(toiIndexB < m_bodyCount); // Initialize the body state. for (s32 i = 0; i < m_bodyCount; ++i) { Body* 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; } ContactSolverDef contactSolverDef; contactSolverDef.contacts = m_contacts; contactSolverDef.count = m_contactCount; contactSolverDef.allocator = m_allocator; contactSolverDef.step = subStep; contactSolverDef.positions = m_positions; contactSolverDef.velocities = m_velocities; ContactSolver contactSolver(&contactSolverDef); // Solve position constraints. for (s32 i = 0; i < subStep.positionIterations; ++i) { bool contactsOkay = contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB); if (contactsOkay) { break; } } #if 0 // Is the new position really safe? for (s32 i = 0; i < m_contactCount; ++i) { Contact* c = m_contacts[i]; Fixture* fA = c->GetFixtureA(); Fixture* fB = c->GetFixtureB(); Body* bA = fA->GetBody(); Body* bB = fB->GetBody(); s32 indexA = c->GetChildIndexA(); s32 indexB = c->GetChildIndexB(); DistanceInput input; input.proxyA.Set(fA->GetShape(), indexA); input.proxyB.Set(fB->GetShape(), indexB); input.Transform2DA = bA->GetTransform2D(); input.Transform2DB = bB->GetTransform2D(); input.useRadii = false; DistanceOutput output; SimplexCache cache; cache.count = 0; Distance(&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 (s32 i = 0; i < subStep.velocityIterations; ++i) { contactSolver.SolveVelocityConstraints(); } // Don't store the TOI contact forces for warm starting // because they can be quite large. real32 h = subStep.delta; // Integrate positions for (s32 i = 0; i < m_bodyCount; ++i) { glm::vec2 c = m_positions[i].c; real32 a = m_positions[i].a; glm::vec2 v = m_velocities[i].v; real32 w = m_velocities[i].w; // Check for large velocities glm::vec2 translation = h * v; if (glm::dot(translation, translation) > maxTranslationSquared) { real32 ratio = maxTranslation / translation.length(); v *= ratio; } real32 rotation = h * w; if (rotation * rotation > maxRotationSquared) { real32 ratio = maxRotation / glm::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 Body* body = m_bodies[i]; body->m_sweep.c = c; body->m_sweep.a = a; body->m_linearVelocity = v; body->m_angularVelocity = w; body->SynchronizeTransform2D(); } Report(contactSolver.m_velocityConstraints); }
void Island::Solve(Profile* profile, const PTimeStep& step, const glm::vec2& gravity, bool allowSleep) { Time timer; real32 h = step.delta; // Integrate velocities and apply damping. Initialize the body state. for (s32 i = 0; i < m_bodyCount; ++i) { Body* b = m_bodies[i]; glm::vec2 c = b->m_sweep.c; real32 a = b->m_sweep.a; glm::vec2 v = b->m_linearVelocity; real32 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 == 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; } auto captureTimer = Services::getPlatform()->getTime(); // Solver data SolverData solverData; solverData.step = step; solverData.positions = m_positions; solverData.velocities = m_velocities; // Initialize velocity constraints. ContactSolverDef contactSolverDef; contactSolverDef.step = step; contactSolverDef.contacts = m_contacts; contactSolverDef.count = m_contactCount; contactSolverDef.positions = m_positions; contactSolverDef.velocities = m_velocities; contactSolverDef.allocator = m_allocator; ContactSolver contactSolver(&contactSolverDef); contactSolver.InitializeVelocityConstraints(); if (step.warmStarting) { contactSolver.WarmStart(); } for (s32 i = 0; i < m_jointCount; ++i) { m_joints[i]->InitVelocityConstraints(solverData); } profile->solveInit = Services::getPlatform()->getTime()-captureTimer; // Solve velocity constraints captureTimer = Services::getPlatform()->getTime(); for (s32 i = 0; i < step.velocityIterations; ++i) { for (s32 j = 0; j < m_jointCount; ++j) { m_joints[j]->SolveVelocityConstraints(solverData); } contactSolver.SolveVelocityConstraints(); } // Store impulses for warm starting contactSolver.StoreImpulses(); profile->solveVelocity = Services::getPlatform()->getTime()-captureTimer; // Integrate positions for (s32 i = 0; i < m_bodyCount; ++i) { glm::vec2 c = m_positions[i].c; real32 a = m_positions[i].a; glm::vec2 v = m_velocities[i].v; real32 w = m_velocities[i].w; // Check for large velocities glm::vec2 translation = h * v; if (glm::dot(translation, translation) > maxTranslationSquared) { real32 ratio = maxTranslation / translation.length(); v *= ratio; } real32 rotation = h * w; if (rotation * rotation > maxRotationSquared) { real32 ratio = maxRotation / glm::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 captureTimer = Services::getPlatform()->getTime(); bool positionSolved = false; for (s32 i = 0; i < step.positionIterations; ++i) { bool contactsOkay = contactSolver.SolvePositionConstraints(); bool jointsOkay = true; for (s32 i = 0; i < m_jointCount; ++i) { bool jointOkay = m_joints[i]->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 (s32 i = 0; i < m_bodyCount; ++i) { Body* 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->SynchronizeTransform2D(); } profile->solvePosition = Services::getPlatform()->getTime()-captureTimer; Report(contactSolver.m_velocityConstraints); if (allowSleep) { real32 minSleepTime = FLT_MAX; const real32 linTolSqr = linearSleepTolerance * linearSleepTolerance; const real32 angTolSqr = angularSleepTolerance * angularSleepTolerance; for (s32 i = 0; i < m_bodyCount; ++i) { Body* b = m_bodies[i]; if (b->GetType() == staticBody) { continue; } if ((b->m_flags & Body::autoSleepFlag) == 0 || b->m_angularVelocity * b->m_angularVelocity > angTolSqr || glm::dot(b->m_linearVelocity, b->m_linearVelocity) > linTolSqr) { b->m_sleepTime = 0.0f; minSleepTime = 0.0f; } else { b->m_sleepTime += h; minSleepTime = glm::min(minSleepTime, b->m_sleepTime); } } if (minSleepTime >= timeToSleep && positionSolved) { for (s32 i = 0; i < m_bodyCount; ++i) { Body* b = m_bodies[i]; b->SetAwake(false); } } } }