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

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

Browse Source
This commit is contained in:
chappuis.daniel 2010-03-17 19:19:11 +00:00
parent 3eb315cb0c
commit bcdf9e92dd
2 changed files with 116 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -19,58 +19,124 @@
// Libraries
#include "ConstraintSolver.h"
#include "LCPProjectedGaussSeidel.h"
using namespace reactphysics3d;
// Constructor
ConstraintSolver::ConstraintSolver() {
ConstraintSolver::ConstraintSolver()
:bodies(0), nbBodies(0), bodyMapping(0) {
// Creation of the LCP Solver
lcpSolver = new LCPProjectedGaussSeidel(MAX_LCP_ITERATIONS);
}
// Destructor
ConstraintSolver~ConstraintSolver() {
ConstraintSolver::~ConstraintSolver() {
}
// Allocate all the matrices needed to solve the LCP problem
void ConstraintSolver::allocate(std::vector<Constraint*>& constraints, double dt) {
std::vector<Constraint*> activeConstraints;
void ConstraintSolver::allocate(std::vector<Constraint*>& constraints, std::vector<Body*>& bodies) {
unsigned int sizeJacobian = 0;
unsigned int nbAuxiliaryVars = 0;
this->bodies = bodies;
this->nbBodies = bodies.size();
// For each constraint
for (unsigned int i=0; i<constraints.size(); ++i) {
// Evaluate the constraint
constraints.at(i)->evaluate(dt);
// If the constraint is active
if (constraints.at(i)->isActive()) {
activeConstraints.push_back(constraints.at(i));
// Evaluate the constraint
constraints.at(i)->evaluate(dt);
// Set the current jacobian index to the constraint
constraints.at(i)->setJacobianIndex(sizeJacobian);
// Update the size of the jacobian matrix
sizeJacobian += constraints.at(i)->getNbJacobianRows();
/Description/ Update the size of the jacobian matrix
sizeJacobian += (1 + constraints.at(i)->getNbAuxConstraints());
}
}
// For each active constraint
for (unsigned int i=0; i<activeConstraints.size(); ++i) {
// Set the auxiliary index
activeConstraints.at(i)->setAuxiliaryIndex(sizeJacobian + nbAuxiliaryVars);
// Allocate all the vectors and matrices
J_sp = Matrix(sizeJacobian, 12);
// Update the number of auxiliary variables
nbAuxiliaryVars += activeConstraints.at(i)->getNbAuxiliaryVars();
bodyMapping = new unsigned int[nbBodies];
for (unsigned int i=0; i<nbBodies; i++) {
bodyMapping[i] = new unsigned int[2];
}
// TODO : Continue to implement this ...
errorVector = Vector(sizeJacobian);
B_sp = Matrix(6*nbBodies, 2;
b = Vector(totalSize);
lambda = Vector(totalSize);
lowLimits = Vector(totalSize);
highLimits = Vector(totalSize);
Minv_sp = Matrix(3*nbBodies, 6*nbBodies);
V = Vector(6*nbBodies);
Fc = Vector(6*nbBodies);
}
// Fill in all the matrices needed to solve the LCP problem
void ConstraintSolver::fillInMatrices(std::vector<Constraint*> constraints) {
// Notice that all the active constraints should have been evaluated first
void ConstraintSolver::fillInMatrices() {
int i,j;
// For each active constraint
for (unsigned int c=0; c<activeConstraints.size(); ++c) {
i = activeConstraints.at(c)->getJacobianIndex();
//j = i + activeConstraint.at(c)->getNbJacobianRows();
J.fillInSubMatrix(i, 0, activeConstraints.at(c)->getBody1LinearJacobian());
J.fillInSubMatrix(i, 3, activeConstraints.at(c)->getBody2LinearJacobian());
J.fillInSubMatrix(i, 6, activeConstraints.at(c)->getBody1AngularJacobian());
J.fillInSubMatrix(i, 9, activeConstraints.at(c)->getBody2AngularJacobian());
errorVector.fillInSubVector(i, activeConstraints.at(c)->getErrorVector());
}
// For each active constraint
for (unsigned int c=0; c<activeConstraints.size(); ++c) {
i = activeConstraints.at(c)->getNbAuxiliaryVars();
// TODO : add activeConstraints.at(c)->getAuxiliaryRowsAndCols(..., ...)
b.fillInSubVector(i, activeConstraints.at(c)->getRightHandSideVector());
}
// For each current body of the simulation
for (unsigned int b=0; b<nbBodies; ++b) {
i = 6*b;
Minv.fillInSubMatrix(i, 0, bodies.at(b)->getCurrentBodyState().getMassInverse().getValue() * Matrix::identity(3));
Minv.fillInSubMatrix(i+3, 3, bodies.at(b)->getCurrentBodyState().getInertiaTensorInverse());
u.fillInSubVector(i, bodies.at(b)->getCurrentBodyState().getLinearVelocity());
u.fillInSubVector(i+3, bodies.at(b)->getCurrentBodyState().getAngularVelocity());
fExt.fillInSubVector(i, bodies.at(b)->getCurrentBodyState().getExternalForce());
Vector3D externalTorque = bodies.at(b)->getCurrentBodyState().getExternalTorque();
Matrix3x3 inertia = bodies.at(b)->getInertiaTensor();
Vector3D angularVelocity = bodies.at(b)->getCurrentBodyState().getAngularVelocity();
fExt.fillInSubVector(i+3, externalTorque - (angularVelocity.crossProduct(inertia*angularVelocity)));
}
}
// Free the memory that was allocated in the allocate() method
void ConstraintSolver::freeMemory() {
// Free the bodyMaping array
for (unsigned int i=0; i<nbBodies; i++) {
delete[] bodyMapping[i];
}
delete[] bodyMapping;
}
// Solve the current LCP problem
void ConstraintSolver::solve(std::vector<Constraint*> constraints, double dt) {
void ConstraintSolver::solve(std::vector<Constraint*>& constraints, std::vector<Body*>* bodies, double dt) {
// Update the current set of bodies in the physics world
this->bodies = bodies;
// Delete all the current actice constraints
activeConstraints.clear();
// Allocate memory for the matrices
allocate(constraints, dt);
// Fill-in all the matrices needed to solve the LCP problem
fillInMatrices();
// Solve the LCP problem (computation of lambda)
lcpSolver.solve(A, b, lowLimits, highLimits, lambda);
// TODO : Implement this method ...
}

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

@ -26,6 +26,9 @@
// ReactPhysics3D namespace
namespace reactphysics3d {
// Constants
const unsigned int MAX_LCP_ITERATIONS = 10; // Maximum number of iterations when solving a LCP problem
/* -------------------------------------------------------------------
Class ConstrainSolver :
This class represents the constraint solver. The goal is to
@ -34,13 +37,32 @@ namespace reactphysics3d {
*/
class ConstraintSolver {
protected:
void allocate(std::vector<Constraint*>& constraints, double dt); // Allocate all the matrices needed to solve the LCP problem
void fillInMatrices(std::vector<Constraint*> constraints); // Fill in all the matrices needed to solve the LCP problem
LCPSolver* lcpSolver; // LCP Solver
std::vector<Constraint*> activeConstraints; // Current active constraints in the physics world
std::vector<Body*> bodies; // Set of bodies in the physics world
unsigned int nbBodies; // Number of bodies in the physics world
unsigned int** bodyMapping // 2-dimensional array that contains the mapping of body index
// in the J_sp and B_sp matrices. For instance the cell bodyMapping[i][j] contains
// the integer index of 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
Matrix J_sp; // Sparse representation of the jacobian matrix of all constraints
Matrix B_sp; // Useful matrix in sparse representation
Vector b; // Vector "b" of the LCP problem
Vector lambda; // Lambda vector of the LCP problem
Vector errorVector; // Error vector of all constraints
Vector lowLimits; // Vector that contains the low limits of the LCP problem
Vector highLimits; // Vector that contains the high limits of the LCP problem
Matrix Minv_sp; // Sparse representation of the Matrix that contains information about mass and inertia of each body
Vector V; // Vector that contains internal linear and angular velocities of each body
Vector Fc; // Vector that contains internal forces and torques of each body due to constraints
void allocate(std::vector<Constraint*>& constraints); // Allocate 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
public:
ConstraintSolver(); // Constructor
virtual ~ConstraintSolver(); // Destructor
void solve(std::vector<Constraint*>& constraints, double dt); // Solve the current LCP problem
void solve(std::vector<Constraint*>& constraints, std::vector<Body*>* bodies, double dt); // Solve the current LCP problem
};
} // End of ReactPhysics3D namespace