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

added the btNNCGConstraintSolver, based on the paper "Nonsmooth Nonlinear Conjugate Gradient Method for interactive

Browse Source 
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
2013-10-31 06:17:08 +00:00
parent 6ca948e22f
commit 644d01d231
11 changed files with 794 additions and 298 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/BulletDynamics/CMakeLists.txt
View File

@@ -14,6 +14,7 @@ SET(BulletDynamics_SRCS
ConstraintSolver/btHingeConstraint.cpp
ConstraintSolver/btPoint2PointConstraint.cpp
ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
ConstraintSolver/btNNCGConstraintSolver.cpp
ConstraintSolver/btSliderConstraint.cpp
ConstraintSolver/btSolve2LinearConstraint.cpp
ConstraintSolver/btTypedConstraint.cpp
@@ -53,6 +54,7 @@ SET(ConstraintSolver_HDRS
ConstraintSolver/btJacobianEntry.h
ConstraintSolver/btPoint2PointConstraint.h
ConstraintSolver/btSequentialImpulseConstraintSolver.h
ConstraintSolver/btNNCGConstraintSolver.h
ConstraintSolver/btSliderConstraint.h
ConstraintSolver/btSolve2LinearConstraint.h
ConstraintSolver/btSolverBody.h

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

@@ -33,7 +33,8 @@ class btDispatcher;
enum btConstraintSolverType
{
BT_SEQUENTIAL_IMPULSE_SOLVER=1,
BT_MLCP_SOLVER=2
BT_MLCP_SOLVER=2,
BT_NNCG_SOLVER=4
};
class btConstraintSolver

463
src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.cpp Normal file
View File

@@ -0,0 +1,463 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "btNNCGConstraintSolver.h"
btScalar btNNCGConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
{
btScalar val = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup( bodies,numBodies,manifoldPtr, numManifolds, constraints,numConstraints,infoGlobal,debugDrawer);
m_pNC.resizeNoInitialize(m_tmpSolverNonContactConstraintPool.size());
m_pC.resizeNoInitialize(m_tmpSolverContactConstraintPool.size());
m_pCF.resizeNoInitialize(m_tmpSolverContactFrictionConstraintPool.size());
m_pCRF.resizeNoInitialize(m_tmpSolverContactRollingFrictionConstraintPool.size());
m_deltafNC.resizeNoInitialize(m_tmpSolverNonContactConstraintPool.size());
m_deltafC.resizeNoInitialize(m_tmpSolverContactConstraintPool.size());
m_deltafCF.resizeNoInitialize(m_tmpSolverContactFrictionConstraintPool.size());
m_deltafCRF.resizeNoInitialize(m_tmpSolverContactRollingFrictionConstraintPool.size());
return val;
}
btScalar btNNCGConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/)
{
int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
int numConstraintPool = m_tmpSolverContactConstraintPool.size();
int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
{
if (1) // uncomment this for a bit less random ((iteration & 7) == 0)
{
for (int j=0; j<numNonContactPool; ++j) {
int tmp = m_orderNonContactConstraintPool[j];
int swapi = btRandInt2(j+1);
m_orderNonContactConstraintPool[j] = m_orderNonContactConstraintPool[swapi];
m_orderNonContactConstraintPool[swapi] = tmp;
}
//contact/friction constraints are not solved more than
if (iteration< infoGlobal.m_numIterations)
{
for (int j=0; j<numConstraintPool; ++j) {
int tmp = m_orderTmpConstraintPool[j];
int swapi = btRandInt2(j+1);
m_orderTmpConstraintPool[j] = m_orderTmpConstraintPool[swapi];
m_orderTmpConstraintPool[swapi] = tmp;
}
for (int j=0; j<numFrictionPool; ++j) {
int tmp = m_orderFrictionConstraintPool[j];
int swapi = btRandInt2(j+1);
m_orderFrictionConstraintPool[j] = m_orderFrictionConstraintPool[swapi];
m_orderFrictionConstraintPool[swapi] = tmp;
}
}
}
}
btScalar deltaflengthsqr = 0;
if (infoGlobal.m_solverMode & SOLVER_SIMD)
{
for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
{
btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
if (iteration < constraint.m_overrideNumSolverIterations)
{
btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
m_deltafNC[j] = deltaf;
deltaflengthsqr += deltaf * deltaf;
}
}
} else
{
for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
{
btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
if (iteration < constraint.m_overrideNumSolverIterations)
{
btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
m_deltafNC[j] = deltaf;
deltaflengthsqr += deltaf * deltaf;
}
}
}
if (m_onlyForNoneContact)
{
if (iteration==0)
{
for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = m_deltafNC[j];
} else {
// deltaflengthsqrprev can be 0 only if the solver solved the problem exactly in the previous iteration. In this case we should have quit, but mainly for debug reason with this 'hack' it is now allowed to continue the calculation
btScalar beta = m_deltafLengthSqrPrev>0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2;
if (beta>1)
{
for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = 0;
} else
{
for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
{
btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
if (iteration < constraint.m_overrideNumSolverIterations)
{
btScalar additionaldeltaimpulse = beta * m_pNC[j];
constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
m_pNC[j] = beta * m_pNC[j] + m_deltafNC[j];
btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
const btSolverConstraint& c = constraint;
body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
}
}
}
}
m_deltafLengthSqrPrev = deltaflengthsqr;
}
if (infoGlobal.m_solverMode & SOLVER_SIMD)
{
if (iteration< infoGlobal.m_numIterations)
{
for (int j=0;j<numConstraints;j++)
{
if (constraints[j]->isEnabled())
{
int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep);
int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
}
}
///solve all contact constraints using SIMD, if available
if (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS)
{
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
int multiplier = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)? 2 : 1;
for (int c=0;c<numPoolConstraints;c++)
{
btScalar totalImpulse =0;
{
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]];
btScalar deltaf = resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
m_deltafC[c] = deltaf;
deltaflengthsqr += deltaf*deltaf;
totalImpulse = solveManifold.m_appliedImpulse;
}
bool applyFriction = true;
if (applyFriction)
{
{
btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier]];
if (totalImpulse>btScalar(0))
{
solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
m_deltafCF[c*multiplier] = deltaf;
deltaflengthsqr += deltaf*deltaf;
} else {
m_deltafCF[c*multiplier] = 0;
}
}
if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)
{
btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[c*multiplier+1]];
if (totalImpulse>btScalar(0))
{
solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
m_deltafCF[c*multiplier+1] = deltaf;
deltaflengthsqr += deltaf*deltaf;
} else {
m_deltafCF[c*multiplier+1] = 0;
}
}
}
}
}
else//SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS
{
//solve the friction constraints after all contact constraints, don't interleave them
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
int j;
for (j=0;j<numPoolConstraints;j++)
{
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
//resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
btScalar deltaf = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
m_deltafC[j] = deltaf;
deltaflengthsqr += deltaf*deltaf;
}
///solve all friction constraints, using SIMD, if available
int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
for (j=0;j<numFrictionPoolConstraints;j++)
{
btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
if (totalImpulse>btScalar(0))
{
solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
//resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
m_deltafCF[j] = deltaf;
deltaflengthsqr += deltaf*deltaf;
} else {
m_deltafCF[j] = 0;
}
}
int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
for (j=0;j<numRollingFrictionPoolConstraints;j++)
{
btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
if (totalImpulse>btScalar(0))
{
btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
btScalar deltaf = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
m_deltafCRF[j] = deltaf;
deltaflengthsqr += deltaf*deltaf;
} else {
m_deltafCRF[j] = 0;
}
}
}
}
} else
{
if (iteration< infoGlobal.m_numIterations)
{
for (int j=0;j<numConstraints;j++)
{
if (constraints[j]->isEnabled())
{
int bodyAid = getOrInitSolverBody(constraints[j]->getRigidBodyA(),infoGlobal.m_timeStep);
int bodyBid = getOrInitSolverBody(constraints[j]->getRigidBodyB(),infoGlobal.m_timeStep);
btSolverBody& bodyA = m_tmpSolverBodyPool[bodyAid];
btSolverBody& bodyB = m_tmpSolverBodyPool[bodyBid];
constraints[j]->solveConstraintObsolete(bodyA,bodyB,infoGlobal.m_timeStep);
}
}
///solve all contact constraints
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
for (int j=0;j<numPoolConstraints;j++)
{
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
btScalar deltaf = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
m_deltafC[j] = deltaf;
deltaflengthsqr += deltaf*deltaf;
}
///solve all friction constraints
int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
for (int j=0;j<numFrictionPoolConstraints;j++)
{
btSolverConstraint& solveManifold = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
btScalar totalImpulse = m_tmpSolverContactConstraintPool[solveManifold.m_frictionIndex].m_appliedImpulse;
if (totalImpulse>btScalar(0))
{
solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
m_deltafCF[j] = deltaf;
deltaflengthsqr += deltaf*deltaf;
} else {
m_deltafCF[j] = 0;
}
}
int numRollingFrictionPoolConstraints = m_tmpSolverContactRollingFrictionConstraintPool.size();
for (int j=0;j<numRollingFrictionPoolConstraints;j++)
{
btSolverConstraint& rollingFrictionConstraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
btScalar totalImpulse = m_tmpSolverContactConstraintPool[rollingFrictionConstraint.m_frictionIndex].m_appliedImpulse;
if (totalImpulse>btScalar(0))
{
btScalar rollingFrictionMagnitude = rollingFrictionConstraint.m_friction*totalImpulse;
if (rollingFrictionMagnitude>rollingFrictionConstraint.m_friction)
rollingFrictionMagnitude = rollingFrictionConstraint.m_friction;
rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
btScalar deltaf = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
m_deltafCRF[j] = deltaf;
deltaflengthsqr += deltaf*deltaf;
} else {
m_deltafCRF[j] = 0;
}
}
}
}
if (!m_onlyForNoneContact)
{
if (iteration==0)
{
for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = m_deltafNC[j];
for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++) m_pC[j] = m_deltafC[j];
for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++) m_pCF[j] = m_deltafCF[j];
if ( (infoGlobal.m_solverMode & SOLVER_SIMD) ==0 || (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) == 0 )
{
for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++) m_pCRF[j] = m_deltafCRF[j];
}
} else
{
// deltaflengthsqrprev can be 0 only if the solver solved the problem exactly in the previous iteration. In this case we should have quit, but mainly for debug reason with this 'hack' it is now allowed to continue the calculation
btScalar beta = m_deltafLengthSqrPrev>0 ? deltaflengthsqr / m_deltafLengthSqrPrev : 2;
if (beta>1) {
for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++) m_pNC[j] = 0;
for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++) m_pC[j] = 0;
for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++) m_pCF[j] = 0;
if ( (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) == 0 ) {
for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++) m_pCRF[j] = 0;
}
} else {
for (int j=0;j<m_tmpSolverNonContactConstraintPool.size();j++)
{
btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
if (iteration < constraint.m_overrideNumSolverIterations) {
btScalar additionaldeltaimpulse = beta * m_pNC[j];
constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
m_pNC[j] = beta * m_pNC[j] + m_deltafNC[j];
btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
const btSolverConstraint& c = constraint;
body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
}
}
for (int j=0;j<m_tmpSolverContactConstraintPool.size();j++)
{
btSolverConstraint& constraint = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
if (iteration< infoGlobal.m_numIterations) {
btScalar additionaldeltaimpulse = beta * m_pC[j];
constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
m_pC[j] = beta * m_pC[j] + m_deltafC[j];
btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
const btSolverConstraint& c = constraint;
body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
}
}
for (int j=0;j<m_tmpSolverContactFrictionConstraintPool.size();j++)
{
btSolverConstraint& constraint = m_tmpSolverContactFrictionConstraintPool[m_orderFrictionConstraintPool[j]];
if (iteration< infoGlobal.m_numIterations) {
btScalar additionaldeltaimpulse = beta * m_pCF[j];
constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
m_pCF[j] = beta * m_pCF[j] + m_deltafCF[j];
btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
const btSolverConstraint& c = constraint;
body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
}
}
if ( (infoGlobal.m_solverMode & SOLVER_SIMD) ==0 || (infoGlobal.m_solverMode & SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS) == 0 ) {
for (int j=0;j<m_tmpSolverContactRollingFrictionConstraintPool.size();j++)
{
btSolverConstraint& constraint = m_tmpSolverContactRollingFrictionConstraintPool[j];
if (iteration< infoGlobal.m_numIterations) {
btScalar additionaldeltaimpulse = beta * m_pCRF[j];
constraint.m_appliedImpulse = btScalar(constraint.m_appliedImpulse) + additionaldeltaimpulse;
m_pCRF[j] = beta * m_pCRF[j] + m_deltafCRF[j];
btSolverBody& body1 = m_tmpSolverBodyPool[constraint.m_solverBodyIdA];
btSolverBody& body2 = m_tmpSolverBodyPool[constraint.m_solverBodyIdB];
const btSolverConstraint& c = constraint;
body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,additionaldeltaimpulse);
body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,additionaldeltaimpulse);
}
}
}
}
}
m_deltafLengthSqrPrev = deltaflengthsqr;
}
return deltaflengthsqr;
}
btScalar btNNCGConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal)
{
m_pNC.resizeNoInitialize(0);
m_pC.resizeNoInitialize(0);
m_pCF.resizeNoInitialize(0);
m_pCRF.resizeNoInitialize(0);
m_deltafNC.resizeNoInitialize(0);
m_deltafC.resizeNoInitialize(0);
m_deltafCF.resizeNoInitialize(0);
m_deltafCRF.resizeNoInitialize(0);
return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal);
}

