AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-14 22:00:05 +00:00
Code Issues Releases Wiki Activity

Merge pull request #890 from erwincoumans/master

Browse Source 
(finally!) add option to terminate PGS constraint solvers based on a least square residual threshold
This commit is contained in:
erwincoumans 2016-12-18 17:03:46 -08:00 committed by GitHub
commit b9f1a5da5c
12 changed files with 139 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
examples/OpenCL/rigidbody/GpuRigidBodyDemo.cpp
View File

@ -80,7 +80,18 @@ m_window(0)
m_window = helper->getAppInterface()->m_window;
m_data = new GpuRigidBodyDemoInternalData;
m_data->m_guiHelper = helper;
}
void GpuRigidBodyDemo::resetCamera()
{
float dist = 114;
float pitch = 52;
float yaw = 35;
float targetPos[3]={0,0,0};
m_data->m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
}
GpuRigidBodyDemo::~GpuRigidBodyDemo()
{

3
examples/OpenCL/rigidbody/GpuRigidBodyDemo.h
View File

@ -31,7 +31,8 @@ public:
virtual void renderScene();
void resetCamera();
virtual void stepSimulation(float deltaTime);
//for picking

4
examples/OpenCL/rigidbody/GpuRigidBodyDemoInternalData.h
View File

@ -32,6 +32,7 @@ struct GpuRigidBodyDemoInternalData
int m_pickGraphicsShapeIndex;
int m_pickGraphicsShapeInstance;
b3Config m_config;
GUIHelperInterface* m_guiHelper;
GpuRigidBodyDemoInternalData()
:m_instancePosOrnColor(0),
@ -45,7 +46,8 @@ struct GpuRigidBodyDemoInternalData
m_pickGraphicsShapeInstance(-1),
m_pickBody(-1),
m_altPressed(0),
m_controlPressed(0)
m_controlPressed(0),
m_guiHelper(0)
{
}

3
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -717,7 +717,8 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.00001;
m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 50;
m_data->m_dynamicsWorld->getSolverInfo().m_leastSquaresResidualThreshold = 1e-7;
}
void PhysicsServerCommandProcessor::deleteCachedInverseKinematicsBodies()

2
src/Bullet3Collision/NarrowPhaseCollision/b3Config.h
View File

@ -20,7 +20,7 @@ struct b3Config
int m_maxTriConvexPairCapacity;
b3Config()
:m_maxConvexBodies(32*1024),
:m_maxConvexBodies(128*1024),
m_maxVerticesPerFace(64),
m_maxFacesPerShape(12),
m_maxConvexVertices(8192),

1
src/BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.cpp
View File

@ -12,6 +12,7 @@ subject to the following restrictions:
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.
*/
#define CLEAR_MANIFOLD 1
#include "btSphereSphereCollisionAlgorithm.h"
#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"

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

@ -58,7 +58,7 @@ struct btContactSolverInfoData
int m_minimumSolverBatchSize;
btScalar m_maxGyroscopicForce;
btScalar m_singleAxisRollingFrictionThreshold;
btScalar m_leastSquaresResidualThreshold;
};
@ -91,6 +91,7 @@ struct btContactSolverInfo : public btContactSolverInfoData
m_minimumSolverBatchSize = 128; //try to combine islands until the amount of constraints reaches this limit
m_maxGyroscopicForce = 100.f; ///it is only used for 'explicit' version of gyroscopic force
m_singleAxisRollingFrictionThreshold = 1e30f;///if the velocity is above this threshold, it will use a single constraint row (axis), otherwise 3 rows.
m_leastSquaresResidualThreshold = 0.f;
}
};

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

