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

Allow the ForkLiftDemo to toggle between MLCP and SI solver, using F6 key.

Browse Source 
Apply patch for CMake config, see Issue 754 (Issue 753)
Fix a few issue with the MLCP solver: allow split impulse, and fix offset in friction dependencies
This commit is contained in:
erwin.coumans@gmail.com
2013-10-23 23:35:13 +00:00
parent e5590575a9
commit a1c962192c
15 changed files with 298 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
src/BulletDynamics/CMakeLists.txt
View File

@@ -31,6 +31,8 @@ SET(BulletDynamics_SRCS
Featherstone/btMultiBodyConstraint.cpp
Featherstone/btMultiBodyPoint2Point.cpp
Featherstone/btMultiBodyJointMotor.cpp
MLCPSolvers/btDantzigLCP.cpp
MLCPSolvers/btMLCPSolver.cpp
)
SET(Root_HDRS
@@ -84,6 +86,16 @@ SET(Featherstone_HDRS
Featherstone/btMultiBodyPoint2Point.h
Featherstone/btMultiBodyJointMotor.h
)
SET(MLCPSolvers_HDRS
MLCPSolvers/btDantzigLCP.h
MLCPSolvers/btDantzigSolver.h
MLCPSolvers/btMLCPSolver.h
MLCPSolvers/btMLCPSolverInterface.h
MLCPSolvers/btPATHSolver.h
MLCPSolvers/btSolveProjectedGaussSeidel.h
)
SET(Character_HDRS
Character/btCharacterControllerInterface.h
Character/btKinematicCharacterController.h
@@ -98,6 +110,7 @@ SET(BulletDynamics_HDRS
${Vehicle_HDRS}
${Character_HDRS}
${Featherstone_HDRS}
${MLCPSolvers_HDRS}
)
@@ -134,6 +147,7 @@ DESTINATION ${INCLUDE_INSTALL_DIR}/BulletDynamics)
SET_PROPERTY(SOURCE ${Vehicle_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/Vehicle)
SET_PROPERTY(SOURCE ${Character_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/Character)
SET_PROPERTY(SOURCE ${Featherstone_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/Featherstone)
SET_PROPERTY(SOURCE ${MLCPSolvers_HDRS} PROPERTY MACOSX_PACKAGE_LOCATION Headers/MLCPSolvers)
ENDIF (APPLE AND BUILD_SHARED_LIBS AND FRAMEWORK)
ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES)
ENDIF (INSTALL_LIBS)

12
src/BulletDynamics/ConstraintSolver/btConstraintSolver.h
View File

@@ -28,6 +28,14 @@ class btIDebugDraw;
class btStackAlloc;
class btDispatcher;
/// btConstraintSolver provides solver interface
enum btConstraintSolverType
{
BT_SEQUENTIAL_IMPULSE_SOLVER=1,
BT_MLCP_SOLVER=2
};
class btConstraintSolver
{
@@ -44,6 +52,10 @@ public:
///clear internal cached data and reset random seed
virtual void reset() = 0;
virtual btConstraintSolverType getSolverType() const=0;
};

5
src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
View File

@@ -134,6 +134,11 @@ public:
return m_btSeed2;
}
virtual btConstraintSolverType getSolverType() const
{
return BT_SEQUENTIAL_IMPULSE_SOLVER;
}
};

4
src/BulletDynamics/MLCPSolvers/btDantzigLCP.cpp
View File

@@ -1782,7 +1782,7 @@ bool btSolveDantzigLCP (int n, btScalar *A, btScalar *x, btScalar *b,
{
s_error = false;
printf("btSolveDantzigLCP n=%d\n",n);
// printf("btSolveDantzigLCP n=%d\n",n);
btAssert (n>0 && A && x && b && lo && hi && nub >= 0 && nub <= n);
#ifdef BT_DEBUG
{
@@ -2011,7 +2011,7 @@ bool btSolveDantzigLCP (int n, btScalar *A, btScalar *x, btScalar *b,
// our fingers and exit with the current solution.
if (s <= btScalar(0.0))
{
printf("LCP internal error, s <= 0 (s=%.4e)",(double)s);
// printf("LCP internal error, s <= 0 (s=%.4e)",(double)s);
if (i < n) {
btSetZero (x+i,n-i);
btSetZero (w+i,n-i);

57
src/BulletDynamics/MLCPSolvers/btDantzigSolver.h
View File

@@ -28,6 +28,14 @@ protected:
btScalar m_acceptableUpperLimitSolution;
btAlignedObjectArray<char> m_tempBuffer;
btAlignedObjectArray<btScalar> m_A;
btAlignedObjectArray<btScalar> m_b;
btAlignedObjectArray<btScalar> m_x;
btAlignedObjectArray<btScalar> m_lo;
btAlignedObjectArray<btScalar> m_hi;
btAlignedObjectArray<int> m_dependencies;
public:
btDantzigSolver()
@@ -41,47 +49,46 @@ public:
int n = b.rows();
if (n)
{
btScalar* AA = (btScalar*) A.getBufferPointer();
btScalar* bb = (btScalar* ) b.getBufferPointer();
btScalar* xx = (btScalar*) x.getBufferPointer();
btScalar* llo = (btScalar*) lo.getBufferPointer();
btScalar* hhi = (btScalar*) hi.getBufferPointer();
int* findex = (int*) &limitDependency[0];
int nub = 0;
btAlignedObjectArray<btScalar> ww;
ww.resize(n);
const btScalar* Aptr = A.getBufferPointer();
m_A.resize(n*n);
for (int i=0;i<n*n;i++)
{
AA[i] = Aptr[i];
m_A[i] = Aptr[i];
}
m_b.resize(n);
m_x.resize(n);
m_lo.resize(n);
m_hi.resize(n);
m_dependencies.resize(n);
for (int i=0;i<n;i++)
{
llo[i] = lo[i];
hhi[i] = hi[i];
bb[i] = b[i];
xx[i] = x[i];
m_lo[i] = lo[i];
m_hi[i] = hi[i];
m_b[i] = b[i];
m_x[i] = x[i];
m_dependencies[i] = limitDependency[i];
}
extern int numAllocas;
numAllocas = 0;
result = btSolveDantzigLCP (n,AA,xx,bb,&ww[0],nub,llo,hhi,findex);
result = btSolveDantzigLCP (n,&m_A[0],&m_x[0],&m_b[0],&ww[0],nub,&m_lo[0],&m_hi[0],&m_dependencies[0]);
if (!result)
return result;
// printf("numAllocas = %d\n",numAllocas);
for (int i=0;i<n;i++)
{
x[i] = xx[i];
//test for NAN
if (x[i] != xx[i])
{
volatile btScalar xx = m_x[i];
if (xx != m_x[i])
return false;
}
if (x[i] >= m_acceptableUpperLimitSolution)
{
return false;
@@ -91,7 +98,11 @@ public:
{
return false;
}
}
for (int i=0;i<n;i++)
{
x[i] = m_x[i];
}
}

63
src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp
View File

@@ -20,7 +20,8 @@ subject to the following restrictions:
#include "btSolveProjectedGaussSeidel.h"
btMLCPSolver::btMLCPSolver( btMLCPSolverInterface* solver)
:m_solver(solver)
:m_solver(solver),
m_fallback(0)
{
}
@@ -56,21 +57,21 @@ btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,
///The btSequentialImpulseConstraintSolver moves all friction constraints at the very end, we can also interleave them instead
int firstContactConstraintOffset=dindex;
if (interleaveContactAndFriction)
{
for (int i=0;i<m_tmpSolverContactConstraintPool.size();i++)
{
int findex = dindex;
m_allConstraintArray.push_back(m_tmpSolverContactConstraintPool[i]);
m_limitDependencies[dindex++] = -1;
m_allConstraintArray.push_back(m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact]);
m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact].m_frictionIndex;//findex;
int findex = (m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact].m_frictionIndex*(1+numFrictionPerContact));
m_limitDependencies[dindex++] = findex +firstContactConstraintOffset;
if (numFrictionPerContact==2)
{
m_allConstraintArray.push_back(m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact+1]);
m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i*numFrictionPerContact+1].m_frictionIndex;//findex;
m_limitDependencies[dindex++] = findex+firstContactConstraintOffset;
}
}
} else
@@ -83,7 +84,7 @@ btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,
for (int i=0;i<m_tmpSolverContactFrictionConstraintPool.size();i++)
{
m_allConstraintArray.push_back(m_tmpSolverContactFrictionConstraintPool[i]);
m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i].m_frictionIndex;
m_limitDependencies[dindex++] = m_tmpSolverContactFrictionConstraintPool[i].m_frictionIndex+firstContactConstraintOffset;
}
}
@@ -117,7 +118,26 @@ btScalar btMLCPSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,
bool btMLCPSolver::solveMLCP(const btContactSolverInfo& infoGlobal)
{
return m_solver->solveMLCP(m_A, m_b, m_x, m_lo,m_hi, m_limitDependencies,infoGlobal.m_numIterations );
bool result = true;
if (m_A.rows()==0)
return true;
//if using split impulse, we solve 2 separate (M)LCPs
if (infoGlobal.m_splitImpulse)
{
btMatrixXu Acopy = m_A;
btAlignedObjectArray<int> limitDependenciesCopy = m_limitDependencies;
// printf("solve first LCP\n");
result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo,m_hi, m_limitDependencies,infoGlobal.m_numIterations );
if (result)
result = m_solver->solveMLCP(Acopy, m_bSplit, m_xSplit, m_lo,m_hi, limitDependenciesCopy,infoGlobal.m_numIterations );
} else
{
result = m_solver->solveMLCP(m_A, m_b, m_x, m_lo,m_hi, m_limitDependencies,infoGlobal.m_numIterations );
}
return result;
}
struct btJointNode
@@ -139,11 +159,16 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
{
BT_PROFILE("init b (rhs)");
m_b.resize(numConstraintRows);
m_bSplit.resize(numConstraintRows);
//m_b.setZero();
for (int i=0;i<numConstraintRows ;i++)
{
if (m_allConstraintArray[i].m_jacDiagABInv)
{
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;
}
}
}
@@ -420,16 +445,20 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
{
BT_PROFILE("resize/init x");
m_x.resize(numConstraintRows);
m_xSplit.resize(numConstraintRows);
if (infoGlobal.m_solverMode&SOLVER_USE_WARMSTARTING)
{
for (int i=0;i<m_allConstraintArray.size();i++)
{
const btSolverConstraint& c = m_allConstraintArray[i];
m_x[i]=c.m_appliedImpulse;
m_xSplit[i] = c.m_appliedPushImpulse;
}
} else
{
m_x.setZero();
m_xSplit.setZero();
}
}
@@ -441,12 +470,17 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
int numConstraintRows = m_allConstraintArray.size();
m_b.resize(numConstraintRows);
// m_b.setZero();
if (infoGlobal.m_splitImpulse)
m_bSplit.resize(numConstraintRows);
for (int i=0;i<numConstraintRows ;i++)
{
if (m_allConstraintArray[i].m_jacDiagABInv)
{
m_b[i]=m_allConstraintArray[i].m_rhs/m_allConstraintArray[i].m_jacDiagABInv;
if (infoGlobal.m_splitImpulse)
m_bSplit[i] = m_allConstraintArray[i].m_rhsPenetration/m_allConstraintArray[i].m_jacDiagABInv;
}
}
static btMatrixXu Minv;
@@ -530,13 +564,16 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
}
m_x.resize(numConstraintRows);
if (infoGlobal.m_splitImpulse)
m_xSplit.resize(numConstraintRows);
// m_x.setZero();
for (int i=0;i<m_allConstraintArray.size();i++)
{
const btSolverConstraint& c = m_allConstraintArray[i];
m_x[i]=c.m_appliedImpulse;
// c.m_numRowsForNonContactConstraint
if (infoGlobal.m_splitImpulse)
m_xSplit[i] = c.m_appliedPushImpulse;
}
}
@@ -569,13 +606,19 @@ btScalar btMLCPSolver::solveGroupCacheFriendlyIterations(btCollisionObject** bod
solverBodyA.internalApplyImpulse(c.m_contactNormal1*solverBodyA.internalGetInvMass(),c.m_angularComponentA,m_x[i]);
solverBodyB.internalApplyImpulse(c.m_contactNormal2*solverBodyB.internalGetInvMass(),c.m_angularComponentB,m_x[i]);
if (infoGlobal.m_splitImpulse)
{
solverBodyA.internalApplyPushImpulse(c.m_contactNormal1*solverBodyA.internalGetInvMass(),c.m_angularComponentA,m_xSplit[i]);
solverBodyB.internalApplyPushImpulse(c.m_contactNormal2*solverBodyB.internalGetInvMass(),c.m_angularComponentB,m_xSplit[i]);
c.m_appliedPushImpulse = m_xSplit[i];
}
c.m_appliedImpulse = m_x[i];
}
}
}
else
{
printf("fallback to btSequentialImpulseConstraintSolver\n");
m_fallback++;
btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
}

25
src/BulletDynamics/MLCPSolvers/btMLCPSolver.h
View File

@@ -25,18 +25,23 @@ class btMLCPSolver : public btSequentialImpulseConstraintSolver
{
protected:
btMatrixXu m_A;
btVectorXu m_b;
btVectorXu m_x;
btVectorXu m_lo;
btVectorXu m_hi;
///when using 'split impulse' we solve two separate (M)LCPs
btVectorXu m_bSplit;
btVectorXu m_xSplit;
btVectorXu m_bSplit1;
btVectorXu m_xSplit2;
btAlignedObjectArray<int> m_limitDependencies;
btConstraintArray m_allConstraintArray;
btMLCPSolverInterface* m_solver;
int m_fallback;
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
@@ -56,6 +61,20 @@ public:
m_solver = solver;
}
int getNumFallbacks() const
{
return m_fallback;
}
void setNumFallbacks(int num)
{
m_fallback = num;
}
virtual btConstraintSolverType getSolverType() const
{
return BT_MLCP_SOLVER;
}
};

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

@@ -72,6 +72,7 @@ public:
x[i]=hi[i]*s;
}
}
return true;
}
};