64
src/BulletDynamics/ConstraintSolver/btNNCGConstraintSolver.h Normal file
View File

@@ -0,0 +1,64 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_NNCG_CONSTRAINT_SOLVER_H
#define BT_NNCG_CONSTRAINT_SOLVER_H
#include "btSequentialImpulseConstraintSolver.h"
ATTRIBUTE_ALIGNED16(class) btNNCGConstraintSolver : public btSequentialImpulseConstraintSolver
{
protected:
btScalar m_deltafLengthSqrPrev;
btAlignedObjectArray<btScalar> m_pNC; // p for None Contact constraints
btAlignedObjectArray<btScalar> m_pC; // p for Contact constraints
btAlignedObjectArray<btScalar> m_pCF; // p for ContactFriction constraints
btAlignedObjectArray<btScalar> m_pCRF; // p for ContactRollingFriction constraints
//These are recalculated in every iterations. We just keep these to prevent reallocation in each iteration.
btAlignedObjectArray<btScalar> m_deltafNC; // deltaf for NoneContact constraints
btAlignedObjectArray<btScalar> m_deltafC; // deltaf for Contact constraints
btAlignedObjectArray<btScalar> m_deltafCF; // deltaf for ContactFriction constraints
btAlignedObjectArray<btScalar> m_deltafCRF; // deltaf for ContactRollingFriction constraints
protected:
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal);
virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
public:
BT_DECLARE_ALIGNED_ALLOCATOR();
btNNCGConstraintSolver() : btSequentialImpulseConstraintSolver(), m_onlyForNoneContact(false) {}
virtual btConstraintSolverType getSolverType() const
{
return BT_NNCG_SOLVER;
}
bool m_onlyForNoneContact;
};
#endif //BT_NNCG_CONSTRAINT_SOLVER_H

