AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-14 22:00:05 +00:00
Code Issues Releases Wiki Activity

Implement 'cone friction' for the rolling friction, to avoid curved trajectories for contact with rolling friction enabled.

Browse Source 
(and separate spinning friction from rolling friction)
Fixes Issue 2117
This commit is contained in:
erwincoumans 2019-03-03 17:10:01 -08:00
parent d07c86842c
commit 04441a29cc
2 changed files with 84 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -70,13 +70,13 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
//solve featherstone frictional contact
if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS && ((infoGlobal.m_solverMode & SOLVER_DISABLE_IMPLICIT_CONE_FRICTION) == 0))
{
for (int j1 = 0; j1 < this->m_multiBodyTorsionalFrictionContactConstraints.size(); j1++)
for (int j1 = 0; j1 < this->m_multiBodySpinningFrictionContactConstraints.size(); j1++)
{
if (iteration < infoGlobal.m_numIterations)
{
int index = j1; //iteration&1? j1 : m_multiBodyTorsionalFrictionContactConstraints.size()-1-j1;
int index = j1;
btMultiBodySolverConstraint& frictionConstraint = m_multiBodyTorsionalFrictionContactConstraints[index];
btMultiBodySolverConstraint& frictionConstraint = m_multiBodySpinningFrictionContactConstraints[index];
btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
//adjust friction limits here
if (totalImpulse > btScalar(0))
@ -94,6 +94,41 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
}
}
for (int j1 = 0; j1 < this->m_multiBodyTorsionalFrictionContactConstraints.size(); j1++)
{
if (iteration < infoGlobal.m_numIterations)
{
int index = j1; //iteration&1? j1 : m_multiBodyTorsionalFrictionContactConstraints.size()-1-j1;
btMultiBodySolverConstraint& frictionConstraint = m_multiBodyTorsionalFrictionContactConstraints[index];
btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse;
j1++;
int index2 = j1;
btMultiBodySolverConstraint& frictionConstraintB = m_multiBodyTorsionalFrictionContactConstraints[index2];
//adjust friction limits here
if (totalImpulse > btScalar(0) && frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex)
{
frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction * totalImpulse);
frictionConstraint.m_upperLimit = frictionConstraint.m_friction * totalImpulse;
frictionConstraintB.m_lowerLimit = -(frictionConstraintB.m_friction * totalImpulse);
frictionConstraintB.m_upperLimit = frictionConstraintB.m_friction * totalImpulse;
btScalar residual = resolveConeFrictionConstraintRows(frictionConstraint, frictionConstraintB);
leastSquaredResidual = btMax(leastSquaredResidual, residual * residual);
if (frictionConstraint.m_multiBodyA)
frictionConstraint.m_multiBodyA->setPosUpdated(false);
if (frictionConstraint.m_multiBodyB)
frictionConstraint.m_multiBodyB->setPosUpdated(false);
if (frictionConstraintB.m_multiBodyA)
frictionConstraintB.m_multiBodyA->setPosUpdated(false);
if (frictionConstraintB.m_multiBodyB)
frictionConstraintB.m_multiBodyB->setPosUpdated(false);
}
}
}
for (int j1 = 0; j1 < this->m_multiBodyFrictionContactConstraints.size(); j1++)
{
if (iteration < infoGlobal.m_numIterations)
@ -164,6 +199,7 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionOb
m_multiBodyNormalContactConstraints.resize(0);
m_multiBodyFrictionContactConstraints.resize(0);
m_multiBodyTorsionalFrictionContactConstraints.resize(0);
m_multiBodySpinningFrictionContactConstraints.resize(0);
m_data.m_jacobians.resize(0);
m_data.m_deltaVelocitiesUnitImpulse.resize(0);
@ -1169,6 +1205,43 @@ btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyTorsionalF
return solverConstraint;
}
btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodySpinningFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp,
btScalar combinedTorsionalFriction,
btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
{
BT_PROFILE("addMultiBodyRollingFrictionConstraint");
btMultiBodySolverConstraint& solverConstraint = m_multiBodySpinningFrictionContactConstraints.expandNonInitializing();
solverConstraint.m_orgConstraint = 0;
solverConstraint.m_orgDofIndex = -1;
solverConstraint.m_frictionIndex = frictionIndex;
bool isFriction = true;
const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
const btMultiBodyLinkCollider* fcB = btMultiBodyLinkCollider::upcast(manifold->getBody1());
btMultiBody* mbA = fcA ? fcA->m_multiBody : 0;
btMultiBody* mbB = fcB ? fcB->m_multiBody : 0;
int solverBodyIdA = mbA ? -1 : getOrInitSolverBody(*colObj0, infoGlobal.m_timeStep);
int solverBodyIdB = mbB ? -1 : getOrInitSolverBody(*colObj1, infoGlobal.m_timeStep);
solverConstraint.m_solverBodyIdA = solverBodyIdA;
solverConstraint.m_solverBodyIdB = solverBodyIdB;
solverConstraint.m_multiBodyA = mbA;
if (mbA)
solverConstraint.m_linkA = fcA->m_link;
solverConstraint.m_multiBodyB = mbB;
if (mbB)
solverConstraint.m_linkB = fcB->m_link;
solverConstraint.m_originalContactPoint = &cp;
setupMultiBodyTorsionalFrictionConstraint(solverConstraint, normalAxis, cp, combinedTorsionalFriction, infoGlobal, relaxation, isFriction, desiredVelocity, cfmSlip);
return solverConstraint;
}
void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* manifold, const btContactSolverInfo& infoGlobal)
{
const btMultiBodyLinkCollider* fcA = btMultiBodyLinkCollider::upcast(manifold->getBody0());
@ -1258,7 +1331,7 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
{
if (cp.m_combinedSpinningFriction > 0)
{
addMultiBodyTorsionalFrictionConstraint(cp.m_normalWorldOnB, manifold, frictionIndex, cp, cp.m_combinedSpinningFriction, colObj0, colObj1, relaxation, infoGlobal);
addMultiBodySpinningFrictionConstraint(cp.m_normalWorldOnB, manifold, frictionIndex, cp, cp.m_combinedSpinningFriction, colObj0, colObj1, relaxation, infoGlobal);
}
if (cp.m_combinedRollingFriction > 0)
{
@ -1267,11 +1340,8 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
applyAnisotropicFriction(colObj0, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
applyAnisotropicFriction(colObj1, cp.m_lateralFrictionDir2, btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
if (cp.m_lateralFrictionDir1.length() > 0.001)
addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir1, manifold, frictionIndex, cp, cp.m_combinedRollingFriction, colObj0, colObj1, relaxation, infoGlobal);
if (cp.m_lateralFrictionDir2.length() > 0.001)
addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir2, manifold, frictionIndex, cp, cp.m_combinedRollingFriction, colObj0, colObj1, relaxation, infoGlobal);
addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir1, manifold, frictionIndex, cp, cp.m_combinedRollingFriction, colObj0, colObj1, relaxation, infoGlobal);
addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir2, manifold, frictionIndex, cp, cp.m_combinedRollingFriction, colObj0, colObj1, relaxation, infoGlobal);
}
rollingFriction--;
}

5
src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h
View File

@ -34,6 +34,7 @@ protected:
btMultiBodyConstraintArray m_multiBodyNormalContactConstraints;
btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints;
btMultiBodyConstraintArray m_multiBodyTorsionalFrictionContactConstraints;
btMultiBodyConstraintArray m_multiBodySpinningFrictionContactConstraints;
btMultiBodyJacobianData m_data;
@ -54,6 +55,10 @@ protected:
btScalar combinedTorsionalFriction,
btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0, btScalar cfmSlip = 0);
btMultiBodySolverConstraint& addMultiBodySpinningFrictionConstraint(const btVector3& normalAxis, btPersistentManifold* manifold, int frictionIndex, btManifoldPoint& cp,
btScalar combinedTorsionalFriction,
btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0, btScalar cfmSlip = 0);
void setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint & constraintRow,
btScalar * jacA, btScalar * jacB,
btScalar penetration, btScalar combinedFrictionCoeff, btScalar combinedRestitutionCoeff,