public void solveTOI(TimeStep 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) { m_positions[i].c.x = m_bodies[i].m_sweep.c.x; m_positions[i].c.y = m_bodies[i].m_sweep.c.y; m_positions[i].a = m_bodies[i].m_sweep.a; m_velocities[i].v.x = m_bodies[i].m_linearVelocity.x; m_velocities[i].v.y = m_bodies[i].m_linearVelocity.y; m_velocities[i].w = m_bodies[i].m_angularVelocity; } toiSolverDef.contacts = m_contacts; toiSolverDef.count = m_contactCount; toiSolverDef.step = subStep; toiSolverDef.positions = m_positions; toiSolverDef.velocities = m_velocities; toiContactSolver.init(toiSolverDef); // Solve position constraints. for (int i = 0; i < subStep.positionIterations; ++i) { bool contactsOkay = toiContactSolver.solveTOIPositionConstraints(toiIndexA, toiIndexB); if (contactsOkay) { break; } } // #if 0 // // Is the new position really safe? // for (int 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(); // // int indexA = c.GetChildIndexA(); // int indexB = c.GetChildIndexB(); // // DistanceInput input; // input.proxyA.Set(fA.GetShape(), indexA); // input.proxyB.Set(fB.GetShape(), indexB); // input.transformA = bA.GetTransform(); // input.transformB = bB.GetTransform(); // 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.x = m_positions[toiIndexA].c.x; m_bodies[toiIndexA].m_sweep.c0.y = m_positions[toiIndexA].c.y; m_bodies[toiIndexA].m_sweep.a0 = m_positions[toiIndexA].a; m_bodies[toiIndexB].m_sweep.c0.set(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. toiContactSolver.initializeVelocityConstraints(); // Solve velocity constraints. for (int i = 0; i < subStep.velocityIterations; ++i) { toiContactSolver.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) { Vec2 c = m_positions[i].c; float a = m_positions[i].a; Vec2 v = m_velocities[i].v; float w = m_velocities[i].w; // Check for large velocities float translationx = v.x*h; float translationy = v.y*h; if (translationx*translationx + translationy*translationy > Settings.maxTranslationSquared) { float ratio = Settings.maxTranslation /MathUtils.sqrt(translationx*translationx + translationy*translationy); v.mulLocal(ratio); } float rotation = h*w; if (rotation*rotation > Settings.maxRotationSquared) { float ratio = Settings.maxRotation/MathUtils.abs(rotation); w *= ratio; } // Integrate c.x += v.x*h; c.y += v.y*h; a += h*w; m_positions[i].c.x = c.x; m_positions[i].c.y = c.y; m_positions[i].a = a; m_velocities[i].v.x = v.x; m_velocities[i].v.y = v.y; m_velocities[i].w = w; // Sync bodies Body body = m_bodies[i]; body.m_sweep.c.x = c.x; body.m_sweep.c.y = c.y; body.m_sweep.a = a; body.m_linearVelocity.x = v.x; body.m_linearVelocity.y = v.y; body.m_angularVelocity = w; body.synchronizeTransform(); } report(toiContactSolver.m_velocityConstraints); }
private void solveTOI(TimeStep step) { Island island = toiIsland; island.init(2*Settings.maxTOIContacts, Settings.maxTOIContacts, 0, m_contactManager.m_contactListener); if (m_stepComplete) { for (Body b = m_bodyList; b != null; b = b.m_next) { b.m_flags &= ~BodyFlags.Island; b.m_sweep.alpha0 = 0.0f; } for (Contact c = m_contactManager.m_contactList; c != null; c = c.m_next) { // Invalidate TOI c.m_flags &= ~(Contact.TOI_FLAG | Contact.ISLAND_FLAG); c.m_toiCount = 0; c.m_toi = 1.0f; } } // Find TOI events and solve them. for (;;) { // Find the first TOI. Contact minContact = null; float minAlpha = 1.0f; Fixture fA; Fixture fB; Body bA; Body bB; for (Contact c = m_contactManager.m_contactList; c != null; c = c.m_next) { // Is this contact disabled? if (c.isEnabled() == false) { continue; } // Prevent excessive sub-stepping. if (c.m_toiCount > Settings.maxSubSteps) { continue; } float alpha = 1.0f; if ((c.m_flags & Contact.TOI_FLAG) != 0) { // This contact has a valid cached TOI. alpha = c.m_toi; } else { fA = c.getFixtureA(); fB = c.getFixtureB(); // Is there a sensor? if (fA.isSensor() || fB.isSensor()) { continue; } bA = fA.getBody(); bB = fB.getBody(); BodyType typeA = bA.m_type; BodyType typeB = bB.m_type; Debug.Assert(typeA == BodyType.DYNAMIC || typeB == BodyType.DYNAMIC); bool activeA = bA.isAwake() && typeA != BodyType.STATIC; bool activeB = bB.isAwake() && typeB != BodyType.STATIC; // Is at least one body active (awake and dynamic or kinematic)? if (activeA == false && activeB == false) { continue; } bool collideA = bA.isBullet() || typeA != BodyType.DYNAMIC; bool collideB = bB.isBullet() || typeB != BodyType.DYNAMIC; // 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.m_sweep.alpha0; if (bA.m_sweep.alpha0 < bB.m_sweep.alpha0) { alpha0 = bB.m_sweep.alpha0; bA.m_sweep.advance(alpha0); } else if (bB.m_sweep.alpha0 < bA.m_sweep.alpha0) { alpha0 = bA.m_sweep.alpha0; bB.m_sweep.advance(alpha0); } Debug.Assert(alpha0 < 1.0f); int indexA = c.getChildIndexA(); int indexB = c.getChildIndexB(); // Compute the time of impact in interval [0, minTOI] TimeOfImpact.TOIInput input = toiInput; input.proxyA.set(fA.getShape(), indexA); input.proxyB.set(fB.getShape(), indexB); input.sweepA.set(bA.m_sweep); input.sweepB.set(bB.m_sweep); input.tMax = 1.0f; pool.getTimeOfImpact().timeOfImpact(toiOutput, input); // Beta is the fraction of the remaining portion of the . float beta = toiOutput.t; if (toiOutput.state == TimeOfImpact.TOIOutputState.TOUCHING) { alpha = MathUtils.min(alpha0 + (1.0f - alpha0)*beta, 1.0f); } else { alpha = 1.0f; } c.m_toi = alpha; c.m_flags |= Contact.TOI_FLAG; } if (alpha < minAlpha) { // This is the minimum TOI found so far. minContact = c; minAlpha = alpha; } } if (minContact == null || 1.0f - 10.0f*Settings.EPSILON < minAlpha) { // No more TOI events. Done! m_stepComplete = true; break; } // Advance the bodies to the TOI. fA = minContact.getFixtureA(); fB = minContact.getFixtureB(); bA = fA.getBody(); bB = fB.getBody(); backup1.set(bA.m_sweep); backup2.set(bB.m_sweep); bA.advance(minAlpha); bB.advance(minAlpha); // The TOI contact likely has some new contact points. minContact.update(m_contactManager.m_contactListener); minContact.m_flags &= ~Contact.TOI_FLAG; ++minContact.m_toiCount; // Is the contact solid? if (minContact.isEnabled() == false || minContact.isTouching() == false) { // Restore the sweeps. minContact.setEnabled(false); bA.m_sweep.set(backup1); bB.m_sweep.set(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.m_flags |= BodyFlags.Island; bB.m_flags |= BodyFlags.Island; minContact.m_flags |= Contact.ISLAND_FLAG; // Get contacts on bodyA and bodyB. tempBodies[0] = bA; tempBodies[1] = bB; for (int i = 0; i < 2; ++i) { Body body = tempBodies[i]; if (body.m_type == BodyType.DYNAMIC) { for (ContactEdge ce = body.m_contactList; ce != null; ce = ce.next) { if (island.m_bodyCount == island.m_bodyCapacity) { break; } if (island.m_contactCount == island.m_contactCapacity) { break; } Contact contact = ce.contact; // Has this contact already been added to the island? if ((contact.m_flags & Contact.ISLAND_FLAG) != 0) { continue; } // Only add static, kinematic, or bullet bodies. Body other = ce.other; if (other.m_type == BodyType.DYNAMIC && 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. backup1.set(other.m_sweep); if ((other.m_flags & BodyFlags.Island) == 0) { other.advance(minAlpha); } // Update the contact points contact.update(m_contactManager.m_contactListener); // Was the contact disabled by the user? if (contact.isEnabled() == false) { other.m_sweep.set(backup1); other.synchronizeTransform(); continue; } // Are there contact points? if (contact.isTouching() == false) { other.m_sweep.set(backup1); other.synchronizeTransform(); continue; } // Add the contact to the island contact.m_flags |= Contact.ISLAND_FLAG; island.add(contact); // Has the other body already been added to the island? if ((other.m_flags & BodyFlags.Island) != 0) { continue; } // Add the other body to the island. other.m_flags |= BodyFlags.Island; if (other.m_type != BodyType.STATIC) { other.setAwake(true); } island.add(other); } } } 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) { Body body = island.m_bodies[i]; body.m_flags &= ~BodyFlags.Island; if (body.m_type != BodyType.DYNAMIC) { continue; } body.synchronizeFixtures(); // Invalidate all contact TOIs on this displaced body. for (ContactEdge ce = body.m_contactList; ce != null; ce = ce.next) { ce.contact.m_flags &= ~(Contact.TOI_FLAG | Contact.ISLAND_FLAG); } } // 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 solve(Profile profile, TimeStep step, Vec2 gravity, bool allowSleep) { // System.ref.println("Solving Island"); float h = step.dt; // Integrate velocities and apply damping. Initialize the body state. for (int i = 0; i < m_bodyCount; ++i) { Body b = m_bodies[i]; Sweep bm_sweep = b.m_sweep; Vec2 c = bm_sweep.c; float a = bm_sweep.a; Vec2 v = b.m_linearVelocity; float w = b.m_angularVelocity; // Store positions for continuous collision. bm_sweep.c0.set(bm_sweep.c); bm_sweep.a0 = bm_sweep.a; if (b.m_type == BodyType.DYNAMIC) { // Integrate velocities. // v += h * (b.m_gravityScale * gravity + b.m_invMass * b.m_force); v.x += h*(b.m_gravityScale*gravity.x + b.m_invMass*b.m_force.x); v.y += h*(b.m_gravityScale*gravity.y + b.m_invMass*b.m_force.y); 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.x *= 1.0f/(1.0f + h*b.m_linearDamping); v.y *= 1.0f/(1.0f + h*b.m_linearDamping); w *= 1.0f/(1.0f + h*b.m_angularDamping); } m_positions[i].c.x = c.x; m_positions[i].c.y = c.y; m_positions[i].a = a; m_velocities[i].v.x = v.x; m_velocities[i].v.y = v.y; m_velocities[i].w = w; } timer.reset(); // Solver data solverData.step = step; solverData.positions = m_positions; solverData.velocities = m_velocities; // Initialize velocity constraints. solverDef.step = step; solverDef.contacts = m_contacts; solverDef.count = m_contactCount; solverDef.positions = m_positions; solverDef.velocities = m_velocities; contactSolver.init(solverDef); // System.ref.println("island init vel"); contactSolver.initializeVelocityConstraints(); if (step.warmStarting) { // System.ref.println("island warm start"); contactSolver.warmStart(); } for (int i = 0; i < m_jointCount; ++i) { m_joints[i].initVelocityConstraints(solverData); } profile.solveInit.accum(timer.getMilliseconds()); // Solve velocity constraints timer.reset(); // System.ref.println("island solving velocities"); 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.accum(timer.getMilliseconds()); // Integrate positions for (int i = 0; i < m_bodyCount; ++i) { Vec2 c = m_positions[i].c; float a = m_positions[i].a; Vec2 v = m_velocities[i].v; float w = m_velocities[i].w; // Check for large velocities float translationx = v.x*h; float translationy = v.y*h; if (translationx*translationx + translationy*translationy > Settings.maxTranslationSquared) { float ratio = Settings.maxTranslation /MathUtils.sqrt(translationx*translationx + translationy*translationy); v.x *= ratio; v.y *= ratio; } float rotation = h*w; if (rotation*rotation > Settings.maxRotationSquared) { float ratio = Settings.maxRotation/MathUtils.abs(rotation); w *= ratio; } // Integrate c.x += h*v.x; c.y += h*v.y; a += h*w; m_positions[i].a = a; 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) { Body body = m_bodies[i]; body.m_sweep.c.x = m_positions[i].c.x; body.m_sweep.c.y = m_positions[i].c.y; body.m_sweep.a = m_positions[i].a; body.m_linearVelocity.x = m_velocities[i].v.x; body.m_linearVelocity.y = m_velocities[i].v.y; body.m_angularVelocity = m_velocities[i].w; body.synchronizeTransform(); } profile.solvePosition.accum(timer.getMilliseconds()); report(contactSolver.m_velocityConstraints); if (allowSleep) { float minSleepTime = float.MaxValue; float linTolSqr = Settings.linearSleepTolerance*Settings.linearSleepTolerance; float angTolSqr = Settings.angularSleepTolerance*Settings.angularSleepTolerance; for (int i = 0; i < m_bodyCount; ++i) { Body b = m_bodies[i]; if (b.getType() == BodyType.STATIC) { continue; } if ((b.m_flags & BodyFlags.AutoSleep) == 0 || b.m_angularVelocity*b.m_angularVelocity > angTolSqr || Vec2.dot(b.m_linearVelocity, b.m_linearVelocity) > linTolSqr) { b.m_sleepTime = 0.0f; minSleepTime = 0.0f; } else { b.m_sleepTime += h; minSleepTime = MathUtils.min(minSleepTime, b.m_sleepTime); } } if (minSleepTime >= Settings.timeToSleep && positionSolved) { for (int i = 0; i < m_bodyCount; ++i) { Body b = m_bodies[i]; b.setAwake(false); } } } }
private void solve(TimeStep step) { m_profile.solveInit.startAccum(); m_profile.solveVelocity.startAccum(); m_profile.solvePosition.startAccum(); // update previous transforms for (Body b = m_bodyList; b != null; b = b.m_next) { b.m_xf0.set(b.m_xf); } // Size the island for the worst case. island.init(m_bodyCount, m_contactManager.m_contactCount, m_jointCount, m_contactManager.m_contactListener); // Clear all the island flags. for (Body b = m_bodyList; b != null; b = b.m_next) { b.m_flags &= ~BodyFlags.Island; } for (Contact c = m_contactManager.m_contactList; c != null; c = c.m_next) { c.m_flags &= ~Contact.ISLAND_FLAG; } for (Joint j = m_jointList; j != null; j = j.m_next) { j.m_islandFlag = false; } // Build and simulate all awake islands. int stackSize = m_bodyCount; if (stack.Length < stackSize) { stack = new Body[stackSize]; } for (Body seed = m_bodyList; seed != null; seed = seed.m_next) { if ((seed.m_flags & BodyFlags.Island) == BodyFlags.Island) { continue; } if (seed.isAwake() == false || seed.isActive() == false) { continue; } // The seed can be dynamic or kinematic. if (seed.getType() == BodyType.STATIC) { continue; } // Reset island and stack. island.clear(); int stackCount = 0; stack[stackCount++] = seed; seed.m_flags |= BodyFlags.Island; // Perform a depth first search (DFS) on the constraint graph. while (stackCount > 0) { // Grab the next body off the stack and add it to the island. Body b = stack[--stackCount]; Debug.Assert(b.isActive() == true); 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.getType() == BodyType.STATIC) { continue; } // Search all contacts connected to this body. for (ContactEdge ce = b.m_contactList; ce != null; ce = ce.next) { Contact contact = ce.contact; // Has this contact already been added to an island? if ((contact.m_flags & Contact.ISLAND_FLAG) == Contact.ISLAND_FLAG) { 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.m_flags |= Contact.ISLAND_FLAG; Body other = ce.other; // Was the other body already added to this island? if ((other.m_flags & BodyFlags.Island) == BodyFlags.Island) { continue; } Debug.Assert(stackCount < stackSize); stack[stackCount++] = other; other.m_flags |= BodyFlags.Island; } // Search all joints connect to this body. for (JointEdge je = b.m_jointList; je != null; je = je.next) { if (je.joint.m_islandFlag == true) { continue; } Body 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.m_flags & BodyFlags.Island) == BodyFlags.Island) { continue; } Debug.Assert(stackCount < stackSize); stack[stackCount++] = other; other.m_flags |= BodyFlags.Island; } } island.solve(m_profile, step, m_gravity, m_allowSleep); // Post solve cleanup. for (int i = 0; i < island.m_bodyCount; ++i) { // Allow static bodies to participate in other islands. Body b = island.m_bodies[i]; if (b.getType() == BodyType.STATIC) { b.m_flags &= ~BodyFlags.Island; } } } m_profile.solveInit.endAccum(); m_profile.solveVelocity.endAccum(); m_profile.solvePosition.endAccum(); broadphaseTimer.reset(); // Synchronize fixtures, check for ref of range bodies. for (Body b = m_bodyList; b != null; b = b.getNext()) { // If a body was not in an island then it did not move. if ((b.m_flags & BodyFlags.Island) == 0) { continue; } if (b.getType() == BodyType.STATIC) { continue; } // Update fixtures (for broad-phase). b.synchronizeFixtures(); } // Look for new contacts. m_contactManager.findNewContacts(); m_profile.broadphase.record(broadphaseTimer.getMilliseconds()); }