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

Merge pull request #770 from erwincoumans/master

Browse Source 
export contact friction/damping through URDF and API
This commit is contained in:
erwincoumans
2016-09-02 18:18:21 -07:00
committed by GitHub
parent 21c60c66ec ecd814c9c5
commit 4c9b009d5b
19 changed files with 269 additions and 125 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
data/cube_no_friction.urdf
View File

@@ -4,8 +4,8 @@
<contact>
<lateral_friction value="0.0"/>
<rolling_friction value="0.0"/>
<contact_cfm value="0.0"/>
<contact_erp value="1.0"/>
<stiffness value="300.0"/>
<damping value="10.0"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>

4
data/cube_small.urdf
View File

@@ -3,10 +3,8 @@
<link name="baseLink">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.3"/>
<rolling_friction value="0.01"/>
<inertia_scaling value="3.0"/>
<contact_cfm value="0.0"/>
<contact_erp value="1.0"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>

32
data/cube_soft.urdf Normal file
View File

@@ -0,0 +1,32 @@
<?xml version="0.0" ?>
<robot name="cube.urdf">
<link name="baseLink">
<contact>
<lateral_friction value="1.0"/>
<rolling_friction value="0.0"/>
<stiffness value="300"/>
<damping value="10"/>
</contact>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="1.0"/>
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="cube.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="1 1 1"/>
</geometry>
</collision>
</link>
</robot>

BIN
data/multibody.bullet
View File

Binary file not shown.

5
examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
View File

@@ -410,7 +410,10 @@ void ConvertURDF2BulletInternal(
{
col->setRollingFriction(contactInfo.m_rollingFriction);
}
if ((contactInfo.m_flags & URDF_CONTACT_HAS_STIFFNESS_DAMPING)!=0)
{
col->setContactStiffnessAndDamping(contactInfo.m_contactStiffness,contactInfo.m_contactDamping);
}
if (mbLinkIndex>=0) //???? double-check +/- 1
{

10
examples/Importers/ImportURDFDemo/URDFJointTypes.h
View File

@@ -19,7 +19,8 @@ enum URDF_LinkContactFlags
URDF_CONTACT_HAS_ROLLING_FRICTION=2,
URDF_CONTACT_HAS_INERTIA_SCALING=2,
URDF_CONTACT_HAS_CONTACT_CFM=4,
URDF_CONTACT_HAS_CONTACT_ERP=8
URDF_CONTACT_HAS_CONTACT_ERP=8,
URDF_CONTACT_HAS_STIFFNESS_DAMPING=16,
};
struct URDFLinkContactInfo
@@ -29,6 +30,9 @@ struct URDFLinkContactInfo
btScalar m_inertiaScaling;
btScalar m_contactCfm;
btScalar m_contactErp;
btScalar m_contactStiffness;
btScalar m_contactDamping;
int m_flags;
URDFLinkContactInfo()
@@ -36,7 +40,9 @@ struct URDFLinkContactInfo
m_rollingFriction(0),
m_inertiaScaling(1),
m_contactCfm(0),
m_contactErp(0)
m_contactErp(0),
m_contactStiffness(1e4),
m_contactDamping(1)
{
m_flags = URDF_CONTACT_HAS_LATERAL_FRICTION;
}

77
examples/Importers/ImportURDFDemo/UrdfParser.cpp
View File

@@ -647,26 +647,75 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
}
}
TiXmlElement *rolling_xml = ci->FirstChildElement("rolling_friction");
if (rolling_xml)
{
if (m_parseSDF)
TiXmlElement *rolling_xml = ci->FirstChildElement("rolling_friction");
if (rolling_xml)
{
link.m_contactInfo.m_rollingFriction = urdfLexicalCast<double>(rolling_xml->GetText());
link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_ROLLING_FRICTION;
} else
{
if (!rolling_xml->Attribute("value"))
if (m_parseSDF)
{
logger->reportError("Link/contact: rolling friction element must have value attribute");
return false;
link.m_contactInfo.m_rollingFriction = urdfLexicalCast<double>(rolling_xml->GetText());
link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_ROLLING_FRICTION;
} else
{
if (!rolling_xml->Attribute("value"))
{
logger->reportError("Link/contact: rolling friction element must have value attribute");
return false;
}
link.m_contactInfo.m_rollingFriction = urdfLexicalCast<double>(rolling_xml->Attribute("value"));
link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_ROLLING_FRICTION;
}
link.m_contactInfo.m_rollingFriction = urdfLexicalCast<double>(rolling_xml->Attribute("value"));
link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_ROLLING_FRICTION;
}
}
{
TiXmlElement *stiffness_xml = ci->FirstChildElement("stiffness");
if (stiffness_xml)
{
if (m_parseSDF)
{
link.m_contactInfo.m_contactStiffness = urdfLexicalCast<double>(stiffness_xml->GetText());
link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_STIFFNESS_DAMPING;
} else
{
if (!stiffness_xml->Attribute("value"))
{
logger->reportError("Link/contact: stiffness element must have value attribute");
return false;
}
link.m_contactInfo.m_contactStiffness = urdfLexicalCast<double>(stiffness_xml->Attribute("value"));
link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_STIFFNESS_DAMPING;
}
}
}
{
TiXmlElement *damping_xml = ci->FirstChildElement("damping");
if (damping_xml)
{
if (m_parseSDF)
{
link.m_contactInfo.m_contactDamping = urdfLexicalCast<double>(damping_xml->GetText());
link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_STIFFNESS_DAMPING;
} else
{
if (!damping_xml->Attribute("value"))
{
logger->reportError("Link/contact: damping element must have value attribute");
return false;
}
link.m_contactInfo.m_contactDamping = urdfLexicalCast<double>(damping_xml->Attribute("value"));
link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_STIFFNESS_DAMPING;
}
}
}
}
}

