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

implemented rolling friction, using a contact constraint. Useful to get rolling spheres to rest, even on a slightly sloped plane.

Browse Source 
See http://www.youtube.com/watch?v=RV7sBAsKu4M and Bullet/Demos/RollingFrictionDemo
Fixes in FractureDemo (mouse picking constraint needs to be removed, otherwise constraint solver crashes/asserts)
This commit is contained in:
erwin.coumans
2012-09-15 06:52:17 +00:00
parent 54744b6ab9
commit 7eebb79ced
14 changed files with 263 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
Demos/BasicDemo/main.cpp
View File

@@ -18,8 +18,7 @@ subject to the following restrictions:
#include "btBulletDynamicsCommon.h"
#include "LinearMath/btHashMap.h"
#include "GLDebugDrawer.h"
static GLDebugDrawer sDebugDraw;
@@ -28,7 +27,7 @@ int main(int argc,char** argv)
BasicDemo ccdDemo;
ccdDemo.initPhysics();
ccdDemo.getDynamicsWorld()->setDebugDrawer(&sDebugDraw);
#ifdef CHECK_MEMORY_LEAKS
ccdDemo.exitPhysics();

3
Demos/CMakeLists.txt
View File

@@ -12,7 +12,7 @@ IF (USE_GLUT)
SET(SharedDemoSubdirs
OpenGL AllBulletDemos ConvexDecompositionDemo
CcdPhysicsDemo ConstraintDemo SliderConstraintDemo GenericJointDemo Raytracer
RagdollDemo ForkLiftDemo BasicDemo RaytestDemo VoronoiFractureDemo GyroscopicDemo FractureDemo Box2dDemo BspDemo MovingConcaveDemo VehicleDemo
RagdollDemo ForkLiftDemo BasicDemo RollingFrictionDemo RaytestDemo VoronoiFractureDemo GyroscopicDemo FractureDemo Box2dDemo BspDemo MovingConcaveDemo VehicleDemo
UserCollisionAlgorithm CharacterDemo SoftDemo
CollisionInterfaceDemo ConcaveConvexcastDemo SimplexDemo DynamicControlDemo
ConvexHullDistance
@@ -48,6 +48,7 @@ ELSE (USE_GLUT)
CollisionInterfaceDemo
ConcaveDemo
ConstraintDemo
RollingFrictionDemo
ConvexDecompositionDemo
InternalEdgeDemo
GimpactTestDemo

4
Demos/CcdPhysicsDemo/CcdPhysicsDemo.cpp
View File

@@ -223,7 +223,7 @@ void CcdPhysicsDemo::initPhysics()
btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
body->setRollingFriction(0.3);
//add the body to the dynamics world
m_dynamicsWorld->addRigidBody(body);
}
@@ -282,7 +282,7 @@ void CcdPhysicsDemo::initPhysics()
btRigidBody* body = localCreateRigidBody(mass,trans,shape);
body->setRollingFriction(0.1);
///when using m_ccdMode
if (m_ccdMode==USE_CCD)
{

14
Demos/FractureDemo/btFractureDynamicsWorld.cpp
View File

@@ -336,8 +336,22 @@ void btFractureDynamicsWorld::removeRigidBody(btRigidBody* body)
if (body->getInternalType() & CUSTOM_FRACTURE_TYPE)
{
btFractureBody* fbody = (btFractureBody*)body;
btAlignedObjectArray<btTypedConstraint*> tmpConstraints;
for (int i=0;i<fbody->getNumConstraintRefs();i++)
{
tmpConstraints.push_back(fbody->getConstraintRef(i));
}
//remove all constraints attached to this rigid body too
for (int i=0;i<tmpConstraints.size();i++)
btDiscreteDynamicsWorld::removeConstraint(tmpConstraints[i]);
m_fractureBodies.remove(fbody);
}
btDiscreteDynamicsWorld::removeRigidBody(body);
}

2
Demos/GyroscopicDemo/GyroscopicDemo.cpp
View File

