diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp index 451539b6..b9dfdf3a 100644 --- a/src/engine/ConstraintSolver.cpp +++ b/src/engine/ConstraintSolver.cpp @@ -35,7 +35,7 @@ using namespace std; // Constructor ConstraintSolver::ConstraintSolver(DynamicsWorld* world) :world(world), nbConstraints(0), mNbIterations(10), mContactConstraints(0), - mLinearVelocities(0), mAngularVelocities(0) { + mLinearVelocities(0), mAngularVelocities(0) { } @@ -44,7 +44,7 @@ ConstraintSolver::~ConstraintSolver() { } - // Initialize the constraint solver before each solving +// Initialize the constraint solver void ConstraintSolver::initialize() { nbConstraints = 0; @@ -120,51 +120,31 @@ void ConstraintSolver::initialize() { assert(mMapBodyToIndex.size() == nbBodies); } -// Initialize bodies velocities +// Initialize the constrained bodies void ConstraintSolver::initializeBodies() { // For each current body that is implied in some constraint RigidBody* rigidBody; - RigidBody* body; - uint b=0; - for (set::iterator it = mConstraintBodies.begin(); it != mConstraintBodies.end(); ++it, b++) { + for (set::iterator it = mConstraintBodies.begin(); it != mConstraintBodies.end(); ++it) { rigidBody = *it; uint bodyNumber = mMapBodyToIndex[rigidBody]; // TODO : Use polymorphism and remove this downcasting assert(rigidBody); - // Compute the vector V1 with initial velocities values - int bodyIndexArray = 6 * bodyNumber; - mLinearVelocities[bodyNumber] = rigidBody->getLinearVelocity() + mTimeStep * rigidBody->getMassInverse() * rigidBody->getExternalForce(); mAngularVelocities[bodyNumber] = rigidBody->getAngularVelocity() + mTimeStep * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque(); - - // Initialize the mass and inertia tensor matrices - Minv_sp_inertia[bodyNumber].setAllValues(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0); - Minv_sp_mass_diag[bodyNumber] = 0.0; - - // If the motion of the rigid body is enabled - if (rigidBody->getIsMotionEnabled()) { - Minv_sp_inertia[bodyNumber] = rigidBody->getInertiaTensorInverseWorld(); - Minv_sp_mass_diag[bodyNumber] = rigidBody->getMassInverse(); - } } } -// Fill in all the matrices needed to solve the LCP problem -// Notice that all the active constraints should have been evaluated first -void ConstraintSolver::initializeContactConstraints(decimal dt) { - decimal oneOverDT = 1.0 / dt; +// Initialize the contact constraints before solving the system +void ConstraintSolver::initializeContactConstraints() { // For each contact constraint for (uint c=0; ccomputeJacobianPenetration(contact.J_spBody1Penetration, contact.J_spBody2Penetration); - realContact->computeJacobianFriction1(contact.J_spBody1Friction1, contact.J_spBody2Friction1); - realContact->computeJacobianFriction2(contact.J_spBody1Friction2, contact.J_spBody2Friction2); - - // Fill in the body mapping matrix - //for(int i=0; igetNbConstraints(); i++) { - // bodyMapping[noConstraint+i][0] = constraint->getBody1(); - // bodyMapping[noConstraint+i][1] = constraint->getBody2(); - //} - // Fill in the limit vectors for the constraint realContact->computeLowerBoundPenetration(contact.lowerBoundPenetration); realContact->computeLowerBoundFriction1(contact.lowerBoundFriction1); @@ -240,147 +209,74 @@ void ConstraintSolver::initializeContactConstraints(decimal dt) { realContact->computeUpperBoundFriction1(contact.upperBoundFriction1); realContact->computeUpperBoundFriction2(contact.upperBoundFriction2); - // Fill in the error vector - realContact->computeErrorPenetration(contact.errorPenetration); - // Get the cached lambda values of the constraint contact.penetrationImpulse = realContact->getCachedLambda(0); contact.friction1Impulse = realContact->getCachedLambda(1); contact.friction2Impulse = realContact->getCachedLambda(2); - //for (int i=0; igetNbConstraints(); i++) { - // lambdaInit[noConstraint + i] = constraint->getCachedLambda(i); - // } - - contact.errorPenetration = 0.0; - decimal slop = 0.005; - if (realContact->getPenetrationDepth() > slop) { - contact.errorPenetration -= 0.2 * oneOverDT * std::max(double(realContact->getPenetrationDepth() - slop), 0.0); - } - - // ---------- Penetration ---------- // - - // b = errorValues * oneOverDT; - contact.b_Penetration = contact.errorPenetration; } } } -// Compute the matrix B_sp -void ConstraintSolver::computeMatrixB_sp() { - uint indexConstraintArray, indexBody1, indexBody2; - +// Warm start the solver +// For each constraint, we apply the previous impulse (from the previous step) +// at the beginning. With this technique, we will converge faster towards +// the solution of the linear system +void ConstraintSolver::warmStart() { + // For each constraint - for (uint m = 0; m activeConstraints; // Current active constraints in the physics world bool isErrorCorrectionActive; // True if error correction (with world order) is active @@ -159,19 +149,6 @@ class ConstraintSolver { uint nbBodies; // Current number of bodies in the physics world RigidBody* bodyMapping[NB_MAX_CONSTRAINTS][2]; // 2-dimensional array that contains the mapping of body reference // in the J_sp and B_sp matrices. For instance the cell bodyMapping[i][j] contains - // the pointer to the body that correspond to the 1x6 J_ij matrix in the - // J_sp matrix. An integer body index refers to its index in the "bodies" std::vector - decimal J_sp[NB_MAX_CONSTRAINTS][2*6]; // 2-dimensional array that correspond to the sparse representation of the jacobian matrix of all constraints - // This array contains for each constraint two 1x6 Jacobian matrices (one for each body of the constraint) - // a 1x6 matrix - decimal B_sp[2][6*NB_MAX_CONSTRAINTS]; // 2-dimensional array that correspond to a useful matrix in sparse representation - // This array contains for each constraint two 6x1 matrices (one for each body of the constraint) - // a 6x1 matrix - decimal b[NB_MAX_CONSTRAINTS]; // Vector "b" of the LCP problem - decimal d[NB_MAX_CONSTRAINTS]; // Vector "d" - Matrix3x3 Minv_sp_inertia[NB_MAX_BODIES]; // 3x3 world inertia tensor matrix I for each body (from the Minv_sp matrix) - decimal Minv_sp_mass_diag[NB_MAX_BODIES]; // Array that contains for each body the inverse of its mass - // This is an array of size nbBodies that contains in each cell a 6x6 matrix Vector3* mLinearVelocities; // Array of constrained linear velocities Vector3* mAngularVelocities; // Array of constrained angular velocities decimal mTimeStep; // Current time step @@ -188,14 +165,26 @@ class ConstraintSolver { // Map body to index std::map mMapBodyToIndex; + // -------------------- Methods -------------------- // - void initialize(); // Initialize the constraint solver before each solving - void initializeBodies(); // Initialize bodies velocities - void initializeContactConstraints(decimal dt); // Fill in all the matrices needed to solve the LCP problem - void computeMatrixB_sp(); // Compute the matrix B_sp - void cacheLambda(); // Cache the lambda values in order to reuse them in the next step to initialize the lambda vector - void warmStart(); // Compute the vector a used in the solve() method - void solveLCP(); // Solve a LCP problem using Projected-Gauss-Seidel algorithm + // Initialize the constraint solver + void initialize(); + + // Initialize the constrained bodies + void initializeBodies(); + + // Initialize the contact constraints before solving the system + void initializeContactConstraints(); + + // Store the computed impulses to use them to + // warm start the solver at the next iteration + void storeImpulses(); + + // Warm start the solver + void warmStart(); + + // Solve the contact constraints by applying sequential impulses + void solveContactConstraints(); // Apply an impulse to the two bodies of a constraint void applyImpulse(const Impulse& impulse, const ContactConstraint& constraint); @@ -204,14 +193,32 @@ class ConstraintSolver { decimal computeMixRestitutionFactor(const RigidBody *body1, const RigidBody *body2) const; public: - ConstraintSolver(DynamicsWorld* world); // Constructor - virtual ~ConstraintSolver(); // Destructor - void solve(decimal dt); // Solve the current LCP problem - bool isConstrainedBody(RigidBody* body) const; // Return true if the body is in at least one constraint - Vector3 getConstrainedLinearVelocityOfBody(RigidBody *body); // Return the constrained linear velocity of a body after solving the LCP problem - Vector3 getConstrainedAngularVelocityOfBody(RigidBody* body); // Return the constrained angular velocity of a body after solving the LCP problem - void cleanup(); // Cleanup of the constraint solver - void setNbLCPIterations(uint mNbIterations); // Set the number of iterations of the LCP solver + + // -------------------- Methods -------------------- // + + // Constructor + ConstraintSolver(DynamicsWorld* world); + + // Destructor + virtual ~ConstraintSolver(); + + // Solve the constraints + void solve(decimal timeStep); + + // Return true if the body is in at least one constraint + bool isConstrainedBody(RigidBody* body) const; + + // Return the constrained linear velocity of a body after solving the constraints + Vector3 getConstrainedLinearVelocityOfBody(RigidBody *body); + + // Return the constrained angular velocity of a body after solving the constraints + Vector3 getConstrainedAngularVelocityOfBody(RigidBody* body); + + // Clean up the constraint solver + void cleanup(); + + // Set the number of iterations of the LCP solver + void setNbLCPIterations(uint mNbIterations); }; // Return true if the body is in at least one constraint @@ -219,21 +226,21 @@ inline bool ConstraintSolver::isConstrainedBody(RigidBody* body) const { return mConstraintBodies.count(body) == 1; } -// Return the constrained linear velocity of a body after solving the LCP problem +// Return the constrained linear velocity of a body after solving the constraints inline Vector3 ConstraintSolver::getConstrainedLinearVelocityOfBody(RigidBody* body) { assert(isConstrainedBody(body)); uint indexBodyArray = mMapBodyToIndex[body]; return mLinearVelocities[indexBodyArray]; } -// Return the constrained angular velocity of a body after solving the LCP problem +// Return the constrained angular velocity of a body after solving the constraints inline Vector3 ConstraintSolver::getConstrainedAngularVelocityOfBody(RigidBody *body) { assert(isConstrainedBody(body)); uint indexBodyArray = mMapBodyToIndex[body]; return mAngularVelocities[indexBodyArray]; } -// Cleanup of the constraint solver +// Clean up the constraint solver inline void ConstraintSolver::cleanup() { mMapBodyToIndex.clear(); mConstraintBodies.clear();