Correction of some errors

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

sources/reactphysics3d/engine/ConstraintSolver.cpp

@@ -26,7 +26,7 @@ using namespace reactphysics3d;
 
 // Constructor
 ConstraintSolver::ConstraintSolver(PhysicsWorld& world)
-                 :physicsWorld(world), bodyMapping(0) , lcpSolver(LCPProjectedGaussSeidel(MAX_LCP_ITERATIONS)) {
+                 :physicsWorld(world), bodyMapping(0), lcpSolver(new LCPProjectedGaussSeidel(MAX_LCP_ITERATIONS)) {
 
 }
 
@@ -38,10 +38,12 @@ ConstraintSolver::~ConstraintSolver() {
 // Allocate all the matrices needed to solve the LCP problem
 void ConstraintSolver::allocate() {
     uint nbConstraints = 0;
+    Constraint* constraint;
 
     // For each constraint
-    for (uint c=0; c<physicsWorld.getConstraints().size(); c++) {
+    std::vector<Constraint*>::iterator it;
-        Constraint* constraint = physicsWorld.getConstraints().at(c);
+    for (it = physicsWorld.getConstraintsBeginIterator(); it <physicsWorld.getConstraintsEndIterator(); it++) {
+        constraint = *it;
 
         // Evaluate the constraint
         constraint->evaluate();
@@ -230,7 +232,7 @@ void ConstraintSolver::solve(double dt) {
     computeMatrixB_sp();
 
     // Solve the LCP problem (computation of lambda)
-    //lcpSolver.solve(A, b, lowLimits, highLimits, lambda);
+    lcpSolver->solve(J_sp, B_sp, activeConstraints.size(), nbBodies, bodyMapping, bodyNumberMapping, b, lowerBounds, upperBounds, lambda);
 
     // TODO : Implement this method ...
 

sources/reactphysics3d/engine/ConstraintSolver.h

@@ -41,8 +41,8 @@ const uint MAX_LCP_ITERATIONS = 10;     // Maximum number of iterations when sol
 */
 class ConstraintSolver {
     protected:
-        LCPSolver& lcpSolver;                                   // LCP Solver
         PhysicsWorld& physicsWorld;                             // Reference to the physics world
+        LCPSolver* lcpSolver;                                   // LCP Solver
         std::vector<Constraint*> activeConstraints;             // Current active constraints in the physics world
         std::vector<Body*> constraintBodies;                    // Bodies that are implied in some constraint
         uint nbBodies;                                          // Current number of bodies in the physics world
@@ -75,7 +75,7 @@ class ConstraintSolver {
         void computeMatrixB_sp();                               // Compute the matrix B_sp
 
     public:
-        ConstraintSolver(const PhysicsWorld& world);            // Constructor
+        ConstraintSolver(PhysicsWorld& world);                  // Constructor
         virtual ~ConstraintSolver();                            // Destructor
         void solve(double dt);                                  // Solve the current LCP problem
 };

sources/reactphysics3d/engine/PhysicsEngine.cpp

@@ -91,7 +91,7 @@ void PhysicsEngine::updateCollision() {
         if (collisionDetection.computeCollisionDetection()) {
 
             // TODO : Delete this ----------------------------------------------------------
-            for (std::vector<Constraint*>::const_iterator it = world->getConstraintListStartIterator(); it != world->getConstraintListEndIterator(); ++it) {
+            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);

sources/reactphysics3d/engine/PhysicsWorld.h

@@ -59,10 +59,10 @@ class PhysicsWorld {
         void addConstraint(Constraint* constraint) throw(std::invalid_argument);            // Add a constraint
         void removeConstraint(Constraint* constraint) throw(std::invalid_argument);         // Remove a constraint
         void removeAllContactConstraints();                                                 // Remove all collision contacts constraints
-        std::vector<Constraint*>::const_iterator getConstraintListStartIterator() const;    // Return a start iterator on the constraint list
+        std::vector<Constraint*>::iterator getConstraintsBeginIterator();                   // Return a start iterator on the constraint list
-        std::vector<Constraint*>::const_iterator getConstraintListEndIterator() const;      // Return a end iterator on the constraint list
+        std::vector<Constraint*>::iterator getConstraintsEndIterator();                     // Return a end iterator on the constraint list
-        std::vector<Body*>& getBodies();                                                    // Return a reference to the bodies of the physics world
+        std::vector<Body*>::iterator getBodiesBeginIterator();                              // Return an iterator to the beginning of the bodies of the physics world
-        std::vector<Constraint*>& getConstraints();                                         // Return a reference to the constraints of the physics world
+        std::vector<Body*>::iterator getBodiesEndIterator();                                // Return an iterator to the end of the bodies of the physics world
 };
 
 // --- Inline functions --- //
@@ -95,23 +95,23 @@ inline void PhysicsWorld::setIsGratityOn(bool isGravityOn) {
 }
 
 // Return a start iterator on the constraint list
-inline std::vector<Constraint*>::const_iterator PhysicsWorld::getConstraintListStartIterator() const {
+inline std::vector<Constraint*>::iterator PhysicsWorld::getConstraintsBeginIterator() {
     return constraints.begin();
 }
 
 // Return a end iterator on the constraint list
-inline std::vector<Constraint*>::const_iterator PhysicsWorld::getConstraintListEndIterator() const {
+inline std::vector<Constraint*>::iterator PhysicsWorld::getConstraintsEndIterator() {
     return constraints.end();
 }
 
-// Return a reference to the bodies of the physics world
+// Return an iterator to the beginning of the bodies of the physics world
-std::vector<Body*>& PhysicsWorld::getBodies() {
+inline std::vector<Body*>::iterator PhysicsWorld::getBodiesBeginIterator() {
-    return bodies;
+    return bodies.begin();
 }
 
-// Return a reference to the constraints of the physics world
+// Return an iterator to the end of the bodies of the physics world
-std::vector<Constraint*>& PhysicsWorld::getConstraints() {
+inline std::vector<Body*>::iterator PhysicsWorld::getBodiesEndIterator() {
-    return constraints;
+    return bodies.end();
 }
 
 }   // End of the ReactPhysics3D namespace

sources/reactphysics3d/mathematics/Matrix.cpp

@@ -276,7 +276,6 @@ double Matrix::getTrace() const throw(MathematicsException) {
         // We throw an exception because the matrix is non-square
         throw MathematicsException("MathematicsException : Impossible to compute the trace for a non-square matrix");
     }
-
 }
 
 // Return a sub matrix of size of the current matrix
@@ -353,6 +352,16 @@ void Matrix::initWithValue(double value) {
     }
 }
 
+
+ Vector Matrix::getVector() const {
+     assert(nbColumn == 1);
+     Vector vec(nbRow);
+     for (int i=0; i<nbRow; i++) {
+         vec.setValue(i, array[i][0]);
+     }
+     return vec;
+ }
+
 // Definition of the operator + for the sum of two matrices with references
 Matrix Matrix::operator+(const Matrix& matrix2) const throw(MathematicsException) {
     if (nbRow == matrix2.nbRow && nbColumn == matrix2.nbColumn) {
@@ -440,6 +449,27 @@ Matrix Matrix::operator*(const Matrix& matrix2) const throw(MathematicsException
     }
 }
 
+// Overloaded operator for multiplication with a vector
+Matrix Matrix::operator*(const Vector& vector) const throw(MathematicsException) {
+    // Check the sizes of the matrices
+    if (nbColumn == vector.getNbComponent()) {
+        Matrix result(nbColumn, 1);
+        for (int i=0; i<nbColumn; i++) {
+            double sum = 0.0;
+            for (int j=0; j<vector.getNbComponent(); j++) {
+                sum += array[i][j] * vector.getValue(j);
+            }
+            result.array[i][0] = sum;
+        }
+
+        return result;
+    }
+    else {
+        // Throw an exception because the multiplication is impossible
+        throw MathematicsException("MathematicsException : The sizes of the matrix and the vector aren't compatible for the multiplication");
+    }
+}
+
 // Overloaded operator = for the assignment
 Matrix& Matrix::operator=(const Matrix& matrix2) throw(MathematicsException) {
 

sources/reactphysics3d/mathematics/Matrix.h

@@ -61,6 +61,7 @@ class Matrix {
         double getTrace() const throw(MathematicsException);                                                            // Return the trace of a square matrix
         Matrix getSubMatrix(unsigned int i, unsigned int j,
                             unsigned int nbRows, unsigned int nbColumns) const throw(std::invalid_argument);            // Return a sub matrix of size of the current matrix
+        Vector getVector() const;
         static Matrix identity(int dimension) throw(std::invalid_argument);                                             // Return the identity matrix I of the given dimension
         void fillInSubMatrix(unsigned int i, unsigned int j, const Matrix& subMatrix);                                  // Fill in a sub-matrix of the current matrix with another matrix
         void initWithValue(double value);                                                                               // Initialize all the matrix with the given value
@@ -71,6 +72,7 @@ class Matrix {
         Matrix operator-(const Matrix& matrix2) const throw(MathematicsException);                                      // Overloaded operator for substraction
         Matrix operator*(double nb) const;                                                                              // Overloaded operator for multiplication with a number
         Matrix operator*(const Matrix& matrix2) const throw(MathematicsException);                                      // Overloaded operator for multiplication with a matrix
+        Matrix operator*(const Vector& vector) const throw(MathematicsException);                                       // Overloaded operator for multiplication with a vector
         Matrix& operator=(const Matrix& matrix2) throw(MathematicsException);                                           // Overloaded operator for assignment
         bool operator==(const Matrix& matrix2) const throw(MathematicsException);                                       // Overloaded operator for equality condition
 };

sources/reactphysics3d/mathematics/Vector.cpp

@@ -100,6 +100,14 @@ Vector Vector::getUnit() const throw(MathematicsException) {
     }
 }
 
+void Vector::setVector(const Vector& vector) {
+    assert(nbComponent == vector.nbComponent);
+
+    for (int i=0; i<nbComponent; i++) {
+        tab[i] = vector.tab[i];
+    }
+}
+
 // Method to compute the scalar product of two vectors
 double Vector::scalarProduct(const Vector& vector) const throw(MathematicsException) {
     // Check the sizes of the two vectors

sources/reactphysics3d/mathematics/Vector.h

@@ -59,7 +59,9 @@ class Vector {
         Vector crossProduct(const Vector& vector) const throw(MathematicsException);                // Cross product of two vectors (in 3D only)
         void fillInSubVector(uint index, const Vector& subVector);                                  // Replace a part of the current vector with another sub-vector
         Vector getSubVector(uint index, uint nbElements) const throw(std::invalid_argument);        // Return a sub-vector of the current vector
+        void setVector(const Vector& vector);
         bool isUnit() const;                                                                        // Return true if the vector is unit and false otherwise
+        void changeSize(uint newSize);
 
         // --- Overloaded operators --- //
         Vector operator+(const Vector& vector) const throw(MathematicsException);           // Overloaded operator for addition
@@ -162,6 +164,17 @@ inline Vector operator*(double number, const Vector& vector) {
     return vector * number;
 }
 
+void Vector::changeSize(uint newSize) {
+    delete[] tab;
+    nbComponent = newSize;
+    tab = new double[nbComponent];
+
+    // Fill the array with the value of the vector
+    for (int i=0; i<nbComponent; ++i) {
+        tab[i] = 0.0;
+    }
+}
+
 } // End of the ReactPhysics3D namespace
 
 #endif

sources/reactphysics3d/mathematics/lcp/LCPProjectedGaussSeidel.cpp

@@ -36,16 +36,19 @@ LCPProjectedGaussSeidel::~LCPProjectedGaussSeidel() {
 
 // Solve a LCP problem using the Projected-Gauss-Seidel algorithm
 // This method outputs the result in the lambda vector
-void LCPProjectedGaussSeidel::solve(const Matrix** const J_sp, const Matrix** const B_sp, uint nbConstraints,
+void LCPProjectedGaussSeidel::solve(Matrix** J_sp, Matrix** B_sp, uint nbConstraints,
-                                    uint nbBodies, const Body*** const bodyMapping, std::map<Body*, uint> bodyNumberMapping,
+                                    uint nbBodies, Body*** const bodyMapping, std::map<Body*, uint> bodyNumberMapping,
                                     const Vector& b, const Vector& lowLimits, const Vector& highLimits, Vector& lambda) const {
     lambda = lambdaInit;
-    double d[] = new double[nbConstraints];         // TODO : Avoid those kind of memory allocation here for optimization (allocate once in the object)
+    double* d = new double[nbConstraints];         // TODO : Avoid those kind of memory allocation here for optimization (allocate once in the object)
-    Body* indexBody1, indexBody2;
+    uint indexBody1, indexBody2;
     double deltaLambda;
     double lambdaTemp;
     uint i, iter;
-    Vector* a = new Vector(6)[nbBodies];           // Array that contains nbBodies vector of dimension 6x1
+    Vector* a = new Vector[nbBodies];           // Array that contains nbBodies vector of dimension 6x1
+    for (i=0; i<nbBodies; i++) {
+        a[i].changeSize(6);
+    }
 
     // For each constraint
     for (i=0; i<nbConstraints; i++) {
@@ -56,12 +59,12 @@ void LCPProjectedGaussSeidel::solve(const Matrix** const J_sp, const Matrix** co
         for (i=0; i<nbConstraints; i++) {
             indexBody1 = bodyNumberMapping[bodyMapping[i][0]];
             indexBody2 = bodyNumberMapping[bodyMapping[i][1]];
-            deltaLambda = (b(i) - J_sp[i][0]*a[indexBody1] - J_sp[i][1]*a[indexBody2]) / d[i];
+            deltaLambda = (b.getValue(i) - (J_sp[i][0] * a[indexBody1]).getValue(0,0) - (J_sp[i][1] * a[indexBody2]).getValue(0,0)) / d[i];
             lambdaTemp = lambda.getValue(i);
             lambda.setValue(i, std::max(lowLimits.getValue(i), std::min(lambda.getValue(i) + deltaLambda, highLimits.getValue(i))));
             deltaLambda = lambda.getValue(i) - lambdaTemp;
-            a[indexBody1] = a[indexBody1] + deltaLambda * B_sp[0][i];
+            a[indexBody1] = a[indexBody1] + (B_sp[0][i] * deltaLambda).getVector();
-            a[indexBody2] = a[indexBody2] + deltaLambda * B_sp[1][i];
+            a[indexBody2] = a[indexBody2] + (B_sp[1][i] * deltaLambda).getVector();
         }
     }
 
@@ -72,11 +75,11 @@ void LCPProjectedGaussSeidel::solve(const Matrix** const J_sp, const Matrix** co
 
 // Compute the vector a used in the solve() method
 // Note that a = B * lambda
-void LCPProjectedGaussSeidel::computeVectorA(const Vector& lambda, uint nbConstraints, const Body*** const bodyMapping,
+void LCPProjectedGaussSeidel::computeVectorA(const Vector& lambda, uint nbConstraints, Body*** const bodyMapping,
                                              const Matrix** const B_sp, std::map<Body*, uint> bodyNumberMapping,
                                              Vector* const a, uint nbBodies) const {
     uint i;
-    Body* indexBody1, indexBody2;
+    uint indexBody1, indexBody2;
     
     // Init the vector a with zero values
     for (i=0; i<nbBodies; i++) {
@@ -86,8 +89,8 @@ void LCPProjectedGaussSeidel::computeVectorA(const Vector& lambda, uint nbConstr
     for(i=0; i<nbConstraints; i++) {
         indexBody1 = bodyNumberMapping[bodyMapping[i][0]];
         indexBody2 = bodyNumberMapping[bodyMapping[i][1]];
-        a[indexBody1] = a[indexBody1] + B_sp[0][i] * lambda(i);
+        a[indexBody1] = a[indexBody1] + (B_sp[0][i].getVector() * lambda.getValue(i));
-        a[indexBody2] = a[indexBody2] + B_sp[1][i] * lambda(i);
+        a[indexBody2] = a[indexBody2] + (B_sp[1][i].getVector() * lambda.getValue(i));
     }
 
 }

sources/reactphysics3d/mathematics/lcp/LCPProjectedGaussSeidel.h

@@ -36,16 +36,16 @@ namespace reactphysics3d {
 class LCPProjectedGaussSeidel : public LCPSolver {
     protected: 
 
-        void computeVectorA(const Vector& lambda, uint nbConstraints, const Body*** const bodyMapping,
+        void computeVectorA(const Vector& lambda, uint nbConstraints, Body*** const bodyMapping,
                                                      const Matrix** const B_sp, std::map<Body*, uint> bodyNumberMapping,
                                                      Vector* const a, uint nbBodies) const ;                                                       // Compute the vector a used in the solve() method
 
     public:
         LCPProjectedGaussSeidel(uint maxIterations);                                                                    // Constructor
         virtual ~LCPProjectedGaussSeidel();                                                                             // Destructor
-        virtual void solve(const Matrix** const J_sp, const Matrix** const B_sp, const Body*** const bodyMapping,
+        virtual void solve(Matrix** J_sp, Matrix** B_sp, uint nbConstraints,
-                           std::map<Body*, uint> bodyNumberMapping, const Vector& b, const Vector& lowLimits,
+                           uint nbBodies, Body*** const bodyMapping, std::map<Body*, uint> bodyNumberMapping,
-                           const Vector& highLimits, Vector& lambda) const;                                             // Solve a LCP problem using Projected-Gauss-Seidel algorithm                                                                                                       // Set the initial value for lambda
+                           const Vector& b, const Vector& lowLimits, const Vector& highLimits, Vector& lambda) const;   // Solve a LCP problem using Projected-Gauss-Seidel algorithm                                                                                                       // Set the initial value for lambda
 };
 
 } // End of the ReactPhysics3D namespace

sources/reactphysics3d/mathematics/lcp/LCPSolver.cpp

@@ -23,7 +23,8 @@
 using namespace reactphysics3d;
 
 // Constructor
-LCPSolver::LCPSolver() {
+LCPSolver::LCPSolver(uint maxIterations)
+          : maxIterations(maxIterations) {
 
 }
 

sources/reactphysics3d/mathematics/lcp/LCPSolver.h

@@ -60,9 +60,9 @@ class LCPSolver {
     public:
         LCPSolver(uint maxIterations);                                                                              // Constructor
         virtual ~LCPSolver();                                                                                       // Destructor
-        virtual void solve(const Matrix** const J_sp, const Matrix** const B_sp, const Body*** const bodyMapping,
+        virtual void solve(Matrix** J_sp, Matrix** B_sp, uint nbConstraints,
-                           std::map<Body*, uint> bodyNumberMapping, const Vector& b, const Vector& lowLimits,
+                           uint nbBodies, Body*** const bodyMapping, std::map<Body*, uint> bodyNumberMapping,
-                           const Vector& highLimits, Vector& lambda) const=0;                                       // Solve a LCP problem
+                           const Vector& b, const Vector& lowLimits, const Vector& highLimits, Vector& lambda) const=0;                                       // Solve a LCP problem
         void setLambdaInit(const Vector& lambdaInit);                                                               // Set the initial lambda vector
         void setMaxIterations(uint maxIterations);                                                                  // Set the maximum number of iterations
 };