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..6350fe4d7 --- /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" + +#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/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index 307340382..c203a3b36 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -445,8 +445,8 @@ void PhysicsClientExample::initPhysics() m_selectedBody = -1; m_prevSelectedBody = -1; - //m_physicsClientHandle = b3ConnectSharedMemory(m_sharedMemoryKey); - m_physicsClientHandle = b3ConnectPhysicsLoopback(SHARED_MEMORY_KEY); + m_physicsClientHandle = b3ConnectSharedMemory(m_sharedMemoryKey); + //m_physicsClientHandle = b3ConnectPhysicsLoopback(SHARED_MEMORY_KEY); //m_physicsClientHandle = b3ConnectPhysicsDirect(); if (!b3CanSubmitCommand(m_physicsClientHandle)) diff --git a/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h b/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h index e40fb1d3d..1bb7a7b99 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h +++ b/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h @@ -35,7 +35,12 @@ typedef sce::PhysicsEffects::PfxConstraintRow btConstraintRow; typedef btConstraintRow PfxConstraintRow; #endif //PFX_USE_FREE_VECTORMATH - +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. /// used to improve stability and performance of rigidbody dynamics response. @@ -44,14 +49,15 @@ class btManifoldPoint public: btManifoldPoint() :m_userPersistentData(0), - m_lateralFrictionInitialized(false), - m_appliedImpulse(0.f), + m_contactPointFlags(0), + m_appliedImpulse(0.f), m_appliedImpulseLateral1(0.f), m_appliedImpulseLateral2(0.f), m_contactMotion1(0.f), m_contactMotion2(0.f), - m_contactCFM1(0.f), - m_contactCFM2(0.f), + m_contactCFM(0.f), + m_contactERP(0.f), + m_frictionCFM(0.f), m_lifeTime(0) { } @@ -67,14 +73,15 @@ class btManifoldPoint m_combinedRollingFriction(btScalar(0.)), m_combinedRestitution(btScalar(0.)), m_userPersistentData(0), - m_lateralFrictionInitialized(false), - m_appliedImpulse(0.f), + m_contactPointFlags(0), + m_appliedImpulse(0.f), m_appliedImpulseLateral1(0.f), m_appliedImpulseLateral2(0.f), m_contactMotion1(0.f), m_contactMotion2(0.f), - m_contactCFM1(0.f), - m_contactCFM2(0.f), + m_contactCFM(0.f), + m_contactERP(0.f), + m_frictionCFM(0.f), m_lifeTime(0) { @@ -101,15 +108,18 @@ class btManifoldPoint int m_index1; mutable void* m_userPersistentData; - bool m_lateralFrictionInitialized; - + //bool m_lateralFrictionInitialized; + int m_contactPointFlags; + btScalar m_appliedImpulse; btScalar m_appliedImpulseLateral1; btScalar m_appliedImpulseLateral2; btScalar m_contactMotion1; btScalar m_contactMotion2; - btScalar m_contactCFM1; - btScalar m_contactCFM2; + btScalar m_contactCFM; + btScalar m_contactERP; + + btScalar m_frictionCFM; int m_lifeTime;//lifetime of the contactpoint in frames diff --git a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h index 5026397f6..d220f2993 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h +++ b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h @@ -163,7 +163,7 @@ public: //get rid of duplicated userPersistentData pointer m_pointCache[lastUsedIndex].m_userPersistentData = 0; m_pointCache[lastUsedIndex].m_appliedImpulse = 0.f; - m_pointCache[lastUsedIndex].m_lateralFrictionInitialized = false; + m_pointCache[lastUsedIndex].m_contactPointFlags = 0; m_pointCache[lastUsedIndex].m_appliedImpulseLateral1 = 0.f; m_pointCache[lastUsedIndex].m_appliedImpulseLateral2 = 0.f; m_pointCache[lastUsedIndex].m_lifeTime = 0; @@ -190,7 +190,6 @@ public: void* cache = m_pointCache[insertIndex].m_userPersistentData; m_pointCache[insertIndex] = newPoint; - m_pointCache[insertIndex].m_userPersistentData = cache; m_pointCache[insertIndex].m_appliedImpulse = appliedImpulse; m_pointCache[insertIndex].m_appliedImpulseLateral1 = appliedLateralImpulse1; diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index 8da572bf7..fe45af42a 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -775,7 +775,12 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra //rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); //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; btVector3 torqueAxis0 = rel_pos1.cross(cp.m_normalWorldOnB); solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); @@ -802,7 +807,7 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra } #endif //COMPUTE_IMPULSE_DENOM - btScalar denom = relaxation/(denom0+denom1); + btScalar denom = relaxation/(denom0+denom1+cfm); solverConstraint.m_jacDiagABInv = denom; } @@ -884,20 +889,16 @@ 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; - velocityError -= penetration / infoGlobal.m_timeStep; + velocityError -= penetration *invTimeStep; } else { - positionalError = -penetration * erp/infoGlobal.m_timeStep; + positionalError = -penetration * erp*invTimeStep; + } btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv; @@ -915,7 +916,7 @@ void btSequentialImpulseConstraintSolver::setupContactConstraint(btSolverConstra solverConstraint.m_rhs = velocityImpulse; solverConstraint.m_rhsPenetration = penetrationImpulse; } - solverConstraint.m_cfm = 0.f; + solverConstraint.m_cfm = cfm*solverConstraint.m_jacDiagABInv; solverConstraint.m_lowerLimit = 0; solverConstraint.m_upperLimit = 1e10f; } @@ -1094,7 +1095,7 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m ///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2) ///this will give a conveyor belt effect /// - if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized) + if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !(cp.m_contactPointFlags&BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED)) { cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2(); @@ -1132,16 +1133,16 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION)) { - cp.m_lateralFrictionInitialized = true; + cp.m_contactPointFlags|=BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED; } } } else { - addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1); + addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_frictionCFM); if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2); + addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_frictionCFM); } setFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index 8a034b38a..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; + + + } } @@ -685,7 +688,7 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* ///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2) ///this will give a conveyor belt effect /// - if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized) + if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !(cp.m_contactPointFlags&BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED)) {/* cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel; btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2(); @@ -724,16 +727,16 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) && (infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION)) { - cp.m_lateralFrictionInitialized = true; + cp.m_contactPointFlags|=BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED; } } } else { - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal,cp.m_contactMotion1, cp.m_contactCFM1); + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal,cp.m_contactMotion1, cp.m_frictionCFM); if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation, infoGlobal,cp.m_contactMotion2, cp.m_contactCFM2); + addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation, infoGlobal,cp.m_contactMotion2, cp.m_frictionCFM); //setMultiBodyFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal); //todo: diff --git a/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp b/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp index 6688694a9..e73f4acc4 100644 --- a/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp +++ b/src/BulletDynamics/MLCPSolvers/btMLCPSolver.cpp @@ -22,8 +22,7 @@ subject to the following restrictions: btMLCPSolver::btMLCPSolver( btMLCPSolverInterface* solver) :m_solver(solver), -m_fallback(0), -m_cfm(0.000001)//0.0000001 +m_fallback(0) { } @@ -436,7 +435,7 @@ void btMLCPSolver::createMLCPFast(const btContactSolverInfo& infoGlobal) // add cfm to the diagonal of m_A for ( int i=0; i m_allConstraintPtrArray; btMLCPSolverInterface* m_solver; int m_fallback; - btScalar m_cfm; virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer); @@ -73,15 +72,6 @@ public: m_fallback = num; } - btScalar getCfm() const - { - return m_cfm; - } - void setCfm(btScalar cfm) - { - m_cfm = cfm; - } - virtual btConstraintSolverType getSolverType() const { return BT_MLCP_SOLVER;