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

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

Browse Source
This commit is contained in:
chappuis.daniel 2010-06-06 21:18:49 +00:00
parent 639e91b21b
commit decc06037a
3 changed files with 34 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -205,8 +205,7 @@ void ConstraintSolver::computeVectorB(double dt) {
// Compute the matrix B_sp
void ConstraintSolver::computeMatrixB_sp() {
uint indexBody1;
uint indexBody2;
uint indexBody1, indexBody2;
// For each constraint
for (uint c = 0; c<activeConstraints.size(); c++) {
@ -217,6 +216,29 @@ void ConstraintSolver::computeMatrixB_sp() {
}
}
// Compute the vector V2 according to the formula
// V2 = dt * (M^-1 * J^T * lambda + M^-1 * F_ext) + V1
// Note that we use the vector V to store both V1 and V2 and that at the beginning
// of this method, the vector V already contains the vector V1.
// Note that M^-1 * J^T = B.
// This method is called after that the LCP solver have computed lambda
void ConstraintSolver::computeVectorV(double dt) {
uint indexBody1, indexBody2;
// Compute dt * (M^-1 * J^T * lambda
for (uint i=0; i<activeConstraints.size(); i++) {
indexBody1 = bodyNumberMapping[bodyMapping[i][0]];
indexBody2 = bodyNumberMapping[bodyMapping[i][1]];
V[indexBody1] = V[indexBody1] + (B_sp[indexBody1][i] * lambda.getValue(i)).getVector() * dt;
V[indexBody2] = V[indexBody2] + (B_sp[indexBody2][i] * lambda.getValue(i)).getVector() * dt;
}
// Compute dt * (M^-1 * F_ext)
for (uint i=0; i<nbBodies; i++) {
V[i] = V[i] + (Minv_sp[i] * Fext[i]).getVector() * dt;
}
}
// Solve the current LCP problem
void ConstraintSolver::solve(double dt) {
// Allocate memory for the matrices
@ -234,7 +256,14 @@ void ConstraintSolver::solve(double dt) {
// Solve the LCP problem (computation of lambda)
lcpSolver->solve(J_sp, B_sp, activeConstraints.size(), nbBodies, bodyMapping, bodyNumberMapping, b, lowerBounds, upperBounds, lambda);
// TODO : Implement this method ...
// Compute the vector V2
computeVectorV(dt);
// Update the velocity of each body
// TODO : Put this code somewhere else
for (int i=0; i<nbBodies; i++) {
constraintBodies.at(i);
}
freeMemory();
}

1
sources/reactphysics3d/engine/ConstraintSolver.h
View File

@ -73,6 +73,7 @@ class ConstraintSolver {
void freeMemory(); // Free the memory that was allocated in the allocate() method
void computeVectorB(double dt); // Compute the vector b
void computeMatrixB_sp(); // Compute the matrix B_sp
void computeVectorV(double dt); // Compute the vector V2
public:
ConstraintSolver(PhysicsWorld& world); // Constructor

2
sources/reactphysics3d/engine/PhysicsEngine.cpp
View File

@ -132,7 +132,7 @@ void PhysicsEngine::updateBodyState(RigidBody* const rigidBody, const Time& time
// If the gravity force is on
if(world->getIsGravityOn()) {
// Apply the current gravity force to the body
rigidBody->getCurrentBodyState().setForce(world->getGravity());
rigidBody->getCurrentBodyState().setExternalForce(world->getGravity());
}
// The current body state of the body becomes the previous body state