public void solveTOI(TimeStep subStep) { ContactSolver contactSolver = new ContactSolver(subStep, m_contacts, m_contactCount); // No warm starting needed for TOI events. // Solve velocity constraints. for (int i = 0; i < subStep.maxIterations; ++i) { contactSolver.solveVelocityConstraints(); } // Don't store the TOI contact forces for warm starting // because they can be quite large. // Integrate positions. for (int i = 0; i < m_bodyCount; ++i) { Body b = m_bodies[i]; if (b.isStatic()) continue; // System.out.println("(Island::SolveTOI 1) :"+b.m_sweep); // Store positions for continuous collision. b.m_sweep.c0.set(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; // System.out.println("(Island::SolveTOI 2) :"+b.m_sweep); // Compute new transform b.synchronizeTransform(); // System.out.println("(Island::SolveTOI 3) :"+b.m_sweep); // Note: shapes are synchronized later. } // Solve position constraints. final float k_toiBaumgarte = 0.75f; for (int i = 0; i < subStep.maxIterations; ++i) { boolean contactsOkay = contactSolver.solvePositionConstraints(k_toiBaumgarte); if (contactsOkay) { break; } } report(contactSolver.m_constraints); }
public void solve(TimeStep step, Vec2 gravity, boolean correctPositions, boolean allowSleep) { // Integrate velocities and apply damping. for (int i = 0; i < m_bodyCount; ++i) { Body b = m_bodies[i]; if (b.isStatic()) continue; // Integrate velocities. b.m_linearVelocity.x += step.dt * (gravity.x + b.m_invMass * b.m_force.x); b.m_linearVelocity.y += step.dt * (gravity.y + b.m_invMass * b.m_force.y); b.m_angularVelocity += step.dt * b.m_invI * b.m_torque; // Reset forces. b.m_force.set(0.0f, 0.0f); b.m_torque = 0.0f; // 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.mulLocal(MathUtils.clamp(1.0f - step.dt * b.m_linearDamping, 0.0f, 1.0f)); b.m_angularVelocity *= MathUtils.clamp(1.0f - step.dt * b.m_angularDamping, 0.0f, 1.0f); // Check for large velocities. if (Vec2.dot(b.m_linearVelocity, b.m_linearVelocity) > Settings.maxLinearVelocitySquared) { b.m_linearVelocity.normalize(); b.m_linearVelocity.mulLocal(Settings.maxLinearVelocity); } if (b.m_angularVelocity * b.m_angularVelocity > Settings.maxAngularVelocitySquared) { if (b.m_angularVelocity < 0.0f) { b.m_angularVelocity = -Settings.maxAngularVelocity; } else { b.m_angularVelocity = Settings.maxAngularVelocity; } } } ContactSolver contactSolver = new ContactSolver(step, m_contacts, m_contactCount); // Initialize velocity constraints. contactSolver.initVelocityConstraints(step); for (int i = 0; i < m_jointCount; ++i) { m_joints[i].initVelocityConstraints(step); } // Solve velocity constraints. for (int i = 0; i < step.maxIterations; ++i) { contactSolver.solveVelocityConstraints(); for (int j = 0; j < m_jointCount; ++j) { m_joints[j].solveVelocityConstraints(step); } } // Post-solve (store impulses for warm starting). contactSolver.finalizeVelocityConstraints(); // Integrate positions. for (int i = 0; i < m_bodyCount; ++i) { Body b = m_bodies[i]; if (b.isStatic()) continue; // Store positions for continuous collision. b.m_sweep.c0.set(b.m_sweep.c); b.m_sweep.a0 = b.m_sweep.a; // Integrate b.m_sweep.c.x += step.dt * b.m_linearVelocity.x; b.m_sweep.c.y += step.dt * b.m_linearVelocity.y; b.m_sweep.a += step.dt * b.m_angularVelocity; // Compute new transform b.synchronizeTransform(); // Note: shapes are synchronized later. } if (correctPositions) { // Initialize position constraints. // Contacts don't need initialization. for (int i = 0; i < m_jointCount; ++i) { m_joints[i].initPositionConstraints(); } // Iterate over constraints. for (m_positionIterationCount = 0; m_positionIterationCount < step.maxIterations; ++m_positionIterationCount) { boolean contactsOkay = contactSolver.solvePositionConstraints(Settings.contactBaumgarte); boolean jointsOkay = true; for (int i = 0; i < m_jointCount; ++i) { boolean jointOkay = m_joints[i].solvePositionConstraints(); jointsOkay = jointsOkay && jointOkay; } if (contactsOkay && jointsOkay) { break; } } } report(contactSolver.m_constraints); if (allowSleep) { float minSleepTime = Float.MAX_VALUE; final float linTolSqr = Settings.linearSleepTolerance * Settings.linearSleepTolerance; final float angTolSqr = Settings.angularSleepTolerance * Settings.angularSleepTolerance; for (int i = 0; i < m_bodyCount; ++i) { Body b = m_bodies[i]; if (b.m_invMass == 0.0f) { continue; } if ((b.m_flags & Body.e_allowSleepFlag) == 0) { b.m_sleepTime = 0.0f; minSleepTime = 0.0f; } if ((b.m_flags & Body.e_allowSleepFlag) == 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 += step.dt; minSleepTime = Math.min(minSleepTime, b.m_sleepTime); } } if (minSleepTime >= Settings.timeToSleep) { for (int i = 0; i < m_bodyCount; ++i) { Body b = m_bodies[i]; b.m_flags |= Body.e_sleepFlag; b.m_linearVelocity = new Vec2(0.0f, 0.0f); b.m_angularVelocity = 0.0f; } } } }
public Object pairAdded(final Object proxyUserData1, final Object proxyUserData2) { Shape shape1 = (Shape) proxyUserData1; Shape shape2 = (Shape) proxyUserData2; Body body1 = shape1.getBody(); Body body2 = shape2.getBody(); if (body1.isStatic() && body2.isStatic()) { return m_nullContact; } if (shape1.getBody() == shape2.getBody()) { return m_nullContact; } if (body2.isConnected(body1)) { return m_nullContact; } if (m_world.m_contactFilter != null && m_world.m_contactFilter.shouldCollide(shape1, shape2) == false) { return m_nullContact; } // Call the factory. final Contact c = Contact.createContact(shape1, shape2); if (c == null) { return m_nullContact; } // Contact creation may swap shapes. shape1 = c.getShape1(); shape2 = c.getShape2(); body1 = shape1.getBody(); body2 = shape2.getBody(); // Insert into the world. c.m_prev = null; c.m_next = m_world.m_contactList; if (m_world.m_contactList != null) { m_world.m_contactList.m_prev = c; } m_world.m_contactList = c; // Connect to island graph. // Connect to body 1 c.m_node1.contact = c; c.m_node1.other = body2; c.m_node1.prev = null; c.m_node1.next = body1.m_contactList; if (body1.m_contactList != null) { body1.m_contactList.prev = c.m_node1; } body1.m_contactList = c.m_node1; // Connect to body 2 c.m_node2.contact = c; c.m_node2.other = body1; c.m_node2.prev = null; c.m_node2.next = body2.m_contactList; if (body2.m_contactList != null) { body2.m_contactList.prev = c.m_node2; } body2.m_contactList = c.m_node2; ++m_world.m_contactCount; return c; }