diff options
Diffstat (limited to 'thirdparty/bullet/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h')
| -rw-r--r-- | thirdparty/bullet/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h | 65 |
1 files changed, 31 insertions, 34 deletions
diff --git a/thirdparty/bullet/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h b/thirdparty/bullet/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h index c0b40ffd9f..c3f4ec3997 100644 --- a/thirdparty/bullet/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h +++ b/thirdparty/bullet/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h @@ -17,25 +17,22 @@ subject to the following restrictions: #ifndef BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H #define BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H - #include "btMLCPSolverInterface.h" ///This solver is mainly for debug/learning purposes: it is functionally equivalent to the btSequentialImpulseConstraintSolver solver, but much slower (it builds the full LCP matrix) class btSolveProjectedGaussSeidel : public btMLCPSolverInterface { - public: - btScalar m_leastSquaresResidualThreshold; btScalar m_leastSquaresResidual; btSolveProjectedGaussSeidel() - :m_leastSquaresResidualThreshold(0), - m_leastSquaresResidual(0) + : m_leastSquaresResidualThreshold(0), + m_leastSquaresResidual(0) { } - 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) + 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) { if (!A.rows()) return true; @@ -46,65 +43,65 @@ public: btAssert(A.rows() == b.rows()); int i, j, numRows = A.rows(); - + btScalar delta; - for (int k = 0; k <numIterations; k++) + for (int k = 0; k < numIterations; k++) { m_leastSquaresResidual = 0.f; - for (i = 0; i <numRows; i++) + for (i = 0; i < numRows; i++) { delta = 0.0f; if (useSparsity) { - for (int h=0;h<A.m_rowNonZeroElements1[i].size();h++) + for (int h = 0; h < A.m_rowNonZeroElements1[i].size(); h++) { - int j = A.m_rowNonZeroElements1[i][h]; - if (j != i)//skip main diagonal + j = A.m_rowNonZeroElements1[i][h]; + if (j != i) //skip main diagonal { - delta += A(i,j) * x[j]; + delta += A(i, j) * x[j]; } } - } else + } + else { - for (j = 0; j <i; j++) - delta += A(i,j) * x[j]; - for (j = i+1; j<numRows; j++) - delta += A(i,j) * x[j]; + for (j = 0; j < i; j++) + delta += A(i, j) * x[j]; + for (j = i + 1; j < numRows; j++) + delta += A(i, j) * x[j]; } - btScalar aDiag = A(i,i); + btScalar aDiag = A(i, i); btScalar xOld = x[i]; - x [i] = (b [i] - delta) / aDiag; + x[i] = (b[i] - delta) / aDiag; btScalar s = 1.f; - if (limitDependency[i]>=0) + if (limitDependency[i] >= 0) { s = x[limitDependency[i]]; - if (s<0) - s=1; + if (s < 0) + s = 1; } - - if (x[i]<lo[i]*s) - x[i]=lo[i]*s; - if (x[i]>hi[i]*s) - x[i]=hi[i]*s; + + if (x[i] < lo[i] * s) + x[i] = lo[i] * s; + if (x[i] > hi[i] * s) + x[i] = hi[i] * s; btScalar diff = x[i] - xOld; - m_leastSquaresResidual += diff*diff; + m_leastSquaresResidual += diff * diff; } - btScalar eps = m_leastSquaresResidualThreshold; - if ((m_leastSquaresResidual < eps) || (k >=(numIterations-1))) + btScalar eps = m_leastSquaresResidualThreshold; + if ((m_leastSquaresResidual < eps) || (k >= (numIterations - 1))) { #ifdef VERBOSE_PRINTF_RESIDUAL - printf("totalLenSqr = %f at iteration #%d\n", m_leastSquaresResidual,k); + printf("totalLenSqr = %f at iteration #%d\n", m_leastSquaresResidual, k); #endif break; } } return true; } - }; -#endif //BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H +#endif //BT_SOLVE_PROJECTED_GAUSS_SEIDEL_H |