@@ -101,7 +101,7 @@ void GyroscopicDemo::initPhysics()
tr.setIdentity();
tr.setOrigin(positions[i]);
body->setCenterOfMassTransform(tr);
body->setAngularVelocity(btVector3(0,0,1000));
body->setAngularVelocity(btVector3(0,0,15));
body->setLinearVelocity(btVector3(0,.2,0));
body->setFriction(btSqrt(1));
m_dynamicsWorld->addRigidBody(body);

4
Demos/OpenGL/DemoApplication.cpp
View File

@@ -787,7 +787,7 @@ void DemoApplication::mouseFunc(int button, int state, int x, int y)
dof6->setAngularLowerLimit(btVector3(0,0,0));
dof6->setAngularUpperLimit(btVector3(0,0,0));
m_dynamicsWorld->addConstraint(dof6);
m_dynamicsWorld->addConstraint(dof6,true);
m_pickConstraint = dof6;
dof6->setParam(BT_CONSTRAINT_STOP_CFM,0.8,0);
@@ -806,7 +806,7 @@ void DemoApplication::mouseFunc(int button, int state, int x, int y)
} else
{
btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body,localPivot);
m_dynamicsWorld->addConstraint(p2p);
m_dynamicsWorld->addConstraint(p2p,true);
m_pickConstraint = p2p;
p2p->m_setting.m_impulseClamp = mousePickClamping;
//very weak constraint for picking

1
Demos/premake4.lua
View File

@@ -72,6 +72,7 @@ end
"RagdollDemo",
"Raytracer",
"RaytestDemo",
"RollingFrictionDemo",
"SimplexDemo",
"SliderConstraintDemo",
"TerrainDemo",

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

@@ -31,6 +31,7 @@ btCollisionObject::btCollisionObject()
m_activationState1(1),
m_deactivationTime(btScalar(0.)),
m_friction(btScalar(0.5)),
m_rollingFriction(0.0f),
m_restitution(btScalar(0.)),
m_internalType(CO_COLLISION_OBJECT),
m_userObjectPointer(0),

11
src/BulletCollision/CollisionDispatch/btCollisionObject.h
View File