27
src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
View File

@@ -58,7 +58,7 @@ static inline __m128 btSimdDot3( __m128 vec0, __m128 vec1 )
#endif//USE_SIMD
// Project Gauss Seidel or the equivalent Sequential Impulse
void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
{
#ifdef USE_SIMD
__m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
@@ -86,13 +86,14 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
body2.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
return deltaImpulse.m128_f32[0];
#else
resolveSingleConstraintRowGeneric(body1,body2,c);
return resolveSingleConstraintRowGeneric(body1,body2,c);
#endif
}
// Project Gauss Seidel or the equivalent Sequential Impulse
void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
{
btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
@@ -120,9 +121,11 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
return deltaImpulse;
}
void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
{
#ifdef USE_SIMD
__m128 cpAppliedImp = _mm_set1_ps(c.m_appliedImpulse);
@@ -147,13 +150,14 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
body1.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body1.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
body2.internalGetDeltaLinearVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaLinearVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
body2.internalGetDeltaAngularVelocity().mVec128 = _mm_add_ps(body2.internalGetDeltaAngularVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
return deltaImpulse.m128_f32[0];
#else
resolveSingleConstraintRowLowerLimit(body1,body2,c);
return resolveSingleConstraintRowLowerLimit(body1,body2,c);
#endif
}
// Projected Gauss Seidel or the equivalent Sequential Impulse
void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
btScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimit(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
{
btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetDeltaLinearVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetDeltaAngularVelocity());
@@ -173,6 +177,8 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
}
body1.internalApplyImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
body2.internalApplyImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
return deltaImpulse;
}
@@ -430,6 +436,7 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
btSimdScalar velocityError = desiredVelocity - rel_vel;
btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
solverConstraint.m_rhs = velocityImpulse;
solverConstraint.m_rhsPenetration = 0.f;
solverConstraint.m_cfm = cfmSlip;
solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
solverConstraint.m_upperLimit = solverConstraint.m_friction;
@@ -1452,8 +1459,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
for (j=0;j<numPoolConstraints;j++)
{
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
//resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
}
@@ -1472,8 +1478,7 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
//resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
}
}

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

