17#ifndef BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H
18#define BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H
40 A.rowComputeNonZeroElements();
45 int i, j, numRows = A.rows();
49 for (
int k = 0; k < numIterations; k++)
52 for (i = 0; i < numRows; i++)
57 for (
int h = 0; h < A.m_rowNonZeroElements1[i].size(); h++)
59 j = A.m_rowNonZeroElements1[i][h];
62 delta += A(i, j) * x[j];
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];
76 x[i] = (b[i] - delta) / aDiag;
79 if (limitDependency[i] >= 0)
81 s = x[limitDependency[i]];
97#ifdef VERBOSE_PRINTF_RESIDUAL
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods It...
original version written by Erwin Coumans, October 2013
original version written by Erwin Coumans, October 2013
btScalar m_leastSquaresResidualThreshold
btScalar m_leastSquaresResidual
btSolveProjectedGaussSeidel()
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)