21
examples/MultiBody/MultiBodySoftContact.cpp
View File

@@ -36,7 +36,6 @@ public:
};
extern ContactAddedCallback gContactAddedCallback;
MultiBodySoftContact::MultiBodySoftContact(struct GUIHelperInterface* helper)
@@ -47,18 +46,7 @@ m_once(true)
MultiBodySoftContact::~MultiBodySoftContact()
{
gContactAddedCallback = 0;
}
static bool btMultiBodySoftContactCallback(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap,int partId0,int index0,const btCollisionObjectWrapper* colObj1Wrap,int partId1,int index1)
{
cp.m_contactCFM = 0.3;
cp.m_contactERP = 0.2;
cp.m_contactPointFlags |= BT_CONTACT_FLAG_HAS_CONTACT_CFM;
cp.m_contactPointFlags |= BT_CONTACT_FLAG_HAS_CONTACT_ERP;
return true;
}
@@ -66,7 +54,7 @@ static bool btMultiBodySoftContactCallback(btManifoldPoint& cp, const btCollisio
void MultiBodySoftContact::initPhysics()
{
gContactAddedCallback = btMultiBodySoftContactCallback;
int upAxis = 2;
m_guiHelper->setUpAxis(upAxis);
@@ -109,12 +97,13 @@ void MultiBodySoftContact::initPhysics()
start.setOrigin(groundOrigin);
// start.setRotation(groundOrn);
btRigidBody* body = createRigidBody(0,start,box);
//setContactStiffnessAndDamping will enable compliant rigid body contact
body->setContactStiffnessAndDamping(300,10);
btVector4 color = colors[curColor];
curColor++;
curColor&=3;
m_guiHelper->createRigidBodyGraphicsObject(body,color);
int flags = body->getCollisionFlags();
body->setCollisionFlags(flags|btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
}

20
examples/RigidBody/RigidBodySoftContact.cpp
View File

@@ -29,7 +29,7 @@ subject to the following restrictions:
#include "BulletDynamics/MLCPSolvers/btMLCPSolver.h"
#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h"
extern ContactAddedCallback gContactAddedCallback;
struct RigidBodySoftContact : public CommonRigidBodyBase
@@ -40,7 +40,7 @@ struct RigidBodySoftContact : public CommonRigidBodyBase
}
virtual ~RigidBodySoftContact()
{
gContactAddedCallback = 0;
}
virtual void initPhysics();
virtual void renderScene();
@@ -55,21 +55,12 @@ struct RigidBodySoftContact : public CommonRigidBodyBase
};
static bool btRigidBodySoftContactCallback(btManifoldPoint& cp, const btCollisionObjectWrapper* colObj0Wrap,int partId0,int index0,const btCollisionObjectWrapper* colObj1Wrap,int partId1,int index1)
{
cp.m_contactCFM = 0.3;
cp.m_contactERP = 0.2;
cp.m_contactPointFlags |= BT_CONTACT_FLAG_HAS_CONTACT_CFM;
cp.m_contactPointFlags |= BT_CONTACT_FLAG_HAS_CONTACT_ERP;
return true;
}
void RigidBodySoftContact::initPhysics()
{
gContactAddedCallback = btRigidBodySoftContactCallback;
m_guiHelper->setUpAxis(1);
@@ -120,8 +111,9 @@ void RigidBodySoftContact::initPhysics()
{
btScalar mass(0.);
btRigidBody* body = createRigidBody(mass,groundTransform,groundShape, btVector4(0,0,1,1));
int flags = body->getCollisionFlags();
body->setCollisionFlags(flags|btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
body->setContactStiffnessAndDamping(300,10);
}

2
examples/RoboticsLearning/GripperGraspExample.cpp
View File

@@ -153,7 +153,7 @@ public:
}
m_robotSim.setGravity(b3MakeVector3(0,0,-10));
//m_robotSim.setNumSimulationSubSteps(4);
m_robotSim.setNumSimulationSubSteps(4);
}
if ((m_options & eTWO_POINT_GRASP)!=0)

