Use first friction vector in the direction of the tangential velocity
This commit is contained in:
Daniel Chappuis
parent
2d0da2cc1c
commit
d546d8208f
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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<Vector3> 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;
|
||||
|
|
|
|||
|
|
@ -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; iter<mNbIterations; iter++) {
|
||||
|
||||
|
|
@ -321,9 +333,9 @@ void ConstraintSolver::solveContactConstraints() {
|
|||
deltaLambda = - (Jv + b);
|
||||
|
||||
//deltaLambda -= contact.b_Penetration;
|
||||
deltaLambda /= contact.inversePenetrationMass;
|
||||
deltaLambda *= contact.inversePenetrationMass;
|
||||
lambdaTemp = contact.penetrationImpulse;
|
||||
contact.penetrationImpulse = std::max(contact.lowerBoundPenetration, std::min(contact.penetrationImpulse + deltaLambda, contact.upperBoundPenetration));
|
||||
contact.penetrationImpulse = std::max(contact.penetrationImpulse + deltaLambda, 0.0f);
|
||||
deltaLambda = contact.penetrationImpulse - lambdaTemp;
|
||||
|
||||
// Compute the impulse P=J^T * lambda
|
||||
|
|
@ -344,9 +356,10 @@ void ConstraintSolver::solveContactConstraints() {
|
|||
Jv = deltaV.dot(contact.frictionVector1);
|
||||
|
||||
deltaLambda = -Jv;
|
||||
deltaLambda /= contact.inverseFriction1Mass;
|
||||
deltaLambda *= contact.inverseFriction1Mass;
|
||||
decimal frictionLimit = 0.3 * contact.penetrationImpulse; // TODO : Use constant here
|
||||
lambdaTemp = contact.friction1Impulse;
|
||||
contact.friction1Impulse = std::max(contact.lowerBoundFriction1, std::min(contact.friction1Impulse + deltaLambda, contact.upperBoundFriction1));
|
||||
contact.friction1Impulse = std::max(-frictionLimit, std::min(contact.friction1Impulse + deltaLambda, frictionLimit));
|
||||
deltaLambda = contact.friction1Impulse - lambdaTemp;
|
||||
|
||||
// Compute the impulse P=J^T * lambda
|
||||
|
|
@ -367,9 +380,10 @@ void ConstraintSolver::solveContactConstraints() {
|
|||
Jv = deltaV.dot(contact.frictionVector2);
|
||||
|
||||
deltaLambda = -Jv;
|
||||
deltaLambda /= contact.inverseFriction2Mass;
|
||||
deltaLambda *= contact.inverseFriction2Mass;
|
||||
frictionLimit = 0.3 * contact.penetrationImpulse; // TODO : Use constant here
|
||||
lambdaTemp = contact.friction2Impulse;
|
||||
contact.friction2Impulse = std::max(contact.lowerBoundFriction2, std::min(contact.friction2Impulse + deltaLambda, contact.upperBoundFriction2));
|
||||
contact.friction2Impulse = std::max(-frictionLimit, std::min(contact.friction2Impulse + deltaLambda, frictionLimit));
|
||||
deltaLambda = contact.friction2Impulse - lambdaTemp;
|
||||
|
||||
// Compute the impulse P=J^T * lambda
|
||||
|
|
@ -387,26 +401,6 @@ void ConstraintSolver::solveContactConstraints() {
|
|||
}
|
||||
}
|
||||
|
||||
// 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; c<mNbContactConstraints; c++) {
|
||||
|
||||
ContactConstraint& constraint = mContactConstraints[c];
|
||||
|
||||
for (uint i=0; i<constraint.nbContacts; i++) {
|
||||
|
||||
ContactPointConstraint& contact = constraint.contacts[i];
|
||||
|
||||
contact.contact->setCachedLambda(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; c<mNbContactConstraints; c++) {
|
||||
|
||||
ContactConstraint& constraint = mContactConstraints[c];
|
||||
|
||||
for (uint i=0; i<constraint.nbContacts; i++) {
|
||||
|
||||
ContactPointConstraint& contact = constraint.contacts[i];
|
||||
|
||||
contact.contact->setCachedLambda(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();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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<RigidBody*, uint> 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 -------------------- //
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user