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

153
src/engine/ConstraintSolver.cpp
View File

@ -101,16 +101,75 @@ void ConstraintSolver::initialize() {
assert(mMapBodyToIndex.size() == nbBodies);
}
// Initialize bodies velocities
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++) {
body = *it;
uint bodyNumber = mMapBodyToIndex[body];
// TODO : Use polymorphism and remove this downcasting
rigidBody = dynamic_cast<RigidBody*>(body);
assert(rigidBody);
// Compute the vector V1 with initial velocities values
Vector3 linearVelocity = rigidBody->getLinearVelocity();
Vector3 angularVelocity = rigidBody->getAngularVelocity();
int bodyIndexArray = 6 * bodyNumber;
V1[bodyIndexArray] = linearVelocity[0];
V1[bodyIndexArray + 1] = linearVelocity[1];
V1[bodyIndexArray + 2] = linearVelocity[2];
V1[bodyIndexArray + 3] = angularVelocity[0];
V1[bodyIndexArray + 4] = angularVelocity[1];
V1[bodyIndexArray + 5] = angularVelocity[2];
// Compute the vector Vconstraint with final constraint velocities
Vconstraint[bodyIndexArray] = 0.0;
Vconstraint[bodyIndexArray + 1] = 0.0;
Vconstraint[bodyIndexArray + 2] = 0.0;
Vconstraint[bodyIndexArray + 3] = 0.0;
Vconstraint[bodyIndexArray + 4] = 0.0;
Vconstraint[bodyIndexArray + 5] = 0.0;
// Compute the vector with forces and torques values
Vector3 externalForce = rigidBody->getExternalForce();
Vector3 externalTorque = rigidBody->getExternalTorque();
Fext[bodyIndexArray] = externalForce[0];
Fext[bodyIndexArray + 1] = externalForce[1];
Fext[bodyIndexArray + 2] = externalForce[2];
Fext[bodyIndexArray + 3] = externalTorque[0];
Fext[bodyIndexArray + 4] = externalTorque[1];
Fext[bodyIndexArray + 5] = externalTorque[2];
// 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::fillInMatrices(decimal dt) {
decimal oneOverDt = 1.0 / dt;
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];
uint indexBody1 = constraint.indexBody1;
uint indexBody2 = constraint.indexBody2;
// For each contact point constraint
for (uint i=0; i<constraint.nbContacts; i++) {
@ -152,98 +211,16 @@ void ConstraintSolver::fillInMatrices(decimal dt) {
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.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);
}
}
*/
}
}
// 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++) {
body = *it;
uint bodyNumber = mMapBodyToIndex[body];
// TODO : Use polymorphism and remove this downcasting
rigidBody = dynamic_cast<RigidBody*>(body);
assert(rigidBody);
// Compute the vector V1 with initial velocities values
Vector3 linearVelocity = rigidBody->getLinearVelocity();
Vector3 angularVelocity = rigidBody->getAngularVelocity();
int bodyIndexArray = 6 * bodyNumber;
V1[bodyIndexArray] = linearVelocity[0];
V1[bodyIndexArray + 1] = linearVelocity[1];
V1[bodyIndexArray + 2] = linearVelocity[2];
V1[bodyIndexArray + 3] = angularVelocity[0];
V1[bodyIndexArray + 4] = angularVelocity[1];
V1[bodyIndexArray + 5] = angularVelocity[2];
// Compute the vector Vconstraint with final constraint velocities
Vconstraint[bodyIndexArray] = 0.0;
Vconstraint[bodyIndexArray + 1] = 0.0;
Vconstraint[bodyIndexArray + 2] = 0.0;
Vconstraint[bodyIndexArray + 3] = 0.0;
Vconstraint[bodyIndexArray + 4] = 0.0;
Vconstraint[bodyIndexArray + 5] = 0.0;
// Compute the vector with forces and torques values
Vector3 externalForce = rigidBody->getExternalForce();
Vector3 externalTorque = rigidBody->getExternalTorque();
Fext[bodyIndexArray] = externalForce[0];
Fext[bodyIndexArray + 1] = externalForce[1];
Fext[bodyIndexArray + 2] = externalForce[2];
Fext[bodyIndexArray + 3] = externalTorque[0];
Fext[bodyIndexArray + 4] = externalTorque[1];
Fext[bodyIndexArray + 5] = externalTorque[2];
// 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();
}
}
}
// Compute the vector b
void ConstraintSolver::computeVectorB(decimal dt) {
uint indexBody1, indexBody2;
decimal oneOverDT = 1.0 / dt;
for (uint c = 0; c<mNbContactConstraints; c++) {
ContactConstraint& constraint = mContactConstraints[c];
// For each contact point
for (uint i=0; i<constraint.nbContacts; i++) {
ContactPointConstraint& contact = constraint.contacts[i];
// ---------- Penetration ---------- //
// b = errorValues * oneOverDT;
contact.b_Penetration = contact.errorPenetration * oneOverDT;
// Substract 1.0/dt*J*V to the vector b
indexBody1 = constraint.indexBody1;
indexBody2 = constraint.indexBody2;
@ -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();