17 #ifndef BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H 18 #define BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H 33 :m_leastSquaresResidualThreshold(0),
34 m_leastSquaresResidual(0)
43 A.rowComputeNonZeroElements();
48 int i, j, numRows = A.rows();
52 for (
int k = 0; k <numIterations; k++)
54 m_leastSquaresResidual = 0.f;
55 for (i = 0; i <numRows; i++)
60 for (
int h=0;h<A.m_rowNonZeroElements1[i].size();h++)
62 int j = A.m_rowNonZeroElements1[i][h];
65 delta += A(i,j) * x[j];
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];
78 x [i] = (b [i] - delta) / aDiag;
81 if (limitDependency[i]>=0)
83 s = x[limitDependency[i]];
93 m_leastSquaresResidual += diff*diff;
97 if ((m_leastSquaresResidual < eps) || (k >=(numIterations-1)))
99 #ifdef VERBOSE_PRINTF_RESIDUAL 100 printf(
"totalLenSqr = %f at iteration #%d\n", m_leastSquaresResidual,k);
110 #endif //BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H btSolveProjectedGaussSeidel()
btScalar m_leastSquaresResidual
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)
btScalar m_leastSquaresResidualThreshold
original version written by Erwin Coumans, October 2013
original version written by Erwin Coumans, October 2013
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...