diff --git a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp index d21bf2503..cfcd7b00a 100644 --- a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp +++ b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp @@ -464,7 +464,7 @@ bool b3RobotSimulatorClientAPI_NoDirect::getBasePositionAndOrientation(int bodyU return true; } -bool b3RobotSimulatorClientAPI_NoDirect::resetBasePositionAndOrientation(int bodyUniqueId, btVector3& basePosition, btQuaternion& baseOrientation) +bool b3RobotSimulatorClientAPI_NoDirect::resetBasePositionAndOrientation(int bodyUniqueId, const btVector3& basePosition, const btQuaternion& baseOrientation) { if (!isConnected()) { @@ -1575,7 +1575,7 @@ bool b3RobotSimulatorClientAPI_NoDirect::removeUserDebugItem(int itemUniqueId) return true; } -int b3RobotSimulatorClientAPI_NoDirect::addUserDebugText(char* text, double* textPosition, struct b3RobotSimulatorAddUserDebugTextArgs& args) +int b3RobotSimulatorClientAPI_NoDirect::addUserDebugText(const char* text, double* textPosition, struct b3RobotSimulatorAddUserDebugTextArgs& args) { b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; if (sm == 0) @@ -1611,7 +1611,7 @@ int b3RobotSimulatorClientAPI_NoDirect::addUserDebugText(char* text, double* tex return -1; } -int b3RobotSimulatorClientAPI_NoDirect::addUserDebugText(char* text, btVector3& textPosition, struct b3RobotSimulatorAddUserDebugTextArgs& args) +int b3RobotSimulatorClientAPI_NoDirect::addUserDebugText(const char* text, btVector3& textPosition, struct b3RobotSimulatorAddUserDebugTextArgs& args) { double dposXYZ[3]; dposXYZ[0] = textPosition.x(); @@ -2168,6 +2168,68 @@ bool b3RobotSimulatorClientAPI_NoDirect::getAABB(int bodyUniqueId, int linkIndex return status; } + +int b3RobotSimulatorClientAPI_NoDirect::createVisualShape(int shapeType, struct b3RobotSimulatorCreateVisualShapeArgs& args) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) + { + b3Warning("Not connected"); + return false; + } + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + int shapeIndex = -1; + + command = b3CreateVisualShapeCommandInit(sm); + + if (shapeType == GEOM_SPHERE && args.m_radius > 0) + { + shapeIndex = b3CreateVisualShapeAddSphere(command, args.m_radius); + } + if (shapeType == GEOM_BOX) + { + double halfExtents[3]; + scalarToDouble3(args.m_halfExtents, halfExtents); + shapeIndex = b3CreateVisualShapeAddBox(command, halfExtents); + } + if (shapeType == GEOM_CAPSULE && args.m_radius > 0 && args.m_height >= 0) + { + shapeIndex = b3CreateVisualShapeAddCapsule(command, args.m_radius, args.m_height); + } + if (shapeType == GEOM_CYLINDER && args.m_radius > 0 && args.m_height >= 0) + { + shapeIndex = b3CreateVisualShapeAddCylinder(command, args.m_radius, args.m_height); + } + if (shapeType == GEOM_MESH && args.m_fileName) + { + double meshScale[3]; + scalarToDouble3(args.m_meshScale, meshScale); + shapeIndex = b3CreateVisualShapeAddMesh(command, args.m_fileName, meshScale); + } + if (shapeType == GEOM_PLANE) + { + double planeConstant = 0; + double planeNormal[3]; + scalarToDouble3(args.m_planeNormal, planeNormal); + shapeIndex = b3CreateVisualShapeAddPlane(command, planeNormal, planeConstant); + } + if (shapeIndex >= 0 && args.m_flags) + { + b3CreateVisualSetFlag(command, shapeIndex, args.m_flags); + } + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_CREATE_VISUAL_SHAPE_COMPLETED) + { + int uid = b3GetStatusVisualShapeUniqueId(statusHandle); + return uid; + } + return -1; +} + int b3RobotSimulatorClientAPI_NoDirect::createCollisionShape(int shapeType, struct b3RobotSimulatorCreateCollisionShapeArgs& args) { b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; diff --git a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h index 90a74f899..26237117a 100644 --- a/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h +++ b/examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h @@ -492,6 +492,38 @@ struct b3RobotSimulatorCreateCollisionShapeArgs } }; + +struct b3RobotSimulatorCreateVisualShapeArgs +{ + int m_shapeType; + double m_radius; + btVector3 m_halfExtents; + double m_height; + char* m_fileName; + btVector3 m_meshScale; + btVector3 m_planeNormal; + int m_flags; + b3RobotSimulatorCreateVisualShapeArgs() + : m_shapeType(-1), + m_radius(0.5), + m_height(1), + m_fileName(NULL), + m_flags(0) + { + m_halfExtents.m_floats[0] = 1; + m_halfExtents.m_floats[1] = 1; + m_halfExtents.m_floats[2] = 1; + + m_meshScale.m_floats[0] = 1; + m_meshScale.m_floats[1] = 1; + m_meshScale.m_floats[2] = 1; + + m_planeNormal.m_floats[0] = 0; + m_planeNormal.m_floats[1] = 0; + m_planeNormal.m_floats[2] = 1; + } +}; + struct b3RobotSimulatorCreateMultiBodyArgs { double m_baseMass; @@ -707,7 +739,7 @@ public: bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo *bodyInfo); bool getBasePositionAndOrientation(int bodyUniqueId, btVector3 &basePosition, btQuaternion &baseOrientation) const; - bool resetBasePositionAndOrientation(int bodyUniqueId, btVector3 &basePosition, btQuaternion &baseOrientation); + bool resetBasePositionAndOrientation(int bodyUniqueId, const btVector3 &basePosition, const btQuaternion &baseOrientation); bool getBaseVelocity(int bodyUniqueId, btVector3 &baseLinearVelocity, btVector3 &baseAngularVelocity) const; bool resetBaseVelocity(int bodyUniqueId, const btVector3 &linearVelocity, const btVector3 &angularVelocity) const; @@ -792,9 +824,9 @@ public: bool removeUserDebugItem(int itemUniqueId); - int addUserDebugText(char *text, double *textPosition, struct b3RobotSimulatorAddUserDebugTextArgs &args); + int addUserDebugText(const char *text, double *textPosition, struct b3RobotSimulatorAddUserDebugTextArgs &args); - int addUserDebugText(char *text, btVector3 &textPosition, struct b3RobotSimulatorAddUserDebugTextArgs &args); + int addUserDebugText(const char *text, btVector3 &textPosition, struct b3RobotSimulatorAddUserDebugTextArgs &args); int addUserDebugLine(double *fromXYZ, double *toXYZ, struct b3RobotSimulatorAddUserDebugLineArgs &args); @@ -830,6 +862,8 @@ public: bool getAABB(int bodyUniqueId, int linkIndex, btVector3 &aabbMin, btVector3 &aabbMax); + int createVisualShape(int shapeType, struct b3RobotSimulatorCreateVisualShapeArgs& args); + int createCollisionShape(int shapeType, struct b3RobotSimulatorCreateCollisionShapeArgs &args); int createMultiBody(struct b3RobotSimulatorCreateMultiBodyArgs &args);