Clean up the code and change the warmstart() method
This commit is contained in:
Daniel Chappuis
parent
9cf672c51c
commit
2d0da2cc1c
|
|
@ -35,7 +35,7 @@ using namespace std;
|
|||
// Constructor
|
||||
ConstraintSolver::ConstraintSolver(DynamicsWorld* world)
|
||||
:world(world), nbConstraints(0), mNbIterations(10), mContactConstraints(0),
|
||||
mLinearVelocities(0), mAngularVelocities(0) {
|
||||
mLinearVelocities(0), mAngularVelocities(0) {
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -44,7 +44,7 @@ ConstraintSolver::~ConstraintSolver() {
|
|||
|
||||
}
|
||||
|
||||
// Initialize the constraint solver before each solving
|
||||
// Initialize the constraint solver
|
||||
void ConstraintSolver::initialize() {
|
||||
|
||||
nbConstraints = 0;
|
||||
|
|
@ -120,51 +120,31 @@ void ConstraintSolver::initialize() {
|
|||
assert(mMapBodyToIndex.size() == nbBodies);
|
||||
}
|
||||
|
||||
// Initialize bodies velocities
|
||||
// Initialize the constrained bodies
|
||||
void ConstraintSolver::initializeBodies() {
|
||||
|
||||
// For each current body that is implied in some constraint
|
||||
RigidBody* rigidBody;
|
||||
RigidBody* body;
|
||||
uint b=0;
|
||||
for (set<RigidBody*>::iterator it = mConstraintBodies.begin(); it != mConstraintBodies.end(); ++it, b++) {
|
||||
for (set<RigidBody*>::iterator it = mConstraintBodies.begin(); it != mConstraintBodies.end(); ++it) {
|
||||
rigidBody = *it;
|
||||
uint bodyNumber = mMapBodyToIndex[rigidBody];
|
||||
|
||||
// TODO : Use polymorphism and remove this downcasting
|
||||
assert(rigidBody);
|
||||
|
||||
// Compute the vector V1 with initial velocities values
|
||||
int bodyIndexArray = 6 * bodyNumber;
|
||||
|
||||
mLinearVelocities[bodyNumber] = rigidBody->getLinearVelocity() + mTimeStep * rigidBody->getMassInverse() * rigidBody->getExternalForce();
|
||||
mAngularVelocities[bodyNumber] = rigidBody->getAngularVelocity() + mTimeStep * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque();
|
||||
|
||||
// Initialize the mass and inertia tensor matrices
|
||||
Minv_sp_inertia[bodyNumber].setAllValues(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
|
||||
Minv_sp_mass_diag[bodyNumber] = 0.0;
|
||||
|
||||
// If the motion of the rigid body is enabled
|
||||
if (rigidBody->getIsMotionEnabled()) {
|
||||
Minv_sp_inertia[bodyNumber] = rigidBody->getInertiaTensorInverseWorld();
|
||||
Minv_sp_mass_diag[bodyNumber] = rigidBody->getMassInverse();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Fill in all the matrices needed to solve the LCP problem
|
||||
// Notice that all the active constraints should have been evaluated first
|
||||
void ConstraintSolver::initializeContactConstraints(decimal dt) {
|
||||
decimal oneOverDT = 1.0 / dt;
|
||||
// Initialize the contact constraints before solving the system
|
||||
void ConstraintSolver::initializeContactConstraints() {
|
||||
|
||||
// For each contact constraint
|
||||
for (uint c=0; c<mNbContactConstraints; c++) {
|
||||
|
||||
ContactConstraint& constraint = mContactConstraints[c];
|
||||
|
||||
uint indexBody1 = constraint.indexBody1;
|
||||
uint indexBody2 = constraint.indexBody2;
|
||||
|
||||
Matrix3x3& I1 = constraint.inverseInertiaTensorBody1;
|
||||
Matrix3x3& I2 = constraint.inverseInertiaTensorBody2;
|
||||
|
||||
|
|
@ -221,17 +201,6 @@ void ConstraintSolver::initializeContactConstraints(decimal dt) {
|
|||
contact.restitutionBias = constraint.restitutionFactor * deltaVDotN;
|
||||
}
|
||||
|
||||
// Fill in the J_sp matrix
|
||||
realContact->computeJacobianPenetration(contact.J_spBody1Penetration, contact.J_spBody2Penetration);
|
||||
realContact->computeJacobianFriction1(contact.J_spBody1Friction1, contact.J_spBody2Friction1);
|
||||
realContact->computeJacobianFriction2(contact.J_spBody1Friction2, contact.J_spBody2Friction2);
|
||||
|
||||
// Fill in the body mapping matrix
|
||||
//for(int i=0; i<realContact->getNbConstraints(); i++) {
|
||||
// bodyMapping[noConstraint+i][0] = constraint->getBody1();
|
||||
// bodyMapping[noConstraint+i][1] = constraint->getBody2();
|
||||
//}
|
||||
|
||||
// Fill in the limit vectors for the constraint
|
||||
realContact->computeLowerBoundPenetration(contact.lowerBoundPenetration);
|
||||
realContact->computeLowerBoundFriction1(contact.lowerBoundFriction1);
|
||||
|
|
@ -240,147 +209,74 @@ void ConstraintSolver::initializeContactConstraints(decimal dt) {
|
|||
realContact->computeUpperBoundFriction1(contact.upperBoundFriction1);
|
||||
realContact->computeUpperBoundFriction2(contact.upperBoundFriction2);
|
||||
|
||||
// Fill in the error vector
|
||||
realContact->computeErrorPenetration(contact.errorPenetration);
|
||||
|
||||
// Get the cached lambda values of the constraint
|
||||
contact.penetrationImpulse = realContact->getCachedLambda(0);
|
||||
contact.friction1Impulse = realContact->getCachedLambda(1);
|
||||
contact.friction2Impulse = realContact->getCachedLambda(2);
|
||||
//for (int i=0; i<constraint->getNbConstraints(); i++) {
|
||||
// lambdaInit[noConstraint + i] = constraint->getCachedLambda(i);
|
||||
// }
|
||||
|
||||
contact.errorPenetration = 0.0;
|
||||
decimal slop = 0.005;
|
||||
if (realContact->getPenetrationDepth() > slop) {
|
||||
contact.errorPenetration -= 0.2 * oneOverDT * std::max(double(realContact->getPenetrationDepth() - slop), 0.0);
|
||||
}
|
||||
|
||||
// ---------- Penetration ---------- //
|
||||
|
||||
// b = errorValues * oneOverDT;
|
||||
contact.b_Penetration = contact.errorPenetration;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Compute the matrix B_sp
|
||||
void ConstraintSolver::computeMatrixB_sp() {
|
||||
uint indexConstraintArray, indexBody1, indexBody2;
|
||||
|
||||
// Warm start the solver
|
||||
// For each constraint, we apply the previous impulse (from the previous step)
|
||||
// at the beginning. With this technique, we will converge faster towards
|
||||
// the solution of the linear system
|
||||
void ConstraintSolver::warmStart() {
|
||||
|
||||
// For each constraint
|
||||
for (uint m = 0; m<mNbContactConstraints; m++) {
|
||||
for (uint c=0; c<mNbContactConstraints; c++) {
|
||||
|
||||
ContactConstraint& constraint = mContactConstraints[m];
|
||||
ContactConstraint& constraint = mContactConstraints[c];
|
||||
|
||||
for (uint c=0; c<constraint.nbContacts; c++) {
|
||||
for (uint i=0; i<constraint.nbContacts; i++) {
|
||||
|
||||
ContactPointConstraint& contact = constraint.contacts[c];
|
||||
ContactPointConstraint& contact = constraint.contacts[i];
|
||||
|
||||
// ---------- Penetration ---------- //
|
||||
// --------- Penetration --------- //
|
||||
|
||||
indexBody1 = constraint.indexBody1;
|
||||
indexBody2 = constraint.indexBody2;
|
||||
contact.B_spBody1Penetration[0] = Minv_sp_mass_diag[indexBody1] * contact.J_spBody1Penetration[0];
|
||||
contact.B_spBody1Penetration[1] = Minv_sp_mass_diag[indexBody1] * contact.J_spBody1Penetration[1];
|
||||
contact.B_spBody1Penetration[2] = Minv_sp_mass_diag[indexBody1] * contact.J_spBody1Penetration[2];
|
||||
contact.B_spBody2Penetration[0] = Minv_sp_mass_diag[indexBody2] * contact.J_spBody2Penetration[0];
|
||||
contact.B_spBody2Penetration[1] = Minv_sp_mass_diag[indexBody2] * contact.J_spBody2Penetration[1];
|
||||
contact.B_spBody2Penetration[2] = Minv_sp_mass_diag[indexBody2] * contact.J_spBody2Penetration[2];
|
||||
// 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);
|
||||
|
||||
for (uint i=0; i<3; i++) {
|
||||
contact.B_spBody1Penetration[3 + i] = 0.0;
|
||||
contact.B_spBody2Penetration[3 + i] = 0.0;
|
||||
for (uint j=0; j<3; j++) {
|
||||
contact.B_spBody1Penetration[3 + i] += Minv_sp_inertia[indexBody1].getValue(i, j) * contact.J_spBody1Penetration[3 + j];
|
||||
contact.B_spBody2Penetration[3 + i] += Minv_sp_inertia[indexBody2].getValue(i, j) * contact.J_spBody2Penetration[3 + j];
|
||||
}
|
||||
}
|
||||
// Apply the impulse to the bodies of the constraint
|
||||
applyImpulse(impulsePenetration, constraint);
|
||||
|
||||
// ---------- Friction 1 ---------- //
|
||||
// --------- Friction 1 --------- //
|
||||
|
||||
Matrix3x3& I1 = constraint.inverseInertiaTensorBody1;
|
||||
Matrix3x3& I2 = constraint.inverseInertiaTensorBody2;
|
||||
// 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);
|
||||
|
||||
Vector3 JspLinBody1 = Vector3(0, 0, 0);
|
||||
Vector3 JspAngBody1 = Vector3(0, 0, 0);
|
||||
Vector3 JspLinBody2 = Vector3(0, 0, 0);
|
||||
Vector3 JspAngBody2 = Vector3(0, 0, 0);
|
||||
// Apply the impulses to the bodies of the constraint
|
||||
applyImpulse(impulseFriction1, constraint);
|
||||
|
||||
if (constraint.isBody1Moving) {
|
||||
JspLinBody1 += -constraint.massInverseBody1 * contact.frictionVector1;
|
||||
JspAngBody1 += I1 * (-contact.r1CrossT1);
|
||||
}
|
||||
if (constraint.isBody2Moving) {
|
||||
JspLinBody2 += constraint.massInverseBody2 * contact.frictionVector1;
|
||||
JspAngBody2 += I1 * (contact.r2CrossT1);
|
||||
}
|
||||
// --------- Friction 2 --------- //
|
||||
|
||||
/*
|
||||
std::cout << "------ New Version ----" << std::endl;
|
||||
std::cout << "JspLinBody1 : " << JspLinBody1.getX() << ", " << JspLinBody1.getY() << ", " << JspLinBody1.getZ() << std::endl;
|
||||
std::cout << "JspAngBody1 : " << JspAngBody1.getX() << ", " << JspAngBody1.getY() << ", " << JspAngBody1.getZ() << std::endl;
|
||||
std::cout << "JspLinBody2 : " << JspLinBody2.getX() << ", " << JspLinBody2.getY() << ", " << JspLinBody2.getZ() << std::endl;
|
||||
std::cout << "JspAngBody2 : " << JspAngBody2.getX() << ", " << JspAngBody2.getY() << ", " << JspAngBody2.getZ() << std::endl;
|
||||
// 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);
|
||||
|
||||
std::cout << ": friction vector 1 : " << contact.frictionVector1.getX() << ", " << contact.frictionVector1.getY() << ", " << contact.frictionVector1.getZ() << std::endl;
|
||||
std::cout << ": r1 x t1 : " << contact.r1CrossT1.getX() << ", " << contact.r1CrossT1.getY() << ", " << contact.r1CrossT1.getZ() << std::endl;
|
||||
std::cout << ": r2 x t1 : " << contact.r2CrossT1.getX() << ", " << contact.r2CrossT1.getY() << ", " << contact.r2CrossT1.getZ() << std::endl;
|
||||
std::cout << ": inverse mass body 1 = " << constraint.massInverseBody1 << std::endl;
|
||||
std::cout << ": inverse mass body 2 = " << constraint.massInverseBody2 << std::endl;
|
||||
*/
|
||||
|
||||
contact.B_spBody1Friction1[0] = Minv_sp_mass_diag[indexBody1] * contact.J_spBody1Friction1[0];
|
||||
contact.B_spBody1Friction1[1] = Minv_sp_mass_diag[indexBody1] * contact.J_spBody1Friction1[1];
|
||||
contact.B_spBody1Friction1[2] = Minv_sp_mass_diag[indexBody1] * contact.J_spBody1Friction1[2];
|
||||
contact.B_spBody2Friction1[0] = Minv_sp_mass_diag[indexBody2] * contact.J_spBody2Friction1[0];
|
||||
contact.B_spBody2Friction1[1] = Minv_sp_mass_diag[indexBody2] * contact.J_spBody2Friction1[1];
|
||||
contact.B_spBody2Friction1[2] = Minv_sp_mass_diag[indexBody2] * contact.J_spBody2Friction1[2];
|
||||
|
||||
for (uint i=0; i<3; i++) {
|
||||
contact.B_spBody1Friction1[3 + i] = 0.0;
|
||||
contact.B_spBody2Friction1[3 + i] = 0.0;
|
||||
for (uint j=0; j<3; j++) {
|
||||
contact.B_spBody1Friction1[3 + i] += Minv_sp_inertia[indexBody1].getValue(i, j) * contact.J_spBody1Friction1[3 + j];
|
||||
contact.B_spBody2Friction1[3 + i] += Minv_sp_inertia[indexBody2].getValue(i, j) * contact.J_spBody2Friction1[3 + j];
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
std::cout << "------ Old Version ----" << std::endl;
|
||||
std::cout << "JspLinBody1 : " << contact.B_spBody1Friction1[0] << ", " << contact.B_spBody1Friction1[1] << ", " << contact.B_spBody1Friction1[2] << std::endl;
|
||||
std::cout << "JspAngBody1 : " << contact.B_spBody1Friction1[3] << ", " << contact.B_spBody1Friction1[4] << ", " << contact.B_spBody1Friction1[5] << std::endl;
|
||||
std::cout << "JspLinBody2 : " << contact.B_spBody2Friction1[0] << ", " << contact.B_spBody2Friction1[1] << ", " << contact.B_spBody2Friction1[2] << std::endl;
|
||||
std::cout << "JspAngBody2 : " << contact.B_spBody2Friction1[3] << ", " << contact.B_spBody2Friction1[4] << ", " << contact.B_spBody2Friction1[5] << std::endl;
|
||||
*/
|
||||
|
||||
// ---------- Friction 2 ---------- //
|
||||
|
||||
contact.B_spBody1Friction2[0] = Minv_sp_mass_diag[indexBody1] * contact.J_spBody1Friction2[0];
|
||||
contact.B_spBody1Friction2[1] = Minv_sp_mass_diag[indexBody1] * contact.J_spBody1Friction2[1];
|
||||
contact.B_spBody1Friction2[2] = Minv_sp_mass_diag[indexBody1] * contact.J_spBody1Friction2[2];
|
||||
contact.B_spBody2Friction2[0] = Minv_sp_mass_diag[indexBody2] * contact.J_spBody2Friction2[0];
|
||||
contact.B_spBody2Friction2[1] = Minv_sp_mass_diag[indexBody2] * contact.J_spBody2Friction2[1];
|
||||
contact.B_spBody2Friction2[2] = Minv_sp_mass_diag[indexBody2] * contact.J_spBody2Friction2[2];
|
||||
|
||||
for (uint i=0; i<3; i++) {
|
||||
contact.B_spBody1Friction2[3 + i] = 0.0;
|
||||
contact.B_spBody2Friction2[3 + i] = 0.0;
|
||||
for (uint j=0; j<3; j++) {
|
||||
contact.B_spBody1Friction2[3 + i] += Minv_sp_inertia[indexBody1].getValue(i, j) * contact.J_spBody1Friction2[3 + j];
|
||||
contact.B_spBody2Friction2[3 + i] += Minv_sp_inertia[indexBody2].getValue(i, j) * contact.J_spBody2Friction2[3 + j];
|
||||
}
|
||||
}
|
||||
// Apply the impulses to the bodies of the constraint
|
||||
applyImpulse(impulseFriction2, constraint);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Solve a LCP problem using the Projected-Gauss-Seidel algorithm
|
||||
// This method outputs the result in the lambda vector
|
||||
void ConstraintSolver::solveLCP() {
|
||||
// Solve the contact constraints by applying sequential impulses
|
||||
void ConstraintSolver::solveContactConstraints() {
|
||||
|
||||
uint indexBody1Array, indexBody2Array;
|
||||
decimal deltaLambda;
|
||||
decimal lambdaTemp;
|
||||
uint iter;
|
||||
|
|
@ -400,9 +296,6 @@ void ConstraintSolver::solveLCP() {
|
|||
|
||||
ContactPointConstraint& contact = constraint.contacts[i];
|
||||
|
||||
indexBody1Array = constraint.indexBody1;
|
||||
indexBody2Array = constraint.indexBody2;
|
||||
|
||||
const Vector3& v1 = mLinearVelocities[constraint.indexBody1];
|
||||
const Vector3& w1 = mAngularVelocities[constraint.indexBody1];
|
||||
const Vector3& v2 = mLinearVelocities[constraint.indexBody2];
|
||||
|
|
@ -434,10 +327,10 @@ void ConstraintSolver::solveLCP() {
|
|||
deltaLambda = contact.penetrationImpulse - lambdaTemp;
|
||||
|
||||
// Compute the impulse P=J^T * lambda
|
||||
const Vector3 linearImpulseBody1 = -contact.normal * deltaLambda;
|
||||
const Vector3 angularImpulseBody1 = -contact.r1CrossN * deltaLambda;
|
||||
const Vector3 linearImpulseBody2 = contact.normal * deltaLambda;
|
||||
const Vector3 angularImpulseBody2 = contact.r2CrossN * deltaLambda;
|
||||
Vector3 linearImpulseBody1 = -contact.normal * deltaLambda;
|
||||
Vector3 angularImpulseBody1 = -contact.r1CrossN * deltaLambda;
|
||||
Vector3 linearImpulseBody2 = contact.normal * deltaLambda;
|
||||
Vector3 angularImpulseBody2 = contact.r2CrossN * deltaLambda;
|
||||
const Impulse impulsePenetration(linearImpulseBody1, angularImpulseBody1,
|
||||
linearImpulseBody2, angularImpulseBody2);
|
||||
|
||||
|
|
@ -457,12 +350,12 @@ void ConstraintSolver::solveLCP() {
|
|||
deltaLambda = contact.friction1Impulse - lambdaTemp;
|
||||
|
||||
// Compute the impulse P=J^T * lambda
|
||||
Vector3 linearImpulseBody1Friction1 = -contact.frictionVector1 * deltaLambda;
|
||||
Vector3 angularImpulseBody1Friction1 = -contact.r1CrossT1 * deltaLambda;
|
||||
Vector3 linearImpulseBody2Friction1 = contact.frictionVector1 * deltaLambda;
|
||||
Vector3 angularImpulseBody2Friction1 = contact.r2CrossT1 * deltaLambda;
|
||||
Impulse impulseFriction1(linearImpulseBody1Friction1, angularImpulseBody1Friction1,
|
||||
linearImpulseBody2Friction1, angularImpulseBody2Friction1);
|
||||
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);
|
||||
|
|
@ -480,12 +373,12 @@ void ConstraintSolver::solveLCP() {
|
|||
deltaLambda = contact.friction2Impulse - lambdaTemp;
|
||||
|
||||
// Compute the impulse P=J^T * lambda
|
||||
Vector3 linearImpulseBody1Friction2 = -contact.frictionVector2 * deltaLambda;
|
||||
Vector3 angularImpulseBody1Friction2 = -contact.r1CrossT2 * deltaLambda;
|
||||
Vector3 linearImpulseBody2Friction2 = contact.frictionVector2 * deltaLambda;
|
||||
Vector3 angularImpulseBody2Friction2 = contact.r2CrossT2 * deltaLambda;
|
||||
Impulse impulseFriction2(linearImpulseBody1Friction2, angularImpulseBody1Friction2,
|
||||
linearImpulseBody2Friction2, angularImpulseBody2Friction2);
|
||||
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);
|
||||
|
|
@ -494,60 +387,9 @@ void ConstraintSolver::solveLCP() {
|
|||
}
|
||||
}
|
||||
|
||||
// Compute the vector a used in the solve() method
|
||||
// Note that a = B * lambda
|
||||
void ConstraintSolver::warmStart() {
|
||||
uint i;
|
||||
uint indexBody1Array, indexBody2Array;
|
||||
|
||||
// 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];
|
||||
|
||||
indexBody1Array = constraint.indexBody1;
|
||||
indexBody2Array = constraint.indexBody2;
|
||||
|
||||
// --------- Penetration --------- //
|
||||
|
||||
for (uint j=0; j<3; j++) {
|
||||
mLinearVelocities[indexBody1Array][j] += contact.B_spBody1Penetration[j] * contact.penetrationImpulse;
|
||||
mAngularVelocities[indexBody1Array][j] += contact.B_spBody1Penetration[j + 3] * contact.penetrationImpulse;
|
||||
|
||||
mLinearVelocities[indexBody2Array][j] += contact.B_spBody2Penetration[j] * contact.penetrationImpulse;
|
||||
mAngularVelocities[indexBody2Array][j] += contact.B_spBody2Penetration[j + 3] * contact.penetrationImpulse;
|
||||
}
|
||||
|
||||
// --------- Friction 1 --------- //
|
||||
|
||||
for (uint j=0; j<3; j++) {
|
||||
mLinearVelocities[indexBody1Array][j] += contact.B_spBody1Friction1[j] * contact.friction1Impulse;
|
||||
mAngularVelocities[indexBody1Array][j] += contact.B_spBody1Friction1[j + 3] * contact.friction1Impulse;
|
||||
|
||||
mLinearVelocities[indexBody2Array][j] += contact.B_spBody2Friction1[j] * contact.friction1Impulse;
|
||||
mAngularVelocities[indexBody2Array][j] += contact.B_spBody2Friction1[j + 3] * contact.friction1Impulse;
|
||||
}
|
||||
|
||||
// --------- Friction 2 --------- //
|
||||
|
||||
for (uint j=0; j<3; j++) {
|
||||
mLinearVelocities[indexBody1Array][j] += contact.B_spBody1Friction2[j] * contact.friction2Impulse;
|
||||
mAngularVelocities[indexBody1Array][j] += contact.B_spBody1Friction2[j + 3] * contact.friction2Impulse;
|
||||
|
||||
mLinearVelocities[indexBody2Array][j] += contact.B_spBody2Friction2[j] * contact.friction2Impulse;
|
||||
mAngularVelocities[indexBody2Array][j] += contact.B_spBody2Friction2[j + 3] * contact.friction2Impulse;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Cache the lambda values in order to reuse them in the next step
|
||||
// to initialize the lambda vector
|
||||
void ConstraintSolver::cacheLambda() {
|
||||
// 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++) {
|
||||
|
|
@ -565,10 +407,10 @@ void ConstraintSolver::cacheLambda() {
|
|||
}
|
||||
}
|
||||
|
||||
// Solve the current LCP problem
|
||||
void ConstraintSolver::solve(decimal dt) {
|
||||
// Solve the constraints
|
||||
void ConstraintSolver::solve(decimal timeStep) {
|
||||
|
||||
mTimeStep = dt;
|
||||
mTimeStep = timeStep;
|
||||
|
||||
// Initialize the solver
|
||||
initialize();
|
||||
|
|
@ -576,16 +418,13 @@ void ConstraintSolver::solve(decimal dt) {
|
|||
initializeBodies();
|
||||
|
||||
// Fill-in all the matrices needed to solve the LCP problem
|
||||
initializeContactConstraints(dt);
|
||||
initializeContactConstraints();
|
||||
|
||||
// Compute the matrix B
|
||||
computeMatrixB_sp();
|
||||
|
||||
// Solve the LCP problem (computation of lambda)
|
||||
solveLCP();
|
||||
// Solve the contact constraints
|
||||
solveContactConstraints();
|
||||
|
||||
// Cache the lambda values in order to use them in the next step
|
||||
cacheLambda();
|
||||
storeImpulses();
|
||||
}
|
||||
|
||||
// Apply an impulse to the two bodies of a constraint
|
||||
|
|
|
|||
|
|
@ -82,27 +82,13 @@ struct ContactPointConstraint {
|
|||
decimal inversePenetrationMass; // Inverse of the matrix K for the penenetration
|
||||
decimal inverseFriction1Mass; // Inverse of the matrix K for the 1st friction
|
||||
decimal inverseFriction2Mass; // Inverse of the matrix K for the 2nd friction
|
||||
decimal J_spBody1Penetration[6];
|
||||
decimal J_spBody2Penetration[6];
|
||||
decimal J_spBody1Friction1[6];
|
||||
decimal J_spBody2Friction1[6];
|
||||
decimal J_spBody1Friction2[6];
|
||||
decimal J_spBody2Friction2[6];
|
||||
decimal lowerBoundPenetration;
|
||||
decimal upperBoundPenetration;
|
||||
decimal lowerBoundFriction1;
|
||||
decimal upperBoundFriction1;
|
||||
decimal lowerBoundFriction2;
|
||||
decimal upperBoundFriction2;
|
||||
decimal errorPenetration;
|
||||
Contact* contact; // TODO : REMOVE THIS
|
||||
decimal b_Penetration;
|
||||
decimal B_spBody1Penetration[6];
|
||||
decimal B_spBody2Penetration[6];
|
||||
decimal B_spBody1Friction1[6];
|
||||
decimal B_spBody2Friction1[6];
|
||||
decimal B_spBody1Friction2[6];
|
||||
decimal B_spBody2Friction2[6];
|
||||
};
|
||||
|
||||
// Structure ContactConstraint
|
||||
|
|
@ -123,7 +109,7 @@ struct ContactConstraint {
|
|||
decimal restitutionFactor; // Mix of the restitution factor for two bodies
|
||||
};
|
||||
|
||||
|
||||
// TODO : Rewrite this coment to use the Sequential Impulse technique
|
||||
/* -------------------------------------------------------------------
|
||||
Class ConstrainSolver :
|
||||
This class represents the constraint solver. The constraint solver
|
||||
|
|
@ -150,7 +136,11 @@ struct ContactConstraint {
|
|||
-------------------------------------------------------------------
|
||||
*/
|
||||
class ConstraintSolver {
|
||||
|
||||
private:
|
||||
|
||||
// -------------------- Attributes -------------------- //
|
||||
|
||||
DynamicsWorld* world; // Reference to the world
|
||||
std::vector<Constraint*> activeConstraints; // Current active constraints in the physics world
|
||||
bool isErrorCorrectionActive; // True if error correction (with world order) is active
|
||||
|
|
@ -159,19 +149,6 @@ class ConstraintSolver {
|
|||
uint nbBodies; // Current number of bodies in the physics world
|
||||
RigidBody* bodyMapping[NB_MAX_CONSTRAINTS][2]; // 2-dimensional array that contains the mapping of body reference
|
||||
// in the J_sp and B_sp matrices. For instance the cell bodyMapping[i][j] contains
|
||||
// the pointer to the body that correspond to the 1x6 J_ij matrix in the
|
||||
// J_sp matrix. An integer body index refers to its index in the "bodies" std::vector
|
||||
decimal J_sp[NB_MAX_CONSTRAINTS][2*6]; // 2-dimensional array that correspond to the sparse representation of the jacobian matrix of all constraints
|
||||
// This array contains for each constraint two 1x6 Jacobian matrices (one for each body of the constraint)
|
||||
// a 1x6 matrix
|
||||
decimal B_sp[2][6*NB_MAX_CONSTRAINTS]; // 2-dimensional array that correspond to a useful matrix in sparse representation
|
||||
// This array contains for each constraint two 6x1 matrices (one for each body of the constraint)
|
||||
// a 6x1 matrix
|
||||
decimal b[NB_MAX_CONSTRAINTS]; // Vector "b" of the LCP problem
|
||||
decimal d[NB_MAX_CONSTRAINTS]; // Vector "d"
|
||||
Matrix3x3 Minv_sp_inertia[NB_MAX_BODIES]; // 3x3 world inertia tensor matrix I for each body (from the Minv_sp matrix)
|
||||
decimal Minv_sp_mass_diag[NB_MAX_BODIES]; // Array that contains for each body the inverse of its mass
|
||||
// This is an array of size nbBodies that contains in each cell a 6x6 matrix
|
||||
Vector3* mLinearVelocities; // Array of constrained linear velocities
|
||||
Vector3* mAngularVelocities; // Array of constrained angular velocities
|
||||
decimal mTimeStep; // Current time step
|
||||
|
|
@ -188,14 +165,26 @@ class ConstraintSolver {
|
|||
// Map body to index
|
||||
std::map<RigidBody*, uint> mMapBodyToIndex;
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
||||
void initialize(); // Initialize the constraint solver before each solving
|
||||
void initializeBodies(); // Initialize bodies velocities
|
||||
void initializeContactConstraints(decimal dt); // Fill in all the matrices needed to solve the LCP problem
|
||||
void computeMatrixB_sp(); // Compute the matrix B_sp
|
||||
void cacheLambda(); // Cache the lambda values in order to reuse them in the next step to initialize the lambda vector
|
||||
void warmStart(); // Compute the vector a used in the solve() method
|
||||
void solveLCP(); // Solve a LCP problem using Projected-Gauss-Seidel algorithm
|
||||
// Initialize the constraint solver
|
||||
void initialize();
|
||||
|
||||
// Initialize the constrained bodies
|
||||
void initializeBodies();
|
||||
|
||||
// Initialize the contact constraints before solving the system
|
||||
void initializeContactConstraints();
|
||||
|
||||
// Store the computed impulses to use them to
|
||||
// warm start the solver at the next iteration
|
||||
void storeImpulses();
|
||||
|
||||
// Warm start the solver
|
||||
void warmStart();
|
||||
|
||||
// Solve the contact constraints by applying sequential impulses
|
||||
void solveContactConstraints();
|
||||
|
||||
// Apply an impulse to the two bodies of a constraint
|
||||
void applyImpulse(const Impulse& impulse, const ContactConstraint& constraint);
|
||||
|
|
@ -204,14 +193,32 @@ class ConstraintSolver {
|
|||
decimal computeMixRestitutionFactor(const RigidBody *body1, const RigidBody *body2) const;
|
||||
|
||||
public:
|
||||
ConstraintSolver(DynamicsWorld* world); // Constructor
|
||||
virtual ~ConstraintSolver(); // Destructor
|
||||
void solve(decimal dt); // Solve the current LCP problem
|
||||
bool isConstrainedBody(RigidBody* body) const; // Return true if the body is in at least one constraint
|
||||
Vector3 getConstrainedLinearVelocityOfBody(RigidBody *body); // Return the constrained linear velocity of a body after solving the LCP problem
|
||||
Vector3 getConstrainedAngularVelocityOfBody(RigidBody* body); // Return the constrained angular velocity of a body after solving the LCP problem
|
||||
void cleanup(); // Cleanup of the constraint solver
|
||||
void setNbLCPIterations(uint mNbIterations); // Set the number of iterations of the LCP solver
|
||||
|
||||
// -------------------- Methods -------------------- //
|
||||
|
||||
// Constructor
|
||||
ConstraintSolver(DynamicsWorld* world);
|
||||
|
||||
// Destructor
|
||||
virtual ~ConstraintSolver();
|
||||
|
||||
// Solve the constraints
|
||||
void solve(decimal timeStep);
|
||||
|
||||
// Return true if the body is in at least one constraint
|
||||
bool isConstrainedBody(RigidBody* body) const;
|
||||
|
||||
// Return the constrained linear velocity of a body after solving the constraints
|
||||
Vector3 getConstrainedLinearVelocityOfBody(RigidBody *body);
|
||||
|
||||
// Return the constrained angular velocity of a body after solving the constraints
|
||||
Vector3 getConstrainedAngularVelocityOfBody(RigidBody* body);
|
||||
|
||||
// Clean up the constraint solver
|
||||
void cleanup();
|
||||
|
||||
// Set the number of iterations of the LCP solver
|
||||
void setNbLCPIterations(uint mNbIterations);
|
||||
};
|
||||
|
||||
// Return true if the body is in at least one constraint
|
||||
|
|
@ -219,21 +226,21 @@ inline bool ConstraintSolver::isConstrainedBody(RigidBody* body) const {
|
|||
return mConstraintBodies.count(body) == 1;
|
||||
}
|
||||
|
||||
// Return the constrained linear velocity of a body after solving the LCP problem
|
||||
// Return the constrained linear velocity of a body after solving the constraints
|
||||
inline Vector3 ConstraintSolver::getConstrainedLinearVelocityOfBody(RigidBody* body) {
|
||||
assert(isConstrainedBody(body));
|
||||
uint indexBodyArray = mMapBodyToIndex[body];
|
||||
return mLinearVelocities[indexBodyArray];
|
||||
}
|
||||
|
||||
// Return the constrained angular velocity of a body after solving the LCP problem
|
||||
// Return the constrained angular velocity of a body after solving the constraints
|
||||
inline Vector3 ConstraintSolver::getConstrainedAngularVelocityOfBody(RigidBody *body) {
|
||||
assert(isConstrainedBody(body));
|
||||
uint indexBodyArray = mMapBodyToIndex[body];
|
||||
return mAngularVelocities[indexBodyArray];
|
||||
}
|
||||
|
||||
// Cleanup of the constraint solver
|
||||
// Clean up the constraint solver
|
||||
inline void ConstraintSolver::cleanup() {
|
||||
mMapBodyToIndex.clear();
|
||||
mConstraintBodies.clear();
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user