From a7aed37632ad7778d6ba587e673a67e0e789cb71 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sun, 4 Jun 2017 22:04:16 -0700 Subject: [PATCH] work on pybullet/C-API createMultiBody (still preliminary, only sphere/box collision shapes, no links/hierarchies yet, soon) pybullet/C-API, expose linear/angular damping fix some warnings (param name needs to be same in .h and .cpp) fix potential startup threading issue (args were deleted in main thread while still possibly use in child thread) fix for spinning/rolling friction in case of mixing maximal and reduced coordinate btMultiBody+btRigidBody --- examples/ExampleBrowser/OpenGLGuiHelper.h | 2 +- examples/SharedMemory/PhysicsClientC_API.cpp | 36 +++++++++ examples/SharedMemory/PhysicsClientC_API.h | 8 +- .../PhysicsServerCommandProcessor.cpp | 29 ++++++- examples/SharedMemory/SharedMemoryCommands.h | 5 ++ .../SharedMemoryInProcessPhysicsC_API.cpp | 16 ++-- .../TinyRendererVisualShapeConverter.h | 2 +- .../examples/createSphereMultiBodies.py | 24 +++--- examples/pybullet/pybullet.c | 38 +++++++-- .../Dynamics/btSimulationIslandManagerMt.h | 2 +- .../Featherstone/btMultiBody.cpp | 10 +-- .../btMultiBodyConstraintSolver.cpp | 79 +++++++++++-------- 12 files changed, 183 insertions(+), 68 deletions(-) diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.h b/examples/ExampleBrowser/OpenGLGuiHelper.h index bdb885a2c..ccc935dcb 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.h +++ b/examples/ExampleBrowser/OpenGLGuiHelper.h @@ -48,7 +48,7 @@ struct OpenGLGuiHelper : public GUIHelperInterface virtual void resetCamera(float camDist, float yaw, float pitch, float camPosX,float camPosY, float camPosZ); - virtual bool getCameraInfo(int* width, int* height, float viewMatrix[16], float projectionMatrix[16], float camUp[3], float camForward[3],float hor[3], float vert[3], float* yaw, float* pitch, float* camDist, float camTarget[3]) const; + virtual bool getCameraInfo(int* width, int* height, float viewMatrix[16], float projectionMatrix[16], float camUp[3], float camForward[3],float hor[3], float vert[3], float* yaw, float* pitch, float* camDist, float cameraTarget[3]) const; virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16], unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index c8b29a115..2ec671d6d 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -877,6 +877,17 @@ int b3CreateMultiBodyBase(b3SharedMemoryCommandHandle commandHandle, double mass return -2; } +//useMaximalCoordinates are disabled by default, enabling them is experimental and not fully supported yet +void b3CreateMultiBodyUseMaximalCoordinates(b3SharedMemoryCommandHandle commandHandle) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_CREATE_MULTI_BODY); + if (command->m_type==CMD_CREATE_MULTI_BODY) + { + command->m_updateFlags |= MULT_BODY_USE_MAXIMAL_COORDINATES; + } +} int b3GetStatusMultiBodyUniqueId(b3SharedMemoryStatusHandle statusHandle) { @@ -1269,6 +1280,11 @@ int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle) bodyId = status->m_rigidBodyCreateArgs.m_bodyUniqueId; break; } + case CMD_CREATE_MULTI_BODY_COMPLETED: + { + bodyId = status->m_dataStreamArguments.m_bodyUniqueId; + break; + } default: { b3Assert(0); @@ -1541,9 +1557,29 @@ int b3ChangeDynamicsInfoSetRestitution(b3SharedMemoryCommandHandle commandHandle command->m_changeDynamicsInfoArgs.m_restitution = restitution; command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_RESTITUTION; return 0; +} +int b3ChangeDynamicsInfoSetLinearDamping(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId,double linearDamping) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO); + command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId; + command->m_changeDynamicsInfoArgs.m_linearDamping = linearDamping; + command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING; + return 0; } + +int b3ChangeDynamicsInfoSetAngularDamping(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId,double angularDamping) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO); + command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId; + command->m_changeDynamicsInfoArgs.m_linearDamping = angularDamping; + command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING; + return 0; +} + b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info) { PhysicsClient* cl = (PhysicsClient* ) physClient; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 62d27faff..441395228 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -87,7 +87,8 @@ int b3ChangeDynamicsInfoSetLateralFriction(b3SharedMemoryCommandHandle commandHa int b3ChangeDynamicsInfoSetSpinningFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction); int b3ChangeDynamicsInfoSetRollingFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction); int b3ChangeDynamicsInfoSetRestitution(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double restitution); - +int b3ChangeDynamicsInfoSetLinearDamping(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId,double linearDamping); +int b3ChangeDynamicsInfoSetAngularDamping(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId,double angularDamping); b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info); @@ -329,9 +330,10 @@ int b3GetStatusVisualShapeUniqueId(b3SharedMemoryStatusHandle statusHandle); b3SharedMemoryCommandHandle b3CreateMultiBodyCommandInit(b3PhysicsClientHandle physClient); int b3CreateMultiBodyBase(b3SharedMemoryCommandHandle commandHandle, double mass, int collisionShapeUnique, int visualShapeUniqueId, double basePosition[3], double baseOrientation[4]); -//int b3CreateMultiBodyAddLink(b3SharedMemoryCommandHandle commandHandle, int jointType, int parentLinkIndex, double linkMass, int linkCollisionShapeUnique, int linkVisualShapeUniqueId); +//useMaximalCoordinates are disabled by default, enabling them is experimental and not fully supported yet +void b3CreateMultiBodyUseMaximalCoordinates(b3SharedMemoryCommandHandle commandHandle); -int b3GetStatusMultiBodyUniqueId(b3SharedMemoryStatusHandle statusHandle); +//int b3CreateMultiBodyAddLink(b3SharedMemoryCommandHandle commandHandle, int jointType, int parentLinkIndex, double linkMass, int linkCollisionShapeUnique, int linkVisualShapeUniqueId); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 9dc8a1b9e..40385d85c 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -3530,9 +3530,16 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm serverStatusOut.m_type = CMD_CREATE_MULTI_BODY_FAILED; if (clientCmd.m_createMultiBodyArgs.m_baseLinkIndex>=0) { + m_data->m_sdfRecentLoadedBodies.clear(); + ProgrammaticUrdfInterface u2b(clientCmd.m_createMultiBodyArgs, m_data); - bool useMultiBody = false; + bool useMultiBody = true; + if (clientCmd.m_updateFlags & MULT_BODY_USE_MAXIMAL_COORDINATES) + { + useMultiBody = false; + } + int flags = 0; bool ok = processImportedObjects("memory", bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, u2b); @@ -4329,6 +4336,15 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { mb->getBaseCollider()->setRestitution(restitution); } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING) + { + mb->setLinearDamping(clientCmd.m_changeDynamicsInfoArgs.m_linearDamping); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING) + { + mb->setLinearDamping(clientCmd.m_changeDynamicsInfoArgs.m_angularDamping); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION) { mb->getBaseCollider()->setFriction(lateralFriction); @@ -4395,6 +4411,17 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { if (body && body->m_rigidBody) { + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING) + { + btScalar angDamping = body->m_rigidBody->getAngularDamping(); + body->m_rigidBody->setDamping(clientCmd.m_changeDynamicsInfoArgs.m_linearDamping,angDamping); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING) + { + btScalar linDamping = body->m_rigidBody->getLinearDamping(); + body->m_rigidBody->setDamping(linDamping, clientCmd.m_changeDynamicsInfoArgs.m_angularDamping); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION) { body->m_rigidBody->setRestitution(restitution); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 0d89aa9bd..bde429122 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -117,6 +117,8 @@ enum EnumChangeDynamicsInfoFlags CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION=8, CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION=16, CHANGE_DYNAMICS_INFO_SET_RESTITUTION=32, + CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING=64, + CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING=128, }; struct ChangeDynamicsInfoArgs @@ -129,6 +131,8 @@ struct ChangeDynamicsInfoArgs double m_spinningFriction; double m_rollingFriction; double m_restitution; + double m_linearDamping; + double m_angularDamping; }; struct GetDynamicsInfoArgs @@ -806,6 +810,7 @@ struct b3CreateVisualShapeArgs enum eCreateMultiBodyEnum { MULTI_BODY_HAS_BASE=1, + MULT_BODY_USE_MAXIMAL_COORDINATES=2, }; struct b3CreateMultiBodyArgs { diff --git a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp index 633441c95..8b5200f6b 100644 --- a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp +++ b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp @@ -93,22 +93,23 @@ b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThread(int arg class InProcessPhysicsClientSharedMemory : public PhysicsClientSharedMemory { btInProcessExampleBrowserInternalData* m_data; -public: + char** m_newargv; +public: + InProcessPhysicsClientSharedMemory(int argc, char* argv[]) { int newargc = argc+2; - char** newargv = (char**)malloc(sizeof(void*)*newargc); + m_newargv = (char**)malloc(sizeof(void*)*newargc); for (int i=0;i=0) + { + b3ChangeDynamicsInfoSetLinearDamping(command,bodyUniqueId, linearDamping); + } + if (angularDamping>=0) + { + b3ChangeDynamicsInfoSetAngularDamping(command,bodyUniqueId,angularDamping); + } + + if (restitution>=0) { b3ChangeDynamicsInfoSetRestitution(command, bodyUniqueId, linkIndex, restitution); @@ -971,6 +983,7 @@ static PyObject* pybullet_loadSDF(PyObject* self, PyObject* args, PyObject* keyw int numBodies = 0; int i; int bodyIndicesOut[MAX_SDF_BODIES]; + int useMaximalCoordinates = -1; PyObject* pylist = 0; b3SharedMemoryStatusHandle statusHandle; int statusType; @@ -978,8 +991,8 @@ static PyObject* pybullet_loadSDF(PyObject* self, PyObject* args, PyObject* keyw b3PhysicsClientHandle sm = 0; int physicsClientId = 0; - static char* kwlist[] = {"sdfFileName", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|i", kwlist, &sdfFileName, &physicsClientId)) + static char* kwlist[] = {"sdfFileName", "useMaximalCoordinates", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|ii", kwlist, &sdfFileName, &useMaximalCoordinates, &physicsClientId)) { return NULL; } @@ -991,6 +1004,11 @@ static PyObject* pybullet_loadSDF(PyObject* self, PyObject* args, PyObject* keyw } commandHandle = b3LoadSdfCommandInit(sm, sdfFileName); + if (useMaximalCoordinates>0) + { + b3LoadSdfCommandSetUseMultiBody(commandHandle,0); + } + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); if (statusType != CMD_SDF_LOADING_COMPLETED) @@ -4805,15 +4823,17 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje double baseMass = 0; int baseCollisionShapeIndex=-1; int baseVisualShapeIndex=-1; + int useMaximalCoordinates = 0; PyObject* basePosObj=0; PyObject* baseOrnObj=0; static char* kwlist[] = {"baseMass","baseCollisionShapeIndex","baseVisualShapeIndex","basePosition", "baseOrientation", // "linkParentIndices", "linkJointTypes","linkMasses","linkCollisionShapeIndices", +"useMaximalCoordinates","physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diiOOi", kwlist, &baseMass,&baseCollisionShapeIndex,&baseVisualShapeIndex, - &basePosObj, &baseOrnObj,&physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diiOOii", kwlist, &baseMass,&baseCollisionShapeIndex,&baseVisualShapeIndex, + &basePosObj, &baseOrnObj,&useMaximalCoordinates, &physicsClientId)) { return NULL; } @@ -4833,11 +4853,15 @@ static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObje pybullet_internalSetVectord(basePosObj,basePosition); pybullet_internalSetVector4d(baseOrnObj,baseOrientation); int baseIndex = b3CreateMultiBodyBase(commandHandle,baseMass,baseCollisionShapeIndex,baseVisualShapeIndex,basePosition,baseOrientation); + if (useMaximalCoordinates>0) + { + b3CreateMultiBodyUseMaximalCoordinates(commandHandle,useMaximalCoordinates); + } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); if (statusType == CMD_CREATE_MULTI_BODY_COMPLETED) { - int uid = b3GetStatusMultiBodyUniqueId(statusHandle); + int uid = b3GetStatusBodyIndex(statusHandle); PyObject* ob = PyLong_FromLong(uid); return ob; } diff --git a/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h b/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h index e3e0ea523..9a781aaef 100644 --- a/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h +++ b/src/BulletDynamics/Dynamics/btSimulationIslandManagerMt.h @@ -59,7 +59,7 @@ public: ) = 0; }; typedef void( *IslandDispatchFunc ) ( btAlignedObjectArray* islands, IslandCallback* callback ); - static void serialIslandDispatch( btAlignedObjectArray* islands, IslandCallback* callback ); + static void serialIslandDispatch( btAlignedObjectArray* islandsPtr, IslandCallback* callback ); static void parallelIslandDispatch( btAlignedObjectArray* islandsPtr, IslandCallback* callback ); protected: btAlignedObjectArray m_allocatedIslands; // owner of all Islands diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index ca9652f1e..c4c3fdea7 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -2013,10 +2013,10 @@ const char* btMultiBody::serialize(void* dataBuffer, class btSerializer* seriali } mbd->m_links = mbd->m_numLinks? (btMultiBodyLinkData*) serializer->getUniquePointer((void*)&m_links[0]):0; - // Fill padding with zeros to appease msan. -#ifdef BT_USE_DOUBLE_PRECISION - memset(mbd->m_padding, 0, sizeof(mbd->m_padding)); -#endif - + // Fill padding with zeros to appease msan. +#ifdef BT_USE_DOUBLE_PRECISION + memset(mbd->m_padding, 0, sizeof(mbd->m_padding)); +#endif + return btMultiBodyDataName; } diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index 2aa62e5e5..1e2d07409 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -665,12 +665,12 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex]; multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v); - btVector3 torqueAxis0 = constraintNormal; + btVector3 torqueAxis0 = -constraintNormal; solverConstraint.m_relpos1CrossNormal = torqueAxis0; solverConstraint.m_contactNormal1 = btVector3(0,0,0); } else { - btVector3 torqueAxis0 = constraintNormal; + btVector3 torqueAxis0 = -constraintNormal; solverConstraint.m_relpos1CrossNormal = torqueAxis0; solverConstraint.m_contactNormal1 = btVector3(0,0,0); solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0); @@ -708,21 +708,20 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v); btVector3 torqueAxis1 = constraintNormal; - solverConstraint.m_relpos2CrossNormal = -torqueAxis1; + solverConstraint.m_relpos2CrossNormal = torqueAxis1; solverConstraint.m_contactNormal2 = -btVector3(0,0,0); } else { btVector3 torqueAxis1 = constraintNormal; - solverConstraint.m_relpos2CrossNormal = -torqueAxis1; + solverConstraint.m_relpos2CrossNormal = torqueAxis1; solverConstraint.m_contactNormal2 = -btVector3(0,0,0); - solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); + solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0); } { - btVector3 vec; btScalar denom0 = 0.f; btScalar denom1 = 0.f; btScalar* jacB = 0; @@ -745,8 +744,8 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu { if (rb0) { - vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1); - denom0 = rb0->getInvMass() + constraintNormal.dot(vec); + btVector3 iMJaA = rb0?rb0->getInvInertiaTensorWorld()*solverConstraint.m_relpos1CrossNormal:btVector3(0,0,0); + denom0 = iMJaA.dot(solverConstraint.m_relpos1CrossNormal); } } if (multiBodyB) @@ -765,8 +764,8 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu { if (rb1) { - vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2); - denom1 = rb1->getInvMass() + constraintNormal.dot(vec); + btVector3 iMJaB = rb1?rb1->getInvInertiaTensorWorld()*solverConstraint.m_relpos2CrossNormal:btVector3(0,0,0); + denom1 = iMJaB.dot(solverConstraint.m_relpos2CrossNormal); } } @@ -808,7 +807,10 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu { if (rb0) { - rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1); + btSolverBody* solverBodyA = &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdA]; + rel_vel += solverConstraint.m_contactNormal1.dot(rb0?solverBodyA->m_linearVelocity+solverBodyA->m_externalForceImpulse:btVector3(0,0,0)) + + solverConstraint.m_relpos1CrossNormal.dot(rb0?solverBodyA->m_angularVelocity:btVector3(0,0,0)); + } } if (multiBodyB) @@ -822,7 +824,10 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu { if (rb1) { - rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2); + btSolverBody* solverBodyB = &m_tmpSolverBodyPool[solverConstraint.m_solverBodyIdB]; + rel_vel += solverConstraint.m_contactNormal2.dot(rb1?solverBodyB->m_linearVelocity+solverBodyB->m_externalForceImpulse:btVector3(0,0,0)) + + solverConstraint.m_relpos2CrossNormal.dot(rb1?solverBodyB->m_angularVelocity:btVector3(0,0,0)); + } } @@ -844,12 +849,9 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu { - 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 velocityError = 0 - rel_vel;// * damping; //note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction - if (penetration>0) - { - velocityError -= penetration / infoGlobal.m_timeStep; - } + btScalar velocityImpulse = velocityError*solverConstraint.m_jacDiagABInv; @@ -1021,6 +1023,33 @@ 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 /// + + btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2); + cp.m_lateralFrictionDir1.normalize(); + cp.m_lateralFrictionDir2.normalize(); + + if (rollingFriction > 0 ) + { + if (cp.m_combinedSpinningFriction>0) + { + addMultiBodyTorsionalFrictionConstraint(cp.m_normalWorldOnB,manifold,frictionIndex,cp,cp.m_combinedSpinningFriction, colObj0,colObj1, relaxation,infoGlobal); + } + if (cp.m_combinedRollingFriction>0) + { + + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); + + if (cp.m_lateralFrictionDir1.length()>0.001) + addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,cp.m_combinedRollingFriction, colObj0,colObj1, relaxation,infoGlobal); + + if (cp.m_lateralFrictionDir2.length()>0.001) + addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,cp.m_combinedRollingFriction, colObj0,colObj1, relaxation,infoGlobal); + } + rollingFriction--; + } 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; @@ -1045,26 +1074,12 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* } else */ { - btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2); + applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION); addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal); - if (rollingFriction > 0 ) - { - if (cp.m_combinedSpinningFriction>0) - { - addMultiBodyTorsionalFrictionConstraint(cp.m_normalWorldOnB,manifold,frictionIndex,cp,cp.m_combinedSpinningFriction, colObj0,colObj1, relaxation,infoGlobal); - } - if (cp.m_combinedRollingFriction>0) - { - - addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,cp.m_combinedRollingFriction, colObj0,colObj1, relaxation,infoGlobal); - addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,cp.m_combinedRollingFriction, colObj0,colObj1, relaxation,infoGlobal); - } - rollingFriction--; - } if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) {