diff options
author | Sergej Reich <sergej.reich@googlemail.com> | 2013-10-25 07:43:20 +0400 |
---|---|---|
committer | Sergej Reich <sergej.reich@googlemail.com> | 2013-10-25 07:43:20 +0400 |
commit | 472a021aca3c0a6278151f7abee4e0970b026e3f (patch) | |
tree | 3456ce084e33450cc21c68beca1bd407801bf256 /extern/bullet2/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h | |
parent | 4514eeaa8e335a86de0bda68aa4814c0bafad7eb (diff) |
bullet: Update to version 2.82 (bullet revision 2705)
Remove patch that has been applied upstream.
Fixes several bugs.
Diffstat (limited to 'extern/bullet2/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h')
-rw-r--r-- | extern/bullet2/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h | 80 |
1 files changed, 80 insertions, 0 deletions
diff --git a/extern/bullet2/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h new file mode 100644 index 00000000000..44fbfeadddc --- /dev/null +++ b/extern/bullet2/src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h @@ -0,0 +1,80 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ +///original version written by Erwin Coumans, October 2013 + +#ifndef BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H +#define BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H + + +#include "btMLCPSolverInterface.h" + +class btSolveProjectedGaussSeidel : public btMLCPSolverInterface +{ +public: + virtual bool solveMLCP(const btMatrixXu & A, const btVectorXu & b, btVectorXu& x, const btVectorXu & lo,const btVectorXu & hi,const btAlignedObjectArray<int>& limitDependency, int numIterations, bool useSparsity = true) + { + //A is a m-n matrix, m rows, n columns + btAssert(A.rows() == b.rows()); + + int i, j, numRows = A.rows(); + + float delta; + + for (int k = 0; k <numIterations; k++) + { + for (i = 0; i <numRows; i++) + { + delta = 0.0f; + if (useSparsity) + { + for (int h=0;h<A.m_rowNonZeroElements1[i].size();h++) + { + int j = A.m_rowNonZeroElements1[i][h]; + if (j != i)//skip main diagonal + { + delta += A(i,j) * x[j]; + } + } + } else + { + for (j = 0; j <i; j++) + delta += A(i,j) * x[j]; + for (j = i+1; j<numRows; j++) + delta += A(i,j) * x[j]; + } + + float aDiag = A(i,i); + x [i] = (b [i] - delta) / A(i,i); + float s = 1.f; + + if (limitDependency[i]>=0) + { + s = x[limitDependency[i]]; + if (s<0) + s=1; + } + + if (x[i]<lo[i]*s) + x[i]=lo[i]*s; + if (x[i]>hi[i]*s) + x[i]=hi[i]*s; + } + } + return true; + } + +}; + +#endif //BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H |