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

Enable softness for btRigidBody contacts. This is implemented by some value (CFM, constraint force mixing) to the main diagonal of A.

Browse Source 
CFM for contacts use world CFM value by default, and can override with custom CFM value using the
BT_CONTACT_FLAG_HAS_CONTACT_CFM stored in m_contactPointFlags.
Boolean m_lateralFrictionInitialized is replaced 'BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED' flag stored in int m_contactPointFlags in btManifoldPoint.
Enable successive over-relaxation parameter (SOR) for contacts. btMLCPSolver uses global CFM.
In one of the next commits, contact softness will be enabled btMultiBody contacts.
Also need to review use of CFM in btMLCPSolvers (only world CFM is used at the moment)
This commit is contained in:
erwin coumans
2016-01-22 17:43:36 -08:00
parent 6bc3f94f52
commit 645a88176d
6 changed files with 40 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -35,7 +35,11 @@ typedef sce::PhysicsEffects::PfxConstraintRow btConstraintRow;
typedef btConstraintRow PfxConstraintRow;
#endif //PFX_USE_FREE_VECTORMATH
enum btContactPointFlags
{
BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED=1,
BT_CONTACT_FLAG_HAS_CONTACT_CFM=2,
};
/// ManifoldContactPoint collects and maintains persistent contactpoints.
/// used to improve stability and performance of rigidbody dynamics response.
@@ -44,14 +48,14 @@ class btManifoldPoint
public:
btManifoldPoint()
:m_userPersistentData(0),
m_lateralFrictionInitialized(false),
m_appliedImpulse(0.f),
m_contactPointFlags(0),
m_appliedImpulse(0.f),
m_appliedImpulseLateral1(0.f),
m_appliedImpulseLateral2(0.f),
m_contactMotion1(0.f),
m_contactMotion2(0.f),
m_contactCFM1(0.f),
m_contactCFM2(0.f),
m_contactCFM(0.f),
m_frictionCFM(0.f),
m_lifeTime(0)
{
}
@@ -67,14 +71,14 @@ class btManifoldPoint
m_combinedRollingFriction(btScalar(0.)),
m_combinedRestitution(btScalar(0.)),
m_userPersistentData(0),
m_lateralFrictionInitialized(false),
m_appliedImpulse(0.f),
m_contactPointFlags(0),
m_appliedImpulse(0.f),
m_appliedImpulseLateral1(0.f),
m_appliedImpulseLateral2(0.f),
m_contactMotion1(0.f),
m_contactMotion2(0.f),
m_contactCFM1(0.f),
m_contactCFM2(0.f),
m_contactCFM(0.f),
m_frictionCFM(0.f),
m_lifeTime(0)
{
@@ -101,15 +105,16 @@ class btManifoldPoint
int m_index1;
mutable void* m_userPersistentData;
bool m_lateralFrictionInitialized;
//bool m_lateralFrictionInitialized;
int m_contactPointFlags;
btScalar m_appliedImpulse;
btScalar m_appliedImpulseLateral1;
btScalar m_appliedImpulseLateral2;
btScalar m_contactMotion1;
btScalar m_contactMotion2;
btScalar m_contactCFM1;
btScalar m_contactCFM2;
btScalar m_contactCFM;
btScalar m_frictionCFM;
int m_lifeTime;//lifetime of the contactpoint in frames

3
src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h
View File

@@ -163,7 +163,7 @@ public:
//get rid of duplicated userPersistentData pointer
m_pointCache[lastUsedIndex].m_userPersistentData = 0;
m_pointCache[lastUsedIndex].m_appliedImpulse = 0.f;
m_pointCache[lastUsedIndex].m_lateralFrictionInitialized = false;
m_pointCache[lastUsedIndex].m_contactPointFlags = 0;
m_pointCache[lastUsedIndex].m_appliedImpulseLateral1 = 0.f;
m_pointCache[lastUsedIndex].m_appliedImpulseLateral2 = 0.f;
m_pointCache[lastUsedIndex].m_lifeTime = 0;
@@ -190,7 +190,6 @@ public:
void* cache = m_pointCache[insertIndex].m_userPersistentData;
m_pointCache[insertIndex] = newPoint;
m_pointCache[insertIndex].m_userPersistentData = cache;
m_pointCache[insertIndex].m_appliedImpulse = appliedImpulse;
m_pointCache[insertIndex].m_appliedImpulseLateral1 = appliedLateralImpulse1;

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

@@ -775,7 +775,11 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
//rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin();
//rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
relaxation = 1.f;
relaxation = infoGlobal.m_sor;
btScalar invTimeStep = btScalar(1)/infoGlobal.m_timeStep;
btScalar cfm = (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM)?cp.m_contactCFM:infoGlobal.m_globalCfm;
cfm *= invTimeStep;
btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
@@ -802,7 +806,7 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
}
#endif //COMPUTE_IMPULSE_DENOM
btScalar denom = relaxation/(denom0+denom1);
btScalar denom = relaxation/(denom0+denom1+cfm);
solverConstraint.m_jacDiagABInv = denom;
}
@@ -894,10 +898,11 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
{
positionalError = 0;
velocityError -= penetration / infoGlobal.m_timeStep;
velocityError -= penetration *invTimeStep;
} else
{
positionalError = -penetration * erp/infoGlobal.m_timeStep;
positionalError = -penetration * erp*invTimeStep;
}
btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
@@ -915,7 +920,7 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
solverConstraint.m_rhs = velocityImpulse;
solverConstraint.m_rhsPenetration = penetrationImpulse;
}
solverConstraint.m_cfm = 0.f;
solverConstraint.m_cfm = cfm*solverConstraint.m_jacDiagABInv;
solverConstraint.m_lowerLimit = 0;
solverConstraint.m_upperLimit = 1e10f;
}
@@ -1094,7 +1099,7 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2)
///this will give a conveyor belt effect
///
if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !(cp.m_contactPointFlags&BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED))
{
cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
@@ -1132,16 +1137,16 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
{
cp.m_lateralFrictionInitialized = true;
cp.m_contactPointFlags|=BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED;
}
}
} else
{
addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1);
addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_frictionCFM);
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);
addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_frictionCFM);
}
setFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);

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

