Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

allow to terminate btSolveProjectedGaussSeidel MLCP solver based on a least squares residual threshold (m_leastSquaresResidualThreshold)

Browse Source
This commit is contained in:
Erwin Coumans
2016-12-16 14:30:12 -08:00
parent 5630a63848
commit 38b10137c9
1 changed files with 27 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

30
src/BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h
View File

@@ -23,7 +23,18 @@ subject to the following restrictions:
///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) ///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 class btSolveProjectedGaussSeidel : public btMLCPSolverInterface
{ {
public: public:
btScalar m_leastSquaresResidualThreshold;
btScalar m_leastSquaresResidual;
btSolveProjectedGaussSeidel()
: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()) if (!A.rows())
@@ -36,10 +47,11 @@ public:
int i, j, numRows = A.rows(); int i, j, numRows = A.rows();
float delta; 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; delta = 0.0f;
@@ -61,9 +73,10 @@ public:
delta += A(i,j) * x[j]; delta += A(i,j) * x[j];
} }
float 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;
float s = 1.f; btScalar s = 1.f;
if (limitDependency[i]>=0) if (limitDependency[i]>=0)
{ {
@@ -76,6 +89,17 @@ public:
x[i]=lo[i]*s; x[i]=lo[i]*s;
if (x[i]>hi[i]*s) if (x[i]>hi[i]*s)
x[i]=hi[i]*s; x[i]=hi[i]*s;
btScalar diff = x[i] - xOld;
m_leastSquaresResidual += diff*diff;
}
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);
#endif
break;
} }
} }
return true; return true;