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

Optimizations in the constraint solver

Browse Source 
git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@406 92aac97c-a6ce-11dd-a772-7fcde58d38e6
This commit is contained in:
chappuis.daniel 2010-09-14 20:03:36 +00:00
parent ddf602c125
commit 6b4e5c0fa2
3 changed files with 29 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
src/engine/ConstraintSolver.cpp
View File

@ -114,9 +114,9 @@ void ConstraintSolver::allocate() {
bodiesCapacity = nbBodies;
Minv_sp = new Matrix6x6[nbBodies];
V1 = new Vector[nbBodies];
Vconstraint = new Vector[nbBodies];
Fext = new Vector[nbBodies];
V1 = new Vector6D[nbBodies];
Vconstraint = new Vector6D[nbBodies];
Fext = new Vector6D[nbBodies];
avBodiesNumber = 0;
avBodiesCounter = 0;
@ -231,10 +231,6 @@ void ConstraintSolver::fillInMatrices() {
// For each current body that is implied in some constraint
RigidBody* rigidBody;
Body* body;
Vector v(6);
Vector f(6);
//Matrix identity = Matrix::identity(3);
//Matrix mInv(6,6);
uint b=0;
for (set<Body*>::iterator it = constraintBodies.begin(); it != constraintBodies.end(); it++, b++) {
body = *it;
@ -245,20 +241,23 @@ void ConstraintSolver::fillInMatrices() {
assert(rigidBody != 0);
// Compute the vector V1 with initial velocities values
v.fillInSubVector(0, rigidBody->getLinearVelocity());
v.fillInSubVector(3, rigidBody->getAngularVelocity());
V1[bodyNumber].changeSize(6);
V1[bodyNumber] = v;
V1[bodyNumber].setValue(0, rigidBody->getLinearVelocity().getValue(0));
V1[bodyNumber].setValue(1, rigidBody->getLinearVelocity().getValue(1));
V1[bodyNumber].setValue(2, rigidBody->getLinearVelocity().getValue(2));
V1[bodyNumber].setValue(3, rigidBody->getAngularVelocity().getValue(0));
V1[bodyNumber].setValue(4, rigidBody->getAngularVelocity().getValue(1));
V1[bodyNumber].setValue(5, rigidBody->getAngularVelocity().getValue(2));
// Compute the vector Vconstraint with final constraint velocities
Vconstraint[bodyNumber].changeSize(6);
Vconstraint[bodyNumber].initWithValue(0.0);
// Compute the vector with forces and torques values
f.fillInSubVector(0, rigidBody->getExternalForce());
f.fillInSubVector(3, rigidBody->getExternalTorque());
Fext[bodyNumber].changeSize(6);
Fext[bodyNumber] = f;
Fext[bodyNumber].setValue(0, rigidBody->getExternalForce().getValue(0));
Fext[bodyNumber].setValue(1, rigidBody->getExternalForce().getValue(1));
Fext[bodyNumber].setValue(2, rigidBody->getExternalForce().getValue(2));
Fext[bodyNumber].setValue(3, rigidBody->getExternalTorque().getValue(0));
Fext[bodyNumber].setValue(4, rigidBody->getExternalTorque().getValue(1));
Fext[bodyNumber].setValue(5, rigidBody->getExternalTorque().getValue(2));
// Compute the inverse sparse mass matrix
Minv_sp[bodyNumber].initWithValue(0.0);
@ -276,11 +275,7 @@ void ConstraintSolver::fillInMatrices() {
Minv_sp[bodyNumber].setValue(5, 3, tensorInv.getValue(2, 0));
Minv_sp[bodyNumber].setValue(5, 4, tensorInv.getValue(2, 1));
Minv_sp[bodyNumber].setValue(5, 5, tensorInv.getValue(2, 2));
//mInv.fillInSubMatrix(0, 0, rigidBody->getMassInverse() * identity);
//mInv.fillInSubMatrix(3, 3, rigidBody->getInertiaTensorInverseWorld());
}
//Minv_sp[bodyNumber].changeSize(6, 6);
//Minv_sp[bodyNumber] = mInv;
}
}
@ -295,12 +290,12 @@ void ConstraintSolver::computeVectorB(double dt) {
// Substract 1.0/dt*J*V to the vector b
indexBody1 = bodyNumberMapping[bodyMapping[c][0]];
indexBody2 = bodyNumberMapping[bodyMapping[c][1]];
b.setValue(c, b.getValue(c) - (Matrix(J_sp[c][0]) * V1[indexBody1]).getValue(0,0) * oneOverDT); // TODO : Remove conversion here
b.setValue(c, b.getValue(c) - (Matrix(J_sp[c][1]) * V1[indexBody2]).getValue(0,0) * oneOverDT);
b.setValue(c, b.getValue(c) - (J_sp[c][0] * V1[indexBody1]) * oneOverDT);
b.setValue(c, b.getValue(c) - (J_sp[c][1] * V1[indexBody2]) * oneOverDT);
// Substract J*M^-1*F_ext to the vector b
b.setValue(c, b.getValue(c) - ((Matrix(J_sp[c][0]) * Matrix(Minv_sp[indexBody1])) * Fext[indexBody1]
+ (Matrix(J_sp[c][1]) * Matrix(Minv_sp[indexBody2]))*Fext[indexBody2]).getValue(0,0)); // TODO : Delete conversion here
b.setValue(c, b.getValue(c) - ((J_sp[c][0] * Minv_sp[indexBody1]) * Fext[indexBody1]
+ (J_sp[c][1] * Minv_sp[indexBody2])*Fext[indexBody2]));
}
}
@ -312,8 +307,6 @@ void ConstraintSolver::computeMatrixB_sp() {
for (uint c = 0; c<nbConstraints; c++) {
indexBody1 = bodyNumberMapping[bodyMapping[c][0]];
indexBody2 = bodyNumberMapping[bodyMapping[c][1]];
//B_sp[0][c].changeSize(6,1);
//B_sp[1][c].changeSize(6,1);
B_sp[0][c] = Minv_sp[indexBody1] * J_sp[c][0].getTranspose();
B_sp[1][c] = Minv_sp[indexBody2] * J_sp[c][1].getTranspose();
}
@ -332,8 +325,8 @@ void ConstraintSolver::computeVectorVconstraint(double dt) {
for (uint i=0; i<nbConstraints; i++) {
indexBody1 = bodyNumberMapping[bodyMapping[i][0]];
indexBody2 = bodyNumberMapping[bodyMapping[i][1]];
Vconstraint[indexBody1] = Vconstraint[indexBody1] + (Matrix(B_sp[0][i]) * lambda.getValue(i)).getVector() * dt;
Vconstraint[indexBody2] = Vconstraint[indexBody2] + (Matrix(B_sp[1][i]) * lambda.getValue(i)).getVector() * dt; // TODO : Remove conversion here
Vconstraint[indexBody1] = Vconstraint[indexBody1] + (B_sp[0][i] * lambda.getValue(i)) * dt;
Vconstraint[indexBody2] = Vconstraint[indexBody2] + (B_sp[1][i] * lambda.getValue(i)) * dt;
}
}

12
src/engine/ConstraintSolver.h
View File

@ -87,10 +87,10 @@ class ConstraintSolver {
Vector upperBounds; // Vector that contains the high limits for the variables of the LCP problem
Matrix6x6* Minv_sp; // Sparse representation of the Matrix that contains information about mass and inertia of each body
// This is an array of size nbBodies that contains in each cell a 6x6 matrix
Vector* V1; // Array that contains for each body the Vector that contains linear and angular velocities
Vector6D* V1; // Array that contains for each body the Vector that contains linear and angular velocities
// Each cell contains a 6x1 vector with linear and angular velocities
Vector* Vconstraint; // Same kind of vector as V1 but contains the final constraint velocities
Vector* Fext; // Array that contains for each body the vector that contains external forces and torques
Vector6D* Vconstraint; // Same kind of vector as V1 but contains the final constraint velocities
Vector6D* Fext; // Array that contains for each body the vector that contains external forces and torques
// Each cell contains a 6x1 vector with external force and torque.
void initialize(); // Initialize the constraint solver before each solving
void allocate(); // Allocate all the memory needed to solve the LCP problem
@ -122,7 +122,7 @@ inline bool ConstraintSolver::isConstrainedBody(Body* body) const {
// Return the constrained linear velocity of a body after solving the LCP problem
inline Vector3D ConstraintSolver::getConstrainedLinearVelocityOfBody(Body* body) {
assert(isConstrainedBody(body));
Vector vec = Vconstraint[bodyNumberMapping[body]].getSubVector(0, 3);
const Vector6D& vec = Vconstraint[bodyNumberMapping[body]];
return Vector3D(vec.getValue(0), vec.getValue(1), vec.getValue(2));
}
@ -130,8 +130,8 @@ inline Vector3D ConstraintSolver::getConstrainedLinearVelocityOfBody(Body* body)
// Return the constrained angular velocity of a body after solving the LCP problem
inline Vector3D ConstraintSolver::getConstrainedAngularVelocityOfBody(Body* body) {
assert(isConstrainedBody(body));
Vector vec = Vconstraint[bodyNumberMapping[body]].getSubVector(3, 3);
return Vector3D(vec.getValue(0), vec.getValue(1), vec.getValue(2));
const Vector6D& vec = Vconstraint[bodyNumberMapping[body]];
return Vector3D(vec.getValue(3), vec.getValue(4), vec.getValue(5));
}
// Cleanup of the constraint solver

7
src/mathematics/lcp/LCPProjectedGaussSeidel.cpp
View File

@ -48,23 +48,20 @@ void LCPProjectedGaussSeidel::solve(Matrix1x6** J_sp, Vector6D** B_sp, uint nbCo
double lambdaTemp;
uint i, iter;
Vector6D* a = new Vector6D[nbBodies]; // Array that contains nbBodies vector of dimension 6x1
//for (i=0; i<nbBodies; i++) {
// a[i].changeSize(6);
//}
// Compute the vector a
computeVectorA(lambda, nbConstraints, bodyMapping, B_sp, bodyNumberMapping, a, nbBodies);
// For each constraint
for (i=0; i<nbConstraints; i++) {
d[i] = (Matrix(J_sp[i][0]) * B_sp[0][i] + Matrix(J_sp[i][1]) * B_sp[1][i]).getValue(0,0); // TODO : Remove conversion here
d[i] = (J_sp[i][0] * B_sp[0][i] + J_sp[i][1] * B_sp[1][i]);
}
for(iter=0; iter<maxIterations; iter++) {
for (i=0; i<nbConstraints; i++) {
indexBody1 = bodyNumberMapping[bodyMapping[i][0]];
indexBody2 = bodyNumberMapping[bodyMapping[i][1]];
deltaLambda = (b.getValue(i) - (Matrix(J_sp[i][0]) * a[indexBody1]).getValue(0,0) - (Matrix(J_sp[i][1]) * a[indexBody2]).getValue(0,0)) / d[i]; // TODO : Remove conversion here
deltaLambda = (b.getValue(i) - (J_sp[i][0] * a[indexBody1]) - (J_sp[i][1] * a[indexBody2])) / d[i];
lambdaTemp = lambda.getValue(i);
lambda.setValue(i, std::max(lowLimits.getValue(i), std::min(lambda.getValue(i) + deltaLambda, highLimits.getValue(i))));
deltaLambda = lambda.getValue(i) - lambdaTemp;