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

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

Browse Source
This commit is contained in:
chappuis.daniel 2010-02-25 23:06:05 +00:00
parent cc563f15a5
commit a17db9ec67
3 changed files with 125 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

61
sources/reactphysics3d/mathematics/lcp/LCPProjectedGaussSeidel.cpp Normal file
View File

@ -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));
}
}
}

48
sources/reactphysics3d/mathematics/lcp/LCPProjectedGaussSeidel.h Normal file
View File

@ -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

16
sources/reactphysics3d/mathematics/lcp/LCPSolver.h
View File

@ -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