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

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

Browse Source
This commit is contained in:
chappuis.daniel 2010-05-19 19:46:11 +00:00
parent a4e7560e21
commit d37110fc38
2 changed files with 43 additions and 39 deletions
sources/reactphysics3d/engine
ConstraintSolver.cppConstraintSolver.h

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

@ -19,7 +19,8 @@
// Libraries // Libraries
#include "ConstraintSolver.h" #include "ConstraintSolver.h"
#include "LCPProjectedGaussSeidel.h" #include "../mathematics/lcp/LCPProjectedGaussSeidel.h"
#include "../body/RigidBody.h"
using namespace reactphysics3d; using namespace reactphysics3d;
@ -136,12 +137,12 @@ void ConstraintSolver::fillInMatrices() {
assert(rigidBody != 0); assert(rigidBody != 0);
// Compute the vector with velocities values // Compute the vector with velocities values
V.fillInSubVector(bodyNumber*6, rigidBody->getCurrentBodyState()->getLinearVelocity()); V.fillInSubVector(bodyNumber*6, rigidBody->getCurrentBodyState().getLinearVelocity());
V.fillInSubVector(bodyNumber*6+3, rigidBody->getCurrentBodyState()->getAngularVelocity()); V.fillInSubVector(bodyNumber*6+3, rigidBody->getCurrentBodyState().getAngularVelocity());
// Compute the vector with forces and torques values // Compute the vector with forces and torques values
Fext.fillInSubVector(bodyNumber*6, rigidBody->getCurrentBodyState()->getExternalForce()); Fext.fillInSubVector(bodyNumber*6, rigidBody->getCurrentBodyState().getExternalForce());
Fext.fillInSubVector(bodyNumber*6+3, rigidBody->getCurrentBodyState()->getExternalTorque()); Fext.fillInSubVector(bodyNumber*6+3, rigidBody->getCurrentBodyState().getExternalTorque());
// Compute the inverse sparse mass matrix // Compute the inverse sparse mass matrix
Minv_sp.fillInSubMatrix(b*6, 0, rigidBody->getCurrentBodyState().getMassInverse().getValue() * Matrix::identity(3)); Minv_sp.fillInSubMatrix(b*6, 0, rigidBody->getCurrentBodyState().getMassInverse().getValue() * Matrix::identity(3));
@ -169,15 +170,17 @@ void ConstraintSolver::computeVectorB(double dt) {
b = errorValues * oneOverDT; b = errorValues * oneOverDT;
// Substract 1.0/dt*J*V to the vector b
for (uint c = 0; c<activeConstraints.size(); c++) { for (uint c = 0; c<activeConstraints.size(); c++) {
// 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 -= oneOverDT * (J_sp(c, 0) * V.getSubVector(indexBody1, 6)); b.setValue(c, b.getValue(c) - oneOverDT * (J_sp.getSubMatrix(c, 0, 1, 6) * V.getSubVector(indexBody1*6, 6)).getValue(0,0));
b -= oneOverDT * (J_sp(c, 1) * V.getSubVector(indexBody2, 6)); b.setValue(c, b.getValue(c) - oneOverDT * (J_sp.getSubMatrix(c, 6, 1, 6) * V.getSubVector(indexBody2*6, 6)).getValue(0,0));
}
// TODO : Continue to implement this method ... compute and remove J*Minv*Fext from b // Substract J*M^-1*F_ext to the vector b
b.setValue(c, b.getValue(c) - ((J_sp.getSubMatrix(c, 0, 1, 6) * Minv_sp.getSubMatrix(indexBody1*6, 0, 6, 6))*Fext.getSubVector(indexBody1*6, 6)
+ (J_sp.getSubMatrix(c, 6, 1, 6) * Minv_sp.getSubMatrix(indexBody2*6, 0, 6, 6))*Fext.getSubVector(indexBody2*6, 6))).getValue(0,0);
}
} }
// Compute the matrix B_sp // Compute the matrix B_sp

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