@@ -88,13 +88,10 @@ protected:
int getOrInitSolverBody(btCollisionObject& body,btScalar timeStep);
void initSolverBody(btSolverBody* solverBody, btCollisionObject* collisionObject, btScalar timeStep);
void resolveSingleConstraintRowGeneric(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
void resolveSingleConstraintRowGenericSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
void resolveSingleConstraintRowLowerLimit(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
void resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
btScalar resolveSingleConstraintRowGeneric(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
btScalar resolveSingleConstraintRowGenericSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
btScalar resolveSingleConstraintRowLowerLimit(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
btScalar resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& bodyA,btSolverBody& bodyB,const btSolverConstraint& contactConstraint);
protected:

8
src/BulletDynamics/MLCPSolvers/btLemkeAlgorithm.h
View File

@@ -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_ */

319
src/BulletDynamics/MLCPSolvers/btLemkeSolver.h
View File

@@ -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;
}
};

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

@@ -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);
}

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

@@ -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());

164
src/LinearMath/btMatrixX.h
View File

@@ -113,10 +113,13 @@ struct btVectorX
}
void setZero()
{
// for (int i=0;i<m_storage.size();i++)
// m_storage[i]=0;
//memset(&m_storage[0],0,sizeof(T)*m_storage.size());
btSetZero(&m_storage[0],m_storage.size());
if (m_storage.size())
{
// for (int i=0;i<m_storage.size();i++)
// m_storage[i]=0;
//memset(&m_storage[0],0,sizeof(T)*m_storage.size());
btSetZero(&m_storage[0],m_storage.size());
}
}
const T& operator[] (int index) const
{
@@ -158,8 +161,7 @@ struct btMatrixX
int m_setElemOperations;
btAlignedObjectArray<T> m_storage;
btAlignedObjectArray< btAlignedObjectArray<int> > m_rowNonZeroElements1;
btAlignedObjectArray< btAlignedObjectArray<int> > m_colNonZeroElements;
mutable btAlignedObjectArray< btAlignedObjectArray<int> > m_rowNonZeroElements1;
T* getBufferPointerWritable()
{
@@ -196,7 +198,6 @@ struct btMatrixX
BT_PROFILE("m_storage.resize");
m_storage.resize(rows*cols);
}
clearSparseInfo();
}
int cols() const
{
@@ -231,14 +232,6 @@ struct btMatrixX
void setElem(int row,int col, T val)
{
m_setElemOperations++;
if (val)
{
if (m_storage[col+row*m_cols]==0.f)
{
m_rowNonZeroElements1[row].push_back(col);
m_colNonZeroElements[col].push_back(row);
}
}
m_storage[row*m_cols+col] = val;
}
@@ -256,11 +249,10 @@ struct btMatrixX
void copyLowerToUpperTriangle()
{
int count=0;
for (int row=0;row<m_rowNonZeroElements1.size();row++)
for (int row=0;row<rows();row++)
{
for (int j=0;j<m_rowNonZeroElements1[row].size();j++)
for (int col=0;col<row;col++)
{
int col = m_rowNonZeroElements1[row][j];
setElem(col,row, (*this)(row,col));
count++;
@@ -274,16 +266,6 @@ struct btMatrixX
return m_storage[col+row*m_cols];
}
void clearSparseInfo()
{
BT_PROFILE("clearSparseInfo=0");
m_rowNonZeroElements1.resize(m_rows);
m_colNonZeroElements.resize(m_cols);
for (int i=0;i<m_rows;i++)
m_rowNonZeroElements1[i].resize(0);
for (int j=0;j<m_cols;j++)
m_colNonZeroElements[j].resize(0);
}
void setZero()
{
@@ -294,10 +276,6 @@ struct btMatrixX
//for (int i=0;i<m_storage.size();i++)
// m_storage[i]=0;
}
{
BT_PROFILE("clearSparseInfo=0");
clearSparseInfo();
}
}
void setIdentity()
@@ -327,28 +305,9 @@ struct btMatrixX
printf("\n---------------------\n");
}
void printNumZeros(const char* msg)
{
printf("%s: ",msg);
int numZeros = 0;
for (int i=0;i<m_storage.size();i++)
if (m_storage[i]==0)
numZeros++;
int total = m_cols*m_rows;
int computedNonZero = total-numZeros;
int nonZero = 0;
for (int i=0;i<m_colNonZeroElements.size();i++)
nonZero += m_colNonZeroElements[i].size();
btAssert(computedNonZero==nonZero);
if(computedNonZero!=nonZero)
{
printf("Error: computedNonZero=%d, but nonZero=%d\n",computedNonZero,nonZero);
}
//printf("%d numZeros out of %d (%f)\n",numZeros,m_cols*m_rows,numZeros/(m_cols*m_rows));
printf("total %d, %d rows, %d cols, %d non-zeros (%f %)\n", total, rows(),cols(), nonZero,100.f*(T)nonZero/T(total));
}
/*
void rowComputeNonZeroElements()
void rowComputeNonZeroElements() const
{
m_rowNonZeroElements1.resize(rows());
for (int i=0;i<rows();i++)
@@ -363,13 +322,11 @@ struct btMatrixX
}
}
}
*/
btMatrixX transpose() const
{
//transpose is optimized for sparse matrices
btMatrixX tr(m_cols,m_rows);
tr.setZero();
#if 0
for (int i=0;i<m_cols;i++)
for (int j=0;j<m_rows;j++)
{
@@ -379,37 +336,13 @@ struct btMatrixX
tr.setElem(i,j,v);
}
}
#else
for (int i=0;i<m_colNonZeroElements.size();i++)
for (int h=0;h<m_colNonZeroElements[i].size();h++)
{
int j = m_colNonZeroElements[i][h];
T v = (*this)(j,i);
tr.setElem(i,j,v);
}
#endif
return tr;
}
void sortRowIndexArrays()
{
for (int i=0;i<m_rowNonZeroElements1[i].size();i++)
{
m_rowNonZeroElements1[i].quickSort(btIntSortPredicate());
}
}
void sortColIndexArrays()
{
for (int i=0;i<m_colNonZeroElements[i].size();i++)
{
m_colNonZeroElements[i].quickSort(btIntSortPredicate());
}
}
btMatrixX operator*(const btMatrixX& other)
{
//btMatrixX*btMatrixX implementation, optimized for sparse matrices
//btMatrixX*btMatrixX implementation, brute force
btAssert(cols() == other.rows());
btMatrixX res(rows(),other.cols());
@@ -417,76 +350,18 @@ struct btMatrixX
// BT_PROFILE("btMatrixX mul");
for (int j=0; j < res.cols(); ++j)
{
//int numZero=other.m_colNonZeroElements[j].size();
//if (numZero)
{
for (int i=0; i < res.rows(); ++i)
//for (int g = 0;g<m_colNonZeroElements[j].size();g++)
{
T dotProd=0;
T dotProd2=0;
int waste=0,waste2=0;
bool doubleWalk = false;
if (doubleWalk)
{
int numRows = m_rowNonZeroElements1[i].size();
int numOtherCols = other.m_colNonZeroElements[j].size();
for (int ii=0;ii<numRows;ii++)
{
int vThis=m_rowNonZeroElements1[i][ii];
}
for (int ii=0;ii<numOtherCols;ii++)
{
int vOther = other.m_colNonZeroElements[j][ii];
}
int indexRow = 0;
int indexOtherCol = 0;
while (indexRow < numRows && indexOtherCol < numOtherCols)
{
int vThis=m_rowNonZeroElements1[i][indexRow];
int vOther = other.m_colNonZeroElements[j][indexOtherCol];
if (vOther==vThis)
{
dotProd += (*this)(i,vThis) * other(vThis,j);
}
if (vThis<vOther)
{
indexRow++;
} else
{
indexOtherCol++;
}
}
} else
{
bool useOtherCol = true;
if (other.m_colNonZeroElements[j].size() <m_rowNonZeroElements1[i].size())
{
useOtherCol=true;
}
if (!useOtherCol )
{
for (int q=0;q<other.m_colNonZeroElements[j].size();q++)
for (int v=0;v<rows();v++)
{
int v = other.m_colNonZeroElements[j][q];
T w = (*this)(i,v);
if (w!=0.f)
{
dotProd+=w*other(v,j);
}
}
}
else
{
for (int q=0;q<m_rowNonZeroElements1[i].size();q++)
{
int v=m_rowNonZeroElements1[i][q];
T w = (*this)(i,v);
if (other(v,j)!=0.f)
{
@@ -593,10 +468,9 @@ struct btMatrixX
btMatrixX negative()
{
btMatrixX neg(rows(),cols());
for (int i=0;i<m_colNonZeroElements.size();i++)
for (int h=0;h<m_colNonZeroElements[i].size();h++)
for (int i=0;i<rows();i++)
for (int j=0;j<cols();j++)
{
int j = m_colNonZeroElements[i][h];
T v = (*this)(i,j);
neg.setElem(i,j,-v);
}
@@ -625,7 +499,7 @@ std::ostream& operator<< (std::ostream& os, const btMatrixX<T>& mat)
{
for (int j=0;j<mat.cols();j++)
{
os << std::setw(10) << mat(i,j);
os << std::setw(12) << mat(i,j);
}
if (i!=mat.rows()-1)
os << std::endl << " ";
@@ -643,7 +517,7 @@ std::ostream& operator<< (std::ostream& os, const btVectorX<T>& mat)
//printf("%s ---------------------\n",msg);
for (int i=0;i<mat.rows();i++)
{
os << std::setw(10) << mat[i];
os << std::setw(12) << mat[i];
if (i!=mat.rows()-1)
os << std::endl << " ";
}