Bullet Collision Detection & Physics Library: btSolveProjectedGaussSeidel.h Source File

Bullet Collision Detection & Physics Library

btSolveProjectedGaussSeidel.h
Go to the documentation of this file.
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2013 Erwin Coumans http://bulletphysics.org
4
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
10
11 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.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
16
17 #ifndef BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H
18 #define BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H
19
20
21 #include "btMLCPSolverInterface.h"
22
24 class btSolveProjectedGaussSeidel : public btMLCPSolverInterface
25 {
26
27 public:
28
29 btScalar m_leastSquaresResidualThreshold;
30 btScalar m_leastSquaresResidual;
31
32 btSolveProjectedGaussSeidel()
33 :m_leastSquaresResidualThreshold(0),
34 m_leastSquaresResidual(0)
35 {
36 }
37
38 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)
39 {
40 if (!A.rows())
41 return true;
42 //the A matrix is sparse, so compute the non-zero elements
43 A.rowComputeNonZeroElements();
44
45 //A is a m-n matrix, m rows, n columns
46 btAssert(A.rows() == b.rows());
47
48 int i, j, numRows = A.rows();
49
50 btScalar delta;
51
52 for (int k = 0; k <numIterations; k++)
53 {
54 m_leastSquaresResidual = 0.f;
55 for (i = 0; i <numRows; i++)
56 {
57 delta = 0.0f;
58 if (useSparsity)
59 {
60 for (int h=0;h<A.m_rowNonZeroElements1[i].size();h++)
61 {
62 int j = A.m_rowNonZeroElements1[i][h];
63 if (j != i)//skip main diagonal
64 {
65 delta += A(i,j) * x[j];
66 }
67 }
68 } else
69 {
70 for (j = 0; j <i; j++)
71 delta += A(i,j) * x[j];
72 for (j = i+1; j<numRows; j++)
73 delta += A(i,j) * x[j];
74 }
75
76 btScalar aDiag = A(i,i);
77 btScalar xOld = x[i];
78 x [i] = (b [i] - delta) / aDiag;
79 btScalar s = 1.f;
80
81 if (limitDependency[i]>=0)
82 {
83 s = x[limitDependency[i]];
84 if (s<0)
85 s=1;
86 }
87
88 if (x[i]<lo[i]*s)
89 x[i]=lo[i]*s;
90 if (x[i]>hi[i]*s)
91 x[i]=hi[i]*s;
92 btScalar diff = x[i] - xOld;
93 m_leastSquaresResidual += diff*diff;
94 }
95
96 btScalar eps = m_leastSquaresResidualThreshold;
97 if ((m_leastSquaresResidual < eps) || (k >=(numIterations-1)))
98 {
99 #ifdef VERBOSE_PRINTF_RESIDUAL
100 printf("totalLenSqr = %f at iteration #%d\n", m_leastSquaresResidual,k);
101 #endif
102 break;
103 }
104 }
105 return true;
106 }
107
108 };
109
110 #endif //BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H
btSolveProjectedGaussSeidel::btSolveProjectedGaussSeidel
btSolveProjectedGaussSeidel()
Definition: btSolveProjectedGaussSeidel.h:32
btSolveProjectedGaussSeidel::m_leastSquaresResidual
btScalar m_leastSquaresResidual
Definition: btSolveProjectedGaussSeidel.h:30
btSolveProjectedGaussSeidel::solveMLCP
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)
Definition: btSolveProjectedGaussSeidel.h:38
btAlignedObjectArray< int >
btMLCPSolverInterface.h
btSolveProjectedGaussSeidel::m_leastSquaresResidualThreshold
btScalar m_leastSquaresResidualThreshold
Definition: btSolveProjectedGaussSeidel.h:29
btAssert
#define btAssert(x)
Definition: btScalar.h:131
btMatrixXu
#define btMatrixXu
Definition: btMatrixX.h:549
btMLCPSolverInterface
original version written by Erwin Coumans, October 2013
Definition: btMLCPSolverInterface.h:22
btVectorXu
#define btVectorXu
Definition: btMatrixX.h:548
btSolveProjectedGaussSeidel
original version written by Erwin Coumans, October 2013
Definition: btSolveProjectedGaussSeidel.h:24
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292