diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 7f144eeac..b0f66c6db 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -759,6 +759,7 @@ b3SharedMemoryCommandHandle b3CreateJoint(b3PhysicsClientHandle physClient, int for (int i = 0; i < 3; ++i) { command->m_createJointArguments.m_jointAxis[i] = info->m_jointAxis[i]; } + command->m_createJointArguments.m_jointType = info->m_jointType; return (b3SharedMemoryCommandHandle)command; } diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 9d7dcf5e5..3b62bc52f 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -12,6 +12,7 @@ #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" #include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h" #include "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h" +#include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h" #include "LinearMath/btHashMap.h" #include "BulletInverseDynamics/MultiBodyTree.hpp" @@ -2268,18 +2269,38 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm btVector3 pivotInChild(clientCmd.m_createJointArguments.m_childFrame[0], clientCmd.m_createJointArguments.m_childFrame[1], clientCmd.m_createJointArguments.m_childFrame[2]); btMatrix3x3 frameInParent(btQuaternion(clientCmd.m_createJointArguments.m_parentFrame[3], clientCmd.m_createJointArguments.m_parentFrame[4], clientCmd.m_createJointArguments.m_parentFrame[5], clientCmd.m_createJointArguments.m_parentFrame[6])); btMatrix3x3 frameInChild(btQuaternion(clientCmd.m_createJointArguments.m_childFrame[3], clientCmd.m_createJointArguments.m_childFrame[4], clientCmd.m_createJointArguments.m_childFrame[5], clientCmd.m_createJointArguments.m_childFrame[6])); - if (childBody->m_multiBody) + btVector3 jointAxis(clientCmd.m_createJointArguments.m_jointAxis[0], clientCmd.m_createJointArguments.m_jointAxis[1], clientCmd.m_createJointArguments.m_jointAxis[2]); + if (clientCmd.m_createJointArguments.m_jointType == btMultibodyLink::eFixed) { - btMultiBodyFixedConstraint* multibodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_createJointArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild); - multibodyFixed->setMaxAppliedImpulse(2.0); - m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodyFixed); + if (childBody->m_multiBody) + { + btMultiBodyFixedConstraint* multibodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_createJointArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild); + multibodyFixed->setMaxAppliedImpulse(2.0); + m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodyFixed); + } + else + { + btMultiBodyFixedConstraint* rigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_rigidBody,pivotInParent,pivotInChild,frameInParent,frameInChild); + rigidbodyFixed->setMaxAppliedImpulse(2.0); + btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; + world->addMultiBodyConstraint(rigidbodyFixed); + } } - else + else if (clientCmd.m_createJointArguments.m_jointType == btMultibodyLink::ePrismatic) { - btMultiBodyFixedConstraint* rigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_rigidBody,pivotInParent,pivotInChild,frameInParent,frameInChild); - rigidbodyFixed->setMaxAppliedImpulse(2.0); - btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; - world->addMultiBodyConstraint(rigidbodyFixed); + if (childBody->m_multiBody) + { + btMultiBodySliderConstraint* multibodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_createJointArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis); + multibodySlider->setMaxAppliedImpulse(2.0); + m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodySlider); + } + else + { + btMultiBodySliderConstraint* rigidbodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_rigidBody,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis); + rigidbodySlider->setMaxAppliedImpulse(2.0); + btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; + world->addMultiBodyConstraint(rigidbodySlider); + } } } } diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 130f786cf..ccbfb36fe 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -375,6 +375,7 @@ struct CreateJointArgs double m_parentFrame[7]; double m_childFrame[7]; double m_jointAxis[3]; + int m_jointType; }; struct SharedMemoryCommand diff --git a/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp index b307385bd..e29ae0691 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp @@ -21,7 +21,7 @@ subject to the following restrictions: #include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h" #include "LinearMath/btIDebugDraw.h" -#define BTMBSLIDERCONSTRAINT_DIM 6 +#define BTMBSLIDERCONSTRAINT_DIM 5 #define EPSILON 0.000001 btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis)