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

sources/reactphysics3d/engine/ConstraintSolver.cpp

@@ -25,8 +25,8 @@
 using namespace reactphysics3d;
 
 // Constructor
-ConstraintSolver::ConstraintSolver(PhysicsWorld& world)
+ConstraintSolver::ConstraintSolver(PhysicsWorld* world)
-                 :physicsWorld(world), bodyMapping(0), lcpSolver(new LCPProjectedGaussSeidel(MAX_LCP_ITERATIONS)) {
+                 :physicsWorld(world), bodyMapping(0), nbConstraints(0), lcpSolver(new LCPProjectedGaussSeidel(MAX_LCP_ITERATIONS)) {
 
 }
 
@@ -37,12 +37,12 @@ ConstraintSolver::~ConstraintSolver() {
 
 // Allocate all the matrices needed to solve the LCP problem
 void ConstraintSolver::allocate() {
-    uint nbConstraints = 0;
+    nbConstraints = 0;
     Constraint* constraint;
 
     // For each constraint
     std::vector<Constraint*>::iterator it;
-    for (it = physicsWorld.getConstraintsBeginIterator(); it <physicsWorld.getConstraintsEndIterator(); it++) {
+    for (it = physicsWorld->getConstraintsBeginIterator(); it <physicsWorld->getConstraintsEndIterator(); it++) {
         constraint = *it;
 
         // Evaluate the constraint
@@ -71,17 +71,18 @@ void ConstraintSolver::allocate() {
     bodyMapping = new Body**[nbConstraints];
     J_sp = new Matrix*[nbConstraints];
     B_sp = new Matrix*[2];
+    B_sp[0] = new Matrix[nbConstraints];
+    B_sp[1] = new Matrix[nbConstraints];
     for (uint i=0; i<nbConstraints; i++) {
         bodyMapping[i] = new Body*[2];
         J_sp[i] = new Matrix[2];
-        B_sp[i] = new Matrix[nbConstraints];
     }
 
-    errorValues = Vector(nbConstraints);
+    errorValues.changeSize(nbConstraints);
-    b = Vector(nbConstraints);
+    b.changeSize(nbConstraints);
-    lambda = Vector(nbConstraints);
+    lambda.changeSize(nbConstraints);
-    lowerBounds = Vector(nbConstraints);
+    lowerBounds.changeSize(nbConstraints);
-    upperBounds = Vector(nbConstraints);
+    upperBounds.changeSize(nbConstraints);
     Minv_sp = new Matrix[nbBodies];
     V = new Vector[nbBodies];
     Fext = new Vector[nbBodies];
@@ -92,26 +93,27 @@ void ConstraintSolver::allocate() {
 void ConstraintSolver::fillInMatrices() {
 
     // For each active constraint
-    for (uint c=0; c<activeConstraints.size(); c++) {
+    for (uint c=0, nbAuxConstraints=0; c<activeConstraints.size(); c += 1 + nbAuxConstraints) {
         Constraint* constraint = activeConstraints.at(c);
 
         // Fill in the J_sp matrix
+        J_sp[c][0].changeSize(1, 6);
+        J_sp[c][1].changeSize(1, 6);
         J_sp[c][0] = constraint->getBody1Jacobian();
         J_sp[c][1] = constraint->getBody2Jacobian();
 
-
         // Fill in the body mapping matrix
         bodyMapping[c][0] = constraint->getBody1();
         bodyMapping[c][1] = constraint->getBody2();
 
         // Fill in the limit vectors for the constraint
-        lowerBounds.fillInSubVector(c, constraint->getLowerBound());
+        lowerBounds.setValue(c, constraint->getLowerBound());
-        upperBounds.fillInSubVector(c, constraint->getUpperBound());
+        upperBounds.setValue(c, constraint->getUpperBound());
 
         // Fill in the error vector
-        errorValues.fillInSubVector(c, constraint->getErrorValue());
+        errorValues.setValue(c, constraint->getErrorValue());
 
-        uint nbAuxConstraints = constraint->getNbAuxConstraints();
+        nbAuxConstraints = constraint->getNbAuxConstraints();
 
         // If the current constraint has auxiliary constraints
         if (nbAuxConstraints > 0) {
@@ -119,6 +121,8 @@ void ConstraintSolver::fillInMatrices() {
             // For each auxiliary constraints
             for (uint i=1; i<=nbAuxConstraints; i++) {
                 // Fill in the J_sp matrix
+                J_sp[c+i][0].changeSize(1, 6);
+                J_sp[c+i][1].changeSize(1, 6);
                 J_sp[c+i][0] = constraint->getAuxJacobian().getSubMatrix(i-1, 0, 1, 6);
                 J_sp[c+i][1] = constraint->getAuxJacobian().getSubMatrix(i-1, 6, 1, 6);
 
@@ -149,11 +153,13 @@ void ConstraintSolver::fillInMatrices() {
         // Compute the vector with velocities values
         v.fillInSubVector(0, rigidBody->getCurrentBodyState().getLinearVelocity());
         v.fillInSubVector(3, rigidBody->getCurrentBodyState().getAngularVelocity());
+        V[bodyNumber].changeSize(6);
         V[bodyNumber] = v;
         
         // Compute the vector with forces and torques values
         f.fillInSubVector(0, rigidBody->getCurrentBodyState().getExternalForce());
         f.fillInSubVector(3, rigidBody->getCurrentBodyState().getExternalTorque());
+        Fext[bodyNumber].changeSize(6);
         Fext[bodyNumber] = f;
 
         // Compute the inverse sparse mass matrix
@@ -161,6 +167,7 @@ void ConstraintSolver::fillInMatrices() {
         mInv.initWithValue(0.0);
         mInv.fillInSubMatrix(0, 0, rigidBody->getCurrentBodyState().getMassInverse().getValue() * Matrix::identity(3));
         mInv.fillInSubMatrix(3, 3, rigidBody->getCurrentBodyState().getInertiaTensorInverse());
+        Minv_sp[bodyNumber].changeSize(6, 6);
         Minv_sp[bodyNumber] = mInv;
     }
 }
@@ -170,13 +177,17 @@ void ConstraintSolver::freeMemory() {
 
     activeConstraints.clear();
     bodyNumberMapping.clear();
+    constraintBodies.clear();
 
     // Free the bodyMaping array
-    for (uint i=0; i<nbBodies; i++) {
+    for (uint i=0; i<nbConstraints; i++) {
         delete[] bodyMapping[i];
+        delete[] J_sp[i];
     }
     delete[] bodyMapping;
     delete[] J_sp;
+    delete[] B_sp[0];
+    delete[] B_sp[1];
     delete[] B_sp;
     delete[] Minv_sp;
     delete[] V;
@@ -190,7 +201,7 @@ void ConstraintSolver::computeVectorB(double dt) {
     
     b = errorValues * oneOverDT;
 
-    for (uint c = 0; c<activeConstraints.size(); c++) {
+    for (uint c = 0; c<nbConstraints; c++) {
         // Substract 1.0/dt*J*V to the vector b
         indexBody1 = bodyNumberMapping[bodyMapping[c][0]];
         indexBody2 = bodyNumberMapping[bodyMapping[c][1]];
@@ -208,9 +219,11 @@ void ConstraintSolver::computeMatrixB_sp() {
     uint indexBody1, indexBody2;
 
     // For each constraint
-    for (uint c = 0; c<activeConstraints.size(); c++) {
+    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();
     }
@@ -226,7 +239,7 @@ void ConstraintSolver::computeVectorV(double dt) {
     uint indexBody1, indexBody2;
 
     // Compute dt * (M^-1 * J^T * lambda
-    for (uint i=0; i<activeConstraints.size(); i++) {
+    for (uint i=0; i<nbConstraints; i++) {
         indexBody1 = bodyNumberMapping[bodyMapping[i][0]];
         indexBody2 = bodyNumberMapping[bodyMapping[i][1]];
         V[indexBody1] = V[indexBody1] + (B_sp[indexBody1][i] * lambda.getValue(i)).getVector() * dt;
@@ -254,7 +267,9 @@ void ConstraintSolver::solve(double dt) {
     computeMatrixB_sp();
 
     // Solve the LCP problem (computation of lambda)
-    lcpSolver->solve(J_sp, B_sp, activeConstraints.size(), nbBodies, bodyMapping, bodyNumberMapping, b, lowerBounds, upperBounds, lambda);
+    Vector lambdaInit(nbConstraints);
+    lcpSolver->setLambdaInit(lambdaInit);
+    lcpSolver->solve(J_sp, B_sp, nbConstraints, nbBodies, bodyMapping, bodyNumberMapping, b, lowerBounds, upperBounds, lambda);
 
     // Compute the vector V2
     computeVectorV(dt);

sources/reactphysics3d/engine/ConstraintSolver.h

@@ -41,9 +41,10 @@ const uint MAX_LCP_ITERATIONS = 10;     // Maximum number of iterations when sol
 */
 class ConstraintSolver {
     protected:
-        PhysicsWorld& physicsWorld;                             // Reference to the physics world
+        PhysicsWorld* physicsWorld;                             // Reference to the physics world
         LCPSolver* lcpSolver;                                   // LCP Solver
         std::vector<Constraint*> activeConstraints;             // Current active constraints in the physics world
+        uint nbConstraints;                                     // Total number of constraints (with the auxiliary constraints)
         std::vector<Body*> constraintBodies;                    // Bodies that are implied in some constraint
         uint nbBodies;                                          // Current number of bodies in the physics world
         std::map<Body*, uint> bodyNumberMapping;                // Map a body pointer with its index number
@@ -76,7 +77,7 @@ class ConstraintSolver {
         void computeVectorV(double dt);                         // Compute the vector V2
 
     public:
-        ConstraintSolver(PhysicsWorld& world);                  // Constructor
+        ConstraintSolver(PhysicsWorld* world);                  // Constructor
         virtual ~ConstraintSolver();                            // Destructor
         void solve(double dt);                                  // Solve the current LCP problem
 };

sources/reactphysics3d/engine/PhysicsEngine.cpp

@@ -26,7 +26,7 @@ using namespace reactphysics3d;
 
 // Constructor
 PhysicsEngine::PhysicsEngine(PhysicsWorld* world, const Time& timeStep) throw (std::invalid_argument)
-              : world(world), timer(Time(0.0), timeStep), collisionDetection(world) {
+              : world(world), timer(Time(0.0), timeStep), collisionDetection(world), constraintSolver(world) {
     // Check if the pointer to the world is not NULL
     if (world == 0) {
         // Throw an exception
@@ -89,13 +89,18 @@ void PhysicsEngine::updateCollision() {
         // Compute the collision detection
         if (collisionDetection.computeCollisionDetection()) {
             // TODO : Delete this ----------------------------------------------------------
+            /*
             for (std::vector<Constraint*>::iterator it = world->getConstraintsBeginIterator(); it != world->getConstraintsEndIterator(); ++it) {
                 RigidBody* rigidBody1 = dynamic_cast<RigidBody*>((*it)->getBody1());
                 RigidBody* rigidBody2 = dynamic_cast<RigidBody*>((*it)->getBody2());
                 rigidBody1->setIsMotionEnabled(false);
                 rigidBody2->setIsMotionEnabled(false);
             }
+            */
             // -----------------------------------------------------------------------------
+
+            // Solve constraints
+            constraintSolver.solve(timer.getTimeStep().getValue());
         }
 
         // For each body in the dynamic world

sources/reactphysics3d/engine/PhysicsEngine.h

@@ -24,6 +24,7 @@
 #include "PhysicsWorld.h"
 #include "../integration/IntegrationAlgorithm.h"
 #include "../collision/CollisionDetection.h"
+#include "ConstraintSolver.h"
 #include "../body/RigidBody.h"
 #include "Timer.h"
 
@@ -42,6 +43,7 @@ class PhysicsEngine {
         Timer timer;                                    // Timer of the physics engine
         IntegrationAlgorithm* integrationAlgorithm;     // Integration algorithm used to solve differential equations of movement
         CollisionDetection collisionDetection;          // Collision detection
+        ConstraintSolver constraintSolver;              // Constraint solver
 
         void updateBodyState(RigidBody* const rigidBody, const Time& timeStep);           // Update the state of a rigid body
 

sources/reactphysics3d/mathematics/Matrix.cpp

@@ -304,7 +304,7 @@ double Matrix::getTrace() const throw(MathematicsException) {
 Matrix Matrix::getSubMatrix(unsigned int i, unsigned int j,
                                    unsigned int sizeRows, unsigned int sizeColumns) const throw(std::invalid_argument) {
     // Check the arguments
-    if (i<0 || j<0 || i+sizeRows >= nbRow || j+sizeColumns >= nbColumn) {
+    if (i<0 || j<0 || i+sizeRows > nbRow || j+sizeColumns > nbColumn) {
         // Throw an exception
         throw std::invalid_argument("Error : The arguments are out of matrix bounds");
     }
@@ -475,10 +475,10 @@ Matrix Matrix::operator*(const Matrix& matrix2) const throw(MathematicsException
 Matrix Matrix::operator*(const Vector& vector) const throw(MathematicsException) {
     // Check the sizes of the matrices
     if (nbColumn == vector.getNbComponent()) {
-        Matrix result(nbColumn, 1);
+        Matrix result(nbRow, 1);
-        for (int i=0; i<nbColumn; i++) {
+        for (int i=0; i<nbRow; i++) {
             double sum = 0.0;
-            for (int j=0; j<vector.getNbComponent(); j++) {
+            for (int j=0; j<nbColumn; j++) {
                 sum += array[i][j] * vector.getValue(j);
             }
             result.array[i][0] = sum;

sources/reactphysics3d/mathematics/lcp/LCPProjectedGaussSeidel.cpp

@@ -39,6 +39,9 @@ LCPProjectedGaussSeidel::~LCPProjectedGaussSeidel() {
 void LCPProjectedGaussSeidel::solve(Matrix** J_sp, Matrix** B_sp, uint nbConstraints,
                                     uint nbBodies, Body*** const bodyMapping, std::map<Body*, uint> bodyNumberMapping,
                                     const Vector& b, const Vector& lowLimits, const Vector& highLimits, Vector& lambda) const {
+
+    int size1 = lambda.getNbComponent();
+    int size2 = lambdaInit.getNbComponent();
     lambda = lambdaInit;
     double* d = new double[nbConstraints];         // TODO : Avoid those kind of memory allocation here for optimization (allocate once in the object)
     uint indexBody1, indexBody2;

sources/reactphysics3d/mathematics/lcp/LCPSolver.h

@@ -69,6 +69,7 @@ class LCPSolver {
 
 // Set the initial lambda vector
 inline void LCPSolver::setLambdaInit(const Vector& lambdaInit) {
+    this->lambdaInit.changeSize(lambdaInit.getNbComponent());
     this->lambdaInit = lambdaInit;
 }