@ -39,7 +39,7 @@ int gNumSplitImpulseRecoveries = 0;
#include "BulletDynamics/Dynamics/btRigidBody.h"
//#define VERBOSE_RESIDUAL_PRINTF 1
///This is the scalar reference implementation of solving a single constraint row, the innerloop of the Projected Gauss Seidel/Sequential Impulse constraint solver
///Below are optional SSE2 and SSE4/FMA3 versions. We assume most hardware has SSE2. For SSE4/FMA3 we perform a CPU feature check.
static btSimdScalar gResolveSingleConstraintRowGeneric_scalar_reference(btSolverBody& body1, btSolverBody& body2, const btSolverConstraint& c)
@ -298,15 +298,17 @@ btSimdScalar btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowe
}
void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFriendly(
btScalar btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFriendly(
btSolverBody& body1,
btSolverBody& body2,
const btSolverConstraint& c)
{
btScalar deltaImpulse = 0.f;
if (c.m_rhsPenetration)
{
gNumSplitImpulseRecoveries++;
btScalar deltaImpulse = c.m_rhsPenetration-btScalar(c.m_appliedPushImpulse)*c.m_cfm;
deltaImpulse = c.m_rhsPenetration-btScalar(c.m_appliedPushImpulse)*c.m_cfm;
const btScalar deltaVel1Dotn = c.m_contactNormal1.dot(body1.internalGetPushVelocity()) + c.m_relpos1CrossNormal.dot(body1.internalGetTurnVelocity());
const btScalar deltaVel2Dotn = c.m_contactNormal2.dot(body2.internalGetPushVelocity()) + c.m_relpos2CrossNormal.dot(body2.internalGetTurnVelocity());
@ -325,13 +327,14 @@ void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFri
body1.internalApplyPushImpulse(c.m_contactNormal1*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
body2.internalApplyPushImpulse(c.m_contactNormal2*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
}
return deltaImpulse;
}
void btSequentialImpulseConstraintSolver::resolveSplitPenetrationSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
btSimdScalar btSequentialImpulseConstraintSolver::resolveSplitPenetrationSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
{
#ifdef USE_SIMD
if (!c.m_rhsPenetration)
return;
return 0.f;
gNumSplitImpulseRecoveries++;
@ -357,8 +360,9 @@ void btSequentialImpulseConstraintSolver::resolveSplitPenetrationImpulseCacheFri
body1.internalGetTurnVelocity().mVec128 = _mm_add_ps(body1.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentA.mVec128,impulseMagnitude));
body2.internalGetPushVelocity().mVec128 = _mm_add_ps(body2.internalGetPushVelocity().mVec128,_mm_mul_ps(linearComponentB,impulseMagnitude));
body2.internalGetTurnVelocity().mVec128 = _mm_add_ps(body2.internalGetTurnVelocity().mVec128 ,_mm_mul_ps(c.m_angularComponentB.mVec128,impulseMagnitude));
return deltaImpulse;
#else
resolveSplitPenetrationImpulseCacheFriendly(body1,body2,c);
return resolveSplitPenetrationImpulseCacheFriendly(body1,body2,c);
#endif
}
@ -1601,6 +1605,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** /*bodies */,int /*numBodies*/,btPersistentManifold** /*manifoldPtr*/, int /*numManifolds*/,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* /*debugDrawer*/)
{
btScalar leastSquaresResidual = 0.f;
int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
int numConstraintPool = m_tmpSolverContactConstraintPool.size();
@ -1645,7 +1650,10 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
{
btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
if (iteration < constraint.m_overrideNumSolverIterations)
resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
{
btScalar residual = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
leastSquaresResidual += residual*residual;
}
}
if (iteration< infoGlobal.m_numIterations)
@ -1674,7 +1682,9 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
{
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[c]];
resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
btScalar residual = resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
leastSquaresResidual += residual*residual;
totalImpulse = solveManifold.m_appliedImpulse;
}
bool applyFriction = true;
@ -1689,7 +1699,8 @@ 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);
btScalar residual = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
leastSquaresResidual += residual*residual;
}
}
@ -1703,7 +1714,8 @@ 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);
btScalar residual = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
leastSquaresResidual += residual*residual;
}
}
}
@ -1719,8 +1731,8 @@ 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);
btScalar residual = resolveSingleConstraintRowLowerLimitSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
leastSquaresResidual += residual*residual;
}
@ -1738,7 +1750,8 @@ 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);
btScalar residual = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
leastSquaresResidual += residual*residual;
}
}
@ -1758,7 +1771,8 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
btScalar residual = resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
leastSquaresResidual += residual*residual;
}
}
@ -1773,7 +1787,10 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
{
btSolverConstraint& constraint = m_tmpSolverNonContactConstraintPool[m_orderNonContactConstraintPool[j]];
if (iteration < constraint.m_overrideNumSolverIterations)
resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
{
btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[constraint.m_solverBodyIdA],m_tmpSolverBodyPool[constraint.m_solverBodyIdB],constraint);
leastSquaresResidual += residual*residual;
}
}
if (iteration< infoGlobal.m_numIterations)
@ -1794,7 +1811,8 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
for (int j=0;j<numPoolConstraints;j++)
{
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
btScalar residual = resolveSingleConstraintRowLowerLimit(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
leastSquaresResidual += residual*residual;
}
///solve all friction constraints
int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
@ -1808,7 +1826,8 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
solveManifold.m_lowerLimit = -(solveManifold.m_friction*totalImpulse);
solveManifold.m_upperLimit = solveManifold.m_friction*totalImpulse;
resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
leastSquaresResidual += residual*residual;
}
}
@ -1826,12 +1845,13 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
rollingFrictionConstraint.m_lowerLimit = -rollingFrictionMagnitude;
rollingFrictionConstraint.m_upperLimit = rollingFrictionMagnitude;
resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
btScalar residual = resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
leastSquaresResidual += residual*residual;
}
}
}
}
return 0.f;
return leastSquaresResidual;
}
@ -1844,6 +1864,7 @@ void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIte
{
for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
{
btScalar leastSquaresResidual =0.f;
{
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
int j;
@ -1851,15 +1872,24 @@ void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIte
{
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
resolveSplitPenetrationSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
btScalar residual = resolveSplitPenetrationSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
leastSquaresResidual += residual*residual;
}
}
if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration>=(infoGlobal.m_numIterations-1))
{
#ifdef VERBOSE_RESIDUAL_PRINTF
printf("residual = %f at iteration #%d\n",leastSquaresResidual,iteration);
#endif
break;
}
}
}
else
{
for ( iteration = 0;iteration<infoGlobal.m_numIterations;iteration++)
{
btScalar leastSquaresResidual = 0.f;
{
int numPoolConstraints = m_tmpSolverContactConstraintPool.size();
int j;
@ -1867,7 +1897,15 @@ void btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIte
{
const btSolverConstraint& solveManifold = m_tmpSolverContactConstraintPool[m_orderTmpConstraintPool[j]];
resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
btScalar residual = resolveSplitPenetrationImpulseCacheFriendly(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
leastSquaresResidual += residual*residual;
}
if (leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || iteration>=(infoGlobal.m_numIterations-1))
{
#ifdef VERBOSE_RESIDUAL_PRINTF
printf("residual = %f at iteration #%d\n",leastSquaresResidual,iteration);
#endif
break;
}
}
}
@ -1888,7 +1926,15 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(
for ( int iteration = 0 ; iteration< maxIterations ; iteration++)
//for ( int iteration = maxIterations-1 ; iteration >= 0;iteration--)
{
solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
m_leastSquaresResidual = solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
if (m_leastSquaresResidual <= infoGlobal.m_leastSquaresResidualThreshold || (iteration>= (maxIterations-1)))
{
#ifdef VERBOSE_RESIDUAL_PRINTF
printf("residual = %f at iteration #%d\n",m_leastSquaresResidual,iteration);
#endif
break;
}
}
}

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

@ -57,6 +57,8 @@ protected:
btSingleConstraintRowSolver m_resolveSingleConstraintRowGeneric;
btSingleConstraintRowSolver m_resolveSingleConstraintRowLowerLimit;
btScalar m_leastSquaresResidual;
void setupFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,
btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,
btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation,
@ -90,11 +92,11 @@ protected:
void convertContact(btPersistentManifold* manifold,const btContactSolverInfo& infoGlobal);
void resolveSplitPenetrationSIMD(
btSimdScalar resolveSplitPenetrationSIMD(
btSolverBody& bodyA,btSolverBody& bodyB,
const btSolverConstraint& contactConstraint);
void resolveSplitPenetrationImpulseCacheFriendly(
btScalar resolveSplitPenetrationImpulseCacheFriendly(
btSolverBody& bodyA,btSolverBody& bodyB,
const btSolverConstraint& contactConstraint);

25
src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
View File

@ -26,7 +26,7 @@ subject to the following restrictions:
btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
{
btScalar val = btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
btScalar leastSquaredResidual = btSequentialImpulseConstraintSolver::solveSingleIteration(iteration, bodies ,numBodies,manifoldPtr, numManifolds,constraints,numConstraints,infoGlobal,debugDrawer);
//solve featherstone non-contact constraints
@ -38,7 +38,9 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[index];
resolveSingleConstraintRowGeneric(constraint);
btScalar residual = resolveSingleConstraintRowGeneric(constraint);
leastSquaredResidual += residual*residual;
if(constraint.m_multiBodyA)
constraint.m_multiBodyA->setPosUpdated(false);
if(constraint.m_multiBodyB)
@ -49,9 +51,15 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
for (int j=0;j<m_multiBodyNormalContactConstraints.size();j++)
{
btMultiBodySolverConstraint& constraint = m_multiBodyNormalContactConstraints[j];
if (iteration < infoGlobal.m_numIterations)
resolveSingleConstraintRowGeneric(constraint);
btScalar residual = 0.f;
if (iteration < infoGlobal.m_numIterations)
{
residual = resolveSingleConstraintRowGeneric(constraint);
}
leastSquaredResidual += residual*residual;
if(constraint.m_multiBodyA)
constraint.m_multiBodyA->setPosUpdated(false);
if(constraint.m_multiBodyB)
@ -71,7 +79,8 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
{
frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse);
frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse;
resolveSingleConstraintRowGeneric(frictionConstraint);
btScalar residual = resolveSingleConstraintRowGeneric(frictionConstraint);
leastSquaredResidual += residual*residual;
if(frictionConstraint.m_multiBodyA)
frictionConstraint.m_multiBodyA->setPosUpdated(false);
@ -80,7 +89,7 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
}
}
}
return val;
return leastSquaredResidual;
}
btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer)
@ -112,7 +121,7 @@ void btMultiBodyConstraintSolver::applyDeltaVee(btScalar* delta_vee, btScalar im
m_data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse;
}
void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c)
btScalar btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c)
{
btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
@ -190,7 +199,7 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
{
bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
}
return deltaImpulse;
}

2
src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h
View File

@ -43,7 +43,7 @@ protected:
btMultiBodyConstraint** m_tmpMultiBodyConstraints;
int m_tmpNumMultiBodyConstraints;
void resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c);
btScalar resolveSingleConstraintRowGeneric(const btMultiBodySolverConstraint& c);
void convertContacts(btPersistentManifold** manifoldPtr,int numManifolds, const btContactSolverInfo& infoGlobal);

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)
class btSolveProjectedGaussSeidel : public btMLCPSolverInterface
{
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)
{
if (!A.rows())
@ -36,10 +47,11 @@ public:
int i, j, numRows = A.rows();
float delta;
btScalar delta;
for (int k = 0; k <numIterations; k++)
{
m_leastSquaresResidual = 0.f;
for (i = 0; i <numRows; i++)
{
delta = 0.0f;
@ -61,9 +73,10 @@ public:
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;
float s = 1.f;
btScalar s = 1.f;
if (limitDependency[i]>=0)
{
@ -76,6 +89,17 @@ public:
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;
}
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;