Continue to transform PGS solver into the sequential impulse solver
This commit is contained in:
parent
e4d47ded09
commit
1bcec415a1
|
@ -34,7 +34,8 @@ using namespace std;
|
||||||
|
|
||||||
// Constructor
|
// Constructor
|
||||||
ConstraintSolver::ConstraintSolver(DynamicsWorld* world)
|
ConstraintSolver::ConstraintSolver(DynamicsWorld* world)
|
||||||
:world(world), nbConstraints(0), mNbIterations(10), mContactConstraints(0), Vconstraint(0), Wconstraint(0), V1(0), W1(0) {
|
:world(world), nbConstraints(0), mNbIterations(10), mContactConstraints(0),
|
||||||
|
mLinearVelocities(0), mAngularVelocities(0) {
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -45,7 +46,7 @@ ConstraintSolver::~ConstraintSolver() {
|
||||||
|
|
||||||
// Initialize the constraint solver before each solving
|
// Initialize the constraint solver before each solving
|
||||||
void ConstraintSolver::initialize() {
|
void ConstraintSolver::initialize() {
|
||||||
|
|
||||||
nbConstraints = 0;
|
nbConstraints = 0;
|
||||||
|
|
||||||
// TOOD : Use better allocation here
|
// TOOD : Use better allocation here
|
||||||
|
@ -111,10 +112,8 @@ void ConstraintSolver::initialize() {
|
||||||
// Compute the number of bodies that are part of some active constraint
|
// Compute the number of bodies that are part of some active constraint
|
||||||
nbBodies = mConstraintBodies.size();
|
nbBodies = mConstraintBodies.size();
|
||||||
|
|
||||||
Vconstraint = new Vector3[nbBodies];
|
mLinearVelocities = new Vector3[nbBodies];
|
||||||
Wconstraint = new Vector3[nbBodies];
|
mAngularVelocities = new Vector3[nbBodies];
|
||||||
V1 = new Vector3[nbBodies];
|
|
||||||
W1 = new Vector3[nbBodies];
|
|
||||||
|
|
||||||
assert(mMapBodyToIndex.size() == nbBodies);
|
assert(mMapBodyToIndex.size() == nbBodies);
|
||||||
}
|
}
|
||||||
|
@ -135,12 +134,9 @@ void ConstraintSolver::initializeBodies() {
|
||||||
|
|
||||||
// Compute the vector V1 with initial velocities values
|
// Compute the vector V1 with initial velocities values
|
||||||
int bodyIndexArray = 6 * bodyNumber;
|
int bodyIndexArray = 6 * bodyNumber;
|
||||||
V1[bodyNumber] = rigidBody->getLinearVelocity();
|
|
||||||
W1[bodyNumber] = rigidBody->getAngularVelocity();
|
|
||||||
|
|
||||||
// Compute the vector Vconstraint with final constraint velocities
|
mLinearVelocities[bodyNumber] = rigidBody->getLinearVelocity() + mTimeStep * rigidBody->getMassInverse() * rigidBody->getExternalForce();
|
||||||
Vconstraint[bodyNumber] = Vector3(0, 0, 0);
|
mAngularVelocities[bodyNumber] = rigidBody->getAngularVelocity() + mTimeStep * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque();
|
||||||
Wconstraint[bodyNumber] = Vector3(0, 0, 0);
|
|
||||||
|
|
||||||
// Initialize the mass and inertia tensor matrices
|
// Initialize the mass and inertia tensor matrices
|
||||||
Minv_sp_inertia[bodyNumber].setAllValues(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
|
Minv_sp_inertia[bodyNumber].setAllValues(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
|
||||||
|
@ -225,8 +221,6 @@ void ConstraintSolver::initializeContactConstraints(decimal dt) {
|
||||||
|
|
||||||
// Fill in the error vector
|
// Fill in the error vector
|
||||||
realContact->computeErrorPenetration(contact.errorPenetration);
|
realContact->computeErrorPenetration(contact.errorPenetration);
|
||||||
contact.errorFriction1 = 0.0;
|
|
||||||
contact.errorFriction2 = 0.0;
|
|
||||||
|
|
||||||
// Get the cached lambda values of the constraint
|
// Get the cached lambda values of the constraint
|
||||||
contact.penetrationImpulse = realContact->getCachedLambda(0);
|
contact.penetrationImpulse = realContact->getCachedLambda(0);
|
||||||
|
@ -239,27 +233,15 @@ void ConstraintSolver::initializeContactConstraints(decimal dt) {
|
||||||
contact.errorPenetration = 0.0;
|
contact.errorPenetration = 0.0;
|
||||||
decimal slop = 0.005;
|
decimal slop = 0.005;
|
||||||
if (realContact->getPenetrationDepth() > slop) {
|
if (realContact->getPenetrationDepth() > slop) {
|
||||||
contact.errorPenetration += 0.2 * oneOverDT * std::max(double(realContact->getPenetrationDepth() - slop), 0.0);
|
contact.errorPenetration -= 0.2 * oneOverDT * std::max(double(realContact->getPenetrationDepth() - slop), 0.0);
|
||||||
}
|
}
|
||||||
contact.errorFriction1 = 0.0;
|
|
||||||
contact.errorFriction2 = 0.0;
|
|
||||||
|
|
||||||
// ---------- Penetration ---------- //
|
// ---------- Penetration ---------- //
|
||||||
|
|
||||||
// b = errorValues * oneOverDT;
|
// b = errorValues * oneOverDT;
|
||||||
contact.b_Penetration = contact.errorPenetration * oneOverDT;
|
contact.b_Penetration = contact.errorPenetration;
|
||||||
|
|
||||||
// ---------- Friction 1 ---------- //
|
|
||||||
|
|
||||||
contact.b_Friction1 = contact.errorFriction1 * oneOverDT;
|
|
||||||
|
|
||||||
// ---------- Friction 2 ---------- //
|
|
||||||
|
|
||||||
contact.b_Friction2 = contact.errorFriction2 * oneOverDT;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Compute the matrix B_sp
|
// Compute the matrix B_sp
|
||||||
|
@ -373,67 +355,9 @@ void ConstraintSolver::computeMatrixB_sp() {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// Compute the vector V_constraint (which corresponds to the constraint part of
|
|
||||||
// the final V2 vector) according to the formula
|
|
||||||
// V_constraint = dt * (M^-1 * J^T * lambda)
|
|
||||||
// Note that we use the vector V to store both V1 and V_constraint.
|
|
||||||
// Note that M^-1 * J^T = B.
|
|
||||||
// This method is called after that the LCP solver has computed lambda
|
|
||||||
void ConstraintSolver::computeVectorVconstraint(decimal dt) {
|
|
||||||
uint indexBody1Array, indexBody2Array;
|
|
||||||
uint j;
|
|
||||||
|
|
||||||
// Compute dt * (M^-1 * J^T * lambda
|
|
||||||
for (uint c=0; c<mNbContactConstraints; c++) {
|
|
||||||
|
|
||||||
ContactConstraint& constraint = mContactConstraints[c];
|
|
||||||
|
|
||||||
for (uint i=0; i<constraint.nbContacts; i++) {
|
|
||||||
|
|
||||||
ContactPointConstraint& contact = constraint.contacts[i];
|
|
||||||
|
|
||||||
// ---------- Penetration ---------- //
|
|
||||||
|
|
||||||
indexBody1Array = constraint.indexBody1;
|
|
||||||
indexBody2Array = constraint.indexBody2;
|
|
||||||
for (j=0; j<3; j++) {
|
|
||||||
Vconstraint[indexBody1Array][j] += contact.B_spBody1Penetration[j] * contact.penetrationImpulse * dt;
|
|
||||||
Wconstraint[indexBody1Array][j] += contact.B_spBody1Penetration[j + 3] * contact.penetrationImpulse * dt;
|
|
||||||
|
|
||||||
Vconstraint[indexBody2Array][j] += contact.B_spBody2Penetration[j] * contact.penetrationImpulse * dt;
|
|
||||||
Wconstraint[indexBody2Array][j] += contact.B_spBody2Penetration[j + 3] * contact.penetrationImpulse * dt;
|
|
||||||
}
|
|
||||||
|
|
||||||
// ---------- Friction 1 ---------- //
|
|
||||||
|
|
||||||
for (j=0; j<3; j++) {
|
|
||||||
Vconstraint[indexBody1Array][j] += contact.B_spBody1Friction1[j] * contact.friction1Impulse * dt;
|
|
||||||
Wconstraint[indexBody1Array][j] += contact.B_spBody1Friction1[j + 3] * contact.friction1Impulse * dt;
|
|
||||||
|
|
||||||
Vconstraint[indexBody2Array][j] += contact.B_spBody2Friction1[j] * contact.friction1Impulse * dt;
|
|
||||||
Wconstraint[indexBody2Array][j] += contact.B_spBody2Friction1[j + 3] * contact.friction1Impulse * dt;
|
|
||||||
}
|
|
||||||
|
|
||||||
// ---------- Friction 2 ---------- //
|
|
||||||
|
|
||||||
for (j=0; j<3; j++) {
|
|
||||||
Vconstraint[indexBody1Array][j] += contact.B_spBody1Friction2[j] * contact.friction2Impulse * dt;
|
|
||||||
Wconstraint[indexBody1Array][j] += contact.B_spBody1Friction2[j + 3] * contact.friction2Impulse * dt;
|
|
||||||
|
|
||||||
Vconstraint[indexBody2Array][j] += contact.B_spBody2Friction2[j] * contact.friction2Impulse * dt;
|
|
||||||
Wconstraint[indexBody2Array][j] += contact.B_spBody2Friction2[j + 3] * contact.friction2Impulse * dt;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Solve a LCP problem using the Projected-Gauss-Seidel algorithm
|
// Solve a LCP problem using the Projected-Gauss-Seidel algorithm
|
||||||
// This method outputs the result in the lambda vector
|
// This method outputs the result in the lambda vector
|
||||||
void ConstraintSolver::solveLCP(decimal dt) {
|
void ConstraintSolver::solveLCP() {
|
||||||
|
|
||||||
// for (uint i=0; i<nbConstraints; i++) {
|
|
||||||
// lambda[i] = lambdaInit[i];
|
|
||||||
// }
|
|
||||||
|
|
||||||
uint indexBody1Array, indexBody2Array;
|
uint indexBody1Array, indexBody2Array;
|
||||||
decimal deltaLambda;
|
decimal deltaLambda;
|
||||||
|
@ -441,7 +365,7 @@ void ConstraintSolver::solveLCP(decimal dt) {
|
||||||
uint iter;
|
uint iter;
|
||||||
|
|
||||||
// Compute the vector a
|
// Compute the vector a
|
||||||
computeVectorA(dt);
|
warmStart();
|
||||||
|
|
||||||
// For each iteration
|
// For each iteration
|
||||||
for(iter=0; iter<mNbIterations; iter++) {
|
for(iter=0; iter<mNbIterations; iter++) {
|
||||||
|
@ -460,59 +384,60 @@ void ConstraintSolver::solveLCP(decimal dt) {
|
||||||
|
|
||||||
// --------- Penetration --------- //
|
// --------- Penetration --------- //
|
||||||
|
|
||||||
deltaLambda = contact.b_Penetration;
|
deltaLambda = 0.0;
|
||||||
for (uint j=0; j<3; j++) {
|
for (uint j=0; j<3; j++) {
|
||||||
deltaLambda -= (contact.J_spBody1Penetration[j] * aLinear[indexBody1Array][j] + contact.J_spBody2Penetration[j] * aLinear[indexBody2Array][j]);
|
deltaLambda -= (contact.J_spBody1Penetration[j] * mLinearVelocities[indexBody1Array][j] + contact.J_spBody2Penetration[j] * mLinearVelocities[indexBody2Array][j]);
|
||||||
deltaLambda -= (contact.J_spBody1Penetration[j + 3] * aAngular[indexBody1Array][j] + contact.J_spBody2Penetration[j + 3] * aAngular[indexBody2Array][j]);
|
deltaLambda -= (contact.J_spBody1Penetration[j + 3] * mAngularVelocities[indexBody1Array][j] + contact.J_spBody2Penetration[j + 3] * mAngularVelocities[indexBody2Array][j]);
|
||||||
}
|
}
|
||||||
|
deltaLambda -= contact.b_Penetration;
|
||||||
deltaLambda /= contact.inversePenetrationMass;
|
deltaLambda /= contact.inversePenetrationMass;
|
||||||
lambdaTemp = contact.penetrationImpulse;
|
lambdaTemp = contact.penetrationImpulse;
|
||||||
contact.penetrationImpulse = std::max(contact.lowerBoundPenetration, std::min(contact.penetrationImpulse + deltaLambda, contact.upperBoundPenetration));
|
contact.penetrationImpulse = std::max(contact.lowerBoundPenetration, std::min(contact.penetrationImpulse + deltaLambda, contact.upperBoundPenetration));
|
||||||
deltaLambda = contact.penetrationImpulse - lambdaTemp;
|
deltaLambda = contact.penetrationImpulse - lambdaTemp;
|
||||||
for (uint j=0; j<3; j++) {
|
for (uint j=0; j<3; j++) {
|
||||||
aLinear[indexBody1Array][j] += contact.B_spBody1Penetration[j] * deltaLambda;
|
mLinearVelocities[indexBody1Array][j] += contact.B_spBody1Penetration[j] * deltaLambda;
|
||||||
aAngular[indexBody1Array][j] += contact.B_spBody1Penetration[j + 3] * deltaLambda;
|
mAngularVelocities[indexBody1Array][j] += contact.B_spBody1Penetration[j + 3] * deltaLambda;
|
||||||
|
|
||||||
aLinear[indexBody2Array][j] += contact.B_spBody2Penetration[j] * deltaLambda;
|
mLinearVelocities[indexBody2Array][j] += contact.B_spBody2Penetration[j] * deltaLambda;
|
||||||
aAngular[indexBody2Array][j] += contact.B_spBody2Penetration[j + 3] * deltaLambda;
|
mAngularVelocities[indexBody2Array][j] += contact.B_spBody2Penetration[j + 3] * deltaLambda;
|
||||||
}
|
}
|
||||||
|
|
||||||
// --------- Friction 1 --------- //
|
// --------- Friction 1 --------- //
|
||||||
|
|
||||||
deltaLambda = contact.b_Friction1;
|
deltaLambda = 0.0;
|
||||||
for (uint j=0; j<3; j++) {
|
for (uint j=0; j<3; j++) {
|
||||||
deltaLambda -= (contact.J_spBody1Friction1[j] * aLinear[indexBody1Array][j] + contact.J_spBody2Friction1[j] * aLinear[indexBody2Array][j]);
|
deltaLambda -= (contact.J_spBody1Friction1[j] * mLinearVelocities[indexBody1Array][j] + contact.J_spBody2Friction1[j] * mLinearVelocities[indexBody2Array][j]);
|
||||||
deltaLambda -= (contact.J_spBody1Friction1[j + 3] * aAngular[indexBody1Array][j] + contact.J_spBody2Friction1[j + 3] * aAngular[indexBody2Array][j]);
|
deltaLambda -= (contact.J_spBody1Friction1[j + 3] * mAngularVelocities[indexBody1Array][j] + contact.J_spBody2Friction1[j + 3] * mAngularVelocities[indexBody2Array][j]);
|
||||||
}
|
}
|
||||||
deltaLambda /= contact.inverseFriction1Mass;
|
deltaLambda /= contact.inverseFriction1Mass;
|
||||||
lambdaTemp = contact.friction1Impulse;
|
lambdaTemp = contact.friction1Impulse;
|
||||||
contact.friction1Impulse = std::max(contact.lowerBoundFriction1, std::min(contact.friction1Impulse + deltaLambda, contact.upperBoundFriction1));
|
contact.friction1Impulse = std::max(contact.lowerBoundFriction1, std::min(contact.friction1Impulse + deltaLambda, contact.upperBoundFriction1));
|
||||||
deltaLambda = contact.friction1Impulse - lambdaTemp;
|
deltaLambda = contact.friction1Impulse - lambdaTemp;
|
||||||
for (uint j=0; j<3; j++) {
|
for (uint j=0; j<3; j++) {
|
||||||
aLinear[indexBody1Array][j] += contact.B_spBody1Friction1[j] * deltaLambda;
|
mLinearVelocities[indexBody1Array][j] += contact.B_spBody1Friction1[j] * deltaLambda;
|
||||||
aAngular[indexBody1Array][j] += contact.B_spBody1Friction1[j + 3] * deltaLambda;
|
mAngularVelocities[indexBody1Array][j] += contact.B_spBody1Friction1[j + 3] * deltaLambda;
|
||||||
|
|
||||||
aLinear[indexBody2Array][j] += contact.B_spBody2Friction1[j] * deltaLambda;
|
mLinearVelocities[indexBody2Array][j] += contact.B_spBody2Friction1[j] * deltaLambda;
|
||||||
aAngular[indexBody2Array][j] += contact.B_spBody2Friction1[j + 3] * deltaLambda;
|
mAngularVelocities[indexBody2Array][j] += contact.B_spBody2Friction1[j + 3] * deltaLambda;
|
||||||
}
|
}
|
||||||
|
|
||||||
// --------- Friction 2 --------- //
|
// --------- Friction 2 --------- //
|
||||||
|
|
||||||
deltaLambda = contact.b_Friction2;
|
deltaLambda = 0.0;
|
||||||
for (uint j=0; j<3; j++) {
|
for (uint j=0; j<3; j++) {
|
||||||
deltaLambda -= (contact.J_spBody1Friction2[j] * aLinear[indexBody1Array][j] + contact.J_spBody2Friction2[j] * aLinear[indexBody2Array][j]);
|
deltaLambda -= (contact.J_spBody1Friction2[j] * mLinearVelocities[indexBody1Array][j] + contact.J_spBody2Friction2[j] * mLinearVelocities[indexBody2Array][j]);
|
||||||
deltaLambda -= (contact.J_spBody1Friction2[j + 3] * aAngular[indexBody1Array][j] + contact.J_spBody2Friction2[j + 3] * aAngular[indexBody2Array][j]);
|
deltaLambda -= (contact.J_spBody1Friction2[j + 3] * mAngularVelocities[indexBody1Array][j] + contact.J_spBody2Friction2[j + 3] * mAngularVelocities[indexBody2Array][j]);
|
||||||
}
|
}
|
||||||
deltaLambda /= contact.inverseFriction2Mass;
|
deltaLambda /= contact.inverseFriction2Mass;
|
||||||
lambdaTemp = contact.friction2Impulse;
|
lambdaTemp = contact.friction2Impulse;
|
||||||
contact.friction2Impulse = std::max(contact.lowerBoundFriction2, std::min(contact.friction2Impulse + deltaLambda, contact.upperBoundFriction2));
|
contact.friction2Impulse = std::max(contact.lowerBoundFriction2, std::min(contact.friction2Impulse + deltaLambda, contact.upperBoundFriction2));
|
||||||
deltaLambda = contact.friction2Impulse - lambdaTemp;
|
deltaLambda = contact.friction2Impulse - lambdaTemp;
|
||||||
for (uint j=0; j<3; j++) {
|
for (uint j=0; j<3; j++) {
|
||||||
aLinear[indexBody1Array][j] += contact.B_spBody1Friction2[j] * deltaLambda;
|
mLinearVelocities[indexBody1Array][j] += contact.B_spBody1Friction2[j] * deltaLambda;
|
||||||
aAngular[indexBody1Array][j] += contact.B_spBody1Friction2[j + 3] * deltaLambda;
|
mAngularVelocities[indexBody1Array][j] += contact.B_spBody1Friction2[j + 3] * deltaLambda;
|
||||||
|
|
||||||
aLinear[indexBody2Array][j] += contact.B_spBody2Friction2[j] * deltaLambda;
|
mLinearVelocities[indexBody2Array][j] += contact.B_spBody2Friction2[j] * deltaLambda;
|
||||||
aAngular[indexBody2Array][j] += contact.B_spBody2Friction2[j + 3] * deltaLambda;
|
mAngularVelocities[indexBody2Array][j] += contact.B_spBody2Friction2[j + 3] * deltaLambda;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -521,19 +446,9 @@ void ConstraintSolver::solveLCP(decimal dt) {
|
||||||
|
|
||||||
// Compute the vector a used in the solve() method
|
// Compute the vector a used in the solve() method
|
||||||
// Note that a = B * lambda
|
// Note that a = B * lambda
|
||||||
void ConstraintSolver::computeVectorA(decimal dt) {
|
void ConstraintSolver::warmStart() {
|
||||||
uint i;
|
uint i;
|
||||||
uint indexBody1Array, indexBody2Array;
|
uint indexBody1Array, indexBody2Array;
|
||||||
decimal oneOverDt = 1.0 / dt;
|
|
||||||
|
|
||||||
// Init the vector a with zero values
|
|
||||||
for (set<RigidBody*>::iterator it = mConstraintBodies.begin(); it != mConstraintBodies.end(); ++it) {
|
|
||||||
RigidBody* rigidBody = *it;
|
|
||||||
uint bodyNumber = mMapBodyToIndex[rigidBody];
|
|
||||||
|
|
||||||
aLinear[bodyNumber] = oneOverDt * V1[bodyNumber] + rigidBody->getMassInverse() * rigidBody->getExternalForce();
|
|
||||||
aAngular[bodyNumber] = oneOverDt * W1[bodyNumber] + rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque();
|
|
||||||
}
|
|
||||||
|
|
||||||
// For each constraint
|
// For each constraint
|
||||||
for (uint c=0; c<mNbContactConstraints; c++) {
|
for (uint c=0; c<mNbContactConstraints; c++) {
|
||||||
|
@ -550,31 +465,31 @@ void ConstraintSolver::computeVectorA(decimal dt) {
|
||||||
// --------- Penetration --------- //
|
// --------- Penetration --------- //
|
||||||
|
|
||||||
for (uint j=0; j<3; j++) {
|
for (uint j=0; j<3; j++) {
|
||||||
aLinear[indexBody1Array][j] += contact.B_spBody1Penetration[j] * contact.penetrationImpulse;
|
mLinearVelocities[indexBody1Array][j] += contact.B_spBody1Penetration[j] * contact.penetrationImpulse;
|
||||||
aAngular[indexBody1Array][j] += contact.B_spBody1Penetration[j + 3] * contact.penetrationImpulse;
|
mAngularVelocities[indexBody1Array][j] += contact.B_spBody1Penetration[j + 3] * contact.penetrationImpulse;
|
||||||
|
|
||||||
aLinear[indexBody2Array][j] += contact.B_spBody2Penetration[j] * contact.penetrationImpulse;
|
mLinearVelocities[indexBody2Array][j] += contact.B_spBody2Penetration[j] * contact.penetrationImpulse;
|
||||||
aAngular[indexBody2Array][j] += contact.B_spBody2Penetration[j + 3] * contact.penetrationImpulse;
|
mAngularVelocities[indexBody2Array][j] += contact.B_spBody2Penetration[j + 3] * contact.penetrationImpulse;
|
||||||
}
|
}
|
||||||
|
|
||||||
// --------- Friction 1 --------- //
|
// --------- Friction 1 --------- //
|
||||||
|
|
||||||
for (uint j=0; j<3; j++) {
|
for (uint j=0; j<3; j++) {
|
||||||
aLinear[indexBody1Array][j] += contact.B_spBody1Friction1[j] * contact.friction1Impulse;
|
mLinearVelocities[indexBody1Array][j] += contact.B_spBody1Friction1[j] * contact.friction1Impulse;
|
||||||
aAngular[indexBody1Array][j] += contact.B_spBody1Friction1[j + 3] * contact.friction1Impulse;
|
mAngularVelocities[indexBody1Array][j] += contact.B_spBody1Friction1[j + 3] * contact.friction1Impulse;
|
||||||
|
|
||||||
aLinear[indexBody2Array][j] += contact.B_spBody2Friction1[j] * contact.friction1Impulse;
|
mLinearVelocities[indexBody2Array][j] += contact.B_spBody2Friction1[j] * contact.friction1Impulse;
|
||||||
aAngular[indexBody2Array][j] += contact.B_spBody2Friction1[j + 3] * contact.friction1Impulse;
|
mAngularVelocities[indexBody2Array][j] += contact.B_spBody2Friction1[j + 3] * contact.friction1Impulse;
|
||||||
}
|
}
|
||||||
|
|
||||||
// --------- Friction 2 --------- //
|
// --------- Friction 2 --------- //
|
||||||
|
|
||||||
for (uint j=0; j<3; j++) {
|
for (uint j=0; j<3; j++) {
|
||||||
aLinear[indexBody1Array][j] += contact.B_spBody1Friction2[j] * contact.friction2Impulse;
|
mLinearVelocities[indexBody1Array][j] += contact.B_spBody1Friction2[j] * contact.friction2Impulse;
|
||||||
aAngular[indexBody1Array][j] += contact.B_spBody1Friction2[j + 3] * contact.friction2Impulse;
|
mAngularVelocities[indexBody1Array][j] += contact.B_spBody1Friction2[j + 3] * contact.friction2Impulse;
|
||||||
|
|
||||||
aLinear[indexBody2Array][j] += contact.B_spBody2Friction2[j] * contact.friction2Impulse;
|
mLinearVelocities[indexBody2Array][j] += contact.B_spBody2Friction2[j] * contact.friction2Impulse;
|
||||||
aAngular[indexBody2Array][j] += contact.B_spBody2Friction2[j + 3] * contact.friction2Impulse;
|
mAngularVelocities[indexBody2Array][j] += contact.B_spBody2Friction2[j + 3] * contact.friction2Impulse;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -599,3 +514,26 @@ void ConstraintSolver::cacheLambda() {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Solve the current LCP problem
|
||||||
|
void ConstraintSolver::solve(decimal dt) {
|
||||||
|
|
||||||
|
mTimeStep = dt;
|
||||||
|
|
||||||
|
// Initialize the solver
|
||||||
|
initialize();
|
||||||
|
|
||||||
|
initializeBodies();
|
||||||
|
|
||||||
|
// Fill-in all the matrices needed to solve the LCP problem
|
||||||
|
initializeContactConstraints(dt);
|
||||||
|
|
||||||
|
// Compute the matrix B
|
||||||
|
computeMatrixB_sp();
|
||||||
|
|
||||||
|
// Solve the LCP problem (computation of lambda)
|
||||||
|
solveLCP();
|
||||||
|
|
||||||
|
// Cache the lambda values in order to use them in the next step
|
||||||
|
cacheLambda();
|
||||||
|
}
|
||||||
|
|
|
@ -77,12 +77,8 @@ struct ContactPointConstraint {
|
||||||
decimal lowerBoundFriction2;
|
decimal lowerBoundFriction2;
|
||||||
decimal upperBoundFriction2;
|
decimal upperBoundFriction2;
|
||||||
decimal errorPenetration;
|
decimal errorPenetration;
|
||||||
decimal errorFriction1;
|
|
||||||
decimal errorFriction2;
|
|
||||||
Contact* contact; // TODO : REMOVE THIS
|
Contact* contact; // TODO : REMOVE THIS
|
||||||
decimal b_Penetration;
|
decimal b_Penetration;
|
||||||
decimal b_Friction1;
|
|
||||||
decimal b_Friction2;
|
|
||||||
decimal B_spBody1Penetration[6];
|
decimal B_spBody1Penetration[6];
|
||||||
decimal B_spBody2Penetration[6];
|
decimal B_spBody2Penetration[6];
|
||||||
decimal B_spBody1Friction1[6];
|
decimal B_spBody1Friction1[6];
|
||||||
|
@ -160,8 +156,6 @@ class ConstraintSolver {
|
||||||
decimal b[NB_MAX_CONSTRAINTS]; // Vector "b" of the LCP problem
|
decimal b[NB_MAX_CONSTRAINTS]; // Vector "b" of the LCP problem
|
||||||
decimal bError[NB_MAX_CONSTRAINTS]; // Vector "b" of the LCP problem for error correction projection
|
decimal bError[NB_MAX_CONSTRAINTS]; // Vector "b" of the LCP problem for error correction projection
|
||||||
decimal d[NB_MAX_CONSTRAINTS]; // Vector "d"
|
decimal d[NB_MAX_CONSTRAINTS]; // Vector "d"
|
||||||
Vector3 aLinear[NB_MAX_BODIES]; // Vector "a"
|
|
||||||
Vector3 aAngular[NB_MAX_BODIES];
|
|
||||||
decimal aError[6*NB_MAX_BODIES]; // Vector "a" for error correction projection
|
decimal aError[6*NB_MAX_BODIES]; // Vector "a" for error correction projection
|
||||||
decimal penetrationDepths[NB_MAX_CONSTRAINTS]; // Array of penetration depths for error correction projection
|
decimal penetrationDepths[NB_MAX_CONSTRAINTS]; // Array of penetration depths for error correction projection
|
||||||
decimal lambda[NB_MAX_CONSTRAINTS]; // Lambda vector of the LCP problem
|
decimal lambda[NB_MAX_CONSTRAINTS]; // Lambda vector of the LCP problem
|
||||||
|
@ -175,11 +169,9 @@ class ConstraintSolver {
|
||||||
Matrix3x3 Minv_sp_inertia[NB_MAX_BODIES]; // 3x3 world inertia tensor matrix I for each body (from the Minv_sp matrix)
|
Matrix3x3 Minv_sp_inertia[NB_MAX_BODIES]; // 3x3 world inertia tensor matrix I for each body (from the Minv_sp matrix)
|
||||||
decimal Minv_sp_mass_diag[NB_MAX_BODIES]; // Array that contains for each body the inverse of its mass
|
decimal Minv_sp_mass_diag[NB_MAX_BODIES]; // Array that contains for each body the inverse of its mass
|
||||||
// This is an array of size nbBodies that contains in each cell a 6x6 matrix
|
// This is an array of size nbBodies that contains in each cell a 6x6 matrix
|
||||||
Vector3* V1; // Array that contains for each body the 6x1 vector that contains linear and angular velocities
|
Vector3* mLinearVelocities; // Array of constrained linear velocities
|
||||||
Vector3* W1;
|
Vector3* mAngularVelocities; // Array of constrained angular velocities
|
||||||
Vector3* Vconstraint; // Same kind of vector as V1 but contains the final constraint velocities
|
decimal mTimeStep; // Current time step
|
||||||
Vector3* Wconstraint;
|
|
||||||
decimal VconstraintError[6*NB_MAX_BODIES]; // Same kind of vector as V1 but contains the final constraint velocities
|
|
||||||
|
|
||||||
// Contact constraints
|
// Contact constraints
|
||||||
ContactConstraint* mContactConstraints;
|
ContactConstraint* mContactConstraints;
|
||||||
|
@ -198,10 +190,9 @@ class ConstraintSolver {
|
||||||
void initializeBodies(); // Initialize bodies velocities
|
void initializeBodies(); // Initialize bodies velocities
|
||||||
void initializeContactConstraints(decimal dt); // Fill in all the matrices needed to solve the LCP problem
|
void initializeContactConstraints(decimal dt); // Fill in all the matrices needed to solve the LCP problem
|
||||||
void computeMatrixB_sp(); // Compute the matrix B_sp
|
void computeMatrixB_sp(); // Compute the matrix B_sp
|
||||||
void computeVectorVconstraint(decimal dt); // Compute the vector V2
|
|
||||||
void cacheLambda(); // Cache the lambda values in order to reuse them in the next step to initialize the lambda vector
|
void cacheLambda(); // Cache the lambda values in order to reuse them in the next step to initialize the lambda vector
|
||||||
void computeVectorA(decimal dt); // Compute the vector a used in the solve() method
|
void warmStart(); // Compute the vector a used in the solve() method
|
||||||
void solveLCP(decimal dt); // Solve a LCP problem using Projected-Gauss-Seidel algorithm
|
void solveLCP(); // Solve a LCP problem using Projected-Gauss-Seidel algorithm
|
||||||
|
|
||||||
public:
|
public:
|
||||||
ConstraintSolver(DynamicsWorld* world); // Constructor
|
ConstraintSolver(DynamicsWorld* world); // Constructor
|
||||||
|
@ -223,14 +214,14 @@ inline bool ConstraintSolver::isConstrainedBody(RigidBody* body) const {
|
||||||
inline Vector3 ConstraintSolver::getConstrainedLinearVelocityOfBody(RigidBody* body) {
|
inline Vector3 ConstraintSolver::getConstrainedLinearVelocityOfBody(RigidBody* body) {
|
||||||
assert(isConstrainedBody(body));
|
assert(isConstrainedBody(body));
|
||||||
uint indexBodyArray = mMapBodyToIndex[body];
|
uint indexBodyArray = mMapBodyToIndex[body];
|
||||||
return Vconstraint[indexBodyArray];
|
return mLinearVelocities[indexBodyArray];
|
||||||
}
|
}
|
||||||
|
|
||||||
// Return the constrained angular velocity of a body after solving the LCP problem
|
// Return the constrained angular velocity of a body after solving the LCP problem
|
||||||
inline Vector3 ConstraintSolver::getConstrainedAngularVelocityOfBody(RigidBody *body) {
|
inline Vector3 ConstraintSolver::getConstrainedAngularVelocityOfBody(RigidBody *body) {
|
||||||
assert(isConstrainedBody(body));
|
assert(isConstrainedBody(body));
|
||||||
uint indexBodyArray = mMapBodyToIndex[body];
|
uint indexBodyArray = mMapBodyToIndex[body];
|
||||||
return Wconstraint[indexBodyArray];
|
return mAngularVelocities[indexBodyArray];
|
||||||
}
|
}
|
||||||
|
|
||||||
// Cleanup of the constraint solver
|
// Cleanup of the constraint solver
|
||||||
|
@ -243,51 +234,19 @@ inline void ConstraintSolver::cleanup() {
|
||||||
delete[] mContactConstraints;
|
delete[] mContactConstraints;
|
||||||
mContactConstraints = 0;
|
mContactConstraints = 0;
|
||||||
}
|
}
|
||||||
if (Vconstraint != 0) {
|
if (mLinearVelocities != 0) {
|
||||||
delete[] Vconstraint;
|
delete[] mLinearVelocities;
|
||||||
Vconstraint = 0;
|
mLinearVelocities = 0;
|
||||||
}
|
}
|
||||||
if (Wconstraint != 0) {
|
if (mAngularVelocities != 0) {
|
||||||
delete[] Wconstraint;
|
delete[] mAngularVelocities;
|
||||||
Wconstraint = 0;
|
mAngularVelocities = 0;
|
||||||
}
|
|
||||||
if (V1 != 0) {
|
|
||||||
delete[] V1;
|
|
||||||
V1 = 0;
|
|
||||||
}
|
|
||||||
if (W1 != 0) {
|
|
||||||
delete[] W1;
|
|
||||||
W1 = 0;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// Set the number of iterations of the LCP solver
|
// Set the number of iterations of the LCP solver
|
||||||
inline void ConstraintSolver::setNbLCPIterations(uint nbIterations) {
|
inline void ConstraintSolver::setNbLCPIterations(uint nbIterations) {
|
||||||
mNbIterations = nbIterations;
|
mNbIterations = nbIterations;
|
||||||
}
|
|
||||||
|
|
||||||
// Solve the current LCP problem
|
|
||||||
inline void ConstraintSolver::solve(decimal dt) {
|
|
||||||
|
|
||||||
// Initialize the solver
|
|
||||||
initialize();
|
|
||||||
|
|
||||||
initializeBodies();
|
|
||||||
|
|
||||||
// Fill-in all the matrices needed to solve the LCP problem
|
|
||||||
initializeContactConstraints(dt);
|
|
||||||
|
|
||||||
// Compute the matrix B
|
|
||||||
computeMatrixB_sp();
|
|
||||||
|
|
||||||
// Solve the LCP problem (computation of lambda)
|
|
||||||
solveLCP(dt);
|
|
||||||
|
|
||||||
// Cache the lambda values in order to use them in the next step
|
|
||||||
cacheLambda();
|
|
||||||
|
|
||||||
// Compute the vector Vconstraint
|
|
||||||
computeVectorVconstraint(dt);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
} // End of ReactPhysics3D namespace
|
} // End of ReactPhysics3D namespace
|
||||||
|
|
|
@ -123,15 +123,16 @@ void DynamicsWorld::updateAllBodiesMotion() {
|
||||||
newLinearVelocity = mConstraintSolver.getConstrainedLinearVelocityOfBody(*it);
|
newLinearVelocity = mConstraintSolver.getConstrainedLinearVelocityOfBody(*it);
|
||||||
newAngularVelocity = mConstraintSolver.getConstrainedAngularVelocityOfBody(*it);
|
newAngularVelocity = mConstraintSolver.getConstrainedAngularVelocityOfBody(*it);
|
||||||
}
|
}
|
||||||
|
else {
|
||||||
|
// Compute V_forces = dt * (M^-1 * F_ext) which is the velocity of the body due to the
|
||||||
|
// external forces and torques.
|
||||||
|
newLinearVelocity += dt * rigidBody->getMassInverse() * rigidBody->getExternalForce();
|
||||||
|
newAngularVelocity += dt * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque();
|
||||||
|
|
||||||
// Compute V_forces = dt * (M^-1 * F_ext) which is the velocity of the body due to the
|
// Add the velocity V1 to the new velocity
|
||||||
// external forces and torques.
|
newLinearVelocity += rigidBody->getLinearVelocity();
|
||||||
newLinearVelocity += dt * rigidBody->getMassInverse() * rigidBody->getExternalForce();
|
newAngularVelocity += rigidBody->getAngularVelocity();
|
||||||
newAngularVelocity += dt * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque();
|
}
|
||||||
|
|
||||||
// Add the velocity V1 to the new velocity
|
|
||||||
newLinearVelocity += rigidBody->getLinearVelocity();
|
|
||||||
newAngularVelocity += rigidBody->getAngularVelocity();
|
|
||||||
|
|
||||||
// Update the position and the orientation of the body according to the new velocity
|
// Update the position and the orientation of the body according to the new velocity
|
||||||
updatePositionAndOrientationOfBody(*it, newLinearVelocity, newAngularVelocity);
|
updatePositionAndOrientationOfBody(*it, newLinearVelocity, newAngularVelocity);
|
||||||
|
|
Loading…
Reference in New Issue
Block a user