added the btNNCGConstraintSolver, based on the paper "Nonsmooth Nonlinear Conjugate Gradient Method for interactive
contact force problems". The solver needs a lot of iterations, before the quality goes up (~ 1000) Thanks to Gabor PUHR for the contribution! Improved the btLemkeSolver. Remove the sparse optimizations from the btMatrixX.h, replace it with explicit call to rowComputeNonZeroElements (only used in the btSolveProjectedGaussSeidel), it was likely slowing things down, without being useful. Re-enable SIMD in the solver (was accidently disabled in Bullet 2.82 release)
This commit is contained in:
erwin.coumans@gmail.com
@@ -15,14 +15,14 @@ subject to the following restrictions:
|
||||
|
||||
//The original version is here
|
||||
//https://code.google.com/p/mbsim-env/source/browse/trunk/kernel/mbsim/numerics/linear_complementarity_problem/lemke_algorithm.cc
|
||||
//This file is re-distributed under the ZLib license, with permission of the original author
|
||||
//This file is re-distributed under the ZLib license, with permission of the original author (Kilian Grundl)
|
||||
//Math library was replaced from fmatvec to a the file src/LinearMath/btMatrixX.h
|
||||
//STL/std::vector replaced by btAlignedObjectArray
|
||||
|
||||
|
||||
|
||||
#ifndef NUMERICS_LEMKE_ALGORITHM_H_
|
||||
#define NUMERICS_LEMKE_ALGORITHM_H_
|
||||
#ifndef BT_NUMERICS_LEMKE_ALGORITHM_H_
|
||||
#define BT_NUMERICS_LEMKE_ALGORITHM_H_
|
||||
|
||||
#include "LinearMath/btMatrixX.h"
|
||||
|
||||
@@ -105,4 +105,4 @@ protected:
|
||||
};
|
||||
|
||||
|
||||
#endif /* NUMERICS_LEMKE_ALGORITHM_H_ */
|
||||
#endif /* BT_NUMERICS_LEMKE_ALGORITHM_H_ */
|
||||
|
||||
@@ -22,103 +22,134 @@ subject to the following restrictions:
|
||||
#include "btLemkeAlgorithm.h"
|
||||
|
||||
|
||||
|
||||
#ifdef BT_DEBUG_OSTREAM
|
||||
using namespace std;
|
||||
#endif //BT_DEBUG_OSTREAM
|
||||
|
||||
|
||||
///The btLemkeSolver is based on "Fast Implementation of Lemke<6B>s Algorithm for Rigid Body Contact Simulation (John E. Lloyd) "
|
||||
///It is a slower but more accurate solver. Increase the m_maxLoops for better convergence, at the cost of more CPU time.
|
||||
///The original implementation of the btLemkeAlgorithm was done by Kilian Grundl from the MBSim team
|
||||
class btLemkeSolver : public btMLCPSolverInterface
|
||||
{
|
||||
protected:
|
||||
|
||||
public:
|
||||
|
||||
btScalar m_maxValue;
|
||||
int m_debugLevel;
|
||||
int m_maxLoops;
|
||||
bool m_useLoHighBounds;
|
||||
|
||||
|
||||
|
||||
btLemkeSolver()
|
||||
:m_maxValue(100000),
|
||||
m_debugLevel(0),
|
||||
m_maxLoops(1000),
|
||||
m_useLoHighBounds(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 (m_useLoHighBounds)
|
||||
{
|
||||
|
||||
BT_PROFILE("btLemkeSolver::solveMLCP");
|
||||
int n = A.rows();
|
||||
if (0==n)
|
||||
return true;
|
||||
|
||||
// printf("================ solving using Lemke/Newton/Fixpoint\n");
|
||||
bool fail = false;
|
||||
|
||||
btVectorXu solution(n);
|
||||
btVectorXu q1;
|
||||
q1.resize(n);
|
||||
for (int row=0;row<n;row++)
|
||||
{
|
||||
q1[row] = -b[row];
|
||||
}
|
||||
|
||||
// cout << "A" << endl;
|
||||
// cout << A << endl;
|
||||
|
||||
/////////////////////////////////////
|
||||
// cout << "A" << endl;
|
||||
// cout << A << endl;
|
||||
|
||||
//slow matrix inversion, replace with LU decomposition
|
||||
btMatrixXu A1;
|
||||
A1.resize(A.rows(),A.cols());
|
||||
for (int row=0;row<A.rows();row++)
|
||||
{
|
||||
for (int col=0;col<A.cols();col++)
|
||||
/////////////////////////////////////
|
||||
|
||||
//slow matrix inversion, replace with LU decomposition
|
||||
btMatrixXu A1;
|
||||
btMatrixXu B(n,n);
|
||||
{
|
||||
A1.setElem(row,col,A(row,col));
|
||||
BT_PROFILE("inverse(slow)");
|
||||
A1.resize(A.rows(),A.cols());
|
||||
for (int row=0;row<A.rows();row++)
|
||||
{
|
||||
for (int col=0;col<A.cols();col++)
|
||||
{
|
||||
A1.setElem(row,col,A(row,col));
|
||||
}
|
||||
}
|
||||
|
||||
btMatrixXu matrix;
|
||||
matrix.resize(n,2*n);
|
||||
for (int row=0;row<n;row++)
|
||||
{
|
||||
for (int col=0;col<n;col++)
|
||||
{
|
||||
matrix.setElem(row,col,A1(row,col));
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
btScalar ratio,a;
|
||||
int i,j,k;
|
||||
for(i = 0; i < n; i++){
|
||||
for(j = n; j < 2*n; j++){
|
||||
if(i==(j-n))
|
||||
matrix.setElem(i,j,1.0);
|
||||
else
|
||||
matrix.setElem(i,j,0.0);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
btMatrixXu matrix;
|
||||
matrix.resize(n,2*n);
|
||||
for (int row=0;row<n;row++)
|
||||
{
|
||||
for (int col=0;col<n;col++)
|
||||
{
|
||||
matrix.setElem(row,col,A1(row,col));
|
||||
for(i = 0; i < n; i++){
|
||||
for(j = 0; j < n; j++){
|
||||
if(i!=j)
|
||||
{
|
||||
btScalar v = matrix(i,i);
|
||||
if (btFuzzyZero(v))
|
||||
{
|
||||
a = 0.000001f;
|
||||
}
|
||||
ratio = matrix(j,i)/matrix(i,i);
|
||||
for(k = 0; k < 2*n; k++){
|
||||
matrix.addElem(j,k,- ratio * matrix(i,k));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
for(i = 0; i < n; i++){
|
||||
a = matrix(i,i);
|
||||
if (btFuzzyZero(a))
|
||||
{
|
||||
a = 0.000001f;
|
||||
}
|
||||
btScalar invA = 1.f/a;
|
||||
for(j = 0; j < 2*n; j++){
|
||||
matrix.mulElem(i,j,invA);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
btScalar ratio,a;
|
||||
int i,j,k;
|
||||
for(i = 0; i < n; i++){
|
||||
for(j = n; j < 2*n; j++){
|
||||
if(i==(j-n))
|
||||
matrix.setElem(i,j,1.0);
|
||||
else
|
||||
matrix.setElem(i,j,0.0);
|
||||
}
|
||||
}
|
||||
for(i = 0; i < n; i++){
|
||||
for(j = 0; j < n; j++){
|
||||
if(i!=j){
|
||||
ratio = matrix(j,i)/matrix(i,i);
|
||||
for(k = 0; k < 2*n; k++){
|
||||
matrix.addElem(j,k,- ratio * matrix(i,k));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
for(i = 0; i < n; i++){
|
||||
a = matrix(i,i);
|
||||
btScalar invA = 1.f/a;
|
||||
for(j = 0; j < 2*n; j++){
|
||||
matrix.mulElem(i,j,invA);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
btMatrixXu B(n,n);
|
||||
|
||||
|
||||
for (int row=0;row<n;row++)
|
||||
{
|
||||
for (int col=0;col<n;col++)
|
||||
{
|
||||
B.setElem(row,col,matrix(row,n+col));
|
||||
for (int row=0;row<n;row++)
|
||||
{
|
||||
for (int col=0;col<n;col++)
|
||||
{
|
||||
B.setElem(row,col,matrix(row,n+col));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// cout << "B" << endl;
|
||||
// cout << B << endl;
|
||||
|
||||
btMatrixXu b1(n,1);
|
||||
|
||||
|
||||
|
||||
btMatrixXu b1(n,1);
|
||||
|
||||
btMatrixXu M(n*2,n*2);
|
||||
for (int row=0;row<n;row++)
|
||||
@@ -135,9 +166,6 @@ public:
|
||||
}
|
||||
}
|
||||
|
||||
// cout << "M:" << endl;
|
||||
// cout << M << endl;
|
||||
|
||||
btMatrixXu Bb1 = B*b1;
|
||||
// q = [ (-B*b1 - lo)' (hi + B*b1)' ]'
|
||||
|
||||
@@ -149,29 +177,16 @@ public:
|
||||
qq[n+row] = Bb1(row,0)+hi[row];
|
||||
}
|
||||
|
||||
// cout << "qq:" << endl;
|
||||
// cout << qq << endl;
|
||||
|
||||
int debugLevel=0;
|
||||
// btLemkeAlgorithm lemke(A,q1,debugLevel);
|
||||
// lemke.setSystem(A,q1);
|
||||
|
||||
btLemkeAlgorithm lemke(M,qq,debugLevel);
|
||||
|
||||
|
||||
int maxloops = 100;
|
||||
btVectorXu z1 = lemke.solve(maxloops);
|
||||
|
||||
// cout << "x" << endl;
|
||||
// cout << z1 << endl;
|
||||
|
||||
//y_plus = z1(1:8);
|
||||
//y_minus = z1(9:16);
|
||||
//y1 = y_plus - y_minus;
|
||||
//x1 = B*(y1-b1);
|
||||
btVectorXu z1;
|
||||
|
||||
btMatrixXu y1;
|
||||
y1.resize(n,1);
|
||||
btLemkeAlgorithm lemke(M,qq,m_debugLevel);
|
||||
{
|
||||
BT_PROFILE("lemke.solve");
|
||||
lemke.setSystem(M,qq);
|
||||
z1 = lemke.solve(m_maxLoops);
|
||||
}
|
||||
for (int row=0;row<n;row++)
|
||||
{
|
||||
y1.setElem(row,0,z1[2*n+row]-z1[3*n+row]);
|
||||
@@ -182,20 +197,15 @@ public:
|
||||
y1_b1.setElem(i,0,y1(i,0)-b1(i,0));
|
||||
}
|
||||
|
||||
btMatrixXu x1 = B*(y1_b1);
|
||||
|
||||
// cout << "x1" << endl;
|
||||
// cout << x1 << endl;
|
||||
btVectorXu solution(n);
|
||||
btMatrixXu x1;
|
||||
|
||||
x1 = B*(y1_b1);
|
||||
|
||||
for (int row=0;row<n;row++)
|
||||
{
|
||||
solution[row] = x1(row,0);//n];
|
||||
}
|
||||
//check solution
|
||||
// cout << "solution" << endl;
|
||||
// cout << solution << endl;
|
||||
|
||||
bool fail = false;
|
||||
|
||||
int errorIndexMax = -1;
|
||||
int errorIndexMin = -1;
|
||||
float errorValueMax = -1e30;
|
||||
@@ -207,13 +217,14 @@ public:
|
||||
volatile btScalar check = x[i];
|
||||
if (x[i] != check)
|
||||
{
|
||||
//printf("Lemke result is #NAN\n");
|
||||
x.setZero();
|
||||
return false;
|
||||
}
|
||||
|
||||
//this is some hack/safety mechanism, to discard invalid solutions from the Lemke solver
|
||||
//we need to figure out why it happens, and fix it, or detect it properly)
|
||||
if (x[i]>100000)
|
||||
if (x[i]>m_maxValue)
|
||||
{
|
||||
if (x[i]> errorValueMax)
|
||||
{
|
||||
@@ -223,7 +234,86 @@ public:
|
||||
}
|
||||
////printf("x[i] = %f,",x[i]);
|
||||
}
|
||||
if (x[i]<-10000)
|
||||
if (x[i]<-m_maxValue)
|
||||
{
|
||||
if (x[i]<errorValueMin)
|
||||
{
|
||||
errorIndexMin = i;
|
||||
errorValueMin = x[i];
|
||||
fail = true;
|
||||
//printf("x[i] = %f,",x[i]);
|
||||
}
|
||||
}
|
||||
}
|
||||
if (fail)
|
||||
{
|
||||
int m_errorCountTimes = 0;
|
||||
if (errorIndexMin<0)
|
||||
errorValueMin = 0.f;
|
||||
if (errorIndexMax<0)
|
||||
errorValueMax = 0.f;
|
||||
m_errorCountTimes++;
|
||||
// printf("Error (x[%d] = %f, x[%d] = %f), resetting %d times\n", errorIndexMin,errorValueMin, errorIndexMax, errorValueMax, errorCountTimes++);
|
||||
for (int i=0;i<n;i++)
|
||||
{
|
||||
x[i]=0.f;
|
||||
}
|
||||
}
|
||||
return !fail;
|
||||
} else
|
||||
|
||||
{
|
||||
int dimension = A.rows();
|
||||
if (0==dimension)
|
||||
return true;
|
||||
|
||||
// printf("================ solving using Lemke/Newton/Fixpoint\n");
|
||||
|
||||
btVectorXu q;
|
||||
q.resize(dimension);
|
||||
for (int row=0;row<dimension;row++)
|
||||
{
|
||||
q[row] = -b[row];
|
||||
}
|
||||
|
||||
btLemkeAlgorithm lemke(A,q,m_debugLevel);
|
||||
|
||||
|
||||
lemke.setSystem(A,q);
|
||||
|
||||
btVectorXu solution = lemke.solve(m_maxLoops);
|
||||
|
||||
//check solution
|
||||
|
||||
bool fail = false;
|
||||
int errorIndexMax = -1;
|
||||
int errorIndexMin = -1;
|
||||
float errorValueMax = -1e30;
|
||||
float errorValueMin = 1e30;
|
||||
|
||||
for (int i=0;i<dimension;i++)
|
||||
{
|
||||
x[i] = solution[i+dimension];
|
||||
volatile btScalar check = x[i];
|
||||
if (x[i] != check)
|
||||
{
|
||||
x.setZero();
|
||||
return false;
|
||||
}
|
||||
|
||||
//this is some hack/safety mechanism, to discard invalid solutions from the Lemke solver
|
||||
//we need to figure out why it happens, and fix it, or detect it properly)
|
||||
if (x[i]>m_maxValue)
|
||||
{
|
||||
if (x[i]> errorValueMax)
|
||||
{
|
||||
fail = true;
|
||||
errorIndexMax = i;
|
||||
errorValueMax = x[i];
|
||||
}
|
||||
////printf("x[i] = %f,",x[i]);
|
||||
}
|
||||
if (x[i]<-m_maxValue)
|
||||
{
|
||||
if (x[i]<errorValueMin)
|
||||
{
|
||||
@@ -242,28 +332,17 @@ public:
|
||||
if (errorIndexMax<0)
|
||||
errorValueMax = 0.f;
|
||||
printf("Error (x[%d] = %f, x[%d] = %f), resetting %d times\n", errorIndexMin,errorValueMin, errorIndexMax, errorValueMax, errorCountTimes++);
|
||||
for (int i=0;i<n;i++)
|
||||
for (int i=0;i<dimension;i++)
|
||||
{
|
||||
x[i]=0.f;
|
||||
}
|
||||
}
|
||||
#if 0
|
||||
if (lemke.getInfo()<0)
|
||||
{
|
||||
printf("Lemke found no solution, info = %d\n",lemke.getInfo());
|
||||
} else
|
||||
{
|
||||
printf("Lemke info = %d, found a solution in %d steps\n",lemke.getInfo(),lemke.getSteps());
|
||||
//printf("Lemke found a solution\n");
|
||||
for (int i=0;i<dimension;i++)
|
||||
{
|
||||
x[i] = solution(i+dimension);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
return !fail;
|
||||
|
||||
}
|
||||
return true;
|
||||
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
@@ -19,6 +19,7 @@ subject to the following restrictions:
|
||||
#include "LinearMath/btQuickprof.h"
|
||||
#include "btSolveProjectedGaussSeidel.h"
|
||||
|
||||
|
||||
btMLCPSolver::btMLCPSolver( btMLCPSolverInterface* solver)
|
||||
:m_solver(solver),
|
||||
m_fallback(0)
|
||||
@@ -160,13 +161,17 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
|
||||
BT_PROFILE("init b (rhs)");
|
||||
m_b.resize(numConstraintRows);
|
||||
m_bSplit.resize(numConstraintRows);
|
||||
//m_b.setZero();
|
||||
m_b.setZero();
|
||||
m_bSplit.setZero();
|
||||
for (int i=0;i<numConstraintRows ;i++)
|
||||
{
|
||||
if (m_allConstraintArray[i].m_jacDiagABInv)
|
||||
btScalar jacDiag = m_allConstraintArray[i].m_jacDiagABInv;
|
||||
if (!btFuzzyZero(jacDiag))
|
||||
{
|
||||
m_b[i]=m_allConstraintArray[i].m_rhs/m_allConstraintArray[i].m_jacDiagABInv;
|
||||
m_bSplit[i] = m_allConstraintArray[i].m_rhsPenetration/m_allConstraintArray[i].m_jacDiagABInv;
|
||||
btScalar rhs = m_allConstraintArray[i].m_rhs;
|
||||
btScalar rhsPenetration = m_allConstraintArray[i].m_rhsPenetration;
|
||||
m_b[i]=rhs/jacDiag;
|
||||
m_bSplit[i] = rhsPenetration/jacDiag;
|
||||
}
|
||||
|
||||
}
|
||||
@@ -425,14 +430,12 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
|
||||
}
|
||||
}
|
||||
|
||||
///todo: use proper cfm values from the constraints (getInfo2)
|
||||
if (1)
|
||||
{
|
||||
// add cfm to the diagonal of m_A
|
||||
for ( int i=0; i<m_A.rows(); ++i)
|
||||
{
|
||||
float cfm = 0.000001f;
|
||||
m_A.setElem(i,i,m_A(i,i)+ cfm / infoGlobal.m_timeStep);
|
||||
m_A.setElem(i,i,m_A(i,i)+ infoGlobal.m_globalCfm / infoGlobal.m_timeStep);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -473,6 +476,9 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
|
||||
if (infoGlobal.m_splitImpulse)
|
||||
m_bSplit.resize(numConstraintRows);
|
||||
|
||||
m_bSplit.setZero();
|
||||
m_b.setZero();
|
||||
|
||||
for (int i=0;i<numConstraintRows ;i++)
|
||||
{
|
||||
if (m_allConstraintArray[i].m_jacDiagABInv)
|
||||
@@ -554,12 +560,10 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
|
||||
|
||||
if (1)
|
||||
{
|
||||
///todo: use proper cfm values from the constraints (getInfo2)
|
||||
// add cfm to the diagonal of m_A
|
||||
for ( int i=0; i<m_A.rows(); ++i)
|
||||
{
|
||||
float cfm = 0.000001f;
|
||||
m_A.setElem(i,i,m_A(i,i)+ cfm / infoGlobal.m_timeStep);
|
||||
m_A.setElem(i,i,m_A(i,i)+ infoGlobal.m_globalCfm / infoGlobal.m_timeStep);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -618,6 +622,7 @@ btScalar btMLCPSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bod
|
||||
}
|
||||
else
|
||||
{
|
||||
// printf("m_fallback = %d\n",m_fallback);
|
||||
m_fallback++;
|
||||
btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
|
||||
}
|
||||
|
||||
@@ -20,11 +20,17 @@ subject to the following restrictions:
|
||||
|
||||
#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:
|
||||
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;
|
||||
//the A matrix is sparse, so compute the non-zero elements
|
||||
A.rowComputeNonZeroElements();
|
||||
|
||||
//A is a m-n matrix, m rows, n columns
|
||||
btAssert(A.rows() == b.rows());
|
||||
|
||||
|
||||
Reference in New Issue
Block a user