From 7172ee4843a81118c0e7b60df9ce1c6e13b46a92 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sun, 16 Dec 2012 16:57:15 +0100 Subject: [PATCH] Loop over the contact manifolds in the constraint solver --- src/constraint/Contact.cpp | 118 +++++++ src/constraint/Contact.h | 16 + src/engine/ConstraintSolver.cpp | 593 +++++++++++++++++++++++--------- src/engine/ConstraintSolver.h | 43 ++- 4 files changed, 600 insertions(+), 170 deletions(-) diff --git a/src/constraint/Contact.cpp b/src/constraint/Contact.cpp index d00d674c..e83feb5f 100644 --- a/src/constraint/Contact.cpp +++ b/src/constraint/Contact.cpp @@ -53,6 +53,112 @@ Contact::~Contact() { } +void Contact::computeJacobianPenetration(decimal J_spBody1[6], decimal J_spBody2[6]) { + + Vector3 body1Position = mBody1->getTransform().getPosition(); + Vector3 body2Position = mBody2->getTransform().getPosition(); + + Vector3 r1 = mWorldPointOnBody1 - body1Position; + Vector3 r2 = mWorldPointOnBody2 - body2Position; + Vector3 r1CrossN = r1.cross(mNormal); + Vector3 r2CrossN = r2.cross(mNormal); + + // Compute the jacobian matrix for the body 1 for the contact constraint + J_spBody1[0] = -mNormal.getX(); + J_spBody1[1] = -mNormal.getY(); + J_spBody1[2] = -mNormal.getZ(); + J_spBody1[3] = -r1CrossN.getX(); + J_spBody1[4] = -r1CrossN.getY(); + J_spBody1[5] = -r1CrossN.getZ(); + + // Compute the jacobian matrix for the body 2 for the contact constraint + J_spBody2[0] = mNormal.getX(); + J_spBody2[1] = mNormal.getY(); + J_spBody2[2] = mNormal.getZ(); + J_spBody2[3] = r2CrossN.getX(); + J_spBody2[4] = r2CrossN.getY(); + J_spBody2[5] = r2CrossN.getZ(); +} + +void Contact::computeJacobianFriction1(decimal J_spBody1[6], decimal J_spBody2[6]) { + + Vector3 body1Position = mBody1->getTransform().getPosition(); + Vector3 body2Position = mBody2->getTransform().getPosition(); + + Vector3 r1 = mWorldPointOnBody1 - body1Position; + Vector3 r2 = mWorldPointOnBody2 - body2Position; + + // Compute the jacobian matrix for the body 1 for the first friction constraint + Vector3 r1CrossU1 = r1.cross(mFrictionVectors[0]); + Vector3 r2CrossU1 = r2.cross(mFrictionVectors[0]); + J_spBody1[0] = -mFrictionVectors[0].getX(); + J_spBody1[1] = -mFrictionVectors[0].getY(); + J_spBody1[2] = -mFrictionVectors[0].getZ(); + J_spBody1[3] = -r1CrossU1.getX(); + J_spBody1[4] = -r1CrossU1.getY(); + J_spBody1[5] = -r1CrossU1.getZ(); + + // Compute the jacobian matrix for the body 2 for the first friction constraint + J_spBody2[0] = mFrictionVectors[0].getX(); + J_spBody2[1] = mFrictionVectors[0].getY(); + J_spBody2[2] = mFrictionVectors[0].getZ(); + J_spBody2[3] = r2CrossU1.getX(); + J_spBody2[4] = r2CrossU1.getY(); + J_spBody2[5] = r2CrossU1.getZ(); +} + +void Contact::computeJacobianFriction2(decimal J_spBody1[6], decimal J_spBody2[6]) { + + Vector3 body1Position = mBody1->getTransform().getPosition(); + Vector3 body2Position = mBody2->getTransform().getPosition(); + + Vector3 r1 = mWorldPointOnBody1 - body1Position; + Vector3 r2 = mWorldPointOnBody2 - body2Position; + + Vector3 r1CrossU2 = r1.cross(mFrictionVectors[1]); + Vector3 r2CrossU2 = r2.cross(mFrictionVectors[1]); + + // Compute the jacobian matrix for the body 1 for the second friction constraint + J_spBody1[0] = -mFrictionVectors[1].getX(); + J_spBody1[1] = -mFrictionVectors[1].getY(); + J_spBody1[2] = -mFrictionVectors[1].getZ(); + J_spBody1[3] = -r1CrossU2.getX(); + J_spBody1[4] = -r1CrossU2.getY(); + J_spBody1[5] = -r1CrossU2.getZ(); + + // Compute the jacobian matrix for the body 2 for the second friction constraint + J_spBody2[0] = mFrictionVectors[1].getX(); + J_spBody2[1] = mFrictionVectors[1].getY(); + J_spBody2[2] = mFrictionVectors[1].getZ(); + J_spBody2[3] = r2CrossU2.getX(); + J_spBody2[4] = r2CrossU2.getY(); + J_spBody2[5] = r2CrossU2.getZ(); +} + +void Contact::computeLowerBoundPenetration(decimal& lowerBound) { + lowerBound = 0.0; +} + +void Contact::computeLowerBoundFriction1(decimal& lowerBound) { + lowerBound = -mMu_mc_g; +} + +void Contact::computeLowerBoundFriction2(decimal& lowerBound) { + lowerBound = -mMu_mc_g; +} + +void Contact::computeUpperBoundPenetration(decimal& upperBound) { + upperBound = DECIMAL_INFINITY; +} + +void Contact::computeUpperBoundFriction1(decimal& upperBound) { + upperBound = mMu_mc_g; +} + +void Contact::computeUpperBoundFriction2(decimal& upperBound) { + upperBound = mMu_mc_g; +} + // This method computes the jacobian matrix for all mathematical constraints // The argument "J_sp" is the jacobian matrix of the constraint solver. This method // fill in this matrix with all the jacobian matrix of the mathematical constraint @@ -150,6 +256,18 @@ void Contact::computeUpperBound(int noConstraint, decimal upperBounds[NB_MAX_CON upperBounds[noConstraint + 2] = mMu_mc_g; // Upper bound for the second friction constraint } +void Contact::computeErrorPenetration(decimal& error) { + // TODO : Do we need this casting anymore ? + RigidBody* rigidBody1 = dynamic_cast(mBody1); + RigidBody* rigidBody2 = dynamic_cast(mBody2); + + // Compute the error value for the contact constraint + Vector3 velocity1 = rigidBody1->getLinearVelocity(); + Vector3 velocity2 = rigidBody2->getLinearVelocity(); + decimal restitutionCoeff = rigidBody1->getRestitution() * rigidBody2->getRestitution(); + error = restitutionCoeff * (mNormal.dot(velocity1) - mNormal.dot(velocity2)); +} + // Compute the error values for all the mathematical constraints. The argument // "errorValues" is the error values vector of the constraint solver and this // method has to fill in this vector starting from the row "noConstraint" diff --git a/src/constraint/Contact.h b/src/constraint/Contact.h index 421ffe5c..99dcd89d 100644 --- a/src/constraint/Contact.h +++ b/src/constraint/Contact.h @@ -150,6 +150,22 @@ class Contact : public Constraint { // Compute the error values for all the mathematical constraints virtual void computeErrorValue(int noConstraint, decimal errorValues[]) const; + void computeErrorPenetration(decimal& error); + + void computeJacobianPenetration(decimal J_spBody1[6], decimal J_spBody2[6]); + + void computeJacobianFriction1(decimal J_spBody1[6], decimal J_spBody2[6]); + + void computeJacobianFriction2(decimal J_spBody1[6], decimal J_spBody2[6]); + + void computeLowerBoundPenetration(decimal& lowerBound); + void computeLowerBoundFriction1(decimal& lowerBound); + void computeLowerBoundFriction2(decimal& lowerBound); + + void computeUpperBoundPenetration(decimal& upperBound); + void computeUpperBoundFriction1(decimal& upperBound); + void computeUpperBoundFriction2(decimal& upperBound); + // Return the penetration depth decimal getPenetrationDepth() const; diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp index b11cb9cb..74b87374 100644 --- a/src/engine/ConstraintSolver.cpp +++ b/src/engine/ConstraintSolver.cpp @@ -34,7 +34,7 @@ using namespace std; // Constructor ConstraintSolver::ConstraintSolver(DynamicsWorld* world) - :world(world), nbConstraints(0), nbIterations(10) { + :world(world), nbConstraints(0), mNbIterations(10), mContactConstraints(0) { } @@ -51,107 +51,125 @@ void ConstraintSolver::initialize() { // TOOD : Use better allocation here mContactConstraints = new ContactConstraint[world->getNbContactManifolds()]; - uint nbContactConstraints = 0; + mNbContactConstraints = 0; // For each contact manifold of the world vector::iterator it; for (it = world->getContactManifoldsBeginIterator(); it != world->getContactManifoldsEndIterator(); ++it) { ContactManifold contactManifold = *it; + ContactConstraint& constraint = mContactConstraints[mNbContactConstraints]; + + assert(contactManifold.nbContacts > 0); + + RigidBody* body1 = contactManifold.contacts[0]->getBody1(); + RigidBody* body2 = contactManifold.contacts[0]->getBody2(); + + // Fill in the body number maping + mMapBodyToIndex.insert(make_pair(body1, mMapBodyToIndex.size())); + mMapBodyToIndex.insert(make_pair(body2, mMapBodyToIndex.size())); + + // Add the two bodies of the constraint in the constraintBodies list + mConstraintBodies.insert(body1); + mConstraintBodies.insert(body2); + + constraint.indexBody1 = mMapBodyToIndex[body1]; + constraint.indexBody2 = mMapBodyToIndex[body2]; + constraint.inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld(); + constraint.inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld(); + constraint.isBody1Moving = body1->getIsMotionEnabled(); + constraint.isBody2Moving = body2->getIsMotionEnabled(); + constraint.massInverseBody1 = body1->getMassInverse(); + constraint.massInverseBody2 = body2->getMassInverse(); + constraint.nbContacts = contactManifold.nbContacts; + // For each contact point of the contact manifold for (uint c=0; cisActive()) { - - RigidBody* body1 = contact->getBody1(); - RigidBody* body2 = contact->getBody2(); - - activeConstraints.push_back(contact); - - // Add the two bodies of the constraint in the constraintBodies list - mConstraintBodies.insert(body1); - mConstraintBodies.insert(body2); - - // Fill in the body number maping - mMapBodyToIndex.insert(make_pair(body1, mMapBodyToIndex.size())); - mMapBodyToIndex.insert(make_pair(body2, mMapBodyToIndex.size())); - - // Update the size of the jacobian matrix - nbConstraints += contact->getNbConstraints(); - - ContactConstraint constraint = mContactConstraints[nbContactConstraints]; - constraint.indexBody1 = mMapBodyToIndex[body1]; - constraint.indexBody2 = mMapBodyToIndex[body2]; - constraint.inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld(); - constraint.inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld(); - constraint.isBody1Moving = body1->getIsMotionEnabled(); - constraint.isBody2Moving = body2->getIsMotionEnabled(); - constraint.massInverseBody1 = body1->getMassInverse(); - constraint.massInverseBody2 = body2->getMassInverse(); - - nbContactConstraints++; - } - + constraint.contacts[c].contact = contact; } + + mNbContactConstraints++; } // Compute the number of bodies that are part of some active constraint - nbBodies = mMapBodyToIndex.size(); + nbBodies = mConstraintBodies.size(); - assert(nbConstraints > 0 && nbConstraints <= NB_MAX_CONSTRAINTS); - assert(nbBodies > 0 && nbBodies <= NB_MAX_BODIES); + assert(mMapBodyToIndex.size() == nbBodies); } // Fill in all the matrices needed to solve the LCP problem // Notice that all the active constraints should have been evaluated first void ConstraintSolver::fillInMatrices(decimal dt) { - Constraint* constraint; decimal oneOverDt = 1.0 / dt; - - // For each active constraint - int noConstraint = 0; - for (int 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 J_sp matrix - constraint->computeJacobian(noConstraint, J_sp); + // 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 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); + realContact->computeLowerBoundFriction2(contact.lowerBoundFriction2); + realContact->computeUpperBoundPenetration(contact.upperBoundPenetration); + realContact->computeUpperBoundFriction1(contact.upperBoundFriction1); + realContact->computeUpperBoundFriction2(contact.upperBoundFriction2); - // Fill in the limit vectors for the constraint - constraint->computeLowerBound(noConstraint, lowerBounds); - constraint->computeUpperBound(noConstraint, upperBounds); + // Fill in the error vector + realContact->computeErrorPenetration(contact.errorPenetration); + contact.errorFriction1 = 0.0; + contact.errorFriction2 = 0.0; - // Fill in the error vector - constraint->computeErrorValue(noConstraint, errorValues); + // 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); + // } - // Get the cached lambda values of the constraint - for (int i=0; igetNbConstraints(); i++) { - lambdaInit[noConstraint + i] = constraint->getCachedLambda(i); - } - - // If the constraint is a contact - if (constraint->getType() == CONTACT) { - Contact* contact = dynamic_cast(constraint); - - // Add the Baumgarte error correction term for contacts + contact.errorPenetration = 0.0; decimal slop = 0.005; - if (contact->getPenetrationDepth() > slop) { - errorValues[noConstraint] += 0.2 * oneOverDt * std::max(double(contact->getPenetrationDepth() - slop), 0.0); + if (realContact->getPenetrationDepth() > slop) { + contact.errorPenetration += 0.2 * oneOverDt * std::max(double(realContact->getPenetrationDepth() - slop), 0.0); } + contact.errorFriction1 = 0.0; + contact.errorFriction2 = 0.0; + + /* + // If the constraint is a contact + if (constraint->getType() == CONTACT) { + Contact* contact = dynamic_cast(constraint); + + // Add the Baumgarte error correction term for contacts + decimal slop = 0.005; + if (contact->getPenetrationDepth() > slop) { + errorValues[noConstraint] += 0.2 * oneOverDt * std::max(double(contact->getPenetrationDepth() - slop), 0.0); + } + } + */ } - - noConstraint += constraint->getNbConstraints(); } // For each current body that is implied in some constraint @@ -212,45 +230,126 @@ void ConstraintSolver::computeVectorB(decimal dt) { uint indexBody1, indexBody2; decimal oneOverDT = 1.0 / dt; - for (uint c = 0; cgetNbConstraints(); i++) { - // Get the lambda value that have just been computed - activeConstraints[c]->setCachedLambda(i, lambda[noConstraint + i]); + ContactConstraint& constraint = mContactConstraints[c]; + + for (uint i=0; isetCachedLambda(0, contact.penetrationImpulse); + contact.contact->setCachedLambda(1, contact.friction1Impulse); + contact.contact->setCachedLambda(2, contact.friction2Impulse); } - - noConstraint += activeConstraints[c]->getNbConstraints(); } } diff --git a/src/engine/ConstraintSolver.h b/src/engine/ConstraintSolver.h index febb51bc..8f8e2fdb 100644 --- a/src/engine/ConstraintSolver.h +++ b/src/engine/ConstraintSolver.h @@ -27,6 +27,7 @@ #define CONSTRAINT_SOLVER_H // Libraries +#include "../constraint/Contact.h" #include "../configuration.h" #include "../constraint/Constraint.h" #include @@ -63,6 +64,34 @@ struct ContactPointConstraint { decimal inversePenetrationMass; // Inverse of the matrix K for the penenetration decimal inverseFriction1Mass; // Inverse of the matrix K for the 1st friction decimal inverseFriction2Mass; // Inverse of the matrix K for the 2nd friction + decimal J_spBody1Penetration[6]; + decimal J_spBody2Penetration[6]; + decimal J_spBody1Friction1[6]; + decimal J_spBody2Friction1[6]; + decimal J_spBody1Friction2[6]; + decimal J_spBody2Friction2[6]; + decimal lowerBoundPenetration; + decimal upperBoundPenetration; + decimal lowerBoundFriction1; + decimal upperBoundFriction1; + decimal lowerBoundFriction2; + decimal upperBoundFriction2; + decimal errorPenetration; + decimal errorFriction1; + decimal errorFriction2; + Contact* contact; // TODO : REMOVE THIS + decimal b_Penetration; + decimal b_Friction1; + decimal b_Friction2; + decimal B_spBody1Penetration[6]; + decimal B_spBody2Penetration[6]; + decimal B_spBody1Friction1[6]; + decimal B_spBody2Friction1[6]; + decimal B_spBody1Friction2[6]; + decimal B_spBody2Friction2[6]; + decimal d_Penetration; + decimal d_Friction1; + decimal d_Friction2; }; // Structure ContactConstraint @@ -114,7 +143,7 @@ class ConstraintSolver { DynamicsWorld* world; // Reference to the world std::vector activeConstraints; // Current active constraints in the physics world bool isErrorCorrectionActive; // True if error correction (with world order) is active - uint nbIterations; // Number of iterations of the LCP solver + uint mNbIterations; // Number of iterations of the LCP solver uint nbIterationsLCPErrorCorrection; // Number of iterations of the LCP solver for error correction uint nbConstraints; // Total number of constraints (with the auxiliary constraints) uint nbConstraintsError; // Number of constraints for error correction projection (only contact constraints) @@ -158,6 +187,9 @@ class ConstraintSolver { // Contact constraints ContactConstraint* mContactConstraints; + // Number of contact constraints + uint mNbContactConstraints; + // Constrained bodies std::set mConstraintBodies; @@ -182,7 +214,7 @@ class ConstraintSolver { 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 nbIterations); // Set the number of iterations of the LCP solver + void setNbLCPIterations(uint mNbIterations); // Set the number of iterations of the LCP solver }; // Return true if the body is in at least one constraint @@ -209,11 +241,16 @@ inline void ConstraintSolver::cleanup() { mMapBodyToIndex.clear(); mConstraintBodies.clear(); activeConstraints.clear(); + + if (mContactConstraints != 0) { + delete[] mContactConstraints; + mContactConstraints = 0; + } } // Set the number of iterations of the LCP solver inline void ConstraintSolver::setNbLCPIterations(uint nbIterations) { - nbIterations = nbIterations; + mNbIterations = nbIterations; } // Solve the current LCP problem