Make possible to solve the friction constraints at the center of the contact manifold
This commit is contained in:
Daniel Chappuis
parent
8cde68f5b9
commit
0695b30704
|
|
@ -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; c<contactManifold.nbContacts; c++) {
|
||||
|
|
@ -108,6 +120,36 @@ void ConstraintSolver::initialize() {
|
|||
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++;
|
||||
|
|
@ -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<constraint.nbContacts; i++) {
|
||||
|
||||
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);
|
||||
contact.r1CrossT2 = contact.r1.cross(contact.frictionVector2);
|
||||
contact.r2CrossT1 = contact.r2.cross(contact.frictionVector1);
|
||||
contact.r2CrossT2 = contact.r2.cross(contact.frictionVector2);
|
||||
|
||||
decimal massPenetration = 0.0;
|
||||
if (constraint.isBody1Moving) {
|
||||
|
|
@ -187,19 +228,30 @@ void ConstraintSolver::initializeContactConstraints() {
|
|||
}
|
||||
massPenetration > 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<constraint.nbContacts; i++) {
|
||||
|
||||
ContactPointConstraint& contact = constraint.contacts[i];
|
||||
|
|
@ -241,6 +333,8 @@ void ConstraintSolver::warmStart() {
|
|||
// If it is not a new contact (this contact was already existing at last time step)
|
||||
if (contact.isRestingContact) {
|
||||
|
||||
atLeastOneRestingContactPoint = true;
|
||||
|
||||
// --------- Penetration --------- //
|
||||
|
||||
// Compute the impulse P=J^T * lambda
|
||||
|
|
@ -254,38 +348,44 @@ void ConstraintSolver::warmStart() {
|
|||
// Apply the impulse to the bodies of the constraint
|
||||
applyImpulse(impulsePenetration, constraint);
|
||||
|
||||
// --------- Friction 1 --------- //
|
||||
// If we do not solve the friction constraints at the center of the contact manifold
|
||||
if (!mIsSolveFrictionAtContactManifoldCenterActive) {
|
||||
|
||||
Vector3 oldFrictionImpulse = contact.friction1Impulse * contact.oldFrictionVector1 +
|
||||
contact.friction2Impulse * contact.oldFrictionVector2;
|
||||
contact.friction1Impulse = oldFrictionImpulse.dot(contact.frictionVector1);
|
||||
contact.friction2Impulse = oldFrictionImpulse.dot(contact.frictionVector2);
|
||||
// Project the old friction impulses (with old friction vectors) into the new friction
|
||||
// vectors to get the new friction impulses
|
||||
Vector3 oldFrictionImpulse = contact.friction1Impulse * contact.oldFrictionVector1 +
|
||||
contact.friction2Impulse * contact.oldFrictionVector2;
|
||||
contact.friction1Impulse = oldFrictionImpulse.dot(contact.frictionVector1);
|
||||
contact.friction2Impulse = oldFrictionImpulse.dot(contact.frictionVector2);
|
||||
|
||||
// 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);
|
||||
// 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 2 --------- //
|
||||
// Apply the impulses to the bodies of the constraint
|
||||
applyImpulse(impulseFriction1, constraint);
|
||||
|
||||
// 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);
|
||||
// --------- Friction 2 --------- //
|
||||
|
||||
// Apply the impulses to the bodies of the constraint
|
||||
applyImpulse(impulseFriction2, constraint);
|
||||
// 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
|
||||
else { // If it is a new contact point
|
||||
|
||||
// Initialize the accumulated impulses to zero
|
||||
contact.penetrationImpulse = 0.0;
|
||||
|
|
@ -293,6 +393,65 @@ void ConstraintSolver::warmStart() {
|
|||
contact.friction2Impulse = 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
// If we solve the friction constraints at the center of the contact manifold and there is
|
||||
// at least one resting contact point in the contact manifold
|
||||
if (mIsSolveFrictionAtContactManifoldCenterActive && atLeastOneRestingContactPoint) {
|
||||
|
||||
// Project the old friction impulses (with old friction vectors) into the new friction
|
||||
// vectors to get the new friction impulses
|
||||
Vector3 oldFrictionImpulse = constraint.friction1Impulse * constraint.oldFrictionVector1 +
|
||||
constraint.friction2Impulse * constraint.oldFrictionVector2;
|
||||
constraint.friction1Impulse = oldFrictionImpulse.dot(constraint.frictionVector1);
|
||||
constraint.friction2Impulse = oldFrictionImpulse.dot(constraint.frictionVector2);
|
||||
|
||||
// ------ First friction constraint at the center of the contact manifol ------ //
|
||||
|
||||
// Compute the impulse P=J^T * lambda
|
||||
Vector3 linearImpulseBody1 = -constraint.frictionVector1 * constraint.friction1Impulse;
|
||||
Vector3 angularImpulseBody1 = -constraint.r1CrossT1 * constraint.friction1Impulse;
|
||||
Vector3 linearImpulseBody2 = constraint.frictionVector1 * constraint.friction1Impulse;
|
||||
Vector3 angularImpulseBody2 = constraint.r2CrossT1 * constraint.friction1Impulse;
|
||||
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 the impulse P=J^T * lambda
|
||||
linearImpulseBody1 = -constraint.frictionVector2 * constraint.friction2Impulse;
|
||||
angularImpulseBody1 = -constraint.r1CrossT2 * constraint.friction2Impulse;
|
||||
linearImpulseBody2 = constraint.frictionVector2 * constraint.friction2Impulse;
|
||||
angularImpulseBody2 = constraint.r2CrossT2 * constraint.friction2Impulse;
|
||||
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 ------ //
|
||||
|
||||
|
||||
// Compute the impulse P=J^T * lambda
|
||||
linearImpulseBody1 = Vector3(0.0, 0.0, 0.0);
|
||||
angularImpulseBody1 = -constraint.normal * constraint.frictionTwistImpulse;
|
||||
linearImpulseBody2 = Vector3(0.0, 0.0, 0.0);
|
||||
angularImpulseBody2 = constraint.normal * constraint.frictionTwistImpulse;
|
||||
const Impulse impulseTwistFriction(linearImpulseBody1, angularImpulseBody1,
|
||||
linearImpulseBody2, angularImpulseBody2);
|
||||
|
||||
// Apply the impulses to the bodies of the constraint
|
||||
applyImpulse(impulseTwistFriction, constraint);
|
||||
}
|
||||
else { // If it is a new contact manifold
|
||||
|
||||
// Initialize the accumulated impulses to zero
|
||||
constraint.friction1Impulse = 0.0;
|
||||
constraint.friction2Impulse = 0.0;
|
||||
constraint.frictionTwistImpulse = 0.0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -311,15 +470,17 @@ void ConstraintSolver::solveContactConstraints() {
|
|||
|
||||
ContactConstraint& constraint = mContactConstraints[c];
|
||||
|
||||
decimal sumPenetrationImpulse = 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 (uint i=0; i<constraint.nbContacts; i++) {
|
||||
|
||||
ContactPointConstraint& contact = constraint.contacts[i];
|
||||
|
||||
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
|
||||
|
|
@ -328,12 +489,10 @@ void ConstraintSolver::solveContactConstraints() {
|
|||
decimal Jv = deltaVDotN;
|
||||
|
||||
// Compute the bias "b" of the constraint
|
||||
decimal beta = 0.2;
|
||||
// TODO : Use a constant for the slop
|
||||
decimal slop = 0.01;
|
||||
decimal beta = mIsSplitImpulseActive ? BETA_SPLIT_IMPULSE : BETA;
|
||||
decimal biasPenetrationDepth = 0.0;
|
||||
if (contact.penetrationDepth > 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();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -28,6 +28,7 @@
|
|||
|
||||
// Libraries
|
||||
#include "../constraint/Contact.h"
|
||||
#include "ContactManifold.h"
|
||||
#include "../configuration.h"
|
||||
#include "../constraint/Constraint.h"
|
||||
#include <map>
|
||||
|
|
@ -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 {
|
||||
|
|
|
|||
|
|
@ -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) {}
|
||||
};
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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() {
|
||||
|
|
|
|||
|
|
@ -26,7 +26,6 @@
|
|||
// Libraries
|
||||
#include "Vector3.h"
|
||||
#include <iostream>
|
||||
#include <cassert>
|
||||
#include <vector>
|
||||
|
||||
// Namespaces
|
||||
|
|
|
|||
|
|
@ -28,6 +28,7 @@
|
|||
|
||||
// Libraries
|
||||
#include <cmath>
|
||||
#include <cassert>
|
||||
#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];
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user