diff --git a/data/cube_no_friction.urdf b/data/cube_no_friction.urdf
index aef2bfa15..6df23a865 100644
--- a/data/cube_no_friction.urdf
+++ b/data/cube_no_friction.urdf
@@ -4,8 +4,8 @@
-
-
+
+
diff --git a/data/cube_small.urdf b/data/cube_small.urdf
index 23ae7fa3b..f6e271ee3 100644
--- a/data/cube_small.urdf
+++ b/data/cube_small.urdf
@@ -3,10 +3,8 @@
-
+
-
-
diff --git a/data/cube_soft.urdf b/data/cube_soft.urdf
new file mode 100644
index 000000000..af40dd379
--- /dev/null
+++ b/data/cube_soft.urdf
@@ -0,0 +1,32 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/data/multibody.bullet b/data/multibody.bullet
index 31800308c..50ed04b49 100644
Binary files a/data/multibody.bullet and b/data/multibody.bullet differ
diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
index c3aaee49b..58a78512e 100644
--- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
+++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
@@ -418,7 +418,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
{
diff --git a/examples/Importers/ImportURDFDemo/URDFJointTypes.h b/examples/Importers/ImportURDFDemo/URDFJointTypes.h
index 2a6bdfef4..08c984268 100644
--- a/examples/Importers/ImportURDFDemo/URDFJointTypes.h
+++ b/examples/Importers/ImportURDFDemo/URDFJointTypes.h
@@ -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;
}
diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp
index bb0af5f79..3fee80465 100644
--- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp
+++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp
@@ -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(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(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(rolling_xml->Attribute("value"));
+ link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_ROLLING_FRICTION;
+
}
-
- link.m_contactInfo.m_rollingFriction = urdfLexicalCast(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(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(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(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(damping_xml->Attribute("value"));
+ link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_STIFFNESS_DAMPING;
+
+ }
+ }
+ }
}
}
diff --git a/examples/MultiBody/MultiBodySoftContact.cpp b/examples/MultiBody/MultiBodySoftContact.cpp
index cc5d1ff9a..b7ce1c9f6 100644
--- a/examples/MultiBody/MultiBodySoftContact.cpp
+++ b/examples/MultiBody/MultiBodySoftContact.cpp
@@ -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);
}
diff --git a/examples/RigidBody/RigidBodySoftContact.cpp b/examples/RigidBody/RigidBodySoftContact.cpp
index 1ea1bb002..1e7cd2c54 100644
--- a/examples/RigidBody/RigidBodySoftContact.cpp
+++ b/examples/RigidBody/RigidBodySoftContact.cpp
@@ -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);
+
}
diff --git a/examples/RoboticsLearning/GripperGraspExample.cpp b/examples/RoboticsLearning/GripperGraspExample.cpp
index 434454985..9a7ab8e7b 100644
--- a/examples/RoboticsLearning/GripperGraspExample.cpp
+++ b/examples/RoboticsLearning/GripperGraspExample.cpp
@@ -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)
diff --git a/examples/RoboticsLearning/R2D2GraspExample.cpp b/examples/RoboticsLearning/R2D2GraspExample.cpp
index e4a412d73..e0c9cadd1 100644
--- a/examples/RoboticsLearning/R2D2GraspExample.cpp
+++ b/examples/RoboticsLearning/R2D2GraspExample.cpp
@@ -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);
diff --git a/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp b/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp
index 032a5828d..e9c889b2c 100644
--- a/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp
+++ b/src/BulletCollision/CollisionDispatch/btCollisionObject.cpp
@@ -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;
diff --git a/src/BulletCollision/CollisionDispatch/btCollisionObject.h b/src/BulletCollision/CollisionDispatch/btCollisionObject.h
index a96809f91..5e08609e3 100644
--- a/src/BulletCollision/CollisionDispatch/btCollisionObject.h
+++ b/src/BulletCollision/CollisionDispatch/btCollisionObject.h
@@ -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;
diff --git a/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp b/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp
index 4b2986a00..0536a2ea2 100644
--- a/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp
+++ b/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp
@@ -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);
diff --git a/src/BulletCollision/CollisionDispatch/btManifoldResult.h b/src/BulletCollision/CollisionDispatch/btManifoldResult.h
index 977b9a02f..66d11417d 100644
--- a/src/BulletCollision/CollisionDispatch/btManifoldResult.h
+++ b/src/BulletCollision/CollisionDispatch/btManifoldResult.h
@@ -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
diff --git a/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h b/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h
index 1bb7a7b99..5ae072ea0 100644
--- a/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h
+++ b/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h
@@ -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;
diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
index 5e1677e27..57ea4e69e 100644
--- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
+++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
@@ -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);
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
index 119a24c60..01a59890c 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
@@ -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;
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
index b40c6d72b..9633acaab 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
@@ -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