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

sources/reactphysics3d/mathematics/lcp/LCPProjectedGaussSeidel.cpp

@@ -0,0 +1,61 @@
+/****************************************************************************
+* Copyright (C) 2009      Daniel Chappuis                                  *
+****************************************************************************
+* This file is part of ReactPhysics3D.                                     *
+*                                                                          *
+* ReactPhysics3D is free software: you can redistribute it and/or modify   *
+* it under the terms of the GNU Lesser General Public License as published *
+* by the Free Software Foundation, either version 3 of the License, or     *
+* (at your option) any later version.                                      *
+*                                                                          *
+* ReactPhysics3D is distributed in the hope that it will be useful,        *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of           *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the             *
+* GNU Lesser General Public License for more details.                      *
+*                                                                          *
+* You should have received a copy of the GNU Lesser General Public License *
+* along with ReactPhysics3D. If not, see <http://www.gnu.org/licenses/>.   *
+***************************************************************************/
+
+// Libraries
+#include "LCPProjectedGaussSeidel.h"
+
+using namespace reactphysics3d;
+
+// Constructor
+LCPProjectedGaussSeidel::LCPProjectedGaussSeidel(unsigned int maxIterations)
+                        :maxIterations(maxIterations) {
+
+}
+
+// Destructor
+LCPProjectedGaussSeidel::~LCPProjectedGaussSeidel() {
+
+}
+
+// Solve a LCP problem using the Projected-Gauss-Seidel algorithm
+void LCPProjectedGaussSeidel::solve(const Matrix& A, const Vector& b, const Vector& lowLimits, const Vector& highLimits, Vector& x) {
+    assert(A.getNbRow() == A.getNbColumn());
+    assert(b.getNbComponent() == A.getNbColumn());
+    assert(lowLimits.getNbComponent() == A.getNbColumn());
+    assert(highLimits.getNbComponent() == A.getNbColumn());
+
+    double delta;
+
+    for (unsigned int k=1; k<=maxIterations; k++) {
+        for (unsigned int i=0; i<A.getNbRow(); i++) {
+            delta = 0.0;
+            for (unsigned int j=0; j<i; j++) {
+                delta += A.getValue(i,j) * x.getValue(j);
+            }
+            for (unsigned int j=i+1; j<A.getNbRow(); j++) {
+                delta += A.getValue(i,j)*x.getValue(j);
+            }
+            x.setValue(i, (b.getValue(i) - delta)/A.getValue(i,i));
+
+            // Clamping according to the limits
+            if (x.getValue(i) > highLimits.getValue(i))  x.setValue(i, highLimits.getValue(i));
+            if (x.getValue(i) < lowLimits.getValue(i))   x.setValue(i, lowLimits.getValue(i));
+        }
+    }
+}

sources/reactphysics3d/mathematics/lcp/LCPProjectedGaussSeidel.h

@@ -0,0 +1,48 @@
+/****************************************************************************
+* Copyright (C) 2009      Daniel Chappuis                                  *
+****************************************************************************
+* This file is part of ReactPhysics3D.                                     *
+*                                                                          *
+* ReactPhysics3D is free software: you can redistribute it and/or modify   *
+* it under the terms of the GNU Lesser General Public License as published *
+* by the Free Software Foundation, either version 3 of the License, or     *
+* (at your option) any later version.                                      *
+*                                                                          *
+* ReactPhysics3D is distributed in the hope that it will be useful,        *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of           *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the             *
+* GNU Lesser General Public License for more details.                      *
+*                                                                          *
+* You should have received a copy of the GNU Lesser General Public License *
+* along with ReactPhysics3D. If not, see <http://www.gnu.org/licenses/>.   *
+***************************************************************************/
+
+#ifndef LCPPROJECTEDGAUSSSEIDEL_H
+#define LCPPROJECTEDGAUSSSEIDEL_H
+
+// Libraries
+#include "LCPSolver.h"
+
+namespace reactphysics3d {
+
+/*  -------------------------------------------------------------------
+    Class LCPProjectedGaussSeidel :
+        This class implements the Projected-Gauss-Seidel (PGS)
+        algorithm in order to solve a LCP problem. This class inherits
+        from the LCPSolver class.
+    -------------------------------------------------------------------
+*/
+class LCPProjectedGaussSeidel : public LCPSolver {
+    protected:
+        unsigned int maxIterations;        // Maximum number of iterations
+
+    public:
+        LCPProjectedGaussSeidel(unsigned int maxIterations);                                                                    // Constructor
+        virtual ~LCPProjectedGaussSeidel();                                                                                     // Destructor
+        virtual void solve(const Matrix& A, const Vector& b, const Vector& lowLimits, const Vector& highLimits, Vector& x);     // Solve a LCP problem using Projected-Gauss-Seidel algorithm
+
+};
+
+} // End of the ReactPhysics3D namespace
+
+#endif

sources/reactphysics3d/mathematics/lcp/LCPSolver.h

@@ -30,7 +30,18 @@ namespace reactphysics3d {
 /*  -------------------------------------------------------------------
     Class LCPSolver :
         This abstract class represents an algorithm to solve a Linear
-        Complementary Problem (LCP).
+        Complementary Problem (LCP). Given a matrix "A", a vector "b",
+        a vector "lowLimit" of lower limits and a vector "highLimits" of
+        upper limits. The goal is to find a vector "x" such that:
+
+                w = Ax - b
+                lowLimits <= x <= highLimits
+
+        and one of the thre following conditions holds :
+
+                x_i = lowLimits_i, w_i >= 0
+                x_i = highLimits_i, w_i >= 0
+                lowLimits_i < x_i < highLimits_i, w_i = 0
     -------------------------------------------------------------------
 */
 class LCPSolver {
@@ -38,8 +49,9 @@ class LCPSolver {
 
     public:
         LCPSolver();                                                                                                            // Constructor
-        virtual void solve(const Matrix& A, const Vector& b, const Vector& lowLimits, const Vector& highLimits, Vector& lambda)=0;  // Solve a LCP problem
         virtual ~LCPSolver();                                                                                                   // Destructor
+        virtual void solve(const Matrix& A, const Vector& b, const Vector& lowLimits, const Vector& highLimits, Vector& x)=0;   // Solve a LCP problem
+
 };
 
 } // End of the ReactPhysics3D namespace