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/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 72aaecfce..d243c1bc9 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -294,7 +294,6 @@ std::string BulletURDFImporter::getJointName(int linkIndex) const void BulletURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const { - //todo(erwincoumans) //the link->m_inertia is NOT necessarily aligned with the inertial frame //so an additional transform might need to be computed UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex); @@ -414,6 +413,13 @@ bool BulletURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joi } +void BulletURDFImporter::setRootTransformInWorld(const btTransform& rootTransformInWorld) +{ + m_data->m_urdfParser.getModel().m_rootTransformInWorld = rootTransformInWorld ; +} + + + bool BulletURDFImporter::getRootTransformInWorld(btTransform& rootTransformInWorld) const { rootTransformInWorld = m_data->m_urdfParser.getModel().m_rootTransformInWorld; diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h index 5ada57d25..0723712dd 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.h @@ -55,6 +55,7 @@ public: virtual bool getJointInfo2(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity) const; virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const; + virtual void setRootTransformInWorld(const btTransform& rootTransformInWorld); virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const; diff --git a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h index 588930184..d6853e3b6 100644 --- a/examples/Importers/ImportURDFDemo/URDFImporterInterface.h +++ b/examples/Importers/ImportURDFDemo/URDFImporterInterface.h @@ -64,7 +64,8 @@ public: }; virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const =0; - + virtual void setRootTransformInWorld(const btTransform& rootTransformInWorld){} + ///quick hack: need to rethink the API/dependencies of this virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const { return -1;} diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h index 1a74cbba2..817536916 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.h +++ b/examples/Importers/ImportURDFDemo/UrdfParser.h @@ -282,12 +282,11 @@ public: int getNumModels() const { //user should have loaded an SDF when calling this method - btAssert(m_parseSDF); if (m_parseSDF) { return m_sdfModels.size(); } - return 0; + return 1; } void activateModel(int modelIndex); diff --git a/examples/MultiBody/InvertedPendulumPDControl.cpp b/examples/MultiBody/InvertedPendulumPDControl.cpp index 8031e773d..d380929c4 100644 --- a/examples/MultiBody/InvertedPendulumPDControl.cpp +++ b/examples/MultiBody/InvertedPendulumPDControl.cpp @@ -244,7 +244,7 @@ btMultiBody* createInvertedPendulumMultiBody(btMultiBodyDynamicsWorld* world, GU world->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);//, 2,1+2); - btVector3 color(0.0,0.0,0.5); + btVector4 color(0.0,0.0,0.5,1); guiHelper->createCollisionObjectGraphicsObject(col,color); // col->setFriction(friction); diff --git a/examples/OpenGLWindow/SimpleOpenGL2App.cpp b/examples/OpenGLWindow/SimpleOpenGL2App.cpp index 198a8bf67..43b8058f2 100644 --- a/examples/OpenGLWindow/SimpleOpenGL2App.cpp +++ b/examples/OpenGLWindow/SimpleOpenGL2App.cpp @@ -481,7 +481,8 @@ void SimpleOpenGL2App::drawText3D( const char* txt, float worldPosX, float world void SimpleOpenGL2App::registerGrid(int cells_x, int cells_z, float color0[4], float color1[4]) { b3Vector3 cubeExtents=b3MakeVector3(0.5,0.5,0.5); - cubeExtents[m_data->m_upAxis] = 0; + double halfHeight=0.1; + cubeExtents[m_data->m_upAxis] = halfHeight; int cubeId = registerCubeShape(cubeExtents[0],cubeExtents[1],cubeExtents[2]); b3Quaternion orn(0,0,0,1); @@ -501,10 +502,10 @@ void SimpleOpenGL2App::registerGrid(int cells_x, int cells_z, float color0[4], f } if (this->m_data->m_upAxis==1) { - center =b3MakeVector3((i + 0.5f) - cells_x * 0.5f, 0.f, (j + 0.5f) - cells_z * 0.5f); + center =b3MakeVector3((i + 0.5f) - cells_x * 0.5f, -halfHeight, (j + 0.5f) - cells_z * 0.5f); } else { - center =b3MakeVector3((i + 0.5f) - cells_x * 0.5f, (j + 0.5f) - cells_z * 0.5f,0.f ); + center =b3MakeVector3((i + 0.5f) - cells_x * 0.5f, (j + 0.5f) - cells_z * 0.5f,-halfHeight ); } m_renderer->registerGraphicsInstance(cubeId,center,orn,color,scaling); } diff --git a/examples/OpenGLWindow/SimpleOpenGL3App.cpp b/examples/OpenGLWindow/SimpleOpenGL3App.cpp index e05056e81..e4475f535 100644 --- a/examples/OpenGLWindow/SimpleOpenGL3App.cpp +++ b/examples/OpenGLWindow/SimpleOpenGL3App.cpp @@ -704,9 +704,9 @@ int SimpleOpenGL3App::registerCubeShape(float halfExtentsX,float halfExtentsY, f void SimpleOpenGL3App::registerGrid(int cells_x, int cells_z, float color0[4], float color1[4]) { b3Vector3 cubeExtents=b3MakeVector3(0.5,0.5,0.5); - cubeExtents[m_data->m_upAxis] = 0; + double halfHeight=0.1; + cubeExtents[m_data->m_upAxis] = halfHeight; int cubeId = registerCubeShape(cubeExtents[0],cubeExtents[1],cubeExtents[2]); - b3Quaternion orn(0,0,0,1); b3Vector3 center=b3MakeVector3(0,0,0,1); b3Vector3 scaling=b3MakeVector3(1,1,1,1); @@ -724,10 +724,10 @@ void SimpleOpenGL3App::registerGrid(int cells_x, int cells_z, float color0[4], f } if (this->m_data->m_upAxis==1) { - center =b3MakeVector3((i + 0.5f) - cells_x * 0.5f, 0.f, (j + 0.5f) - cells_z * 0.5f); + center =b3MakeVector3((i + 0.5f) - cells_x * 0.5f, -halfHeight, (j + 0.5f) - cells_z * 0.5f); } else { - center =b3MakeVector3((i + 0.5f) - cells_x * 0.5f, (j + 0.5f) - cells_z * 0.5f,0.f ); + center =b3MakeVector3((i + 0.5f) - cells_x * 0.5f, (j + 0.5f) - cells_z * 0.5f,-halfHeight ); } m_instancingRenderer->registerGraphicsInstance(cubeId,center,orn,color,scaling); } diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 33066175c..2ec671d6d 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -691,6 +691,216 @@ int b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle return 0; } +b3SharedMemoryCommandHandle b3CreateCollisionShapeCommandInit(b3PhysicsClientHandle physClient) +{ + PhysicsClient* cl = (PhysicsClient* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + if (cl) + { + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + command->m_type = CMD_CREATE_COLLISION_SHAPE; + command->m_updateFlags =0; + command->m_createCollisionShapeArgs.m_numCollisionShapes = 0; + return (b3SharedMemoryCommandHandle) command; + } + return 0; +} +void b3CreateCollisionShapeAddSphere(b3SharedMemoryCommandHandle commandHandle,double radius) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_CREATE_COLLISION_SHAPE); + if (command->m_type==CMD_CREATE_COLLISION_SHAPE) + { + int shapeIndex = command->m_createCollisionShapeArgs.m_numCollisionShapes; + if (shapeIndex m_createCollisionShapeArgs.m_shapes[shapeIndex].m_type = GEOM_SPHERE; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_sphereRadius = radius; + command->m_createCollisionShapeArgs.m_numCollisionShapes++; + } + } +} + +void b3CreateCollisionShapeAddBox(b3SharedMemoryCommandHandle commandHandle,double halfExtents[3]) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_CREATE_COLLISION_SHAPE); + if (command->m_type==CMD_CREATE_COLLISION_SHAPE) + { + int shapeIndex = command->m_createCollisionShapeArgs.m_numCollisionShapes; + if (shapeIndex m_createCollisionShapeArgs.m_shapes[shapeIndex].m_type = GEOM_BOX; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_boxHalfExtents[0] = halfExtents[0]; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_boxHalfExtents[1] = halfExtents[1]; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_boxHalfExtents[2] = halfExtents[2]; + command->m_createCollisionShapeArgs.m_numCollisionShapes++; + } + } +} + + +void b3CreateCollisionShapeSetChildTransform(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, double childPosition[3], double childOrientation[4]) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_CREATE_COLLISION_SHAPE); + if (command->m_type==CMD_CREATE_COLLISION_SHAPE) + { + if (shapeIndexm_createCollisionShapeArgs.m_numCollisionShapes) + { + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 1; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_childPosition[0] = childPosition[0]; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_childPosition[1] = childPosition[1]; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_childPosition[2] = childPosition[2]; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_childOrientation[0] = childOrientation[0]; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_childOrientation[1] = childOrientation[1]; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_childOrientation[2] = childOrientation[2]; + command->m_createCollisionShapeArgs.m_shapes[shapeIndex].m_childOrientation[3] = childOrientation[3]; + } + } +} + + +int b3GetStatusCollisionShapeUniqueId(b3SharedMemoryStatusHandle statusHandle) +{ + const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; + b3Assert(status); + b3Assert(status->m_type == CMD_CREATE_COLLISION_SHAPE_COMPLETED); + if (status && status->m_type == CMD_CREATE_COLLISION_SHAPE_COMPLETED) + { + return status->m_createCollisionShapeResultArgs.m_collisionShapeUniqueId; + } + return -1; +} + + +b3SharedMemoryCommandHandle b3CreateVisualShapeCommandInit(b3PhysicsClientHandle physClient) +{ + PhysicsClient* cl = (PhysicsClient* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + if (cl) + { + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + command->m_type = CMD_CREATE_VISUAL_SHAPE; + command->m_updateFlags =0; + return (b3SharedMemoryCommandHandle) command; + } + return 0; +} + +int b3GetStatusVisualShapeUniqueId(b3SharedMemoryStatusHandle statusHandle) +{ + const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; + b3Assert(status); + b3Assert(status->m_type == CMD_CREATE_VISUAL_SHAPE_COMPLETED); + if (status && status->m_type == CMD_CREATE_VISUAL_SHAPE_COMPLETED) + { + return status->m_createVisualShapeResultArgs.m_visualShapeUniqueId; + } + return -1; +} + +b3SharedMemoryCommandHandle b3CreateMultiBodyCommandInit(b3PhysicsClientHandle physClient) +{ + PhysicsClient* cl = (PhysicsClient* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + if (cl) + { + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + command->m_type = CMD_CREATE_MULTI_BODY; + command->m_updateFlags =0; + command->m_createMultiBodyArgs.m_bodyName[0] = 0; + command->m_createMultiBodyArgs.m_baseLinkIndex = -1; + command->m_createMultiBodyArgs.m_numLinks = 0; + return (b3SharedMemoryCommandHandle) command; + } + return 0; +} + + +int b3CreateMultiBodyBase(b3SharedMemoryCommandHandle commandHandle, double mass, int collisionShapeUnique, int visualShapeUniqueId, double basePosition[3], double baseOrientation[4]) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_CREATE_MULTI_BODY); + if (command->m_type==CMD_CREATE_MULTI_BODY) + { + int numLinks = command->m_createMultiBodyArgs.m_numLinks; + + if (numLinksm_updateFlags |=MULTI_BODY_HAS_BASE; + command->m_createMultiBodyArgs.m_baseLinkIndex = baseLinkIndex; + command->m_createMultiBodyArgs.m_baseWorldPosition[0]=basePosition[0]; + command->m_createMultiBodyArgs.m_baseWorldPosition[1]=basePosition[1]; + command->m_createMultiBodyArgs.m_baseWorldPosition[2]=basePosition[2]; + + command->m_createMultiBodyArgs.m_baseWorldOrientation[0]=baseOrientation[0]; + command->m_createMultiBodyArgs.m_baseWorldOrientation[1]=baseOrientation[1]; + command->m_createMultiBodyArgs.m_baseWorldOrientation[2]=baseOrientation[2]; + command->m_createMultiBodyArgs.m_baseWorldOrientation[3]=baseOrientation[3]; + + command->m_createMultiBodyArgs.m_linkInertias[baseLinkIndex*3+0] = mass; + command->m_createMultiBodyArgs.m_linkInertias[baseLinkIndex*3+1] = mass; + command->m_createMultiBodyArgs.m_linkInertias[baseLinkIndex*3+2] = mass; + + + command->m_createMultiBodyArgs.m_linkInertialFramePositions[baseLinkIndex*3+0] = 0; + command->m_createMultiBodyArgs.m_linkInertialFramePositions[baseLinkIndex*3+1] = 0; + command->m_createMultiBodyArgs.m_linkInertialFramePositions[baseLinkIndex*3+2] = 0; + + command->m_createMultiBodyArgs.m_linkInertialFrameOrientations[baseLinkIndex*4+0] = 0; + command->m_createMultiBodyArgs.m_linkInertialFrameOrientations[baseLinkIndex*4+1] = 0; + command->m_createMultiBodyArgs.m_linkInertialFrameOrientations[baseLinkIndex*4+2] = 0; + command->m_createMultiBodyArgs.m_linkInertialFrameOrientations[baseLinkIndex*4+3] = 1; + + command->m_createMultiBodyArgs.m_linkCollisionShapeUniqueIds[baseLinkIndex]= collisionShapeUnique; + command->m_createMultiBodyArgs.m_linkVisualShapeUniqueIds[baseLinkIndex] = visualShapeUniqueId; + + command->m_createMultiBodyArgs.m_linkMasses[command->m_createMultiBodyArgs.m_numLinks] = mass; + command->m_createMultiBodyArgs.m_numLinks++; + } + return numLinks; + } + 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) +{ + const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; + b3Assert(status); + b3Assert(status->m_type == CMD_CREATE_MULTI_BODY_COMPLETED); + if (status && status->m_type == CMD_CREATE_MULTI_BODY_COMPLETED) + { + return status->m_createMultiBodyResultArgs.m_bodyUniqueId; + } + return -1; +} + b3SharedMemoryCommandHandle b3CreateBoxShapeCommandInit(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient* ) physClient; @@ -1070,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); @@ -1342,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 c11ac9d97..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); @@ -313,6 +314,29 @@ int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandl ///the creation of collision shapes and rigid bodies etc is likely going to change, ///but good to have a b3CreateBoxShapeCommandInit for now + +b3SharedMemoryCommandHandle b3CreateCollisionShapeCommandInit(b3PhysicsClientHandle physClient); +void b3CreateCollisionShapeAddSphere(b3SharedMemoryCommandHandle commandHandle,double radius); +void b3CreateCollisionShapeAddBox(b3SharedMemoryCommandHandle commandHandle,double halfExtents[3]); + + +void b3CreateCollisionShapeSetChildTransform(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, double childPosition[3], double childOrientation[4]); + +int b3GetStatusCollisionShapeUniqueId(b3SharedMemoryStatusHandle statusHandle); + +b3SharedMemoryCommandHandle b3CreateVisualShapeCommandInit(b3PhysicsClientHandle physClient); +int b3GetStatusVisualShapeUniqueId(b3SharedMemoryStatusHandle statusHandle); + +b3SharedMemoryCommandHandle b3CreateMultiBodyCommandInit(b3PhysicsClientHandle physClient); +int b3CreateMultiBodyBase(b3SharedMemoryCommandHandle commandHandle, double mass, int collisionShapeUnique, int visualShapeUniqueId, double basePosition[3], double baseOrientation[4]); + +//useMaximalCoordinates are disabled by default, enabling them is experimental and not fully supported yet +void b3CreateMultiBodyUseMaximalCoordinates(b3SharedMemoryCommandHandle commandHandle); + +//int b3CreateMultiBodyAddLink(b3SharedMemoryCommandHandle commandHandle, int jointType, int parentLinkIndex, double linkMass, int linkCollisionShapeUnique, int linkVisualShapeUniqueId); + + + ///create a box of size (1,1,1) at world origin (0,0,0) at orientation quat (0,0,0,1) ///after that, you can optionally adjust the initial position, orientation and size b3SharedMemoryCommandHandle b3CreateBoxShapeCommandInit(b3PhysicsClientHandle physClient); diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 4872a151a..a2061b246 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -442,6 +442,8 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { break; } + + case CMD_CREATE_MULTI_BODY_COMPLETED: case CMD_URDF_LOADING_COMPLETED: { B3_PROFILE("CMD_URDF_LOADING_COMPLETED"); @@ -1056,6 +1058,28 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { b3Warning("Request dynamics info failed"); break; } + case CMD_CREATE_COLLISION_SHAPE_FAILED: + { + b3Warning("Request createCollisionShape failed"); + break; + } + case CMD_CREATE_COLLISION_SHAPE_COMPLETED: + case CMD_CREATE_VISUAL_SHAPE_COMPLETED: + { + break; + } + + case CMD_CREATE_MULTI_BODY_FAILED: + { + b3Warning("Request createMultiBody failed"); + break; + } + case CMD_CREATE_VISUAL_SHAPE_FAILED: + { + b3Warning("Request createVisualShape failed"); + break; + } + default: { b3Error("Unknown server status %d\n", serverCmd.m_type); btAssert(0); diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index e03e2d7f0..ed8376c9f 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -835,6 +835,7 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd } break; } + case CMD_CREATE_MULTI_BODY_COMPLETED: case CMD_URDF_LOADING_COMPLETED: { @@ -881,6 +882,33 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd b3Warning("createConstraint failed"); break; } + + case CMD_CREATE_COLLISION_SHAPE_FAILED: + { + b3Warning("createCollisionShape failed"); + break; + } + case CMD_CREATE_COLLISION_SHAPE_COMPLETED: + { + break; + } + + case CMD_CREATE_VISUAL_SHAPE_FAILED: + { + b3Warning("createVisualShape failed"); + break; + } + case CMD_CREATE_VISUAL_SHAPE_COMPLETED: + { + break; + } + + case CMD_CREATE_MULTI_BODY_FAILED: + { + b3Warning("createMultiBody failed"); + break; + } + default: { //b3Warning("Unknown server status type"); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 39a18f909..40385d85c 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -124,7 +124,14 @@ struct SharedMemoryDebugDrawer : public btIDebugDraw } }; - +struct InteralCollisionShapeData +{ + btCollisionShape* m_collisionShape; + void clear() + { + m_collisionShape=0; + } +}; struct InteralBodyData { @@ -171,7 +178,7 @@ struct InteralUserConstraintData }; typedef b3PoolBodyHandle InternalBodyHandle; - +typedef b3PoolBodyHandle InternalCollisionShapeHandle; class btCommandChunk { @@ -1133,7 +1140,7 @@ struct PhysicsServerCommandProcessorInternalData { ///handle management b3ResizablePool< InternalBodyHandle > m_bodyHandles; - + b3ResizablePool m_userCollisionShapeHandles; bool m_allowRealTimeSimulation; @@ -1244,6 +1251,9 @@ struct PhysicsServerCommandProcessorInternalData m_bodyHandles.exitHandles(); m_bodyHandles.initHandles(); + m_userCollisionShapeHandles.exitHandles(); + m_userCollisionShapeHandles.initHandles(); + #if 0 btAlignedObjectArray bla; @@ -1490,6 +1500,211 @@ void PhysicsServerCommandProcessor::logObjectStates(btScalar timeStep) } +struct ProgrammaticUrdfInterface : public URDFImporterInterface +{ + int m_bodyUniqueId; + + const b3CreateMultiBodyArgs& m_createBodyArgs; + mutable b3AlignedObjectArray m_allocatedCollisionShapes; + PhysicsServerCommandProcessorInternalData* m_data; + + ProgrammaticUrdfInterface(const b3CreateMultiBodyArgs& bodyArgs, PhysicsServerCommandProcessorInternalData* data) + :m_bodyUniqueId(-1), + m_createBodyArgs(bodyArgs), + m_data(data) + { + + } + + virtual ~ProgrammaticUrdfInterface() + { + + } + + virtual bool loadURDF(const char* fileName, bool forceFixedBase = false) + { + b3Assert(0); + return false; + } + + virtual const char* getPathPrefix() + { + return ""; + } + + ///return >=0 for the root link index, -1 if there is no root link + virtual int getRootLinkIndex() const + { + return m_createBodyArgs.m_baseLinkIndex; + } + + ///pure virtual interfaces, precondition is a valid linkIndex (you can assert/terminate if the linkIndex is out of range) + virtual std::string getLinkName(int linkIndex) const + { + return "link"; + } + + //various derived class in internal source code break with new pure virtual methods, so provide some default implementation + virtual std::string getBodyName() const + { + return m_createBodyArgs.m_bodyName; + } + + /// optional method to provide the link color. return true if the color is available and copied into colorRGBA, return false otherwise + virtual bool getLinkColor(int linkIndex, btVector4& colorRGBA) const + { + b3Assert(0); + return false; + } + + virtual bool getLinkColor2(int linkIndex, struct UrdfMaterialColor& matCol) const + { + return false; + } + + virtual int getCollisionGroupAndMask(int linkIndex, int& colGroup, int& colMask) const + { + return 0; + } + ///this API will likely change, don't override it! + virtual bool getLinkContactInfo(int linkIndex, URDFLinkContactInfo& contactInfo ) const + { + + return false; + } + + virtual bool getLinkAudioSource(int linkIndex, SDFAudioSource& audioSource) const + { + b3Assert(0); + return false; + } + + virtual std::string getJointName(int linkIndex) const + { + b3Assert(0); + return "joint"; + } + + //fill mass and inertial data. If inertial data is missing, please initialize mass, inertia to sensitive values, and inertialFrame to identity. + virtual void getMassAndInertia(int urdfLinkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const + { + if (urdfLinkIndex>=0 && urdfLinkIndex < m_createBodyArgs.m_numLinks) + { + mass = m_createBodyArgs.m_linkMasses[urdfLinkIndex]; + localInertiaDiagonal.setValue( + m_createBodyArgs.m_linkInertias[urdfLinkIndex*3+0], + m_createBodyArgs.m_linkInertias[urdfLinkIndex*3+1], + m_createBodyArgs.m_linkInertias[urdfLinkIndex*3+2]); + inertialFrame.setOrigin(btVector3( + m_createBodyArgs.m_linkInertialFramePositions[urdfLinkIndex*3+0], + m_createBodyArgs.m_linkInertialFramePositions[urdfLinkIndex*3+1], + m_createBodyArgs.m_linkInertialFramePositions[urdfLinkIndex*3+2])); + inertialFrame.setRotation(btQuaternion( + m_createBodyArgs.m_linkInertialFrameOrientations[urdfLinkIndex*4+0], + m_createBodyArgs.m_linkInertialFrameOrientations[urdfLinkIndex*4+1], + m_createBodyArgs.m_linkInertialFrameOrientations[urdfLinkIndex*4+2], + m_createBodyArgs.m_linkInertialFrameOrientations[urdfLinkIndex*4+3])); + } else + { + mass = 0; + localInertiaDiagonal.setValue(0,0,0); + inertialFrame.setIdentity(); + } + } + + ///fill an array of child link indices for this link, btAlignedObjectArray behaves like a std::vector so just use push_back and resize(0) if needed + virtual void getLinkChildIndices(int urdfLinkIndex, btAlignedObjectArray& childLinkIndices) const + { + } + + virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const + { + return false; + }; + + virtual bool getJointInfo2(int urdfLinkIndex, btTransform& parent2joint, btTransform& linkTransformInWorld, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction, btScalar& jointMaxForce, btScalar& jointMaxVelocity) const + { + //backwards compatibility for custom file importers + jointMaxForce = 0; + jointMaxVelocity = 0; + return getJointInfo(urdfLinkIndex, parent2joint, linkTransformInWorld, jointAxisInJointSpace, jointType, jointLowerLimit, jointUpperLimit, jointDamping, jointFriction); + }; + + virtual bool getRootTransformInWorld(btTransform& rootTransformInWorld) const + { + rootTransformInWorld.setOrigin(btVector3( + m_createBodyArgs.m_baseWorldPosition[0], + m_createBodyArgs.m_baseWorldPosition[1], + m_createBodyArgs.m_baseWorldPosition[2])); + rootTransformInWorld.setRotation(btQuaternion( + m_createBodyArgs.m_baseWorldOrientation[0], + m_createBodyArgs.m_baseWorldOrientation[1], + m_createBodyArgs.m_baseWorldOrientation[2], + m_createBodyArgs.m_baseWorldOrientation[3])); + return true; + } + virtual void setRootTransformInWorld(const btTransform& rootTransformInWorld) + { + b3Assert(0); + } + + ///quick hack: need to rethink the API/dependencies of this + virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const + { + return -1; + } + + virtual void convertLinkVisualShapes2(int linkIndex, int urdfIndex, const char* pathPrefix, const btTransform& inertialFrame, class btCollisionObject* colObj, int objectIndex) const + { + } + virtual void setBodyUniqueId(int bodyId) + { + m_bodyUniqueId = bodyId; + } + virtual int getBodyUniqueId() const + { + return m_bodyUniqueId; + } + + //default implementation for backward compatibility + virtual class btCompoundShape* convertLinkCollisionShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const + { + btCompoundShape* compound = new btCompoundShape(); + + int colShapeUniqueId = m_createBodyArgs.m_linkCollisionShapeUniqueIds[linkIndex]; + if (colShapeUniqueId>=0) + { + InternalCollisionShapeHandle* handle = m_data->m_userCollisionShapeHandles.getHandle(colShapeUniqueId); + if (handle) + { + btTransform childTrans; + childTrans.setIdentity(); + compound->addChildShape(localInertiaFrame.inverse()*childTrans,handle->m_collisionShape); + } + } + m_allocatedCollisionShapes.push_back(compound); + return compound; + } + + virtual int getNumAllocatedCollisionShapes() const + { + return m_allocatedCollisionShapes.size(); + } + + virtual class btCollisionShape* getAllocatedCollisionShape(int index) + { + return m_allocatedCollisionShapes[index]; + } + virtual int getNumModels() const + { + return 1; + } + virtual void activateModel(int /*modelIndex*/) + { + } +}; + + void PhysicsServerCommandProcessor::createEmptyDynamicsWorld() { ///collision configuration contains default setup for memory, collision setup @@ -1829,7 +2044,22 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName, createJointMotors(mb); - +#ifdef B3_ENABLE_TINY_AUDIO + { + SDFAudioSource audioSource; + int urdfRootLink = u2b.getRootLinkIndex();//LinkIndex = creation.m_mb2urdfLink[-1]; + if (u2b.getLinkAudioSource(urdfRootLink,audioSource)) + { + int flags = mb->getBaseCollider()->getCollisionFlags(); + mb->getBaseCollider()->setCollisionFlags(flags | btCollisionObject::CF_HAS_COLLISION_SOUND_TRIGGER); + audioSource.m_userIndex = m_data->m_soundEngine.loadWavFile(audioSource.m_uri.c_str()); + if (audioSource.m_userIndex>=0) + { + bodyHandle->m_audioSources.insert(-1, audioSource); + } + } + } +#endif //disable serialization of the collision objects (they are too big, and the client likely doesn't need them); bodyHandle->m_linkLocalInertialFrames.reserve(mb->getNumLinks()); @@ -1853,14 +2083,33 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName, m_data->m_strings.push_back(jointName); mb->getLink(i).m_jointName = jointName->c_str(); + +#ifdef B3_ENABLE_TINY_AUDIO + { + SDFAudioSource audioSource; + int urdfLinkIndex = creation.m_mb2urdfLink[link]; + if (u2b.getLinkAudioSource(urdfLinkIndex,audioSource)) + { + int flags = mb->getLink(link).m_collider->getCollisionFlags(); + mb->getLink(i).m_collider->setCollisionFlags(flags | btCollisionObject::CF_HAS_COLLISION_SOUND_TRIGGER); + audioSource.m_userIndex = m_data->m_soundEngine.loadWavFile(audioSource.m_uri.c_str()); + if (audioSource.m_userIndex>=0) + { + bodyHandle->m_audioSources.insert(link, audioSource); + } + } + } +#endif } std::string* baseName = new std::string(u2b.getLinkName(u2b.getRootLinkIndex())); m_data->m_strings.push_back(baseName); mb->setBaseName(baseName->c_str()); } else { - b3Warning("No multibody loaded from URDF. Could add btRigidBody+btTypedConstraint solution later."); + //b3Warning("No multibody loaded from URDF. Could add btRigidBody+btTypedConstraint solution later."); bodyHandle->m_rigidBody = rb; + rb->setUserIndex2(bodyUniqueId); + m_data->m_sdfRecentLoadedBodies.push_back(bodyUniqueId); } } @@ -1945,6 +2194,9 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVector3& pos, const btQuaternion& orn, bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes, int flags) { + m_data->m_sdfRecentLoadedBodies.clear(); + *bodyUniqueIdPtr = -1; + BT_PROFILE("loadURDF"); btAssert(m_data->m_dynamicsWorld); if (!m_data->m_dynamicsWorld) @@ -1962,6 +2214,25 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto if (loadOk) { + +#if 1 + btTransform rootTrans; + rootTrans.setOrigin(pos); + rootTrans.setRotation(orn); + u2b.setRootTransformInWorld(rootTrans); + bool ok = processImportedObjects(fileName, bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, u2b); + if (ok) + { + if (m_data->m_sdfRecentLoadedBodies.size()==1) + { + *bodyUniqueIdPtr = m_data->m_sdfRecentLoadedBodies[0]; + + } + m_data->m_sdfRecentLoadedBodies.clear(); + } + return ok; +#else + //get a body index int bodyUniqueId = m_data->m_bodyHandles.allocHandle(); if (bodyUniqueIdPtr) @@ -2132,9 +2403,9 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto return true; } } - + #endif } - + return false; } @@ -3155,6 +3426,154 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm hasStatus = true; break; } + case CMD_CREATE_COLLISION_SHAPE: + { + hasStatus = true; + serverStatusOut.m_type = CMD_CREATE_COLLISION_SHAPE_FAILED; + + btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_data->m_dynamicsWorld); + btCollisionShape* shape = 0; + + btCompoundShape* compound = 0; + + if (clientCmd.m_createCollisionShapeArgs.m_numCollisionShapes>1) + { + compound = worldImporter->createCompoundShape(); + } + for (int i=0;icreateCompoundShape(); + } + } + + switch (clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_type) + { + case GEOM_SPHERE: + { + double radius = clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_sphereRadius; + shape = worldImporter->createSphereShape(radius); + if (compound) + { + compound->addChildShape(childTransform,shape); + } + break; + } + case GEOM_BOX: + { + //double halfExtents[3] = clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_sphereRadius; + shape = worldImporter->createBoxShape(btVector3( + clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_boxHalfExtents[0], + clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_boxHalfExtents[1], + clientCmd.m_createCollisionShapeArgs.m_shapes[i].m_boxHalfExtents[2])); + if (compound) + { + compound->addChildShape(childTransform,shape); + } + break; + } + } + } + #if 0 + shape = worldImporter->createCylinderShapeX(radius,height); + shape = worldImporter->createCylinderShapeY(radius,height); + shape = worldImporter->createCylinderShapeZ(radius,height); + shape = worldImporter->createCapsuleShapeX(radius,height); + shape = worldImporter->createCapsuleShapeY(radius,height); + shape = worldImporter->createCapsuleShapeZ(radius,height); + shape = worldImporter->createBoxShape(halfExtents); + #endif + if (compound && compound->getNumChildShapes()) + { + shape = compound; + } + + if (shape) + { + int collisionShapeUid = m_data->m_userCollisionShapeHandles.allocHandle(); + InternalCollisionShapeHandle* handle = m_data->m_userCollisionShapeHandles.getHandle(collisionShapeUid); + handle->m_collisionShape = shape; + serverStatusOut.m_createCollisionShapeResultArgs.m_collisionShapeUniqueId = collisionShapeUid; + m_data->m_worldImporters.push_back(worldImporter); + serverStatusOut.m_type = CMD_CREATE_COLLISION_SHAPE_COMPLETED; + } else + { + delete worldImporter; + } + + + break; + } + case CMD_CREATE_VISUAL_SHAPE: + { + hasStatus = true; + serverStatusOut.m_type = CMD_CREATE_VISUAL_SHAPE_FAILED; + break; + } + case CMD_CREATE_MULTI_BODY: + { + hasStatus = true; + 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 = true; + if (clientCmd.m_updateFlags & MULT_BODY_USE_MAXIMAL_COORDINATES) + { + useMultiBody = false; + } + + int flags = 0; + bool ok = processImportedObjects("memory", bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, u2b); + + if (ok) + { + int bodyUniqueId = -1; + + if (m_data->m_sdfRecentLoadedBodies.size()==1) + { + bodyUniqueId = m_data->m_sdfRecentLoadedBodies[0]; + } + m_data->m_sdfRecentLoadedBodies.clear(); + if (bodyUniqueId>=0) + { + m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); + serverStatusOut.m_type = CMD_CREATE_MULTI_BODY_COMPLETED; + + int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes); + if (m_data->m_urdfLinkNameMapper.size()) + { + serverStatusOut.m_numDataStreamBytes = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size()-1)->m_memSerializer->getCurrentBufferSize(); + } + serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId; + InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str()); + } + } + + //ConvertURDF2Bullet(u2b,creation, rootTrans,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix(),flags); + + + } + break; + } case CMD_LOAD_URDF: { BT_PROFILE("CMD_LOAD_URDF"); @@ -3193,13 +3612,16 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm initialPos,initialOrn, useMultiBody, useFixedBase,&bodyUniqueId, bufferServerToClient, bufferSizeInBytes, urdfFlags); - if (completedOk) + if (completedOk && bodyUniqueId>=0) { + m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); serverStatusOut.m_type = CMD_URDF_LOADING_COMPLETED; + int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes); + if (m_data->m_urdfLinkNameMapper.size()) { @@ -3914,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); @@ -3980,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); @@ -6409,8 +6851,8 @@ void PhysicsServerCommandProcessor::resetSimulation() m_data->m_bodyHandles.exitHandles(); m_data->m_bodyHandles.initHandles(); - - + m_data->m_userCollisionShapeHandles.exitHandles(); + m_data->m_userCollisionShapeHandles.initHandles(); } diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index b322c163d..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 @@ -761,6 +765,96 @@ struct ConfigureOpenGLVisualizerRequest int m_setEnabled; }; +enum +{ + URDF_GEOM_HAS_RADIUS = 1, +}; + +struct b3CreateCollisionShape +{ + int m_type;//see UrdfGeomTypes + + int m_hasChildTransform; + double m_childPosition[3]; + double m_childOrientation[4]; + + double m_sphereRadius; + double m_boxHalfExtents[3]; + double m_capsuleRadius; + double m_capsuleHeight; + int m_hasFromTo; + double m_capsuleFrom[3]; + double m_capsuleTo[3]; + double m_planeNormal[3]; + + int m_meshFileType; + char m_meshFileName[1024]; + double m_meshScale; +}; + +#define MAX_COMPOUND_COLLISION_SHAPES 128 + +struct b3CreateCollisionShapeArgs +{ + int m_numCollisionShapes; + b3CreateCollisionShape m_shapes[MAX_COMPOUND_COLLISION_SHAPES]; +}; + + +struct b3CreateVisualShapeArgs +{ + int m_visualShapeUniqueId; +}; + +#define MAX_CREATE_MULTI_BODY_LINKS 128 +enum eCreateMultiBodyEnum +{ + MULTI_BODY_HAS_BASE=1, + MULT_BODY_USE_MAXIMAL_COORDINATES=2, +}; +struct b3CreateMultiBodyArgs +{ + char m_bodyName[1024]; + int m_baseLinkIndex; + + double m_baseWorldPosition[3]; + double m_baseWorldOrientation[4]; + + int m_numLinks; + double m_linkMasses[MAX_CREATE_MULTI_BODY_LINKS]; + double m_linkInertias[MAX_CREATE_MULTI_BODY_LINKS*3]; + double m_linkInertialFramePositions[MAX_CREATE_MULTI_BODY_LINKS*3]; + double m_linkInertialFrameOrientations[MAX_CREATE_MULTI_BODY_LINKS*4]; + int m_linkJointTypes[MAX_CREATE_MULTI_BODY_LINKS]; + int m_linkCollisionShapeUniqueIds[MAX_CREATE_MULTI_BODY_LINKS]; + int m_linkVisualShapeUniqueIds[MAX_CREATE_MULTI_BODY_LINKS]; + + #if 0 + std::string m_name; + std::string m_sourceFile; + btTransform m_rootTransformInWorld; + btHashMap m_materials; + btHashMap m_links; + btHashMap m_joints; + #endif +}; + +struct b3CreateCollisionShapeResultArgs +{ + int m_collisionShapeUniqueId; +}; + +struct b3CreateVisualShapeResultArgs +{ + int m_visualShapeUniqueId; +}; + +struct b3CreateMultiBodyResultArgs +{ + int m_bodyUniqueId; +}; + + struct SharedMemoryCommand { int m_type; @@ -808,6 +902,9 @@ struct SharedMemoryCommand struct ConfigureOpenGLVisualizerRequest m_configureOpenGLVisualizerArguments; struct b3ObjectArgs m_removeObjectArgs; struct b3Profile m_profile; + struct b3CreateCollisionShapeArgs m_createCollisionShapeArgs; + struct b3CreateVisualShapeArgs m_createVisualShapeArgs; + struct b3CreateMultiBodyArgs m_createMultiBodyArgs; }; }; @@ -874,6 +971,9 @@ struct SharedMemoryStatus struct b3OpenGLVisualizerCameraInfo m_visualizerCameraResultArgs; struct b3ObjectArgs m_removeObjectArgs; struct b3DynamicsInfo m_dynamicsInfo; + struct b3CreateCollisionShapeResultArgs m_createCollisionShapeResultArgs; + struct b3CreateVisualShapeResultArgs m_createVisualShapeResultArgs; + struct b3CreateMultiBodyResultArgs m_createMultiBodyResultArgs; }; }; 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) @@ -4701,6 +4719,158 @@ static PyObject* pybullet_enableJointForceTorqueSensor(PyObject* self, PyObject* return NULL; } +static PyObject* pybullet_createCollisionShape(PyObject* self, PyObject* args, PyObject* keywds) +{ + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + int shapeType=-1; + double radius=-1; + PyObject* halfExtentsObj=0; + + static char* kwlist[] = {"shapeType","radius","halfExtents", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|dOi", kwlist, + &shapeType, &radius,&halfExtentsObj, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + if (shapeType>=GEOM_SPHERE) + { + b3SharedMemoryStatusHandle statusHandle; + int statusType; + b3SharedMemoryCommandHandle commandHandle = b3CreateCollisionShapeCommandInit(sm); + if (shapeType==GEOM_SPHERE && radius>0) + { + b3CreateCollisionShapeAddSphere(commandHandle,radius); + } + if (shapeType==GEOM_BOX && halfExtentsObj) + { + double halfExtents[3] = {1,1,1}; + pybullet_internalSetVectord(halfExtentsObj,halfExtents); + b3CreateCollisionShapeAddBox(commandHandle,halfExtents); + } + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_CREATE_COLLISION_SHAPE_COMPLETED) + { + int uid = b3GetStatusCollisionShapeUniqueId(statusHandle); + PyObject* ob = PyLong_FromLong(uid); + return ob; + } + } + PyErr_SetString(SpamError, "createCollisionShape failed."); + return NULL; +} +#if 0 +b3SharedMemoryCommandHandle b3CreateCollisionShapeCommandInit(b3PhysicsClientHandle physClient); +int b3GetStatusCollisionShapeUniqueId(b3SharedMemoryStatusHandle statusHandle); + +b3SharedMemoryCommandHandle b3CreateVisualShapeCommandInit(b3PhysicsClientHandle physClient); +int b3GetStatusVisualShapeUniqueId(b3SharedMemoryStatusHandle statusHandle); + +b3SharedMemoryCommandHandle b3CreateMultiBodyCommandInit(b3PhysicsClientHandle physClient); +int b3GetStatusMultiBodyUniqueId(b3SharedMemoryStatusHandle statusHandle); +#endif + + +static PyObject* pybullet_createVisualShape(PyObject* self, PyObject* args, PyObject* keywds) +{ + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + int test=-1; + + static char* kwlist[] = {"test",NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &test, + &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + if (test>=0) + { + b3SharedMemoryStatusHandle statusHandle; + int statusType; + b3SharedMemoryCommandHandle commandHandle = b3CreateVisualShapeCommandInit(sm); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_CREATE_VISUAL_SHAPE_COMPLETED) + { + int uid = b3GetStatusVisualShapeUniqueId(statusHandle); + PyObject* ob = PyLong_FromLong(uid); + return ob; + } + } + + PyErr_SetString(SpamError, "createVisualShape failed."); + return NULL; +} + +static PyObject* pybullet_createMultiBody(PyObject* self, PyObject* args, PyObject* keywds) +{ + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + 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, "|diiOOii", kwlist, &baseMass,&baseCollisionShapeIndex,&baseVisualShapeIndex, + &basePosObj, &baseOrnObj,&useMaximalCoordinates, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + { + b3SharedMemoryStatusHandle statusHandle; + int statusType; + b3SharedMemoryCommandHandle commandHandle = b3CreateMultiBodyCommandInit(sm); + double basePosition[3]={0,0,0}; + double baseOrientation[4]={0,0,0,1}; + pybullet_internalSetVectord(basePosObj,basePosition); + pybullet_internalSetVector4d(baseOrnObj,baseOrientation); + int baseIndex = b3CreateMultiBodyBase(commandHandle,baseMass,baseCollisionShapeIndex,baseVisualShapeIndex,basePosition,baseOrientation); + if (useMaximalCoordinates>0) + { + b3CreateMultiBodyUseMaximalCoordinates(commandHandle); + } + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_CREATE_MULTI_BODY_COMPLETED) + { + int uid = b3GetStatusBodyIndex(statusHandle); + PyObject* ob = PyLong_FromLong(uid); + return ob; + } + } + PyErr_SetString(SpamError, "createMultiBody failed."); + return NULL; +} + + static PyObject* pybullet_createUserConstraint(PyObject* self, PyObject* args, PyObject* keywds) { b3SharedMemoryCommandHandle commandHandle; @@ -6218,6 +6388,16 @@ static PyMethodDef SpamMethods[] = { {"loadMJCF", (PyCFunction)pybullet_loadMJCF, METH_VARARGS | METH_KEYWORDS, "Load multibodies from an MJCF file."}, + {"createCollisionShape", (PyCFunction)pybullet_createCollisionShape, METH_VARARGS | METH_KEYWORDS, + "Create a collision shape. Returns a non-negative (int) unique id, if successfull, negative otherwise."}, + + {"createVisualShape", (PyCFunction)pybullet_createVisualShape, METH_VARARGS | METH_KEYWORDS, + "Create a visual shape. Returns a non-negative (int) unique id, if successfull, negative otherwise."}, + + {"createMultiBody", (PyCFunction)pybullet_createMultiBody, METH_VARARGS | METH_KEYWORDS, + "Create a multi body. Returns a non-negative (int) unique id, if successfull, negative otherwise."}, + + {"createConstraint", (PyCFunction)pybullet_createUserConstraint, METH_VARARGS | METH_KEYWORDS, "Create a constraint between two bodies. Returns a (int) unique id, if successfull."}, @@ -6652,6 +6832,15 @@ initpybullet(void) PyModule_AddIntConstant(m, "B3G_ALT", B3G_ALT); PyModule_AddIntConstant(m, "B3G_RETURN", B3G_RETURN); + PyModule_AddIntConstant(m, "GEOM_SPHERE", GEOM_SPHERE); + PyModule_AddIntConstant(m, "GEOM_BOX", GEOM_BOX); + PyModule_AddIntConstant(m, "GEOM_CYLINDER", GEOM_CYLINDER); + PyModule_AddIntConstant(m, "GEOM_MESH", GEOM_MESH); + PyModule_AddIntConstant(m, "GEOM_PLANE", GEOM_PLANE); + PyModule_AddIntConstant(m, "GEOM_CAPSULE", GEOM_CAPSULE); + + + SpamError = PyErr_NewException("pybullet.error", NULL, NULL); Py_INCREF(SpamError); PyModule_AddObject(m, "error", SpamError); 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)) { diff --git a/src/LinearMath/btThreads.h b/src/LinearMath/btThreads.h index b592ec3e5..284702d71 100644 --- a/src/LinearMath/btThreads.h +++ b/src/LinearMath/btThreads.h @@ -79,6 +79,7 @@ const unsigned int BT_MAX_THREAD_COUNT = 64; SIMD_FORCE_INLINE void btMutexLock( btSpinMutex* ) {} SIMD_FORCE_INLINE void btMutexUnlock( btSpinMutex* ) {} SIMD_FORCE_INLINE bool btMutexTryLock( btSpinMutex* ) {return true;} +SIMD_FORCE_INLINE bool btThreadsAreRunning() { return false;} #endif //