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

add option to terminate PGS constraint solvers based on a least square residual threshold

Browse Source 
(for example solverInfo().m_leastSquaresResidualThreshold = 1e-7 and use large m_numSolverIterations
disable sphere-sphere contact cache, it is buggy (some contact point stay in the cache, when sphere penetrates more than total margins)
tweak some gpu demo settings
This commit is contained in:
Erwin Coumans
2016-12-16 18:09:52 -08:00
parent 38b10137c9
commit dcd02a1e15
11 changed files with 111 additions and 37 deletions
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;
}
};

88
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,9 +327,10 @@ 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)
btScalar btSequentialImpulseConstraintSolver::resolveSplitPenetrationSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
{
#ifdef USE_SIMD
if (!c.m_rhsPenetration)
@@ -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(
btScalar 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);