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; + + + } }