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

git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@331 92aac97c-a6ce-11dd-a772-7fcde58d38e6

Browse Source
This commit is contained in:
chappuis.daniel 2010-06-10 20:49:07 +00:00
parent ff8c689870
commit 6ff9026fd5
1 changed files with 30 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
sources/reactphysics3d/engine/ConstraintSolver.cpp
View File

@ -58,7 +58,7 @@ void ConstraintSolver::allocate() {
// Fill in the body number maping // Fill in the body number maping
bodyNumberMapping.insert(std::pair<Body*, unsigned int>(constraint->getBody1(), bodyNumberMapping.size())); bodyNumberMapping.insert(std::pair<Body*, unsigned int>(constraint->getBody1(), bodyNumberMapping.size()));
bodyNumberMapping.insert(std::pair<Body*, unsigned int>(constraint->getBody1(), bodyNumberMapping.size())); bodyNumberMapping.insert(std::pair<Body*, unsigned int>(constraint->getBody2(), bodyNumberMapping.size()));
// Update the size of the jacobian matrix // Update the size of the jacobian matrix
nbConstraints += (1 + constraint->getNbAuxConstraints()); nbConstraints += (1 + constraint->getNbAuxConstraints());
@ -93,25 +93,28 @@ void ConstraintSolver::allocate() {
void ConstraintSolver::fillInMatrices() { void ConstraintSolver::fillInMatrices() {
// For each active constraint // For each active constraint
for (uint c=0, nbAuxConstraints=0; c<activeConstraints.size(); c += 1 + nbAuxConstraints) { uint noConstraint = 0;
uint nbAuxConstraints = 0;
for (uint c=0; c<activeConstraints.size(); c++) {
Constraint* constraint = activeConstraints.at(c); Constraint* constraint = activeConstraints.at(c);
// Fill in the J_sp matrix // Fill in the J_sp matrix
J_sp[c][0].changeSize(1, 6); J_sp[noConstraint][0].changeSize(1, 6);
J_sp[c][1].changeSize(1, 6); J_sp[noConstraint][1].changeSize(1, 6);
J_sp[c][0] = constraint->getBody1Jacobian(); J_sp[noConstraint][0] = constraint->getBody1Jacobian();
J_sp[c][1] = constraint->getBody2Jacobian(); J_sp[noConstraint][1] = constraint->getBody2Jacobian();
// Fill in the body mapping matrix // Fill in the body mapping matrix
bodyMapping[c][0] = constraint->getBody1(); bodyMapping[noConstraint][0] = constraint->getBody1();
bodyMapping[c][1] = constraint->getBody2(); bodyMapping[noConstraint][1] = constraint->getBody2();
// Fill in the limit vectors for the constraint // Fill in the limit vectors for the constraint
lowerBounds.setValue(c, constraint->getLowerBound()); lowerBounds.setValue(noConstraint, constraint->getLowerBound());
upperBounds.setValue(c, constraint->getUpperBound()); upperBounds.setValue(noConstraint, constraint->getUpperBound());
// Fill in the error vector // Fill in the error vector
errorValues.setValue(c, constraint->getErrorValue()); errorValues.setValue(noConstraint, constraint->getErrorValue());
nbAuxConstraints = constraint->getNbAuxConstraints(); nbAuxConstraints = constraint->getNbAuxConstraints();
@ -121,20 +124,22 @@ void ConstraintSolver::fillInMatrices() {
// For each auxiliary constraints // For each auxiliary constraints
for (uint i=1; i<=nbAuxConstraints; i++) { for (uint i=1; i<=nbAuxConstraints; i++) {
// Fill in the J_sp matrix // Fill in the J_sp matrix
J_sp[c+i][0].changeSize(1, 6); J_sp[noConstraint+i][0].changeSize(1, 6);
J_sp[c+i][1].changeSize(1, 6); J_sp[noConstraint+i][1].changeSize(1, 6);
J_sp[c+i][0] = constraint->getAuxJacobian().getSubMatrix(i-1, 0, 1, 6); J_sp[noConstraint+i][0] = constraint->getAuxJacobian().getSubMatrix(i-1, 0, 1, 6);
J_sp[c+i][1] = constraint->getAuxJacobian().getSubMatrix(i-1, 6, 1, 6); J_sp[noConstraint+i][1] = constraint->getAuxJacobian().getSubMatrix(i-1, 6, 1, 6);
// Fill in the body mapping matrix // Fill in the body mapping matrix
bodyMapping[c+i][0] = constraint->getBody1(); bodyMapping[noConstraint+i][0] = constraint->getBody1();
bodyMapping[c+i][1] = constraint->getBody2(); bodyMapping[noConstraint+i][1] = constraint->getBody2();
} }
// Fill in the limit vectors for the auxilirary constraints // Fill in the limit vectors for the auxilirary constraints
lowerBounds.fillInSubVector(c+1, constraint->getAuxLowerBounds()); lowerBounds.fillInSubVector(noConstraint+1, constraint->getAuxLowerBounds());
upperBounds.fillInSubVector(c+1, constraint->getAuxUpperBounds()); upperBounds.fillInSubVector(noConstraint+1, constraint->getAuxUpperBounds());
} }
noConstraint += 1 + nbAuxConstraints;
} }
// For each current body that is implied in some constraint // For each current body that is implied in some constraint
@ -202,11 +207,12 @@ void ConstraintSolver::computeVectorB(double dt) {
b = errorValues * oneOverDT; b = errorValues * oneOverDT;
for (uint c = 0; c<nbConstraints; c++) { for (uint c = 0; c<nbConstraints; c++) {
int size1 = J_sp[c][0].getNbColumn(); // TODO : Delete this
// Substract 1.0/dt*J*V to the vector b // Substract 1.0/dt*J*V to the vector b
indexBody1 = bodyNumberMapping[bodyMapping[c][0]]; indexBody1 = bodyNumberMapping[bodyMapping[c][0]];
indexBody2 = bodyNumberMapping[bodyMapping[c][1]]; indexBody2 = bodyNumberMapping[bodyMapping[c][1]];
b.setValue(c, b.getValue(c) - oneOverDT * (J_sp[c][0] * V[indexBody1]).getValue(0,0)); b.setValue(c, b.getValue(c) - (J_sp[c][0] * V[indexBody1]).getValue(0,0) * oneOverDT);
b.setValue(c, b.getValue(c) - oneOverDT * (J_sp[c][1] * V[indexBody2]).getValue(0,0)); b.setValue(c, b.getValue(c) - (J_sp[c][1] * V[indexBody2]).getValue(0,0) * oneOverDT);
// Substract J*M^-1*F_ext to the vector b // Substract J*M^-1*F_ext to the vector b
b.setValue(c, b.getValue(c) - ((J_sp[c][0] * Minv_sp[indexBody1]) * Fext[indexBody1] b.setValue(c, b.getValue(c) - ((J_sp[c][0] * Minv_sp[indexBody1]) * Fext[indexBody1]
@ -277,7 +283,9 @@ void ConstraintSolver::solve(double dt) {
// Update the velocity of each body // Update the velocity of each body
// TODO : Put this code somewhere else // TODO : Put this code somewhere else
for (int i=0; i<nbBodies; i++) { for (int i=0; i<nbBodies; i++) {
constraintBodies.at(i); RigidBody* body = dynamic_cast<RigidBody*>(constraintBodies.at(i));
//std::cout << "Velocity Y before : " << body->getCurrentBodyState().getLinearVelocity().getY() << std::endl;
//std::cout << "Velocity Y after : " << V[bodyNumberMapping[constraintBodies.at(i)]].getValue(1) << std::endl;
} }
freeMemory(); freeMemory();