Continue to transform PGS solver into the sequential impulse solver
This commit is contained in:
Daniel Chappuis
parent
e4d47ded09
commit
1bcec415a1
|
|
@ -34,7 +34,8 @@ using namespace std;
|
|||
|
||||
// Constructor
|
||||
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
|
||||
void ConstraintSolver::initialize() {
|
||||
|
||||
|
||||
nbConstraints = 0;
|
||||
|
||||
// TOOD : Use better allocation here
|
||||
|
|
@ -111,10 +112,8 @@ void ConstraintSolver::initialize() {
|
|||
// Compute the number of bodies that are part of some active constraint
|
||||
nbBodies = mConstraintBodies.size();
|
||||
|
||||
Vconstraint = new Vector3[nbBodies];
|
||||
Wconstraint = new Vector3[nbBodies];
|
||||
V1 = new Vector3[nbBodies];
|
||||
W1 = new Vector3[nbBodies];
|
||||
mLinearVelocities = new Vector3[nbBodies];
|
||||
mAngularVelocities = new Vector3[nbBodies];
|
||||
|
||||
assert(mMapBodyToIndex.size() == nbBodies);
|
||||
}
|
||||
|
|
@ -135,12 +134,9 @@ void ConstraintSolver::initializeBodies() {
|
|||
|
||||
// Compute the vector V1 with initial velocities values
|
||||
int bodyIndexArray = 6 * bodyNumber;
|
||||
V1[bodyNumber] = rigidBody->getLinearVelocity();
|
||||
W1[bodyNumber] = rigidBody->getAngularVelocity();
|
||||
|
||||
// Compute the vector Vconstraint with final constraint velocities
|
||||
Vconstraint[bodyNumber] = Vector3(0, 0, 0);
|
||||
Wconstraint[bodyNumber] = Vector3(0, 0, 0);
|
||||
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);
|
||||
|
|
@ -225,8 +221,6 @@ void ConstraintSolver::initializeContactConstraints(decimal dt) {
|
|||
|
||||
// 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);
|
||||
|
|
@ -239,27 +233,15 @@ void ConstraintSolver::initializeContactConstraints(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;
|
||||
|
||||
// ---------- Penetration ---------- //
|
||||
|
||||
// b = errorValues * oneOverDT;
|
||||
contact.b_Penetration = contact.errorPenetration * oneOverDT;
|
||||
|
||||
// ---------- Friction 1 ---------- //
|
||||
|
||||
contact.b_Friction1 = contact.errorFriction1 * oneOverDT;
|
||||
|
||||
// ---------- Friction 2 ---------- //
|
||||
|
||||
contact.b_Friction2 = contact.errorFriction2 * oneOverDT;
|
||||
contact.b_Penetration = contact.errorPenetration;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
// 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
|
||||
// This method outputs the result in the lambda vector
|
||||
void ConstraintSolver::solveLCP(decimal dt) {
|
||||
|
||||
// for (uint i=0; i<nbConstraints; i++) {
|
||||
// lambda[i] = lambdaInit[i];
|
||||
// }
|
||||
void ConstraintSolver::solveLCP() {
|
||||
|
||||
uint indexBody1Array, indexBody2Array;
|
||||
decimal deltaLambda;
|
||||
|
|
@ -441,7 +365,7 @@ void ConstraintSolver::solveLCP(decimal dt) {
|
|||
uint iter;
|
||||
|
||||
// Compute the vector a
|
||||
computeVectorA(dt);
|
||||
warmStart();
|
||||
|
||||
// For each iteration
|
||||
for(iter=0; iter<mNbIterations; iter++) {
|
||||
|
|
@ -460,59 +384,60 @@ void ConstraintSolver::solveLCP(decimal dt) {
|
|||
|
||||
// --------- Penetration --------- //
|
||||
|
||||
deltaLambda = contact.b_Penetration;
|
||||
deltaLambda = 0.0;
|
||||
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 + 3] * aAngular[indexBody1Array][j] + contact.J_spBody2Penetration[j + 3] * aAngular[indexBody2Array][j]);
|
||||
deltaLambda -= (contact.J_spBody1Penetration[j] * mLinearVelocities[indexBody1Array][j] + contact.J_spBody2Penetration[j] * mLinearVelocities[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;
|
||||
lambdaTemp = contact.penetrationImpulse;
|
||||
contact.penetrationImpulse = std::max(contact.lowerBoundPenetration, std::min(contact.penetrationImpulse + deltaLambda, contact.upperBoundPenetration));
|
||||
deltaLambda = contact.penetrationImpulse - lambdaTemp;
|
||||
for (uint j=0; j<3; j++) {
|
||||
aLinear[indexBody1Array][j] += contact.B_spBody1Penetration[j] * deltaLambda;
|
||||
aAngular[indexBody1Array][j] += contact.B_spBody1Penetration[j + 3] * deltaLambda;
|
||||
mLinearVelocities[indexBody1Array][j] += contact.B_spBody1Penetration[j] * deltaLambda;
|
||||
mAngularVelocities[indexBody1Array][j] += contact.B_spBody1Penetration[j + 3] * deltaLambda;
|
||||
|
||||
aLinear[indexBody2Array][j] += contact.B_spBody2Penetration[j] * deltaLambda;
|
||||
aAngular[indexBody2Array][j] += contact.B_spBody2Penetration[j + 3] * deltaLambda;
|
||||
mLinearVelocities[indexBody2Array][j] += contact.B_spBody2Penetration[j] * deltaLambda;
|
||||
mAngularVelocities[indexBody2Array][j] += contact.B_spBody2Penetration[j + 3] * deltaLambda;
|
||||
}
|
||||
|
||||
// --------- Friction 1 --------- //
|
||||
|
||||
deltaLambda = contact.b_Friction1;
|
||||
deltaLambda = 0.0;
|
||||
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 + 3] * aAngular[indexBody1Array][j] + contact.J_spBody2Friction1[j + 3] * aAngular[indexBody2Array][j]);
|
||||
deltaLambda -= (contact.J_spBody1Friction1[j] * mLinearVelocities[indexBody1Array][j] + contact.J_spBody2Friction1[j] * mLinearVelocities[indexBody2Array][j]);
|
||||
deltaLambda -= (contact.J_spBody1Friction1[j + 3] * mAngularVelocities[indexBody1Array][j] + contact.J_spBody2Friction1[j + 3] * mAngularVelocities[indexBody2Array][j]);
|
||||
}
|
||||
deltaLambda /= contact.inverseFriction1Mass;
|
||||
lambdaTemp = contact.friction1Impulse;
|
||||
contact.friction1Impulse = std::max(contact.lowerBoundFriction1, std::min(contact.friction1Impulse + deltaLambda, contact.upperBoundFriction1));
|
||||
deltaLambda = contact.friction1Impulse - lambdaTemp;
|
||||
for (uint j=0; j<3; j++) {
|
||||
aLinear[indexBody1Array][j] += contact.B_spBody1Friction1[j] * deltaLambda;
|
||||
aAngular[indexBody1Array][j] += contact.B_spBody1Friction1[j + 3] * deltaLambda;
|
||||
mLinearVelocities[indexBody1Array][j] += contact.B_spBody1Friction1[j] * deltaLambda;
|
||||
mAngularVelocities[indexBody1Array][j] += contact.B_spBody1Friction1[j + 3] * deltaLambda;
|
||||
|
||||
aLinear[indexBody2Array][j] += contact.B_spBody2Friction1[j] * deltaLambda;
|
||||
aAngular[indexBody2Array][j] += contact.B_spBody2Friction1[j + 3] * deltaLambda;
|
||||
mLinearVelocities[indexBody2Array][j] += contact.B_spBody2Friction1[j] * deltaLambda;
|
||||
mAngularVelocities[indexBody2Array][j] += contact.B_spBody2Friction1[j + 3] * deltaLambda;
|
||||
}
|
||||
|
||||
// --------- Friction 2 --------- //
|
||||
|
||||
deltaLambda = contact.b_Friction2;
|
||||
deltaLambda = 0.0;
|
||||
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 + 3] * aAngular[indexBody1Array][j] + contact.J_spBody2Friction2[j + 3] * aAngular[indexBody2Array][j]);
|
||||
deltaLambda -= (contact.J_spBody1Friction2[j] * mLinearVelocities[indexBody1Array][j] + contact.J_spBody2Friction2[j] * mLinearVelocities[indexBody2Array][j]);
|
||||
deltaLambda -= (contact.J_spBody1Friction2[j + 3] * mAngularVelocities[indexBody1Array][j] + contact.J_spBody2Friction2[j + 3] * mAngularVelocities[indexBody2Array][j]);
|
||||
}
|
||||
deltaLambda /= contact.inverseFriction2Mass;
|
||||
lambdaTemp = contact.friction2Impulse;
|
||||
contact.friction2Impulse = std::max(contact.lowerBoundFriction2, std::min(contact.friction2Impulse + deltaLambda, contact.upperBoundFriction2));
|
||||
deltaLambda = contact.friction2Impulse - lambdaTemp;
|
||||
for (uint j=0; j<3; j++) {
|
||||
aLinear[indexBody1Array][j] += contact.B_spBody1Friction2[j] * deltaLambda;
|
||||
aAngular[indexBody1Array][j] += contact.B_spBody1Friction2[j + 3] * deltaLambda;
|
||||
mLinearVelocities[indexBody1Array][j] += contact.B_spBody1Friction2[j] * deltaLambda;
|
||||
mAngularVelocities[indexBody1Array][j] += contact.B_spBody1Friction2[j + 3] * deltaLambda;
|
||||
|
||||
aLinear[indexBody2Array][j] += contact.B_spBody2Friction2[j] * deltaLambda;
|
||||
aAngular[indexBody2Array][j] += contact.B_spBody2Friction2[j + 3] * deltaLambda;
|
||||
mLinearVelocities[indexBody2Array][j] += contact.B_spBody2Friction2[j] * 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
|
||||
// Note that a = B * lambda
|
||||
void ConstraintSolver::computeVectorA(decimal dt) {
|
||||
void ConstraintSolver::warmStart() {
|
||||
uint i;
|
||||
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 (uint c=0; c<mNbContactConstraints; c++) {
|
||||
|
|
@ -550,31 +465,31 @@ void ConstraintSolver::computeVectorA(decimal dt) {
|
|||
// --------- Penetration --------- //
|
||||
|
||||
for (uint j=0; j<3; j++) {
|
||||
aLinear[indexBody1Array][j] += contact.B_spBody1Penetration[j] * contact.penetrationImpulse;
|
||||
aAngular[indexBody1Array][j] += contact.B_spBody1Penetration[j + 3] * contact.penetrationImpulse;
|
||||
mLinearVelocities[indexBody1Array][j] += contact.B_spBody1Penetration[j] * contact.penetrationImpulse;
|
||||
mAngularVelocities[indexBody1Array][j] += contact.B_spBody1Penetration[j + 3] * contact.penetrationImpulse;
|
||||
|
||||
aLinear[indexBody2Array][j] += contact.B_spBody2Penetration[j] * contact.penetrationImpulse;
|
||||
aAngular[indexBody2Array][j] += contact.B_spBody2Penetration[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++) {
|
||||
aLinear[indexBody1Array][j] += contact.B_spBody1Friction1[j] * contact.friction1Impulse;
|
||||
aAngular[indexBody1Array][j] += contact.B_spBody1Friction1[j + 3] * contact.friction1Impulse;
|
||||
mLinearVelocities[indexBody1Array][j] += contact.B_spBody1Friction1[j] * contact.friction1Impulse;
|
||||
mAngularVelocities[indexBody1Array][j] += contact.B_spBody1Friction1[j + 3] * contact.friction1Impulse;
|
||||
|
||||
aLinear[indexBody2Array][j] += contact.B_spBody2Friction1[j] * contact.friction1Impulse;
|
||||
aAngular[indexBody2Array][j] += contact.B_spBody2Friction1[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++) {
|
||||
aLinear[indexBody1Array][j] += contact.B_spBody1Friction2[j] * contact.friction2Impulse;
|
||||
aAngular[indexBody1Array][j] += contact.B_spBody1Friction2[j + 3] * contact.friction2Impulse;
|
||||
mLinearVelocities[indexBody1Array][j] += contact.B_spBody1Friction2[j] * contact.friction2Impulse;
|
||||
mAngularVelocities[indexBody1Array][j] += contact.B_spBody1Friction2[j + 3] * contact.friction2Impulse;
|
||||
|
||||
aLinear[indexBody2Array][j] += contact.B_spBody2Friction2[j] * contact.friction2Impulse;
|
||||
aAngular[indexBody2Array][j] += contact.B_spBody2Friction2[j + 3] * contact.friction2Impulse;
|
||||
mLinearVelocities[indexBody2Array][j] += contact.B_spBody2Friction2[j] * 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 upperBoundFriction2;
|
||||
decimal errorPenetration;
|
||||
decimal errorFriction1;
|
||||
decimal errorFriction2;
|
||||
Contact* contact; // TODO : REMOVE THIS
|
||||
decimal b_Penetration;
|
||||
decimal b_Friction1;
|
||||
decimal b_Friction2;
|
||||
decimal B_spBody1Penetration[6];
|
||||
decimal B_spBody2Penetration[6];
|
||||
decimal B_spBody1Friction1[6];
|
||||
|
|
@ -160,8 +156,6 @@ class ConstraintSolver {
|
|||
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 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 penetrationDepths[NB_MAX_CONSTRAINTS]; // Array of penetration depths for error correction projection
|
||||
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)
|
||||
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* V1; // Array that contains for each body the 6x1 vector that contains linear and angular velocities
|
||||
Vector3* W1;
|
||||
Vector3* Vconstraint; // Same kind of vector as V1 but contains the final constraint velocities
|
||||
Vector3* Wconstraint;
|
||||
decimal VconstraintError[6*NB_MAX_BODIES]; // Same kind of vector as V1 but contains the final constraint velocities
|
||||
Vector3* mLinearVelocities; // Array of constrained linear velocities
|
||||
Vector3* mAngularVelocities; // Array of constrained angular velocities
|
||||
decimal mTimeStep; // Current time step
|
||||
|
||||
// Contact constraints
|
||||
ContactConstraint* mContactConstraints;
|
||||
|
|
@ -198,10 +190,9 @@ class ConstraintSolver {
|
|||
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
|
||||
void computeVectorA(decimal dt); // Compute the vector a used in the solve() method
|
||||
void solveLCP(decimal dt); // Solve a LCP problem using Projected-Gauss-Seidel algorithm
|
||||
void warmStart(); // Compute the vector a used in the solve() method
|
||||
void solveLCP(); // Solve a LCP problem using Projected-Gauss-Seidel algorithm
|
||||
|
||||
public:
|
||||
ConstraintSolver(DynamicsWorld* world); // Constructor
|
||||
|
|
@ -223,14 +214,14 @@ inline bool ConstraintSolver::isConstrainedBody(RigidBody* body) const {
|
|||
inline Vector3 ConstraintSolver::getConstrainedLinearVelocityOfBody(RigidBody* body) {
|
||||
assert(isConstrainedBody(body));
|
||||
uint indexBodyArray = mMapBodyToIndex[body];
|
||||
return Vconstraint[indexBodyArray];
|
||||
return mLinearVelocities[indexBodyArray];
|
||||
}
|
||||
|
||||
// Return the constrained angular velocity of a body after solving the LCP problem
|
||||
inline Vector3 ConstraintSolver::getConstrainedAngularVelocityOfBody(RigidBody *body) {
|
||||
assert(isConstrainedBody(body));
|
||||
uint indexBodyArray = mMapBodyToIndex[body];
|
||||
return Wconstraint[indexBodyArray];
|
||||
return mAngularVelocities[indexBodyArray];
|
||||
}
|
||||
|
||||
// Cleanup of the constraint solver
|
||||
|
|
@ -243,51 +234,19 @@ inline void ConstraintSolver::cleanup() {
|
|||
delete[] mContactConstraints;
|
||||
mContactConstraints = 0;
|
||||
}
|
||||
if (Vconstraint != 0) {
|
||||
delete[] Vconstraint;
|
||||
Vconstraint = 0;
|
||||
if (mLinearVelocities != 0) {
|
||||
delete[] mLinearVelocities;
|
||||
mLinearVelocities = 0;
|
||||
}
|
||||
if (Wconstraint != 0) {
|
||||
delete[] Wconstraint;
|
||||
Wconstraint = 0;
|
||||
}
|
||||
if (V1 != 0) {
|
||||
delete[] V1;
|
||||
V1 = 0;
|
||||
}
|
||||
if (W1 != 0) {
|
||||
delete[] W1;
|
||||
W1 = 0;
|
||||
if (mAngularVelocities != 0) {
|
||||
delete[] mAngularVelocities;
|
||||
mAngularVelocities = 0;
|
||||
}
|
||||
}
|
||||
|
||||
// Set the number of iterations of the LCP solver
|
||||
inline void ConstraintSolver::setNbLCPIterations(uint 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
|
||||
|
|
|
|||
|
|
@ -123,15 +123,16 @@ void DynamicsWorld::updateAllBodiesMotion() {
|
|||
newLinearVelocity = mConstraintSolver.getConstrainedLinearVelocityOfBody(*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
|
||||
// external forces and torques.
|
||||
newLinearVelocity += dt * rigidBody->getMassInverse() * rigidBody->getExternalForce();
|
||||
newAngularVelocity += dt * rigidBody->getInertiaTensorInverseWorld() * rigidBody->getExternalTorque();
|
||||
|
||||
// Add the velocity V1 to the new velocity
|
||||
newLinearVelocity += rigidBody->getLinearVelocity();
|
||||
newAngularVelocity += rigidBody->getAngularVelocity();
|
||||
// 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
|
||||
updatePositionAndOrientationOfBody(*it, newLinearVelocity, newAngularVelocity);
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user