@ -23,6 +23,7 @@
// Libraries // Libraries
#include "../typeDefinitions.h" #include "../typeDefinitions.h"
#include "../constraint/Constraint.h" #include "../constraint/Constraint.h"
#include "../mathematics/lcp/LCPProjectedGaussSeidel.h"
#include "PhysicsWorld.h" #include "PhysicsWorld.h"
#include <map> #include <map>
@ -40,37 +41,37 @@ const uint MAX_LCP_ITERATIONS = 10; // Maximum number of iterations when sol
*/ */
class ConstraintSolver { class ConstraintSolver {
protected: protected:
LCPSolver& lcpSolver; // LCP Solver LCPSolver& lcpSolver; // LCP Solver
PhysicsWorld& physicsWorld; // Reference to the physics world PhysicsWorld& physicsWorld; // Reference to the physics world
std::vector<Constraint*> activeConstraints; // Current active constraints in the physics world std::vector<Constraint*> activeConstraints; // Current active constraints in the physics world
std::vector<Body*> constraintBodies; // Bodies that are implied in some constraint std::vector<Body*> constraintBodies; // Bodies that are implied in some constraint
uint nbBodies; // Current number of bodies in the physics world uint nbBodies; // Current number of bodies in the physics world
std::map<Body*, uint> bodyNumberMapping; // Map a body pointer with its index number std::map<Body*, uint> bodyNumberMapping; // Map a body pointer with its index number
Body*** bodyMapping; // 2-dimensional array that contains the mapping of body reference Body*** bodyMapping; // 2-dimensional array that contains the mapping of body reference
// in the J_sp and B_sp matrices. For instance the cell bodyMapping[i][j] contains // in the J_sp and B_sp matrices. For instance the cell bodyMapping[i][j] contains
// the pointer to the body that correspond to the 1x6 J_ij matrix in the // the pointer to the body that correspond to the 1x6 J_ij matrix in the
// J_sp matrix. A integer body index refers to its index in the "bodies" std::vector // J_sp matrix. A integer body index refers to its index in the "bodies" std::vector
Matrix J_sp; // Sparse representation of the jacobian matrix of all constraints Matrix J_sp; // Sparse representation of the jacobian matrix of all constraints
Matrix B_sp; // Useful matrix in sparse representation Matrix B_sp; // Useful matrix in sparse representation
Vector b; // Vector "b" of the LCP problem Vector b; // Vector "b" of the LCP problem
Vector lambda; // Lambda vector of the LCP problem Vector lambda; // Lambda vector of the LCP problem
Vector errorValues; // Error vector of all constraints Vector errorValues; // Error vector of all constraints
Vector lowerBounds; // Vector that contains the low limits for the variables of the LCP problem Vector lowerBounds; // Vector that contains the low limits for the variables of the LCP problem
Vector upperBounds; // Vector that contains the high limits for the variables of the LCP problem Vector upperBounds; // Vector that contains the high limits for the variables of the LCP problem
Matrix Minv_sp; // Sparse representation of the Matrix that contains information about mass and inertia of each body Matrix Minv_sp; // Sparse representation of the Matrix that contains information about mass and inertia of each body
Vector V; // Vector that contains linear and angular velocities of each body Vector V; // Vector that contains linear and angular velocities of each body
Vector Fext; // Vector that contains external forces and torques of each body Vector Fext; // Vector that contains external forces and torques of each body
void allocate(); // Allocate all the matrices needed to solve the LCP problem void allocate(); // Allocate all the matrices needed to solve the LCP problem
void fillInMatrices(); // Fill in all the matrices needed to solve the LCP problem void fillInMatrices(); // Fill in all the matrices needed to solve the LCP problem
void freeMemory(); // Free the memory that was allocated in the allocate() method void freeMemory(); // Free the memory that was allocated in the allocate() method
void computeVectorB(double dt); // Compute the vector b void computeVectorB(double dt); // Compute the vector b
void computeMatrixB_sp(); // Compute the matrix B_sp void computeMatrixB_sp(); // Compute the matrix B_sp
public: public:
ConstraintSolver(); // Constructor ConstraintSolver(); // Constructor
virtual ~ConstraintSolver(); // Destructor virtual ~ConstraintSolver(); // Destructor
void solve(double dt); // Solve the current LCP problem void solve(double dt); // Solve the current LCP problem
}; };
} // End of ReactPhysics3D namespace } // End of ReactPhysics3D namespace