From 6c9bfce975c5bd4b6d332bc8f76cf0b80e3ad2ef Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Mon, 22 Feb 2016 18:40:00 -0800 Subject: [PATCH] Support btMultiBody soft contact using ERP and CFM. Also support custom relaxation parameter to allow successive over relaxation. Added demos for rigid and multi body soft (compliant) contact. Will also add simplified Hertz compliant contact, by dynamically modifying the ERP/CFM to mimic a non-linear spring. Note that btManifoldPoint is growing too big, we need to implement proper contact constraints derived from btTypedConstraint. --- examples/ExampleBrowser/CMakeLists.txt | 2 + examples/ExampleBrowser/ExampleEntries.cpp | 9 +- examples/ExampleBrowser/premake4.lua | 2 + examples/MultiBody/MultiBodySoftContact.cpp | 182 ++++++++++++++++ examples/MultiBody/MultiBodySoftContact.h | 7 + examples/RigidBody/RigidBodySoftContact.cpp | 194 ++++++++++++++++++ examples/RigidBody/RigidBodySoftContact.h | 22 ++ .../NarrowPhaseCollision/btManifoldPoint.h | 5 + .../btSequentialImpulseConstraintSolver.cpp | 8 +- .../btMultiBodyConstraintSolver.cpp | 23 ++- 10 files changed, 435 insertions(+), 19 deletions(-) create mode 100644 examples/MultiBody/MultiBodySoftContact.cpp create mode 100644 examples/MultiBody/MultiBodySoftContact.h create mode 100644 examples/RigidBody/RigidBodySoftContact.cpp create mode 100644 examples/RigidBody/RigidBodySoftContact.h diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index 92c4ae0a6..010e27466 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -137,6 +137,7 @@ SET(App_ExampleBrowser_SRCS ../Vehicles/Hinge2Vehicle.cpp ../Vehicles/Hinge2Vehicle.h ../MultiBody/Pendulum.cpp + ../MultiBody/MultiBodySoftContact.cpp ../MultiBody/TestJointTorqueSetup.cpp ../MultiBody/TestJointTorqueSetup.h ../MultiBody/InvertedPendulumPDControl.cpp @@ -144,6 +145,7 @@ SET(App_ExampleBrowser_SRCS ../MultiBody/MultiBodyConstraintFeedback.cpp ../MultiBody/MultiDofDemo.cpp ../MultiBody/MultiDofDemo.h + ../RigidBody/RigidBodySoftContact.cpp ../Constraints/TestHingeTorque.cpp ../Constraints/TestHingeTorque.h ../Constraints/ConstraintDemo.cpp diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index b25299310..6a2a10924 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -22,10 +22,11 @@ #include "../Constraints/ConstraintPhysicsSetup.h" #include "../MultiBody/TestJointTorqueSetup.h" #include "../MultiBody/Pendulum.h" - +#include "../MultiBody/MultiBodySoftContact.h" #include "../MultiBody/MultiBodyConstraintFeedback.h" #include "../MultiBody/MultiDofDemo.h" #include "../MultiBody/InvertedPendulumPDControl.h" +#include "../RigidBody/RigidBodySoftContact.h" #include "../VoronoiFracture/VoronoiFractureDemo.h" #include "../SoftDemo/SoftDemo.h" #include "../Constraints/ConstraintDemo.h" @@ -103,6 +104,7 @@ static ExampleEntry gDefaultExamples[]= ExampleEntry(1,"Gyroscopic", "Show the Dzhanibekov effect using various settings of the gyroscopic term. You can select the gyroscopic term computation using btRigidBody::setFlags, with arguments BT_ENABLE_GYROSCOPIC_FORCE_EXPLICIT (using explicit integration, which adds energy and can lead to explosions), BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_WORLD, BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY. If you don't set any of these flags, there is no gyroscopic term used.", GyroscopicCreateFunc), + ExampleEntry(1,"Soft Contact", "Using the error correction parameter (ERP) and constraint force mixing (CFM) values for contacts to simulate compliant contact.",RigidBodySoftContactCreateFunc), @@ -113,12 +115,13 @@ static ExampleEntry gDefaultExamples[]= ExampleEntry(1,"Constraint Feedback", "The example shows how to receive joint reaction forces in a btMultiBody. Also the applied impulse is available for a btMultiBodyJointMotor", MultiBodyConstraintFeedbackCreateFunc), ExampleEntry(1,"Inverted Pendulum PD","Keep an inverted pendulum up using open loop PD control", InvertedPendulumPDControlCreateFunc), + ExampleEntry(1,"MultiBody Soft Contact", "Using the error correction parameter (ERP) and constraint force mixing (CFM) values for contacts to simulate compliant contact.",MultiBodySoftContactCreateFunc,0), + ExampleEntry(0,"Inverse Dynamics"), ExampleEntry(1,"Inverse Dynamics URDF", "Create a btMultiBody from URDF. Create an inverse MultiBodyTree model from that. Use either decoupled PD control or computed torque control using the inverse model to track joint position targets", InverseDynamicsExampleCreateFunc,BT_ID_LOAD_URDF), ExampleEntry(1,"Inverse Dynamics Prog", "Create a btMultiBody programatically. Create an inverse MultiBodyTree model from that. Use either decoupled PD control or computed torque control using the inverse model to track joint position targets", InverseDynamicsExampleCreateFunc,BT_ID_PROGRAMMATICALLY), - - + ExampleEntry(0,"Tutorial"), ExampleEntry(1,"Constant Velocity","Free moving rigid body, without external or constraint forces", TutorialCreateFunc,TUT_VELOCITY), diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua index 4b464e4f4..7a27a65e1 100644 --- a/examples/ExampleBrowser/premake4.lua +++ b/examples/ExampleBrowser/premake4.lua @@ -101,8 +101,10 @@ "../MultiBody/MultiDofDemo.cpp", "../MultiBody/TestJointTorqueSetup.cpp", "../MultiBody/Pendulum.cpp", + "../MultiBody/MultiBodySoftContact.cpp", "../MultiBody/MultiBodyConstraintFeedback.cpp", "../MultiBody/InvertedPendulumPDControl.cpp", + "../RigidBody/RigidBodySoftContact.cpp", "../ThirdPartyLibs/stb_image/*", "../ThirdPartyLibs/Wavefront/tiny_obj_loader.*", "../ThirdPartyLibs/tinyxml/*", diff --git a/examples/MultiBody/MultiBodySoftContact.cpp b/examples/MultiBody/MultiBodySoftContact.cpp new file mode 100644 index 000000000..89b7530ce --- /dev/null +++ b/examples/MultiBody/MultiBodySoftContact.cpp @@ -0,0 +1,182 @@ + +#include "MultiBodySoftContact.h" + +#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" +#include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h" + +#include "../CommonInterfaces/CommonMultiBodyBase.h" +#include "../Utils/b3ResourcePath.h" + +static btScalar radius(0.2); + +struct MultiBodySoftContact : public CommonMultiBodyBase +{ + btMultiBody* m_multiBody; + btAlignedObjectArray m_jointFeedbacks; + + bool m_once; +public: + + MultiBodySoftContact(struct GUIHelperInterface* helper); + virtual ~MultiBodySoftContact(); + + virtual void initPhysics(); + + virtual void stepSimulation(float deltaTime); + + virtual void resetCamera() + { + float dist = 5; + float pitch = 270; + float yaw = 21; + float targetPos[3]={0,0,0}; + m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]); + } + + +}; + +MultiBodySoftContact::MultiBodySoftContact(struct GUIHelperInterface* helper) +:CommonMultiBodyBase(helper), +m_once(true) +{ +} + +MultiBodySoftContact::~MultiBodySoftContact() +{ + +} + + +extern ContactAddedCallback gContactAddedCallback; +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; + +} + + + + +void MultiBodySoftContact::initPhysics() +{ + gContactAddedCallback = btMultiBodySoftContactCallback; + int upAxis = 2; + + m_guiHelper->setUpAxis(upAxis); + + btVector4 colors[4] = + { + btVector4(1,0,0,1), + btVector4(0,1,0,1), + btVector4(0,1,1,1), + btVector4(1,1,0,1), + }; + int curColor = 0; + + + + + this->createEmptyDynamicsWorld(); + m_dynamicsWorld->setGravity(btVector3(0,0,-10)); + + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + m_dynamicsWorld->getDebugDrawer()->setDebugMode( + //btIDebugDraw::DBG_DrawConstraints + +btIDebugDraw::DBG_DrawWireframe + +btIDebugDraw::DBG_DrawContactPoints + +btIDebugDraw::DBG_DrawAabb + );//+btIDebugDraw::DBG_DrawConstraintLimits); + + + + //create a static ground object + if (1) + { + btVector3 groundHalfExtents(50,50,50); + btBoxShape* box = new btBoxShape(groundHalfExtents); + box->initializePolyhedralFeatures(); + + m_guiHelper->createCollisionShapeGraphicsObject(box); + btTransform start; start.setIdentity(); + btVector3 groundOrigin(0,0,-50.5); + start.setOrigin(groundOrigin); + // start.setRotation(groundOrn); + btRigidBody* body = createRigidBody(0,start,box); + btVector4 color = colors[curColor]; + curColor++; + curColor&=3; + m_guiHelper->createRigidBodyGraphicsObject(body,color); + int flags = body->getCollisionFlags(); + body->setCollisionFlags(flags|btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK); + + } + + { + btCollisionShape* childShape = new btSphereShape(btScalar(0.5)); + m_guiHelper->createCollisionShapeGraphicsObject(childShape); + + btScalar mass = 1; + btVector3 baseInertiaDiag; + bool isFixed = (mass == 0); + childShape->calculateLocalInertia(mass,baseInertiaDiag); + btMultiBody *pMultiBody = new btMultiBody(0, 1, baseInertiaDiag, false, false); + btTransform startTrans; + startTrans.setIdentity(); + startTrans.setOrigin(btVector3(0,0,3)); + + pMultiBody->setBaseWorldTransform(startTrans); + + btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(pMultiBody, -1); + col->setCollisionShape(childShape); + pMultiBody->setBaseCollider(col); + bool isDynamic = (mass > 0 && !isFixed); + short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter); + short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter); + + m_dynamicsWorld->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);//, 2,1+2); + + + pMultiBody->finalizeMultiDof(); + + m_dynamicsWorld->addMultiBody(pMultiBody); + + btAlignedObjectArray scratch_q; + btAlignedObjectArray scratch_m; + pMultiBody->forwardKinematics(scratch_q,scratch_m); + btAlignedObjectArray world_to_local; + btAlignedObjectArray local_origin; + pMultiBody->updateCollisionObjectWorldTransforms(world_to_local,local_origin); + } + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); + + +} + +void MultiBodySoftContact::stepSimulation(float deltaTime) +{ + + if (0)//m_once) + { + m_once=false; + m_multiBody->addJointTorque(0, 10.0); + + btScalar torque = m_multiBody->getJointTorque(0); + b3Printf("t = %f,%f,%f\n",torque,torque,torque);//[0],torque[1],torque[2]); + } + + m_dynamicsWorld->stepSimulation(deltaTime); + + + +} + + +class CommonExampleInterface* MultiBodySoftContactCreateFunc(struct CommonExampleOptions& options) +{ + return new MultiBodySoftContact(options.m_guiHelper); +} diff --git a/examples/MultiBody/MultiBodySoftContact.h b/examples/MultiBody/MultiBodySoftContact.h new file mode 100644 index 000000000..e501cc9da --- /dev/null +++ b/examples/MultiBody/MultiBodySoftContact.h @@ -0,0 +1,7 @@ +#ifndef MULTI_BODY_SOFT_CONTACT_H +#define MULTI_BODY_SOFT_CONTACT_H + +class CommonExampleInterface* MultiBodySoftContactCreateFunc(struct CommonExampleOptions& options); + +#endif //MULTI_BODY_SOFT_CONTACT_H + diff --git a/examples/RigidBody/RigidBodySoftContact.cpp b/examples/RigidBody/RigidBodySoftContact.cpp new file mode 100644 index 000000000..c356ed72c --- /dev/null +++ b/examples/RigidBody/RigidBodySoftContact.cpp @@ -0,0 +1,194 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2015 Google Inc. http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + + + +#include "RigidBodySoftContact.h"f + +#include "btBulletDynamicsCommon.h" +#define ARRAY_SIZE_Y 1 +#define ARRAY_SIZE_X 1 +#define ARRAY_SIZE_Z 1 + +#include "LinearMath/btVector3.h" +#include "LinearMath/btAlignedObjectArray.h" + +#include "../CommonInterfaces/CommonRigidBodyBase.h" +#include "BulletDynamics/MLCPSolvers/btMLCPSolver.h" +#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h" + + +struct RigidBodySoftContact : public CommonRigidBodyBase +{ + RigidBodySoftContact(struct GUIHelperInterface* helper) + :CommonRigidBodyBase(helper) + { + } + virtual ~RigidBodySoftContact(){} + virtual void initPhysics(); + virtual void renderScene(); + void resetCamera() + { + float dist = 3; + float pitch = 52; + float yaw = 35; + float targetPos[3]={0,0.46,0}; + m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]); + } +}; + +extern ContactAddedCallback gContactAddedCallback; +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); + + //createEmptyDynamicsWorld(); + { + ///collision configuration contains default setup for memory, collision setup + m_collisionConfiguration = new btDefaultCollisionConfiguration(); + //m_collisionConfiguration->setConvexConvexMultipointIterations(); + + ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) + m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration); + + m_broadphase = new btDbvtBroadphase(); + + ///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded) + btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver; + //btMLCPSolver* sol = new btMLCPSolver(new btSolveProjectedGaussSeidel()); + m_solver = sol; + + m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration); + + m_dynamicsWorld->setGravity(btVector3(0, -10, 0)); + } + + m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); + + if (m_dynamicsWorld->getDebugDrawer()) + m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints); + m_dynamicsWorld->getSolverInfo().m_erp2 = 0.f; + m_dynamicsWorld->getSolverInfo().m_globalCfm = 0.f; + m_dynamicsWorld->getSolverInfo().m_numIterations = 3; + m_dynamicsWorld->getSolverInfo().m_solverMode = SOLVER_SIMD;// | SOLVER_RANDMIZE_ORDER; + m_dynamicsWorld->getSolverInfo().m_splitImpulse = false; + + ///create a few basic rigid bodies + btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.))); + + + //groundShape->initializePolyhedralFeatures(); +// btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50); + + m_collisionShapes.push_back(groundShape); + + btTransform groundTransform; + groundTransform.setIdentity(); + groundTransform.setOrigin(btVector3(0,-50,0)); + + { + 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); + } + + + { + //create a few dynamic rigidbodies + // Re-using the same collision is better for memory usage and performance + + //btBoxShape* colShape = createBoxShape(btVector3(1,1,1)); + + + btCollisionShape* childShape = new btSphereShape(btScalar(0.5)); + btCompoundShape* colShape = new btCompoundShape(); + colShape->addChildShape(btTransform::getIdentity(),childShape); + + m_collisionShapes.push_back(colShape); + + /// Create Dynamic Objects + btTransform startTransform; + startTransform.setIdentity(); + + startTransform.setRotation(btQuaternion(btVector3(1,1,1),SIMD_PI/10.)); + btScalar mass(1.f); + + //rigidbody is dynamic if and only if mass is non zero, otherwise static + bool isDynamic = (mass != 0.f); + + btVector3 localInertia(0,0,0); + if (isDynamic) + colShape->calculateLocalInertia(mass,localInertia); + + + for (int k=0;ksetAngularVelocity(btVector3(1,1,1)); + + + } + } + } + } + + m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld); +} + + +void RigidBodySoftContact::renderScene() +{ + CommonRigidBodyBase::renderScene(); + +} + + + + + + + +CommonExampleInterface* RigidBodySoftContactCreateFunc(CommonExampleOptions& options) +{ + return new RigidBodySoftContact(options.m_guiHelper); +} + + + diff --git a/examples/RigidBody/RigidBodySoftContact.h b/examples/RigidBody/RigidBodySoftContact.h new file mode 100644 index 000000000..aa195307b --- /dev/null +++ b/examples/RigidBody/RigidBodySoftContact.h @@ -0,0 +1,22 @@ +/* +Bullet Continuous Collision Detection and Physics Library +Copyright (c) 2015 Google Inc. http://bulletphysics.org + +This software is provided 'as-is', without any express or implied warranty. +In no event will the authors be held liable for any damages arising from the use of this software. +Permission is granted to anyone to use this software for any purpose, +including commercial applications, and to alter it and redistribute it freely, +subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. +2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. +3. This notice may not be removed or altered from any source distribution. +*/ + +#ifndef RIGID_BODY_SOFT_CONTACT_H +#define RIGID_BODY_SOFT_CONTACT_H + +class CommonExampleInterface* RigidBodySoftContactCreateFunc(struct CommonExampleOptions& options); + + +#endif //RIGID_BODY_SOFT_CONTACT_H diff --git a/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h b/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h index 572e899c3..1bb7a7b99 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h +++ b/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h @@ -39,6 +39,7 @@ enum btContactPointFlags { BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED=1, BT_CONTACT_FLAG_HAS_CONTACT_CFM=2, + BT_CONTACT_FLAG_HAS_CONTACT_ERP=4, }; /// ManifoldContactPoint collects and maintains persistent contactpoints. @@ -55,6 +56,7 @@ class btManifoldPoint m_contactMotion1(0.f), m_contactMotion2(0.f), m_contactCFM(0.f), + m_contactERP(0.f), m_frictionCFM(0.f), m_lifeTime(0) { @@ -78,6 +80,7 @@ class btManifoldPoint m_contactMotion1(0.f), m_contactMotion2(0.f), m_contactCFM(0.f), + m_contactERP(0.f), m_frictionCFM(0.f), m_lifeTime(0) { @@ -114,6 +117,8 @@ class btManifoldPoint btScalar m_contactMotion1; btScalar m_contactMotion2; btScalar m_contactCFM; + btScalar m_contactERP; + btScalar m_frictionCFM; int m_lifeTime;//lifetime of the contactpoint in frames diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index e01f4ea87..fe45af42a 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -780,6 +780,7 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra 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; btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB); solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); @@ -888,12 +889,7 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra btScalar velocityError = restitution - rel_vel;// * damping; - btScalar erp = infoGlobal.m_erp2; - if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) - { - erp = infoGlobal.m_erp; - } - + if (penetration>0) { positionalError = 0; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index db20a06b2..a4a696166 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -221,9 +221,15 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol if (bodyB) 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; + + btScalar erp = (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP)?cp.m_contactERP:infoGlobal.m_erp2; + + if (multiBodyA) @@ -366,7 +372,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol - btScalar d = denom0+denom1; + btScalar d = denom0+denom1+cfm; if (d>SIMD_EPSILON) { solverConstraint.m_jacDiagABInv = relaxation/(d); @@ -477,12 +483,6 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol btScalar positionalError = 0.f; btScalar velocityError = restitution - rel_vel;// * damping; //note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction - btScalar erp = infoGlobal.m_erp2; - if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold)) - { - erp = infoGlobal.m_erp; - } - if (penetration>0) { positionalError = 0; @@ -522,7 +522,10 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol solverConstraint.m_upperLimit = solverConstraint.m_friction; } - solverConstraint.m_cfm = 0.f; //why not use cfmSlip? + solverConstraint.m_cfm = cfm*solverConstraint.m_jacDiagABInv; + + + } }