From d546d8208f9af8f39878b5feed2f78613dd96324 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sun, 27 Jan 2013 10:38:41 +0100 Subject: [PATCH] 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;