From 645a88176dbb617c95095fd0702ddfba5e6232d0 Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Fri, 22 Jan 2016 17:43:36 -0800 Subject: [PATCH] Enable softness for btRigidBody contacts. This is implemented by some value (CFM, constraint force mixing) to the main diagonal of A. 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) --- .../NarrowPhaseCollision/btManifoldPoint.h | 31 +++++++++++-------- .../btPersistentManifold.h | 3 +- .../btSequentialImpulseConstraintSolver.cpp | 23 ++++++++------ .../btMultiBodyConstraintSolver.cpp | 8 ++--- .../MLCPSolvers/btMLCPSolver.cpp | 7 ++--- src/BulletDynamics/MLCPSolvers/btMLCPSolver.h | 10 ------ 6 files changed, 40 insertions(+), 42 deletions(-) diff --git a/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h b/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h index e40fb1d3d..572e899c3 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h +++ b/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h @@ -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 diff --git a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h index 5026397f6..d220f2993 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h +++ b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h @@ -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; diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index 8da572bf7..e01f4ea87 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -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); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index 8a034b38a..db20a06b2 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -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: diff --git a/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp b/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp index 6688694a9..e73f4acc4 100644 --- a/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp +++ b/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp @@ -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_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;