2
examples/RoboticsLearning/R2D2GraspExample.cpp
View File

@@ -117,7 +117,7 @@ public:
b3RobotSimLoadFileArgs args("");
b3RobotSimLoadFileResults results;
{
args.m_fileName = "cube.urdf";
args.m_fileName = "cube_soft.urdf";
args.m_startPosition.setValue(0,0,2.5);
args.m_startOrientation.setEulerZYX(0,0.2,0);
m_robotSim.loadFile(args,results);

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

@@ -33,6 +33,8 @@ btCollisionObject::btCollisionObject()
m_friction(btScalar(0.5)),
m_restitution(btScalar(0.)),
m_rollingFriction(0.0f),
m_contactDamping(.1),
m_contactStiffness(1e4),
m_internalType(CO_COLLISION_OBJECT),
m_userObjectPointer(0),
m_userIndex2(-1),
@@ -92,6 +94,8 @@ const char* btCollisionObject::serialize(void* dataBuffer, btSerializer* seriali
dataOut->m_deactivationTime = m_deactivationTime;
dataOut->m_friction = m_friction;
dataOut->m_rollingFriction = m_rollingFriction;
dataOut->m_contactDamping = m_contactDamping;
dataOut->m_contactStiffness = m_contactStiffness;
dataOut->m_restitution = m_restitution;
dataOut->m_internalType = m_internalType;

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

@@ -86,6 +86,10 @@ protected:
btScalar m_friction;
btScalar m_restitution;
btScalar m_rollingFriction;
btScalar m_contactDamping;
btScalar m_contactStiffness;
///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.
@@ -129,7 +133,8 @@ public:
CF_CUSTOM_MATERIAL_CALLBACK = 8,//this allows per-triangle material (friction/restitution)
CF_CHARACTER_OBJECT = 16,
CF_DISABLE_VISUALIZE_OBJECT = 32, //disable debug drawing
CF_DISABLE_SPU_COLLISION_PROCESSING = 64//disable parallel/SPU processing
CF_DISABLE_SPU_COLLISION_PROCESSING = 64,//disable parallel/SPU processing
CF_HAS_CONTACT_STIFFNESS_DAMPING = 128
};
enum CollisionObjectTypes
@@ -319,7 +324,31 @@ public:
return m_rollingFriction;
}
void setContactStiffnessAndDamping(btScalar stiffness, btScalar damping)
{
m_updateRevision++;
m_contactStiffness = stiffness;
m_contactDamping = damping;
m_collisionFlags |=CF_HAS_CONTACT_STIFFNESS_DAMPING;
//avoid divisions by zero...
if (m_contactStiffness< SIMD_EPSILON)
{
m_contactStiffness = SIMD_EPSILON;
}
}
btScalar getContactStiffness() const
{
return m_contactStiffness;
}
btScalar getContactDamping() const
{
return m_contactDamping;
}
///reserved for Bullet internal usage
int getInternalType() const
{
@@ -541,6 +570,8 @@ struct btCollisionObjectDoubleData
double m_deactivationTime;
double m_friction;
double m_rollingFriction;
double m_contactDamping;
double m_contactStiffness;
double m_restitution;
double m_hitFraction;
double m_ccdSweptSphereRadius;
@@ -574,7 +605,8 @@ struct btCollisionObjectFloatData
float m_deactivationTime;
float m_friction;
float m_rollingFriction;
float m_contactDamping;
float m_contactStiffness;
float m_restitution;
float m_hitFraction;
float m_ccdSweptSphereRadius;

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

@@ -25,7 +25,7 @@ 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 btManifoldResult::calculateCombinedRollingFriction(const btCollisionObject* body0,const btCollisionObject* body1)
{
btScalar friction = body0->getRollingFriction() * body1->getRollingFriction();
@@ -58,6 +58,22 @@ btScalar btManifoldResult::calculateCombinedRestitution(const btCollisionObject*
return body0->getRestitution() * body1->getRestitution();
}
btScalar btManifoldResult::calculateCombinedContactDamping(const btCollisionObject* body0,const btCollisionObject* body1)
{
return body0->getContactDamping() + body1->getContactDamping();
}
btScalar btManifoldResult::calculateCombinedContactStiffness(const btCollisionObject* body0,const btCollisionObject* body1)
{
btScalar s0 = body0->getContactStiffness();
btScalar s1 = body1->getContactStiffness();
btScalar tmp0 = btScalar(1)/s0;
btScalar tmp1 = btScalar(1)/s1;
btScalar combinedStiffness = btScalar(1) / (tmp0+tmp1);
return combinedStiffness;
}
btManifoldResult::btManifoldResult(const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap)
@@ -109,6 +125,15 @@ 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());
if ( (m_body0Wrap->getCollisionObject()->getCollisionFlags()& btCollisionObject::CF_HAS_CONTACT_STIFFNESS_DAMPING) ||
(m_body1Wrap->getCollisionObject()->getCollisionFlags()& btCollisionObject::CF_HAS_CONTACT_STIFFNESS_DAMPING))
{
newPt.m_combinedContactDamping1 = calculateCombinedContactDamping(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
newPt.m_combinedContactStiffness1 = calculateCombinedContactStiffness(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
newPt.m_contactPointFlags |= BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING;
}
btPlaneSpace1(newPt.m_normalWorldOnB,newPt.m_lateralFrictionDir1,newPt.m_lateralFrictionDir2);

3
src/BulletCollision/CollisionDispatch/btManifoldResult.h
View File

@@ -145,6 +145,9 @@ public:
/// in the future we can let the user override the methods to combine restitution and friction
static btScalar calculateCombinedRestitution(const btCollisionObject* body0,const btCollisionObject* body1);
static btScalar calculateCombinedFriction(const btCollisionObject* body0,const btCollisionObject* body1);
static btScalar calculateCombinedRollingFriction(const btCollisionObject* body0,const btCollisionObject* body1);
static btScalar calculateCombinedContactDamping(const btCollisionObject* body0,const btCollisionObject* body1);
static btScalar calculateCombinedContactStiffness(const btCollisionObject* body0,const btCollisionObject* body1);
};
#endif //BT_MANIFOLD_RESULT_H

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

@@ -40,6 +40,7 @@ enum btContactPointFlags
BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED=1,
BT_CONTACT_FLAG_HAS_CONTACT_CFM=2,
BT_CONTACT_FLAG_HAS_CONTACT_ERP=4,
BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING = 8,
};
/// ManifoldContactPoint collects and maintains persistent contactpoints.
@@ -116,8 +117,18 @@ class btManifoldPoint
btScalar m_appliedImpulseLateral2;
btScalar m_contactMotion1;
btScalar m_contactMotion2;
btScalar m_contactCFM;
btScalar m_contactERP;
union
{
btScalar m_contactCFM;
btScalar m_combinedContactStiffness1;
};
union
{
btScalar m_contactERP;
btScalar m_combinedContactDamping1;
};
btScalar m_frictionCFM;

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

@@ -777,10 +777,35 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra
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;
btScalar erp = (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP)?cp.m_contactERP:infoGlobal.m_erp2;
//cfm = 1 / ( dt * kp + kd )
//erp = dt * kp / ( dt * kp + kd )
btScalar cfm = infoGlobal.m_globalCfm;
btScalar erp = infoGlobal.m_erp2;
if ((cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM) || (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP))
{
if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM)
cfm = cp.m_contactCFM;
if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP)
erp = cp.m_contactERP;
} else
{
if (cp.m_contactPointFlags & BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING)
{
btScalar denom = ( infoGlobal.m_timeStep * cp.m_combinedContactStiffness1 + cp.m_combinedContactDamping1 );
if (denom < SIMD_EPSILON)
{
denom = SIMD_EPSILON;
}
cfm = btScalar(1) / denom;
erp = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1) / denom;
}
}
cfm *= invTimeStep;
btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB);
solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
@@ -1053,8 +1078,8 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))
{
//disabled: only a single rollingFriction per manifold
// rollingFriction--;
//only a single rollingFriction per manifold
//rollingFriction--;
if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold)
{
relAngVel.normalize();
@@ -1068,6 +1093,9 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
addRollingFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
btVector3 axis0,axis1;
btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1);
axis0.normalize();
axis1.normalize();
applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
applyAnisotropicFriction(colObj1,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);

