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

Bullet Collision Detection & Physics Library

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