From a0800ac33d15b55ed4cc8b01db7698ea41b9243f Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 10 Dec 2012 07:52:57 +0100 Subject: [PATCH 01/26] Start to replace PGS solver by sequential impulse and improve of persistent contact cache --- src/configuration.h | 4 +- src/engine/ConstraintSolver.cpp | 261 ++++++-------------------- src/engine/ConstraintSolver.h | 131 +++++++------ src/engine/DynamicsWorld.cpp | 50 +---- src/engine/DynamicsWorld.h | 41 ++-- src/engine/PersistentContactCache.cpp | 43 +++-- 6 files changed, 191 insertions(+), 339 deletions(-) diff --git a/src/configuration.h b/src/configuration.h index a5be511f..fd26fb72 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -79,8 +79,8 @@ const reactphysics3d::decimal OBJECT_MARGIN = 0.04; // Friction coefficient const reactphysics3d::decimal FRICTION_COEFFICIENT = 0.4; -// Distance threshold for two contact points for a valid persistent contact -const reactphysics3d::decimal PERSISTENT_CONTACT_DIST_THRESHOLD = 0.02; +// Distance threshold for two contact points for a valid persistent contact (in meters) +const reactphysics3d::decimal PERSISTENT_CONTACT_DIST_THRESHOLD = 0.03; // Maximum number of bodies const int NB_MAX_BODIES = 100000; diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp index cd98bb64..97c95107 100644 --- a/src/engine/ConstraintSolver.cpp +++ b/src/engine/ConstraintSolver.cpp @@ -34,9 +34,7 @@ using namespace std; // Constructor ConstraintSolver::ConstraintSolver(DynamicsWorld* world) - :world(world), nbConstraints(0), nbIterationsLCP(DEFAULT_LCP_ITERATIONS), - nbIterationsLCPErrorCorrection(DEFAULT_LCP_ITERATIONS_ERROR_CORRECTION), - isErrorCorrectionActive(false) { + :world(world), nbConstraints(0), nbIterations(10) { } @@ -47,43 +45,56 @@ ConstraintSolver::~ConstraintSolver() { // Initialize the constraint solver before each solving void ConstraintSolver::initialize() { - Constraint* constraint; nbConstraints = 0; - nbConstraintsError = 0; + + // TOOD : Use better allocation here + mContactConstraints = new ContactConstraint[world->getNbContactConstraints()]; + + uint nbContactConstraints = 0; // For each constraint - vector::iterator it; - for (it = world->getConstraintsBeginIterator(); it != world->getConstraintsEndIterator(); ++it) { - constraint = *it; + vector::iterator it; + for (it = world->getContactConstraintsBeginIterator(); it != world->getContactConstraintsEndIterator(); ++it) { + Contact* contact = *it; // If the constraint is active - if (constraint->isActive()) { - activeConstraints.push_back(constraint); + if (contact->isActive()) { + + RigidBody* body1 = contact->getBody1(); + RigidBody* body2 = contact->getBody2(); + + activeConstraints.push_back(contact); // Add the two bodies of the constraint in the constraintBodies list - constraintBodies.insert(constraint->getBody1()); - constraintBodies.insert(constraint->getBody2()); + mConstraintBodies.insert(body1); + mConstraintBodies.insert(body2); // Fill in the body number maping - bodyNumberMapping.insert(pair(constraint->getBody1(), bodyNumberMapping.size())); - bodyNumberMapping.insert(pair(constraint->getBody2(), bodyNumberMapping.size())); + mMapBodyToIndex.insert(make_pair(body1, mMapBodyToIndex.size())); + mMapBodyToIndex.insert(make_pair(body2, mMapBodyToIndex.size())); // Update the size of the jacobian matrix - nbConstraints += constraint->getNbConstraints(); + nbConstraints += contact->getNbConstraints(); - // Update the size of the jacobian matrix for error correction projection - if (constraint->getType() == CONTACT) { - nbConstraintsError++; - } + 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++; } } // Compute the number of bodies that are part of some active constraint - nbBodies = bodyNumberMapping.size(); + nbBodies = mMapBodyToIndex.size(); assert(nbConstraints > 0 && nbConstraints <= NB_MAX_CONSTRAINTS); - assert(nbConstraintsError > 0 && nbConstraintsError <= NB_MAX_CONSTRAINTS); assert(nbBodies > 0 && nbBodies <= NB_MAX_BODIES); } @@ -95,7 +106,6 @@ void ConstraintSolver::fillInMatrices(decimal dt) { // For each active constraint int noConstraint = 0; - int noConstraintError = 0; for (int c=0; cgetType() == CONTACT) { Contact* contact = dynamic_cast(constraint); - decimal penetrationDepth = contact->getPenetrationDepth(); - // If error correction with projection is active - if (isErrorCorrectionActive) { - - // Fill in the error correction projection parameters - lowerBoundsError[noConstraintError] = lowerBounds[noConstraint]; - upperBoundsError[noConstraintError] = upperBounds[noConstraint]; - for (int i=0; i<12; i++) { - J_spError[noConstraintError][i] = J_sp[noConstraint][i]; - } - bodyMappingError[noConstraintError][0] = constraint->getBody1(); - bodyMappingError[noConstraintError][1] = constraint->getBody2(); - penetrationDepths[noConstraintError] = contact->getPenetrationDepth(); - - // If the penetration depth is small - if (penetrationDepth < PENETRATION_DEPTH_THRESHOLD_ERROR_CORRECTION) { - // Use the Baumgarte error correction term for contacts instead of - // first order world projection - errorValues[noConstraint] += 0.1 * oneOverDt * contact->getPenetrationDepth(); - } - - noConstraintError++; - } - else { // If error correction with projection is not active - // Add the Baumgarte error correction term for contacts - errorValues[noConstraint] += 0.1 * oneOverDt * contact->getPenetrationDepth(); + // 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); } } @@ -160,11 +148,11 @@ void ConstraintSolver::fillInMatrices(decimal dt) { // For each current body that is implied in some constraint RigidBody* rigidBody; - Body* body; + RigidBody* body; uint b=0; - for (set::iterator it = constraintBodies.begin(); it != constraintBodies.end(); ++it, b++) { + for (set::iterator it = mConstraintBodies.begin(); it != mConstraintBodies.end(); ++it, b++) { body = *it; - uint bodyNumber = bodyNumberMapping[body]; + uint bodyNumber = mMapBodyToIndex[body]; // TODO : Use polymorphism and remove this downcasting rigidBody = dynamic_cast(body); @@ -189,16 +177,6 @@ void ConstraintSolver::fillInMatrices(decimal dt) { Vconstraint[bodyIndexArray + 4] = 0.0; Vconstraint[bodyIndexArray + 5] = 0.0; - // Compute the vector Vconstraint with final constraint velocities - if (isErrorCorrectionActive) { - VconstraintError[bodyIndexArray] = 0.0; - VconstraintError[bodyIndexArray + 1] = 0.0; - VconstraintError[bodyIndexArray + 2] = 0.0; - VconstraintError[bodyIndexArray + 3] = 0.0; - VconstraintError[bodyIndexArray + 4] = 0.0; - VconstraintError[bodyIndexArray + 5] = 0.0; - } - // Compute the vector with forces and torques values Vector3 externalForce = rigidBody->getExternalForce(); Vector3 externalTorque = rigidBody->getExternalTorque(); @@ -232,8 +210,8 @@ void ConstraintSolver::computeVectorB(decimal dt) { b[c] = errorValues[c] * oneOverDT; // Substract 1.0/dt*J*V to the vector b - indexBody1 = bodyNumberMapping[bodyMapping[c][0]]; - indexBody2 = bodyNumberMapping[bodyMapping[c][1]]; + indexBody1 = mMapBodyToIndex[bodyMapping[c][0]]; + indexBody2 = mMapBodyToIndex[bodyMapping[c][1]]; decimal multiplication = 0.0; int body1ArrayIndex = 6 * indexBody1; int body2ArrayIndex = 6 * indexBody2; @@ -268,22 +246,6 @@ void ConstraintSolver::computeVectorB(decimal dt) { } } -// Compute the vector b for error correction projection -void ConstraintSolver::computeVectorBError(decimal dt) { - decimal oneOverDT = 1.0 / dt; - - for (uint c = 0; c= PENETRATION_DEPTH_THRESHOLD_ERROR_CORRECTION) { - bError[c] = penetrationDepths[c] * oneOverDT; - } - else { - bError[c] = 0.0; - } - } -} - // Compute the matrix B_sp void ConstraintSolver::computeMatrixB_sp() { uint indexConstraintArray, indexBody1, indexBody2; @@ -292,8 +254,8 @@ void ConstraintSolver::computeMatrixB_sp() { for (uint c = 0; c activeConstraints; // Current active constraints in the physics world bool isErrorCorrectionActive; // True if error correction (with world order) is active - uint nbIterationsLCP; // Number of iterations of the LCP solver + uint nbIterations; // 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) uint nbBodies; // Current number of bodies in the physics world - std::set constraintBodies; // Bodies that are implied in some constraint - std::map bodyNumberMapping; // Map a body pointer with its index number - Body* bodyMapping[NB_MAX_CONSTRAINTS][2]; // 2-dimensional array that contains the mapping of body reference + 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 - Body* bodyMappingError[NB_MAX_CONSTRAINTS][2]; // Same as bodyMapping but for error correction projection 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 @@ -112,89 +154,68 @@ class ConstraintSolver { decimal VconstraintError[6*NB_MAX_BODIES]; // Same kind of vector as V1 but contains the final constraint velocities decimal Fext[6*NB_MAX_BODIES]; // Array that contains for each body the 6x1 vector that contains external forces and torques // Each cell contains a 6x1 vector with external force and torque. + + // Contact constraints + ContactConstraint* mContactConstraints; + + // Constrained bodies + std::set mConstraintBodies; + + // Map body to index + std::map mMapBodyToIndex; + + void initialize(); // Initialize the constraint solver before each solving void fillInMatrices(decimal dt); // Fill in all the matrices needed to solve the LCP problem void computeVectorB(decimal dt); // Compute the vector b - void computeVectorBError(decimal dt); // Compute the vector b for error correction projection void computeMatrixB_sp(); // Compute the matrix B_sp - void computeMatrixB_spErrorCorrection(); // Compute the matrix B_spError for error correction projection void computeVectorVconstraint(decimal dt); // Compute the vector V2 - void computeVectorVconstraintError(decimal dt); // Same as computeVectorVconstraint() but for error correction projection void cacheLambda(); // Cache the lambda values in order to reuse them in the next step to initialize the lambda vector void computeVectorA(); // Compute the vector a used in the solve() method - void computeVectorAError(); // Same as computeVectorA() but for error correction projection void solveLCP(); // Solve a LCP problem using Projected-Gauss-Seidel algorithm - void solveLCPErrorCorrection(); // Solve the LCP problem for error correction projection public: ConstraintSolver(DynamicsWorld* world); // Constructor virtual ~ConstraintSolver(); // Destructor void solve(decimal dt); // Solve the current LCP problem - bool isConstrainedBody(Body* body) const; // Return true if the body is in at least one constraint - Vector3 getConstrainedLinearVelocityOfBody(Body* body); // Return the constrained linear velocity of a body after solving the LCP problem - Vector3 getConstrainedAngularVelocityOfBody(Body* body); // Return the constrained angular velocity of a body after solving the LCP problem - Vector3 getErrorConstrainedLinearVelocityOfBody(Body* body); // Return the constrained linear velocity of a body after solving the LCP problem for error correction - Vector3 getErrorConstrainedAngularVelocityOfBody(Body* body); // Return the constrained angular velocity of a body after solving the LCP problem for error correction + 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 nbIterations); // Set the number of iterations of the LCP solver - void setIsErrorCorrectionActive(bool isErrorCorrectionActive); // Set the isErrorCorrectionActive value }; // Return true if the body is in at least one constraint -inline bool ConstraintSolver::isConstrainedBody(Body* body) const { - if(constraintBodies.find(body) != constraintBodies.end()) { - return true; - } - return false; +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 -inline Vector3 ConstraintSolver::getConstrainedLinearVelocityOfBody(Body* body) { +inline Vector3 ConstraintSolver::getConstrainedLinearVelocityOfBody(RigidBody* body) { assert(isConstrainedBody(body)); - uint indexBodyArray = 6 * bodyNumberMapping[body]; + uint indexBodyArray = 6 * mMapBodyToIndex[body]; return Vector3(Vconstraint[indexBodyArray], Vconstraint[indexBodyArray + 1], Vconstraint[indexBodyArray + 2]); } // Return the constrained angular velocity of a body after solving the LCP problem -inline Vector3 ConstraintSolver::getConstrainedAngularVelocityOfBody(Body* body) { +inline Vector3 ConstraintSolver::getConstrainedAngularVelocityOfBody(RigidBody *body) { assert(isConstrainedBody(body)); - uint indexBodyArray = 6 * bodyNumberMapping[body]; + uint indexBodyArray = 6 * mMapBodyToIndex[body]; return Vector3(Vconstraint[indexBodyArray + 3], Vconstraint[indexBodyArray + 4], Vconstraint[indexBodyArray + 5]); } -// Return the constrained linear velocity of a body after solving the LCP problem for error correction -inline Vector3 ConstraintSolver::getErrorConstrainedLinearVelocityOfBody(Body* body) { - //assert(isConstrainedBody(body)); - uint indexBodyArray = 6 * bodyNumberMapping[body]; - return Vector3(VconstraintError[indexBodyArray], VconstraintError[indexBodyArray + 1], VconstraintError[indexBodyArray + 2]); -} - -// Return the constrained angular velocity of a body after solving the LCP problem for error correction -inline Vector3 ConstraintSolver::getErrorConstrainedAngularVelocityOfBody(Body* body) { - //assert(isConstrainedBody(body)); - uint indexBodyArray = 6 * bodyNumberMapping[body]; - return Vector3(VconstraintError[indexBodyArray + 3], VconstraintError[indexBodyArray + 4], VconstraintError[indexBodyArray + 5]); -} - // Cleanup of the constraint solver inline void ConstraintSolver::cleanup() { - bodyNumberMapping.clear(); - constraintBodies.clear(); + mMapBodyToIndex.clear(); + mConstraintBodies.clear(); activeConstraints.clear(); } // Set the number of iterations of the LCP solver inline void ConstraintSolver::setNbLCPIterations(uint nbIterations) { - nbIterationsLCP = nbIterations; + nbIterations = nbIterations; } - -// Set the isErrorCorrectionActive value -inline void ConstraintSolver::setIsErrorCorrectionActive(bool isErrorCorrectionActive) { - this->isErrorCorrectionActive = isErrorCorrectionActive; -} - - // Solve the current LCP problem inline void ConstraintSolver::solve(decimal dt) { @@ -206,30 +227,18 @@ inline void ConstraintSolver::solve(decimal dt) { // Compute the vector b computeVectorB(dt); - if (isErrorCorrectionActive) { - computeVectorBError(dt); - } // Compute the matrix B computeMatrixB_sp(); - if (isErrorCorrectionActive) { - computeMatrixB_spErrorCorrection(); - } // Solve the LCP problem (computation of lambda) solveLCP(); - if (isErrorCorrectionActive) { - solveLCPErrorCorrection(); - } // Cache the lambda values in order to use them in the next step cacheLambda(); // Compute the vector Vconstraint computeVectorVconstraint(dt); - if (isErrorCorrectionActive) { - computeVectorVconstraintError(dt); - } } } // End of ReactPhysics3D namespace diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index 1c54d39b..35a5359f 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -65,7 +65,8 @@ void DynamicsWorld::update() { existCollision = false; - removeAllContactConstraints(); + // Remove all contact constraints + mContactConstraints.clear(); // Compute the collision detection if (mCollisionDetection.computeCollisionDetection()) { @@ -107,8 +108,6 @@ void DynamicsWorld::updateAllBodiesMotion() { decimal dt = mTimer.getTimeStep(); Vector3 newLinearVelocity; Vector3 newAngularVelocity; - Vector3 linearVelocityErrorCorrection; - Vector3 angularVelocityErrorCorrection; // For each body of thephysics world for (set::iterator it=getRigidBodiesBeginIterator(); it != getRigidBodiesEndIterator(); ++it) { @@ -120,17 +119,12 @@ void DynamicsWorld::updateAllBodiesMotion() { if (rigidBody->getIsMotionEnabled()) { newLinearVelocity.setAllValues(0.0, 0.0, 0.0); newAngularVelocity.setAllValues(0.0, 0.0, 0.0); - linearVelocityErrorCorrection.setAllValues(0.0, 0.0, 0.0); - angularVelocityErrorCorrection.setAllValues(0.0, 0.0, 0.0); // If it's a constrained body if (mConstraintSolver.isConstrainedBody(*it)) { // Get the constrained linear and angular velocities from the constraint solver newLinearVelocity = mConstraintSolver.getConstrainedLinearVelocityOfBody(*it); newAngularVelocity = mConstraintSolver.getConstrainedAngularVelocityOfBody(*it); - - linearVelocityErrorCorrection = mConstraintSolver.getErrorConstrainedLinearVelocityOfBody(rigidBody); - angularVelocityErrorCorrection = mConstraintSolver.getErrorConstrainedAngularVelocityOfBody(rigidBody); } // Compute V_forces = dt * (M^-1 * F_ext) which is the velocity of the body due to the @@ -143,8 +137,7 @@ void DynamicsWorld::updateAllBodiesMotion() { newAngularVelocity += rigidBody->getAngularVelocity(); // Update the position and the orientation of the body according to the new velocity - updatePositionAndOrientationOfBody(*it, newLinearVelocity, newAngularVelocity, - linearVelocityErrorCorrection, angularVelocityErrorCorrection); + updatePositionAndOrientationOfBody(*it, newLinearVelocity, newAngularVelocity); // Update the AABB of the rigid body rigidBody->updateAABB(); @@ -155,8 +148,9 @@ void DynamicsWorld::updateAllBodiesMotion() { // Update the position and orientation of a body // Use the Semi-Implicit Euler (Sympletic Euler) method to compute the new position and the new // orientation of the body -void DynamicsWorld::updatePositionAndOrientationOfBody(RigidBody* rigidBody, const Vector3& newLinVelocity, const Vector3& newAngVelocity, - const Vector3& linearVelocityErrorCorrection, const Vector3& angularVelocityErrorCorrection) { +void DynamicsWorld::updatePositionAndOrientationOfBody(RigidBody* rigidBody, + const Vector3& newLinVelocity, + const Vector3& newAngVelocity) { decimal dt = mTimer.getTimeStep(); assert(rigidBody); @@ -171,13 +165,9 @@ void DynamicsWorld::updatePositionAndOrientationOfBody(RigidBody* rigidBody, con // Get current position and orientation of the body const Vector3& currentPosition = rigidBody->getTransform().getPosition(); const Quaternion& currentOrientation = rigidBody->getTransform().getOrientation(); - - // Error correction projection - Vector3 correctedPosition = currentPosition + dt * linearVelocityErrorCorrection; - Quaternion correctedOrientation = currentOrientation + Quaternion(angularVelocityErrorCorrection.getX(), angularVelocityErrorCorrection.getY(), angularVelocityErrorCorrection.getZ(), 0) * currentOrientation * 0.5 * dt; - - Vector3 newPosition = correctedPosition + newLinVelocity * dt; - Quaternion newOrientation = correctedOrientation + Quaternion(newAngVelocity.getX(), newAngVelocity.getY(), newAngVelocity.getZ(), 0) * correctedOrientation * 0.5 * dt; + + Vector3 newPosition = currentPosition + newLinVelocity * dt; + Quaternion newOrientation = currentOrientation + Quaternion(newAngVelocity.getX(), newAngVelocity.getY(), newAngVelocity.getZ(), 0) * currentOrientation * 0.5 * dt; Transform newTransform(newPosition, newOrientation.getUnit()); rigidBody->setTransform(newTransform); @@ -262,26 +252,6 @@ void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) { mMemoryPoolRigidBodies.freeObject(rigidBody); } -// Remove all collision contacts constraints -// TODO : This method should be in the collision detection class -void DynamicsWorld::removeAllContactConstraints() { - // For all constraints - for (vector::iterator it = mConstraints.begin(); it != mConstraints.end(); ) { - - // Try a downcasting - Contact* contact = dynamic_cast(*it); - - // If the constraint is a contact - if (contact) { - // Remove it from the constraints of the physics world - it = mConstraints.erase(it); - } - else { - ++it; - } - } -} - // Remove all constraints in the physics world void DynamicsWorld::removeAllConstraints() { mConstraints.clear(); @@ -335,6 +305,6 @@ void DynamicsWorld::notifyNewContact(const BroadPhasePair* broadPhasePair, const // Add all the contacts in the contact cache of the two bodies // to the set of constraints in the physics world for (uint i=0; igetNbContacts(); i++) { - addConstraint(overlappingPair->getContact(i)); + mContactConstraints.push_back(overlappingPair->getContact(i)); } } diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index e31d6fef..84e8122e 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -62,7 +62,10 @@ class DynamicsWorld : public CollisionWorld { // All the rigid bodies of the physics world std::set mRigidBodies; - // List that contains all the current constraints + // All the contact constraints + std::vector mContactConstraints; + + // All the constraints (except contact constraints) std::vector mConstraints; // Gravity vector of the world @@ -93,9 +96,7 @@ class DynamicsWorld : public CollisionWorld { // Update the position and orientation of a body void updatePositionAndOrientationOfBody(RigidBody* body, const Vector3& newLinVelocity, - const Vector3& newAngVelocity, - const Vector3& linearVelocityErrorCorrection, - const Vector3& angularVelocityErrorCorrection); + const Vector3& newAngVelocity); // Compute and set the interpolation factor to all bodies void setInterpolationFactorToAllBodies(); @@ -166,18 +167,24 @@ public : // Remove a constraint void removeConstraint(Constraint* constraint); - // Remove all collision contacts constraints - void removeAllContactConstraints(); - // Remove all constraints and delete them (free their memory) void removeAllConstraints(); + // Return the number of contact constraints in the world + uint getNbContactConstraints() const; + // Return a start iterator on the constraint list std::vector::iterator getConstraintsBeginIterator(); // Return a end iterator on the constraint list std::vector::iterator getConstraintsEndIterator(); + // Return a start iterator on the contact constraint list + std::vector::iterator getContactConstraintsBeginIterator(); + + // Return a end iterator on the contact constraint list + std::vector::iterator getContactConstraintsEndIterator(); + // Return an iterator to the beginning of the rigid bodies of the physics world std::set::iterator getRigidBodiesBeginIterator(); @@ -202,11 +209,6 @@ inline void DynamicsWorld::setNbLCPIterations(uint nbIterations) { mConstraintSolver.setNbLCPIterations(nbIterations); } -// Set the isErrorCorrectionActive value -inline void DynamicsWorld::setIsErrorCorrectionActive(bool isErrorCorrectionActive) { - mConstraintSolver.setIsErrorCorrectionActive(isErrorCorrectionActive); -} - // Reset the boolean movement variable of each body inline void DynamicsWorld::resetBodiesMovementVariable() { @@ -275,6 +277,11 @@ inline std::set::iterator DynamicsWorld::getRigidBodiesEndIterator() return mRigidBodies.end(); } +// Return the number of contact constraints in the world +inline uint DynamicsWorld::getNbContactConstraints() const { + return mContactConstraints.size(); +} + // Return a start iterator on the constraint list inline std::vector::iterator DynamicsWorld::getConstraintsBeginIterator() { return mConstraints.begin(); @@ -285,6 +292,16 @@ inline std::vector::iterator DynamicsWorld::getConstraintsEndIterat return mConstraints.end(); } +// Return a start iterator on the contact constraint list +inline std::vector::iterator DynamicsWorld::getContactConstraintsBeginIterator() { + return mContactConstraints.begin(); +} + +// Return a end iterator on the contact constraint list +inline std::vector::iterator DynamicsWorld::getContactConstraintsEndIterator() { + return mContactConstraints.end(); +} + } #endif diff --git a/src/engine/PersistentContactCache.cpp b/src/engine/PersistentContactCache.cpp index 36bdbc45..49b7490d 100644 --- a/src/engine/PersistentContactCache.cpp +++ b/src/engine/PersistentContactCache.cpp @@ -41,19 +41,22 @@ PersistentContactCache::~PersistentContactCache() { // Add a contact in the cache void PersistentContactCache::addContact(Contact* contact) { - - int indexNewContact = mNbContacts; // For contact already in the cache for (uint i=0; igetLocalPointOnBody1(), mContacts[i]->getLocalPointOnBody1())) { - // Delete the new contact - contact->Contact::~Contact(); - mMemoryPoolContacts.freeObject(contact); - - return; + // already in the cache. + decimal distance = (mContacts[i]->getWorldPointOnBody1() - contact->getWorldPointOnBody1()).lengthSquare(); + if (distance <= PERSISTENT_CONTACT_DIST_THRESHOLD*PERSISTENT_CONTACT_DIST_THRESHOLD) { + + // Delete the new contact + contact->Contact::~Contact(); + mMemoryPoolContacts.freeObject(contact); + //removeContact(i); + + return; + //break; } } @@ -62,11 +65,10 @@ void PersistentContactCache::addContact(Contact* contact) { int indexMaxPenetration = getIndexOfDeepestPenetration(contact); int indexToRemove = getIndexToRemove(indexMaxPenetration, contact->getLocalPointOnBody1()); removeContact(indexToRemove); - indexNewContact = indexToRemove; } // Add the new contact in the cache - mContacts[indexNewContact] = contact; + mContacts[mNbContacts] = contact; mNbContacts++; } @@ -104,21 +106,28 @@ void PersistentContactCache::update(const Transform& transform1, const Transform mContacts[i]->setPenetrationDepth((mContacts[i]->getWorldPointOnBody1() - mContacts[i]->getWorldPointOnBody2()).dot(mContacts[i]->getNormal())); } + const decimal squarePersistentContactThreshold = PERSISTENT_CONTACT_DIST_THRESHOLD * + PERSISTENT_CONTACT_DIST_THRESHOLD; + // Remove the contacts that don't represent very well the persistent contact for (int i=mNbContacts-1; i>=0; i--) { assert(i>= 0 && i < mNbContacts); + + // Compute the distance between contact points in the normal direction + decimal distanceNormal = -mContacts[i]->getPenetrationDepth(); - // Remove the contacts with a negative penetration depth (meaning that the bodies are not penetrating anymore) - if (mContacts[i]->getPenetrationDepth() <= 0.0) { + // If the contacts points are too far from each other in the normal direction + if (distanceNormal > squarePersistentContactThreshold) { removeContact(i); } else { - // Compute the distance of the two contact points in the place orthogonal to the contact normal - Vector3 projOfPoint1 = mContacts[i]->getWorldPointOnBody1() - mContacts[i]->getNormal() * mContacts[i]->getPenetrationDepth(); + // Compute the distance of the two contact points in the plane orthogonal to the contact normal + Vector3 projOfPoint1 = mContacts[i]->getWorldPointOnBody1() + + mContacts[i]->getNormal() * distanceNormal; Vector3 projDifference = mContacts[i]->getWorldPointOnBody2() - projOfPoint1; // If the orthogonal distance is larger than the valid distance threshold, we remove the contact - if (projDifference.lengthSquare() > PERSISTENT_CONTACT_DIST_THRESHOLD * PERSISTENT_CONTACT_DIST_THRESHOLD) { + if (projDifference.lengthSquare() > squarePersistentContactThreshold) { removeContact(i); } } @@ -224,4 +233,4 @@ void PersistentContactCache::clear() { mMemoryPoolContacts.freeObject(mContacts[i]); } mNbContacts = 0; -} \ No newline at end of file +} From 2fc8beaa77f23142a03f91b66b6aeffedeeff3c3 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Tue, 11 Dec 2012 23:09:54 +0100 Subject: [PATCH 02/26] Fix two compilation errors --- src/collision/narrowphase/EPA/EdgeEPA.h | 1 + src/engine/Timer.h | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/src/collision/narrowphase/EPA/EdgeEPA.h b/src/collision/narrowphase/EPA/EdgeEPA.h index cd9f06bf..bc3c64d2 100644 --- a/src/collision/narrowphase/EPA/EdgeEPA.h +++ b/src/collision/narrowphase/EPA/EdgeEPA.h @@ -105,6 +105,7 @@ inline int EdgeEPA::getIndex() const { inline EdgeEPA& EdgeEPA::operator=(const EdgeEPA& edge) { mOwnerTriangle = edge.mOwnerTriangle; mIndex = edge.mIndex; + return *this; } // Return the index of the next counter-clockwise edge of the ownver triangle diff --git a/src/engine/Timer.h b/src/engine/Timer.h index aab7160a..474481ac 100644 --- a/src/engine/Timer.h +++ b/src/engine/Timer.h @@ -153,7 +153,7 @@ inline void Timer::start() { LARGE_INTEGER ticks; QueryPerformanceFrequency(&ticksPerSecond); QueryPerformanceCounter(&ticks); - lastUpdateTime = double(ticks.QuadPart) / double(ticksPerSecond.QuadPart); + mLastUpdateTime = double(ticks.QuadPart) / double(ticksPerSecond.QuadPart); #else // Initialize the lastUpdateTime with the current time in seconds timeval timeValue; From 5e997f1c5cd95d14f4713d5604a38df04cf7fd49 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 12 Dec 2012 08:19:03 +0100 Subject: [PATCH 03/26] Add the class Contact that contains the contact points between two bodies --- src/engine/ConstraintSolver.cpp | 66 ++++++++++++++++++--------------- src/engine/DynamicsWorld.cpp | 29 +++++++++------ src/engine/DynamicsWorld.h | 31 ++++++++-------- 3 files changed, 70 insertions(+), 56 deletions(-) diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp index 97c95107..b11cb9cb 100644 --- a/src/engine/ConstraintSolver.cpp +++ b/src/engine/ConstraintSolver.cpp @@ -49,45 +49,53 @@ void ConstraintSolver::initialize() { nbConstraints = 0; // TOOD : Use better allocation here - mContactConstraints = new ContactConstraint[world->getNbContactConstraints()]; + mContactConstraints = new ContactConstraint[world->getNbContactManifolds()]; uint nbContactConstraints = 0; - // For each constraint - vector::iterator it; - for (it = world->getContactConstraintsBeginIterator(); it != world->getContactConstraintsEndIterator(); ++it) { - Contact* contact = *it; + // For each contact manifold of the world + vector::iterator it; + for (it = world->getContactManifoldsBeginIterator(); it != world->getContactManifoldsEndIterator(); ++it) { + ContactManifold contactManifold = *it; - // If the constraint is active - if (contact->isActive()) { + // For each contact point of the contact manifold + for (uint c=0; cgetBody1(); - RigidBody* body2 = contact->getBody2(); + // Get a contact point + Contact* contact = contactManifold.contacts[c]; - activeConstraints.push_back(contact); + // If the constraint is active + if (contact->isActive()) { - // Add the two bodies of the constraint in the constraintBodies list - mConstraintBodies.insert(body1); - mConstraintBodies.insert(body2); + RigidBody* body1 = contact->getBody1(); + RigidBody* body2 = contact->getBody2(); - // Fill in the body number maping - mMapBodyToIndex.insert(make_pair(body1, mMapBodyToIndex.size())); - mMapBodyToIndex.insert(make_pair(body2, mMapBodyToIndex.size())); + activeConstraints.push_back(contact); - // 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(); + // 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++; + } - nbContactConstraints++; } } diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index 35a5359f..b8d326c0 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -50,7 +50,6 @@ DynamicsWorld::~DynamicsWorld() { // Update the physics simulation void DynamicsWorld::update() { - bool existCollision = false; assert(mTimer.getIsRunning()); @@ -63,16 +62,16 @@ void DynamicsWorld::update() { // While the time accumulator is not empty while(mTimer.isPossibleToTakeStep()) { - existCollision = false; - - // Remove all contact constraints - mContactConstraints.clear(); + // Remove all contact manifolds + mContactManifolds.clear(); // Compute the collision detection - if (mCollisionDetection.computeCollisionDetection()) { - existCollision = true; + mCollisionDetection.computeCollisionDetection(); - // Solve constraints + // If there are constraints or contacts + if (!mConstraints.empty() || !mContactManifolds.empty()) { + + // Solve the constraints and contacts mConstraintSolver.solve(mTimer.getTimeStep()); } @@ -86,9 +85,7 @@ void DynamicsWorld::update() { updateAllBodiesMotion(); // Cleanup of the constraint solver - if (existCollision) { - mConstraintSolver.cleanup(); - } + mConstraintSolver.cleanup(); } // Compute and set the interpolation factor to all the bodies @@ -302,9 +299,17 @@ void DynamicsWorld::notifyNewContact(const BroadPhasePair* broadPhasePair, const // Add the contact to the contact cache of the corresponding overlapping pair overlappingPair->addContact(contact); + // Create a contact manifold with the contact points of the two bodies + ContactManifold contactManifold; + contactManifold.nbContacts = 0; + // Add all the contacts in the contact cache of the two bodies // to the set of constraints in the physics world for (uint i=0; igetNbContacts(); i++) { - mContactConstraints.push_back(overlappingPair->getContact(i)); + contactManifold.contacts[i] = overlappingPair->getContact(i); + contactManifold.nbContacts++; } + + // Add the contact manifold to the world + mContactManifolds.push_back(contactManifold); } diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index 84e8122e..73737ff9 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -28,6 +28,7 @@ // Libraries #include "CollisionWorld.h" +#include "ContactManifold.h" #include "../collision/CollisionDetection.h" #include "ConstraintSolver.h" #include "../body/RigidBody.h" @@ -63,7 +64,7 @@ class DynamicsWorld : public CollisionWorld { std::set mRigidBodies; // All the contact constraints - std::vector mContactConstraints; + std::vector mContactManifolds; // All the constraints (except contact constraints) std::vector mConstraints; @@ -171,7 +172,7 @@ public : void removeAllConstraints(); // Return the number of contact constraints in the world - uint getNbContactConstraints() const; + uint getNbContactManifolds() const; // Return a start iterator on the constraint list std::vector::iterator getConstraintsBeginIterator(); @@ -179,11 +180,11 @@ public : // Return a end iterator on the constraint list std::vector::iterator getConstraintsEndIterator(); - // Return a start iterator on the contact constraint list - std::vector::iterator getContactConstraintsBeginIterator(); + // Return a start iterator on the contact manifolds list + std::vector::iterator getContactManifoldsBeginIterator(); - // Return a end iterator on the contact constraint list - std::vector::iterator getContactConstraintsEndIterator(); + // Return a end iterator on the contact manifolds list + std::vector::iterator getContactManifoldsEndIterator(); // Return an iterator to the beginning of the rigid bodies of the physics world std::set::iterator getRigidBodiesBeginIterator(); @@ -277,9 +278,9 @@ inline std::set::iterator DynamicsWorld::getRigidBodiesEndIterator() return mRigidBodies.end(); } -// Return the number of contact constraints in the world -inline uint DynamicsWorld::getNbContactConstraints() const { - return mContactConstraints.size(); +// Return the number of contact manifolds in the world +inline uint DynamicsWorld::getNbContactManifolds() const { + return mContactManifolds.size(); } // Return a start iterator on the constraint list @@ -292,14 +293,14 @@ inline std::vector::iterator DynamicsWorld::getConstraintsEndIterat return mConstraints.end(); } -// Return a start iterator on the contact constraint list -inline std::vector::iterator DynamicsWorld::getContactConstraintsBeginIterator() { - return mContactConstraints.begin(); +// Return a start iterator on the contact manifolds list +inline std::vector::iterator DynamicsWorld::getContactManifoldsBeginIterator() { + return mContactManifolds.begin(); } -// Return a end iterator on the contact constraint list -inline std::vector::iterator DynamicsWorld::getContactConstraintsEndIterator() { - return mContactConstraints.end(); +// Return a end iterator on the contact manifolds list +inline std::vector::iterator DynamicsWorld::getContactManifoldsEndIterator() { + return mContactManifolds.end(); } } From 3259f545582e80252d25802f1395c2895ff359bf Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 12 Dec 2012 19:28:52 +0100 Subject: [PATCH 04/26] Add the ContactManifold file --- src/engine/ContactManifold.h | 55 ++++++++++++++++++++++++++++++++++++ 1 file changed, 55 insertions(+) create mode 100644 src/engine/ContactManifold.h diff --git a/src/engine/ContactManifold.h b/src/engine/ContactManifold.h new file mode 100644 index 00000000..ce5a7c67 --- /dev/null +++ b/src/engine/ContactManifold.h @@ -0,0 +1,55 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010-2012 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef CONTACT_MANIFOLD_H +#define CONTACT_MANIFOLD_H + +// Libraries +#include "../constraint/Contact.h" +#include "../configuration.h" + +namespace reactphysics3d { + +// Class ContactManifold +// This class contains several contact points between two bodies +struct ContactManifold { + + // -------------------- Attributes -------------------- // + + // Contact for each contact point + Contact* contacts[4]; // TODO : Use a constant here for the nb of contacts + + // Number of contacts in the manifold + uint nbContacts; + + // -------------------- Methods -------------------- // + + // Constructor + ContactManifold() : nbContacts(0) { } +}; + +} + +#endif From 7172ee4843a81118c0e7b60df9ce1c6e13b46a92 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sun, 16 Dec 2012 16:57:15 +0100 Subject: [PATCH 05/26] 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 From d615f9af12c5174b1a8f03a7c5341b911b85cb70 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Fri, 21 Dec 2012 08:39:21 +0100 Subject: [PATCH 06/26] Separate code for bodies initialization and contact constraints initialization --- src/engine/ConstraintSolver.cpp | 153 ++++++++++++++------------------ src/engine/ConstraintSolver.h | 11 ++- 2 files changed, 71 insertions(+), 93 deletions(-) diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp index 74b87374..b4b7ad0c 100644 --- a/src/engine/ConstraintSolver.cpp +++ b/src/engine/ConstraintSolver.cpp @@ -101,16 +101,75 @@ void ConstraintSolver::initialize() { assert(mMapBodyToIndex.size() == nbBodies); } +// Initialize bodies velocities +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++) { + body = *it; + uint bodyNumber = mMapBodyToIndex[body]; + + // TODO : Use polymorphism and remove this downcasting + rigidBody = dynamic_cast(body); + assert(rigidBody); + + // Compute the vector V1 with initial velocities values + Vector3 linearVelocity = rigidBody->getLinearVelocity(); + Vector3 angularVelocity = rigidBody->getAngularVelocity(); + int bodyIndexArray = 6 * bodyNumber; + V1[bodyIndexArray] = linearVelocity[0]; + V1[bodyIndexArray + 1] = linearVelocity[1]; + V1[bodyIndexArray + 2] = linearVelocity[2]; + V1[bodyIndexArray + 3] = angularVelocity[0]; + V1[bodyIndexArray + 4] = angularVelocity[1]; + V1[bodyIndexArray + 5] = angularVelocity[2]; + + // Compute the vector Vconstraint with final constraint velocities + Vconstraint[bodyIndexArray] = 0.0; + Vconstraint[bodyIndexArray + 1] = 0.0; + Vconstraint[bodyIndexArray + 2] = 0.0; + Vconstraint[bodyIndexArray + 3] = 0.0; + Vconstraint[bodyIndexArray + 4] = 0.0; + Vconstraint[bodyIndexArray + 5] = 0.0; + + // Compute the vector with forces and torques values + Vector3 externalForce = rigidBody->getExternalForce(); + Vector3 externalTorque = rigidBody->getExternalTorque(); + Fext[bodyIndexArray] = externalForce[0]; + Fext[bodyIndexArray + 1] = externalForce[1]; + Fext[bodyIndexArray + 2] = externalForce[2]; + Fext[bodyIndexArray + 3] = externalTorque[0]; + Fext[bodyIndexArray + 4] = externalTorque[1]; + Fext[bodyIndexArray + 5] = externalTorque[2]; + + // 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::fillInMatrices(decimal dt) { - decimal oneOverDt = 1.0 / dt; +void ConstraintSolver::initializeContactConstraints(decimal dt) { + decimal oneOverDT = 1.0 / dt; // For each contact constraint for (uint c=0; cgetPenetrationDepth() > slop) { - contact.errorPenetration += 0.2 * oneOverDt * std::max(double(realContact->getPenetrationDepth() - slop), 0.0); + 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); - } - } - */ - } - } - - // 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++) { - body = *it; - uint bodyNumber = mMapBodyToIndex[body]; - - // TODO : Use polymorphism and remove this downcasting - rigidBody = dynamic_cast(body); - assert(rigidBody); - - // Compute the vector V1 with initial velocities values - Vector3 linearVelocity = rigidBody->getLinearVelocity(); - Vector3 angularVelocity = rigidBody->getAngularVelocity(); - int bodyIndexArray = 6 * bodyNumber; - V1[bodyIndexArray] = linearVelocity[0]; - V1[bodyIndexArray + 1] = linearVelocity[1]; - V1[bodyIndexArray + 2] = linearVelocity[2]; - V1[bodyIndexArray + 3] = angularVelocity[0]; - V1[bodyIndexArray + 4] = angularVelocity[1]; - V1[bodyIndexArray + 5] = angularVelocity[2]; - - // Compute the vector Vconstraint with final constraint velocities - Vconstraint[bodyIndexArray] = 0.0; - Vconstraint[bodyIndexArray + 1] = 0.0; - Vconstraint[bodyIndexArray + 2] = 0.0; - Vconstraint[bodyIndexArray + 3] = 0.0; - Vconstraint[bodyIndexArray + 4] = 0.0; - Vconstraint[bodyIndexArray + 5] = 0.0; - - // Compute the vector with forces and torques values - Vector3 externalForce = rigidBody->getExternalForce(); - Vector3 externalTorque = rigidBody->getExternalTorque(); - Fext[bodyIndexArray] = externalForce[0]; - Fext[bodyIndexArray + 1] = externalForce[1]; - Fext[bodyIndexArray + 2] = externalForce[2]; - Fext[bodyIndexArray + 3] = externalTorque[0]; - Fext[bodyIndexArray + 4] = externalTorque[1]; - Fext[bodyIndexArray + 5] = externalTorque[2]; - - // 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(); - } - } -} - -// Compute the vector b -void ConstraintSolver::computeVectorB(decimal dt) { - uint indexBody1, indexBody2; - decimal oneOverDT = 1.0 / dt; - - for (uint c = 0; c Date: Fri, 21 Dec 2012 11:00:13 +0100 Subject: [PATCH 07/26] Use vectors in the Vconstraint array --- src/engine/ConstraintSolver.cpp | 44 +++++++++++++++++++-------------- src/engine/ConstraintSolver.h | 19 ++++++++++---- 2 files changed, 40 insertions(+), 23 deletions(-) diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp index b4b7ad0c..84a56705 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), mNbIterations(10), mContactConstraints(0) { + :world(world), nbConstraints(0), mNbIterations(100), mContactConstraints(0), Vconstraint(0), Wconstraint(0) { } @@ -98,6 +98,9 @@ void ConstraintSolver::initialize() { // Compute the number of bodies that are part of some active constraint nbBodies = mConstraintBodies.size(); + Vconstraint = new Vector3[nbBodies]; + Wconstraint = new Vector3[nbBodies]; + assert(mMapBodyToIndex.size() == nbBodies); } @@ -128,12 +131,8 @@ void ConstraintSolver::initializeBodies() { V1[bodyIndexArray + 5] = angularVelocity[2]; // Compute the vector Vconstraint with final constraint velocities - Vconstraint[bodyIndexArray] = 0.0; - Vconstraint[bodyIndexArray + 1] = 0.0; - Vconstraint[bodyIndexArray + 2] = 0.0; - Vconstraint[bodyIndexArray + 3] = 0.0; - Vconstraint[bodyIndexArray + 4] = 0.0; - Vconstraint[bodyIndexArray + 5] = 0.0; + Vconstraint[bodyNumber] = Vector3(0, 0, 0); + Wconstraint[bodyNumber] = Vector3(0, 0, 0); // Compute the vector with forces and torques values Vector3 externalForce = rigidBody->getExternalForce(); @@ -425,25 +424,34 @@ void ConstraintSolver::computeVectorVconstraint(decimal dt) { // ---------- Penetration ---------- // - indexBody1Array = 6 * constraint.indexBody1; - indexBody2Array = 6 * constraint.indexBody2; - for (j=0; j<6; j++) { - Vconstraint[indexBody1Array + j] += contact.B_spBody1Penetration[j] * contact.penetrationImpulse * dt; - Vconstraint[indexBody2Array + j] += contact.B_spBody2Penetration[j] * contact.penetrationImpulse * dt; + indexBody1Array = constraint.indexBody1; + indexBody2Array = constraint.indexBody2; + for (j=0; j<3; j++) { + Vconstraint[indexBody1Array][j] += contact.B_spBody1Penetration[j] * contact.penetrationImpulse * dt; + Wconstraint[indexBody1Array][j] += contact.B_spBody1Penetration[j + 3] * contact.penetrationImpulse * dt; + + Vconstraint[indexBody2Array][j] += contact.B_spBody2Penetration[j] * contact.penetrationImpulse * dt; + Wconstraint[indexBody2Array][j] += contact.B_spBody2Penetration[j + 3] * contact.penetrationImpulse * dt; } // ---------- Friction 1 ---------- // - for (j=0; j<6; j++) { - Vconstraint[indexBody1Array + j] += contact.B_spBody1Friction1[j] * contact.friction1Impulse * dt; - Vconstraint[indexBody2Array + j] += contact.B_spBody2Friction1[j] * contact.friction1Impulse * dt; + for (j=0; j<3; j++) { + Vconstraint[indexBody1Array][j] += contact.B_spBody1Friction1[j] * contact.friction1Impulse * dt; + Wconstraint[indexBody1Array][j] += contact.B_spBody1Friction1[j + 3] * contact.friction1Impulse * dt; + + Vconstraint[indexBody2Array][j] += contact.B_spBody2Friction1[j] * contact.friction1Impulse * dt; + Wconstraint[indexBody2Array][j] += contact.B_spBody2Friction1[j + 3] * contact.friction1Impulse * dt; } // ---------- Friction 2 ---------- // - for (j=0; j<6; j++) { - Vconstraint[indexBody1Array + j] += contact.B_spBody1Friction2[j] * contact.friction2Impulse * dt; - Vconstraint[indexBody2Array + j] += contact.B_spBody2Friction2[j] * contact.friction2Impulse * dt; + for (j=0; j<3; j++) { + Vconstraint[indexBody1Array][j] += contact.B_spBody1Friction2[j] * contact.friction2Impulse * dt; + Wconstraint[indexBody1Array][j] += contact.B_spBody1Friction2[j + 3] * contact.friction2Impulse * dt; + + Vconstraint[indexBody2Array][j] += contact.B_spBody2Friction2[j] * contact.friction2Impulse * dt; + Wconstraint[indexBody2Array][j] += contact.B_spBody2Friction2[j + 3] * contact.friction2Impulse * dt; } } } diff --git a/src/engine/ConstraintSolver.h b/src/engine/ConstraintSolver.h index 0a4db5b2..fbbe0469 100644 --- a/src/engine/ConstraintSolver.h +++ b/src/engine/ConstraintSolver.h @@ -179,7 +179,8 @@ class ConstraintSolver { // This is an array of size nbBodies that contains in each cell a 6x6 matrix decimal V1[6*NB_MAX_BODIES]; // Array that contains for each body the 6x1 vector that contains linear and angular velocities // Each cell contains a 6x1 vector with linear and angular velocities - decimal Vconstraint[6*NB_MAX_BODIES]; // Same kind of vector as V1 but contains the final constraint velocities + Vector3* Vconstraint; // Same kind of vector as V1 but contains the final constraint velocities + Vector3* Wconstraint; decimal VconstraintError[6*NB_MAX_BODIES]; // Same kind of vector as V1 but contains the final constraint velocities decimal Fext[6*NB_MAX_BODIES]; // Array that contains for each body the 6x1 vector that contains external forces and torques // Each cell contains a 6x1 vector with external force and torque. @@ -225,15 +226,15 @@ inline bool ConstraintSolver::isConstrainedBody(RigidBody* body) const { // Return the constrained linear velocity of a body after solving the LCP problem inline Vector3 ConstraintSolver::getConstrainedLinearVelocityOfBody(RigidBody* body) { assert(isConstrainedBody(body)); - uint indexBodyArray = 6 * mMapBodyToIndex[body]; - return Vector3(Vconstraint[indexBodyArray], Vconstraint[indexBodyArray + 1], Vconstraint[indexBodyArray + 2]); + uint indexBodyArray = mMapBodyToIndex[body]; + return Vconstraint[indexBodyArray]; } // Return the constrained angular velocity of a body after solving the LCP problem inline Vector3 ConstraintSolver::getConstrainedAngularVelocityOfBody(RigidBody *body) { assert(isConstrainedBody(body)); - uint indexBodyArray = 6 * mMapBodyToIndex[body]; - return Vector3(Vconstraint[indexBodyArray + 3], Vconstraint[indexBodyArray + 4], Vconstraint[indexBodyArray + 5]); + uint indexBodyArray = mMapBodyToIndex[body]; + return Wconstraint[indexBodyArray]; } // Cleanup of the constraint solver @@ -246,6 +247,14 @@ inline void ConstraintSolver::cleanup() { delete[] mContactConstraints; mContactConstraints = 0; } + if (Vconstraint != 0) { + delete[] Vconstraint; + Vconstraint = 0; + } + if (Wconstraint != 0) { + delete[] Wconstraint; + Wconstraint = 0; + } } // Set the number of iterations of the LCP solver From c8d216aafe8b67d8c8ce5aca0926adad4dc3c40b Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Fri, 21 Dec 2012 11:33:11 +0100 Subject: [PATCH 08/26] Use Vector3 inside the V1 array --- src/engine/ConstraintSolver.cpp | 41 ++++++++++++++++++--------------- src/engine/ConstraintSolver.h | 12 ++++++++-- 2 files changed, 33 insertions(+), 20 deletions(-) diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp index 84a56705..97db82c7 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), mNbIterations(100), mContactConstraints(0), Vconstraint(0), Wconstraint(0) { + :world(world), nbConstraints(0), mNbIterations(10), mContactConstraints(0), Vconstraint(0), Wconstraint(0), V1(0), W1(0) { } @@ -100,6 +100,8 @@ void ConstraintSolver::initialize() { Vconstraint = new Vector3[nbBodies]; Wconstraint = new Vector3[nbBodies]; + V1 = new Vector3[nbBodies]; + W1 = new Vector3[nbBodies]; assert(mMapBodyToIndex.size() == nbBodies); } @@ -120,15 +122,9 @@ void ConstraintSolver::initializeBodies() { assert(rigidBody); // Compute the vector V1 with initial velocities values - Vector3 linearVelocity = rigidBody->getLinearVelocity(); - Vector3 angularVelocity = rigidBody->getAngularVelocity(); int bodyIndexArray = 6 * bodyNumber; - V1[bodyIndexArray] = linearVelocity[0]; - V1[bodyIndexArray + 1] = linearVelocity[1]; - V1[bodyIndexArray + 2] = linearVelocity[2]; - V1[bodyIndexArray + 3] = angularVelocity[0]; - V1[bodyIndexArray + 4] = angularVelocity[1]; - V1[bodyIndexArray + 5] = angularVelocity[2]; + V1[bodyNumber] = rigidBody->getLinearVelocity(); + W1[bodyNumber] = rigidBody->getAngularVelocity(); // Compute the vector Vconstraint with final constraint velocities Vconstraint[bodyNumber] = Vector3(0, 0, 0); @@ -226,9 +222,12 @@ void ConstraintSolver::initializeContactConstraints(decimal dt) { decimal multiplication = 0.0; int body1ArrayIndex = 6 * indexBody1; int body2ArrayIndex = 6 * indexBody2; - for (uint i=0; i<6; i++) { - multiplication += contact.J_spBody1Penetration[i] * V1[body1ArrayIndex + i]; - multiplication += contact.J_spBody2Penetration[i] * V1[body2ArrayIndex + i]; + for (uint i=0; i<3; i++) { + multiplication += contact.J_spBody1Penetration[i] * V1[indexBody1][i]; + multiplication += contact.J_spBody1Penetration[i + 3] * W1[indexBody1][i]; + + multiplication += contact.J_spBody2Penetration[i] * V1[indexBody2][i]; + multiplication += contact.J_spBody2Penetration[i + 3] * W1[indexBody2][i]; } contact.b_Penetration -= multiplication * oneOverDT ; @@ -262,9 +261,12 @@ void ConstraintSolver::initializeContactConstraints(decimal dt) { // Substract 1.0/dt*J*V to the vector b multiplication = 0.0; - for (uint i=0; i<6; i++) { - multiplication += contact.J_spBody1Friction1[i] * V1[body1ArrayIndex + i]; - multiplication += contact.J_spBody2Friction1[i] * V1[body2ArrayIndex + i]; + for (uint i=0; i<3; i++) { + multiplication += contact.J_spBody1Friction1[i] * V1[indexBody1][i]; + multiplication += contact.J_spBody1Friction1[i + 3] * W1[indexBody1][i]; + + multiplication += contact.J_spBody2Friction1[i] * V1[indexBody2][i]; + multiplication += contact.J_spBody2Friction1[i + 3] * W1[indexBody2][i]; } contact.b_Friction1 -= multiplication * oneOverDT ; @@ -298,9 +300,12 @@ void ConstraintSolver::initializeContactConstraints(decimal dt) { // Substract 1.0/dt*J*V to the vector b multiplication = 0.0; - for (uint i=0; i<6; i++) { - multiplication += contact.J_spBody1Friction2[i] * V1[body1ArrayIndex + i]; - multiplication += contact.J_spBody2Friction2[i] * V1[body2ArrayIndex + i]; + for (uint i=0; i<3; i++) { + multiplication += contact.J_spBody1Friction2[i] * V1[indexBody1][i]; + multiplication += contact.J_spBody1Friction2[i + 3] * W1[indexBody1][i]; + + multiplication += contact.J_spBody2Friction2[i] * V1[indexBody2][i]; + multiplication += contact.J_spBody2Friction2[i + 3] * W1[indexBody2][i]; } contact.b_Friction2 -= multiplication * oneOverDT ; diff --git a/src/engine/ConstraintSolver.h b/src/engine/ConstraintSolver.h index fbbe0469..01fe55d6 100644 --- a/src/engine/ConstraintSolver.h +++ b/src/engine/ConstraintSolver.h @@ -177,8 +177,8 @@ class ConstraintSolver { 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 - decimal V1[6*NB_MAX_BODIES]; // Array that contains for each body the 6x1 vector that contains linear and angular velocities - // Each cell contains a 6x1 vector with linear and angular velocities + Vector3* V1; // Array that contains for each body the 6x1 vector that contains linear and angular velocities + Vector3* W1; Vector3* Vconstraint; // Same kind of vector as V1 but contains the final constraint velocities Vector3* Wconstraint; decimal VconstraintError[6*NB_MAX_BODIES]; // Same kind of vector as V1 but contains the final constraint velocities @@ -255,6 +255,14 @@ inline void ConstraintSolver::cleanup() { delete[] Wconstraint; Wconstraint = 0; } + if (V1 != 0) { + delete[] V1; + V1 = 0; + } + if (W1 != 0) { + delete[] W1; + W1 = 0; + } } // Set the number of iterations of the LCP solver From 91d148e31109cab9ec5960ca68cac1ad525c8296 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Tue, 25 Dec 2012 13:03:06 +0100 Subject: [PATCH 09/26] Change the way to compute the inverse constraint matrix K for the penetration constraint --- src/engine/ConstraintSolver.cpp | 42 +++++++++++++++++++++++++-------- src/engine/ConstraintSolver.h | 1 - 2 files changed, 32 insertions(+), 11 deletions(-) diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp index 97db82c7..d64d88a2 100644 --- a/src/engine/ConstraintSolver.cpp +++ b/src/engine/ConstraintSolver.cpp @@ -73,6 +73,9 @@ void ConstraintSolver::initialize() { mConstraintBodies.insert(body1); mConstraintBodies.insert(body2); + Vector3 x1 = body1->getTransform().getPosition(); + Vector3 x2 = body2->getTransform().getPosition(); + constraint.indexBody1 = mMapBodyToIndex[body1]; constraint.indexBody2 = mMapBodyToIndex[body2]; constraint.inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld(); @@ -86,10 +89,18 @@ void ConstraintSolver::initialize() { // For each contact point of the contact manifold for (uint c=0; cgetWorldPointOnBody1(); + Vector3 p2 = contact->getWorldPointOnBody2(); + + contactPointConstraint.contact = contact; + contactPointConstraint.normal = contact->getNormal(); + contactPointConstraint.r1 = p1 - x1; + contactPointConstraint.r2 = p2 - x2; } mNbContactConstraints++; @@ -165,11 +176,30 @@ void ConstraintSolver::initializeContactConstraints(decimal dt) { uint indexBody1 = constraint.indexBody1; uint indexBody2 = constraint.indexBody2; + Matrix3x3& I1 = constraint.inverseInertiaTensorBody1; + Matrix3x3& I2 = constraint.inverseInertiaTensorBody2; + // For each contact point constraint for (uint i=0; icomputeJacobianPenetration(contact.J_spBody1Penetration, contact.J_spBody2Penetration); @@ -488,14 +518,6 @@ void ConstraintSolver::solveLCP() { ContactPointConstraint& contact = constraint.contacts[i]; - // --------- Penetration --------- // - - contact.d_Penetration = 0.0; - for (uint j=0; j<6; j++) { - contact.d_Penetration += contact.J_spBody1Penetration[j] * contact.B_spBody1Penetration[j] - + contact.J_spBody2Penetration[j] * contact.B_spBody2Penetration[j]; - } - // --------- Friction 1 --------- // contact.d_Friction1 = 0.0; @@ -535,7 +557,7 @@ void ConstraintSolver::solveLCP() { for (uint j=0; j<6; j++) { deltaLambda -= (contact.J_spBody1Penetration[j] * a[indexBody1Array + j] + contact.J_spBody2Penetration[j] * a[indexBody2Array + j]); } - deltaLambda /= contact.d_Penetration; + deltaLambda /= contact.inversePenetrationMass; lambdaTemp = contact.penetrationImpulse; contact.penetrationImpulse = std::max(contact.lowerBoundPenetration, std::min(contact.penetrationImpulse + deltaLambda, contact.upperBoundPenetration)); deltaLambda = contact.penetrationImpulse - lambdaTemp; diff --git a/src/engine/ConstraintSolver.h b/src/engine/ConstraintSolver.h index 01fe55d6..31c19e0f 100644 --- a/src/engine/ConstraintSolver.h +++ b/src/engine/ConstraintSolver.h @@ -89,7 +89,6 @@ struct ContactPointConstraint { decimal B_spBody2Friction1[6]; decimal B_spBody1Friction2[6]; decimal B_spBody2Friction2[6]; - decimal d_Penetration; decimal d_Friction1; decimal d_Friction2; }; From 3872e9615c863f22abd9966f5e5b8b0c87892e3e Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Tue, 25 Dec 2012 17:35:52 +0100 Subject: [PATCH 10/26] Use unit friction vectors --- src/constraint/Contact.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/constraint/Contact.h b/src/constraint/Contact.h index 99dcd89d..ab4b1f86 100644 --- a/src/constraint/Contact.h +++ b/src/constraint/Contact.h @@ -182,11 +182,11 @@ inline void Contact::computeFrictionVectors() { mFrictionVectors.clear(); // Compute the first orthogonal vector - Vector3 vector1 = mNormal.getOneOrthogonalVector(); + Vector3 vector1 = mNormal.getOneOrthogonalVector().getUnit(); mFrictionVectors.push_back(vector1); // Compute the second orthogonal vector using the cross product - mFrictionVectors.push_back(mNormal.cross(vector1)); + mFrictionVectors.push_back(mNormal.cross(vector1).getUnit()); } // Return the normal vector of the contact From e64cac45711cf038b7875faae3fe2acd9c368082 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 26 Dec 2012 02:15:49 +0100 Subject: [PATCH 11/26] Start to compute the inverse mass matrix K for the friction constraints --- src/engine/ConstraintSolver.cpp | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp index d64d88a2..b2421de6 100644 --- a/src/engine/ConstraintSolver.cpp +++ b/src/engine/ConstraintSolver.cpp @@ -101,6 +101,8 @@ void ConstraintSolver::initialize() { contactPointConstraint.normal = contact->getNormal(); contactPointConstraint.r1 = p1 - x1; contactPointConstraint.r2 = p2 - x2; + contactPointConstraint.frictionVector1 = contact->getFrictionVector1(); + contactPointConstraint.frictionVector2 = contact->getFrictionVector2(); } mNbContactConstraints++; @@ -200,6 +202,18 @@ void ConstraintSolver::initializeContactConstraints(decimal dt) { contact.inversePenetrationMass += constraint.massInverseBody2 + ((I2 * contact.r2CrossN).cross(contact.r2)).dot(contact.normal); } + + // Compute the inverse mass matrix K for the friction constraints + contact.inverseFriction1Mass = 0.0; + contact.inverseFriction2Mass = 0.0; + if (constraint.isBody1Moving) { + contact.inverseFriction1Mass += constraint.massInverseBody1 + ((I1 * contact.r1CrossT1).cross(contact.r1)).dot(contact.frictionVector1); + contact.inverseFriction2Mass += constraint.massInverseBody1 + ((I1 * contact.r1CrossT2).cross(contact.r1)).dot(contact.frictionVector2); + } + if (constraint.isBody2Moving) { + contact.inverseFriction1Mass += constraint.massInverseBody2 + ((I2 * contact.r2CrossT1).cross(contact.r2)).dot(contact.frictionVector1); + contact.inverseFriction2Mass += constraint.massInverseBody2 + ((I2 * contact.r2CrossT2).cross(contact.r2)).dot(contact.frictionVector2); + } // Fill in the J_sp matrix realContact->computeJacobianPenetration(contact.J_spBody1Penetration, contact.J_spBody2Penetration); @@ -568,6 +582,9 @@ void ConstraintSolver::solveLCP() { // --------- Friction 1 --------- // + //std::cout << "contact.b_Friction1 : " << contact.b_Friction1 << std::endl; + //std::cout << "contact.inverseFriction1Mass : " << contact.inverseFriction1Mass << std::endl; + deltaLambda = contact.b_Friction1; for (uint j=0; j<6; j++) { deltaLambda -= (contact.J_spBody1Friction1[j] * a[indexBody1Array + j] + contact.J_spBody2Friction1[j] * a[indexBody2Array + j]); From 158c19541b7ff671308f603a2df1e4cf626ea851 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 26 Dec 2012 02:18:51 +0100 Subject: [PATCH 12/26] Add methods to get the friction vectors of the contact --- src/constraint/Contact.h | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/src/constraint/Contact.h b/src/constraint/Contact.h index ab4b1f86..57203845 100644 --- a/src/constraint/Contact.h +++ b/src/constraint/Contact.h @@ -135,6 +135,12 @@ class Contact : public Constraint { // Set the contact world point on body 2 void setWorldPointOnBody2(const Vector3& worldPoint); + // Get the first friction vector + Vector3 getFrictionVector1() const; + + // Get the second friction vector + Vector3 getFrictionVector2() const; + // Compute the jacobian matrix for all mathematical constraints virtual void computeJacobian(int noConstraint, decimal J_SP[NB_MAX_CONSTRAINTS][2*6]) const; @@ -229,6 +235,16 @@ inline void Contact::setWorldPointOnBody2(const Vector3& worldPoint) { mWorldPointOnBody2 = worldPoint; } +// Get the first friction vector +inline Vector3 Contact::getFrictionVector1() const { + return mFrictionVectors[0]; +} + +// Get the second friction vector +inline Vector3 Contact::getFrictionVector2() const { + return mFrictionVectors[1]; +} + // Return the penetration depth of the contact inline decimal Contact::getPenetrationDepth() const { return mPenetrationDepth; From f2f168f6c8fc84162de5d80e45caa113713957ad Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sat, 29 Dec 2012 00:05:44 +0100 Subject: [PATCH 13/26] Change the way to compute the inverse constraint matrix K for the friction constraints --- src/engine/ConstraintSolver.cpp | 81 ++++++++++++++++++--------------- src/engine/ConstraintSolver.h | 2 - 2 files changed, 44 insertions(+), 39 deletions(-) diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp index b2421de6..5c9e0641 100644 --- a/src/engine/ConstraintSolver.cpp +++ b/src/engine/ConstraintSolver.cpp @@ -127,11 +127,10 @@ void ConstraintSolver::initializeBodies() { RigidBody* body; uint b=0; for (set::iterator it = mConstraintBodies.begin(); it != mConstraintBodies.end(); ++it, b++) { - body = *it; - uint bodyNumber = mMapBodyToIndex[body]; + rigidBody = *it; + uint bodyNumber = mMapBodyToIndex[rigidBody]; // TODO : Use polymorphism and remove this downcasting - rigidBody = dynamic_cast(body); assert(rigidBody); // Compute the vector V1 with initial velocities values @@ -214,7 +213,7 @@ void ConstraintSolver::initializeContactConstraints(decimal dt) { contact.inverseFriction1Mass += constraint.massInverseBody2 + ((I2 * contact.r2CrossT1).cross(contact.r2)).dot(contact.frictionVector1); contact.inverseFriction2Mass += constraint.massInverseBody2 + ((I2 * contact.r2CrossT2).cross(contact.r2)).dot(contact.frictionVector2); } - + // Fill in the J_sp matrix realContact->computeJacobianPenetration(contact.J_spBody1Penetration, contact.J_spBody2Penetration); realContact->computeJacobianFriction1(contact.J_spBody1Friction1, contact.J_spBody2Friction1); @@ -415,6 +414,37 @@ void ConstraintSolver::computeMatrixB_sp() { // ---------- Friction 1 ---------- // + Matrix3x3& I1 = constraint.inverseInertiaTensorBody1; + Matrix3x3& I2 = constraint.inverseInertiaTensorBody2; + + Vector3 JspLinBody1 = Vector3(0, 0, 0); + Vector3 JspAngBody1 = Vector3(0, 0, 0); + Vector3 JspLinBody2 = Vector3(0, 0, 0); + Vector3 JspAngBody2 = Vector3(0, 0, 0); + + if (constraint.isBody1Moving) { + JspLinBody1 += -constraint.massInverseBody1 * contact.frictionVector1; + JspAngBody1 += I1 * (-contact.r1CrossT1); + } + if (constraint.isBody2Moving) { + JspLinBody2 += constraint.massInverseBody2 * contact.frictionVector1; + JspAngBody2 += I1 * (contact.r2CrossT1); + } + + /* + std::cout << "------ New Version ----" << std::endl; + std::cout << "JspLinBody1 : " << JspLinBody1.getX() << ", " << JspLinBody1.getY() << ", " << JspLinBody1.getZ() << std::endl; + std::cout << "JspAngBody1 : " << JspAngBody1.getX() << ", " << JspAngBody1.getY() << ", " << JspAngBody1.getZ() << std::endl; + std::cout << "JspLinBody2 : " << JspLinBody2.getX() << ", " << JspLinBody2.getY() << ", " << JspLinBody2.getZ() << std::endl; + std::cout << "JspAngBody2 : " << JspAngBody2.getX() << ", " << JspAngBody2.getY() << ", " << JspAngBody2.getZ() << std::endl; + + std::cout << ": friction vector 1 : " << contact.frictionVector1.getX() << ", " << contact.frictionVector1.getY() << ", " << contact.frictionVector1.getZ() << std::endl; + std::cout << ": r1 x t1 : " << contact.r1CrossT1.getX() << ", " << contact.r1CrossT1.getY() << ", " << contact.r1CrossT1.getZ() << std::endl; + std::cout << ": r2 x t1 : " << contact.r2CrossT1.getX() << ", " << contact.r2CrossT1.getY() << ", " << contact.r2CrossT1.getZ() << std::endl; + std::cout << ": inverse mass body 1 = " << constraint.massInverseBody1 << std::endl; + std::cout << ": inverse mass body 2 = " << constraint.massInverseBody2 << std::endl; + */ + contact.B_spBody1Friction1[0] = Minv_sp_mass_diag[indexBody1] * contact.J_spBody1Friction1[0]; contact.B_spBody1Friction1[1] = Minv_sp_mass_diag[indexBody1] * contact.J_spBody1Friction1[1]; contact.B_spBody1Friction1[2] = Minv_sp_mass_diag[indexBody1] * contact.J_spBody1Friction1[2]; @@ -431,6 +461,14 @@ void ConstraintSolver::computeMatrixB_sp() { } } + /* + std::cout << "------ Old Version ----" << std::endl; + std::cout << "JspLinBody1 : " << contact.B_spBody1Friction1[0] << ", " << contact.B_spBody1Friction1[1] << ", " << contact.B_spBody1Friction1[2] << std::endl; + std::cout << "JspAngBody1 : " << contact.B_spBody1Friction1[3] << ", " << contact.B_spBody1Friction1[4] << ", " << contact.B_spBody1Friction1[5] << std::endl; + std::cout << "JspLinBody2 : " << contact.B_spBody2Friction1[0] << ", " << contact.B_spBody2Friction1[1] << ", " << contact.B_spBody2Friction1[2] << std::endl; + std::cout << "JspAngBody2 : " << contact.B_spBody2Friction1[3] << ", " << contact.B_spBody2Friction1[4] << ", " << contact.B_spBody2Friction1[5] << std::endl; + */ + // ---------- Friction 2 ---------- // contact.B_spBody1Friction2[0] = Minv_sp_mass_diag[indexBody1] * contact.J_spBody1Friction2[0]; @@ -522,34 +560,6 @@ void ConstraintSolver::solveLCP() { // Compute the vector a computeVectorA(); - // For each constraint - // For each constraint - for (uint c=0; c Date: Sat, 29 Dec 2012 14:15:07 +0100 Subject: [PATCH 14/26] Use Vector3 type into the a[] array --- src/engine/ConstraintSolver.cpp | 82 +++++++++++++++++++++------------ src/engine/ConstraintSolver.h | 3 +- 2 files changed, 54 insertions(+), 31 deletions(-) diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp index 5c9e0641..4d0886f7 100644 --- a/src/engine/ConstraintSolver.cpp +++ b/src/engine/ConstraintSolver.cpp @@ -572,52 +572,64 @@ void ConstraintSolver::solveLCP() { ContactPointConstraint& contact = constraint.contacts[i]; - indexBody1Array = 6 * constraint.indexBody1; - indexBody2Array = 6 * constraint.indexBody2; + indexBody1Array = constraint.indexBody1; + indexBody2Array = constraint.indexBody2; // --------- Penetration --------- // deltaLambda = contact.b_Penetration; - for (uint j=0; j<6; j++) { - deltaLambda -= (contact.J_spBody1Penetration[j] * a[indexBody1Array + j] + contact.J_spBody2Penetration[j] * a[indexBody2Array + j]); + for (uint j=0; j<3; j++) { + deltaLambda -= (contact.J_spBody1Penetration[j] * aLinear[indexBody1Array][j] + contact.J_spBody2Penetration[j] * aLinear[indexBody2Array][j]); + deltaLambda -= (contact.J_spBody1Penetration[j + 3] * aAngular[indexBody1Array][j] + contact.J_spBody2Penetration[j + 3] * aAngular[indexBody2Array][j]); } deltaLambda /= contact.inversePenetrationMass; lambdaTemp = contact.penetrationImpulse; contact.penetrationImpulse = std::max(contact.lowerBoundPenetration, std::min(contact.penetrationImpulse + deltaLambda, contact.upperBoundPenetration)); deltaLambda = contact.penetrationImpulse - lambdaTemp; - for (uint j=0; j<6; j++) { - a[indexBody1Array + j] += contact.B_spBody1Penetration[j] * deltaLambda; - a[indexBody2Array + j] += contact.B_spBody2Penetration[j] * deltaLambda; + for (uint j=0; j<3; j++) { + aLinear[indexBody1Array][j] += contact.B_spBody1Penetration[j] * deltaLambda; + aAngular[indexBody1Array][j] += contact.B_spBody1Penetration[j + 3] * deltaLambda; + + aLinear[indexBody2Array][j] += contact.B_spBody2Penetration[j] * deltaLambda; + aAngular[indexBody2Array][j] += contact.B_spBody2Penetration[j + 3] * deltaLambda; } // --------- Friction 1 --------- // deltaLambda = contact.b_Friction1; - for (uint j=0; j<6; j++) { - deltaLambda -= (contact.J_spBody1Friction1[j] * a[indexBody1Array + j] + contact.J_spBody2Friction1[j] * a[indexBody2Array + j]); + for (uint j=0; j<3; j++) { + deltaLambda -= (contact.J_spBody1Friction1[j] * aLinear[indexBody1Array][j] + contact.J_spBody2Friction1[j] * aLinear[indexBody2Array][j]); + deltaLambda -= (contact.J_spBody1Friction1[j + 3] * aAngular[indexBody1Array][j] + contact.J_spBody2Friction1[j + 3] * aAngular[indexBody2Array][j]); } deltaLambda /= contact.inverseFriction1Mass; lambdaTemp = contact.friction1Impulse; contact.friction1Impulse = std::max(contact.lowerBoundFriction1, std::min(contact.friction1Impulse + deltaLambda, contact.upperBoundFriction1)); deltaLambda = contact.friction1Impulse - lambdaTemp; - for (uint j=0; j<6; j++) { - a[indexBody1Array + j] += contact.B_spBody1Friction1[j] * deltaLambda; - a[indexBody2Array + j] += contact.B_spBody2Friction1[j] * deltaLambda; + for (uint j=0; j<3; j++) { + aLinear[indexBody1Array][j] += contact.B_spBody1Friction1[j] * deltaLambda; + aAngular[indexBody1Array][j] += contact.B_spBody1Friction1[j + 3] * deltaLambda; + + aLinear[indexBody2Array][j] += contact.B_spBody2Friction1[j] * deltaLambda; + aAngular[indexBody2Array][j] += contact.B_spBody2Friction1[j + 3] * deltaLambda; } // --------- Friction 2 --------- // deltaLambda = contact.b_Friction2; - for (uint j=0; j<6; j++) { - deltaLambda -= (contact.J_spBody1Friction2[j] * a[indexBody1Array + j] + contact.J_spBody2Friction2[j] * a[indexBody2Array + j]); + for (uint j=0; j<3; j++) { + deltaLambda -= (contact.J_spBody1Friction2[j] * aLinear[indexBody1Array][j] + contact.J_spBody2Friction2[j] * aLinear[indexBody2Array][j]); + deltaLambda -= (contact.J_spBody1Friction2[j + 3] * aAngular[indexBody1Array][j] + contact.J_spBody2Friction2[j + 3] * aAngular[indexBody2Array][j]); } deltaLambda /= contact.inverseFriction2Mass; lambdaTemp = contact.friction2Impulse; contact.friction2Impulse = std::max(contact.lowerBoundFriction2, std::min(contact.friction2Impulse + deltaLambda, contact.upperBoundFriction2)); deltaLambda = contact.friction2Impulse - lambdaTemp; - for (uint j=0; j<6; j++) { - a[indexBody1Array + j] += contact.B_spBody1Friction2[j] * deltaLambda; - a[indexBody2Array + j] += contact.B_spBody2Friction2[j] * deltaLambda; + for (uint j=0; j<3; j++) { + aLinear[indexBody1Array][j] += contact.B_spBody1Friction2[j] * deltaLambda; + aAngular[indexBody1Array][j] += contact.B_spBody1Friction2[j + 3] * deltaLambda; + + aLinear[indexBody2Array][j] += contact.B_spBody2Friction2[j] * deltaLambda; + aAngular[indexBody2Array][j] += contact.B_spBody2Friction2[j + 3] * deltaLambda; } } } @@ -631,8 +643,9 @@ void ConstraintSolver::computeVectorA() { uint indexBody1Array, indexBody2Array; // Init the vector a with zero values - for (i=0; i<6*nbBodies; i++) { - a[i] = 0.0; + for (i=0; i Date: Sun, 30 Dec 2012 12:45:06 +0100 Subject: [PATCH 15/26] Change the way we solve the linear system --- src/engine/ConstraintSolver.cpp | 133 +++----------------------------- src/engine/ConstraintSolver.h | 8 +- 2 files changed, 13 insertions(+), 128 deletions(-) diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp index 4d0886f7..87e0f3e4 100644 --- a/src/engine/ConstraintSolver.cpp +++ b/src/engine/ConstraintSolver.cpp @@ -142,16 +142,6 @@ void ConstraintSolver::initializeBodies() { Vconstraint[bodyNumber] = Vector3(0, 0, 0); Wconstraint[bodyNumber] = Vector3(0, 0, 0); - // Compute the vector with forces and torques values - Vector3 externalForce = rigidBody->getExternalForce(); - Vector3 externalTorque = rigidBody->getExternalTorque(); - Fext[bodyIndexArray] = externalForce[0]; - Fext[bodyIndexArray + 1] = externalForce[1]; - Fext[bodyIndexArray + 2] = externalForce[2]; - Fext[bodyIndexArray + 3] = externalTorque[0]; - Fext[bodyIndexArray + 4] = externalTorque[1]; - Fext[bodyIndexArray + 5] = externalTorque[2]; - // 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; @@ -259,120 +249,13 @@ void ConstraintSolver::initializeContactConstraints(decimal dt) { // b = errorValues * oneOverDT; contact.b_Penetration = contact.errorPenetration * oneOverDT; - // Substract 1.0/dt*J*V to the vector b - indexBody1 = constraint.indexBody1; - indexBody2 = constraint.indexBody2; - decimal multiplication = 0.0; - int body1ArrayIndex = 6 * indexBody1; - int body2ArrayIndex = 6 * indexBody2; - for (uint i=0; i<3; i++) { - multiplication += contact.J_spBody1Penetration[i] * V1[indexBody1][i]; - multiplication += contact.J_spBody1Penetration[i + 3] * W1[indexBody1][i]; - - multiplication += contact.J_spBody2Penetration[i] * V1[indexBody2][i]; - multiplication += contact.J_spBody2Penetration[i + 3] * W1[indexBody2][i]; - } - contact.b_Penetration -= multiplication * oneOverDT ; - - // Substract J*M^-1*F_ext to the vector b - decimal value1 = 0.0; - decimal value2 = 0.0; - decimal sum1, sum2; - value1 += contact.J_spBody1Penetration[0] * Minv_sp_mass_diag[indexBody1] * Fext[body1ArrayIndex] + - contact.J_spBody1Penetration[1] * Minv_sp_mass_diag[indexBody1] * Fext[body1ArrayIndex + 1] + - contact.J_spBody1Penetration[2] * Minv_sp_mass_diag[indexBody1] * Fext[body1ArrayIndex + 2]; - value2 += contact.J_spBody2Penetration[0] * Minv_sp_mass_diag[indexBody2] * Fext[body2ArrayIndex] + - contact.J_spBody2Penetration[1] * Minv_sp_mass_diag[indexBody2] * Fext[body2ArrayIndex + 1] + - contact.J_spBody2Penetration[2] * Minv_sp_mass_diag[indexBody2] * Fext[body2ArrayIndex + 2]; - for (uint i=0; i<3; i++) { - sum1 = 0.0; - sum2 = 0.0; - for (uint j=0; j<3; j++) { - sum1 += contact.J_spBody1Penetration[3 + j] * Minv_sp_inertia[indexBody1].getValue(j, i); - sum2 += contact.J_spBody2Penetration[3 + j] * Minv_sp_inertia[indexBody2].getValue(j, i); - } - value1 += sum1 * Fext[body1ArrayIndex + 3 + i]; - value2 += sum2 * Fext[body2ArrayIndex + 3 + i]; - } - - contact.b_Penetration -= value1 + value2; - // ---------- Friction 1 ---------- // - // b = errorValues * oneOverDT; contact.b_Friction1 = contact.errorFriction1 * oneOverDT; - // Substract 1.0/dt*J*V to the vector b - multiplication = 0.0; - for (uint i=0; i<3; i++) { - multiplication += contact.J_spBody1Friction1[i] * V1[indexBody1][i]; - multiplication += contact.J_spBody1Friction1[i + 3] * W1[indexBody1][i]; - - multiplication += contact.J_spBody2Friction1[i] * V1[indexBody2][i]; - multiplication += contact.J_spBody2Friction1[i + 3] * W1[indexBody2][i]; - } - contact.b_Friction1 -= multiplication * oneOverDT ; - - // Substract J*M^-1*F_ext to the vector b - value1 = 0.0; - value2 = 0.0; - value1 += contact.J_spBody1Friction1[0] * Minv_sp_mass_diag[indexBody1] * Fext[body1ArrayIndex] + - contact.J_spBody1Friction1[1] * Minv_sp_mass_diag[indexBody1] * Fext[body1ArrayIndex + 1] + - contact.J_spBody1Friction1[2] * Minv_sp_mass_diag[indexBody1] * Fext[body1ArrayIndex + 2]; - value2 += contact.J_spBody2Friction1[0] * Minv_sp_mass_diag[indexBody2] * Fext[body2ArrayIndex] + - contact.J_spBody2Friction1[1] * Minv_sp_mass_diag[indexBody2] * Fext[body2ArrayIndex + 1] + - contact.J_spBody2Friction1[2] * Minv_sp_mass_diag[indexBody2] * Fext[body2ArrayIndex + 2]; - for (uint i=0; i<3; i++) { - sum1 = 0.0; - sum2 = 0.0; - for (uint j=0; j<3; j++) { - sum1 += contact.J_spBody1Friction1[3 + j] * Minv_sp_inertia[indexBody1].getValue(j, i); - sum2 += contact.J_spBody2Friction1[3 + j] * Minv_sp_inertia[indexBody2].getValue(j, i); - } - value1 += sum1 * Fext[body1ArrayIndex + 3 + i]; - value2 += sum2 * Fext[body2ArrayIndex + 3 + i]; - } - - contact.b_Friction1 -= value1 + value2; - - // ---------- Friction 2 ---------- // - // b = errorValues * oneOverDT; contact.b_Friction2 = contact.errorFriction2 * oneOverDT; - - // Substract 1.0/dt*J*V to the vector b - multiplication = 0.0; - for (uint i=0; i<3; i++) { - multiplication += contact.J_spBody1Friction2[i] * V1[indexBody1][i]; - multiplication += contact.J_spBody1Friction2[i + 3] * W1[indexBody1][i]; - - multiplication += contact.J_spBody2Friction2[i] * V1[indexBody2][i]; - multiplication += contact.J_spBody2Friction2[i + 3] * W1[indexBody2][i]; - } - contact.b_Friction2 -= multiplication * oneOverDT ; - - // Substract J*M^-1*F_ext to the vector b - value1 = 0.0; - value2 = 0.0; - value1 += contact.J_spBody1Friction2[0] * Minv_sp_mass_diag[indexBody1] * Fext[body1ArrayIndex] + - contact.J_spBody1Friction2[1] * Minv_sp_mass_diag[indexBody1] * Fext[body1ArrayIndex + 1] + - contact.J_spBody1Friction2[2] * Minv_sp_mass_diag[indexBody1] * Fext[body1ArrayIndex + 2]; - value2 += contact.J_spBody2Friction2[0] * Minv_sp_mass_diag[indexBody2] * Fext[body2ArrayIndex] + - contact.J_spBody2Friction2[1] * Minv_sp_mass_diag[indexBody2] * Fext[body2ArrayIndex + 1] + - contact.J_spBody2Friction2[2] * Minv_sp_mass_diag[indexBody2] * Fext[body2ArrayIndex + 2]; - for (uint i=0; i<3; i++) { - sum1 = 0.0; - sum2 = 0.0; - for (uint j=0; j<3; j++) { - sum1 += contact.J_spBody1Friction2[3 + j] * Minv_sp_inertia[indexBody1].getValue(j, i); - sum2 += contact.J_spBody2Friction2[3 + j] * Minv_sp_inertia[indexBody2].getValue(j, i); - } - value1 += sum1 * Fext[body1ArrayIndex + 3 + i]; - value2 += sum2 * Fext[body2ArrayIndex + 3 + i]; - } - - contact.b_Friction2 -= value1 + value2; } } @@ -546,7 +429,7 @@ void ConstraintSolver::computeVectorVconstraint(decimal dt) { // Solve a LCP problem using the Projected-Gauss-Seidel algorithm // This method outputs the result in the lambda vector -void ConstraintSolver::solveLCP() { +void ConstraintSolver::solveLCP(decimal dt) { // for (uint i=0; i::iterator it = mConstraintBodies.begin(); it != mConstraintBodies.end(); ++it) { + RigidBody* rigidBody = *it; + uint bodyNumber = mMapBodyToIndex[rigidBody]; + + aLinear[bodyNumber] = oneOverDt * V1[bodyNumber] + rigidBody->getMassInverse() * rigidBody->getExternalForce(); + aAngular[bodyNumber] = oneOverDt * W1[bodyNumber] + rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque(); } // For each constraint diff --git a/src/engine/ConstraintSolver.h b/src/engine/ConstraintSolver.h index d83635f8..c5ae3a80 100644 --- a/src/engine/ConstraintSolver.h +++ b/src/engine/ConstraintSolver.h @@ -180,8 +180,6 @@ class ConstraintSolver { Vector3* Vconstraint; // Same kind of vector as V1 but contains the final constraint velocities Vector3* Wconstraint; decimal VconstraintError[6*NB_MAX_BODIES]; // Same kind of vector as V1 but contains the final constraint velocities - decimal Fext[6*NB_MAX_BODIES]; // Array that contains for each body the 6x1 vector that contains external forces and torques - // Each cell contains a 6x1 vector with external force and torque. // Contact constraints ContactConstraint* mContactConstraints; @@ -202,8 +200,8 @@ class ConstraintSolver { void computeMatrixB_sp(); // Compute the matrix B_sp void computeVectorVconstraint(decimal dt); // Compute the vector V2 void cacheLambda(); // Cache the lambda values in order to reuse them in the next step to initialize the lambda vector - void computeVectorA(); // Compute the vector a used in the solve() method - void solveLCP(); // Solve a LCP problem using Projected-Gauss-Seidel algorithm + void computeVectorA(decimal dt); // Compute the vector a used in the solve() method + void solveLCP(decimal dt); // Solve a LCP problem using Projected-Gauss-Seidel algorithm public: ConstraintSolver(DynamicsWorld* world); // Constructor @@ -283,7 +281,7 @@ inline void ConstraintSolver::solve(decimal dt) { computeMatrixB_sp(); // Solve the LCP problem (computation of lambda) - solveLCP(); + solveLCP(dt); // Cache the lambda values in order to use them in the next step cacheLambda(); From 1bcec415a11ee83071a584f083fc72d2502f7011 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sun, 6 Jan 2013 19:28:56 +0100 Subject: [PATCH 16/26] Continue to transform PGS solver into the sequential impulse solver --- src/engine/ConstraintSolver.cpp | 200 +++++++++++--------------------- src/engine/ConstraintSolver.h | 67 +++-------- src/engine/DynamicsWorld.cpp | 17 +-- 3 files changed, 91 insertions(+), 193 deletions(-) diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp index 87e0f3e4..5190d1f4 100644 --- a/src/engine/ConstraintSolver.cpp +++ b/src/engine/ConstraintSolver.cpp @@ -34,7 +34,8 @@ using namespace std; // Constructor ConstraintSolver::ConstraintSolver(DynamicsWorld* world) - :world(world), nbConstraints(0), mNbIterations(10), mContactConstraints(0), Vconstraint(0), Wconstraint(0), V1(0), W1(0) { + :world(world), nbConstraints(0), mNbIterations(10), mContactConstraints(0), + mLinearVelocities(0), mAngularVelocities(0) { } @@ -45,7 +46,7 @@ ConstraintSolver::~ConstraintSolver() { // Initialize the constraint solver before each solving void ConstraintSolver::initialize() { - + nbConstraints = 0; // TOOD : Use better allocation here @@ -111,10 +112,8 @@ void ConstraintSolver::initialize() { // Compute the number of bodies that are part of some active constraint nbBodies = mConstraintBodies.size(); - Vconstraint = new Vector3[nbBodies]; - Wconstraint = new Vector3[nbBodies]; - V1 = new Vector3[nbBodies]; - W1 = new Vector3[nbBodies]; + mLinearVelocities = new Vector3[nbBodies]; + mAngularVelocities = new Vector3[nbBodies]; assert(mMapBodyToIndex.size() == nbBodies); } @@ -135,12 +134,9 @@ void ConstraintSolver::initializeBodies() { // Compute the vector V1 with initial velocities values int bodyIndexArray = 6 * bodyNumber; - V1[bodyNumber] = rigidBody->getLinearVelocity(); - W1[bodyNumber] = rigidBody->getAngularVelocity(); - // Compute the vector Vconstraint with final constraint velocities - Vconstraint[bodyNumber] = Vector3(0, 0, 0); - Wconstraint[bodyNumber] = Vector3(0, 0, 0); + 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); @@ -225,8 +221,6 @@ void ConstraintSolver::initializeContactConstraints(decimal dt) { // Fill in the error vector realContact->computeErrorPenetration(contact.errorPenetration); - contact.errorFriction1 = 0.0; - contact.errorFriction2 = 0.0; // Get the cached lambda values of the constraint contact.penetrationImpulse = realContact->getCachedLambda(0); @@ -239,27 +233,15 @@ void ConstraintSolver::initializeContactConstraints(decimal dt) { 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); + contact.errorPenetration -= 0.2 * oneOverDT * std::max(double(realContact->getPenetrationDepth() - slop), 0.0); } - contact.errorFriction1 = 0.0; - contact.errorFriction2 = 0.0; // ---------- Penetration ---------- // // b = errorValues * oneOverDT; - contact.b_Penetration = contact.errorPenetration * oneOverDT; - - // ---------- Friction 1 ---------- // - - contact.b_Friction1 = contact.errorFriction1 * oneOverDT; - - // ---------- Friction 2 ---------- // - - contact.b_Friction2 = contact.errorFriction2 * oneOverDT; + contact.b_Penetration = contact.errorPenetration; } } - - } // Compute the matrix B_sp @@ -373,67 +355,9 @@ void ConstraintSolver::computeMatrixB_sp() { } } -// Compute the vector V_constraint (which corresponds to the constraint part of -// the final V2 vector) according to the formula -// V_constraint = dt * (M^-1 * J^T * lambda) -// Note that we use the vector V to store both V1 and V_constraint. -// Note that M^-1 * J^T = B. -// This method is called after that the LCP solver has computed lambda -void ConstraintSolver::computeVectorVconstraint(decimal dt) { - uint indexBody1Array, indexBody2Array; - uint j; - - // Compute dt * (M^-1 * J^T * lambda - for (uint c=0; c::iterator it = mConstraintBodies.begin(); it != mConstraintBodies.end(); ++it) { - RigidBody* rigidBody = *it; - uint bodyNumber = mMapBodyToIndex[rigidBody]; - - aLinear[bodyNumber] = oneOverDt * V1[bodyNumber] + rigidBody->getMassInverse() * rigidBody->getExternalForce(); - aAngular[bodyNumber] = oneOverDt * W1[bodyNumber] + rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque(); - } // For each constraint for (uint c=0; cgetMassInverse() * rigidBody->getExternalForce(); + newAngularVelocity += dt * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque(); - // Compute V_forces = dt * (M^-1 * F_ext) which is the velocity of the body due to the - // external forces and torques. - newLinearVelocity += dt * rigidBody->getMassInverse() * rigidBody->getExternalForce(); - newAngularVelocity += dt * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque(); - - // Add the velocity V1 to the new velocity - newLinearVelocity += rigidBody->getLinearVelocity(); - newAngularVelocity += rigidBody->getAngularVelocity(); + // Add the velocity V1 to the new velocity + newLinearVelocity += rigidBody->getLinearVelocity(); + newAngularVelocity += rigidBody->getAngularVelocity(); + } // Update the position and the orientation of the body according to the new velocity updatePositionAndOrientationOfBody(*it, newLinearVelocity, newAngularVelocity); From 5c941cf88b09cab6e4eb4bb3745b697b163d12b4 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 16 Jan 2013 13:23:37 +0100 Subject: [PATCH 17/26] Continue to implement the Sequential Impulse solver --- src/engine/ConstraintSolver.cpp | 66 ++++++++++++++++++++++++++++++++- src/engine/ConstraintSolver.h | 51 +++++++++++++++++-------- 2 files changed, 100 insertions(+), 17 deletions(-) diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp index 5190d1f4..33347390 100644 --- a/src/engine/ConstraintSolver.cpp +++ b/src/engine/ConstraintSolver.cpp @@ -86,6 +86,7 @@ void ConstraintSolver::initialize() { constraint.massInverseBody1 = body1->getMassInverse(); constraint.massInverseBody2 = body2->getMassInverse(); constraint.nbContacts = contactManifold.nbContacts; + constraint.restitutionFactor = computeMixRestitutionFactor(body1, body2); // For each contact point of the contact manifold for (uint c=0; cgetFrictionVector1(); contactPointConstraint.frictionVector2 = contact->getFrictionVector2(); + contactPointConstraint.penetrationDepth = contact->getPenetrationDepth(); } mNbContactConstraints++; @@ -200,6 +202,24 @@ void ConstraintSolver::initializeContactConstraints(decimal dt) { contact.inverseFriction2Mass += constraint.massInverseBody2 + ((I2 * contact.r2CrossT2).cross(contact.r2)).dot(contact.frictionVector2); } + const Vector3& v1 = mLinearVelocities[constraint.indexBody1]; + const Vector3& w1 = mAngularVelocities[constraint.indexBody1]; + const Vector3& v2 = mLinearVelocities[constraint.indexBody2]; + const Vector3& w2 = mAngularVelocities[constraint.indexBody2]; + + // Compute the restitution velocity bias "b". We compute this here instead + // of inside the solve() method because we need to use the velocity difference + // at the beginning of the contact. Note that if it is a resting contact (normal velocity + // under a given threshold), we don't add a restitution velocity bias + contact.restitutionBias = 0.0; + Vector3 deltaV = v2 + w2.cross(contact.r2) - v1 - w1.cross(contact.r1); + decimal deltaVDotN = deltaV.dot(contact.normal); + // TODO : Use a constant here + decimal elasticLinearVelocityThreshold = 0.3; + if (deltaVDotN < -elasticLinearVelocityThreshold) { + contact.restitutionBias = constraint.restitutionFactor * deltaVDotN; + } + // Fill in the J_sp matrix realContact->computeJacobianPenetration(contact.J_spBody1Penetration, contact.J_spBody2Penetration); realContact->computeJacobianFriction1(contact.J_spBody1Friction1, contact.J_spBody2Friction1); @@ -382,14 +402,38 @@ void ConstraintSolver::solveLCP() { indexBody1Array = constraint.indexBody1; indexBody2Array = constraint.indexBody2; + const Vector3& v1 = mLinearVelocities[constraint.indexBody1]; + const Vector3& w1 = mAngularVelocities[constraint.indexBody1]; + const Vector3& v2 = mLinearVelocities[constraint.indexBody2]; + const Vector3& w2 = mAngularVelocities[constraint.indexBody2]; + // --------- Penetration --------- // + // Compute J*v + Vector3 deltaV = v2 + w2.cross(contact.r2) - v1 - w1.cross(contact.r1); + decimal deltaVDotN = deltaV.dot(contact.normal); + decimal Jv = deltaVDotN; + + // Compute the bias "b" of the constraint + decimal beta = 0.2; + // TODO : Use a constant for the slop + decimal slop = 0.005; + decimal biasPenetrationDepth = 0.0; + if (contact.penetrationDepth > slop) biasPenetrationDepth = -(beta/mTimeStep) * + max(0.0f, float(contact.penetrationDepth - slop)); + decimal b = biasPenetrationDepth + contact.restitutionBias; + + // Compute the Lagrange multiplier + deltaLambda = - (Jv + b); + + /* deltaLambda = 0.0; for (uint j=0; j<3; j++) { deltaLambda -= (contact.J_spBody1Penetration[j] * mLinearVelocities[indexBody1Array][j] + contact.J_spBody2Penetration[j] * mLinearVelocities[indexBody2Array][j]); deltaLambda -= (contact.J_spBody1Penetration[j + 3] * mAngularVelocities[indexBody1Array][j] + contact.J_spBody2Penetration[j + 3] * mAngularVelocities[indexBody2Array][j]); } - deltaLambda -= contact.b_Penetration; + */ + //deltaLambda -= contact.b_Penetration; deltaLambda /= contact.inversePenetrationMass; lambdaTemp = contact.penetrationImpulse; contact.penetrationImpulse = std::max(contact.lowerBoundPenetration, std::min(contact.penetrationImpulse + deltaLambda, contact.upperBoundPenetration)); @@ -537,3 +581,23 @@ void ConstraintSolver::solve(decimal dt) { // Cache the lambda values in order to use them in the next step cacheLambda(); } + +// Apply an impulse to the two bodies of a constraint +void ConstraintSolver::applyImpulse(const Impulse& impulse, const ContactConstraint& constraint) { + + // Update the velocities of the bodies by applying the impulse P + if (constraint.isBody1Moving) { + mLinearVelocities[constraint.indexBody1] += constraint.massInverseBody1 * + impulse.linearImpulseBody1; + mAngularVelocities[constraint.indexBody1] += constraint.inverseInertiaTensorBody1 * + impulse.angularImpulseBody1; + } + if (constraint.isBody2Moving) { + mLinearVelocities[constraint.indexBody2] += constraint.massInverseBody2 * + impulse.linearImpulseBody2; + mAngularVelocities[constraint.indexBody2] += constraint.inverseInertiaTensorBody2 * + impulse.angularImpulseBody2; + } +} + + diff --git a/src/engine/ConstraintSolver.h b/src/engine/ConstraintSolver.h index 3ac82eb6..7a703b0d 100644 --- a/src/engine/ConstraintSolver.h +++ b/src/engine/ConstraintSolver.h @@ -39,6 +39,24 @@ namespace reactphysics3d { // Declarations class DynamicsWorld; +// Structure Impulse +struct Impulse { + + public: + Vector3& linearImpulseBody1; + Vector3& linearImpulseBody2; + Vector3& angularImpulseBody1; + Vector3& angularImpulseBody2; + + // Constructor + Impulse(Vector3& linearImpulseBody1, Vector3& angularImpulseBody1, + Vector3& linearImpulseBody2, Vector3& angularImpulseBody2) + : linearImpulseBody1(linearImpulseBody1), angularImpulseBody1(angularImpulseBody1), + linearImpulseBody2(linearImpulseBody2), angularImpulseBody2(angularImpulseBody2) { + + } +}; + // Structure ContactPointConstraint // Internal structure for a contact point constraint struct ContactPointConstraint { @@ -137,9 +155,7 @@ class ConstraintSolver { std::vector activeConstraints; // Current active constraints in the physics world bool isErrorCorrectionActive; // True if error correction (with world order) is active 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) 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 @@ -151,21 +167,8 @@ class ConstraintSolver { 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 J_spError[NB_MAX_CONSTRAINTS][2*6]; // Same as J_sp for error correction projection (only contact constraints) - decimal B_spError[2][6*NB_MAX_CONSTRAINTS]; // Same as B_sp for error correction projection (only contact constraints) decimal b[NB_MAX_CONSTRAINTS]; // Vector "b" of the LCP problem - decimal bError[NB_MAX_CONSTRAINTS]; // Vector "b" of the LCP problem for error correction projection decimal d[NB_MAX_CONSTRAINTS]; // Vector "d" - decimal aError[6*NB_MAX_BODIES]; // Vector "a" for error correction projection - decimal penetrationDepths[NB_MAX_CONSTRAINTS]; // Array of penetration depths for error correction projection - decimal lambda[NB_MAX_CONSTRAINTS]; // Lambda vector of the LCP problem - decimal lambdaError[NB_MAX_CONSTRAINTS]; // Lambda vector of the LCP problem for error correction projections - decimal lambdaInit[NB_MAX_CONSTRAINTS]; // Lambda init vector for the LCP solver - decimal errorValues[NB_MAX_CONSTRAINTS]; // Error vector of all constraints - decimal lowerBounds[NB_MAX_CONSTRAINTS]; // Vector that contains the low limits for the variables of the LCP problem - decimal upperBounds[NB_MAX_CONSTRAINTS]; // Vector that contains the high limits for the variables of the LCP problem - decimal lowerBoundsError[NB_MAX_CONSTRAINTS]; // Same as "lowerBounds" but for error correction projections - decimal upperBoundsError[NB_MAX_CONSTRAINTS]; // Same as "upperBounds" but for error correction projections 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 @@ -193,7 +196,13 @@ class ConstraintSolver { 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 - + + // Apply an impulse to the two bodies of a constraint + void applyImpulse(const Impulse& impulse, const ContactConstraint& constraint); + + // Compute the collision restitution factor from the restitution factor of each body + decimal computeMixRestitutionFactor(const RigidBody *body1, const RigidBody *body2) const; + public: ConstraintSolver(DynamicsWorld* world); // Constructor virtual ~ConstraintSolver(); // Destructor @@ -249,6 +258,16 @@ inline void ConstraintSolver::setNbLCPIterations(uint nbIterations) { mNbIterations = nbIterations; } +// Compute the collision restitution factor from the restitution factor of each body +inline decimal ConstraintSolver::computeMixRestitutionFactor(const RigidBody* body1, + const RigidBody* body2) const { + decimal restitution1 = body1->getRestitution(); + decimal restitution2 = body2->getRestitution(); + + // Return the largest restitution factor + return (restitution1 > restitution2) ? restitution1 : restitution2; +} + } // End of ReactPhysics3D namespace #endif From 9cf672c51c8f4c3e49e9fbf210c0bda3b3b63935 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 16 Jan 2013 19:47:49 +0100 Subject: [PATCH 18/26] Don't use the contact.B_sp fields anymore in the solveLCP() method and fix bug with contact.r2CrossN that were not initialized in the constraint solver --- src/engine/ConstraintSolver.cpp | 76 ++++++++++++++++++--------------- src/engine/ConstraintSolver.h | 12 +++--- 2 files changed, 47 insertions(+), 41 deletions(-) diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp index 33347390..451539b6 100644 --- a/src/engine/ConstraintSolver.cpp +++ b/src/engine/ConstraintSolver.cpp @@ -175,6 +175,7 @@ void ConstraintSolver::initializeContactConstraints(decimal dt) { Contact* realContact = contact.contact; contact.r1CrossN = contact.r1.cross(contact.normal); + contact.r2CrossN = contact.r2.cross(contact.normal); contact.r1CrossT1 = contact.r1.cross(contact.frictionVector1); contact.r1CrossT2 = contact.r1.cross(contact.frictionVector2); contact.r2CrossT1 = contact.r2.cross(contact.frictionVector1); @@ -426,63 +427,68 @@ void ConstraintSolver::solveLCP() { // Compute the Lagrange multiplier deltaLambda = - (Jv + b); - /* - deltaLambda = 0.0; - for (uint j=0; j<3; j++) { - deltaLambda -= (contact.J_spBody1Penetration[j] * mLinearVelocities[indexBody1Array][j] + contact.J_spBody2Penetration[j] * mLinearVelocities[indexBody2Array][j]); - deltaLambda -= (contact.J_spBody1Penetration[j + 3] * mAngularVelocities[indexBody1Array][j] + contact.J_spBody2Penetration[j + 3] * mAngularVelocities[indexBody2Array][j]); - } - */ //deltaLambda -= contact.b_Penetration; deltaLambda /= contact.inversePenetrationMass; lambdaTemp = contact.penetrationImpulse; contact.penetrationImpulse = std::max(contact.lowerBoundPenetration, std::min(contact.penetrationImpulse + deltaLambda, contact.upperBoundPenetration)); deltaLambda = contact.penetrationImpulse - lambdaTemp; - for (uint j=0; j<3; j++) { - mLinearVelocities[indexBody1Array][j] += contact.B_spBody1Penetration[j] * deltaLambda; - mAngularVelocities[indexBody1Array][j] += contact.B_spBody1Penetration[j + 3] * deltaLambda; - mLinearVelocities[indexBody2Array][j] += contact.B_spBody2Penetration[j] * deltaLambda; - mAngularVelocities[indexBody2Array][j] += contact.B_spBody2Penetration[j + 3] * deltaLambda; - } + // Compute the impulse P=J^T * lambda + const Vector3 linearImpulseBody1 = -contact.normal * deltaLambda; + const Vector3 angularImpulseBody1 = -contact.r1CrossN * deltaLambda; + const Vector3 linearImpulseBody2 = contact.normal * deltaLambda; + const Vector3 angularImpulseBody2 = contact.r2CrossN * deltaLambda; + const Impulse impulsePenetration(linearImpulseBody1, angularImpulseBody1, + linearImpulseBody2, angularImpulseBody2); + + // Apply the impulse to the bodies of the constraint + applyImpulse(impulsePenetration, constraint); // --------- Friction 1 --------- // - deltaLambda = 0.0; - for (uint j=0; j<3; j++) { - deltaLambda -= (contact.J_spBody1Friction1[j] * mLinearVelocities[indexBody1Array][j] + contact.J_spBody2Friction1[j] * mLinearVelocities[indexBody2Array][j]); - deltaLambda -= (contact.J_spBody1Friction1[j + 3] * mAngularVelocities[indexBody1Array][j] + contact.J_spBody2Friction1[j + 3] * mAngularVelocities[indexBody2Array][j]); - } + // Compute J*v + deltaV = v2 + w2.cross(contact.r2) - v1 - w1.cross(contact.r1); + Jv = deltaV.dot(contact.frictionVector1); + + deltaLambda = -Jv; deltaLambda /= contact.inverseFriction1Mass; lambdaTemp = contact.friction1Impulse; contact.friction1Impulse = std::max(contact.lowerBoundFriction1, std::min(contact.friction1Impulse + deltaLambda, contact.upperBoundFriction1)); deltaLambda = contact.friction1Impulse - lambdaTemp; - for (uint j=0; j<3; j++) { - mLinearVelocities[indexBody1Array][j] += contact.B_spBody1Friction1[j] * deltaLambda; - mAngularVelocities[indexBody1Array][j] += contact.B_spBody1Friction1[j + 3] * deltaLambda; - mLinearVelocities[indexBody2Array][j] += contact.B_spBody2Friction1[j] * deltaLambda; - mAngularVelocities[indexBody2Array][j] += contact.B_spBody2Friction1[j + 3] * deltaLambda; - } + // Compute the impulse P=J^T * lambda + Vector3 linearImpulseBody1Friction1 = -contact.frictionVector1 * deltaLambda; + Vector3 angularImpulseBody1Friction1 = -contact.r1CrossT1 * deltaLambda; + Vector3 linearImpulseBody2Friction1 = contact.frictionVector1 * deltaLambda; + Vector3 angularImpulseBody2Friction1 = contact.r2CrossT1 * deltaLambda; + Impulse impulseFriction1(linearImpulseBody1Friction1, angularImpulseBody1Friction1, + linearImpulseBody2Friction1, angularImpulseBody2Friction1); + + // Apply the impulses to the bodies of the constraint + applyImpulse(impulseFriction1, constraint); // --------- Friction 2 --------- // - deltaLambda = 0.0; - for (uint j=0; j<3; j++) { - deltaLambda -= (contact.J_spBody1Friction2[j] * mLinearVelocities[indexBody1Array][j] + contact.J_spBody2Friction2[j] * mLinearVelocities[indexBody2Array][j]); - deltaLambda -= (contact.J_spBody1Friction2[j + 3] * mAngularVelocities[indexBody1Array][j] + contact.J_spBody2Friction2[j + 3] * mAngularVelocities[indexBody2Array][j]); - } + // Compute J*v + deltaV = v2 + w2.cross(contact.r2) - v1 - w1.cross(contact.r1); + Jv = deltaV.dot(contact.frictionVector2); + + deltaLambda = -Jv; deltaLambda /= contact.inverseFriction2Mass; lambdaTemp = contact.friction2Impulse; contact.friction2Impulse = std::max(contact.lowerBoundFriction2, std::min(contact.friction2Impulse + deltaLambda, contact.upperBoundFriction2)); deltaLambda = contact.friction2Impulse - lambdaTemp; - for (uint j=0; j<3; j++) { - mLinearVelocities[indexBody1Array][j] += contact.B_spBody1Friction2[j] * deltaLambda; - mAngularVelocities[indexBody1Array][j] += contact.B_spBody1Friction2[j + 3] * deltaLambda; - mLinearVelocities[indexBody2Array][j] += contact.B_spBody2Friction2[j] * deltaLambda; - mAngularVelocities[indexBody2Array][j] += contact.B_spBody2Friction2[j + 3] * deltaLambda; - } + // Compute the impulse P=J^T * lambda + Vector3 linearImpulseBody1Friction2 = -contact.frictionVector2 * deltaLambda; + Vector3 angularImpulseBody1Friction2 = -contact.r1CrossT2 * deltaLambda; + Vector3 linearImpulseBody2Friction2 = contact.frictionVector2 * deltaLambda; + Vector3 angularImpulseBody2Friction2 = contact.r2CrossT2 * deltaLambda; + Impulse impulseFriction2(linearImpulseBody1Friction2, angularImpulseBody1Friction2, + linearImpulseBody2Friction2, angularImpulseBody2Friction2); + + // Apply the impulses to the bodies of the constraint + applyImpulse(impulseFriction2, constraint); } } } diff --git a/src/engine/ConstraintSolver.h b/src/engine/ConstraintSolver.h index 7a703b0d..254364c9 100644 --- a/src/engine/ConstraintSolver.h +++ b/src/engine/ConstraintSolver.h @@ -43,14 +43,14 @@ class DynamicsWorld; struct Impulse { public: - Vector3& linearImpulseBody1; - Vector3& linearImpulseBody2; - Vector3& angularImpulseBody1; - Vector3& angularImpulseBody2; + const Vector3& linearImpulseBody1; + const Vector3& linearImpulseBody2; + const Vector3& angularImpulseBody1; + const Vector3& angularImpulseBody2; // Constructor - Impulse(Vector3& linearImpulseBody1, Vector3& angularImpulseBody1, - Vector3& linearImpulseBody2, Vector3& angularImpulseBody2) + Impulse(const Vector3& linearImpulseBody1, const Vector3& angularImpulseBody1, + const Vector3& linearImpulseBody2, const Vector3& angularImpulseBody2) : linearImpulseBody1(linearImpulseBody1), angularImpulseBody1(angularImpulseBody1), linearImpulseBody2(linearImpulseBody2), angularImpulseBody2(angularImpulseBody2) { From 2d0da2cc1c52a00dfb7482de73b26ef0106a79a1 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Thu, 17 Jan 2013 23:13:18 +0100 Subject: [PATCH 19/26] Clean up the code and change the warmstart() method --- src/engine/ConstraintSolver.cpp | 309 ++++++++------------------------ src/engine/ConstraintSolver.h | 99 +++++----- 2 files changed, 127 insertions(+), 281 deletions(-) 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(); From d546d8208f9af8f39878b5feed2f78613dd96324 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sun, 27 Jan 2013 10:38:41 +0100 Subject: [PATCH 20/26] Use first friction vector in the direction of the tangential velocity --- src/constraint/Contact.cpp | 3 +- src/constraint/Contact.h | 43 +++++- src/engine/ConstraintSolver.cpp | 228 ++++++++++++++++++++------------ src/engine/ConstraintSolver.h | 15 ++- src/mathematics/Vector3.cpp | 24 ++++ src/mathematics/Vector3.h | 9 ++ 6 files changed, 222 insertions(+), 100 deletions(-) diff --git a/src/constraint/Contact.cpp b/src/constraint/Contact.cpp index e83feb5f..e89341b4 100644 --- a/src/constraint/Contact.cpp +++ b/src/constraint/Contact.cpp @@ -36,7 +36,8 @@ Contact::Contact(RigidBody* const body1, RigidBody* const body2, const ContactIn mLocalPointOnBody1(contactInfo->localPoint1), mLocalPointOnBody2(contactInfo->localPoint2), mWorldPointOnBody1(body1->getTransform() * contactInfo->localPoint1), - mWorldPointOnBody2(body2->getTransform() * contactInfo->localPoint2) { + mWorldPointOnBody2(body2->getTransform() * contactInfo->localPoint2), + mIsRestingContact(false) { assert(mPenetrationDepth > 0.0); // Compute the auxiliary lower and upper bounds diff --git a/src/constraint/Contact.h b/src/constraint/Contact.h index 57203845..05d8f215 100644 --- a/src/constraint/Contact.h +++ b/src/constraint/Contact.h @@ -85,6 +85,9 @@ class Contact : public Constraint { // Contact point on body 2 in world space Vector3 mWorldPointOnBody2; + // True if the contact is a resting contact (exists for more than one time step) + bool mIsRestingContact; + // Two orthogonal vectors that span the tangential friction plane std::vector mFrictionVectors; @@ -135,12 +138,24 @@ class Contact : public Constraint { // Set the contact world point on body 2 void setWorldPointOnBody2(const Vector3& worldPoint); + // Return true if the contact is a resting contact + bool getIsRestingContact() const; + + // Set the mIsRestingContact variable + void setIsRestingContact(bool isRestingContact); + // Get the first friction vector Vector3 getFrictionVector1() const; + // Set the first friction vector + void setFrictionVector1(const Vector3& frictionVector1); + // Get the second friction vector Vector3 getFrictionVector2() const; + // Set the second friction vector + void setFrictionVector2(const Vector3& frictionVector2); + // Compute the jacobian matrix for all mathematical constraints virtual void computeJacobian(int noConstraint, decimal J_SP[NB_MAX_CONSTRAINTS][2*6]) const; @@ -187,12 +202,8 @@ inline void Contact::computeFrictionVectors() { // Delete the current friction vectors mFrictionVectors.clear(); - // Compute the first orthogonal vector - Vector3 vector1 = mNormal.getOneOrthogonalVector().getUnit(); - mFrictionVectors.push_back(vector1); - - // Compute the second orthogonal vector using the cross product - mFrictionVectors.push_back(mNormal.cross(vector1).getUnit()); + mFrictionVectors.push_back(Vector3(0, 0, 0)); + mFrictionVectors.push_back(Vector3(0, 0, 0)); } // Return the normal vector of the contact @@ -235,16 +246,36 @@ inline void Contact::setWorldPointOnBody2(const Vector3& worldPoint) { mWorldPointOnBody2 = worldPoint; } +// Return true if the contact is a resting contact +inline bool Contact::getIsRestingContact() const { + return mIsRestingContact; +} + +// Set the mIsRestingContact variable +inline void Contact::setIsRestingContact(bool isRestingContact) { + mIsRestingContact = isRestingContact; +} + // Get the first friction vector inline Vector3 Contact::getFrictionVector1() const { return mFrictionVectors[0]; } +// Set the first friction vector +inline void Contact::setFrictionVector1(const Vector3& frictionVector1) { + mFrictionVectors[0] = frictionVector1; +} + // Get the second friction vector inline Vector3 Contact::getFrictionVector2() const { return mFrictionVectors[1]; } +// Set the second friction vector +inline void Contact::setFrictionVector2(const Vector3& frictionVector2) { + mFrictionVectors[1] = frictionVector2; +} + // Return the penetration depth of the contact inline decimal Contact::getPenetrationDepth() const { return mPenetrationDepth; diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp index b9dfdf3a..3d8d0507 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), mIsWarmStartingActive(false) { } @@ -103,9 +103,11 @@ void ConstraintSolver::initialize() { contactPointConstraint.normal = contact->getNormal(); contactPointConstraint.r1 = p1 - x1; contactPointConstraint.r2 = p2 - x2; - contactPointConstraint.frictionVector1 = contact->getFrictionVector1(); - contactPointConstraint.frictionVector2 = contact->getFrictionVector2(); contactPointConstraint.penetrationDepth = contact->getPenetrationDepth(); + contactPointConstraint.isRestingContact = contact->getIsRestingContact(); + contact->setIsRestingContact(true); + contactPointConstraint.oldFrictionVector1 = contact->getFrictionVector1(); + contactPointConstraint.oldFrictionVector2 = contact->getFrictionVector2(); } mNbContactConstraints++; @@ -154,6 +156,15 @@ void ConstraintSolver::initializeContactConstraints() { ContactPointConstraint& contact = constraint.contacts[i]; Contact* realContact = contact.contact; + const Vector3& v1 = mLinearVelocities[constraint.indexBody1]; + const Vector3& w1 = mAngularVelocities[constraint.indexBody1]; + const Vector3& v2 = mLinearVelocities[constraint.indexBody2]; + const Vector3& w2 = mAngularVelocities[constraint.indexBody2]; + Vector3 deltaV = v2 + w2.cross(contact.r2) - v1 - w1.cross(contact.r1); + + // Compute the friction vectors + computeFrictionVectors(deltaV, contact); + contact.r1CrossN = contact.r1.cross(contact.normal); contact.r2CrossN = contact.r2.cross(contact.normal); contact.r1CrossT1 = contact.r1.cross(contact.frictionVector1); @@ -161,54 +172,42 @@ void ConstraintSolver::initializeContactConstraints() { contact.r2CrossT1 = contact.r2.cross(contact.frictionVector1); contact.r2CrossT2 = contact.r2.cross(contact.frictionVector2); - contact.inversePenetrationMass = 0.0; + decimal massPenetration = 0.0; if (constraint.isBody1Moving) { - contact.inversePenetrationMass += constraint.massInverseBody1 + + massPenetration += constraint.massInverseBody1 + ((I1 * contact.r1CrossN).cross(contact.r1)).dot(contact.normal); } if (constraint.isBody2Moving) { - contact.inversePenetrationMass += constraint.massInverseBody2 + + massPenetration += constraint.massInverseBody2 + ((I2 * contact.r2CrossN).cross(contact.r2)).dot(contact.normal); } + massPenetration > 0.0 ? contact.inversePenetrationMass = 1.0 / massPenetration : 0.0; // Compute the inverse mass matrix K for the friction constraints - contact.inverseFriction1Mass = 0.0; - contact.inverseFriction2Mass = 0.0; + decimal friction1Mass = 0.0; + decimal friction2Mass = 0.0; if (constraint.isBody1Moving) { - contact.inverseFriction1Mass += constraint.massInverseBody1 + ((I1 * contact.r1CrossT1).cross(contact.r1)).dot(contact.frictionVector1); - contact.inverseFriction2Mass += constraint.massInverseBody1 + ((I1 * contact.r1CrossT2).cross(contact.r1)).dot(contact.frictionVector2); + friction1Mass += constraint.massInverseBody1 + ((I1 * contact.r1CrossT1).cross(contact.r1)).dot(contact.frictionVector1); + friction2Mass += constraint.massInverseBody1 + ((I1 * contact.r1CrossT2).cross(contact.r1)).dot(contact.frictionVector2); } if (constraint.isBody2Moving) { - contact.inverseFriction1Mass += constraint.massInverseBody2 + ((I2 * contact.r2CrossT1).cross(contact.r2)).dot(contact.frictionVector1); - contact.inverseFriction2Mass += constraint.massInverseBody2 + ((I2 * contact.r2CrossT2).cross(contact.r2)).dot(contact.frictionVector2); + friction1Mass += constraint.massInverseBody2 + ((I2 * contact.r2CrossT1).cross(contact.r2)).dot(contact.frictionVector1); + friction2Mass += constraint.massInverseBody2 + ((I2 * contact.r2CrossT2).cross(contact.r2)).dot(contact.frictionVector2); } - - const Vector3& v1 = mLinearVelocities[constraint.indexBody1]; - const Vector3& w1 = mAngularVelocities[constraint.indexBody1]; - const Vector3& v2 = mLinearVelocities[constraint.indexBody2]; - const Vector3& w2 = mAngularVelocities[constraint.indexBody2]; + friction1Mass > 0.0 ? contact.inverseFriction1Mass = 1.0 / friction1Mass : 0.0; + friction2Mass > 0.0 ? contact.inverseFriction2Mass = 1.0 / friction2Mass : 0.0; // Compute the restitution velocity bias "b". We compute this here instead // of inside the solve() method because we need to use the velocity difference // at the beginning of the contact. Note that if it is a resting contact (normal velocity // under a given threshold), we don't add a restitution velocity bias contact.restitutionBias = 0.0; - Vector3 deltaV = v2 + w2.cross(contact.r2) - v1 - w1.cross(contact.r1); decimal deltaVDotN = deltaV.dot(contact.normal); // TODO : Use a constant here - decimal elasticLinearVelocityThreshold = 0.3; - if (deltaVDotN < -elasticLinearVelocityThreshold) { + if (!contact.isRestingContact) { contact.restitutionBias = constraint.restitutionFactor * deltaVDotN; } - // 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); - // Get the cached lambda values of the constraint contact.penetrationImpulse = realContact->getCachedLambda(0); contact.friction1Impulse = realContact->getCachedLambda(1); @@ -232,44 +231,60 @@ void ConstraintSolver::warmStart() { ContactPointConstraint& contact = constraint.contacts[i]; - // --------- Penetration --------- // + // If it is not a new contact (this contact was already existing at last time step) + if (contact.isRestingContact) { - // Compute the impulse P=J^T * lambda - const Vector3 linearImpulseBody1 = -contact.normal * contact.penetrationImpulse; - const Vector3 angularImpulseBody1 = -contact.r1CrossN * contact.penetrationImpulse; - const Vector3 linearImpulseBody2 = contact.normal * contact.penetrationImpulse; - const Vector3 angularImpulseBody2 = contact.r2CrossN * contact.penetrationImpulse; - const Impulse impulsePenetration(linearImpulseBody1, angularImpulseBody1, - linearImpulseBody2, angularImpulseBody2); + // --------- Penetration --------- // - // Apply the impulse to the bodies of the constraint - applyImpulse(impulsePenetration, constraint); + // Compute the impulse P=J^T * lambda + const Vector3 linearImpulseBody1 = -contact.normal * contact.penetrationImpulse; + const Vector3 angularImpulseBody1 = -contact.r1CrossN * contact.penetrationImpulse; + const Vector3 linearImpulseBody2 = contact.normal * contact.penetrationImpulse; + const Vector3 angularImpulseBody2 = contact.r2CrossN * contact.penetrationImpulse; + const Impulse impulsePenetration(linearImpulseBody1, angularImpulseBody1, + linearImpulseBody2, angularImpulseBody2); - // --------- Friction 1 --------- // + // Apply the impulse to the bodies of the constraint + applyImpulse(impulsePenetration, constraint); - // Compute the impulse P=J^T * lambda - Vector3 linearImpulseBody1Friction1 = -contact.frictionVector1 * contact.friction1Impulse; - Vector3 angularImpulseBody1Friction1 = -contact.r1CrossT1 * contact.friction1Impulse; - Vector3 linearImpulseBody2Friction1 = contact.frictionVector1 * contact.friction1Impulse; - Vector3 angularImpulseBody2Friction1 = contact.r2CrossT1 * contact.friction1Impulse; - Impulse impulseFriction1(linearImpulseBody1Friction1, angularImpulseBody1Friction1, - linearImpulseBody2Friction1, angularImpulseBody2Friction1); + // --------- Friction 1 --------- // - // Apply the impulses to the bodies of the constraint - applyImpulse(impulseFriction1, constraint); + Vector3 oldFrictionImpulse = contact.friction1Impulse * contact.oldFrictionVector1 + + contact.friction2Impulse * contact.oldFrictionVector2; + contact.friction1Impulse = oldFrictionImpulse.dot(contact.frictionVector1); + contact.friction2Impulse = oldFrictionImpulse.dot(contact.frictionVector2); - // --------- Friction 2 --------- // + // Compute the impulse P=J^T * lambda + Vector3 linearImpulseBody1Friction1 = -contact.frictionVector1 * contact.friction1Impulse; + Vector3 angularImpulseBody1Friction1 = -contact.r1CrossT1 * contact.friction1Impulse; + Vector3 linearImpulseBody2Friction1 = contact.frictionVector1 * contact.friction1Impulse; + Vector3 angularImpulseBody2Friction1 = contact.r2CrossT1 * contact.friction1Impulse; + Impulse impulseFriction1(linearImpulseBody1Friction1, angularImpulseBody1Friction1, + linearImpulseBody2Friction1, angularImpulseBody2Friction1); - // Compute the impulse P=J^T * lambda - Vector3 linearImpulseBody1Friction2 = -contact.frictionVector2 * contact.friction2Impulse; - Vector3 angularImpulseBody1Friction2 = -contact.r1CrossT2 * contact.friction2Impulse; - Vector3 linearImpulseBody2Friction2 = contact.frictionVector2 * contact.friction2Impulse; - Vector3 angularImpulseBody2Friction2 = contact.r2CrossT2 * contact.friction2Impulse; - Impulse impulseFriction2(linearImpulseBody1Friction2, angularImpulseBody1Friction2, - linearImpulseBody2Friction2, angularImpulseBody2Friction2); + // Apply the impulses to the bodies of the constraint + applyImpulse(impulseFriction1, constraint); - // Apply the impulses to the bodies of the constraint - applyImpulse(impulseFriction2, constraint); + // --------- Friction 2 --------- // + + // Compute the impulse P=J^T * lambda + Vector3 linearImpulseBody1Friction2 = -contact.frictionVector2 * contact.friction2Impulse; + Vector3 angularImpulseBody1Friction2 = -contact.r1CrossT2 * contact.friction2Impulse; + Vector3 linearImpulseBody2Friction2 = contact.frictionVector2 * contact.friction2Impulse; + Vector3 angularImpulseBody2Friction2 = contact.r2CrossT2 * contact.friction2Impulse; + Impulse impulseFriction2(linearImpulseBody1Friction2, angularImpulseBody1Friction2, + linearImpulseBody2Friction2, angularImpulseBody2Friction2); + + // Apply the impulses to the bodies of the constraint + applyImpulse(impulseFriction2, constraint); + } + else { // If it is a new contact + + // Initialize the accumulated impulses to zero + contact.penetrationImpulse = 0.0; + contact.friction1Impulse = 0.0; + contact.friction2Impulse = 0.0; + } } } } @@ -281,9 +296,6 @@ void ConstraintSolver::solveContactConstraints() { decimal lambdaTemp; uint iter; - // Compute the vector a - warmStart(); - // For each iteration for(iter=0; itersetCachedLambda(0, contact.penetrationImpulse); - contact.contact->setCachedLambda(1, contact.friction1Impulse); - contact.contact->setCachedLambda(2, contact.friction2Impulse); - } - } -} - // Solve the constraints void ConstraintSolver::solve(decimal timeStep) { @@ -420,6 +414,11 @@ void ConstraintSolver::solve(decimal timeStep) { // Fill-in all the matrices needed to solve the LCP problem initializeContactConstraints(); + // Warm start the solver + if (mIsWarmStartingActive) { + warmStart(); + } + // Solve the contact constraints solveContactConstraints(); @@ -427,6 +426,29 @@ void ConstraintSolver::solve(decimal timeStep) { storeImpulses(); } +// Store the computed impulses to use them to +// warm start the solver at the next iteration +void ConstraintSolver::storeImpulses() { + + // For each constraint + for (uint c=0; csetCachedLambda(0, contact.penetrationImpulse); + contact.contact->setCachedLambda(1, contact.friction1Impulse); + contact.contact->setCachedLambda(2, contact.friction2Impulse); + + contact.contact->setFrictionVector1(contact.frictionVector1); + contact.contact->setFrictionVector2(contact.frictionVector2); + } + } +} + // Apply an impulse to the two bodies of a constraint void ConstraintSolver::applyImpulse(const Impulse& impulse, const ContactConstraint& constraint) { @@ -445,4 +467,36 @@ void ConstraintSolver::applyImpulse(const Impulse& impulse, const ContactConstra } } +// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane +// The two vectors have to be such that : t1 x t2 = contactNormal +void ConstraintSolver::computeFrictionVectors(const Vector3& deltaVelocity, + ContactPointConstraint& contact) const { + // Update the old friction vectors + //contact.oldFrictionVector1 = contact.frictionVector1; + //contact.oldFrictionVector2 = contact.frictionVector2; + + assert(contact.normal.length() > 0.0); + + // Compute the velocity difference vector in the tangential plane + Vector3 normalVelocity = deltaVelocity.dot(contact.normal) * contact.normal; + Vector3 tangentVelocity = deltaVelocity - normalVelocity; + + // If the velocty difference in the tangential plane is not zero + decimal lengthTangenVelocity = tangentVelocity.length(); + if (lengthTangenVelocity > 0.0) { + + // Compute the first friction vector in the direction of the tangent + // velocity difference + contact.frictionVector1 = tangentVelocity / lengthTangenVelocity; + } + else { + + // Get any orthogonal vector to the normal as the first friction vector + contact.frictionVector1 = contact.normal.getOneUnitOrthogonalVector(); + } + + // The second friction vector is computed by the cross product of the firs + // friction vector and the contact normal + contact.frictionVector2 = contact.normal.cross(contact.frictionVector1).getUnit(); +} diff --git a/src/engine/ConstraintSolver.h b/src/engine/ConstraintSolver.h index 6b842d89..e59657e5 100644 --- a/src/engine/ConstraintSolver.h +++ b/src/engine/ConstraintSolver.h @@ -82,12 +82,7 @@ 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 lowerBoundPenetration; - decimal upperBoundPenetration; - decimal lowerBoundFriction1; - decimal upperBoundFriction1; - decimal lowerBoundFriction2; - decimal upperBoundFriction2; + bool isRestingContact; // True if the contact was existing last time step Contact* contact; // TODO : REMOVE THIS }; @@ -165,6 +160,9 @@ class ConstraintSolver { // Map body to index std::map mMapBodyToIndex; + // True if the warm starting of the solver is active + bool mIsWarmStartingActive; + // -------------------- Methods -------------------- // // Initialize the constraint solver @@ -192,6 +190,11 @@ class ConstraintSolver { // Compute the collision restitution factor from the restitution factor of each body decimal computeMixRestitutionFactor(const RigidBody *body1, const RigidBody *body2) const; + // Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane + // The two vectors have to be such that : t1 x t2 = contactNormal + void computeFrictionVectors(const Vector3& deltaVelocity, + ContactPointConstraint& contact) const; + public: // -------------------- Methods -------------------- // diff --git a/src/mathematics/Vector3.cpp b/src/mathematics/Vector3.cpp index 437448e8..188c9933 100644 --- a/src/mathematics/Vector3.cpp +++ b/src/mathematics/Vector3.cpp @@ -97,3 +97,27 @@ Vector3 Vector3::getOneOrthogonalVector() const { //assert(vector1.isUnit()); return vector1; } + +// Return one unit orthogonal vector of the current vector +Vector3 Vector3::getOneUnitOrthogonalVector() const { + assert(!this->isZero()); + + decimal x = mValues[0]; + decimal y = mValues[1]; + decimal z = mValues[2]; + + // Get the minimum element of the vector + Vector3 vectorAbs(fabs(x), fabs(y), fabs(z)); + int minElement = vectorAbs.getMinAxis(); + + if (minElement == 0) { + return Vector3(0.0, -z, y) / sqrt(y*y + z*z); + } + else if (minElement == 1) { + return Vector3(-z, 0.0, x) / sqrt(x*x + z*z); + } + else { + return Vector3(-y, x, 0.0) / sqrt(x*x + y*y); + } + +} diff --git a/src/mathematics/Vector3.h b/src/mathematics/Vector3.h index 7cce12d5..f8599c4e 100644 --- a/src/mathematics/Vector3.h +++ b/src/mathematics/Vector3.h @@ -95,6 +95,9 @@ class Vector3 { // Return the corresponding unit vector Vector3 getUnit() const; + // Return one unit orthogonal vector of the current vector + Vector3 getOneUnitOrthogonalVector() const; + // Return true if the vector is unit and false otherwise bool isUnit() const; @@ -153,6 +156,7 @@ class Vector3 { friend Vector3 operator-(const Vector3& vector); friend Vector3 operator*(const Vector3& vector, decimal number); friend Vector3 operator*(decimal number, const Vector3& vector); + friend Vector3 operator/(const Vector3& vector, decimal number); }; // Get the x component of the vector @@ -313,6 +317,11 @@ inline Vector3 operator*(const Vector3& vector, decimal number) { return Vector3(number * vector.mValues[0], number * vector.mValues[1], number * vector.mValues[2]); } +// Overloaded operator for division by a number +inline Vector3 operator/(const Vector3& vector, decimal number) { + return Vector3(vector.mValues[0] / number, vector.mValues[1] / number, vector.mValues[2] / number); +} + // Overloaded operator for multiplication with a number inline Vector3 operator*(decimal number, const Vector3& vector) { return vector * number; From 8cde68f5b9225bd191edb300c5e0d55b8b7030db Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Thu, 31 Jan 2013 22:42:11 +0100 Subject: [PATCH 21/26] Implement the split impulse technique for position correction --- src/engine/ConstraintSolver.cpp | 69 +++++++++++++++++++++++++++++---- src/engine/ConstraintSolver.h | 42 ++++++++++++++++++-- src/engine/DynamicsWorld.cpp | 14 +++++-- src/engine/DynamicsWorld.h | 4 +- 4 files changed, 111 insertions(+), 18 deletions(-) diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp index 3d8d0507..6bd8a941 100644 --- a/src/engine/ConstraintSolver.cpp +++ b/src/engine/ConstraintSolver.cpp @@ -31,11 +31,11 @@ using namespace reactphysics3d; using namespace std; - // Constructor ConstraintSolver::ConstraintSolver(DynamicsWorld* world) :world(world), nbConstraints(0), mNbIterations(10), mContactConstraints(0), - mLinearVelocities(0), mAngularVelocities(0), mIsWarmStartingActive(false) { + mLinearVelocities(0), mAngularVelocities(0), mIsWarmStartingActive(true), + mIsSplitImpulseActive(true) { } @@ -118,6 +118,8 @@ void ConstraintSolver::initialize() { mLinearVelocities = new Vector3[nbBodies]; mAngularVelocities = new Vector3[nbBodies]; + mSplitLinearVelocities = new Vector3[nbBodies]; + mSplitAngularVelocities = new Vector3[nbBodies]; assert(mMapBodyToIndex.size() == nbBodies); } @@ -136,6 +138,8 @@ void ConstraintSolver::initializeBodies() { mLinearVelocities[bodyNumber] = rigidBody->getLinearVelocity() + mTimeStep * rigidBody->getMassInverse() * rigidBody->getExternalForce(); mAngularVelocities[bodyNumber] = rigidBody->getAngularVelocity() + mTimeStep * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque(); + mSplitLinearVelocities[bodyNumber] = Vector3(0, 0, 0); + mSplitAngularVelocities[bodyNumber] = Vector3(0, 0, 0); } } @@ -204,7 +208,7 @@ void ConstraintSolver::initializeContactConstraints() { contact.restitutionBias = 0.0; decimal deltaVDotN = deltaV.dot(contact.normal); // TODO : Use a constant here - if (!contact.isRestingContact) { + if (deltaVDotN < 1.0f) { contact.restitutionBias = constraint.restitutionFactor * deltaVDotN; } @@ -212,6 +216,9 @@ void ConstraintSolver::initializeContactConstraints() { contact.penetrationImpulse = realContact->getCachedLambda(0); contact.friction1Impulse = realContact->getCachedLambda(1); contact.friction2Impulse = realContact->getCachedLambda(2); + + // Initialize the split impulses to zero + contact.penetrationSplitImpulse = 0.0; } } } @@ -323,17 +330,19 @@ void ConstraintSolver::solveContactConstraints() { // Compute the bias "b" of the constraint decimal beta = 0.2; // TODO : Use a constant for the slop - decimal slop = 0.005; + decimal slop = 0.01; decimal biasPenetrationDepth = 0.0; if (contact.penetrationDepth > slop) biasPenetrationDepth = -(beta/mTimeStep) * max(0.0f, float(contact.penetrationDepth - slop)); decimal b = biasPenetrationDepth + contact.restitutionBias; // Compute the Lagrange multiplier - deltaLambda = - (Jv + b); - - //deltaLambda -= contact.b_Penetration; - deltaLambda *= contact.inversePenetrationMass; + if (mIsSplitImpulseActive) { + deltaLambda = - (Jv + contact.restitutionBias) * contact.inversePenetrationMass; + } + else { + deltaLambda = - (Jv + b) * contact.inversePenetrationMass; + } lambdaTemp = contact.penetrationImpulse; contact.penetrationImpulse = std::max(contact.penetrationImpulse + deltaLambda, 0.0f); deltaLambda = contact.penetrationImpulse - lambdaTemp; @@ -349,6 +358,32 @@ void ConstraintSolver::solveContactConstraints() { // Apply the impulse to the bodies of the constraint applyImpulse(impulsePenetration, constraint); + // If the split impulse position correction is active + if (mIsSplitImpulseActive) { + + // Split impulse (position correction) + const Vector3& v1Split = mSplitLinearVelocities[constraint.indexBody1]; + const Vector3& w1Split = mSplitAngularVelocities[constraint.indexBody1]; + const Vector3& v2Split = mSplitLinearVelocities[constraint.indexBody2]; + const Vector3& w2Split = mSplitAngularVelocities[constraint.indexBody2]; + Vector3 deltaVSplit = v2Split + w2Split.cross(contact.r2) - v1Split - w1Split.cross(contact.r1); + decimal JvSplit = deltaVSplit.dot(contact.normal); + decimal deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) * contact.inversePenetrationMass; + decimal lambdaTempSplit = contact.penetrationSplitImpulse; + contact.penetrationSplitImpulse = std::max(contact.penetrationSplitImpulse + deltaLambdaSplit, 0.0f); + deltaLambda = contact.penetrationSplitImpulse - lambdaTempSplit; + + // Compute the impulse P=J^T * lambda + linearImpulseBody1 = -contact.normal * deltaLambdaSplit; + angularImpulseBody1 = -contact.r1CrossN * deltaLambdaSplit; + linearImpulseBody2 = contact.normal * deltaLambdaSplit; + angularImpulseBody2 = contact.r2CrossN * deltaLambdaSplit; + const Impulse splitImpulsePenetration(linearImpulseBody1, angularImpulseBody1, + linearImpulseBody2, angularImpulseBody2); + + applySplitImpulse(splitImpulsePenetration, constraint); + } + // --------- Friction 1 --------- // // Compute J*v @@ -467,6 +502,24 @@ void ConstraintSolver::applyImpulse(const Impulse& impulse, const ContactConstra } } +// Apply an impulse to the two bodies of a constraint +void ConstraintSolver::applySplitImpulse(const Impulse& impulse, const ContactConstraint& constraint) { + + // Update the velocities of the bodies by applying the impulse P + if (constraint.isBody1Moving) { + mSplitLinearVelocities[constraint.indexBody1] += constraint.massInverseBody1 * + impulse.linearImpulseBody1; + mSplitAngularVelocities[constraint.indexBody1] += constraint.inverseInertiaTensorBody1 * + impulse.angularImpulseBody1; + } + if (constraint.isBody2Moving) { + mSplitLinearVelocities[constraint.indexBody2] += constraint.massInverseBody2 * + impulse.linearImpulseBody2; + mSplitAngularVelocities[constraint.indexBody2] += constraint.inverseInertiaTensorBody2 * + impulse.angularImpulseBody2; + } +} + // Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane // The two vectors have to be such that : t1 x t2 = contactNormal void ConstraintSolver::computeFrictionVectors(const Vector3& deltaVelocity, diff --git a/src/engine/ConstraintSolver.h b/src/engine/ConstraintSolver.h index e59657e5..9675a63e 100644 --- a/src/engine/ConstraintSolver.h +++ b/src/engine/ConstraintSolver.h @@ -64,6 +64,7 @@ struct ContactPointConstraint { decimal penetrationImpulse; // Accumulated normal impulse decimal friction1Impulse; // Accumulated impulse in the 1st friction direction decimal friction2Impulse; // Accumulated impulse in the 2nd friction direction + decimal penetrationSplitImpulse; // Accumulated split impulse for penetration correction Vector3 normal; // Normal vector of the contact Vector3 frictionVector1; // First friction vector in the tangent plane Vector3 frictionVector2; // Second friction vector in the tangent plane @@ -146,6 +147,13 @@ class ConstraintSolver { // in the J_sp and B_sp matrices. For instance the cell bodyMapping[i][j] contains Vector3* mLinearVelocities; // Array of constrained linear velocities Vector3* mAngularVelocities; // Array of constrained angular velocities + + // Split linear velocities for the position contact solver (split impulse) + Vector3* mSplitLinearVelocities; + + // Split angular velocities for the position contact solver (split impulse) + Vector3* mSplitAngularVelocities; + decimal mTimeStep; // Current time step // Contact constraints @@ -163,6 +171,9 @@ class ConstraintSolver { // True if the warm starting of the solver is active bool mIsWarmStartingActive; + // True if the split impulse position correction is active + bool mIsSplitImpulseActive; + // -------------------- Methods -------------------- // // Initialize the constraint solver @@ -187,6 +198,9 @@ class ConstraintSolver { // Apply an impulse to the two bodies of a constraint void applyImpulse(const Impulse& impulse, const ContactConstraint& constraint); + // Apply an impulse to the two bodies of a constraint + void applySplitImpulse(const Impulse& impulse, const ContactConstraint& constraint); + // Compute the collision restitution factor from the restitution factor of each body decimal computeMixRestitutionFactor(const RigidBody *body1, const RigidBody *body2) const; @@ -214,9 +228,15 @@ class ConstraintSolver { // Return the constrained linear velocity of a body after solving the constraints Vector3 getConstrainedLinearVelocityOfBody(RigidBody *body); + // Return the split linear velocity + Vector3 getSplitLinearVelocityOfBody(RigidBody* body); + // Return the constrained angular velocity of a body after solving the constraints Vector3 getConstrainedAngularVelocityOfBody(RigidBody* body); + // Return the split angular velocity + Vector3 getSplitAngularVelocityOfBody(RigidBody* body); + // Clean up the constraint solver void cleanup(); @@ -232,15 +252,29 @@ inline bool ConstraintSolver::isConstrainedBody(RigidBody* body) const { // 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]; + uint indexBody = mMapBodyToIndex[body]; + return mLinearVelocities[indexBody]; +} + +// Return the split linear velocity +inline Vector3 ConstraintSolver::getSplitLinearVelocityOfBody(RigidBody* body) { + assert(isConstrainedBody(body)); + uint indexBody = mMapBodyToIndex[body]; + return mSplitLinearVelocities[indexBody]; } // 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]; + uint indexBody = mMapBodyToIndex[body]; + return mAngularVelocities[indexBody]; +} + +// Return the split angular velocity +inline Vector3 ConstraintSolver::getSplitAngularVelocityOfBody(RigidBody* body) { + assert(isConstrainedBody(body)); + uint indexBody = mMapBodyToIndex[body]; + return mSplitAngularVelocities[indexBody]; } // Clean up the constraint solver diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index bd4b3c6d..f03a48c1 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -120,8 +120,8 @@ void DynamicsWorld::updateAllBodiesMotion() { // If it's a constrained body if (mConstraintSolver.isConstrainedBody(*it)) { // Get the constrained linear and angular velocities from the constraint solver - newLinearVelocity = mConstraintSolver.getConstrainedLinearVelocityOfBody(*it); - newAngularVelocity = mConstraintSolver.getConstrainedAngularVelocityOfBody(*it); + newLinearVelocity = mConstraintSolver.getConstrainedLinearVelocityOfBody(rigidBody); + newAngularVelocity = mConstraintSolver.getConstrainedAngularVelocityOfBody(rigidBody); } else { // Compute V_forces = dt * (M^-1 * F_ext) which is the velocity of the body due to the @@ -147,8 +147,8 @@ void DynamicsWorld::updateAllBodiesMotion() { // Use the Semi-Implicit Euler (Sympletic Euler) method to compute the new position and the new // orientation of the body void DynamicsWorld::updatePositionAndOrientationOfBody(RigidBody* rigidBody, - const Vector3& newLinVelocity, - const Vector3& newAngVelocity) { + Vector3 newLinVelocity, + Vector3 newAngVelocity) { decimal dt = mTimer.getTimeStep(); assert(rigidBody); @@ -159,6 +159,12 @@ void DynamicsWorld::updatePositionAndOrientationOfBody(RigidBody* rigidBody, // Update the linear and angular velocity of the body rigidBody->setLinearVelocity(newLinVelocity); rigidBody->setAngularVelocity(newAngVelocity); + + // Split velocity (only used to update the position) + if (mConstraintSolver.isConstrainedBody(rigidBody)) { + newLinVelocity += mConstraintSolver.getSplitLinearVelocityOfBody(rigidBody); + newAngVelocity += mConstraintSolver.getSplitAngularVelocityOfBody(rigidBody); + } // Get current position and orientation of the body const Vector3& currentPosition = rigidBody->getTransform().getPosition(); diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index 73737ff9..2d970784 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -96,8 +96,8 @@ class DynamicsWorld : public CollisionWorld { void updateAllBodiesMotion(); // Update the position and orientation of a body - void updatePositionAndOrientationOfBody(RigidBody* body, const Vector3& newLinVelocity, - const Vector3& newAngVelocity); + void updatePositionAndOrientationOfBody(RigidBody* body, Vector3 newLinVelocity, + Vector3 newAngVelocity); // Compute and set the interpolation factor to all bodies void setInterpolationFactorToAllBodies(); From 0695b30704e00b37403162ddfae90643d5b91d42 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sat, 16 Feb 2013 16:14:04 +0100 Subject: [PATCH 22/26] Make possible to solve the friction constraints at the center of the contact manifold --- src/engine/ConstraintSolver.cpp | 460 +++++++++++++++++++++++++------- src/engine/ConstraintSolver.h | 183 ++++++++++--- src/engine/ContactManifold.h | 18 +- src/engine/DynamicsWorld.h | 30 ++- src/mathematics/Vector3.cpp | 1 - src/mathematics/Vector3.h | 24 ++ 6 files changed, 580 insertions(+), 136 deletions(-) diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp index 6bd8a941..8e81b266 100644 --- a/src/engine/ConstraintSolver.cpp +++ b/src/engine/ConstraintSolver.cpp @@ -31,11 +31,16 @@ using namespace reactphysics3d; using namespace std; +// Constants initialization +const decimal ConstraintSolver::BETA = 0.2; +const decimal ConstraintSolver::BETA_SPLIT_IMPULSE = 0.2; +const decimal ConstraintSolver::SLOP = 0.01; + // Constructor ConstraintSolver::ConstraintSolver(DynamicsWorld* world) :world(world), nbConstraints(0), mNbIterations(10), mContactConstraints(0), mLinearVelocities(0), mAngularVelocities(0), mIsWarmStartingActive(true), - mIsSplitImpulseActive(true) { + mIsSplitImpulseActive(true), mIsSolveFrictionAtContactManifoldCenterActive(true) { } @@ -49,7 +54,7 @@ void ConstraintSolver::initialize() { nbConstraints = 0; - // TOOD : Use better allocation here + // TODO : Use better allocation here mContactConstraints = new ContactConstraint[world->getNbContactManifolds()]; mNbContactConstraints = 0; @@ -87,6 +92,13 @@ void ConstraintSolver::initialize() { constraint.massInverseBody2 = body2->getMassInverse(); constraint.nbContacts = contactManifold.nbContacts; constraint.restitutionFactor = computeMixRestitutionFactor(body1, body2); + constraint.contactManifold = &contactManifold; + + // If we solve the friction constraints at the center of the contact manifold + if (mIsSolveFrictionAtContactManifoldCenterActive) { + constraint.frictionPointBody1 = Vector3(0.0, 0.0, 0.0); + constraint.frictionPointBody2 = Vector3(0.0, 0.0, 0.0); + } // For each contact point of the contact manifold for (uint c=0; csetIsRestingContact(true); contactPointConstraint.oldFrictionVector1 = contact->getFrictionVector1(); contactPointConstraint.oldFrictionVector2 = contact->getFrictionVector2(); + contactPointConstraint.penetrationImpulse = 0.0; + contactPointConstraint.friction1Impulse = 0.0; + contactPointConstraint.friction2Impulse = 0.0; + + // If we solve the friction constraints at the center of the contact manifold + if (mIsSolveFrictionAtContactManifoldCenterActive) { + constraint.frictionPointBody1 += p1; + constraint.frictionPointBody2 += p2; + } + } + + // If we solve the friction constraints at the center of the contact manifold + if (mIsSolveFrictionAtContactManifoldCenterActive) { + constraint.frictionPointBody1 /= constraint.nbContacts; + constraint.frictionPointBody2 /= constraint.nbContacts; + constraint.r1Friction = constraint.frictionPointBody1 - x1; + constraint.r2Friction = constraint.frictionPointBody2 - x2; + constraint.oldFrictionVector1 = contactManifold.frictionVector1; + constraint.oldFrictionVector2 = contactManifold.frictionVector2; + + if (mIsWarmStartingActive) { + constraint.friction1Impulse = contactManifold.friction1Impulse; + constraint.friction2Impulse = contactManifold.friction2Impulse; + constraint.frictionTwistImpulse = contactManifold.frictionTwistImpulse; + } + else { + constraint.friction1Impulse = 0.0; + constraint.friction2Impulse = 0.0; + constraint.frictionTwistImpulse = 0.0; + } } mNbContactConstraints++; @@ -154,27 +196,26 @@ void ConstraintSolver::initializeContactConstraints() { Matrix3x3& I1 = constraint.inverseInertiaTensorBody1; Matrix3x3& I2 = constraint.inverseInertiaTensorBody2; + // If we solve the friction constraints at the center of the contact manifold + if (mIsSolveFrictionAtContactManifoldCenterActive) { + constraint.normal = Vector3(0.0, 0.0, 0.0); + } + + const Vector3& v1 = mLinearVelocities[constraint.indexBody1]; + const Vector3& w1 = mAngularVelocities[constraint.indexBody1]; + const Vector3& v2 = mLinearVelocities[constraint.indexBody2]; + const Vector3& w2 = mAngularVelocities[constraint.indexBody2]; + // For each contact point constraint for (uint i=0; i 0.0 ? contact.inversePenetrationMass = 1.0 / massPenetration : 0.0; - // Compute the inverse mass matrix K for the friction constraints - decimal friction1Mass = 0.0; - decimal friction2Mass = 0.0; - if (constraint.isBody1Moving) { - friction1Mass += constraint.massInverseBody1 + ((I1 * contact.r1CrossT1).cross(contact.r1)).dot(contact.frictionVector1); - friction2Mass += constraint.massInverseBody1 + ((I1 * contact.r1CrossT2).cross(contact.r1)).dot(contact.frictionVector2); + if (!mIsSolveFrictionAtContactManifoldCenterActive) { + + // Compute the friction vectors + computeFrictionVectors(deltaV, contact); + + contact.r1CrossT1 = contact.r1.cross(contact.frictionVector1); + contact.r1CrossT2 = contact.r1.cross(contact.frictionVector2); + contact.r2CrossT1 = contact.r2.cross(contact.frictionVector1); + contact.r2CrossT2 = contact.r2.cross(contact.frictionVector2); + + // Compute the inverse mass matrix K for the friction constraints at each contact point + decimal friction1Mass = 0.0; + decimal friction2Mass = 0.0; + if (constraint.isBody1Moving) { + friction1Mass += constraint.massInverseBody1 + ((I1 * contact.r1CrossT1).cross(contact.r1)).dot(contact.frictionVector1); + friction2Mass += constraint.massInverseBody1 + ((I1 * contact.r1CrossT2).cross(contact.r1)).dot(contact.frictionVector2); + } + if (constraint.isBody2Moving) { + friction1Mass += constraint.massInverseBody2 + ((I2 * contact.r2CrossT1).cross(contact.r2)).dot(contact.frictionVector1); + friction2Mass += constraint.massInverseBody2 + ((I2 * contact.r2CrossT2).cross(contact.r2)).dot(contact.frictionVector2); + } + friction1Mass > 0.0 ? contact.inverseFriction1Mass = 1.0 / friction1Mass : 0.0; + friction2Mass > 0.0 ? contact.inverseFriction2Mass = 1.0 / friction2Mass : 0.0; } - if (constraint.isBody2Moving) { - friction1Mass += constraint.massInverseBody2 + ((I2 * contact.r2CrossT1).cross(contact.r2)).dot(contact.frictionVector1); - friction2Mass += constraint.massInverseBody2 + ((I2 * contact.r2CrossT2).cross(contact.r2)).dot(contact.frictionVector2); - } - friction1Mass > 0.0 ? contact.inverseFriction1Mass = 1.0 / friction1Mass : 0.0; - friction2Mass > 0.0 ? contact.inverseFriction2Mass = 1.0 / friction2Mass : 0.0; // Compute the restitution velocity bias "b". We compute this here instead // of inside the solve() method because we need to use the velocity difference @@ -213,12 +265,50 @@ void ConstraintSolver::initializeContactConstraints() { } // Get the cached lambda values of the constraint - contact.penetrationImpulse = realContact->getCachedLambda(0); - contact.friction1Impulse = realContact->getCachedLambda(1); - contact.friction2Impulse = realContact->getCachedLambda(2); + if (mIsWarmStartingActive) { + contact.penetrationImpulse = realContact->getCachedLambda(0); + contact.friction1Impulse = realContact->getCachedLambda(1); + contact.friction2Impulse = realContact->getCachedLambda(2); + } // Initialize the split impulses to zero contact.penetrationSplitImpulse = 0.0; + + // If we solve the friction constraints at the center of the contact manifold + if (mIsSolveFrictionAtContactManifoldCenterActive) { + constraint.normal += contact.normal; + } + } + + // If we solve the friction constraints at the center of the contact manifold + if (mIsSolveFrictionAtContactManifoldCenterActive) { + + constraint.normal.normalize(); + + Vector3 deltaVFrictionPoint = v2 + w2.cross(constraint.r2Friction) - + v1 - w1.cross(constraint.r1Friction); + + // Compute the friction vectors + computeFrictionVectors(deltaVFrictionPoint, constraint); + + // Compute the inverse mass matrix K for the friction constraints at the center of + // the contact manifold + constraint.r1CrossT1 = constraint.r1Friction.cross(constraint.frictionVector1); + constraint.r1CrossT2 = constraint.r1Friction.cross(constraint.frictionVector2); + constraint.r2CrossT1 = constraint.r2Friction.cross(constraint.frictionVector1); + constraint.r2CrossT2 = constraint.r2Friction.cross(constraint.frictionVector2); + decimal friction1Mass = 0.0; + decimal friction2Mass = 0.0; + if (constraint.isBody1Moving) { + friction1Mass += constraint.massInverseBody1 + ((I1 * constraint.r1CrossT1).cross(constraint.r1Friction)).dot(constraint.frictionVector1); + friction2Mass += constraint.massInverseBody1 + ((I1 * constraint.r1CrossT2).cross(constraint.r1Friction)).dot(constraint.frictionVector2); + } + if (constraint.isBody2Moving) { + friction1Mass += constraint.massInverseBody2 + ((I2 * constraint.r2CrossT1).cross(constraint.r2Friction)).dot(constraint.frictionVector1); + friction2Mass += constraint.massInverseBody2 + ((I2 * constraint.r2CrossT2).cross(constraint.r2Friction)).dot(constraint.frictionVector2); + } + friction1Mass > 0.0 ? constraint.inverseFriction1Mass = 1.0 / friction1Mass : 0.0; + friction2Mass > 0.0 ? constraint.inverseFriction2Mass = 1.0 / friction2Mass : 0.0; } } } @@ -234,6 +324,8 @@ void ConstraintSolver::warmStart() { ContactConstraint& constraint = mContactConstraints[c]; + bool atLeastOneRestingContactPoint = false; + for (uint i=0; i slop) biasPenetrationDepth = -(beta/mTimeStep) * - max(0.0f, float(contact.penetrationDepth - slop)); + if (contact.penetrationDepth > SLOP) biasPenetrationDepth = -(beta/mTimeStep) * + max(0.0f, float(contact.penetrationDepth - SLOP)); decimal b = biasPenetrationDepth + contact.restitutionBias; // Compute the Lagrange multiplier @@ -358,6 +517,8 @@ void ConstraintSolver::solveContactConstraints() { // Apply the impulse to the bodies of the constraint applyImpulse(impulsePenetration, constraint); + sumPenetrationImpulse += contact.penetrationImpulse; + // If the split impulse position correction is active if (mIsSplitImpulseActive) { @@ -384,53 +545,136 @@ void ConstraintSolver::solveContactConstraints() { applySplitImpulse(splitImpulsePenetration, constraint); } - // --------- Friction 1 --------- // + // If we do not solve the friction constraints at the center of the contact manifold + if (!mIsSolveFrictionAtContactManifoldCenterActive) { + + // --------- Friction 1 --------- // + + // Compute J*v + deltaV = v2 + w2.cross(contact.r2) - v1 - w1.cross(contact.r1); + Jv = deltaV.dot(contact.frictionVector1); + + deltaLambda = -Jv; + deltaLambda *= contact.inverseFriction1Mass; + decimal frictionLimit = 0.3 * contact.penetrationImpulse; // TODO : Use constant here + lambdaTemp = contact.friction1Impulse; + contact.friction1Impulse = std::max(-frictionLimit, std::min(contact.friction1Impulse + deltaLambda, frictionLimit)); + deltaLambda = contact.friction1Impulse - lambdaTemp; + + // Compute the impulse P=J^T * lambda + linearImpulseBody1 = -contact.frictionVector1 * deltaLambda; + angularImpulseBody1 = -contact.r1CrossT1 * deltaLambda; + linearImpulseBody2 = contact.frictionVector1 * deltaLambda; + angularImpulseBody2 = contact.r2CrossT1 * deltaLambda; + const Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1, + linearImpulseBody2, angularImpulseBody2); + + // Apply the impulses to the bodies of the constraint + applyImpulse(impulseFriction1, constraint); + + // --------- Friction 2 --------- // + + // Compute J*v + deltaV = v2 + w2.cross(contact.r2) - v1 - w1.cross(contact.r1); + Jv = deltaV.dot(contact.frictionVector2); + + deltaLambda = -Jv; + deltaLambda *= contact.inverseFriction2Mass; + frictionLimit = 0.3 * contact.penetrationImpulse; // TODO : Use constant here + lambdaTemp = contact.friction2Impulse; + contact.friction2Impulse = std::max(-frictionLimit, std::min(contact.friction2Impulse + deltaLambda, frictionLimit)); + deltaLambda = contact.friction2Impulse - lambdaTemp; + + // Compute the impulse P=J^T * lambda + linearImpulseBody1 = -contact.frictionVector2 * deltaLambda; + angularImpulseBody1 = -contact.r1CrossT2 * deltaLambda; + linearImpulseBody2 = contact.frictionVector2 * deltaLambda; + angularImpulseBody2 = contact.r2CrossT2 * deltaLambda; + const Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1, + linearImpulseBody2, angularImpulseBody2); + + // Apply the impulses to the bodies of the constraint + applyImpulse(impulseFriction2, constraint); + } + } + + // If we solve the friction constraints at the center of the contact manifold + if (mIsSolveFrictionAtContactManifoldCenterActive) { + + // ------ First friction constraint at the center of the contact manifol ------ // // Compute J*v - deltaV = v2 + w2.cross(contact.r2) - v1 - w1.cross(contact.r1); - Jv = deltaV.dot(contact.frictionVector1); + Vector3 deltaV = v2 + w2.cross(constraint.r2Friction) - v1 - w1.cross(constraint.r1Friction); + decimal Jv = deltaV.dot(constraint.frictionVector1); - deltaLambda = -Jv; - deltaLambda *= contact.inverseFriction1Mass; - decimal frictionLimit = 0.3 * contact.penetrationImpulse; // TODO : Use constant here - lambdaTemp = contact.friction1Impulse; - contact.friction1Impulse = std::max(-frictionLimit, std::min(contact.friction1Impulse + deltaLambda, frictionLimit)); - deltaLambda = contact.friction1Impulse - lambdaTemp; + decimal deltaLambda = -Jv * constraint.inverseFriction1Mass; + decimal frictionLimit = 0.3 * sumPenetrationImpulse; // TODO : Use constant here + lambdaTemp = constraint.friction1Impulse; + constraint.friction1Impulse = std::max(-frictionLimit, std::min(constraint.friction1Impulse + deltaLambda, frictionLimit)); + deltaLambda = constraint.friction1Impulse - lambdaTemp; // Compute the impulse P=J^T * lambda - linearImpulseBody1 = -contact.frictionVector1 * deltaLambda; - angularImpulseBody1 = -contact.r1CrossT1 * deltaLambda; - linearImpulseBody2 = contact.frictionVector1 * deltaLambda; - angularImpulseBody2 = contact.r2CrossT1 * deltaLambda; + Vector3 linearImpulseBody1 = -constraint.frictionVector1 * deltaLambda; + Vector3 angularImpulseBody1 = -constraint.r1CrossT1 * deltaLambda; + Vector3 linearImpulseBody2 = constraint.frictionVector1 * deltaLambda; + Vector3 angularImpulseBody2 = constraint.r2CrossT1 * deltaLambda; const Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1, linearImpulseBody2, angularImpulseBody2); // Apply the impulses to the bodies of the constraint applyImpulse(impulseFriction1, constraint); - // --------- Friction 2 --------- // + // ------ Second friction constraint at the center of the contact manifol ----- // // Compute J*v - deltaV = v2 + w2.cross(contact.r2) - v1 - w1.cross(contact.r1); - Jv = deltaV.dot(contact.frictionVector2); + deltaV = v2 + w2.cross(constraint.r2Friction) - v1 - w1.cross(constraint.r1Friction); + Jv = deltaV.dot(constraint.frictionVector2); - deltaLambda = -Jv; - deltaLambda *= contact.inverseFriction2Mass; - frictionLimit = 0.3 * contact.penetrationImpulse; // TODO : Use constant here - lambdaTemp = contact.friction2Impulse; - contact.friction2Impulse = std::max(-frictionLimit, std::min(contact.friction2Impulse + deltaLambda, frictionLimit)); - deltaLambda = contact.friction2Impulse - lambdaTemp; + deltaLambda = -Jv * constraint.inverseFriction2Mass; + frictionLimit = 0.3 * sumPenetrationImpulse; // TODO : Use constant here + lambdaTemp = constraint.friction2Impulse; + constraint.friction2Impulse = std::max(-frictionLimit, std::min(constraint.friction2Impulse + deltaLambda, frictionLimit)); + deltaLambda = constraint.friction2Impulse - lambdaTemp; // Compute the impulse P=J^T * lambda - linearImpulseBody1 = -contact.frictionVector2 * deltaLambda; - angularImpulseBody1 = -contact.r1CrossT2 * deltaLambda; - linearImpulseBody2 = contact.frictionVector2 * deltaLambda; - angularImpulseBody2 = contact.r2CrossT2 * deltaLambda; + linearImpulseBody1 = -constraint.frictionVector2 * deltaLambda; + angularImpulseBody1 = -constraint.r1CrossT2 * deltaLambda; + linearImpulseBody2 = constraint.frictionVector2 * deltaLambda; + angularImpulseBody2 = constraint.r2CrossT2 * deltaLambda; const Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1, linearImpulseBody2, angularImpulseBody2); // Apply the impulses to the bodies of the constraint applyImpulse(impulseFriction2, constraint); + + // ------ Twist friction constraint at the center of the contact manifol ------ // + + // TODO : Put this in the initialization method + decimal K = constraint.normal.dot(constraint.inverseInertiaTensorBody1 * constraint.normal) + + constraint.normal.dot(constraint.inverseInertiaTensorBody2 * constraint.normal); + + + // Compute J*v + deltaV = w2 - w1; + Jv = deltaV.dot(constraint.normal); + + // TODO : Compute the inverse mass matrix here for twist friction + deltaLambda = -Jv * (1.0 / K); + frictionLimit = 0.3 * sumPenetrationImpulse; // TODO : Use constant here + lambdaTemp = constraint.frictionTwistImpulse; + constraint.frictionTwistImpulse = std::max(-frictionLimit, std::min(constraint.frictionTwistImpulse + deltaLambda, frictionLimit)); + deltaLambda = constraint.frictionTwistImpulse - lambdaTemp; + + // Compute the impulse P=J^T * lambda + linearImpulseBody1 = Vector3(0.0, 0.0, 0.0); + angularImpulseBody1 = -constraint.normal * deltaLambda; + linearImpulseBody2 = Vector3(0.0, 0.0, 0.0);; + angularImpulseBody2 = constraint.normal * deltaLambda; + const Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1, + linearImpulseBody2, angularImpulseBody2); + + // Apply the impulses to the bodies of the constraint + applyImpulse(impulseTwistFriction, constraint); } } } @@ -481,6 +725,12 @@ void ConstraintSolver::storeImpulses() { contact.contact->setFrictionVector1(contact.frictionVector1); contact.contact->setFrictionVector2(contact.frictionVector2); } + + constraint.contactManifold->friction1Impulse = constraint.friction1Impulse; + constraint.contactManifold->friction2Impulse = constraint.friction2Impulse; + constraint.contactManifold->frictionTwistImpulse = constraint.frictionTwistImpulse; + constraint.contactManifold->frictionVector1 = constraint.frictionVector1; + constraint.contactManifold->frictionVector2 = constraint.frictionVector2; } } @@ -521,7 +771,7 @@ void ConstraintSolver::applySplitImpulse(const Impulse& impulse, const ContactCo } // Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane -// The two vectors have to be such that : t1 x t2 = contactNormal +// for a contact point constraint. The two vectors have to be such that : t1 x t2 = contactNormal. void ConstraintSolver::computeFrictionVectors(const Vector3& deltaVelocity, ContactPointConstraint& contact) const { @@ -553,3 +803,33 @@ void ConstraintSolver::computeFrictionVectors(const Vector3& deltaVelocity, // friction vector and the contact normal contact.frictionVector2 = contact.normal.cross(contact.frictionVector1).getUnit(); } + +// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane +// for a contact constraint. The two vectors have to be such that : t1 x t2 = contactNormal. +void ConstraintSolver::computeFrictionVectors(const Vector3& deltaVelocity, + ContactConstraint& contact) const { + + assert(contact.normal.length() > 0.0); + + // Compute the velocity difference vector in the tangential plane + Vector3 normalVelocity = deltaVelocity.dot(contact.normal) * contact.normal; + Vector3 tangentVelocity = deltaVelocity - normalVelocity; + + // If the velocty difference in the tangential plane is not zero + decimal lengthTangenVelocity = tangentVelocity.length(); + if (lengthTangenVelocity > 0.0) { + + // Compute the first friction vector in the direction of the tangent + // velocity difference + contact.frictionVector1 = tangentVelocity / lengthTangenVelocity; + } + else { + + // Get any orthogonal vector to the normal as the first friction vector + contact.frictionVector1 = contact.normal.getOneUnitOrthogonalVector(); + } + + // The second friction vector is computed by the cross product of the firs + // friction vector and the contact normal + contact.frictionVector2 = contact.normal.cross(contact.frictionVector1).getUnit(); +} diff --git a/src/engine/ConstraintSolver.h b/src/engine/ConstraintSolver.h index 9675a63e..0125dd5e 100644 --- a/src/engine/ConstraintSolver.h +++ b/src/engine/ConstraintSolver.h @@ -28,6 +28,7 @@ // Libraries #include "../constraint/Contact.h" +#include "ContactManifold.h" #include "../configuration.h" #include "../constraint/Constraint.h" #include @@ -92,42 +93,109 @@ struct ContactConstraint { // TODO : Use a constant for the number of contact points - uint indexBody1; // Index of body 1 in the constraint solver - uint indexBody2; // Index of body 2 in the constraint solver - decimal massInverseBody1; // Inverse of the mass of body 1 - decimal massInverseBody2; // Inverse of the mass of body 2 - Matrix3x3 inverseInertiaTensorBody1; // Inverse inertia tensor of body 1 - Matrix3x3 inverseInertiaTensorBody2; // Inverse inertia tensor of body 2 - bool isBody1Moving; // True if the body 1 is allowed to move - bool isBody2Moving; // True if the body 2 is allowed to move - ContactPointConstraint contacts[4]; // Contact point constraints - uint nbContacts; // Number of contact points - decimal restitutionFactor; // Mix of the restitution factor for two bodies + uint indexBody1; // Index of body 1 in the constraint solver + uint indexBody2; // Index of body 2 in the constraint solver + decimal massInverseBody1; // Inverse of the mass of body 1 + decimal massInverseBody2; // Inverse of the mass of body 2 + Matrix3x3 inverseInertiaTensorBody1; // Inverse inertia tensor of body 1 + Matrix3x3 inverseInertiaTensorBody2; // Inverse inertia tensor of body 2 + bool isBody1Moving; // True if the body 1 is allowed to move + bool isBody2Moving; // True if the body 2 is allowed to move + ContactPointConstraint contacts[4]; // Contact point constraints + uint nbContacts; // Number of contact points + decimal restitutionFactor; // Mix of the restitution factor for two bodies + ContactManifold* contactManifold; // Contact manifold + + // --- Variables used when friction constraints are apply at the center of the manifold --- // + + Vector3 normal; // Average normal vector of the contact manifold + Vector3 frictionPointBody1; // Point on body 1 where to apply the friction constraints + Vector3 frictionPointBody2; // Point on body 2 where to apply the friction constraints + Vector3 r1Friction; // R1 vector for the friction constraints + Vector3 r2Friction; // R2 vector for the friction constraints + Vector3 r1CrossT1; // Cross product of r1 with 1st friction vector + Vector3 r1CrossT2; // Cross product of r1 with 2nd friction vector + Vector3 r2CrossT1; // Cross product of r2 with 1st friction vector + Vector3 r2CrossT2; // Cross product of r2 with 2nd friction vector + decimal inverseFriction1Mass; // Matrix K for the first friction constraint + decimal inverseFriction2Mass; // Matrix K for the second friction constraint + Vector3 frictionVector1; // First friction direction at contact manifold center + Vector3 frictionVector2; // Second friction direction at contact manifold center + Vector3 oldFrictionVector1; // Old 1st friction direction at contact manifold center + Vector3 oldFrictionVector2; // Old 2nd friction direction at contact manifold center + decimal friction1Impulse; // First friction direction impulse at manifold center + decimal friction2Impulse; // Second friction direction impulse at manifold center + decimal frictionTwistImpulse; // Twist friction impulse at contact manifold center }; -// TODO : Rewrite this coment to use the Sequential Impulse technique /* ------------------------------------------------------------------- Class ConstrainSolver : - This class represents the constraint solver. The constraint solver - is based on the theory from the paper "Iterative Dynamics with - Temporal Coherence" from Erin Catto. We keep the same notations as - in the paper. The idea is to construct a LCP problem and then solve - it using a Projected Gauss Seidel (PGS) solver. - - The idea is to solve the following system for lambda : - JM^-1J^T * lamdba - 1/dt * b_error + 1/dt * JV^1 + JM^-1F_ext >= 0 - - By default, error correction using first world order projections (described - by Erleben in section 4.16 in his PhD thesis "Stable, Robust, and - Versatile Multibody Dynamics Animation") is used for very large penetration - depths. Error correction with projection requires to solve another LCP problem - that is simpler than the above one and by considering only the contact - constraints. The LCP problem for error correction is the following one : - J_contact M^-1 J_contact^T * lambda + 1/dt * -d >= 0 - - where "d" is a vector with penetration depths. If the penetration depth of - a given contact is not very large, we use the Baumgarte error correction (see - the paper from Erin Catto). + This class represents the constraint solver that is used is solve constraints and + rigid bodies contacts. The constraint solver is based on the "Sequential Impulse" technique + described by Erin Catto in his GDC slides (http://code.google.com/p/box2d/downloads/list). + + A constraint between two bodies is represented by a function C(x) which is equal to zero + when the constraint is satisfied. The condition C(x)=0 describes a valid position and the + condition dC(x)/dt=0 describes a valid velocity. We have dC(x)/dt = Jv + b = 0 where J is + the Jacobian matrix of the constraint, v is a vector that contains the velocity of both + bodies and b is the constraint bias. We are looking for a force F_c that will act on the + bodies to keep the constraint satisfied. Note that from the virtual work principle, we have + F_c = J^t * lambda where J^t is the transpose of the Jacobian matrix and lambda is a + Lagrange multiplier. Therefore, finding the force F_c is equivalent to finding the Lagrange + multiplier lambda. + + An impulse P = F * dt where F is a force and dt is the timestep. We can apply impulses a + body to change its velocity. The idea of the Sequential Impulse technique is to apply + impulses to bodies of each constraints in order to keep the constraint satisfied. + + --- Step 1 --- + + First, we integrate the applied force F_a acting of each rigid body (like gravity, ...) and + we obtain some new velocities v2' that tends to violate the constraints. + + v2' = v1 + dt * M^-1 * F_a + + where M is a matrix that contains mass and inertia tensor information. + + --- Step 2 --- + + During the second step, we iterate over all the constraints for a certain number of + iterations and for each constraint we compute the impulse to apply to the bodies needed + so that the new velocity of the bodies satisfy Jv + b = 0. From the Newton law, we know that + M * deltaV = P_c where M is the mass of the body, deltaV is the difference of velocity and + P_c is the constraint impulse to apply to the body. Therefore, we have + v2 = v2' + M^-1 * P_c. For each constraint, we can compute the Lagrange multiplier lambda + using : lambda = -m_c (Jv2' + b) where m_c = 1 / (J * M^-1 * J^t). Now that we have the + Lagrange multiplier lambda, we can compute the impulse P_c = J^t * lambda * dt to apply to + the bodies to satisfy the constraint. + + --- Step 3 --- + + In the third step, we integrate the new position x2 of the bodies using the new velocities + v2 computed in the second step with : x2 = x1 + dt * v2. + + Note that in the following code (as it is also explained in the slides from Erin Catto), + the value lambda is not only the lagrange multiplier but is the multiplication of the + Lagrange multiplier with the timestep dt. Therefore, in the following code, when we use + lambda, we mean (lambda * dt). + + We are using the accumulated impulse technique that is also described in the slides from + Erin Catto. + + We are also using warm starting. The idea is to warm start the solver at the beginning of + each step by applying the last impulstes for the constraints that we already existing at the + previous step. This allows the iterative solver to converge faster towards the solution. + + For contact constraints, we are also using split impulses so that the position correction + that uses Baumgarte stabilization does not change the momentum of the bodies. + + There are two ways to apply the friction constraints. Either the friction constraints are + applied at each contact point or they are applied only at the center of the contact manifold + between two bodies. If we solve the friction constraints at each contact point, we need + two constraints (two tangential friction directions) and if we solve the friction constraints + at the center of the contact manifold, we need two constraints for tangential friction but + also another twist friction constraint to prevent spin of the body around the contact + manifold center. ------------------------------------------------------------------- */ @@ -135,6 +203,17 @@ class ConstraintSolver { private: + // -------------------- Constants --------------------- // + + // Beta value for the penetration depth position correction without split impulses + static const decimal BETA; + + // Beta value for the penetration depth position correction with split impulses + static const decimal BETA_SPLIT_IMPULSE; + + // Slop distance (allowed penetration distance between bodies) + static const decimal SLOP; + // -------------------- Attributes -------------------- // DynamicsWorld* world; // Reference to the world @@ -174,6 +253,10 @@ class ConstraintSolver { // True if the split impulse position correction is active bool mIsSplitImpulseActive; + // True if we solve 3 friction constraints at the contact manifold center only + // instead of 2 friction constraints at each contact point + bool mIsSolveFrictionAtContactManifoldCenterActive; + // -------------------- Methods -------------------- // // Initialize the constraint solver @@ -204,11 +287,17 @@ class ConstraintSolver { // Compute the collision restitution factor from the restitution factor of each body decimal computeMixRestitutionFactor(const RigidBody *body1, const RigidBody *body2) const; - // Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane - // The two vectors have to be such that : t1 x t2 = contactNormal + // Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction + // plane for a contact point constraint. The two vectors have to be + // such that : t1 x t2 = contactNormal. void computeFrictionVectors(const Vector3& deltaVelocity, ContactPointConstraint& contact) const; + // Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction + // plane for a contact constraint. The two vectors have to be + // such that : t1 x t2 = contactNormal. + void computeFrictionVectors(const Vector3& deltaVelocity, ContactConstraint& contact) const; + public: // -------------------- Methods -------------------- // @@ -240,8 +329,15 @@ class ConstraintSolver { // Clean up the constraint solver void cleanup(); - // Set the number of iterations of the LCP solver - void setNbLCPIterations(uint mNbIterations); + // Set the number of iterations of the constraint solver + void setNbIterationsSolver(uint nbIterations); + + // Activate or Deactivate the split impulses for contacts + void setIsSplitImpulseActive(bool isActive); + + // Activate or deactivate the solving of friction constraints at the center of + // the contact manifold instead of solving them at each contact point + void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive); }; // Return true if the body is in at least one constraint @@ -297,11 +393,22 @@ inline void ConstraintSolver::cleanup() { } } -// Set the number of iterations of the LCP solver -inline void ConstraintSolver::setNbLCPIterations(uint nbIterations) { +// Set the number of iterations of the constraint solver +inline void ConstraintSolver::setNbIterationsSolver(uint nbIterations) { mNbIterations = nbIterations; } +// Activate or Deactivate the split impulses for contacts +inline void ConstraintSolver::setIsSplitImpulseActive(bool isActive) { + mIsSplitImpulseActive = isActive; +} + +// Activate or deactivate the solving of friction constraints at the center of +// the contact manifold instead of solving them at each contact point +inline void ConstraintSolver::setIsSolveFrictionAtContactManifoldCenterActive(bool isActive) { + mIsSolveFrictionAtContactManifoldCenterActive = isActive; +} + // Compute the collision restitution factor from the restitution factor of each body inline decimal ConstraintSolver::computeMixRestitutionFactor(const RigidBody* body1, const RigidBody* body2) const { diff --git a/src/engine/ContactManifold.h b/src/engine/ContactManifold.h index ce5a7c67..7e8a7816 100644 --- a/src/engine/ContactManifold.h +++ b/src/engine/ContactManifold.h @@ -44,10 +44,26 @@ struct ContactManifold { // Number of contacts in the manifold uint nbContacts; + // First friction vector of the contact manifold + Vector3 frictionVector1; + + // Second friction vector of the contact manifold + Vector3 frictionVector2; + + // First friction constraint accumulated impulse + decimal friction1Impulse; + + // Second friction constraint accumulated impulse + decimal friction2Impulse; + + // Twist friction constraint accumulated impulse + decimal frictionTwistImpulse; + // -------------------- Methods -------------------- // // Constructor - ContactManifold() : nbContacts(0) { } + ContactManifold() : nbContacts(0), friction1Impulse(0.0), friction2Impulse(0.0), + frictionTwistImpulse(0.0) {} }; } diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index 2d970784..ae8b685a 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -139,8 +139,15 @@ public : // Update the physics simulation void update(); - // Set the number of iterations of the LCP solver - void setNbLCPIterations(uint nbIterations); + // Set the number of iterations of the constraint solver + void setNbIterationsSolver(uint nbIterations); + + // Activate or Deactivate the split impulses for contacts + void setIsSplitImpulseActive(bool isActive); + + // Activate or deactivate the solving of friction constraints at the center of + // the contact manifold instead of solving them at each contact point + void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive); // Set the isErrorCorrectionActive value void setIsErrorCorrectionActive(bool isErrorCorrectionActive); @@ -205,10 +212,21 @@ inline void DynamicsWorld::stop() { mTimer.stop(); } -// Set the number of iterations of the LCP solver -inline void DynamicsWorld::setNbLCPIterations(uint nbIterations) { - mConstraintSolver.setNbLCPIterations(nbIterations); -} +// Set the number of iterations of the constraint solver +inline void DynamicsWorld::setNbIterationsSolver(uint nbIterations) { + mConstraintSolver.setNbIterationsSolver(nbIterations); +} + +// Activate or Deactivate the split impulses for contacts +inline void DynamicsWorld::setIsSplitImpulseActive(bool isActive) { + mConstraintSolver.setIsSplitImpulseActive(isActive); +} + +// Activate or deactivate the solving of friction constraints at the center of +// the contact manifold instead of solving them at each contact point +inline void DynamicsWorld::setIsSolveFrictionAtContactManifoldCenterActive(bool isActive) { + mConstraintSolver.setIsSolveFrictionAtContactManifoldCenterActive(isActive); +} // Reset the boolean movement variable of each body inline void DynamicsWorld::resetBodiesMovementVariable() { diff --git a/src/mathematics/Vector3.cpp b/src/mathematics/Vector3.cpp index 188c9933..c44e3820 100644 --- a/src/mathematics/Vector3.cpp +++ b/src/mathematics/Vector3.cpp @@ -26,7 +26,6 @@ // Libraries #include "Vector3.h" #include -#include #include // Namespaces diff --git a/src/mathematics/Vector3.h b/src/mathematics/Vector3.h index f8599c4e..bc549a08 100644 --- a/src/mathematics/Vector3.h +++ b/src/mathematics/Vector3.h @@ -28,6 +28,7 @@ // Libraries #include +#include #include "mathematics_functions.h" #include "../decimal.h" @@ -113,6 +114,9 @@ class Vector3 { // Cross product of two vectors Vector3 cross(const Vector3& vector) const; + // Normalize the vector + void normalize(); + // Return the corresponding absolute value vector Vector3 getAbsoluteVector() const; @@ -140,6 +144,9 @@ class Vector3 { // Overloaded operator for multiplication with a number with assignment Vector3& operator*=(decimal number); + // Overloaded operator for division by a number with assignment + Vector3& operator/=(decimal number); + // Overloaded operator for value access decimal& operator[] (int index); @@ -221,6 +228,15 @@ inline Vector3 Vector3::cross(const Vector3& vector) const { mValues[0] * vector.mValues[1] - mValues[1] * vector.mValues[0]); } +// Normalize the vector +inline void Vector3::normalize() { + decimal l = length(); + assert(l != 0.0); + mValues[0] /= l; + mValues[1] /= l; + mValues[2] /= l; +} + // Return the corresponding absolute value vector inline Vector3 Vector3::getAbsoluteVector() const { return Vector3(std::abs(mValues[0]), std::abs(mValues[1]), std::abs(mValues[2])); @@ -287,6 +303,14 @@ inline Vector3& Vector3::operator*=(decimal number) { return *this; } +// Overloaded operator for division by a number with assignment +inline Vector3& Vector3::operator/=(decimal number) { + mValues[0] /= number; + mValues[1] /= number; + mValues[2] /= number; + return *this; +} + // Overloaded operator for value access inline decimal& Vector3::operator[] (int index) { return mValues[index]; From e84f6468c82061a8b5a6c35c3e28bb9bf11205bd Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Tue, 19 Feb 2013 23:16:20 +0100 Subject: [PATCH 23/26] Clean the code --- src/body/RigidBody.cpp | 4 +- src/body/RigidBody.h | 18 + src/collision/narrowphase/GJK/GJKAlgorithm.h | 2 +- src/collision/shapes/BoxShape.cpp | 2 +- src/collision/shapes/ConeShape.cpp | 2 +- src/collision/shapes/ConeShape.h | 10 +- src/collision/shapes/CylinderShape.h | 10 +- src/collision/shapes/SphereShape.h | 6 +- src/configuration.h | 34 +- src/constraint/Constraint.h | 15 - src/constraint/Contact.cpp | 249 +----- src/constraint/Contact.h | 57 +- src/engine/ContactSolver.cpp | 837 +++++++++++++++++++ src/engine/ContactSolver.h | 459 ++++++++++ src/engine/DynamicsWorld.cpp | 26 +- src/engine/DynamicsWorld.h | 12 +- src/engine/PersistentContactCache.h | 2 +- src/mathematics/Transform.h | 2 +- 18 files changed, 1372 insertions(+), 375 deletions(-) create mode 100644 src/engine/ContactSolver.cpp create mode 100644 src/engine/ContactSolver.h diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index 3a084e18..a4500d01 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -35,9 +35,9 @@ using namespace reactphysics3d; CollisionShape *collisionShape, bodyindex id) : CollisionBody(transform, collisionShape, id), mInertiaTensorLocal(inertiaTensorLocal), mMass(mass), mInertiaTensorLocalInverse(inertiaTensorLocal.getInverse()), - mMassInverse(1.0/mass) { + mMassInverse(decimal(1.0) / mass), mFrictionCoefficient(DEFAULT_FRICTION_COEFFICIENT) { - mRestitution = 1.0; + mRestitution = decimal(1.0); // Set the body pointer of the AABB and the collision shape mAabb->setBodyPointer(this); diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h index 5e6930d6..7c34addd 100644 --- a/src/body/RigidBody.h +++ b/src/body/RigidBody.h @@ -77,6 +77,8 @@ class RigidBody : public CollisionBody { // Coefficient of restitution (between 0 and 1) where 1 is for a very bouncy body decimal mRestitution; + // Friction coefficient + decimal mFrictionCoefficient; // -------------------- Methods -------------------- // @@ -153,6 +155,12 @@ class RigidBody : public CollisionBody { // Set the restitution coefficient void setRestitution(decimal restitution) throw(std::invalid_argument); + + // Get the friction coefficient + decimal getFrictionCoefficient() const; + + // Set the friction coefficient + void setFrictionCoefficient(decimal frictionCoefficient); }; // Method that return the mass of the body @@ -277,6 +285,16 @@ inline void RigidBody::setRestitution(decimal restitution) throw(std::invalid_ar } } +// Get the friction coefficient +inline decimal RigidBody::getFrictionCoefficient() const { + return mFrictionCoefficient; +} + +// Set the friction coefficient +inline void RigidBody::setFrictionCoefficient(decimal frictionCoefficient) { + mFrictionCoefficient = frictionCoefficient; +} + } // End of the ReactPhyscis3D namespace #endif diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.h b/src/collision/narrowphase/GJK/GJKAlgorithm.h index b45d5696..b58054d3 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.h +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.h @@ -37,7 +37,7 @@ namespace reactphysics3d { // Constants -const decimal REL_ERROR = 1.0e-3; +const decimal REL_ERROR = decimal(1.0e-3); const decimal REL_ERROR_SQUARE = REL_ERROR * REL_ERROR; /* ------------------------------------------------------------------- diff --git a/src/collision/shapes/BoxShape.cpp b/src/collision/shapes/BoxShape.cpp index 2d39f127..0b0c3708 100644 --- a/src/collision/shapes/BoxShape.cpp +++ b/src/collision/shapes/BoxShape.cpp @@ -57,7 +57,7 @@ BoxShape::~BoxShape() { // Return the local inertia tensor of the collision shape void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const { - decimal factor = (1.0 / 3.0) * mass; + decimal factor = (decimal(1.0) / decimal(3.0)) * mass; decimal xSquare = mExtent.getX() * mExtent.getX(); decimal ySquare = mExtent.getY() * mExtent.getY(); decimal zSquare = mExtent.getZ() * mExtent.getZ(); diff --git a/src/collision/shapes/ConeShape.cpp b/src/collision/shapes/ConeShape.cpp index 18436fff..49e622f0 100644 --- a/src/collision/shapes/ConeShape.cpp +++ b/src/collision/shapes/ConeShape.cpp @@ -45,7 +45,7 @@ using namespace reactphysics3d; // Constructor ConeShape::ConeShape(decimal radius, decimal height) - : CollisionShape(CONE), mRadius(radius), mHalfHeight(height/2.0) { + : CollisionShape(CONE), mRadius(radius), mHalfHeight(height / decimal(2.0)) { assert(radius > 0.0); assert(mHalfHeight > 0.0); diff --git a/src/collision/shapes/ConeShape.h b/src/collision/shapes/ConeShape.h index dfcfc25f..4aaeddb7 100644 --- a/src/collision/shapes/ConeShape.h +++ b/src/collision/shapes/ConeShape.h @@ -118,12 +118,12 @@ inline void ConeShape::setRadius(decimal radius) { // Return the height inline decimal ConeShape::getHeight() const { - return 2.0 * mHalfHeight; + return decimal(2.0) * mHalfHeight; } // Set the height inline void ConeShape::setHeight(decimal height) { - mHalfHeight = height / 2.0; + mHalfHeight = height * decimal(0.5); // Update the sine of the semi-angle at the apex point mSinTheta = mRadius / (sqrt(mRadius * mRadius + height * height)); @@ -137,8 +137,10 @@ inline Vector3 ConeShape::getLocalExtents(decimal margin) const { // Return the local inertia tensor of the collision shape inline void ConeShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const { decimal rSquare = mRadius * mRadius; - decimal diagXZ = 0.15 * mass * (rSquare + mHalfHeight); - tensor.setAllValues(diagXZ, 0.0, 0.0, 0.0, 0.3 * mass * rSquare, 0.0, 0.0, 0.0, diagXZ); + decimal diagXZ = decimal(0.15) * mass * (rSquare + mHalfHeight); + tensor.setAllValues(diagXZ, 0.0, 0.0, + 0.0, decimal(0.3) * mass * rSquare, + 0.0, 0.0, 0.0, diagXZ); } }; // End of the ReactPhysics3D namespace diff --git a/src/collision/shapes/CylinderShape.h b/src/collision/shapes/CylinderShape.h index 0c20de00..97381e05 100644 --- a/src/collision/shapes/CylinderShape.h +++ b/src/collision/shapes/CylinderShape.h @@ -116,7 +116,7 @@ inline decimal CylinderShape::getHeight() const { // Set the height inline void CylinderShape::setHeight(decimal height) { - this->mHalfHeight = height / 2.0; + mHalfHeight = height * decimal(0.5); } // Return the local extents in x,y and z direction @@ -126,9 +126,11 @@ inline Vector3 CylinderShape::getLocalExtents(decimal margin) const { // Return the local inertia tensor of the cylinder inline void CylinderShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const { - decimal height = 2.0 * mHalfHeight; - decimal diag = (1.0 / 12.0) * mass * (3 * mRadius * mRadius + height * height); - tensor.setAllValues(diag, 0.0, 0.0, 0.0, 0.5 * mass * mRadius * mRadius, 0.0, 0.0, 0.0, diag); + decimal height = decimal(2.0) * mHalfHeight; + decimal diag = (decimal(1.0) / decimal(12.0)) * mass * (3 * mRadius * mRadius + height * height); + tensor.setAllValues(diag, 0.0, 0.0, 0.0, + decimal(0.5) * mass * mRadius * mRadius, 0.0, + 0.0, 0.0, diag); } }; // End of the ReactPhysics3D namespace diff --git a/src/collision/shapes/SphereShape.h b/src/collision/shapes/SphereShape.h index b161d910..afc42bba 100644 --- a/src/collision/shapes/SphereShape.h +++ b/src/collision/shapes/SphereShape.h @@ -121,8 +121,10 @@ inline Vector3 SphereShape::getLocalExtents(decimal margin) const { // Return the local inertia tensor of the sphere inline void SphereShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const { - decimal diag = 0.4 * mass * mRadius * mRadius; - tensor.setAllValues(diag, 0.0, 0.0, 0.0, diag, 0.0, 0.0, 0.0, diag); + decimal diag = decimal(0.4) * mass * mRadius * mRadius; + tensor.setAllValues(diag, 0.0, 0.0, + 0.0, diag, 0.0, + 0.0, 0.0, diag); } }; // End of the ReactPhysics3D namespace diff --git a/src/configuration.h b/src/configuration.h index fd26fb72..935ef6ff 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -53,43 +53,34 @@ typedef std::pair bodyindexpair; // ------------------- Constants ------------------- // -const reactphysics3d::decimal DECIMAL_SMALLEST = - std::numeric_limits::max(); +const decimal DECIMAL_SMALLEST = - std::numeric_limits::max(); // Maximum decimal value -const reactphysics3d::decimal DECIMAL_LARGEST = std::numeric_limits::max(); +const decimal DECIMAL_LARGEST = std::numeric_limits::max(); // Machine epsilon -const reactphysics3d::decimal MACHINE_EPSILON = std::numeric_limits::epsilon(); - -// Infinity -const reactphysics3d::decimal DECIMAL_INFINITY = std::numeric_limits::infinity(); +const decimal MACHINE_EPSILON = std::numeric_limits::epsilon(); // Pi constant -const reactphysics3d::decimal PI = 3.14159265; +const decimal PI = decimal(3.14159265); // Default internal constant timestep in seconds -const reactphysics3d::decimal DEFAULT_TIMESTEP = 1.0 / 60.0; +const decimal DEFAULT_TIMESTEP = decimal(1.0 / 60.0); + +// Default friction coefficient for a rigid body +const decimal DEFAULT_FRICTION_COEFFICIENT = decimal(0.3); // True if the deactivation (sleeping) of inactive bodies is enabled const bool DEACTIVATION_ENABLED = true; // // Object margin for collision detection in cm (For GJK-EPA Algorithm) -const reactphysics3d::decimal OBJECT_MARGIN = 0.04; - -// Friction coefficient -const reactphysics3d::decimal FRICTION_COEFFICIENT = 0.4; +const decimal OBJECT_MARGIN = decimal(0.04); // Distance threshold for two contact points for a valid persistent contact (in meters) -const reactphysics3d::decimal PERSISTENT_CONTACT_DIST_THRESHOLD = 0.03; - -// Maximum number of bodies -const int NB_MAX_BODIES = 100000; - -// Maximum number of constraints -const int NB_MAX_CONSTRAINTS = 100000; +const decimal PERSISTENT_CONTACT_DIST_THRESHOLD = decimal(0.03); // Number of iterations when solving a LCP problem -const uint DEFAULT_LCP_ITERATIONS = 15; +const uint DEFAULT_CONSTRAINTS_SOLVER_NB_ITERATIONS = 15; // Number of iterations when solving a LCP problem for error correction const uint DEFAULT_LCP_ITERATIONS_ERROR_CORRECTION = 5; @@ -97,9 +88,6 @@ const uint DEFAULT_LCP_ITERATIONS_ERROR_CORRECTION = 5; // True if the error correction projection (first order world) is active in the constraint solver const bool ERROR_CORRECTION_PROJECTION_ENABLED = true; -// Contacts with penetration depth (in meters) larger that this use error correction with projection -const reactphysics3d::decimal PENETRATION_DEPTH_THRESHOLD_ERROR_CORRECTION = 0.20; - } #endif diff --git a/src/constraint/Constraint.h b/src/constraint/Constraint.h index ceeafec6..9f5d8508 100644 --- a/src/constraint/Constraint.h +++ b/src/constraint/Constraint.h @@ -100,21 +100,6 @@ class Constraint { // Return the type of the constraint ConstraintType getType() const; - // Compute the jacobian matrix for all mathematical constraints - virtual void computeJacobian(int noConstraint, - decimal J_sp[NB_MAX_CONSTRAINTS][2*6]) const=0; - - // Compute the lowerbounds values for all the mathematical constraints - virtual void computeLowerBound(int noConstraint, - decimal lowerBounds[NB_MAX_CONSTRAINTS]) const=0; - - // Compute the upperbounds values for all the mathematical constraints - virtual void computeUpperBound(int noConstraint, - decimal upperBounds[NB_MAX_CONSTRAINTS]) const=0; - - // Compute the error values for all the mathematical constraints - virtual void computeErrorValue(int noConstraint, decimal errorValues[]) const=0; - // Return the number of mathematical constraints unsigned int getNbConstraints() const; diff --git a/src/constraint/Contact.cpp b/src/constraint/Contact.cpp index e89341b4..61403dfc 100644 --- a/src/constraint/Contact.cpp +++ b/src/constraint/Contact.cpp @@ -37,259 +37,12 @@ Contact::Contact(RigidBody* const body1, RigidBody* const body2, const ContactIn mLocalPointOnBody2(contactInfo->localPoint2), mWorldPointOnBody1(body1->getTransform() * contactInfo->localPoint1), mWorldPointOnBody2(body2->getTransform() * contactInfo->localPoint2), - mIsRestingContact(false) { + mIsRestingContact(false), mFrictionVectors(2, Vector3(0, 0, 0)) { assert(mPenetrationDepth > 0.0); - - // Compute the auxiliary lower and upper bounds - // TODO : Now mC is only the mass of the first body but it is probably wrong - // TODO : Now g is 9.81 but we should use the true gravity value of the physics world. - mMu_mc_g = FRICTION_COEFFICIENT * body1->getMass() * 9.81; - // Compute the friction vectors that span the tangential friction plane - computeFrictionVectors(); } // Destructor 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 -// of the contact. The argument "noConstraint", is the row were the method have -// to start to fill in the J_sp matrix. -void Contact::computeJacobian(int noConstraint, decimal J_sp[NB_MAX_CONSTRAINTS][2*6]) const { - assert(mBody1); - assert(mBody2); - - Vector3 body1Position = mBody1->getTransform().getPosition(); - Vector3 body2Position = mBody2->getTransform().getPosition(); - int currentIndex = noConstraint; // Current constraint index - - 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_sp[currentIndex][0] = -mNormal.getX(); - J_sp[currentIndex][1] = -mNormal.getY(); - J_sp[currentIndex][2] = -mNormal.getZ(); - J_sp[currentIndex][3] = -r1CrossN.getX(); - J_sp[currentIndex][4] = -r1CrossN.getY(); - J_sp[currentIndex][5] = -r1CrossN.getZ(); - - // Compute the jacobian matrix for the body 2 for the contact constraint - J_sp[currentIndex][6] = mNormal.getX(); - J_sp[currentIndex][7] = mNormal.getY(); - J_sp[currentIndex][8] = mNormal.getZ(); - J_sp[currentIndex][9] = r2CrossN.getX(); - J_sp[currentIndex][10] = r2CrossN.getY(); - J_sp[currentIndex][11] = r2CrossN.getZ(); - - currentIndex++; - - // 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]); - Vector3 r1CrossU2 = r1.cross(mFrictionVectors[1]); - Vector3 r2CrossU2 = r2.cross(mFrictionVectors[1]); - J_sp[currentIndex][0] = -mFrictionVectors[0].getX(); - J_sp[currentIndex][1] = -mFrictionVectors[0].getY(); - J_sp[currentIndex][2] = -mFrictionVectors[0].getZ(); - J_sp[currentIndex][3] = -r1CrossU1.getX(); - J_sp[currentIndex][4] = -r1CrossU1.getY(); - J_sp[currentIndex][5] = -r1CrossU1.getZ(); - - // Compute the jacobian matrix for the body 2 for the first friction constraint - J_sp[currentIndex][6] = mFrictionVectors[0].getX(); - J_sp[currentIndex][7] = mFrictionVectors[0].getY(); - J_sp[currentIndex][8] = mFrictionVectors[0].getZ(); - J_sp[currentIndex][9] = r2CrossU1.getX(); - J_sp[currentIndex][10] = r2CrossU1.getY(); - J_sp[currentIndex][11] = r2CrossU1.getZ(); - - currentIndex++; - - // Compute the jacobian matrix for the body 1 for the second friction constraint - J_sp[currentIndex][0] = -mFrictionVectors[1].getX(); - J_sp[currentIndex][1] = -mFrictionVectors[1].getY(); - J_sp[currentIndex][2] = -mFrictionVectors[1].getZ(); - J_sp[currentIndex][3] = -r1CrossU2.getX(); - J_sp[currentIndex][4] = -r1CrossU2.getY(); - J_sp[currentIndex][5] = -r1CrossU2.getZ(); - - // Compute the jacobian matrix for the body 2 for the second friction constraint - J_sp[currentIndex][6] = mFrictionVectors[1].getX(); - J_sp[currentIndex][7] = mFrictionVectors[1].getY(); - J_sp[currentIndex][8] = mFrictionVectors[1].getZ(); - J_sp[currentIndex][9] = r2CrossU2.getX(); - J_sp[currentIndex][10] = r2CrossU2.getY(); - J_sp[currentIndex][11] = r2CrossU2.getZ(); -} - -// Compute the lowerbounds values for all the mathematical constraints. The -// argument "lowerBounds" is the lowerbounds values vector of the constraint solver and -// this methods has to fill in this vector starting from the row "noConstraint" -void Contact::computeLowerBound(int noConstraint, decimal lowerBounds[NB_MAX_CONSTRAINTS]) const { - assert(noConstraint >= 0 && noConstraint + mNbConstraints <= NB_MAX_CONSTRAINTS); - - lowerBounds[noConstraint] = 0.0; // Lower bound for the contact constraint - lowerBounds[noConstraint + 1] = -mMu_mc_g; // Lower bound for the first friction constraint - lowerBounds[noConstraint + 2] = -mMu_mc_g; // Lower bound for the second friction constraint -} - -// Compute the upperbounds values for all the mathematical constraints. The -// argument "upperBounds" is the upperbounds values vector of the constraint solver and -// this methods has to fill in this vector starting from the row "noConstraint" -void Contact::computeUpperBound(int noConstraint, decimal upperBounds[NB_MAX_CONSTRAINTS]) const { - assert(noConstraint >= 0 && noConstraint + mNbConstraints <= NB_MAX_CONSTRAINTS); - - upperBounds[noConstraint] = DECIMAL_INFINITY; // Upper bound for the contact constraint - upperBounds[noConstraint + 1] = mMu_mc_g; // Upper bound for the first friction constraint - 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" -void Contact::computeErrorValue(int noConstraint, decimal errorValues[]) const { - assert(mBody1); - assert(mBody2); - - // TODO : Do we need this casting anymore ? - RigidBody* rigidBody1 = dynamic_cast(mBody1); - RigidBody* rigidBody2 = dynamic_cast(mBody2); - - assert(noConstraint >= 0 && noConstraint + mNbConstraints <= NB_MAX_CONSTRAINTS); - - // Compute the error value for the contact constraint - Vector3 velocity1 = rigidBody1->getLinearVelocity(); - Vector3 velocity2 = rigidBody2->getLinearVelocity(); - decimal restitutionCoeff = rigidBody1->getRestitution() * rigidBody2->getRestitution(); - decimal errorValue = restitutionCoeff * (mNormal.dot(velocity1) - mNormal.dot(velocity2)); - - // Assign the error value to the vector of error values - errorValues[noConstraint] = errorValue; // Error value for contact constraint - errorValues[noConstraint + 1] = 0.0; // Error value for friction constraint - errorValues[noConstraint + 2] = 0.0; // Error value for friction constraint -} diff --git a/src/constraint/Contact.h b/src/constraint/Contact.h index 05d8f215..d18ef023 100644 --- a/src/constraint/Contact.h +++ b/src/constraint/Contact.h @@ -53,12 +53,9 @@ namespace reactphysics3d { /* ------------------------------------------------------------------- Class Contact : - This class represents a collision contact between two bodies in - the physics engine. The contact class inherits from the - Constraint class. Each Contact represent a contact between two bodies - and contains the two contact points on each body. The contact has 3 - mathematical constraints (1 for the contact constraint, and 2 - for the friction constraints). + This class represents a collision contact point between two + bodies in the physics engine. The contact class inherits from + the Constraint class. ------------------------------------------------------------------- */ class Contact : public Constraint { @@ -90,8 +87,6 @@ class Contact : public Constraint { // Two orthogonal vectors that span the tangential friction plane std::vector mFrictionVectors; - - decimal mMu_mc_g; // -------------------- Methods -------------------- // @@ -101,9 +96,6 @@ class Contact : public Constraint { // Private assignment operator Contact& operator=(const Contact& contact); - // Compute the two friction vectors that span the tangential friction plane - void computeFrictionVectors(); - public : // -------------------- Methods -------------------- // @@ -156,37 +148,6 @@ class Contact : public Constraint { // Set the second friction vector void setFrictionVector2(const Vector3& frictionVector2); - // Compute the jacobian matrix for all mathematical constraints - virtual void computeJacobian(int noConstraint, - decimal J_SP[NB_MAX_CONSTRAINTS][2*6]) const; - - // Compute the lowerbounds values for all the mathematical constraints - virtual void computeLowerBound(int noConstraint, - decimal lowerBounds[NB_MAX_CONSTRAINTS]) const; - - // Compute the upperbounds values for all the mathematical constraints - virtual void computeUpperBound(int noConstraint, - decimal upperBounds[NB_MAX_CONSTRAINTS]) const; - - // 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; @@ -196,16 +157,6 @@ class Contact : public Constraint { #endif }; -// Compute the two unit orthogonal vectors "v1" and "v2" that span the tangential friction plane -// The two vectors have to be such that : v1 x v2 = contactNormal -inline void Contact::computeFrictionVectors() { - // Delete the current friction vectors - mFrictionVectors.clear(); - - mFrictionVectors.push_back(Vector3(0, 0, 0)); - mFrictionVectors.push_back(Vector3(0, 0, 0)); -} - // Return the normal vector of the contact inline Vector3 Contact::getNormal() const { return mNormal; @@ -289,6 +240,6 @@ inline void Contact::draw() const { } #endif -} // End of the ReactPhysics3D namespace +} #endif diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp new file mode 100644 index 00000000..56329af2 --- /dev/null +++ b/src/engine/ContactSolver.cpp @@ -0,0 +1,837 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010-2012 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +// Libraries +#include "ContactSolver.h" +#include "DynamicsWorld.h" +#include "../body/RigidBody.h" + +using namespace reactphysics3d; +using namespace std; + +// Constants initialization +const decimal ContactSolver::BETA = decimal(0.2); +const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2); +const decimal ContactSolver::SLOP = decimal(0.01); + +// Constructor +ContactSolver::ContactSolver(DynamicsWorld& world) + :mWorld(world), mNbIterations(DEFAULT_CONSTRAINTS_SOLVER_NB_ITERATIONS), mContactConstraints(0), + mLinearVelocities(0), mAngularVelocities(0), mIsWarmStartingActive(true), + mIsSplitImpulseActive(true), mIsSolveFrictionAtContactManifoldCenterActive(true) { + +} + +// Destructor +ContactSolver::~ContactSolver() { + +} + +// Initialize the constraint solver +void ContactSolver::initialize() { + + // TODO : Use better memory allocation here + mContactConstraints = new ContactManifoldSolver[mWorld.getNbContactManifolds()]; + + mNbContactConstraints = 0; + + // For each contact manifold of the world + vector::iterator it; + for (it = mWorld.getContactManifoldsBeginIterator(); + it != mWorld.getContactManifoldsEndIterator(); ++it) { + + ContactManifold& externalContactManifold = *it; + + ContactManifoldSolver& internalContactManifold = mContactConstraints[mNbContactConstraints]; + + assert(externalContactManifold.nbContacts > 0); + + // Get the two bodies of the contact + RigidBody* body1 = externalContactManifold.contacts[0]->getBody1(); + RigidBody* body2 = externalContactManifold.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); + + // Get the position of the two bodies + Vector3 x1 = body1->getTransform().getPosition(); + Vector3 x2 = body2->getTransform().getPosition(); + + internalContactManifold.indexBody1 = mMapBodyToIndex[body1]; + internalContactManifold.indexBody2 = mMapBodyToIndex[body2]; + internalContactManifold.inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld(); + internalContactManifold.inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld(); + internalContactManifold.isBody1Moving = body1->getIsMotionEnabled(); + internalContactManifold.isBody2Moving = body2->getIsMotionEnabled(); + internalContactManifold.massInverseBody1 = body1->getMassInverse(); + internalContactManifold.massInverseBody2 = body2->getMassInverse(); + internalContactManifold.nbContacts = externalContactManifold.nbContacts; + internalContactManifold.restitutionFactor = computeMixedRestitutionFactor(body1, body2); + internalContactManifold.frictionCoefficient = computeMixedFrictionCoefficient(body1, body2); + internalContactManifold.contactManifold = &(*it); + + // If we solve the friction constraints at the center of the contact manifold + if (mIsSolveFrictionAtContactManifoldCenterActive) { + internalContactManifold.frictionPointBody1 = Vector3(0.0, 0.0, 0.0); + internalContactManifold.frictionPointBody2 = Vector3(0.0, 0.0, 0.0); + } + + // For each contact point of the contact manifold + for (uint c=0; cgetWorldPointOnBody1(); + Vector3 p2 = externalContact->getWorldPointOnBody2(); + + contactPoint.contact = externalContact; + contactPoint.normal = externalContact->getNormal(); + contactPoint.r1 = p1 - x1; + contactPoint.r2 = p2 - x2; + contactPoint.penetrationDepth = externalContact->getPenetrationDepth(); + contactPoint.isRestingContact = externalContact->getIsRestingContact(); + externalContact->setIsRestingContact(true); + contactPoint.oldFrictionVector1 = externalContact->getFrictionVector1(); + contactPoint.oldFrictionVector2 = externalContact->getFrictionVector2(); + contactPoint.penetrationImpulse = 0.0; + contactPoint.friction1Impulse = 0.0; + contactPoint.friction2Impulse = 0.0; + + // If we solve the friction constraints at the center of the contact manifold + if (mIsSolveFrictionAtContactManifoldCenterActive) { + internalContactManifold.frictionPointBody1 += p1; + internalContactManifold.frictionPointBody2 += p2; + } + } + + // If we solve the friction constraints at the center of the contact manifold + if (mIsSolveFrictionAtContactManifoldCenterActive) { + + internalContactManifold.frictionPointBody1 /= static_cast(internalContactManifold.nbContacts); + internalContactManifold.frictionPointBody2 /= static_cast(internalContactManifold.nbContacts); + internalContactManifold.r1Friction = internalContactManifold.frictionPointBody1 - x1; + internalContactManifold.r2Friction = internalContactManifold.frictionPointBody2 - x2; + internalContactManifold.oldFrictionVector1 = externalContactManifold.frictionVector1; + internalContactManifold.oldFrictionVector2 = externalContactManifold.frictionVector2; + + // If warm starting is active + if (mIsWarmStartingActive) { + + // Initialize the accumulated impulses with the previous step accumulated impulses + internalContactManifold.friction1Impulse = externalContactManifold.friction1Impulse; + internalContactManifold.friction2Impulse = externalContactManifold.friction2Impulse; + internalContactManifold.frictionTwistImpulse = externalContactManifold.frictionTwistImpulse; + } + else { + + // Initialize the accumulated impulses to zero + internalContactManifold.friction1Impulse = 0.0; + internalContactManifold.friction2Impulse = 0.0; + internalContactManifold.frictionTwistImpulse = 0.0; + } + } + + mNbContactConstraints++; + } + + // Compute the number of bodies that are part of some active constraint + uint nbBodies = mConstraintBodies.size(); + + // Allocated memory for velocities + // TODO : Use better memory allocation here + mLinearVelocities = new Vector3[nbBodies]; + mAngularVelocities = new Vector3[nbBodies]; + mSplitLinearVelocities = new Vector3[nbBodies]; + mSplitAngularVelocities = new Vector3[nbBodies]; + + assert(mMapBodyToIndex.size() == nbBodies); +} + +// Initialize the constrained bodies +void ContactSolver::initializeBodies() { + + // For each current body that is implied in some constraint + set::iterator it; + for (it = mConstraintBodies.begin(); it != mConstraintBodies.end(); ++it) { + RigidBody* rigidBody = *it; + assert(rigidBody); + + uint bodyNumber = mMapBodyToIndex[rigidBody]; + + mLinearVelocities[bodyNumber] = rigidBody->getLinearVelocity() + mTimeStep * rigidBody->getMassInverse() * rigidBody->getExternalForce(); + mAngularVelocities[bodyNumber] = rigidBody->getAngularVelocity() + mTimeStep * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque(); + mSplitLinearVelocities[bodyNumber] = Vector3(0, 0, 0); + mSplitAngularVelocities[bodyNumber] = Vector3(0, 0, 0); + } +} + +// Initialize the contact constraints before solving the system +void ContactSolver::initializeContactConstraints() { + + // For each contact constraint + for (uint c=0; c 0.0 ? contactPoint.inversePenetrationMass = decimal(1.0) / massPenetration : decimal(0.0); + + if (!mIsSolveFrictionAtContactManifoldCenterActive) { + + // Compute the friction vectors + computeFrictionVectors(deltaV, contactPoint); + + contactPoint.r1CrossT1 = contactPoint.r1.cross(contactPoint.frictionVector1); + contactPoint.r1CrossT2 = contactPoint.r1.cross(contactPoint.frictionVector2); + contactPoint.r2CrossT1 = contactPoint.r2.cross(contactPoint.frictionVector1); + contactPoint.r2CrossT2 = contactPoint.r2.cross(contactPoint.frictionVector2); + + // Compute the inverse mass matrix K for the friction constraints at each contact point + decimal friction1Mass = 0.0; + decimal friction2Mass = 0.0; + if (contactManifold.isBody1Moving) { + friction1Mass += contactManifold.massInverseBody1 + ((I1 * contactPoint.r1CrossT1).cross(contactPoint.r1)).dot(contactPoint.frictionVector1); + friction2Mass += contactManifold.massInverseBody1 + ((I1 * contactPoint.r1CrossT2).cross(contactPoint.r1)).dot(contactPoint.frictionVector2); + } + if (contactManifold.isBody2Moving) { + friction1Mass += contactManifold.massInverseBody2 + ((I2 * contactPoint.r2CrossT1).cross(contactPoint.r2)).dot(contactPoint.frictionVector1); + friction2Mass += contactManifold.massInverseBody2 + ((I2 * contactPoint.r2CrossT2).cross(contactPoint.r2)).dot(contactPoint.frictionVector2); + } + friction1Mass > 0.0 ? contactPoint.inverseFriction1Mass = decimal(1.0) / friction1Mass : decimal(0.0); + friction2Mass > 0.0 ? contactPoint.inverseFriction2Mass = decimal(1.0) / friction2Mass : decimal(0.0); + } + + // Compute the restitution velocity bias "b". We compute this here instead + // of inside the solve() method because we need to use the velocity difference + // at the beginning of the contact. Note that if it is a resting contact (normal velocity + // under a given threshold), we don't add a restitution velocity bias + contactPoint.restitutionBias = 0.0; + decimal deltaVDotN = deltaV.dot(contactPoint.normal); + // TODO : Use a constant here + if (deltaVDotN < 1.0f) { + contactPoint.restitutionBias = contactManifold.restitutionFactor * deltaVDotN; + } + + // Get the cached lambda values of the constraint + if (mIsWarmStartingActive) { + contactPoint.penetrationImpulse = externalContact->getCachedLambda(0); + contactPoint.friction1Impulse = externalContact->getCachedLambda(1); + contactPoint.friction2Impulse = externalContact->getCachedLambda(2); + } + + // Initialize the split impulses to zero + contactPoint.penetrationSplitImpulse = 0.0; + + // If we solve the friction constraints at the center of the contact manifold + if (mIsSolveFrictionAtContactManifoldCenterActive) { + contactManifold.normal += contactPoint.normal; + } + } + + // If we solve the friction constraints at the center of the contact manifold + if (mIsSolveFrictionAtContactManifoldCenterActive) { + + contactManifold.normal.normalize(); + + Vector3 deltaVFrictionPoint = v2 + w2.cross(contactManifold.r2Friction) - + v1 - w1.cross(contactManifold.r1Friction); + + // Compute the friction vectors + computeFrictionVectors(deltaVFrictionPoint, contactManifold); + + // Compute the inverse mass matrix K for the friction constraints at the center of + // the contact manifold + contactManifold.r1CrossT1 = contactManifold.r1Friction.cross(contactManifold.frictionVector1); + contactManifold.r1CrossT2 = contactManifold.r1Friction.cross(contactManifold.frictionVector2); + contactManifold.r2CrossT1 = contactManifold.r2Friction.cross(contactManifold.frictionVector1); + contactManifold.r2CrossT2 = contactManifold.r2Friction.cross(contactManifold.frictionVector2); + decimal friction1Mass = 0.0; + decimal friction2Mass = 0.0; + if (contactManifold.isBody1Moving) { + friction1Mass += contactManifold.massInverseBody1 + ((I1 * contactManifold.r1CrossT1).cross(contactManifold.r1Friction)).dot(contactManifold.frictionVector1); + friction2Mass += contactManifold.massInverseBody1 + ((I1 * contactManifold.r1CrossT2).cross(contactManifold.r1Friction)).dot(contactManifold.frictionVector2); + } + if (contactManifold.isBody2Moving) { + friction1Mass += contactManifold.massInverseBody2 + ((I2 * contactManifold.r2CrossT1).cross(contactManifold.r2Friction)).dot(contactManifold.frictionVector1); + friction2Mass += contactManifold.massInverseBody2 + ((I2 * contactManifold.r2CrossT2).cross(contactManifold.r2Friction)).dot(contactManifold.frictionVector2); + } + friction1Mass > 0.0 ? contactManifold.inverseFriction1Mass = decimal(1.0) / friction1Mass : decimal(0.0); + friction2Mass > 0.0 ? contactManifold.inverseFriction2Mass = decimal(1.0) / friction2Mass : decimal(0.0); + } + } +} + +// 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 ContactSolver::warmStart() { + + // For each constraint + for (uint c=0; c SLOP) biasPenetrationDepth = -(beta/mTimeStep) * + max(0.0f, float(contactPoint.penetrationDepth - SLOP)); + decimal b = biasPenetrationDepth + contactPoint.restitutionBias; + + // Compute the Lagrange multiplier + if (mIsSplitImpulseActive) { + deltaLambda = - (Jv + contactPoint.restitutionBias) * contactPoint.inversePenetrationMass; + } + else { + deltaLambda = - (Jv + b) * contactPoint.inversePenetrationMass; + } + lambdaTemp = contactPoint.penetrationImpulse; + contactPoint.penetrationImpulse = std::max(contactPoint.penetrationImpulse + deltaLambda, 0.0f); + deltaLambda = contactPoint.penetrationImpulse - lambdaTemp; + + // Compute the impulse P=J^T * lambda + const Impulse impulsePenetration = computePenetrationImpulse(deltaLambda, contactPoint); + + // Apply the impulse to the bodies of the constraint + applyImpulse(impulsePenetration, contactManifold); + + sumPenetrationImpulse += contactPoint.penetrationImpulse; + + // If the split impulse position correction is active + if (mIsSplitImpulseActive) { + + // Split impulse (position correction) + const Vector3& v1Split = mSplitLinearVelocities[contactManifold.indexBody1]; + const Vector3& w1Split = mSplitAngularVelocities[contactManifold.indexBody1]; + const Vector3& v2Split = mSplitLinearVelocities[contactManifold.indexBody2]; + const Vector3& w2Split = mSplitAngularVelocities[contactManifold.indexBody2]; + Vector3 deltaVSplit = v2Split + w2Split.cross(contactPoint.r2) - v1Split - w1Split.cross(contactPoint.r1); + decimal JvSplit = deltaVSplit.dot(contactPoint.normal); + decimal deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) * contactPoint.inversePenetrationMass; + decimal lambdaTempSplit = contactPoint.penetrationSplitImpulse; + contactPoint.penetrationSplitImpulse = std::max(contactPoint.penetrationSplitImpulse + deltaLambdaSplit, 0.0f); + deltaLambda = contactPoint.penetrationSplitImpulse - lambdaTempSplit; + + // Compute the impulse P=J^T * lambda + const Impulse splitImpulsePenetration = computePenetrationImpulse(deltaLambdaSplit, + contactPoint); + + applySplitImpulse(splitImpulsePenetration, contactManifold); + } + + // If we do not solve the friction constraints at the center of the contact manifold + if (!mIsSolveFrictionAtContactManifoldCenterActive) { + + // --------- Friction 1 --------- // + + // Compute J*v + deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1); + Jv = deltaV.dot(contactPoint.frictionVector1); + + deltaLambda = -Jv; + deltaLambda *= contactPoint.inverseFriction1Mass; + decimal frictionLimit = contactManifold.frictionCoefficient * contactPoint.penetrationImpulse; + lambdaTemp = contactPoint.friction1Impulse; + contactPoint.friction1Impulse = std::max(-frictionLimit, std::min(contactPoint.friction1Impulse + deltaLambda, frictionLimit)); + deltaLambda = contactPoint.friction1Impulse - lambdaTemp; + + // Compute the impulse P=J^T * lambda + const Impulse impulseFriction1 = computeFriction1Impulse(deltaLambda, contactPoint); + + // Apply the impulses to the bodies of the constraint + applyImpulse(impulseFriction1, contactManifold); + + // --------- Friction 2 --------- // + + // Compute J*v + deltaV = v2 + w2.cross(contactPoint.r2) - v1 - w1.cross(contactPoint.r1); + Jv = deltaV.dot(contactPoint.frictionVector2); + + deltaLambda = -Jv; + deltaLambda *= contactPoint.inverseFriction2Mass; + frictionLimit = contactManifold.frictionCoefficient * contactPoint.penetrationImpulse; + lambdaTemp = contactPoint.friction2Impulse; + contactPoint.friction2Impulse = std::max(-frictionLimit, std::min(contactPoint.friction2Impulse + deltaLambda, frictionLimit)); + deltaLambda = contactPoint.friction2Impulse - lambdaTemp; + + // Compute the impulse P=J^T * lambda + const Impulse impulseFriction2 = computeFriction2Impulse(deltaLambda, contactPoint); + + // Apply the impulses to the bodies of the constraint + applyImpulse(impulseFriction2, contactManifold); + } + } + + // If we solve the friction constraints at the center of the contact manifold + if (mIsSolveFrictionAtContactManifoldCenterActive) { + + // ------ First friction constraint at the center of the contact manifol ------ // + + // Compute J*v + Vector3 deltaV = v2 + w2.cross(contactManifold.r2Friction) - v1 - w1.cross(contactManifold.r1Friction); + decimal Jv = deltaV.dot(contactManifold.frictionVector1); + + decimal deltaLambda = -Jv * contactManifold.inverseFriction1Mass; + decimal frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse; + lambdaTemp = contactManifold.friction1Impulse; + contactManifold.friction1Impulse = std::max(-frictionLimit, std::min(contactManifold.friction1Impulse + deltaLambda, frictionLimit)); + deltaLambda = contactManifold.friction1Impulse - lambdaTemp; + + // Compute the impulse P=J^T * lambda + Vector3 linearImpulseBody1 = -contactManifold.frictionVector1 * deltaLambda; + Vector3 angularImpulseBody1 = -contactManifold.r1CrossT1 * deltaLambda; + Vector3 linearImpulseBody2 = contactManifold.frictionVector1 * deltaLambda; + Vector3 angularImpulseBody2 = contactManifold.r2CrossT1 * deltaLambda; + const Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1, + linearImpulseBody2, angularImpulseBody2); + + // Apply the impulses to the bodies of the constraint + applyImpulse(impulseFriction1, contactManifold); + + // ------ Second friction constraint at the center of the contact manifol ----- // + + // Compute J*v + deltaV = v2 + w2.cross(contactManifold.r2Friction) - v1 - w1.cross(contactManifold.r1Friction); + Jv = deltaV.dot(contactManifold.frictionVector2); + + deltaLambda = -Jv * contactManifold.inverseFriction2Mass; + frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse; + lambdaTemp = contactManifold.friction2Impulse; + contactManifold.friction2Impulse = std::max(-frictionLimit, std::min(contactManifold.friction2Impulse + deltaLambda, frictionLimit)); + deltaLambda = contactManifold.friction2Impulse - lambdaTemp; + + // Compute the impulse P=J^T * lambda + linearImpulseBody1 = -contactManifold.frictionVector2 * deltaLambda; + angularImpulseBody1 = -contactManifold.r1CrossT2 * deltaLambda; + linearImpulseBody2 = contactManifold.frictionVector2 * deltaLambda; + angularImpulseBody2 = contactManifold.r2CrossT2 * deltaLambda; + const Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1, + linearImpulseBody2, angularImpulseBody2); + + // Apply the impulses to the bodies of the constraint + applyImpulse(impulseFriction2, contactManifold); + + // ------ Twist friction constraint at the center of the contact manifol ------ // + + // TODO : Put this in the initialization method + decimal K = contactManifold.normal.dot(contactManifold.inverseInertiaTensorBody1 * contactManifold.normal) + + contactManifold.normal.dot(contactManifold.inverseInertiaTensorBody2 * contactManifold.normal); + + // Compute J*v + deltaV = w2 - w1; + Jv = deltaV.dot(contactManifold.normal); + + // TODO : Compute the inverse mass matrix here for twist friction + deltaLambda = -Jv * (decimal(1.0) / K); + frictionLimit = contactManifold.frictionCoefficient * sumPenetrationImpulse; + lambdaTemp = contactManifold.frictionTwistImpulse; + contactManifold.frictionTwistImpulse = std::max(-frictionLimit, std::min(contactManifold.frictionTwistImpulse + deltaLambda, frictionLimit)); + deltaLambda = contactManifold.frictionTwistImpulse - lambdaTemp; + + // Compute the impulse P=J^T * lambda + linearImpulseBody1 = Vector3(0.0, 0.0, 0.0); + angularImpulseBody1 = -contactManifold.normal * deltaLambda; + linearImpulseBody2 = Vector3(0.0, 0.0, 0.0);; + angularImpulseBody2 = contactManifold.normal * deltaLambda; + const Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1, + linearImpulseBody2, angularImpulseBody2); + + // Apply the impulses to the bodies of the constraint + applyImpulse(impulseTwistFriction, contactManifold); + } + } + } +} + +// Solve the constraints +void ContactSolver::solve(decimal timeStep) { + + mTimeStep = timeStep; + + // Initialize the solver + initialize(); + + initializeBodies(); + + // Fill-in all the matrices needed to solve the LCP problem + initializeContactConstraints(); + + // Warm start the solver + if (mIsWarmStartingActive) { + warmStart(); + } + + // Solve the contact constraints + solveContactConstraints(); + + // Cache the lambda values in order to use them in the next step + storeImpulses(); +} + +// Store the computed impulses to use them to +// warm start the solver at the next iteration +void ContactSolver::storeImpulses() { + + // For each constraint + for (uint c=0; csetCachedLambda(0, contactPoint.penetrationImpulse); + contactPoint.contact->setCachedLambda(1, contactPoint.friction1Impulse); + contactPoint.contact->setCachedLambda(2, contactPoint.friction2Impulse); + + contactPoint.contact->setFrictionVector1(contactPoint.frictionVector1); + contactPoint.contact->setFrictionVector2(contactPoint.frictionVector2); + } + + contactManifold.contactManifold->friction1Impulse = contactManifold.friction1Impulse; + contactManifold.contactManifold->friction2Impulse = contactManifold.friction2Impulse; + contactManifold.contactManifold->frictionTwistImpulse = contactManifold.frictionTwistImpulse; + contactManifold.contactManifold->frictionVector1 = contactManifold.frictionVector1; + contactManifold.contactManifold->frictionVector2 = contactManifold.frictionVector2; + } +} + +// Apply an impulse to the two bodies of a constraint +void ContactSolver::applyImpulse(const Impulse& impulse, const ContactManifoldSolver& contactManifold) { + + // Update the velocities of the bodies by applying the impulse P + if (contactManifold.isBody1Moving) { + mLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 * + impulse.linearImpulseBody1; + mAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * + impulse.angularImpulseBody1; + } + if (contactManifold.isBody2Moving) { + mLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 * + impulse.linearImpulseBody2; + mAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * + impulse.angularImpulseBody2; + } +} + +// Apply an impulse to the two bodies of a constraint +void ContactSolver::applySplitImpulse(const Impulse& impulse, + const ContactManifoldSolver& contactManifold) { + + // Update the velocities of the bodies by applying the impulse P + if (contactManifold.isBody1Moving) { + mSplitLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 * + impulse.linearImpulseBody1; + mSplitAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * + impulse.angularImpulseBody1; + } + if (contactManifold.isBody2Moving) { + mSplitLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 * + impulse.linearImpulseBody2; + mSplitAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * + impulse.angularImpulseBody2; + } +} + +// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane +// for a contact point constraint. The two vectors have to be such that : t1 x t2 = contactNormal. +void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity, + ContactPointSolver& contactPoint) const { + + // Update the old friction vectors + //contact.oldFrictionVector1 = contact.frictionVector1; + //contact.oldFrictionVector2 = contact.frictionVector2; + + assert(contactPoint.normal.length() > 0.0); + + // Compute the velocity difference vector in the tangential plane + Vector3 normalVelocity = deltaVelocity.dot(contactPoint.normal) * contactPoint.normal; + Vector3 tangentVelocity = deltaVelocity - normalVelocity; + + // If the velocty difference in the tangential plane is not zero + decimal lengthTangenVelocity = tangentVelocity.length(); + if (lengthTangenVelocity > 0.0) { + + // Compute the first friction vector in the direction of the tangent + // velocity difference + contactPoint.frictionVector1 = tangentVelocity / lengthTangenVelocity; + } + else { + + // Get any orthogonal vector to the normal as the first friction vector + contactPoint.frictionVector1 = contactPoint.normal.getOneUnitOrthogonalVector(); + } + + // The second friction vector is computed by the cross product of the firs + // friction vector and the contact normal + contactPoint.frictionVector2 = contactPoint.normal.cross(contactPoint.frictionVector1).getUnit(); +} + +// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane +// for a contact constraint. The two vectors have to be such that : t1 x t2 = contactNormal. +void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity, + ContactManifoldSolver& contact) const { + + assert(contact.normal.length() > 0.0); + + // Compute the velocity difference vector in the tangential plane + Vector3 normalVelocity = deltaVelocity.dot(contact.normal) * contact.normal; + Vector3 tangentVelocity = deltaVelocity - normalVelocity; + + // If the velocty difference in the tangential plane is not zero + decimal lengthTangenVelocity = tangentVelocity.length(); + if (lengthTangenVelocity > 0.0) { + + // Compute the first friction vector in the direction of the tangent + // velocity difference + contact.frictionVector1 = tangentVelocity / lengthTangenVelocity; + } + else { + + // Get any orthogonal vector to the normal as the first friction vector + contact.frictionVector1 = contact.normal.getOneUnitOrthogonalVector(); + } + + // The second friction vector is computed by the cross product of the firs + // friction vector and the contact normal + contact.frictionVector2 = contact.normal.cross(contact.frictionVector1).getUnit(); +} + +// Clean up the constraint solver +void ContactSolver::cleanup() { + mMapBodyToIndex.clear(); + mConstraintBodies.clear(); + + if (mContactConstraints != 0) { + delete[] mContactConstraints; + mContactConstraints = 0; + } + if (mLinearVelocities != 0) { + delete[] mLinearVelocities; + mLinearVelocities = 0; + } + if (mAngularVelocities != 0) { + delete[] mAngularVelocities; + mAngularVelocities = 0; + } +} diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h new file mode 100644 index 00000000..6b1d9c61 --- /dev/null +++ b/src/engine/ContactSolver.h @@ -0,0 +1,459 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * +* Copyright (c) 2010-2012 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef CONSTRAINT_SOLVER_H +#define CONSTRAINT_SOLVER_H + +// Libraries +#include "../constraint/Contact.h" +#include "ContactManifold.h" +#include "../configuration.h" +#include "../constraint/Constraint.h" +#include +#include + +// ReactPhysics3D namespace +namespace reactphysics3d { + +// Declarations +class DynamicsWorld; + +// Structure Impulse +struct Impulse { + + public: + const Vector3 linearImpulseBody1; + const Vector3 linearImpulseBody2; + const Vector3 angularImpulseBody1; + const Vector3 angularImpulseBody2; + + // Constructor + Impulse(const Vector3& linearImpulseBody1, const Vector3& angularImpulseBody1, + const Vector3& linearImpulseBody2, const Vector3& angularImpulseBody2) + : linearImpulseBody1(linearImpulseBody1), angularImpulseBody1(angularImpulseBody1), + linearImpulseBody2(linearImpulseBody2), angularImpulseBody2(angularImpulseBody2) { + + } +}; + + + +/* ------------------------------------------------------------------- + Class ContactSolver : + This class represents the contacts solver that is used is solve rigid bodies contacts. + The constraint solver is based on the "Sequential Impulse" technique described by + Erin Catto in his GDC slides (http://code.google.com/p/box2d/downloads/list). + + A constraint between two bodies is represented by a function C(x) which is equal to zero + when the constraint is satisfied. The condition C(x)=0 describes a valid position and the + condition dC(x)/dt=0 describes a valid velocity. We have dC(x)/dt = Jv + b = 0 where J is + the Jacobian matrix of the constraint, v is a vector that contains the velocity of both + bodies and b is the constraint bias. We are looking for a force F_c that will act on the + bodies to keep the constraint satisfied. Note that from the virtual work principle, we have + F_c = J^t * lambda where J^t is the transpose of the Jacobian matrix and lambda is a + Lagrange multiplier. Therefore, finding the force F_c is equivalent to finding the Lagrange + multiplier lambda. + + An impulse P = F * dt where F is a force and dt is the timestep. We can apply impulses a + body to change its velocity. The idea of the Sequential Impulse technique is to apply + impulses to bodies of each constraints in order to keep the constraint satisfied. + + --- Step 1 --- + + First, we integrate the applied force F_a acting of each rigid body (like gravity, ...) and + we obtain some new velocities v2' that tends to violate the constraints. + + v2' = v1 + dt * M^-1 * F_a + + where M is a matrix that contains mass and inertia tensor information. + + --- Step 2 --- + + During the second step, we iterate over all the constraints for a certain number of + iterations and for each constraint we compute the impulse to apply to the bodies needed + so that the new velocity of the bodies satisfy Jv + b = 0. From the Newton law, we know that + M * deltaV = P_c where M is the mass of the body, deltaV is the difference of velocity and + P_c is the constraint impulse to apply to the body. Therefore, we have + v2 = v2' + M^-1 * P_c. For each constraint, we can compute the Lagrange multiplier lambda + using : lambda = -m_c (Jv2' + b) where m_c = 1 / (J * M^-1 * J^t). Now that we have the + Lagrange multiplier lambda, we can compute the impulse P_c = J^t * lambda * dt to apply to + the bodies to satisfy the constraint. + + --- Step 3 --- + + In the third step, we integrate the new position x2 of the bodies using the new velocities + v2 computed in the second step with : x2 = x1 + dt * v2. + + Note that in the following code (as it is also explained in the slides from Erin Catto), + the value lambda is not only the lagrange multiplier but is the multiplication of the + Lagrange multiplier with the timestep dt. Therefore, in the following code, when we use + lambda, we mean (lambda * dt). + + We are using the accumulated impulse technique that is also described in the slides from + Erin Catto. + + We are also using warm starting. The idea is to warm start the solver at the beginning of + each step by applying the last impulstes for the constraints that we already existing at the + previous step. This allows the iterative solver to converge faster towards the solution. + + For contact constraints, we are also using split impulses so that the position correction + that uses Baumgarte stabilization does not change the momentum of the bodies. + + There are two ways to apply the friction constraints. Either the friction constraints are + applied at each contact point or they are applied only at the center of the contact manifold + between two bodies. If we solve the friction constraints at each contact point, we need + two constraints (two tangential friction directions) and if we solve the friction constraints + at the center of the contact manifold, we need two constraints for tangential friction but + also another twist friction constraint to prevent spin of the body around the contact + manifold center. + + ------------------------------------------------------------------- +*/ +class ContactSolver { + + private: + + // Structure ContactPoint + // Contact solver internal data structure that to store all the + // information relative to a contact point + struct ContactPointSolver { + + decimal penetrationImpulse; // Accumulated normal impulse + decimal friction1Impulse; // Accumulated impulse in the 1st friction direction + decimal friction2Impulse; // Accumulated impulse in the 2nd friction direction + decimal penetrationSplitImpulse; // Accumulated split impulse for penetration correction + Vector3 normal; // Normal vector of the contact + Vector3 frictionVector1; // First friction vector in the tangent plane + Vector3 frictionVector2; // Second friction vector in the tangent plane + Vector3 oldFrictionVector1; // Old first friction vector in the tangent plane + Vector3 oldFrictionVector2; // Old second friction vector in the tangent plane + Vector3 r1; // Vector from the body 1 center to the contact point + Vector3 r2; // Vector from the body 2 center to the contact point + Vector3 r1CrossT1; // Cross product of r1 with 1st friction vector + Vector3 r1CrossT2; // Cross product of r1 with 2nd friction vector + Vector3 r2CrossT1; // Cross product of r2 with 1st friction vector + Vector3 r2CrossT2; // Cross product of r2 with 2nd friction vector + Vector3 r1CrossN; // Cross product of r1 with the contact normal + Vector3 r2CrossN; // Cross product of r2 with the contact normal + decimal penetrationDepth; // Penetration depth + decimal restitutionBias; // Velocity restitution bias + 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 + bool isRestingContact; // True if the contact was existing last time step + Contact* contact; // Pointer to the external contact + }; + + // Structure ContactManifold + // Contact solver internal data structure to store all the + // information relative to a contact manifold + struct ContactManifoldSolver { + + // TODO : Use a constant for the number of contact points + + uint indexBody1; // Index of body 1 in the constraint solver + uint indexBody2; // Index of body 2 in the constraint solver + decimal massInverseBody1; // Inverse of the mass of body 1 + decimal massInverseBody2; // Inverse of the mass of body 2 + Matrix3x3 inverseInertiaTensorBody1; // Inverse inertia tensor of body 1 + Matrix3x3 inverseInertiaTensorBody2; // Inverse inertia tensor of body 2 + bool isBody1Moving; // True if the body 1 is allowed to move + bool isBody2Moving; // True if the body 2 is allowed to move + ContactPointSolver contacts[4]; // Contact point constraints + uint nbContacts; // Number of contact points + decimal restitutionFactor; // Mix of the restitution factor for two bodies + decimal frictionCoefficient; // Mix friction coefficient for the two bodies + ContactManifold* contactManifold; // Pointer to the external contact manifold + + // Variables used when friction constraints are apply at the center of the manifold // + + Vector3 normal; // Average normal vector of the contact manifold + Vector3 frictionPointBody1; // Point on body 1 where to apply the friction constraints + Vector3 frictionPointBody2; // Point on body 2 where to apply the friction constraints + Vector3 r1Friction; // R1 vector for the friction constraints + Vector3 r2Friction; // R2 vector for the friction constraints + Vector3 r1CrossT1; // Cross product of r1 with 1st friction vector + Vector3 r1CrossT2; // Cross product of r1 with 2nd friction vector + Vector3 r2CrossT1; // Cross product of r2 with 1st friction vector + Vector3 r2CrossT2; // Cross product of r2 with 2nd friction vector + decimal inverseFriction1Mass; // Matrix K for the first friction constraint + decimal inverseFriction2Mass; // Matrix K for the second friction constraint + Vector3 frictionVector1; // First friction direction at contact manifold center + Vector3 frictionVector2; // Second friction direction at contact manifold center + Vector3 oldFrictionVector1; // Old 1st friction direction at contact manifold center + Vector3 oldFrictionVector2; // Old 2nd friction direction at contact manifold center + decimal friction1Impulse; // First friction direction impulse at manifold center + decimal friction2Impulse; // Second friction direction impulse at manifold center + decimal frictionTwistImpulse; // Twist friction impulse at contact manifold center + }; + + // -------------------- Constants --------------------- // + + // Beta value for the penetration depth position correction without split impulses + static const decimal BETA; + + // Beta value for the penetration depth position correction with split impulses + static const decimal BETA_SPLIT_IMPULSE; + + // Slop distance (allowed penetration distance between bodies) + static const decimal SLOP; + + // -------------------- Attributes -------------------- // + + // Reference to the world + DynamicsWorld& mWorld; + + // Number of iterations of the constraints solver + uint mNbIterations; + + // Array of constrained linear velocities + Vector3* mLinearVelocities; + + // Array of constrained angular velocities + Vector3* mAngularVelocities; + + // Split linear velocities for the position contact solver (split impulse) + Vector3* mSplitLinearVelocities; + + // Split angular velocities for the position contact solver (split impulse) + Vector3* mSplitAngularVelocities; + + // Current time step + decimal mTimeStep; + + // Contact constraints + ContactManifoldSolver* mContactConstraints; + + // Number of contact constraints + uint mNbContactConstraints; + + // Constrained bodies + std::set mConstraintBodies; + + // Map body to index + std::map mMapBodyToIndex; + + // True if the warm starting of the solver is active + bool mIsWarmStartingActive; + + // True if the split impulse position correction is active + bool mIsSplitImpulseActive; + + // True if we solve 3 friction constraints at the contact manifold center only + // instead of 2 friction constraints at each contact point + bool mIsSolveFrictionAtContactManifoldCenterActive; + + // -------------------- Methods -------------------- // + + // 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 ContactManifoldSolver& contactManifold); + + // Apply an impulse to the two bodies of a constraint + void applySplitImpulse(const Impulse& impulse, const ContactManifoldSolver& contactManifold); + + // Compute the collision restitution factor from the restitution factor of each body + decimal computeMixedRestitutionFactor(const RigidBody* body1, const RigidBody* body2) const; + + // Compute the mixed friction coefficient from the friction coefficient of each body + decimal computeMixedFrictionCoefficient(const RigidBody* body1, const RigidBody* body2)const; + + // Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction + // plane for a contact point constraint. The two vectors have to be + // such that : t1 x t2 = contactNormal. + void computeFrictionVectors(const Vector3& deltaVelocity, + ContactPointSolver &contactPoint) const; + + // Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction + // plane for a contact constraint. The two vectors have to be + // such that : t1 x t2 = contactNormal. + void computeFrictionVectors(const Vector3& deltaVelocity, + ContactManifoldSolver& contactPoint) const; + + // Compute a penetration constraint impulse + const Impulse computePenetrationImpulse(decimal deltaLambda, + const ContactPointSolver& contactPoint) const; + + // Compute the first friction constraint impulse + const Impulse computeFriction1Impulse(decimal deltaLambda, + const ContactPointSolver& contactPoint) const; + + // Compute the second friction constraint impulse + const Impulse computeFriction2Impulse(decimal deltaLambda, + const ContactPointSolver& contactPoint) const; + + public: + + // -------------------- Methods -------------------- // + + // Constructor + ContactSolver(DynamicsWorld& mWorld); + + // Destructor + virtual ~ContactSolver(); + + // 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 split linear velocity + Vector3 getSplitLinearVelocityOfBody(RigidBody* body); + + // Return the constrained angular velocity of a body after solving the constraints + Vector3 getConstrainedAngularVelocityOfBody(RigidBody* body); + + // Return the split angular velocity + Vector3 getSplitAngularVelocityOfBody(RigidBody* body); + + // Clean up the constraint solver + void cleanup(); + + // Set the number of iterations of the constraint solver + void setNbIterationsSolver(uint nbIterations); + + // Activate or Deactivate the split impulses for contacts + void setIsSplitImpulseActive(bool isActive); + + // Activate or deactivate the solving of friction constraints at the center of + // the contact manifold instead of solving them at each contact point + void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive); +}; + +// Return true if the body is in at least one constraint +inline bool ContactSolver::isConstrainedBody(RigidBody* body) const { + return mConstraintBodies.count(body) == 1; +} + +// Return the constrained linear velocity of a body after solving the constraints +inline Vector3 ContactSolver::getConstrainedLinearVelocityOfBody(RigidBody* body) { + assert(isConstrainedBody(body)); + uint indexBody = mMapBodyToIndex[body]; + return mLinearVelocities[indexBody]; +} + +// Return the split linear velocity +inline Vector3 ContactSolver::getSplitLinearVelocityOfBody(RigidBody* body) { + assert(isConstrainedBody(body)); + uint indexBody = mMapBodyToIndex[body]; + return mSplitLinearVelocities[indexBody]; +} + +// Return the constrained angular velocity of a body after solving the constraints +inline Vector3 ContactSolver::getConstrainedAngularVelocityOfBody(RigidBody *body) { + assert(isConstrainedBody(body)); + uint indexBody = mMapBodyToIndex[body]; + return mAngularVelocities[indexBody]; +} + +// Return the split angular velocity +inline Vector3 ContactSolver::getSplitAngularVelocityOfBody(RigidBody* body) { + assert(isConstrainedBody(body)); + uint indexBody = mMapBodyToIndex[body]; + return mSplitAngularVelocities[indexBody]; +} + +// Set the number of iterations of the constraint solver +inline void ContactSolver::setNbIterationsSolver(uint nbIterations) { + mNbIterations = nbIterations; +} + +// Activate or Deactivate the split impulses for contacts +inline void ContactSolver::setIsSplitImpulseActive(bool isActive) { + mIsSplitImpulseActive = isActive; +} + +// Activate or deactivate the solving of friction constraints at the center of +// the contact manifold instead of solving them at each contact point +inline void ContactSolver::setIsSolveFrictionAtContactManifoldCenterActive(bool isActive) { + mIsSolveFrictionAtContactManifoldCenterActive = isActive; +} + +// Compute the collision restitution factor from the restitution factor of each body +inline decimal ContactSolver::computeMixedRestitutionFactor(const RigidBody* body1, + const RigidBody* body2) const { + decimal restitution1 = body1->getRestitution(); + decimal restitution2 = body2->getRestitution(); + + // Return the largest restitution factor + return (restitution1 > restitution2) ? restitution1 : restitution2; +} + +// Compute the mixed friction coefficient from the friction coefficient of each body +inline decimal ContactSolver::computeMixedFrictionCoefficient(const RigidBody* body1, + const RigidBody* body2) const { + // Use the geometric mean to compute the mixed friction coefficient + return sqrt(body1->getFrictionCoefficient() * body2->getFrictionCoefficient()); +} + +// Compute a penetration constraint impulse +inline const Impulse ContactSolver::computePenetrationImpulse(decimal deltaLambda, + const ContactPointSolver& contactPoint) + const { + return Impulse(-contactPoint.normal * deltaLambda, -contactPoint.r1CrossN * deltaLambda, + contactPoint.normal * deltaLambda, contactPoint.r2CrossN * deltaLambda); +} + +// Compute the first friction constraint impulse +inline const Impulse ContactSolver::computeFriction1Impulse(decimal deltaLambda, + const ContactPointSolver& contactPoint) + const { + return Impulse(-contactPoint.frictionVector1 * deltaLambda, -contactPoint.r1CrossT1 * deltaLambda, + contactPoint.frictionVector1 * deltaLambda, contactPoint.r2CrossT1 * deltaLambda); +} + +// Compute the second friction constraint impulse +inline const Impulse ContactSolver::computeFriction2Impulse(decimal deltaLambda, + const ContactPointSolver& contactPoint) + const { + return Impulse(-contactPoint.frictionVector2 * deltaLambda, -contactPoint.r1CrossT2 * deltaLambda, + contactPoint.frictionVector2 * deltaLambda, contactPoint.r2CrossT2 * deltaLambda); +} + +} // End of ReactPhysics3D namespace + +#endif diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index f03a48c1..f638bd8d 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -32,7 +32,7 @@ using namespace std; // Constructor DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_TIMESTEP) - : CollisionWorld(), mTimer(timeStep), mGravity(gravity), mIsGravityOn(true), mConstraintSolver(this), + : CollisionWorld(), mTimer(timeStep), mGravity(gravity), mIsGravityOn(true), mContactSolver(*this), mIsDeactivationActive(DEACTIVATION_ENABLED) { } @@ -68,11 +68,11 @@ void DynamicsWorld::update() { // Compute the collision detection mCollisionDetection.computeCollisionDetection(); - // If there are constraints or contacts - if (!mConstraints.empty() || !mContactManifolds.empty()) { + // If there are contacts + if (!mContactManifolds.empty()) { - // Solve the constraints and contacts - mConstraintSolver.solve(mTimer.getTimeStep()); + // Solve the contacts + mContactSolver.solve(mTimer.getTimeStep()); } // Update the timer @@ -84,8 +84,8 @@ void DynamicsWorld::update() { // Update the position and orientation of each body updateAllBodiesMotion(); - // Cleanup of the constraint solver - mConstraintSolver.cleanup(); + // Cleanup of the contact solver + mContactSolver.cleanup(); } // Compute and set the interpolation factor to all the bodies @@ -118,10 +118,10 @@ void DynamicsWorld::updateAllBodiesMotion() { newAngularVelocity.setAllValues(0.0, 0.0, 0.0); // If it's a constrained body - if (mConstraintSolver.isConstrainedBody(*it)) { + if (mContactSolver.isConstrainedBody(*it)) { // Get the constrained linear and angular velocities from the constraint solver - newLinearVelocity = mConstraintSolver.getConstrainedLinearVelocityOfBody(rigidBody); - newAngularVelocity = mConstraintSolver.getConstrainedAngularVelocityOfBody(rigidBody); + newLinearVelocity = mContactSolver.getConstrainedLinearVelocityOfBody(rigidBody); + newAngularVelocity = mContactSolver.getConstrainedAngularVelocityOfBody(rigidBody); } else { // Compute V_forces = dt * (M^-1 * F_ext) which is the velocity of the body due to the @@ -161,9 +161,9 @@ void DynamicsWorld::updatePositionAndOrientationOfBody(RigidBody* rigidBody, rigidBody->setAngularVelocity(newAngVelocity); // Split velocity (only used to update the position) - if (mConstraintSolver.isConstrainedBody(rigidBody)) { - newLinVelocity += mConstraintSolver.getSplitLinearVelocityOfBody(rigidBody); - newAngVelocity += mConstraintSolver.getSplitAngularVelocityOfBody(rigidBody); + if (mContactSolver.isConstrainedBody(rigidBody)) { + newLinVelocity += mContactSolver.getSplitLinearVelocityOfBody(rigidBody); + newAngVelocity += mContactSolver.getSplitAngularVelocityOfBody(rigidBody); } // Get current position and orientation of the body diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index ae8b685a..51f3b6b1 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -30,7 +30,7 @@ #include "CollisionWorld.h" #include "ContactManifold.h" #include "../collision/CollisionDetection.h" -#include "ConstraintSolver.h" +#include "ContactSolver.h" #include "../body/RigidBody.h" #include "Timer.h" #include "../configuration.h" @@ -54,8 +54,8 @@ class DynamicsWorld : public CollisionWorld { // Timer of the physics engine Timer mTimer; - // Constraint solver - ConstraintSolver mConstraintSolver; + // Contact solver + ContactSolver mContactSolver; // True if the deactivation (sleeping) of inactive bodies is enabled bool mIsDeactivationActive; @@ -214,18 +214,18 @@ inline void DynamicsWorld::stop() { // Set the number of iterations of the constraint solver inline void DynamicsWorld::setNbIterationsSolver(uint nbIterations) { - mConstraintSolver.setNbIterationsSolver(nbIterations); + mContactSolver.setNbIterationsSolver(nbIterations); } // Activate or Deactivate the split impulses for contacts inline void DynamicsWorld::setIsSplitImpulseActive(bool isActive) { - mConstraintSolver.setIsSplitImpulseActive(isActive); + mContactSolver.setIsSplitImpulseActive(isActive); } // Activate or deactivate the solving of friction constraints at the center of // the contact manifold instead of solving them at each contact point inline void DynamicsWorld::setIsSolveFrictionAtContactManifoldCenterActive(bool isActive) { - mConstraintSolver.setIsSolveFrictionAtContactManifoldCenterActive(isActive); + mContactSolver.setIsSolveFrictionAtContactManifoldCenterActive(isActive); } // Reset the boolean movement variable of each body diff --git a/src/engine/PersistentContactCache.h b/src/engine/PersistentContactCache.h index 0419ad71..a1b28a8d 100644 --- a/src/engine/PersistentContactCache.h +++ b/src/engine/PersistentContactCache.h @@ -136,7 +136,7 @@ inline Contact* PersistentContactCache::getContact(uint index) const { // Return true if two vectors are approximatively equal inline bool PersistentContactCache::isApproxEqual(const Vector3& vector1, const Vector3& vector2) const { - const decimal epsilon = 0.1; + const decimal epsilon = decimal(0.1); return (approxEqual(vector1.getX(), vector2.getX(), epsilon) && approxEqual(vector1.getY(), vector2.getY(), epsilon) && approxEqual(vector1.getZ(), vector2.getZ(), epsilon)); diff --git a/src/mathematics/Transform.h b/src/mathematics/Transform.h index ecdca9b5..71d82044 100644 --- a/src/mathematics/Transform.h +++ b/src/mathematics/Transform.h @@ -176,7 +176,7 @@ inline Transform Transform::interpolateTransforms(const Transform& oldTransform, const Transform& newTransform, decimal interpolationFactor) { - Vector3 interPosition = oldTransform.mPosition * (1.0 - interpolationFactor) + + Vector3 interPosition = oldTransform.mPosition * (decimal(1.0) - interpolationFactor) + newTransform.mPosition * interpolationFactor; Quaternion interOrientation = Quaternion::slerp(oldTransform.mOrientation, From aa236286de76d380cb77dd0ff237b48c952989b8 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Tue, 19 Feb 2013 23:16:48 +0100 Subject: [PATCH 24/26] Remove files --- src/engine/ConstraintSolver.cpp | 835 -------------------------------- src/engine/ConstraintSolver.h | 424 ---------------- 2 files changed, 1259 deletions(-) delete mode 100644 src/engine/ConstraintSolver.cpp delete mode 100644 src/engine/ConstraintSolver.h diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp deleted file mode 100644 index 8e81b266..00000000 --- a/src/engine/ConstraintSolver.cpp +++ /dev/null @@ -1,835 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * -* Copyright (c) 2010-2012 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -// Libraries -#include "ConstraintSolver.h" -#include "DynamicsWorld.h" -#include "../body/RigidBody.h" - -using namespace reactphysics3d; -using namespace std; - -// Constants initialization -const decimal ConstraintSolver::BETA = 0.2; -const decimal ConstraintSolver::BETA_SPLIT_IMPULSE = 0.2; -const decimal ConstraintSolver::SLOP = 0.01; - -// Constructor -ConstraintSolver::ConstraintSolver(DynamicsWorld* world) - :world(world), nbConstraints(0), mNbIterations(10), mContactConstraints(0), - mLinearVelocities(0), mAngularVelocities(0), mIsWarmStartingActive(true), - mIsSplitImpulseActive(true), mIsSolveFrictionAtContactManifoldCenterActive(true) { - -} - -// Destructor -ConstraintSolver::~ConstraintSolver() { - -} - -// Initialize the constraint solver -void ConstraintSolver::initialize() { - - nbConstraints = 0; - - // TODO : Use better allocation here - mContactConstraints = new ContactConstraint[world->getNbContactManifolds()]; - - 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); - - Vector3 x1 = body1->getTransform().getPosition(); - Vector3 x2 = body2->getTransform().getPosition(); - - 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; - constraint.restitutionFactor = computeMixRestitutionFactor(body1, body2); - constraint.contactManifold = &contactManifold; - - // If we solve the friction constraints at the center of the contact manifold - if (mIsSolveFrictionAtContactManifoldCenterActive) { - constraint.frictionPointBody1 = Vector3(0.0, 0.0, 0.0); - constraint.frictionPointBody2 = Vector3(0.0, 0.0, 0.0); - } - - // For each contact point of the contact manifold - for (uint c=0; cgetWorldPointOnBody1(); - Vector3 p2 = contact->getWorldPointOnBody2(); - - contactPointConstraint.contact = contact; - contactPointConstraint.normal = contact->getNormal(); - contactPointConstraint.r1 = p1 - x1; - contactPointConstraint.r2 = p2 - x2; - contactPointConstraint.penetrationDepth = contact->getPenetrationDepth(); - contactPointConstraint.isRestingContact = contact->getIsRestingContact(); - contact->setIsRestingContact(true); - contactPointConstraint.oldFrictionVector1 = contact->getFrictionVector1(); - contactPointConstraint.oldFrictionVector2 = contact->getFrictionVector2(); - contactPointConstraint.penetrationImpulse = 0.0; - contactPointConstraint.friction1Impulse = 0.0; - contactPointConstraint.friction2Impulse = 0.0; - - // If we solve the friction constraints at the center of the contact manifold - if (mIsSolveFrictionAtContactManifoldCenterActive) { - constraint.frictionPointBody1 += p1; - constraint.frictionPointBody2 += p2; - } - } - - // If we solve the friction constraints at the center of the contact manifold - if (mIsSolveFrictionAtContactManifoldCenterActive) { - constraint.frictionPointBody1 /= constraint.nbContacts; - constraint.frictionPointBody2 /= constraint.nbContacts; - constraint.r1Friction = constraint.frictionPointBody1 - x1; - constraint.r2Friction = constraint.frictionPointBody2 - x2; - constraint.oldFrictionVector1 = contactManifold.frictionVector1; - constraint.oldFrictionVector2 = contactManifold.frictionVector2; - - if (mIsWarmStartingActive) { - constraint.friction1Impulse = contactManifold.friction1Impulse; - constraint.friction2Impulse = contactManifold.friction2Impulse; - constraint.frictionTwistImpulse = contactManifold.frictionTwistImpulse; - } - else { - constraint.friction1Impulse = 0.0; - constraint.friction2Impulse = 0.0; - constraint.frictionTwistImpulse = 0.0; - } - } - - mNbContactConstraints++; - } - - // Compute the number of bodies that are part of some active constraint - nbBodies = mConstraintBodies.size(); - - mLinearVelocities = new Vector3[nbBodies]; - mAngularVelocities = new Vector3[nbBodies]; - mSplitLinearVelocities = new Vector3[nbBodies]; - mSplitAngularVelocities = new Vector3[nbBodies]; - - assert(mMapBodyToIndex.size() == nbBodies); -} - -// Initialize the constrained bodies -void ConstraintSolver::initializeBodies() { - - // For each current body that is implied in some constraint - RigidBody* rigidBody; - 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); - - mLinearVelocities[bodyNumber] = rigidBody->getLinearVelocity() + mTimeStep * rigidBody->getMassInverse() * rigidBody->getExternalForce(); - mAngularVelocities[bodyNumber] = rigidBody->getAngularVelocity() + mTimeStep * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque(); - mSplitLinearVelocities[bodyNumber] = Vector3(0, 0, 0); - mSplitAngularVelocities[bodyNumber] = Vector3(0, 0, 0); - } -} - -// Initialize the contact constraints before solving the system -void ConstraintSolver::initializeContactConstraints() { - - // For each contact constraint - for (uint c=0; c 0.0 ? contact.inversePenetrationMass = 1.0 / massPenetration : 0.0; - - if (!mIsSolveFrictionAtContactManifoldCenterActive) { - - // Compute the friction vectors - computeFrictionVectors(deltaV, contact); - - contact.r1CrossT1 = contact.r1.cross(contact.frictionVector1); - contact.r1CrossT2 = contact.r1.cross(contact.frictionVector2); - contact.r2CrossT1 = contact.r2.cross(contact.frictionVector1); - contact.r2CrossT2 = contact.r2.cross(contact.frictionVector2); - - // Compute the inverse mass matrix K for the friction constraints at each contact point - decimal friction1Mass = 0.0; - decimal friction2Mass = 0.0; - if (constraint.isBody1Moving) { - friction1Mass += constraint.massInverseBody1 + ((I1 * contact.r1CrossT1).cross(contact.r1)).dot(contact.frictionVector1); - friction2Mass += constraint.massInverseBody1 + ((I1 * contact.r1CrossT2).cross(contact.r1)).dot(contact.frictionVector2); - } - if (constraint.isBody2Moving) { - friction1Mass += constraint.massInverseBody2 + ((I2 * contact.r2CrossT1).cross(contact.r2)).dot(contact.frictionVector1); - friction2Mass += constraint.massInverseBody2 + ((I2 * contact.r2CrossT2).cross(contact.r2)).dot(contact.frictionVector2); - } - friction1Mass > 0.0 ? contact.inverseFriction1Mass = 1.0 / friction1Mass : 0.0; - friction2Mass > 0.0 ? contact.inverseFriction2Mass = 1.0 / friction2Mass : 0.0; - } - - // Compute the restitution velocity bias "b". We compute this here instead - // of inside the solve() method because we need to use the velocity difference - // at the beginning of the contact. Note that if it is a resting contact (normal velocity - // under a given threshold), we don't add a restitution velocity bias - contact.restitutionBias = 0.0; - decimal deltaVDotN = deltaV.dot(contact.normal); - // TODO : Use a constant here - if (deltaVDotN < 1.0f) { - contact.restitutionBias = constraint.restitutionFactor * deltaVDotN; - } - - // Get the cached lambda values of the constraint - if (mIsWarmStartingActive) { - contact.penetrationImpulse = realContact->getCachedLambda(0); - contact.friction1Impulse = realContact->getCachedLambda(1); - contact.friction2Impulse = realContact->getCachedLambda(2); - } - - // Initialize the split impulses to zero - contact.penetrationSplitImpulse = 0.0; - - // If we solve the friction constraints at the center of the contact manifold - if (mIsSolveFrictionAtContactManifoldCenterActive) { - constraint.normal += contact.normal; - } - } - - // If we solve the friction constraints at the center of the contact manifold - if (mIsSolveFrictionAtContactManifoldCenterActive) { - - constraint.normal.normalize(); - - Vector3 deltaVFrictionPoint = v2 + w2.cross(constraint.r2Friction) - - v1 - w1.cross(constraint.r1Friction); - - // Compute the friction vectors - computeFrictionVectors(deltaVFrictionPoint, constraint); - - // Compute the inverse mass matrix K for the friction constraints at the center of - // the contact manifold - constraint.r1CrossT1 = constraint.r1Friction.cross(constraint.frictionVector1); - constraint.r1CrossT2 = constraint.r1Friction.cross(constraint.frictionVector2); - constraint.r2CrossT1 = constraint.r2Friction.cross(constraint.frictionVector1); - constraint.r2CrossT2 = constraint.r2Friction.cross(constraint.frictionVector2); - decimal friction1Mass = 0.0; - decimal friction2Mass = 0.0; - if (constraint.isBody1Moving) { - friction1Mass += constraint.massInverseBody1 + ((I1 * constraint.r1CrossT1).cross(constraint.r1Friction)).dot(constraint.frictionVector1); - friction2Mass += constraint.massInverseBody1 + ((I1 * constraint.r1CrossT2).cross(constraint.r1Friction)).dot(constraint.frictionVector2); - } - if (constraint.isBody2Moving) { - friction1Mass += constraint.massInverseBody2 + ((I2 * constraint.r2CrossT1).cross(constraint.r2Friction)).dot(constraint.frictionVector1); - friction2Mass += constraint.massInverseBody2 + ((I2 * constraint.r2CrossT2).cross(constraint.r2Friction)).dot(constraint.frictionVector2); - } - friction1Mass > 0.0 ? constraint.inverseFriction1Mass = 1.0 / friction1Mass : 0.0; - friction2Mass > 0.0 ? constraint.inverseFriction2Mass = 1.0 / friction2Mass : 0.0; - } - } -} - -// 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 c=0; c SLOP) biasPenetrationDepth = -(beta/mTimeStep) * - max(0.0f, float(contact.penetrationDepth - SLOP)); - decimal b = biasPenetrationDepth + contact.restitutionBias; - - // Compute the Lagrange multiplier - if (mIsSplitImpulseActive) { - deltaLambda = - (Jv + contact.restitutionBias) * contact.inversePenetrationMass; - } - else { - deltaLambda = - (Jv + b) * contact.inversePenetrationMass; - } - lambdaTemp = contact.penetrationImpulse; - contact.penetrationImpulse = std::max(contact.penetrationImpulse + deltaLambda, 0.0f); - deltaLambda = contact.penetrationImpulse - lambdaTemp; - - // Compute the impulse P=J^T * lambda - Vector3 linearImpulseBody1 = -contact.normal * deltaLambda; - Vector3 angularImpulseBody1 = -contact.r1CrossN * deltaLambda; - Vector3 linearImpulseBody2 = contact.normal * deltaLambda; - Vector3 angularImpulseBody2 = contact.r2CrossN * deltaLambda; - const Impulse impulsePenetration(linearImpulseBody1, angularImpulseBody1, - linearImpulseBody2, angularImpulseBody2); - - // Apply the impulse to the bodies of the constraint - applyImpulse(impulsePenetration, constraint); - - sumPenetrationImpulse += contact.penetrationImpulse; - - // If the split impulse position correction is active - if (mIsSplitImpulseActive) { - - // Split impulse (position correction) - const Vector3& v1Split = mSplitLinearVelocities[constraint.indexBody1]; - const Vector3& w1Split = mSplitAngularVelocities[constraint.indexBody1]; - const Vector3& v2Split = mSplitLinearVelocities[constraint.indexBody2]; - const Vector3& w2Split = mSplitAngularVelocities[constraint.indexBody2]; - Vector3 deltaVSplit = v2Split + w2Split.cross(contact.r2) - v1Split - w1Split.cross(contact.r1); - decimal JvSplit = deltaVSplit.dot(contact.normal); - decimal deltaLambdaSplit = - (JvSplit + biasPenetrationDepth) * contact.inversePenetrationMass; - decimal lambdaTempSplit = contact.penetrationSplitImpulse; - contact.penetrationSplitImpulse = std::max(contact.penetrationSplitImpulse + deltaLambdaSplit, 0.0f); - deltaLambda = contact.penetrationSplitImpulse - lambdaTempSplit; - - // Compute the impulse P=J^T * lambda - linearImpulseBody1 = -contact.normal * deltaLambdaSplit; - angularImpulseBody1 = -contact.r1CrossN * deltaLambdaSplit; - linearImpulseBody2 = contact.normal * deltaLambdaSplit; - angularImpulseBody2 = contact.r2CrossN * deltaLambdaSplit; - const Impulse splitImpulsePenetration(linearImpulseBody1, angularImpulseBody1, - linearImpulseBody2, angularImpulseBody2); - - applySplitImpulse(splitImpulsePenetration, constraint); - } - - // If we do not solve the friction constraints at the center of the contact manifold - if (!mIsSolveFrictionAtContactManifoldCenterActive) { - - // --------- Friction 1 --------- // - - // Compute J*v - deltaV = v2 + w2.cross(contact.r2) - v1 - w1.cross(contact.r1); - Jv = deltaV.dot(contact.frictionVector1); - - deltaLambda = -Jv; - deltaLambda *= contact.inverseFriction1Mass; - decimal frictionLimit = 0.3 * contact.penetrationImpulse; // TODO : Use constant here - lambdaTemp = contact.friction1Impulse; - contact.friction1Impulse = std::max(-frictionLimit, std::min(contact.friction1Impulse + deltaLambda, frictionLimit)); - deltaLambda = contact.friction1Impulse - lambdaTemp; - - // Compute the impulse P=J^T * lambda - linearImpulseBody1 = -contact.frictionVector1 * deltaLambda; - angularImpulseBody1 = -contact.r1CrossT1 * deltaLambda; - linearImpulseBody2 = contact.frictionVector1 * deltaLambda; - angularImpulseBody2 = contact.r2CrossT1 * deltaLambda; - const Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1, - linearImpulseBody2, angularImpulseBody2); - - // Apply the impulses to the bodies of the constraint - applyImpulse(impulseFriction1, constraint); - - // --------- Friction 2 --------- // - - // Compute J*v - deltaV = v2 + w2.cross(contact.r2) - v1 - w1.cross(contact.r1); - Jv = deltaV.dot(contact.frictionVector2); - - deltaLambda = -Jv; - deltaLambda *= contact.inverseFriction2Mass; - frictionLimit = 0.3 * contact.penetrationImpulse; // TODO : Use constant here - lambdaTemp = contact.friction2Impulse; - contact.friction2Impulse = std::max(-frictionLimit, std::min(contact.friction2Impulse + deltaLambda, frictionLimit)); - deltaLambda = contact.friction2Impulse - lambdaTemp; - - // Compute the impulse P=J^T * lambda - linearImpulseBody1 = -contact.frictionVector2 * deltaLambda; - angularImpulseBody1 = -contact.r1CrossT2 * deltaLambda; - linearImpulseBody2 = contact.frictionVector2 * deltaLambda; - angularImpulseBody2 = contact.r2CrossT2 * deltaLambda; - const Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1, - linearImpulseBody2, angularImpulseBody2); - - // Apply the impulses to the bodies of the constraint - applyImpulse(impulseFriction2, constraint); - } - } - - // If we solve the friction constraints at the center of the contact manifold - if (mIsSolveFrictionAtContactManifoldCenterActive) { - - // ------ First friction constraint at the center of the contact manifol ------ // - - // Compute J*v - Vector3 deltaV = v2 + w2.cross(constraint.r2Friction) - v1 - w1.cross(constraint.r1Friction); - decimal Jv = deltaV.dot(constraint.frictionVector1); - - decimal deltaLambda = -Jv * constraint.inverseFriction1Mass; - decimal frictionLimit = 0.3 * sumPenetrationImpulse; // TODO : Use constant here - lambdaTemp = constraint.friction1Impulse; - constraint.friction1Impulse = std::max(-frictionLimit, std::min(constraint.friction1Impulse + deltaLambda, frictionLimit)); - deltaLambda = constraint.friction1Impulse - lambdaTemp; - - // Compute the impulse P=J^T * lambda - Vector3 linearImpulseBody1 = -constraint.frictionVector1 * deltaLambda; - Vector3 angularImpulseBody1 = -constraint.r1CrossT1 * deltaLambda; - Vector3 linearImpulseBody2 = constraint.frictionVector1 * deltaLambda; - Vector3 angularImpulseBody2 = constraint.r2CrossT1 * deltaLambda; - const Impulse impulseFriction1(linearImpulseBody1, angularImpulseBody1, - linearImpulseBody2, angularImpulseBody2); - - // Apply the impulses to the bodies of the constraint - applyImpulse(impulseFriction1, constraint); - - // ------ Second friction constraint at the center of the contact manifol ----- // - - // Compute J*v - deltaV = v2 + w2.cross(constraint.r2Friction) - v1 - w1.cross(constraint.r1Friction); - Jv = deltaV.dot(constraint.frictionVector2); - - deltaLambda = -Jv * constraint.inverseFriction2Mass; - frictionLimit = 0.3 * sumPenetrationImpulse; // TODO : Use constant here - lambdaTemp = constraint.friction2Impulse; - constraint.friction2Impulse = std::max(-frictionLimit, std::min(constraint.friction2Impulse + deltaLambda, frictionLimit)); - deltaLambda = constraint.friction2Impulse - lambdaTemp; - - // Compute the impulse P=J^T * lambda - linearImpulseBody1 = -constraint.frictionVector2 * deltaLambda; - angularImpulseBody1 = -constraint.r1CrossT2 * deltaLambda; - linearImpulseBody2 = constraint.frictionVector2 * deltaLambda; - angularImpulseBody2 = constraint.r2CrossT2 * deltaLambda; - const Impulse impulseFriction2(linearImpulseBody1, angularImpulseBody1, - linearImpulseBody2, angularImpulseBody2); - - // Apply the impulses to the bodies of the constraint - applyImpulse(impulseFriction2, constraint); - - // ------ Twist friction constraint at the center of the contact manifol ------ // - - // TODO : Put this in the initialization method - decimal K = constraint.normal.dot(constraint.inverseInertiaTensorBody1 * constraint.normal) + - constraint.normal.dot(constraint.inverseInertiaTensorBody2 * constraint.normal); - - - // Compute J*v - deltaV = w2 - w1; - Jv = deltaV.dot(constraint.normal); - - // TODO : Compute the inverse mass matrix here for twist friction - deltaLambda = -Jv * (1.0 / K); - frictionLimit = 0.3 * sumPenetrationImpulse; // TODO : Use constant here - lambdaTemp = constraint.frictionTwistImpulse; - constraint.frictionTwistImpulse = std::max(-frictionLimit, std::min(constraint.frictionTwistImpulse + deltaLambda, frictionLimit)); - deltaLambda = constraint.frictionTwistImpulse - lambdaTemp; - - // Compute the impulse P=J^T * lambda - linearImpulseBody1 = Vector3(0.0, 0.0, 0.0); - angularImpulseBody1 = -constraint.normal * deltaLambda; - linearImpulseBody2 = Vector3(0.0, 0.0, 0.0);; - angularImpulseBody2 = constraint.normal * deltaLambda; - const Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1, - linearImpulseBody2, angularImpulseBody2); - - // Apply the impulses to the bodies of the constraint - applyImpulse(impulseTwistFriction, constraint); - } - } - } -} - -// Solve the constraints -void ConstraintSolver::solve(decimal timeStep) { - - mTimeStep = timeStep; - - // Initialize the solver - initialize(); - - initializeBodies(); - - // Fill-in all the matrices needed to solve the LCP problem - initializeContactConstraints(); - - // Warm start the solver - if (mIsWarmStartingActive) { - warmStart(); - } - - // Solve the contact constraints - solveContactConstraints(); - - // Cache the lambda values in order to use them in the next step - storeImpulses(); -} - -// Store the computed impulses to use them to -// warm start the solver at the next iteration -void ConstraintSolver::storeImpulses() { - - // For each constraint - for (uint c=0; csetCachedLambda(0, contact.penetrationImpulse); - contact.contact->setCachedLambda(1, contact.friction1Impulse); - contact.contact->setCachedLambda(2, contact.friction2Impulse); - - contact.contact->setFrictionVector1(contact.frictionVector1); - contact.contact->setFrictionVector2(contact.frictionVector2); - } - - constraint.contactManifold->friction1Impulse = constraint.friction1Impulse; - constraint.contactManifold->friction2Impulse = constraint.friction2Impulse; - constraint.contactManifold->frictionTwistImpulse = constraint.frictionTwistImpulse; - constraint.contactManifold->frictionVector1 = constraint.frictionVector1; - constraint.contactManifold->frictionVector2 = constraint.frictionVector2; - } -} - -// Apply an impulse to the two bodies of a constraint -void ConstraintSolver::applyImpulse(const Impulse& impulse, const ContactConstraint& constraint) { - - // Update the velocities of the bodies by applying the impulse P - if (constraint.isBody1Moving) { - mLinearVelocities[constraint.indexBody1] += constraint.massInverseBody1 * - impulse.linearImpulseBody1; - mAngularVelocities[constraint.indexBody1] += constraint.inverseInertiaTensorBody1 * - impulse.angularImpulseBody1; - } - if (constraint.isBody2Moving) { - mLinearVelocities[constraint.indexBody2] += constraint.massInverseBody2 * - impulse.linearImpulseBody2; - mAngularVelocities[constraint.indexBody2] += constraint.inverseInertiaTensorBody2 * - impulse.angularImpulseBody2; - } -} - -// Apply an impulse to the two bodies of a constraint -void ConstraintSolver::applySplitImpulse(const Impulse& impulse, const ContactConstraint& constraint) { - - // Update the velocities of the bodies by applying the impulse P - if (constraint.isBody1Moving) { - mSplitLinearVelocities[constraint.indexBody1] += constraint.massInverseBody1 * - impulse.linearImpulseBody1; - mSplitAngularVelocities[constraint.indexBody1] += constraint.inverseInertiaTensorBody1 * - impulse.angularImpulseBody1; - } - if (constraint.isBody2Moving) { - mSplitLinearVelocities[constraint.indexBody2] += constraint.massInverseBody2 * - impulse.linearImpulseBody2; - mSplitAngularVelocities[constraint.indexBody2] += constraint.inverseInertiaTensorBody2 * - impulse.angularImpulseBody2; - } -} - -// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane -// for a contact point constraint. The two vectors have to be such that : t1 x t2 = contactNormal. -void ConstraintSolver::computeFrictionVectors(const Vector3& deltaVelocity, - ContactPointConstraint& contact) const { - - // Update the old friction vectors - //contact.oldFrictionVector1 = contact.frictionVector1; - //contact.oldFrictionVector2 = contact.frictionVector2; - - assert(contact.normal.length() > 0.0); - - // Compute the velocity difference vector in the tangential plane - Vector3 normalVelocity = deltaVelocity.dot(contact.normal) * contact.normal; - Vector3 tangentVelocity = deltaVelocity - normalVelocity; - - // If the velocty difference in the tangential plane is not zero - decimal lengthTangenVelocity = tangentVelocity.length(); - if (lengthTangenVelocity > 0.0) { - - // Compute the first friction vector in the direction of the tangent - // velocity difference - contact.frictionVector1 = tangentVelocity / lengthTangenVelocity; - } - else { - - // Get any orthogonal vector to the normal as the first friction vector - contact.frictionVector1 = contact.normal.getOneUnitOrthogonalVector(); - } - - // The second friction vector is computed by the cross product of the firs - // friction vector and the contact normal - contact.frictionVector2 = contact.normal.cross(contact.frictionVector1).getUnit(); -} - -// Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane -// for a contact constraint. The two vectors have to be such that : t1 x t2 = contactNormal. -void ConstraintSolver::computeFrictionVectors(const Vector3& deltaVelocity, - ContactConstraint& contact) const { - - assert(contact.normal.length() > 0.0); - - // Compute the velocity difference vector in the tangential plane - Vector3 normalVelocity = deltaVelocity.dot(contact.normal) * contact.normal; - Vector3 tangentVelocity = deltaVelocity - normalVelocity; - - // If the velocty difference in the tangential plane is not zero - decimal lengthTangenVelocity = tangentVelocity.length(); - if (lengthTangenVelocity > 0.0) { - - // Compute the first friction vector in the direction of the tangent - // velocity difference - contact.frictionVector1 = tangentVelocity / lengthTangenVelocity; - } - else { - - // Get any orthogonal vector to the normal as the first friction vector - contact.frictionVector1 = contact.normal.getOneUnitOrthogonalVector(); - } - - // The second friction vector is computed by the cross product of the firs - // friction vector and the contact normal - contact.frictionVector2 = contact.normal.cross(contact.frictionVector1).getUnit(); -} diff --git a/src/engine/ConstraintSolver.h b/src/engine/ConstraintSolver.h deleted file mode 100644 index 0125dd5e..00000000 --- a/src/engine/ConstraintSolver.h +++ /dev/null @@ -1,424 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * -* Copyright (c) 2010-2012 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -#ifndef CONSTRAINT_SOLVER_H -#define CONSTRAINT_SOLVER_H - -// Libraries -#include "../constraint/Contact.h" -#include "ContactManifold.h" -#include "../configuration.h" -#include "../constraint/Constraint.h" -#include -#include - -// ReactPhysics3D namespace -namespace reactphysics3d { - -// Declarations -class DynamicsWorld; - -// Structure Impulse -struct Impulse { - - public: - const Vector3& linearImpulseBody1; - const Vector3& linearImpulseBody2; - const Vector3& angularImpulseBody1; - const Vector3& angularImpulseBody2; - - // Constructor - Impulse(const Vector3& linearImpulseBody1, const Vector3& angularImpulseBody1, - const Vector3& linearImpulseBody2, const Vector3& angularImpulseBody2) - : linearImpulseBody1(linearImpulseBody1), angularImpulseBody1(angularImpulseBody1), - linearImpulseBody2(linearImpulseBody2), angularImpulseBody2(angularImpulseBody2) { - - } -}; - -// Structure ContactPointConstraint -// Internal structure for a contact point constraint -struct ContactPointConstraint { - - decimal penetrationImpulse; // Accumulated normal impulse - decimal friction1Impulse; // Accumulated impulse in the 1st friction direction - decimal friction2Impulse; // Accumulated impulse in the 2nd friction direction - decimal penetrationSplitImpulse; // Accumulated split impulse for penetration correction - Vector3 normal; // Normal vector of the contact - Vector3 frictionVector1; // First friction vector in the tangent plane - Vector3 frictionVector2; // Second friction vector in the tangent plane - Vector3 oldFrictionVector1; // Old first friction vector in the tangent plane - Vector3 oldFrictionVector2; // Old second friction vector in the tangent plane - Vector3 r1; // Vector from the body 1 center to the contact point - Vector3 r2; // Vector from the body 2 center to the contact point - Vector3 r1CrossT1; // Cross product of r1 with 1st friction vector - Vector3 r1CrossT2; // Cross product of r1 with 2nd friction vector - Vector3 r2CrossT1; // Cross product of r2 with 1st friction vector - Vector3 r2CrossT2; // Cross product of r2 with 2nd friction vector - Vector3 r1CrossN; // Cross product of r1 with the contact normal - Vector3 r2CrossN; // Cross product of r2 with the contact normal - decimal penetrationDepth; // Penetration depth - decimal restitutionBias; // Velocity restitution bias - 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 - bool isRestingContact; // True if the contact was existing last time step - Contact* contact; // TODO : REMOVE THIS -}; - -// Structure ContactConstraint -struct ContactConstraint { - - // TODO : Use a constant for the number of contact points - - uint indexBody1; // Index of body 1 in the constraint solver - uint indexBody2; // Index of body 2 in the constraint solver - decimal massInverseBody1; // Inverse of the mass of body 1 - decimal massInverseBody2; // Inverse of the mass of body 2 - Matrix3x3 inverseInertiaTensorBody1; // Inverse inertia tensor of body 1 - Matrix3x3 inverseInertiaTensorBody2; // Inverse inertia tensor of body 2 - bool isBody1Moving; // True if the body 1 is allowed to move - bool isBody2Moving; // True if the body 2 is allowed to move - ContactPointConstraint contacts[4]; // Contact point constraints - uint nbContacts; // Number of contact points - decimal restitutionFactor; // Mix of the restitution factor for two bodies - ContactManifold* contactManifold; // Contact manifold - - // --- Variables used when friction constraints are apply at the center of the manifold --- // - - Vector3 normal; // Average normal vector of the contact manifold - Vector3 frictionPointBody1; // Point on body 1 where to apply the friction constraints - Vector3 frictionPointBody2; // Point on body 2 where to apply the friction constraints - Vector3 r1Friction; // R1 vector for the friction constraints - Vector3 r2Friction; // R2 vector for the friction constraints - Vector3 r1CrossT1; // Cross product of r1 with 1st friction vector - Vector3 r1CrossT2; // Cross product of r1 with 2nd friction vector - Vector3 r2CrossT1; // Cross product of r2 with 1st friction vector - Vector3 r2CrossT2; // Cross product of r2 with 2nd friction vector - decimal inverseFriction1Mass; // Matrix K for the first friction constraint - decimal inverseFriction2Mass; // Matrix K for the second friction constraint - Vector3 frictionVector1; // First friction direction at contact manifold center - Vector3 frictionVector2; // Second friction direction at contact manifold center - Vector3 oldFrictionVector1; // Old 1st friction direction at contact manifold center - Vector3 oldFrictionVector2; // Old 2nd friction direction at contact manifold center - decimal friction1Impulse; // First friction direction impulse at manifold center - decimal friction2Impulse; // Second friction direction impulse at manifold center - decimal frictionTwistImpulse; // Twist friction impulse at contact manifold center -}; - -/* ------------------------------------------------------------------- - Class ConstrainSolver : - This class represents the constraint solver that is used is solve constraints and - rigid bodies contacts. The constraint solver is based on the "Sequential Impulse" technique - described by Erin Catto in his GDC slides (http://code.google.com/p/box2d/downloads/list). - - A constraint between two bodies is represented by a function C(x) which is equal to zero - when the constraint is satisfied. The condition C(x)=0 describes a valid position and the - condition dC(x)/dt=0 describes a valid velocity. We have dC(x)/dt = Jv + b = 0 where J is - the Jacobian matrix of the constraint, v is a vector that contains the velocity of both - bodies and b is the constraint bias. We are looking for a force F_c that will act on the - bodies to keep the constraint satisfied. Note that from the virtual work principle, we have - F_c = J^t * lambda where J^t is the transpose of the Jacobian matrix and lambda is a - Lagrange multiplier. Therefore, finding the force F_c is equivalent to finding the Lagrange - multiplier lambda. - - An impulse P = F * dt where F is a force and dt is the timestep. We can apply impulses a - body to change its velocity. The idea of the Sequential Impulse technique is to apply - impulses to bodies of each constraints in order to keep the constraint satisfied. - - --- Step 1 --- - - First, we integrate the applied force F_a acting of each rigid body (like gravity, ...) and - we obtain some new velocities v2' that tends to violate the constraints. - - v2' = v1 + dt * M^-1 * F_a - - where M is a matrix that contains mass and inertia tensor information. - - --- Step 2 --- - - During the second step, we iterate over all the constraints for a certain number of - iterations and for each constraint we compute the impulse to apply to the bodies needed - so that the new velocity of the bodies satisfy Jv + b = 0. From the Newton law, we know that - M * deltaV = P_c where M is the mass of the body, deltaV is the difference of velocity and - P_c is the constraint impulse to apply to the body. Therefore, we have - v2 = v2' + M^-1 * P_c. For each constraint, we can compute the Lagrange multiplier lambda - using : lambda = -m_c (Jv2' + b) where m_c = 1 / (J * M^-1 * J^t). Now that we have the - Lagrange multiplier lambda, we can compute the impulse P_c = J^t * lambda * dt to apply to - the bodies to satisfy the constraint. - - --- Step 3 --- - - In the third step, we integrate the new position x2 of the bodies using the new velocities - v2 computed in the second step with : x2 = x1 + dt * v2. - - Note that in the following code (as it is also explained in the slides from Erin Catto), - the value lambda is not only the lagrange multiplier but is the multiplication of the - Lagrange multiplier with the timestep dt. Therefore, in the following code, when we use - lambda, we mean (lambda * dt). - - We are using the accumulated impulse technique that is also described in the slides from - Erin Catto. - - We are also using warm starting. The idea is to warm start the solver at the beginning of - each step by applying the last impulstes for the constraints that we already existing at the - previous step. This allows the iterative solver to converge faster towards the solution. - - For contact constraints, we are also using split impulses so that the position correction - that uses Baumgarte stabilization does not change the momentum of the bodies. - - There are two ways to apply the friction constraints. Either the friction constraints are - applied at each contact point or they are applied only at the center of the contact manifold - between two bodies. If we solve the friction constraints at each contact point, we need - two constraints (two tangential friction directions) and if we solve the friction constraints - at the center of the contact manifold, we need two constraints for tangential friction but - also another twist friction constraint to prevent spin of the body around the contact - manifold center. - - ------------------------------------------------------------------- -*/ -class ConstraintSolver { - - private: - - // -------------------- Constants --------------------- // - - // Beta value for the penetration depth position correction without split impulses - static const decimal BETA; - - // Beta value for the penetration depth position correction with split impulses - static const decimal BETA_SPLIT_IMPULSE; - - // Slop distance (allowed penetration distance between bodies) - static const decimal SLOP; - - // -------------------- Attributes -------------------- // - - 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 mNbIterations; // Number of iterations of the LCP solver - uint nbConstraints; // Total number of constraints (with the auxiliary constraints) - 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 - Vector3* mLinearVelocities; // Array of constrained linear velocities - Vector3* mAngularVelocities; // Array of constrained angular velocities - - // Split linear velocities for the position contact solver (split impulse) - Vector3* mSplitLinearVelocities; - - // Split angular velocities for the position contact solver (split impulse) - Vector3* mSplitAngularVelocities; - - decimal mTimeStep; // Current time step - - // Contact constraints - ContactConstraint* mContactConstraints; - - // Number of contact constraints - uint mNbContactConstraints; - - // Constrained bodies - std::set mConstraintBodies; - - // Map body to index - std::map mMapBodyToIndex; - - // True if the warm starting of the solver is active - bool mIsWarmStartingActive; - - // True if the split impulse position correction is active - bool mIsSplitImpulseActive; - - // True if we solve 3 friction constraints at the contact manifold center only - // instead of 2 friction constraints at each contact point - bool mIsSolveFrictionAtContactManifoldCenterActive; - - // -------------------- Methods -------------------- // - - // 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); - - // Apply an impulse to the two bodies of a constraint - void applySplitImpulse(const Impulse& impulse, const ContactConstraint& constraint); - - // Compute the collision restitution factor from the restitution factor of each body - decimal computeMixRestitutionFactor(const RigidBody *body1, const RigidBody *body2) const; - - // Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction - // plane for a contact point constraint. The two vectors have to be - // such that : t1 x t2 = contactNormal. - void computeFrictionVectors(const Vector3& deltaVelocity, - ContactPointConstraint& contact) const; - - // Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction - // plane for a contact constraint. The two vectors have to be - // such that : t1 x t2 = contactNormal. - void computeFrictionVectors(const Vector3& deltaVelocity, ContactConstraint& contact) const; - - public: - - // -------------------- 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 split linear velocity - Vector3 getSplitLinearVelocityOfBody(RigidBody* body); - - // Return the constrained angular velocity of a body after solving the constraints - Vector3 getConstrainedAngularVelocityOfBody(RigidBody* body); - - // Return the split angular velocity - Vector3 getSplitAngularVelocityOfBody(RigidBody* body); - - // Clean up the constraint solver - void cleanup(); - - // Set the number of iterations of the constraint solver - void setNbIterationsSolver(uint nbIterations); - - // Activate or Deactivate the split impulses for contacts - void setIsSplitImpulseActive(bool isActive); - - // Activate or deactivate the solving of friction constraints at the center of - // the contact manifold instead of solving them at each contact point - void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive); -}; - -// Return true if the body is in at least one constraint -inline bool ConstraintSolver::isConstrainedBody(RigidBody* body) const { - return mConstraintBodies.count(body) == 1; -} - -// Return the constrained linear velocity of a body after solving the constraints -inline Vector3 ConstraintSolver::getConstrainedLinearVelocityOfBody(RigidBody* body) { - assert(isConstrainedBody(body)); - uint indexBody = mMapBodyToIndex[body]; - return mLinearVelocities[indexBody]; -} - -// Return the split linear velocity -inline Vector3 ConstraintSolver::getSplitLinearVelocityOfBody(RigidBody* body) { - assert(isConstrainedBody(body)); - uint indexBody = mMapBodyToIndex[body]; - return mSplitLinearVelocities[indexBody]; -} - -// Return the constrained angular velocity of a body after solving the constraints -inline Vector3 ConstraintSolver::getConstrainedAngularVelocityOfBody(RigidBody *body) { - assert(isConstrainedBody(body)); - uint indexBody = mMapBodyToIndex[body]; - return mAngularVelocities[indexBody]; -} - -// Return the split angular velocity -inline Vector3 ConstraintSolver::getSplitAngularVelocityOfBody(RigidBody* body) { - assert(isConstrainedBody(body)); - uint indexBody = mMapBodyToIndex[body]; - return mSplitAngularVelocities[indexBody]; -} - -// Clean up the constraint solver -inline void ConstraintSolver::cleanup() { - mMapBodyToIndex.clear(); - mConstraintBodies.clear(); - activeConstraints.clear(); - - if (mContactConstraints != 0) { - delete[] mContactConstraints; - mContactConstraints = 0; - } - if (mLinearVelocities != 0) { - delete[] mLinearVelocities; - mLinearVelocities = 0; - } - if (mAngularVelocities != 0) { - delete[] mAngularVelocities; - mAngularVelocities = 0; - } -} - -// Set the number of iterations of the constraint solver -inline void ConstraintSolver::setNbIterationsSolver(uint nbIterations) { - mNbIterations = nbIterations; -} - -// Activate or Deactivate the split impulses for contacts -inline void ConstraintSolver::setIsSplitImpulseActive(bool isActive) { - mIsSplitImpulseActive = isActive; -} - -// Activate or deactivate the solving of friction constraints at the center of -// the contact manifold instead of solving them at each contact point -inline void ConstraintSolver::setIsSolveFrictionAtContactManifoldCenterActive(bool isActive) { - mIsSolveFrictionAtContactManifoldCenterActive = isActive; -} - -// Compute the collision restitution factor from the restitution factor of each body -inline decimal ConstraintSolver::computeMixRestitutionFactor(const RigidBody* body1, - const RigidBody* body2) const { - decimal restitution1 = body1->getRestitution(); - decimal restitution2 = body2->getRestitution(); - - // Return the largest restitution factor - return (restitution1 > restitution2) ? restitution1 : restitution2; -} - -} // End of ReactPhysics3D namespace - -#endif From 9e499be150154c83bc8e0844f05f4883744abab0 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Tue, 26 Feb 2013 08:15:58 +0100 Subject: [PATCH 25/26] Move the constrained velocities outside the contact solver --- src/engine/ContactSolver.cpp | 91 ++++++++++----------- src/engine/ContactSolver.h | 44 ++++------ src/engine/DynamicsWorld.cpp | 150 +++++++++++++++++++---------------- src/engine/DynamicsWorld.h | 27 ++++++- 4 files changed, 167 insertions(+), 145 deletions(-) diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp index 56329af2..e10bc29b 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/engine/ContactSolver.cpp @@ -37,10 +37,17 @@ const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2); const decimal ContactSolver::SLOP = decimal(0.01); // Constructor -ContactSolver::ContactSolver(DynamicsWorld& world) - :mWorld(world), mNbIterations(DEFAULT_CONSTRAINTS_SOLVER_NB_ITERATIONS), mContactConstraints(0), - mLinearVelocities(0), mAngularVelocities(0), mIsWarmStartingActive(true), - mIsSplitImpulseActive(true), mIsSolveFrictionAtContactManifoldCenterActive(true) { +ContactSolver::ContactSolver(DynamicsWorld& world, std::vector& constrainedLinearVelocities, + std::vector& constrainedAngularVelocities, + const std::map& mapBodyToVelocityIndex) + :mWorld(world), mNbIterations(DEFAULT_CONSTRAINTS_SOLVER_NB_ITERATIONS), + mSplitLinearVelocities(NULL), mSplitAngularVelocities(NULL), mContactConstraints(NULL), + mConstrainedLinearVelocities(constrainedLinearVelocities), + mConstrainedAngularVelocities(constrainedAngularVelocities), + mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex), + mIsWarmStartingActive(true), + mIsSplitImpulseActive(true), + mIsSolveFrictionAtContactManifoldCenterActive(true) { } @@ -72,10 +79,6 @@ void ContactSolver::initialize() { RigidBody* body1 = externalContactManifold.contacts[0]->getBody1(); RigidBody* body2 = externalContactManifold.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); @@ -84,8 +87,8 @@ void ContactSolver::initialize() { Vector3 x1 = body1->getTransform().getPosition(); Vector3 x2 = body2->getTransform().getPosition(); - internalContactManifold.indexBody1 = mMapBodyToIndex[body1]; - internalContactManifold.indexBody2 = mMapBodyToIndex[body2]; + internalContactManifold.indexBody1 = mMapBodyToConstrainedVelocityIndex.find(body1)->second; + internalContactManifold.indexBody2 = mMapBodyToConstrainedVelocityIndex.find(body2)->second; internalContactManifold.inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld(); internalContactManifold.inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld(); internalContactManifold.isBody1Moving = body1->getIsMotionEnabled(); @@ -165,17 +168,17 @@ void ContactSolver::initialize() { mNbContactConstraints++; } - // Compute the number of bodies that are part of some active constraint - uint nbBodies = mConstraintBodies.size(); - - // Allocated memory for velocities + // Allocated memory for split impulse velocities // TODO : Use better memory allocation here - mLinearVelocities = new Vector3[nbBodies]; - mAngularVelocities = new Vector3[nbBodies]; - mSplitLinearVelocities = new Vector3[nbBodies]; - mSplitAngularVelocities = new Vector3[nbBodies]; + mSplitLinearVelocities = new Vector3[mWorld.getNbRigidBodies()]; + mSplitAngularVelocities = new Vector3[mWorld.getNbRigidBodies()]; + assert(mSplitLinearVelocities != NULL); + assert(mSplitAngularVelocities != NULL); - assert(mMapBodyToIndex.size() == nbBodies); + assert(mConstraintBodies.size() > 0); + assert(mMapBodyToConstrainedVelocityIndex.size() >= mConstraintBodies.size()); + assert(mConstrainedLinearVelocities.size() >= mConstraintBodies.size()); + assert(mConstrainedAngularVelocities.size() >= mConstraintBodies.size()); } // Initialize the constrained bodies @@ -187,10 +190,8 @@ void ContactSolver::initializeBodies() { RigidBody* rigidBody = *it; assert(rigidBody); - uint bodyNumber = mMapBodyToIndex[rigidBody]; + uint bodyNumber = mMapBodyToConstrainedVelocityIndex.find(rigidBody)->second; - mLinearVelocities[bodyNumber] = rigidBody->getLinearVelocity() + mTimeStep * rigidBody->getMassInverse() * rigidBody->getExternalForce(); - mAngularVelocities[bodyNumber] = rigidBody->getAngularVelocity() + mTimeStep * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque(); mSplitLinearVelocities[bodyNumber] = Vector3(0, 0, 0); mSplitAngularVelocities[bodyNumber] = Vector3(0, 0, 0); } @@ -214,10 +215,10 @@ void ContactSolver::initializeContactConstraints() { } // Get the velocities of the bodies - const Vector3& v1 = mLinearVelocities[contactManifold.indexBody1]; - const Vector3& w1 = mAngularVelocities[contactManifold.indexBody1]; - const Vector3& v2 = mLinearVelocities[contactManifold.indexBody2]; - const Vector3& w2 = mAngularVelocities[contactManifold.indexBody2]; + const Vector3& v1 = mConstrainedLinearVelocities[contactManifold.indexBody1]; + const Vector3& w1 = mConstrainedAngularVelocities[contactManifold.indexBody1]; + const Vector3& v2 = mConstrainedLinearVelocities[contactManifold.indexBody2]; + const Vector3& w2 = mConstrainedAngularVelocities[contactManifold.indexBody2]; // For each contact point constraint for (uint i=0; i 0.0); // Compute the velocity difference vector in the tangential plane @@ -819,19 +816,19 @@ void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity, // Clean up the constraint solver void ContactSolver::cleanup() { - mMapBodyToIndex.clear(); + //mMapBodyToIndex.clear(); mConstraintBodies.clear(); - if (mContactConstraints != 0) { + if (mContactConstraints != NULL) { delete[] mContactConstraints; - mContactConstraints = 0; + mContactConstraints = NULL; } - if (mLinearVelocities != 0) { - delete[] mLinearVelocities; - mLinearVelocities = 0; + if (mSplitLinearVelocities != NULL) { + delete[] mSplitLinearVelocities; + mSplitLinearVelocities = NULL; } - if (mAngularVelocities != 0) { - delete[] mAngularVelocities; - mAngularVelocities = 0; + if (mSplitAngularVelocities != NULL) { + delete[] mSplitAngularVelocities; + mSplitAngularVelocities = NULL; } } diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h index 6b1d9c61..bae7a9bb 100644 --- a/src/engine/ContactSolver.h +++ b/src/engine/ContactSolver.h @@ -62,7 +62,7 @@ struct Impulse { /* ------------------------------------------------------------------- Class ContactSolver : - This class represents the contacts solver that is used is solve rigid bodies contacts. + This class represents the contact solver that is used is solve rigid bodies contacts. The constraint solver is based on the "Sequential Impulse" technique described by Erin Catto in his GDC slides (http://code.google.com/p/box2d/downloads/list). @@ -228,12 +228,6 @@ class ContactSolver { // Number of iterations of the constraints solver uint mNbIterations; - // Array of constrained linear velocities - Vector3* mLinearVelocities; - - // Array of constrained angular velocities - Vector3* mAngularVelocities; - // Split linear velocities for the position contact solver (split impulse) Vector3* mSplitLinearVelocities; @@ -252,8 +246,16 @@ class ContactSolver { // Constrained bodies std::set mConstraintBodies; - // Map body to index - std::map mMapBodyToIndex; + // Pointer to the array of constrained linear velocities (state of the linear velocities + // after solving the constraints) + std::vector& mConstrainedLinearVelocities; + + // Pointer to the array of constrained angular velocities (state of the angular velocities + // after solving the constraints) + std::vector& mConstrainedAngularVelocities; + + // Reference to the map of rigid body to their index in the constrained velocities array + const std::map& mMapBodyToConstrainedVelocityIndex; // True if the warm starting of the solver is active bool mIsWarmStartingActive; @@ -327,7 +329,9 @@ class ContactSolver { // -------------------- Methods -------------------- // // Constructor - ContactSolver(DynamicsWorld& mWorld); + ContactSolver(DynamicsWorld& mWorld, std::vector& constrainedLinearVelocities, + std::vector& constrainedAngularVelocities, const std::map& + mapBodyToVelocityIndex); // Destructor virtual ~ContactSolver(); @@ -369,31 +373,17 @@ inline bool ContactSolver::isConstrainedBody(RigidBody* body) const { return mConstraintBodies.count(body) == 1; } -// Return the constrained linear velocity of a body after solving the constraints -inline Vector3 ContactSolver::getConstrainedLinearVelocityOfBody(RigidBody* body) { - assert(isConstrainedBody(body)); - uint indexBody = mMapBodyToIndex[body]; - return mLinearVelocities[indexBody]; -} - // Return the split linear velocity inline Vector3 ContactSolver::getSplitLinearVelocityOfBody(RigidBody* body) { assert(isConstrainedBody(body)); - uint indexBody = mMapBodyToIndex[body]; + const uint indexBody = mMapBodyToConstrainedVelocityIndex.find(body)->second; return mSplitLinearVelocities[indexBody]; } -// Return the constrained angular velocity of a body after solving the constraints -inline Vector3 ContactSolver::getConstrainedAngularVelocityOfBody(RigidBody *body) { - assert(isConstrainedBody(body)); - uint indexBody = mMapBodyToIndex[body]; - return mAngularVelocities[indexBody]; -} - // Return the split angular velocity inline Vector3 ContactSolver::getSplitAngularVelocityOfBody(RigidBody* body) { assert(isConstrainedBody(body)); - uint indexBody = mMapBodyToIndex[body]; + const uint indexBody = mMapBodyToConstrainedVelocityIndex.find(body)->second; return mSplitAngularVelocities[indexBody]; } @@ -454,6 +444,6 @@ inline const Impulse ContactSolver::computeFriction2Impulse(decimal deltaLambda, contactPoint.frictionVector2 * deltaLambda, contactPoint.r2CrossT2 * deltaLambda); } -} // End of ReactPhysics3D namespace +} #endif diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index f638bd8d..d1ad2f96 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -26,13 +26,15 @@ // Libraries #include "DynamicsWorld.h" -// We want to use the ReactPhysics3D namespace +// Namespaces using namespace reactphysics3d; using namespace std; // Constructor DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_TIMESTEP) - : CollisionWorld(), mTimer(timeStep), mGravity(gravity), mIsGravityOn(true), mContactSolver(*this), + : CollisionWorld(), mTimer(timeStep), mGravity(gravity), mIsGravityOn(true), + mContactSolver(*this, mConstrainedLinearVelocities, mConstrainedAngularVelocities, + mMapBodyToConstrainedVelocityIndex), mIsDeactivationActive(DEACTIVATION_ENABLED) { } @@ -46,6 +48,9 @@ DynamicsWorld::~DynamicsWorld() { (*it).second->OverlappingPair::~OverlappingPair(); mMemoryPoolOverlappingPairs.freeObject((*it).second); } + + // Free the allocated memory for the constrained velocities + cleanupConstrainedVelocitiesArray(); } // Update the physics simulation @@ -68,6 +73,9 @@ void DynamicsWorld::update() { // Compute the collision detection mCollisionDetection.computeCollisionDetection(); + // Initialize the constrained velocities + initConstrainedVelocitiesArray(); + // If there are contacts if (!mContactManifolds.empty()) { @@ -82,60 +90,61 @@ void DynamicsWorld::update() { resetBodiesMovementVariable(); // Update the position and orientation of each body - updateAllBodiesMotion(); + updateRigidBodiesPositionAndOrientation(); // Cleanup of the contact solver mContactSolver.cleanup(); + + // Cleanup the constrained velocities + cleanupConstrainedVelocitiesArray(); } // Compute and set the interpolation factor to all the bodies setInterpolationFactorToAllBodies(); } -// Compute the motion of all bodies and update their positions and orientations -// First this method compute the vector V2 = V_constraint + V_forces + V1 where -// V_constraint = dt * (M^-1 * J^T * lambda) and V_forces = dt * (M^-1 * F_ext) -// V2 is the final velocity after the timestep and V1 is the velocity before the -// timestep. -// After having computed the velocity V2, this method will update the position -// and orientation of each body. -// This method uses the semi-implicit Euler method to update the position and -// orientation of the body -void DynamicsWorld::updateAllBodiesMotion() { +// Update the position and orientation of the rigid bodies +void DynamicsWorld::updateRigidBodiesPositionAndOrientation() { decimal dt = mTimer.getTimeStep(); - Vector3 newLinearVelocity; - Vector3 newAngularVelocity; - // For each body of thephysics world + // For each rigid body of the world for (set::iterator it=getRigidBodiesBeginIterator(); it != getRigidBodiesEndIterator(); ++it) { RigidBody* rigidBody = *it; assert(rigidBody); - // If the body is able to move + // If the body is allowed to move if (rigidBody->getIsMotionEnabled()) { - newLinearVelocity.setAllValues(0.0, 0.0, 0.0); - newAngularVelocity.setAllValues(0.0, 0.0, 0.0); - // If it's a constrained body - if (mContactSolver.isConstrainedBody(*it)) { - // Get the constrained linear and angular velocities from the constraint solver - newLinearVelocity = mContactSolver.getConstrainedLinearVelocityOfBody(rigidBody); - newAngularVelocity = mContactSolver.getConstrainedAngularVelocityOfBody(rigidBody); - } - else { - // Compute V_forces = dt * (M^-1 * F_ext) which is the velocity of the body due to the - // external forces and torques. - newLinearVelocity += dt * rigidBody->getMassInverse() * rigidBody->getExternalForce(); - newAngularVelocity += dt * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque(); + // Update the old Transform of the body + rigidBody->updateOldTransform(); - // Add the velocity V1 to the new velocity - newLinearVelocity += rigidBody->getLinearVelocity(); - newAngularVelocity += rigidBody->getAngularVelocity(); + // Get the constrained velocity + uint indexArray = mMapBodyToConstrainedVelocityIndex[rigidBody]; + Vector3 newLinVelocity = mConstrainedLinearVelocities[indexArray]; + Vector3 newAngVelocity = mConstrainedAngularVelocities[indexArray]; + + // Update the linear and angular velocity of the body + rigidBody->setLinearVelocity(newLinVelocity); + rigidBody->setAngularVelocity(newAngVelocity); + + // Add the split impulse velocity from Contact Solver (only used to update the position) + if (mContactSolver.isConstrainedBody(rigidBody)) { + newLinVelocity += mContactSolver.getSplitLinearVelocityOfBody(rigidBody); + newAngVelocity += mContactSolver.getSplitAngularVelocityOfBody(rigidBody); } - - // Update the position and the orientation of the body according to the new velocity - updatePositionAndOrientationOfBody(*it, newLinearVelocity, newAngularVelocity); + + // Get current position and orientation of the body + const Vector3& currentPosition = rigidBody->getTransform().getPosition(); + const Quaternion& currentOrientation = rigidBody->getTransform().getOrientation(); + + // Compute the new position of the body + Vector3 newPosition = currentPosition + newLinVelocity * dt; + Quaternion newOrientation = currentOrientation + Quaternion(newAngVelocity.getX(), newAngVelocity.getY(), newAngVelocity.getZ(), 0) * currentOrientation * 0.5 * dt; + + // Update the Transform of the body + Transform newTransform(newPosition, newOrientation.getUnit()); + rigidBody->setTransform(newTransform); // Update the AABB of the rigid body rigidBody->updateAABB(); @@ -143,40 +152,6 @@ void DynamicsWorld::updateAllBodiesMotion() { } } -// Update the position and orientation of a body -// Use the Semi-Implicit Euler (Sympletic Euler) method to compute the new position and the new -// orientation of the body -void DynamicsWorld::updatePositionAndOrientationOfBody(RigidBody* rigidBody, - Vector3 newLinVelocity, - Vector3 newAngVelocity) { - decimal dt = mTimer.getTimeStep(); - - assert(rigidBody); - - // Update the old position and orientation of the body - rigidBody->updateOldTransform(); - - // Update the linear and angular velocity of the body - rigidBody->setLinearVelocity(newLinVelocity); - rigidBody->setAngularVelocity(newAngVelocity); - - // Split velocity (only used to update the position) - if (mContactSolver.isConstrainedBody(rigidBody)) { - newLinVelocity += mContactSolver.getSplitLinearVelocityOfBody(rigidBody); - newAngVelocity += mContactSolver.getSplitAngularVelocityOfBody(rigidBody); - } - - // Get current position and orientation of the body - const Vector3& currentPosition = rigidBody->getTransform().getPosition(); - const Quaternion& currentOrientation = rigidBody->getTransform().getOrientation(); - - Vector3 newPosition = currentPosition + newLinVelocity * dt; - Quaternion newOrientation = currentOrientation + Quaternion(newAngVelocity.getX(), newAngVelocity.getY(), newAngVelocity.getZ(), 0) * currentOrientation * 0.5 * dt; - - Transform newTransform(newPosition, newOrientation.getUnit()); - rigidBody->setTransform(newTransform); -} - // Compute and set the interpolation factor to all bodies void DynamicsWorld::setInterpolationFactorToAllBodies() { @@ -194,6 +169,41 @@ void DynamicsWorld::setInterpolationFactorToAllBodies() { } } +// Initialize the constrained velocities array at each step +void DynamicsWorld::initConstrainedVelocitiesArray() { + + // TODO : Use better memory allocation here + mConstrainedLinearVelocities = std::vector(mRigidBodies.size(), Vector3(0, 0, 0)); + mConstrainedAngularVelocities = std::vector(mRigidBodies.size(), Vector3(0, 0, 0)); + + double dt = mTimer.getTimeStep(); + + // Fill in the mapping of rigid body to their index in the constrained + // velocities arrays + uint i = 0; + for (std::set::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) { + RigidBody* rigidBody = *it; + mMapBodyToConstrainedVelocityIndex.insert(std::make_pair(rigidBody, i)); + + // TODO : Move it somewhere else + mConstrainedLinearVelocities[i] = rigidBody->getLinearVelocity() + dt * rigidBody->getMassInverse() *rigidBody->getExternalForce(); + mConstrainedAngularVelocities[i] = rigidBody->getAngularVelocity() + dt * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque(); + + i++; + } +} + +// Cleanup the constrained velocities array at each step +void DynamicsWorld::cleanupConstrainedVelocitiesArray() { + + // Clear the constrained velocites + mConstrainedLinearVelocities.clear(); + mConstrainedAngularVelocities.clear(); + + // Clear the rigid body to velocities array index mapping + mMapBodyToConstrainedVelocityIndex.clear(); +} + // Apply the gravity force to all bodies of the physics world void DynamicsWorld::applyGravity() { diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index 51f3b6b1..b2254fca 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -84,6 +84,17 @@ class DynamicsWorld : public CollisionWorld { // Memory pool for the contacts MemoryPool mMemoryPoolContacts; + // Array of constrained linear velocities (state of the linear velocities + // after solving the constraints) + std::vector mConstrainedLinearVelocities; + + // Array of constrained angular velocities (state of the angular velocities + // after solving the constraints) + std::vector mConstrainedAngularVelocities; + + // Map body to their index in the constrained velocities array + std::map mMapBodyToConstrainedVelocityIndex; + // -------------------- Methods -------------------- // // Private copy-constructor @@ -93,7 +104,7 @@ class DynamicsWorld : public CollisionWorld { DynamicsWorld& operator=(const DynamicsWorld& world); // Compute the motion of all bodies and update their positions and orientations - void updateAllBodiesMotion(); + void updateRigidBodiesPositionAndOrientation(); // Update the position and orientation of a body void updatePositionAndOrientationOfBody(RigidBody* body, Vector3 newLinVelocity, @@ -102,6 +113,12 @@ class DynamicsWorld : public CollisionWorld { // Compute and set the interpolation factor to all bodies void setInterpolationFactorToAllBodies(); + // Initialize the constrained velocities array at each step + void initConstrainedVelocitiesArray(); + + // Cleanup the constrained velocities array at each step + void cleanupConstrainedVelocitiesArray(); + // Apply the gravity force to all bodies void applyGravity(); @@ -169,6 +186,9 @@ public : // Set the isGravityOn attribute void setIsGratityOn(bool isGravityOn); + // Return the number of rigid bodies in the world + uint getNbRigidBodies() const; + // Add a constraint void addConstraint(Constraint* constraint); @@ -286,6 +306,11 @@ inline void DynamicsWorld::setIsGratityOn(bool isGravityOn) { mIsGravityOn = isGravityOn; } +// Return the number of rigid bodies in the world +inline uint DynamicsWorld::getNbRigidBodies() const { + return mRigidBodies.size(); +} + // Return an iterator to the beginning of the bodies of the physics world inline std::set::iterator DynamicsWorld::getRigidBodiesBeginIterator() { return mRigidBodies.begin(); From a362171532499f6512c25e564254ee9a3677b432 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Tue, 26 Feb 2013 22:43:45 +0100 Subject: [PATCH 26/26] Clean the code of the sequential impulse contact solver --- .../narrowphase/GJK/GJKAlgorithm.cpp | 2 +- src/configuration.h | 8 +- .../{Contact.cpp => ContactPoint.cpp} | 22 +- src/constraint/{Contact.h => ContactPoint.h} | 50 +- src/engine/CollisionWorld.cpp | 2 +- src/engine/CollisionWorld.h | 2 +- ...ntContactCache.cpp => ContactManifold.cpp} | 153 +++--- src/engine/ContactManifold.h | 207 +++++++- src/engine/ContactSolver.cpp | 471 ++++++++++-------- src/engine/ContactSolver.h | 62 +-- src/engine/DynamicsWorld.cpp | 7 +- src/engine/DynamicsWorld.h | 13 +- src/engine/OverlappingPair.cpp | 6 +- src/engine/OverlappingPair.h | 37 +- src/engine/PersistentContactCache.h | 147 ------ 15 files changed, 661 insertions(+), 528 deletions(-) rename src/constraint/{Contact.cpp => ContactPoint.cpp} (74%) rename src/constraint/{Contact.h => ContactPoint.h} (82%) rename src/engine/{PersistentContactCache.cpp => ContactManifold.cpp} (55%) delete mode 100644 src/engine/PersistentContactCache.h diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp index 0dcb4e48..33779795 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp @@ -26,7 +26,7 @@ // Libraries #include "GJKAlgorithm.h" #include "Simplex.h" -#include "../../../constraint/Contact.h" +#include "../../../constraint/ContactPoint.h" #include "../../../configuration.h" #include #include diff --git a/src/configuration.h b/src/configuration.h index 935ef6ff..e570f2d5 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -79,15 +79,11 @@ const decimal OBJECT_MARGIN = decimal(0.04); // Distance threshold for two contact points for a valid persistent contact (in meters) const decimal PERSISTENT_CONTACT_DIST_THRESHOLD = decimal(0.03); +const decimal RESTITUTION_VELOCITY_THRESHOLD = decimal(1.0); + // Number of iterations when solving a LCP problem const uint DEFAULT_CONSTRAINTS_SOLVER_NB_ITERATIONS = 15; -// Number of iterations when solving a LCP problem for error correction -const uint DEFAULT_LCP_ITERATIONS_ERROR_CORRECTION = 5; - -// True if the error correction projection (first order world) is active in the constraint solver -const bool ERROR_CORRECTION_PROJECTION_ENABLED = true; - } #endif diff --git a/src/constraint/Contact.cpp b/src/constraint/ContactPoint.cpp similarity index 74% rename from src/constraint/Contact.cpp rename to src/constraint/ContactPoint.cpp index 61403dfc..b5b87dcf 100644 --- a/src/constraint/Contact.cpp +++ b/src/constraint/ContactPoint.cpp @@ -24,25 +24,27 @@ ********************************************************************************/ // Libraries -#include "Contact.h" +#include "ContactPoint.h" using namespace reactphysics3d; using namespace std; // Constructor -Contact::Contact(RigidBody* const body1, RigidBody* const body2, const ContactInfo* contactInfo) - : Constraint(body1, body2, 3, true, CONTACT), mNormal(contactInfo->normal), - mPenetrationDepth(contactInfo->penetrationDepth), - mLocalPointOnBody1(contactInfo->localPoint1), - mLocalPointOnBody2(contactInfo->localPoint2), - mWorldPointOnBody1(body1->getTransform() * contactInfo->localPoint1), - mWorldPointOnBody2(body2->getTransform() * contactInfo->localPoint2), - mIsRestingContact(false), mFrictionVectors(2, Vector3(0, 0, 0)) { +ContactPoint::ContactPoint(RigidBody* const body1, RigidBody* const body2, + const ContactInfo* contactInfo) + : Constraint(body1, body2, 3, true, CONTACT), mNormal(contactInfo->normal), + mPenetrationDepth(contactInfo->penetrationDepth), + mLocalPointOnBody1(contactInfo->localPoint1), + mLocalPointOnBody2(contactInfo->localPoint2), + mWorldPointOnBody1(body1->getTransform() * contactInfo->localPoint1), + mWorldPointOnBody2(body2->getTransform() * contactInfo->localPoint2), + mIsRestingContact(false), mFrictionVectors(2, Vector3(0, 0, 0)) { + assert(mPenetrationDepth > 0.0); } // Destructor -Contact::~Contact() { +ContactPoint::~ContactPoint() { } diff --git a/src/constraint/Contact.h b/src/constraint/ContactPoint.h similarity index 82% rename from src/constraint/Contact.h rename to src/constraint/ContactPoint.h index d18ef023..38379d2c 100644 --- a/src/constraint/Contact.h +++ b/src/constraint/ContactPoint.h @@ -23,8 +23,8 @@ * * ********************************************************************************/ -#ifndef CONTACT_H -#define CONTACT_H +#ifndef CONTACT_POINT_H +#define CONTACT_POINT_H // Libraries #include "Constraint.h" @@ -52,13 +52,13 @@ namespace reactphysics3d { /* ------------------------------------------------------------------- - Class Contact : + Class ContactPoint : This class represents a collision contact point between two - bodies in the physics engine. The contact class inherits from + bodies in the physics engine. The ContactPoint class inherits from the Constraint class. ------------------------------------------------------------------- */ -class Contact : public Constraint { +class ContactPoint : public Constraint { protected : @@ -91,20 +91,20 @@ class Contact : public Constraint { // -------------------- Methods -------------------- // // Private copy-constructor - Contact(const Contact& contact); + ContactPoint(const ContactPoint& contact); // Private assignment operator - Contact& operator=(const Contact& contact); + ContactPoint& operator=(const ContactPoint& contact); public : // -------------------- Methods -------------------- // // Constructor - Contact(RigidBody* const body1, RigidBody* const body2, const ContactInfo* contactInfo); + ContactPoint(RigidBody* const body1, RigidBody* const body2, const ContactInfo* contactInfo); // Destructor - virtual ~Contact(); + virtual ~ContactPoint(); // Return the normal vector of the contact Vector3 getNormal() const; @@ -158,83 +158,83 @@ class Contact : public Constraint { }; // Return the normal vector of the contact -inline Vector3 Contact::getNormal() const { +inline Vector3 ContactPoint::getNormal() const { return mNormal; } // Set the penetration depth of the contact -inline void Contact::setPenetrationDepth(decimal penetrationDepth) { +inline void ContactPoint::setPenetrationDepth(decimal penetrationDepth) { this->mPenetrationDepth = penetrationDepth; } // Return the contact point on body 1 -inline Vector3 Contact::getLocalPointOnBody1() const { +inline Vector3 ContactPoint::getLocalPointOnBody1() const { return mLocalPointOnBody1; } // Return the contact point on body 2 -inline Vector3 Contact::getLocalPointOnBody2() const { +inline Vector3 ContactPoint::getLocalPointOnBody2() const { return mLocalPointOnBody2; } // Return the contact world point on body 1 -inline Vector3 Contact::getWorldPointOnBody1() const { +inline Vector3 ContactPoint::getWorldPointOnBody1() const { return mWorldPointOnBody1; } // Return the contact world point on body 2 -inline Vector3 Contact::getWorldPointOnBody2() const { +inline Vector3 ContactPoint::getWorldPointOnBody2() const { return mWorldPointOnBody2; } // Set the contact world point on body 1 -inline void Contact::setWorldPointOnBody1(const Vector3& worldPoint) { +inline void ContactPoint::setWorldPointOnBody1(const Vector3& worldPoint) { mWorldPointOnBody1 = worldPoint; } // Set the contact world point on body 2 -inline void Contact::setWorldPointOnBody2(const Vector3& worldPoint) { +inline void ContactPoint::setWorldPointOnBody2(const Vector3& worldPoint) { mWorldPointOnBody2 = worldPoint; } // Return true if the contact is a resting contact -inline bool Contact::getIsRestingContact() const { +inline bool ContactPoint::getIsRestingContact() const { return mIsRestingContact; } // Set the mIsRestingContact variable -inline void Contact::setIsRestingContact(bool isRestingContact) { +inline void ContactPoint::setIsRestingContact(bool isRestingContact) { mIsRestingContact = isRestingContact; } // Get the first friction vector -inline Vector3 Contact::getFrictionVector1() const { +inline Vector3 ContactPoint::getFrictionVector1() const { return mFrictionVectors[0]; } // Set the first friction vector -inline void Contact::setFrictionVector1(const Vector3& frictionVector1) { +inline void ContactPoint::setFrictionVector1(const Vector3& frictionVector1) { mFrictionVectors[0] = frictionVector1; } // Get the second friction vector -inline Vector3 Contact::getFrictionVector2() const { +inline Vector3 ContactPoint::getFrictionVector2() const { return mFrictionVectors[1]; } // Set the second friction vector -inline void Contact::setFrictionVector2(const Vector3& frictionVector2) { +inline void ContactPoint::setFrictionVector2(const Vector3& frictionVector2) { mFrictionVectors[1] = frictionVector2; } // Return the penetration depth of the contact -inline decimal Contact::getPenetrationDepth() const { +inline decimal ContactPoint::getPenetrationDepth() const { return mPenetrationDepth; } #ifdef VISUAL_DEBUG -inline void Contact::draw() const { +inline void ContactPoint::draw() const { glColor3f(1.0, 0.0, 0.0); glutSolidSphere(0.3, 20, 20); } diff --git a/src/engine/CollisionWorld.cpp b/src/engine/CollisionWorld.cpp index 697103a4..52203b11 100644 --- a/src/engine/CollisionWorld.cpp +++ b/src/engine/CollisionWorld.cpp @@ -101,7 +101,7 @@ void CollisionWorld::destroyCollisionBody(CollisionBody* collisionBody) { collisionBody->CollisionBody::~CollisionBody(); // Remove the collision body from the list of bodies - mBodies.erase(collisionBody); // TOOD : Maybe use a set to make this faster + mBodies.erase(collisionBody); // TODO : Maybe use a set to make this faster // Free the object from the memory pool mMemoryPoolCollisionBodies.freeObject(collisionBody); diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h index 5d9d01d9..8d105882 100644 --- a/src/engine/CollisionWorld.h +++ b/src/engine/CollisionWorld.h @@ -35,7 +35,7 @@ #include "OverlappingPair.h" #include "../collision/CollisionDetection.h" #include "../constraint/Constraint.h" -#include "../constraint/Contact.h" +#include "../constraint/ContactPoint.h" #include "../memory/MemoryPool.h" // Namespace reactphysics3d diff --git a/src/engine/PersistentContactCache.cpp b/src/engine/ContactManifold.cpp similarity index 55% rename from src/engine/PersistentContactCache.cpp rename to src/engine/ContactManifold.cpp index 49b7490d..389a59d6 100644 --- a/src/engine/PersistentContactCache.cpp +++ b/src/engine/ContactManifold.cpp @@ -24,34 +24,37 @@ ********************************************************************************/ // Libraries -#include "PersistentContactCache.h" +#include "ContactManifold.h" using namespace reactphysics3d; // Constructor -PersistentContactCache::PersistentContactCache(Body* const body1, Body* const body2, MemoryPool& memoryPoolContacts) - : mBody1(body1), mBody2(body2), mNbContacts(0), mMemoryPoolContacts(memoryPoolContacts) { +ContactManifold::ContactManifold(Body* const body1, Body* const body2, + MemoryPool& memoryPoolContacts) + : mBody1(body1), mBody2(body2), mNbContactPoints(0), mFrictionImpulse1(0.0), + mFrictionImpulse2(0.0), mFrictionTwistImpulse(0.0), + mMemoryPoolContacts(memoryPoolContacts) { } // Destructor -PersistentContactCache::~PersistentContactCache() { +ContactManifold::~ContactManifold() { clear(); } -// Add a contact in the cache -void PersistentContactCache::addContact(Contact* contact) { +// Add a contact point in the manifold +void ContactManifold::addContactPoint(ContactPoint* contact) { - // For contact already in the cache - for (uint i=0; igetWorldPointOnBody1() - contact->getWorldPointOnBody1()).lengthSquare(); + // already in the manifold. + decimal distance = (mContactPoints[i]->getWorldPointOnBody1() - contact->getWorldPointOnBody1()).lengthSquare(); if (distance <= PERSISTENT_CONTACT_DIST_THRESHOLD*PERSISTENT_CONTACT_DIST_THRESHOLD) { // Delete the new contact - contact->Contact::~Contact(); + contact->ContactPoint::~ContactPoint(); mMemoryPoolContacts.freeObject(contact); //removeContact(i); @@ -60,93 +63,95 @@ void PersistentContactCache::addContact(Contact* contact) { } } - // If the contact cache is full - if (mNbContacts == MAX_CONTACTS_IN_CACHE) { + // If the contact manifold is full + if (mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD) { int indexMaxPenetration = getIndexOfDeepestPenetration(contact); int indexToRemove = getIndexToRemove(indexMaxPenetration, contact->getLocalPointOnBody1()); - removeContact(indexToRemove); + removeContactPoint(indexToRemove); } - // Add the new contact in the cache - mContacts[mNbContacts] = contact; - mNbContacts++; + // Add the new contact point in the manifold + mContactPoints[mNbContactPoints] = contact; + mNbContactPoints++; } -// Remove a contact from the cache -void PersistentContactCache::removeContact(int index) { - assert(index >= 0 && index < mNbContacts); - assert(mNbContacts > 0); +// Remove a contact point from the manifold +void ContactManifold::removeContactPoint(int index) { + assert(index >= 0 && index < mNbContactPoints); + assert(mNbContactPoints > 0); // Call the destructor explicitly and tell the memory pool that // the corresponding memory block is now free - mContacts[index]->Contact::~Contact(); - mMemoryPoolContacts.freeObject(mContacts[index]); + mContactPoints[index]->ContactPoint::~ContactPoint(); + mMemoryPoolContacts.freeObject(mContactPoints[index]); // If we don't remove the last index - if (index < mNbContacts - 1) { - mContacts[index] = mContacts[mNbContacts - 1]; + if (index < mNbContactPoints - 1) { + mContactPoints[index] = mContactPoints[mNbContactPoints - 1]; } - mNbContacts--; + mNbContactPoints--; } -// Update the contact cache -// First the world space coordinates of the current contacts in the cache are recomputed from +// Update the contact manifold +// First the world space coordinates of the current contacts in the manifold are recomputed from // the corresponding transforms of the bodies because they have moved. Then we remove the contacts // with a negative penetration depth (meaning that the bodies are not penetrating anymore) and also // the contacts with a too large distance between the contact points in the plane orthogonal to the // contact normal -void PersistentContactCache::update(const Transform& transform1, const Transform& transform2) { - if (mNbContacts == 0) return; +void ContactManifold::update(const Transform& transform1, const Transform& transform2) { + if (mNbContactPoints == 0) return; - // Update the world coordinates and penetration depth of the contacts in the cache - for (int i=0; isetWorldPointOnBody1(transform1 * mContacts[i]->getLocalPointOnBody1()); - mContacts[i]->setWorldPointOnBody2(transform2 * mContacts[i]->getLocalPointOnBody2()); - mContacts[i]->setPenetrationDepth((mContacts[i]->getWorldPointOnBody1() - mContacts[i]->getWorldPointOnBody2()).dot(mContacts[i]->getNormal())); + // Update the world coordinates and penetration depth of the contact points in the manifold + for (int i=0; isetWorldPointOnBody1(transform1 * mContactPoints[i]->getLocalPointOnBody1()); + mContactPoints[i]->setWorldPointOnBody2(transform2 * mContactPoints[i]->getLocalPointOnBody2()); + mContactPoints[i]->setPenetrationDepth((mContactPoints[i]->getWorldPointOnBody1() - mContactPoints[i]->getWorldPointOnBody2()).dot(mContactPoints[i]->getNormal())); } const decimal squarePersistentContactThreshold = PERSISTENT_CONTACT_DIST_THRESHOLD * PERSISTENT_CONTACT_DIST_THRESHOLD; - // Remove the contacts that don't represent very well the persistent contact - for (int i=mNbContacts-1; i>=0; i--) { - assert(i>= 0 && i < mNbContacts); + // Remove the contact points that don't represent very well the contact manifold + for (int i=mNbContactPoints-1; i>=0; i--) { + assert(i>= 0 && i < mNbContactPoints); // Compute the distance between contact points in the normal direction - decimal distanceNormal = -mContacts[i]->getPenetrationDepth(); + decimal distanceNormal = -mContactPoints[i]->getPenetrationDepth(); // If the contacts points are too far from each other in the normal direction if (distanceNormal > squarePersistentContactThreshold) { - removeContact(i); + removeContactPoint(i); } else { - // Compute the distance of the two contact points in the plane orthogonal to the contact normal - Vector3 projOfPoint1 = mContacts[i]->getWorldPointOnBody1() + - mContacts[i]->getNormal() * distanceNormal; - Vector3 projDifference = mContacts[i]->getWorldPointOnBody2() - projOfPoint1; + // Compute the distance of the two contact points in the plane + // orthogonal to the contact normal + Vector3 projOfPoint1 = mContactPoints[i]->getWorldPointOnBody1() + + mContactPoints[i]->getNormal() * distanceNormal; + Vector3 projDifference = mContactPoints[i]->getWorldPointOnBody2() - projOfPoint1; - // If the orthogonal distance is larger than the valid distance threshold, we remove the contact + // If the orthogonal distance is larger than the valid distance + // threshold, we remove the contact if (projDifference.lengthSquare() > squarePersistentContactThreshold) { - removeContact(i); + removeContactPoint(i); } } } } -// Return the index of the contact with the larger penetration depth. This +// Return the index of the contact point with the larger penetration depth. This // corresponding contact will be kept in the cache. The method returns -1 is // the new contact is the deepest. -int PersistentContactCache::getIndexOfDeepestPenetration(Contact* newContact) const { - assert(mNbContacts == MAX_CONTACTS_IN_CACHE); +int ContactManifold::getIndexOfDeepestPenetration(ContactPoint* newContact) const { + assert(mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD); int indexMaxPenetrationDepth = -1; decimal maxPenetrationDepth = newContact->getPenetrationDepth(); // For each contact in the cache - for (uint i=0; igetPenetrationDepth() > maxPenetrationDepth) { - maxPenetrationDepth = mContacts[i]->getPenetrationDepth(); + if (mContactPoints[i]->getPenetrationDepth() > maxPenetrationDepth) { + maxPenetrationDepth = mContactPoints[i]->getPenetrationDepth(); indexMaxPenetrationDepth = i; } } @@ -155,12 +160,18 @@ int PersistentContactCache::getIndexOfDeepestPenetration(Contact* newContact) co return indexMaxPenetrationDepth; } -// Return the index that will be removed. The index of the contact with the larger penetration +// Return the index that will be removed. The index of the contact point with the larger penetration // depth is given as a parameter. This contact won't be removed. Given this contact, we compute // the different area and we want to keep the contacts with the largest area. The new point is also -// kept. -int PersistentContactCache::getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const { - assert(mNbContacts == MAX_CONTACTS_IN_CACHE); +// kept. In order to compute the area of a quadrilateral, we use the formula : +// Area = 0.5 * | AC x BD | where AC and BD form the diagonals of the quadrilateral. Note that +// when we compute this area, we do not calculate it exactly but we +// only estimate it because we do not compute the actual diagonals of the quadrialteral. Therefore, +// this is only a guess that is faster to compute. +int ContactManifold::getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const { + + assert(mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD); + decimal area0 = 0.0; // Area with contact 1,2,3 and newPoint decimal area1 = 0.0; // Area with contact 0,2,3 and newPoint decimal area2 = 0.0; // Area with contact 0,1,3 and newPoint @@ -168,29 +179,29 @@ int PersistentContactCache::getIndexToRemove(int indexMaxPenetration, const Vect if (indexMaxPenetration != 0) { // Compute the area - Vector3 vector1 = newPoint - mContacts[1]->getLocalPointOnBody1(); - Vector3 vector2 = mContacts[3]->getLocalPointOnBody1() - mContacts[2]->getLocalPointOnBody1(); + Vector3 vector1 = newPoint - mContactPoints[1]->getLocalPointOnBody1(); + Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() - mContactPoints[2]->getLocalPointOnBody1(); Vector3 crossProduct = vector1.cross(vector2); area0 = crossProduct.lengthSquare(); } if (indexMaxPenetration != 1) { // Compute the area - Vector3 vector1 = newPoint - mContacts[0]->getLocalPointOnBody1(); - Vector3 vector2 = mContacts[3]->getLocalPointOnBody1() - mContacts[2]->getLocalPointOnBody1(); + Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1(); + Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() - mContactPoints[2]->getLocalPointOnBody1(); Vector3 crossProduct = vector1.cross(vector2); area1 = crossProduct.lengthSquare(); } if (indexMaxPenetration != 2) { // Compute the area - Vector3 vector1 = newPoint - mContacts[0]->getLocalPointOnBody1(); - Vector3 vector2 = mContacts[3]->getLocalPointOnBody1() - mContacts[1]->getLocalPointOnBody1(); + Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1(); + Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() - mContactPoints[1]->getLocalPointOnBody1(); Vector3 crossProduct = vector1.cross(vector2); area2 = crossProduct.lengthSquare(); } if (indexMaxPenetration != 3) { // Compute the area - Vector3 vector1 = newPoint - mContacts[0]->getLocalPointOnBody1(); - Vector3 vector2 = mContacts[2]->getLocalPointOnBody1() - mContacts[1]->getLocalPointOnBody1(); + Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1(); + Vector3 vector2 = mContactPoints[2]->getLocalPointOnBody1() - mContactPoints[1]->getLocalPointOnBody1(); Vector3 crossProduct = vector1.cross(vector2); area3 = crossProduct.lengthSquare(); } @@ -200,7 +211,7 @@ int PersistentContactCache::getIndexToRemove(int indexMaxPenetration, const Vect } // Return the index of maximum area -int PersistentContactCache::getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const { +int ContactManifold::getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const { if (area0 < area1) { if (area1 < area2) { if (area2 < area3) return 3; @@ -223,14 +234,14 @@ int PersistentContactCache::getMaxArea(decimal area0, decimal area1, decimal are } } -// Clear the cache -void PersistentContactCache::clear() { - for (uint i=0; iContact::~Contact(); - mMemoryPoolContacts.freeObject(mContacts[i]); + mContactPoints[i]->ContactPoint::~ContactPoint(); + mMemoryPoolContacts.freeObject(mContactPoints[i]); } - mNbContacts = 0; + mNbContactPoints = 0; } diff --git a/src/engine/ContactManifold.h b/src/engine/ContactManifold.h index 7e8a7816..33dff5d8 100644 --- a/src/engine/ContactManifold.h +++ b/src/engine/ContactManifold.h @@ -24,48 +24,221 @@ ********************************************************************************/ #ifndef CONTACT_MANIFOLD_H -#define CONTACT_MANIFOLD_H +#define CONTACT_MANIFOLD_H // Libraries -#include "../constraint/Contact.h" -#include "../configuration.h" +#include +#include "../body/Body.h" +#include "../constraint/ContactPoint.h" +// ReactPhysics3D namespace namespace reactphysics3d { -// Class ContactManifold -// This class contains several contact points between two bodies -struct ContactManifold { +// Constants +const uint MAX_CONTACT_POINTS_IN_MANIFOLD = 4; // Maximum number of contacts in the manifold + +/* ------------------------------------------------------------------- + Class ContactManifold : + This class represents the set of contact points between two bodies. + The contact manifold is implemented in a way to cache the contact + points among the frames for better stability following the + "Contact Generation" presentation of Erwin Coumans at GDC 2010 + conference (bullet.googlecode.com/files/GDC10_Coumans_Erwin_Contact.pdf). + Some code of this class is based on the implementation of the + btPersistentManifold class from Bullet physics engine (www.http://bulletphysics.org). + The contacts between two bodies are added one after the other in the cache. + When the cache is full, we have to remove one point. The idea is to keep + the point with the deepest penetration depth and also to keep the + points producing the larger area (for a more stable contact manifold). + The new added point is always kept. + ------------------------------------------------------------------- +*/ +class ContactManifold { + + private: // -------------------- Attributes -------------------- // - // Contact for each contact point - Contact* contacts[4]; // TODO : Use a constant here for the nb of contacts + // Pointer to the first body + Body* const mBody1; - // Number of contacts in the manifold - uint nbContacts; + // Pointer to the second body + Body* const mBody2; + + // Contact points in the manifold + ContactPoint* mContactPoints[MAX_CONTACT_POINTS_IN_MANIFOLD]; + + // Number of contacts in the cache + uint mNbContactPoints; // First friction vector of the contact manifold - Vector3 frictionVector1; + Vector3 mFrictionVector1; // Second friction vector of the contact manifold - Vector3 frictionVector2; + Vector3 mFrictionVector2; // First friction constraint accumulated impulse - decimal friction1Impulse; + decimal mFrictionImpulse1; // Second friction constraint accumulated impulse - decimal friction2Impulse; + decimal mFrictionImpulse2; // Twist friction constraint accumulated impulse - decimal frictionTwistImpulse; + decimal mFrictionTwistImpulse; + + // Reference to the memory pool with the contacts + MemoryPool& mMemoryPoolContacts; + + // -------------------- Methods -------------------- // + + // Private copy-constructor + ContactManifold(const ContactManifold& contactManifold); + + // Private assignment operator + ContactManifold& operator=(const ContactManifold& contactManifold); + + // Return the index of maximum area + int getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const; + + // Return the index of the contact with the larger penetration depth + int getIndexOfDeepestPenetration(ContactPoint* newContact) const; + + // Return the index that will be removed + int getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const; + + // Remove a contact point from the manifold + void removeContactPoint(int index); + + // Return true if two vectors are approximatively equal + bool isApproxEqual(const Vector3& vector1, const Vector3& vector2) const; + + public: // -------------------- Methods -------------------- // // Constructor - ContactManifold() : nbContacts(0), friction1Impulse(0.0), friction2Impulse(0.0), - frictionTwistImpulse(0.0) {} + ContactManifold(Body* const mBody1, Body* const mBody2, + MemoryPool& mMemoryPoolContacts); + + // Destructor + ~ContactManifold(); + + // Add a contact point to the manifold + void addContactPoint(ContactPoint* contact); + + // Update the contact manifold + void update(const Transform& transform1, const Transform& transform2); + + // Clear the contact manifold + void clear(); + + // Return the number of contact points in the manifold + uint getNbContactPoints() const; + + // Return the first friction vector at the center of the contact manifold + const Vector3& getFrictionVector1() const; + + // set the first friction vector at the center of the contact manifold + void setFrictionVector1(const Vector3& mFrictionVector1); + + // Return the second friction vector at the center of the contact manifold + const Vector3& getFrictionVector2() const; + + // set the second friction vector at the center of the contact manifold + void setFrictionVector2(const Vector3& mFrictionVector2); + + // Return the first friction accumulated impulse + decimal getFrictionImpulse1() const; + + // Set the first friction accumulated impulse + void setFrictionImpulse1(decimal frictionImpulse1); + + // Return the second friction accumulated impulse + decimal getFrictionImpulse2() const; + + // Set the second friction accumulated impulse + void setFrictionImpulse2(decimal frictionImpulse2); + + // Return the friction twist accumulated impulse + decimal getFrictionTwistImpulse() const; + + // Set the friction twist accumulated impulse + void setFrictionTwistImpulse(decimal frictionTwistImpulse); + + // Return a contact point of the manifold + ContactPoint* getContactPoint(uint index) const; }; +// Return the number of contact points in the manifold +inline uint ContactManifold::getNbContactPoints() const { + return mNbContactPoints; } +// Return the first friction vector at the center of the contact manifold +inline const Vector3& ContactManifold::getFrictionVector1() const { + return mFrictionVector1; +} + +// set the first friction vector at the center of the contact manifold +inline void ContactManifold::setFrictionVector1(const Vector3& frictionVector1) { + mFrictionVector1 = frictionVector1; +} + +// Return the second friction vector at the center of the contact manifold +inline const Vector3& ContactManifold::getFrictionVector2() const { + return mFrictionVector2; +} + +// set the second friction vector at the center of the contact manifold +inline void ContactManifold::setFrictionVector2(const Vector3& frictionVector2) { + mFrictionVector2 = frictionVector2; +} + +// Return the first friction accumulated impulse +inline decimal ContactManifold::getFrictionImpulse1() const { + return mFrictionImpulse1; +} + +// Set the first friction accumulated impulse +inline void ContactManifold::setFrictionImpulse1(decimal frictionImpulse1) { + mFrictionImpulse1 = frictionImpulse1; +} + +// Return the second friction accumulated impulse +inline decimal ContactManifold::getFrictionImpulse2() const { + return mFrictionImpulse2; +} + +// Set the second friction accumulated impulse +inline void ContactManifold::setFrictionImpulse2(decimal frictionImpulse2) { + mFrictionImpulse2 = frictionImpulse2; +} + +// Return the friction twist accumulated impulse +inline decimal ContactManifold::getFrictionTwistImpulse() const { + return mFrictionTwistImpulse; +} + +// Set the friction twist accumulated impulse +inline void ContactManifold::setFrictionTwistImpulse(decimal frictionTwistImpulse) { + mFrictionTwistImpulse = frictionTwistImpulse; +} + +// Return a contact point of the manifold +inline ContactPoint* ContactManifold::getContactPoint(uint index) const { + assert(index >= 0 && index < mNbContactPoints); + return mContactPoints[index]; +} + +// Return true if two vectors are approximatively equal +inline bool ContactManifold::isApproxEqual(const Vector3& vector1, + const Vector3& vector2) const { + const decimal epsilon = decimal(0.1); + return (approxEqual(vector1.getX(), vector2.getX(), epsilon) && + approxEqual(vector1.getY(), vector2.getY(), epsilon) && + approxEqual(vector1.getZ(), vector2.getZ(), epsilon)); +} + +} #endif + diff --git a/src/engine/ContactSolver.cpp b/src/engine/ContactSolver.cpp index e10bc29b..b0dfa4ac 100644 --- a/src/engine/ContactSolver.cpp +++ b/src/engine/ContactSolver.cpp @@ -37,16 +37,16 @@ const decimal ContactSolver::BETA_SPLIT_IMPULSE = decimal(0.2); const decimal ContactSolver::SLOP = decimal(0.01); // Constructor -ContactSolver::ContactSolver(DynamicsWorld& world, std::vector& constrainedLinearVelocities, +ContactSolver::ContactSolver(DynamicsWorld& world,std::vector& constrainedLinearVelocities, std::vector& constrainedAngularVelocities, const std::map& mapBodyToVelocityIndex) :mWorld(world), mNbIterations(DEFAULT_CONSTRAINTS_SOLVER_NB_ITERATIONS), - mSplitLinearVelocities(NULL), mSplitAngularVelocities(NULL), mContactConstraints(NULL), + mSplitLinearVelocities(NULL), mSplitAngularVelocities(NULL), + mContactConstraints(NULL), mConstrainedLinearVelocities(constrainedLinearVelocities), mConstrainedAngularVelocities(constrainedAngularVelocities), mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex), - mIsWarmStartingActive(true), - mIsSplitImpulseActive(true), + mIsWarmStartingActive(true), mIsSplitImpulseActive(true), mIsSolveFrictionAtContactManifoldCenterActive(true) { } @@ -62,22 +62,22 @@ void ContactSolver::initialize() { // TODO : Use better memory allocation here mContactConstraints = new ContactManifoldSolver[mWorld.getNbContactManifolds()]; - mNbContactConstraints = 0; + mNbContactManifolds = 0; // For each contact manifold of the world - vector::iterator it; + vector::iterator it; for (it = mWorld.getContactManifoldsBeginIterator(); it != mWorld.getContactManifoldsEndIterator(); ++it) { - ContactManifold& externalContactManifold = *it; + ContactManifold* externalManifold = *it; - ContactManifoldSolver& internalContactManifold = mContactConstraints[mNbContactConstraints]; + ContactManifoldSolver& internalManifold = mContactConstraints[mNbContactManifolds]; - assert(externalContactManifold.nbContacts > 0); + assert(externalManifold->getNbContactPoints() > 0); // Get the two bodies of the contact - RigidBody* body1 = externalContactManifold.contacts[0]->getBody1(); - RigidBody* body2 = externalContactManifold.contacts[0]->getBody2(); + RigidBody* body1 = externalManifold->getContactPoint(0)->getBody1(); + RigidBody* body2 = externalManifold->getContactPoint(0)->getBody2(); // Add the two bodies of the constraint in the constraintBodies list mConstraintBodies.insert(body1); @@ -87,38 +87,40 @@ void ContactSolver::initialize() { Vector3 x1 = body1->getTransform().getPosition(); Vector3 x2 = body2->getTransform().getPosition(); - internalContactManifold.indexBody1 = mMapBodyToConstrainedVelocityIndex.find(body1)->second; - internalContactManifold.indexBody2 = mMapBodyToConstrainedVelocityIndex.find(body2)->second; - internalContactManifold.inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld(); - internalContactManifold.inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld(); - internalContactManifold.isBody1Moving = body1->getIsMotionEnabled(); - internalContactManifold.isBody2Moving = body2->getIsMotionEnabled(); - internalContactManifold.massInverseBody1 = body1->getMassInverse(); - internalContactManifold.massInverseBody2 = body2->getMassInverse(); - internalContactManifold.nbContacts = externalContactManifold.nbContacts; - internalContactManifold.restitutionFactor = computeMixedRestitutionFactor(body1, body2); - internalContactManifold.frictionCoefficient = computeMixedFrictionCoefficient(body1, body2); - internalContactManifold.contactManifold = &(*it); + // Initialize the internal contact manifold structure using the external + // contact manifold + internalManifold.indexBody1 = mMapBodyToConstrainedVelocityIndex.find(body1)->second; + internalManifold.indexBody2 = mMapBodyToConstrainedVelocityIndex.find(body2)->second; + internalManifold.inverseInertiaTensorBody1 = body1->getInertiaTensorInverseWorld(); + internalManifold.inverseInertiaTensorBody2 = body2->getInertiaTensorInverseWorld(); + internalManifold.isBody1Moving = body1->getIsMotionEnabled(); + internalManifold.isBody2Moving = body2->getIsMotionEnabled(); + internalManifold.massInverseBody1 = body1->getMassInverse(); + internalManifold.massInverseBody2 = body2->getMassInverse(); + internalManifold.nbContacts = externalManifold->getNbContactPoints(); + internalManifold.restitutionFactor = computeMixedRestitutionFactor(body1, body2); + internalManifold.frictionCoefficient = computeMixedFrictionCoefficient(body1, body2); + internalManifold.externalContactManifold = externalManifold; // If we solve the friction constraints at the center of the contact manifold if (mIsSolveFrictionAtContactManifoldCenterActive) { - internalContactManifold.frictionPointBody1 = Vector3(0.0, 0.0, 0.0); - internalContactManifold.frictionPointBody2 = Vector3(0.0, 0.0, 0.0); + internalManifold.frictionPointBody1 = Vector3(0.0, 0.0, 0.0); + internalManifold.frictionPointBody2 = Vector3(0.0, 0.0, 0.0); } - // For each contact point of the contact manifold - for (uint c=0; cgetNbContactPoints(); c++) { - ContactPointSolver& contactPoint = internalContactManifold.contacts[c]; + ContactPointSolver& contactPoint = internalManifold.contacts[c]; // Get a contact point - Contact* externalContact = externalContactManifold.contacts[c]; + ContactPoint* externalContact = externalManifold->getContactPoint(c); // Get the contact point on the two bodies Vector3 p1 = externalContact->getWorldPointOnBody1(); Vector3 p2 = externalContact->getWorldPointOnBody2(); - contactPoint.contact = externalContact; + contactPoint.externalContact = externalContact; contactPoint.normal = externalContact->getNormal(); contactPoint.r1 = p1 - x1; contactPoint.r2 = p2 - x2; @@ -133,39 +135,39 @@ void ContactSolver::initialize() { // If we solve the friction constraints at the center of the contact manifold if (mIsSolveFrictionAtContactManifoldCenterActive) { - internalContactManifold.frictionPointBody1 += p1; - internalContactManifold.frictionPointBody2 += p2; + internalManifold.frictionPointBody1 += p1; + internalManifold.frictionPointBody2 += p2; } } // If we solve the friction constraints at the center of the contact manifold if (mIsSolveFrictionAtContactManifoldCenterActive) { - internalContactManifold.frictionPointBody1 /= static_cast(internalContactManifold.nbContacts); - internalContactManifold.frictionPointBody2 /= static_cast(internalContactManifold.nbContacts); - internalContactManifold.r1Friction = internalContactManifold.frictionPointBody1 - x1; - internalContactManifold.r2Friction = internalContactManifold.frictionPointBody2 - x2; - internalContactManifold.oldFrictionVector1 = externalContactManifold.frictionVector1; - internalContactManifold.oldFrictionVector2 = externalContactManifold.frictionVector2; + internalManifold.frictionPointBody1 /=static_cast(internalManifold.nbContacts); + internalManifold.frictionPointBody2 /=static_cast(internalManifold.nbContacts); + internalManifold.r1Friction = internalManifold.frictionPointBody1 - x1; + internalManifold.r2Friction = internalManifold.frictionPointBody2 - x2; + internalManifold.oldFrictionVector1 = externalManifold->getFrictionVector1(); + internalManifold.oldFrictionVector2 = externalManifold->getFrictionVector2(); // If warm starting is active if (mIsWarmStartingActive) { // Initialize the accumulated impulses with the previous step accumulated impulses - internalContactManifold.friction1Impulse = externalContactManifold.friction1Impulse; - internalContactManifold.friction2Impulse = externalContactManifold.friction2Impulse; - internalContactManifold.frictionTwistImpulse = externalContactManifold.frictionTwistImpulse; + internalManifold.friction1Impulse = externalManifold->getFrictionImpulse1(); + internalManifold.friction2Impulse = externalManifold->getFrictionImpulse2(); + internalManifold.frictionTwistImpulse = externalManifold->getFrictionTwistImpulse(); } else { // Initialize the accumulated impulses to zero - internalContactManifold.friction1Impulse = 0.0; - internalContactManifold.friction2Impulse = 0.0; - internalContactManifold.frictionTwistImpulse = 0.0; + internalManifold.friction1Impulse = 0.0; + internalManifold.friction2Impulse = 0.0; + internalManifold.frictionTwistImpulse = 0.0; } } - mNbContactConstraints++; + mNbContactManifolds++; } // Allocated memory for split impulse velocities @@ -179,10 +181,13 @@ void ContactSolver::initialize() { assert(mMapBodyToConstrainedVelocityIndex.size() >= mConstraintBodies.size()); assert(mConstrainedLinearVelocities.size() >= mConstraintBodies.size()); assert(mConstrainedAngularVelocities.size() >= mConstraintBodies.size()); + + // Initialize the split impulse velocities + initializeSplitImpulseVelocities(); } -// Initialize the constrained bodies -void ContactSolver::initializeBodies() { +// Initialize the split impulse velocities +void ContactSolver::initializeSplitImpulseVelocities() { // For each current body that is implied in some constraint set::iterator it; @@ -192,6 +197,7 @@ void ContactSolver::initializeBodies() { uint bodyNumber = mMapBodyToConstrainedVelocityIndex.find(rigidBody)->second; + // Initialize the split impulse velocities to zero mSplitLinearVelocities[bodyNumber] = Vector3(0, 0, 0); mSplitAngularVelocities[bodyNumber] = Vector3(0, 0, 0); } @@ -201,30 +207,30 @@ void ContactSolver::initializeBodies() { void ContactSolver::initializeContactConstraints() { // For each contact constraint - for (uint c=0; c 0.0 ? contactPoint.inversePenetrationMass = decimal(1.0) / massPenetration : decimal(0.0); + massPenetration > 0.0 ? contactPoint.inversePenetrationMass = decimal(1.0) / + massPenetration : + decimal(0.0); + // If we do not solve the friction constraints at the center of the contact manifold if (!mIsSolveFrictionAtContactManifoldCenterActive) { // Compute the friction vectors @@ -253,34 +263,48 @@ void ContactSolver::initializeContactConstraints() { contactPoint.r2CrossT1 = contactPoint.r2.cross(contactPoint.frictionVector1); contactPoint.r2CrossT2 = contactPoint.r2.cross(contactPoint.frictionVector2); - // Compute the inverse mass matrix K for the friction constraints at each contact point + // Compute the inverse mass matrix K for the friction + // constraints at each contact point decimal friction1Mass = 0.0; decimal friction2Mass = 0.0; - if (contactManifold.isBody1Moving) { - friction1Mass += contactManifold.massInverseBody1 + ((I1 * contactPoint.r1CrossT1).cross(contactPoint.r1)).dot(contactPoint.frictionVector1); - friction2Mass += contactManifold.massInverseBody1 + ((I1 * contactPoint.r1CrossT2).cross(contactPoint.r1)).dot(contactPoint.frictionVector2); + if (manifold.isBody1Moving) { + friction1Mass += manifold.massInverseBody1 + + ((I1 * contactPoint.r1CrossT1).cross(contactPoint.r1)).dot( + contactPoint.frictionVector1); + friction2Mass += manifold.massInverseBody1 + + ((I1 * contactPoint.r1CrossT2).cross(contactPoint.r1)).dot( + contactPoint.frictionVector2); } - if (contactManifold.isBody2Moving) { - friction1Mass += contactManifold.massInverseBody2 + ((I2 * contactPoint.r2CrossT1).cross(contactPoint.r2)).dot(contactPoint.frictionVector1); - friction2Mass += contactManifold.massInverseBody2 + ((I2 * contactPoint.r2CrossT2).cross(contactPoint.r2)).dot(contactPoint.frictionVector2); + if (manifold.isBody2Moving) { + friction1Mass += manifold.massInverseBody2 + + ((I2 * contactPoint.r2CrossT1).cross(contactPoint.r2)).dot( + contactPoint.frictionVector1); + friction2Mass += manifold.massInverseBody2 + + ((I2 * contactPoint.r2CrossT2).cross(contactPoint.r2)).dot( + contactPoint.frictionVector2); } - friction1Mass > 0.0 ? contactPoint.inverseFriction1Mass = decimal(1.0) / friction1Mass : decimal(0.0); - friction2Mass > 0.0 ? contactPoint.inverseFriction2Mass = decimal(1.0) / friction2Mass : decimal(0.0); + friction1Mass > 0.0 ? contactPoint.inverseFriction1Mass = decimal(1.0) / + friction1Mass : + decimal(0.0); + friction2Mass > 0.0 ? contactPoint.inverseFriction2Mass = decimal(1.0) / + friction2Mass : + decimal(0.0); } // Compute the restitution velocity bias "b". We compute this here instead // of inside the solve() method because we need to use the velocity difference - // at the beginning of the contact. Note that if it is a resting contact (normal velocity - // under a given threshold), we don't add a restitution velocity bias + // at the beginning of the contact. Note that if it is a resting contact (normal + // velocity under a given threshold), we don't add a restitution velocity bias contactPoint.restitutionBias = 0.0; decimal deltaVDotN = deltaV.dot(contactPoint.normal); - // TODO : Use a constant here - if (deltaVDotN < 1.0f) { - contactPoint.restitutionBias = contactManifold.restitutionFactor * deltaVDotN; + if (deltaVDotN < RESTITUTION_VELOCITY_THRESHOLD) { + contactPoint.restitutionBias = manifold.restitutionFactor * deltaVDotN; } - // Get the cached lambda values of the constraint + // If the warm starting of the contact solver is active if (mIsWarmStartingActive) { + + // Get the cached accumulated impulses from the previous step contactPoint.penetrationImpulse = externalContact->getCachedLambda(0); contactPoint.friction1Impulse = externalContact->getCachedLambda(1); contactPoint.friction2Impulse = externalContact->getCachedLambda(2); @@ -291,39 +315,58 @@ void ContactSolver::initializeContactConstraints() { // If we solve the friction constraints at the center of the contact manifold if (mIsSolveFrictionAtContactManifoldCenterActive) { - contactManifold.normal += contactPoint.normal; + manifold.normal += contactPoint.normal; } } // If we solve the friction constraints at the center of the contact manifold if (mIsSolveFrictionAtContactManifoldCenterActive) { - contactManifold.normal.normalize(); + manifold.normal.normalize(); - Vector3 deltaVFrictionPoint = v2 + w2.cross(contactManifold.r2Friction) - - v1 - w1.cross(contactManifold.r1Friction); + Vector3 deltaVFrictionPoint = v2 + w2.cross(manifold.r2Friction) - + v1 - w1.cross(manifold.r1Friction); // Compute the friction vectors - computeFrictionVectors(deltaVFrictionPoint, contactManifold); + computeFrictionVectors(deltaVFrictionPoint, manifold); // Compute the inverse mass matrix K for the friction constraints at the center of // the contact manifold - contactManifold.r1CrossT1 = contactManifold.r1Friction.cross(contactManifold.frictionVector1); - contactManifold.r1CrossT2 = contactManifold.r1Friction.cross(contactManifold.frictionVector2); - contactManifold.r2CrossT1 = contactManifold.r2Friction.cross(contactManifold.frictionVector1); - contactManifold.r2CrossT2 = contactManifold.r2Friction.cross(contactManifold.frictionVector2); + manifold.r1CrossT1 = manifold.r1Friction.cross(manifold.frictionVector1); + manifold.r1CrossT2 = manifold.r1Friction.cross(manifold.frictionVector2); + manifold.r2CrossT1 = manifold.r2Friction.cross(manifold.frictionVector1); + manifold.r2CrossT2 = manifold.r2Friction.cross(manifold.frictionVector2); decimal friction1Mass = 0.0; decimal friction2Mass = 0.0; - if (contactManifold.isBody1Moving) { - friction1Mass += contactManifold.massInverseBody1 + ((I1 * contactManifold.r1CrossT1).cross(contactManifold.r1Friction)).dot(contactManifold.frictionVector1); - friction2Mass += contactManifold.massInverseBody1 + ((I1 * contactManifold.r1CrossT2).cross(contactManifold.r1Friction)).dot(contactManifold.frictionVector2); + if (manifold.isBody1Moving) { + friction1Mass += manifold.massInverseBody1 + + ((I1 * manifold.r1CrossT1).cross(manifold.r1Friction)).dot( + manifold.frictionVector1); + friction2Mass += manifold.massInverseBody1 + + ((I1 * manifold.r1CrossT2).cross(manifold.r1Friction)).dot( + manifold.frictionVector2); } - if (contactManifold.isBody2Moving) { - friction1Mass += contactManifold.massInverseBody2 + ((I2 * contactManifold.r2CrossT1).cross(contactManifold.r2Friction)).dot(contactManifold.frictionVector1); - friction2Mass += contactManifold.massInverseBody2 + ((I2 * contactManifold.r2CrossT2).cross(contactManifold.r2Friction)).dot(contactManifold.frictionVector2); + if (manifold.isBody2Moving) { + friction1Mass += manifold.massInverseBody2 + + ((I2 * manifold.r2CrossT1).cross(manifold.r2Friction)).dot( + manifold.frictionVector1); + friction2Mass += manifold.massInverseBody2 + + ((I2 * manifold.r2CrossT2).cross(manifold.r2Friction)).dot( + manifold.frictionVector2); } - friction1Mass > 0.0 ? contactManifold.inverseFriction1Mass = decimal(1.0) / friction1Mass : decimal(0.0); - friction2Mass > 0.0 ? contactManifold.inverseFriction2Mass = decimal(1.0) / friction2Mass : decimal(0.0); + decimal frictionTwistMass = manifold.normal.dot( + manifold.inverseInertiaTensorBody1 * + manifold.normal) + + manifold.normal.dot( + manifold.inverseInertiaTensorBody2 * + manifold.normal); + friction1Mass > 0.0 ? manifold.inverseFriction1Mass = decimal(1.0)/friction1Mass + : decimal(0.0); + friction2Mass > 0.0 ? manifold.inverseFriction2Mass = decimal(1.0)/friction2Mass + : decimal(0.0); + frictionTwistMass > 0.0 ? manifold.inverseTwistFrictionMass = decimal(1.0) / + frictionTwistMass : + decimal(0.0); } } } @@ -335,7 +378,7 @@ void ContactSolver::initializeContactConstraints() { void ContactSolver::warmStart() { // For each constraint - for (uint c=0; csetCachedLambda(0, contactPoint.penetrationImpulse); - contactPoint.contact->setCachedLambda(1, contactPoint.friction1Impulse); - contactPoint.contact->setCachedLambda(2, contactPoint.friction2Impulse); + contactPoint.externalContact->setCachedLambda(0, contactPoint.penetrationImpulse); + contactPoint.externalContact->setCachedLambda(1, contactPoint.friction1Impulse); + contactPoint.externalContact->setCachedLambda(2, contactPoint.friction2Impulse); - contactPoint.contact->setFrictionVector1(contactPoint.frictionVector1); - contactPoint.contact->setFrictionVector2(contactPoint.frictionVector2); + contactPoint.externalContact->setFrictionVector1(contactPoint.frictionVector1); + contactPoint.externalContact->setFrictionVector2(contactPoint.frictionVector2); } - contactManifold.contactManifold->friction1Impulse = contactManifold.friction1Impulse; - contactManifold.contactManifold->friction2Impulse = contactManifold.friction2Impulse; - contactManifold.contactManifold->frictionTwistImpulse = contactManifold.frictionTwistImpulse; - contactManifold.contactManifold->frictionVector1 = contactManifold.frictionVector1; - contactManifold.contactManifold->frictionVector2 = contactManifold.frictionVector2; + manifold.externalContactManifold->setFrictionImpulse1(manifold.friction1Impulse); + manifold.externalContactManifold->setFrictionImpulse2(manifold.friction2Impulse); + manifold.externalContactManifold->setFrictionTwistImpulse(manifold.frictionTwistImpulse); + manifold.externalContactManifold->setFrictionVector1(manifold.frictionVector1); + manifold.externalContactManifold->setFrictionVector2(manifold.frictionVector2); } } // Apply an impulse to the two bodies of a constraint -void ContactSolver::applyImpulse(const Impulse& impulse, const ContactManifoldSolver& contactManifold) { +void ContactSolver::applyImpulse(const Impulse& impulse, + const ContactManifoldSolver& manifold) { // Update the velocities of the bodies by applying the impulse P - if (contactManifold.isBody1Moving) { - mConstrainedLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 * + if (manifold.isBody1Moving) { + mConstrainedLinearVelocities[manifold.indexBody1] += manifold.massInverseBody1 * impulse.linearImpulseBody1; - mConstrainedAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * + mConstrainedAngularVelocities[manifold.indexBody1] += manifold.inverseInertiaTensorBody1 * impulse.angularImpulseBody1; } - if (contactManifold.isBody2Moving) { - mConstrainedLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 * + if (manifold.isBody2Moving) { + mConstrainedLinearVelocities[manifold.indexBody2] += manifold.massInverseBody2 * impulse.linearImpulseBody2; - mConstrainedAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * + mConstrainedAngularVelocities[manifold.indexBody2] += manifold.inverseInertiaTensorBody2 * impulse.angularImpulseBody2; } } // Apply an impulse to the two bodies of a constraint void ContactSolver::applySplitImpulse(const Impulse& impulse, - const ContactManifoldSolver& contactManifold) { + const ContactManifoldSolver& manifold) { // Update the velocities of the bodies by applying the impulse P - if (contactManifold.isBody1Moving) { - mSplitLinearVelocities[contactManifold.indexBody1] += contactManifold.massInverseBody1 * + if (manifold.isBody1Moving) { + mSplitLinearVelocities[manifold.indexBody1] += manifold.massInverseBody1 * impulse.linearImpulseBody1; - mSplitAngularVelocities[contactManifold.indexBody1] += contactManifold.inverseInertiaTensorBody1 * + mSplitAngularVelocities[manifold.indexBody1] += manifold.inverseInertiaTensorBody1 * impulse.angularImpulseBody1; } - if (contactManifold.isBody2Moving) { - mSplitLinearVelocities[contactManifold.indexBody2] += contactManifold.massInverseBody2 * - impulse.linearImpulseBody2; - mSplitAngularVelocities[contactManifold.indexBody2] += contactManifold.inverseInertiaTensorBody2 * + if (manifold.isBody2Moving) { + mSplitLinearVelocities[manifold.indexBody2] += manifold.massInverseBody2 * + impulse.linearImpulseBody2; + mSplitAngularVelocities[manifold.indexBody2] += manifold.inverseInertiaTensorBody2 * impulse.angularImpulseBody2; } } @@ -781,7 +862,7 @@ void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity, // The second friction vector is computed by the cross product of the firs // friction vector and the contact normal - contactPoint.frictionVector2 = contactPoint.normal.cross(contactPoint.frictionVector1).getUnit(); + contactPoint.frictionVector2 =contactPoint.normal.cross(contactPoint.frictionVector1).getUnit(); } // Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction plane @@ -816,7 +897,7 @@ void ContactSolver::computeFrictionVectors(const Vector3& deltaVelocity, // Clean up the constraint solver void ContactSolver::cleanup() { - //mMapBodyToIndex.clear(); + mConstraintBodies.clear(); if (mContactConstraints != NULL) { diff --git a/src/engine/ContactSolver.h b/src/engine/ContactSolver.h index bae7a9bb..1eb7c059 100644 --- a/src/engine/ContactSolver.h +++ b/src/engine/ContactSolver.h @@ -23,14 +23,14 @@ * * ********************************************************************************/ -#ifndef CONSTRAINT_SOLVER_H -#define CONSTRAINT_SOLVER_H +#ifndef CONTACT_SOLVER_H +#define CONTACT_SOLVER_H // Libraries -#include "../constraint/Contact.h" -#include "ContactManifold.h" +#include "../constraint/ContactPoint.h" #include "../configuration.h" #include "../constraint/Constraint.h" +#include "ContactManifold.h" #include #include @@ -124,10 +124,10 @@ struct Impulse { There are two ways to apply the friction constraints. Either the friction constraints are applied at each contact point or they are applied only at the center of the contact manifold between two bodies. If we solve the friction constraints at each contact point, we need - two constraints (two tangential friction directions) and if we solve the friction constraints - at the center of the contact manifold, we need two constraints for tangential friction but - also another twist friction constraint to prevent spin of the body around the contact - manifold center. + two constraints (two tangential friction directions) and if we solve the friction + constraints at the center of the contact manifold, we need two constraints for tangential + friction but also another twist friction constraint to prevent spin of the body around the + contact manifold center. ------------------------------------------------------------------- */ @@ -135,7 +135,7 @@ class ContactSolver { private: - // Structure ContactPoint + // Structure ContactPointSolver // Contact solver internal data structure that to store all the // information relative to a contact point struct ContactPointSolver { @@ -163,16 +163,14 @@ class ContactSolver { decimal inverseFriction1Mass; // Inverse of the matrix K for the 1st friction decimal inverseFriction2Mass; // Inverse of the matrix K for the 2nd friction bool isRestingContact; // True if the contact was existing last time step - Contact* contact; // Pointer to the external contact + ContactPoint* externalContact; // Pointer to the external contact }; - // Structure ContactManifold + // Structure ContactManifoldSolver // Contact solver internal data structure to store all the // information relative to a contact manifold struct ContactManifoldSolver { - // TODO : Use a constant for the number of contact points - uint indexBody1; // Index of body 1 in the constraint solver uint indexBody2; // Index of body 2 in the constraint solver decimal massInverseBody1; // Inverse of the mass of body 1 @@ -181,11 +179,11 @@ class ContactSolver { Matrix3x3 inverseInertiaTensorBody2; // Inverse inertia tensor of body 2 bool isBody1Moving; // True if the body 1 is allowed to move bool isBody2Moving; // True if the body 2 is allowed to move - ContactPointSolver contacts[4]; // Contact point constraints + ContactPointSolver contacts[MAX_CONTACT_POINTS_IN_MANIFOLD];// Contact point constraints uint nbContacts; // Number of contact points decimal restitutionFactor; // Mix of the restitution factor for two bodies decimal frictionCoefficient; // Mix friction coefficient for the two bodies - ContactManifold* contactManifold; // Pointer to the external contact manifold + ContactManifold* externalContactManifold; // Pointer to the external contact manifold // Variables used when friction constraints are apply at the center of the manifold // @@ -200,6 +198,7 @@ class ContactSolver { Vector3 r2CrossT2; // Cross product of r2 with 2nd friction vector decimal inverseFriction1Mass; // Matrix K for the first friction constraint decimal inverseFriction2Mass; // Matrix K for the second friction constraint + decimal inverseTwistFrictionMass; // Matrix K for the twist friction constraint Vector3 frictionVector1; // First friction direction at contact manifold center Vector3 frictionVector2; // Second friction direction at contact manifold center Vector3 oldFrictionVector1; // Old 1st friction direction at contact manifold center @@ -241,7 +240,7 @@ class ContactSolver { ContactManifoldSolver* mContactConstraints; // Number of contact constraints - uint mNbContactConstraints; + uint mNbContactManifolds; // Constrained bodies std::set mConstraintBodies; @@ -272,8 +271,8 @@ class ContactSolver { // Initialize the constraint solver void initialize(); - // Initialize the constrained bodies - void initializeBodies(); + // Initialize the split impulse velocities + void initializeSplitImpulseVelocities(); // Initialize the contact constraints before solving the system void initializeContactConstraints(); @@ -289,16 +288,19 @@ class ContactSolver { void solveContactConstraints(); // Apply an impulse to the two bodies of a constraint - void applyImpulse(const Impulse& impulse, const ContactManifoldSolver& contactManifold); + void applyImpulse(const Impulse& impulse, const ContactManifoldSolver& manifold); // Apply an impulse to the two bodies of a constraint - void applySplitImpulse(const Impulse& impulse, const ContactManifoldSolver& contactManifold); + void applySplitImpulse(const Impulse& impulse, + const ContactManifoldSolver& manifold); // Compute the collision restitution factor from the restitution factor of each body - decimal computeMixedRestitutionFactor(const RigidBody* body1, const RigidBody* body2) const; + decimal computeMixedRestitutionFactor(const RigidBody* body1, + const RigidBody* body2) const; // Compute the mixed friction coefficient from the friction coefficient of each body - decimal computeMixedFrictionCoefficient(const RigidBody* body1, const RigidBody* body2)const; + decimal computeMixedFrictionCoefficient(const RigidBody* body1, + const RigidBody* body2)const; // Compute the two unit orthogonal vectors "t1" and "t2" that span the tangential friction // plane for a contact point constraint. The two vectors have to be @@ -330,8 +332,8 @@ class ContactSolver { // Constructor ContactSolver(DynamicsWorld& mWorld, std::vector& constrainedLinearVelocities, - std::vector& constrainedAngularVelocities, const std::map& - mapBodyToVelocityIndex); + std::vector& constrainedAngularVelocities, + const std::map& mapBodyToVelocityIndex); // Destructor virtual ~ContactSolver(); @@ -432,16 +434,20 @@ inline const Impulse ContactSolver::computePenetrationImpulse(decimal deltaLambd inline const Impulse ContactSolver::computeFriction1Impulse(decimal deltaLambda, const ContactPointSolver& contactPoint) const { - return Impulse(-contactPoint.frictionVector1 * deltaLambda, -contactPoint.r1CrossT1 * deltaLambda, - contactPoint.frictionVector1 * deltaLambda, contactPoint.r2CrossT1 * deltaLambda); + return Impulse(-contactPoint.frictionVector1 * deltaLambda, + -contactPoint.r1CrossT1 * deltaLambda, + contactPoint.frictionVector1 * deltaLambda, + contactPoint.r2CrossT1 * deltaLambda); } // Compute the second friction constraint impulse inline const Impulse ContactSolver::computeFriction2Impulse(decimal deltaLambda, const ContactPointSolver& contactPoint) const { - return Impulse(-contactPoint.frictionVector2 * deltaLambda, -contactPoint.r1CrossT2 * deltaLambda, - contactPoint.frictionVector2 * deltaLambda, contactPoint.r2CrossT2 * deltaLambda); + return Impulse(-contactPoint.frictionVector2 * deltaLambda, + -contactPoint.r1CrossT2 * deltaLambda, + contactPoint.frictionVector2 * deltaLambda, + contactPoint.r2CrossT2 * deltaLambda); } } diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp index d1ad2f96..a24147f8 100644 --- a/src/engine/DynamicsWorld.cpp +++ b/src/engine/DynamicsWorld.cpp @@ -305,7 +305,7 @@ void DynamicsWorld::notifyNewContact(const BroadPhasePair* broadPhasePair, const assert(rigidBody2); // Create a new contact - Contact* contact = new (mMemoryPoolContacts.allocateObject()) Contact(rigidBody1, rigidBody2, contactInfo); + ContactPoint* contact = new (mMemoryPoolContacts.allocateObject()) ContactPoint(rigidBody1, rigidBody2, contactInfo); assert(contact); // Get the corresponding overlapping pair @@ -316,6 +316,8 @@ void DynamicsWorld::notifyNewContact(const BroadPhasePair* broadPhasePair, const // Add the contact to the contact cache of the corresponding overlapping pair overlappingPair->addContact(contact); + // TODO : Remove this + /* // Create a contact manifold with the contact points of the two bodies ContactManifold contactManifold; contactManifold.nbContacts = 0; @@ -326,7 +328,8 @@ void DynamicsWorld::notifyNewContact(const BroadPhasePair* broadPhasePair, const contactManifold.contacts[i] = overlappingPair->getContact(i); contactManifold.nbContacts++; } + */ // Add the contact manifold to the world - mContactManifolds.push_back(contactManifold); + mContactManifolds.push_back(overlappingPair->getContactManifold()); } diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index b2254fca..7bddf900 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -28,7 +28,6 @@ // Libraries #include "CollisionWorld.h" -#include "ContactManifold.h" #include "../collision/CollisionDetection.h" #include "ContactSolver.h" #include "../body/RigidBody.h" @@ -64,7 +63,7 @@ class DynamicsWorld : public CollisionWorld { std::set mRigidBodies; // All the contact constraints - std::vector mContactManifolds; + std::vector mContactManifolds; // All the constraints (except contact constraints) std::vector mConstraints; @@ -82,7 +81,7 @@ class DynamicsWorld : public CollisionWorld { MemoryPool mMemoryPoolRigidBodies; // Memory pool for the contacts - MemoryPool mMemoryPoolContacts; + MemoryPool mMemoryPoolContacts; // Array of constrained linear velocities (state of the linear velocities // after solving the constraints) @@ -208,10 +207,10 @@ public : std::vector::iterator getConstraintsEndIterator(); // Return a start iterator on the contact manifolds list - std::vector::iterator getContactManifoldsBeginIterator(); + std::vector::iterator getContactManifoldsBeginIterator(); // Return a end iterator on the contact manifolds list - std::vector::iterator getContactManifoldsEndIterator(); + std::vector::iterator getContactManifoldsEndIterator(); // Return an iterator to the beginning of the rigid bodies of the physics world std::set::iterator getRigidBodiesBeginIterator(); @@ -337,12 +336,12 @@ inline std::vector::iterator DynamicsWorld::getConstraintsEndIterat } // Return a start iterator on the contact manifolds list -inline std::vector::iterator DynamicsWorld::getContactManifoldsBeginIterator() { +inline std::vector::iterator DynamicsWorld::getContactManifoldsBeginIterator() { return mContactManifolds.begin(); } // Return a end iterator on the contact manifolds list -inline std::vector::iterator DynamicsWorld::getContactManifoldsEndIterator() { +inline std::vector::iterator DynamicsWorld::getContactManifoldsEndIterator() { return mContactManifolds.end(); } diff --git a/src/engine/OverlappingPair.cpp b/src/engine/OverlappingPair.cpp index 933dac4e..cfd0aef4 100644 --- a/src/engine/OverlappingPair.cpp +++ b/src/engine/OverlappingPair.cpp @@ -31,11 +31,11 @@ using namespace reactphysics3d; // Constructor OverlappingPair::OverlappingPair(CollisionBody* body1, CollisionBody* body2, - MemoryPool& memoryPoolContacts) - : mBody1(body1), mBody2(body2), mContactsCache(body1, body2, memoryPoolContacts), + MemoryPool& memoryPoolContacts) + : mBody1(body1), mBody2(body2), mContactManifold(body1, body2, memoryPoolContacts), mCachedSeparatingAxis(1.0, 1.0, 1.0) { -} +} // Destructor OverlappingPair::~OverlappingPair() { diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPair.h index f8d31aff..d8d977eb 100644 --- a/src/engine/OverlappingPair.h +++ b/src/engine/OverlappingPair.h @@ -27,7 +27,7 @@ #define OVERLAPPING_PAIR_H // Libraries -#include "PersistentContactCache.h" +#include "ContactManifold.h" // ReactPhysics3D namespace namespace reactphysics3d { @@ -53,8 +53,8 @@ class OverlappingPair { // Pointer to the second body of the contact CollisionBody* const mBody2; - // Persistent contact cache - PersistentContactCache mContactsCache; + // Persistent contact manifold + ContactManifold mContactManifold; // Cached previous separating axis Vector3 mCachedSeparatingAxis; @@ -73,7 +73,7 @@ class OverlappingPair { // Constructor OverlappingPair(CollisionBody* body1, CollisionBody* body2, - MemoryPool& memoryPoolContacts); + MemoryPool& memoryPoolContacts); // Destructor ~OverlappingPair(); @@ -85,7 +85,7 @@ class OverlappingPair { CollisionBody* const getBody2() const; // Add a contact to the contact cache - void addContact(Contact* contact); + void addContact(ContactPoint* contact); // Update the contact cache void update(); @@ -99,8 +99,12 @@ class OverlappingPair { // Return the number of contacts in the cache uint getNbContacts() const; + // Return the contact manifold + ContactManifold* getContactManifold(); + // Return a contact of the cache - Contact* getContact(uint index) const; + // TODO : Maybe remove this method + ContactPoint* getContact(uint index) const; }; // Return the pointer to first body @@ -113,14 +117,14 @@ inline CollisionBody* const OverlappingPair::getBody2() const { return mBody2; } -// Add a contact to the contact cache -inline void OverlappingPair::addContact(Contact* contact) { - mContactsCache.addContact(contact); +// Add a contact to the contact manifold +inline void OverlappingPair::addContact(ContactPoint* contact) { + mContactManifold.addContactPoint(contact); } -// Update the contact cache +// Update the contact manifold inline void OverlappingPair::update() { - mContactsCache.update(mBody1->getTransform(), mBody2->getTransform()); + mContactManifold.update(mBody1->getTransform(), mBody2->getTransform()); } // Return the cached separating axis @@ -136,12 +140,17 @@ inline void OverlappingPair::setCachedSeparatingAxis(const Vector3& axis) { // Return the number of contacts in the cache inline uint OverlappingPair::getNbContacts() const { - return mContactsCache.getNbContacts(); + return mContactManifold.getNbContactPoints(); +} + +// Return the contact manifold +inline ContactManifold* OverlappingPair::getContactManifold() { + return &mContactManifold; } // Return a contact of the cache -inline Contact* OverlappingPair::getContact(uint index) const { - return mContactsCache.getContact(index); +inline ContactPoint* OverlappingPair::getContact(uint index) const { + return mContactManifold.getContactPoint(index); } } // End of the ReactPhysics3D namespace diff --git a/src/engine/PersistentContactCache.h b/src/engine/PersistentContactCache.h deleted file mode 100644 index a1b28a8d..00000000 --- a/src/engine/PersistentContactCache.h +++ /dev/null @@ -1,147 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * -* Copyright (c) 2010-2012 Daniel Chappuis * -********************************************************************************* -* * -* This software is provided 'as-is', without any express or implied warranty. * -* In no event will the authors be held liable for any damages arising from the * -* use of this software. * -* * -* Permission is granted to anyone to use this software for any purpose, * -* including commercial applications, and to alter it and redistribute it * -* freely, subject to the following restrictions: * -* * -* 1. The origin of this software must not be misrepresented; you must not claim * -* that you wrote the original software. If you use this software in a * -* product, an acknowledgment in the product documentation would be * -* appreciated but is not required. * -* * -* 2. Altered source versions must be plainly marked as such, and must not be * -* misrepresented as being the original software. * -* * -* 3. This notice may not be removed or altered from any source distribution. * -* * -********************************************************************************/ - -#ifndef PERSISTENT_CONTACT_CACHE_H -#define PERSISTENT_CONTACT_CACHE_H - -// Libraries -#include -#include "../body/Body.h" -#include "../constraint/Contact.h" - -// ReactPhysics3D namespace -namespace reactphysics3d { - -// Constants -const uint MAX_CONTACTS_IN_CACHE = 4; // Maximum number of contacts in the persistent cache - - -/* ------------------------------------------------------------------- - Class PersistentContactCache : - This class represents a cache of at most 4 contact points between - two given bodies. The contacts between two bodies are added one - after the other in the cache. When the cache is full, we have - to remove one point. The idea is to keep the point with the deepest - penetration depth and also to keep the points producing the larger - area (for a more stable contact manifold). The new added point is - always kept. This kind of persistent cache has been explained for - instance in the presentation from Erin Catto about contact manifolds - at the GDC 2007 conference. - ------------------------------------------------------------------- -*/ -class PersistentContactCache { - - private: - - // -------------------- Attributes -------------------- // - - // Pointer to the first body - Body* const mBody1; - - // Pointer to the second body - Body* const mBody2; - - // Contacts in the cache - Contact* mContacts[MAX_CONTACTS_IN_CACHE]; - - // Number of contacts in the cache - uint mNbContacts; - - // Reference to the memory pool with the contacts - MemoryPool& mMemoryPoolContacts; - - // -------------------- Methods -------------------- // - - // Private copy-constructor - PersistentContactCache(const PersistentContactCache& persistentContactCache); - - // Private assignment operator - PersistentContactCache& operator=(const PersistentContactCache& persistentContactCache); - - // Return the index of maximum area - int getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const; - - // Return the index of the contact with the larger penetration depth - int getIndexOfDeepestPenetration(Contact* newContact) const; - - // Return the index that will be removed - int getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const; - - // Remove a contact from the cache - void removeContact(int index); - - // Return true if two vectors are approximatively equal - bool isApproxEqual(const Vector3& vector1, const Vector3& vector2) const; - - public: - - // -------------------- Methods -------------------- // - - // Constructor - PersistentContactCache(Body* const mBody1, Body* const mBody2, - MemoryPool& mMemoryPoolContacts); - - // Destructor - ~PersistentContactCache(); - - // Add a contact - void addContact(Contact* contact); - - // Update the contact cache - void update(const Transform& transform1, const Transform& transform2); - - // Clear the cache - void clear(); - - // Return the number of contacts in the cache - uint getNbContacts() const; - - // Return a contact of the cache - Contact* getContact(uint index) const; -}; - -// Return the number of contacts in the cache -inline uint PersistentContactCache::getNbContacts() const { - return mNbContacts; -} - -// Return a contact of the cache -inline Contact* PersistentContactCache::getContact(uint index) const { - assert(index >= 0 && index < mNbContacts); - return mContacts[index]; -} - -// Return true if two vectors are approximatively equal -inline bool PersistentContactCache::isApproxEqual(const Vector3& vector1, - const Vector3& vector2) const { - const decimal epsilon = decimal(0.1); - return (approxEqual(vector1.getX(), vector2.getX(), epsilon) && - approxEqual(vector1.getY(), vector2.getY(), epsilon) && - approxEqual(vector1.getZ(), vector2.getZ(), epsilon)); -} - -} -#endif -