2
src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
View File

@@ -299,7 +299,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr
//compute rhs and remaining solverConstraint fields
btScalar penetration = isFriction? 0 : posError+infoGlobal.m_linearSlop;
btScalar penetration = isFriction? 0 : posError;
btScalar rel_vel = 0.f;
int ndofA = 0;

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

@@ -224,11 +224,34 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
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;
btScalar erp = (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP)?cp.m_contactERP:infoGlobal.m_erp2;
//cfm = 1 / ( dt * kp + kd )
//erp = dt * kp / ( dt * kp + kd )
btScalar cfm = infoGlobal.m_globalCfm;
btScalar erp = infoGlobal.m_erp2;
if ((cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM) || (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP))
{
if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM)
cfm = cp.m_contactCFM;
if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP)
erp = cp.m_contactERP;
} else
{
if (cp.m_contactPointFlags & BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING)
{
btScalar denom = ( infoGlobal.m_timeStep * cp.m_combinedContactStiffness1 + cp.m_combinedContactDamping1 );
if (denom < SIMD_EPSILON)
{
denom = SIMD_EPSILON;
}
cfm = btScalar(1) / denom;
erp = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1) / denom;
}
}
cfm *= invTimeStep;
@@ -565,12 +588,6 @@ void btMultiBodyConstraintSolver::setupMultiBodyRollingFrictionConstraint(btMult
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;
btScalar erp = (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP)?cp.m_contactERP:infoGlobal.m_erp2;
if (multiBodyA)
@@ -713,7 +730,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyRollingFrictionConstraint(btMult
btScalar d = denom0+denom1+cfm;
btScalar d = denom0+denom1+infoGlobal.m_globalCfm;
if (d>SIMD_EPSILON)
{
solverConstraint.m_jacDiagABInv = relaxation/(d);
@@ -731,7 +748,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyRollingFrictionConstraint(btMult
btScalar restitution = 0.f;
btScalar penetration = isFriction? 0 : cp.getDistance()+infoGlobal.m_linearSlop;
btScalar penetration = isFriction? 0 : cp.getDistance();
btScalar rel_vel = 0.f;
int ndofA = 0;
@@ -790,15 +807,9 @@ void btMultiBodyConstraintSolver::setupMultiBodyRollingFrictionConstraint(btMult
if (penetration>0)
{
positionalError = 0;
velocityError -= penetration / infoGlobal.m_timeStep;
} else
{
positionalError = -penetration * erp/infoGlobal.m_timeStep;
}
}
btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
btScalar velocityImpulse = velocityError*solverConstraint.m_jacDiagABInv;
solverConstraint.m_rhs = velocityImpulse;
@@ -806,7 +817,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyRollingFrictionConstraint(btMult
solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
solverConstraint.m_upperLimit = solverConstraint.m_friction;
solverConstraint.m_cfm = cfm*solverConstraint.m_jacDiagABInv;
solverConstraint.m_cfm = infoGlobal.m_globalCfm*solverConstraint.m_jacDiagABInv;
@@ -951,45 +962,6 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
#define ENABLE_FRICTION
#ifdef ENABLE_FRICTION
solverConstraint.m_frictionIndex = frictionIndex;
//#define ROLLING_FRICTION
#ifdef ROLLING_FRICTION
int rollingFriction=1;
btVector3 angVelA(0,0,0),angVelB(0,0,0);
if (mbA)
angVelA = mbA->getVelocityVector()>getLink(fcA->m_link).l>getAngularVelocity();
if (mbB)
angVelB = mbB->getAngularVelocity();
btVector3 relAngVel = angVelB-angVelA;
if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))
{
//disabled: only a single rollingFriction per manifold
//rollingFriction--;
if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold)
{
relAngVel.normalize();
applyAnisotropicFriction(colObj0,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
applyAnisotropicFriction(colObj1,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
if (relAngVel.length()>0.001)
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);
applyAnisotropicFriction(colObj0,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
applyAnisotropicFriction(colObj1,axis0,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
applyAnisotropicFriction(colObj1,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
if (axis0.length()>0.001)
addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
if (axis1.length()>0.001)
addRollingFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
}
}
#endif //ROLLING_FRICTION
///Bullet has several options to set the friction directions
///By default, each contact has only a single friction direction that is recomputed automatically every frame