@@ -85,6 +85,7 @@ protected:
btScalar m_friction;
btScalar m_restitution;
btScalar m_rollingFriction;
///m_internalType is reserved to distinguish Bullet's btCollisionObject, btRigidBody, btSoftBody, btGhostObject etc.
///do not assign your own m_internalType unless you write a new dynamics object class.
@@ -263,6 +264,16 @@ public:
return m_friction;
}
void setRollingFriction(btScalar frict)
{
m_rollingFriction = frict;
}
btScalar getRollingFriction() const
{
return m_rollingFriction;
}
///reserved for Bullet internal usage
int getInternalType() const
{

19
src/BulletCollision/CollisionDispatch/btManifoldResult.cpp
View File

@@ -22,6 +22,23 @@ subject to the following restrictions:
///This is to allow MaterialCombiner/Custom Friction/Restitution values
ContactAddedCallback gContactAddedCallback=0;
///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback;
inline btScalar calculateCombinedRollingFriction(const btCollisionObject* body0,const btCollisionObject* body1)
{
btScalar friction = body0->getRollingFriction() * body1->getRollingFriction();
const btScalar MAX_FRICTION = btScalar(10.);
if (friction < -MAX_FRICTION)
friction = -MAX_FRICTION;
if (friction > MAX_FRICTION)
friction = MAX_FRICTION;
return friction;
}
///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback;
inline btScalar calculateCombinedFriction(const btCollisionObject* body0,const btCollisionObject* body1)
{
@@ -91,7 +108,7 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b
newPt.m_combinedFriction = calculateCombinedFriction(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
newPt.m_combinedRestitution = calculateCombinedRestitution(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
newPt.m_combinedRollingFriction = calculateCombinedRollingFriction(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
btPlaneSpace1(newPt.m_normalWorldOnB,newPt.m_lateralFrictionDir1,newPt.m_lateralFrictionDir2);

2
src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h
View File

@@ -64,6 +64,7 @@ class btManifoldPoint
m_normalWorldOnB( normal ),
m_distance1( distance ),
m_combinedFriction(btScalar(0.)),
m_combinedRollingFriction(btScalar(0.)),
m_combinedRestitution(btScalar(0.)),
m_userPersistentData(0),
m_lateralFrictionInitialized(false),
@@ -90,6 +91,7 @@ class btManifoldPoint
btScalar m_distance1;
btScalar m_combinedFriction;
btScalar m_combinedRollingFriction;
btScalar m_combinedRestitution;
//BP mod, store contact triangles.

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

@@ -57,6 +57,7 @@ struct btContactSolverInfoData
int m_restingContactRestitutionThreshold;
int m_minimumSolverBatchSize;
btScalar m_maxGyroscopicForce;
btScalar m_singleAxisRollingFrictionThreshold;
};
@@ -88,6 +89,7 @@ struct btContactSolverInfo : public btContactSolverInfoData
m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD;// | SOLVER_RANDMIZE_ORDER;
m_minimumSolverBatchSize = 128; //try to combine islands until the amount of constraints reaches this limit
m_maxGyroscopicForce = 100.f; ///only used to clamp forces for bodies that have their BT_ENABLE_GYROPSCOPIC_FORCE flag set (using btRigidBody::setFlag)
m_singleAxisRollingFrictionThreshold = 1e30f;///if the velocity is above this threshold, it will use a single constraint row (axis), otherwise 3 rows.
}
};

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

@@ -115,8 +115,9 @@ void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGenericSIMD(
{
c.m_appliedImpulse = sum;
}
body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
body1.internalApplyImpulse(c.m_contactNormal*body1.internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
body2.internalApplyImpulse(-c.m_contactNormal*body2.internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
}
void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowLowerLimitSIMD(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
@@ -342,6 +343,8 @@ static void applyAnisotropicFriction(btCollisionObject* colObj,btVector3& fricti
}
void btSequentialImpulseConstraintSolver::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, btScalar desiredVelocity, btScalar cfmSlip)
{
@@ -373,38 +376,102 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0);
}
#ifdef COMPUTE_IMPULSE_DENOM
btScalar denom0 = rb0->computeImpulseDenominator(pos1,solverConstraint.m_contactNormal);
btScalar denom1 = rb1->computeImpulseDenominator(pos2,solverConstraint.m_contactNormal);
#else
btVector3 vec;
btScalar denom0 = 0.f;
btScalar denom1 = 0.f;
if (body0)
{
vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
denom0 = body0->getInvMass() + normalAxis.dot(vec);
btVector3 vec;
btScalar denom0 = 0.f;
btScalar denom1 = 0.f;
if (body0)
{
vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
denom0 = body0->getInvMass() + normalAxis.dot(vec);
}
if (body1)
{
vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
denom1 = body1->getInvMass() + normalAxis.dot(vec);
}
btScalar denom = relaxation/(denom0+denom1);
solverConstraint.m_jacDiagABInv = denom;
}
if (body1)
{
vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
denom1 = body1->getInvMass() + normalAxis.dot(vec);
btScalar rel_vel;
btScalar vel1Dotn = solverConstraint.m_contactNormal.dot(body0?solverBodyA.m_linearVelocity:btVector3(0,0,0))
+ solverConstraint.m_relpos1CrossNormal.dot(body0?solverBodyA.m_angularVelocity:btVector3(0,0,0));
btScalar vel2Dotn = -solverConstraint.m_contactNormal.dot(body1?solverBodyB.m_linearVelocity:btVector3(0,0,0))
+ solverConstraint.m_relpos2CrossNormal.dot(body1?solverBodyB.m_angularVelocity:btVector3(0,0,0));
rel_vel = vel1Dotn+vel2Dotn;
// btScalar positionalError = 0.f;
btSimdScalar velocityError = desiredVelocity - rel_vel;
btSimdScalar velocityImpulse = velocityError * btSimdScalar(solverConstraint.m_jacDiagABInv);
solverConstraint.m_rhs = velocityImpulse;
solverConstraint.m_cfm = cfmSlip;
solverConstraint.m_lowerLimit = 0;
solverConstraint.m_upperLimit = 1e10f;
}
}
btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
{
btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing();
solverConstraint.m_frictionIndex = frictionIndex;
setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2,
colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
return solverConstraint;
}
void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis1,int solverBodyIdA,int solverBodyIdB,
btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,
btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation,
btScalar desiredVelocity, btScalar cfmSlip)
{
btVector3 normalAxis(0,0,0);
solverConstraint.m_contactNormal = normalAxis;
btSolverBody& solverBodyA = m_tmpSolverBodyPool[solverBodyIdA];
btSolverBody& solverBodyB = m_tmpSolverBodyPool[solverBodyIdB];
btRigidBody* body0 = m_tmpSolverBodyPool[solverBodyIdA].m_originalBody;
btRigidBody* body1 = m_tmpSolverBodyPool[solverBodyIdB].m_originalBody;
solverConstraint.m_solverBodyIdA = solverBodyIdA;
solverConstraint.m_solverBodyIdB = solverBodyIdB;
solverConstraint.m_friction = cp.m_combinedRollingFriction;
solverConstraint.m_originalContactPoint = 0;
solverConstraint.m_appliedImpulse = 0.f;
solverConstraint.m_appliedPushImpulse = 0.f;
{
btVector3 ftorqueAxis1 = -normalAxis1;
solverConstraint.m_relpos1CrossNormal = ftorqueAxis1;
solverConstraint.m_angularComponentA = body0 ? body0->getInvInertiaTensorWorld()*ftorqueAxis1*body0->getAngularFactor() : btVector3(0,0,0);
}
{
btVector3 ftorqueAxis1 = normalAxis1;
solverConstraint.m_relpos2CrossNormal = ftorqueAxis1;
solverConstraint.m_angularComponentB = body1 ? body1->getInvInertiaTensorWorld()*ftorqueAxis1*body1->getAngularFactor() : btVector3(0,0,0);
}
#endif //COMPUTE_IMPULSE_DENOM
btScalar denom = relaxation/(denom0+denom1);
solverConstraint.m_jacDiagABInv = denom;
#ifdef _USE_JACOBIAN
solverConstraint.m_jac = btJacobianEntry (
rel_pos1,rel_pos2,solverConstraint.m_contactNormal,
body0->getInvInertiaDiagLocal(),
body0->getInvMass(),
body1->getInvInertiaDiagLocal(),
body1->getInvMass());
#endif //_USE_JACOBIAN
{
btVector3 iMJaA = body0?body0->getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal:btVector3(0,0,0);
btVector3 iMJaB = body1?body1->getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal:btVector3(0,0,0);
btScalar sum = 0;
sum += iMJaA.dot(solverConstraint.m_relpos1CrossNormal);
sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
solverConstraint.m_jacDiagABInv = btScalar(1.)/sum;
}
{
@@ -431,15 +498,21 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
btSolverConstraint& btSequentialImpulseConstraintSolver::addRollingFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
{
btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing();
btSolverConstraint& solverConstraint = m_tmpSolverContactRollingFrictionConstraintPool.expandNonInitializing();
solverConstraint.m_frictionIndex = frictionIndex;
setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2,
setupRollingFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2,
colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
return solverConstraint;
}
int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject& body)
{
@@ -690,6 +763,7 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
if (!solverBodyA || (!solverBodyA->m_originalBody && (!solverBodyB || !solverBodyB->m_originalBody)))
return;
int rollingFriction=1;
for (int j=0;j<manifold->getNumContacts();j++)
{
@@ -721,6 +795,34 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size();
btVector3 angVelA,angVelB;
solverBodyA->getAngularVelocity(angVelA);
solverBodyB->getAngularVelocity(angVelB);
btVector3 relAngVel = angVelB-angVelA;
if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))
{
//only a single rollingFriction per manifold
rollingFriction--;
if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold)
{
relAngVel.normalize();
addRollingFrictionConstraint(relAngVel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
} else
{
addRollingFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
btVector3 axis0,axis1;
btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1);
addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
addRollingFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
}
}
cp.m_lateralFrictionInitialized = false;
if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
{
cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
@@ -735,11 +837,14 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
}
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
cp.m_lateralFrictionInitialized = true;
} else
{
@@ -757,16 +862,19 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
cp.m_lateralFrictionInitialized = true;
}
} else
{
addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1);
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2);
}
setFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
}
@@ -782,6 +890,37 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
m_maxOverrideNumSolverIterations = 0;
#ifdef BT_DEBUG
for (int i=0;i<numConstraints;i++)
{
btTypedConstraint* constraint = constraints[i];
if (!constraint->getRigidBodyA().isStaticOrKinematicObject())
{
bool found=false;
for (int b=0;b<numBodies;b++)
{
if (&constraint->getRigidBodyA()==bodies[b])
{
found = true;
break;
}
}
btAssert(found);
}
if (!constraint->getRigidBodyB().isStaticOrKinematicObject())
{
bool found=false;
for (int b=0;b<numBodies;b++)
{
if (&constraint->getRigidBodyB()==bodies[b])
{
found = true;
break;
}
}
btAssert(found);
}
}
//make sure that dynamic bodies exist for all contact manifolds
for (int i=0;i<numManifolds;i++)
{
@@ -1071,13 +1210,14 @@ 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*/,btStackAlloc* /*stackAlloc*/)
{
int numNonContactPool = m_tmpSolverNonContactConstraintPool.size();
int numConstraintPool = m_tmpSolverContactConstraintPool.size();
int numFrictionPool = m_tmpSolverContactFrictionConstraintPool.size();
int numRollingFrictionPool = m_tmpSolverContactRollingFrictionConstraintPool.size();
if (infoGlobal.m_solverMode & SOLVER_RANDMIZE_ORDER)
{
@@ -1195,7 +1335,10 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
}
///solve all friction constraints, using SIMD, if available
int numFrictionPoolConstraints = m_tmpSolverContactFrictionConstraintPool.size();
for (j=0;j<numFrictionPoolConstraints;j++)
{
@@ -1210,6 +1353,23 @@ btScalar btSequentialImpulseConstraintSolver::solveSingleIteration(int iteration
resolveSingleConstraintRowGenericSIMD(m_tmpSolverBodyPool[solveManifold.m_solverBodyIdA],m_tmpSolverBodyPool[solveManifold.m_solverBodyIdB],solveManifold);
}
}
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))
{
rollingFrictionConstraint.m_lowerLimit = -(rollingFrictionConstraint.m_friction*totalImpulse);
rollingFrictionConstraint.m_upperLimit = rollingFrictionConstraint.m_friction*totalImpulse;
resolveSingleConstraintRowGeneric(m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdA],m_tmpSolverBodyPool[rollingFrictionConstraint.m_solverBodyIdB],rollingFrictionConstraint);
}
}
}
}
} else
@@ -1396,6 +1556,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCo
m_tmpSolverContactConstraintPool.resizeNoInitialize(0);
m_tmpSolverNonContactConstraintPool.resizeNoInitialize(0);
m_tmpSolverContactFrictionConstraintPool.resizeNoInitialize(0);
m_tmpSolverContactRollingFrictionConstraintPool.resizeNoInitialize(0);
m_tmpSolverBodyPool.resizeNoInitialize(0);
return 0.f;
}

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

@@ -36,6 +36,8 @@ protected:
btConstraintArray m_tmpSolverContactConstraintPool;
btConstraintArray m_tmpSolverNonContactConstraintPool;
btConstraintArray m_tmpSolverContactFrictionConstraintPool;
btConstraintArray m_tmpSolverContactRollingFrictionConstraintPool;
btAlignedObjectArray<int> m_orderTmpConstraintPool;
btAlignedObjectArray<int> m_orderNonContactConstraintPool;
btAlignedObjectArray<int> m_orderFrictionConstraintPool;
@@ -47,8 +49,15 @@ protected:
btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation,
btScalar desiredVelocity=0., btScalar cfmSlip=0.);
void setupRollingFrictionConstraint( 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,
btScalar desiredVelocity=0., btScalar cfmSlip=0.);
btSolverConstraint& addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0., btScalar cfmSlip=0.);
btSolverConstraint& addRollingFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0, btScalar cfmSlip=0.f);
void setupContactConstraint(btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp,
const btContactSolverInfo& infoGlobal, btVector3& vel, btScalar& rel_vel, btScalar& relaxation,
btVector3& rel_pos1, btVector3& rel_pos2);