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

Bullet Collision Detection & Physics Library

- Main Page
- Namespaces
- Classes
- Files

- File List
- File Members

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