mrq/reactphysics3d
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Separate code for bodies initialization and contact constraints initialization

Browse Source
This commit is contained in:
Daniel Chappuis 2012-12-21 08:39:21 +01:00
parent 7172ee4843
commit d615f9af12
2 changed files with 71 additions and 93 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

127
src/engine/ConstraintSolver.cpp
View File

@ -101,76 +101,8 @@ void ConstraintSolver::initialize() {
assert(mMapBodyToIndex.size() == nbBodies);
}
// Fill in all the matrices needed to solve the LCP problem
// Notice that all the active constraints should have been evaluated first
void ConstraintSolver::fillInMatrices(decimal dt) {
decimal oneOverDt = 1.0 / dt;
// For each contact constraint
for (uint c=0; c<mNbContactConstraints; c++) {
ContactConstraint& constraint = mContactConstraints[c];
// For each contact point constraint
for (uint i=0; i<constraint.nbContacts; i++) {
ContactPointConstraint& contact = constraint.contacts[i];
Contact* realContact = contact.contact;
// 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);
realContact->computeLowerBoundFriction2(contact.lowerBoundFriction2);
realContact->computeUpperBoundPenetration(contact.upperBoundPenetration);
realContact->computeUpperBoundFriction1(contact.upperBoundFriction1);
realContact->computeUpperBoundFriction2(contact.upperBoundFriction2);
// Fill in the error vector
realContact->computeErrorPenetration(contact.errorPenetration);
contact.errorFriction1 = 0.0;
contact.errorFriction2 = 0.0;
// 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);
}
contact.errorFriction1 = 0.0;
contact.errorFriction2 = 0.0;
/*
// If the constraint is a contact
if (constraint->getType() == CONTACT) {
Contact* contact = dynamic_cast<Contact*>(constraint);
// Add the Baumgarte error correction term for contacts
decimal slop = 0.005;
if (contact->getPenetrationDepth() > slop) {
errorValues[noConstraint] += 0.2 * oneOverDt * std::max(double(contact->getPenetrationDepth() - slop), 0.0);
}
}
*/
}
}
// Initialize bodies velocities
void ConstraintSolver::initializeBodies() {
// For each current body that is implied in some constraint
RigidBody* rigidBody;
@ -225,19 +157,64 @@ void ConstraintSolver::fillInMatrices(decimal dt) {
}
}
// Compute the vector b
void ConstraintSolver::computeVectorB(decimal dt) {
uint indexBody1, indexBody2;
// 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;
// For each contact constraint
for (uint c=0; c<mNbContactConstraints; c++) {
ContactConstraint& constraint = mContactConstraints[c];
// For each contact point
uint indexBody1 = constraint.indexBody1;
uint indexBody2 = constraint.indexBody2;
// For each contact point constraint
for (uint i=0; i<constraint.nbContacts; i++) {
ContactPointConstraint& contact = constraint.contacts[i];
Contact* realContact = contact.contact;
// 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);
realContact->computeLowerBoundFriction2(contact.lowerBoundFriction2);
realContact->computeUpperBoundPenetration(contact.upperBoundPenetration);
realContact->computeUpperBoundFriction1(contact.upperBoundFriction1);
realContact->computeUpperBoundFriction2(contact.upperBoundFriction2);
// Fill in the error vector
realContact->computeErrorPenetration(contact.errorPenetration);
contact.errorFriction1 = 0.0;
contact.errorFriction2 = 0.0;
// 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);
}
contact.errorFriction1 = 0.0;
contact.errorFriction2 = 0.0;
// ---------- Penetration ---------- //
@ -351,6 +328,8 @@ void ConstraintSolver::computeVectorB(decimal dt) {
contact.b_Friction2 -= value1 + value2;
}
}
}
// Compute the matrix B_sp

11
src/engine/ConstraintSolver.h
View File

@ -198,8 +198,8 @@ class ConstraintSolver {
void initialize(); // Initialize the constraint solver before each solving
void fillInMatrices(decimal dt); // Fill in all the matrices needed to solve the LCP problem
void computeVectorB(decimal dt); // Compute the vector b
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 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
@ -259,11 +259,10 @@ inline void ConstraintSolver::solve(decimal dt) {
// Initialize the solver
initialize();
// Fill-in all the matrices needed to solve the LCP problem
fillInMatrices(dt);
initializeBodies();
// Compute the vector b
computeVectorB(dt);
// Fill-in all the matrices needed to solve the LCP problem
initializeContactConstraints(dt);
// Compute the matrix B
computeMatrixB_sp();