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;