@@ -685,7 +685,7 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2)
///this will give a conveyor belt effect
///
if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !(cp.m_contactPointFlags&BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED))
{/*
cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
@@ -724,16 +724,16 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION))
{
cp.m_lateralFrictionInitialized = true;
cp.m_contactPointFlags|=BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED;
}
}
} else
{
addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal,cp.m_contactMotion1, cp.m_contactCFM1);
addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal,cp.m_contactMotion1, cp.m_frictionCFM);
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation, infoGlobal,cp.m_contactMotion2, cp.m_contactCFM2);
addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation, infoGlobal,cp.m_contactMotion2, cp.m_frictionCFM);
//setMultiBodyFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
//todo:

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

@@ -22,8 +22,7 @@ subject to the following restrictions:
btMLCPSolver::btMLCPSolver( btMLCPSolverInterface* solver)
:m_solver(solver),
m_fallback(0),
m_cfm(0.000001)//0.0000001
m_fallback(0)
{
}
@@ -436,7 +435,7 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal)
// add cfm to the diagonal of m_A
for ( int i=0; i<m_A.rows(); ++i)
{
m_A.setElem(i,i,m_A(i,i)+ m_cfm / infoGlobal.m_timeStep);
m_A.setElem(i,i,m_A(i,i)+ infoGlobal.m_globalCfm/ infoGlobal.m_timeStep);
}
}
@@ -564,7 +563,7 @@ void btMLCPSolver::createMLCP(const btContactSolverInfo& infoGlobal)
// add cfm to the diagonal of m_A
for ( int i=0; i<m_A.rows(); ++i)
{
m_A.setElem(i,i,m_A(i,i)+ m_cfm / infoGlobal.m_timeStep);
m_A.setElem(i,i,m_A(i,i)+ infoGlobal.m_globalCfm / infoGlobal.m_timeStep);
}
}

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

@@ -42,7 +42,6 @@ protected:
btAlignedObjectArray<btSolverConstraint*> m_allConstraintPtrArray;
btMLCPSolverInterface* m_solver;
int m_fallback;
btScalar m_cfm;
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
@@ -73,15 +72,6 @@ public:
m_fallback = num;
}
btScalar getCfm() const
{
return m_cfm;
}
void setCfm(btScalar cfm)
{
m_cfm = cfm;
}
virtual btConstraintSolverType getSolverType() const
{
return BT_MLCP_SOLVER;