From 5fe4c6bb5b87e14359a303de8638b0e257bb5dbe Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Sun, 7 May 2017 22:21:38 -0700 Subject: [PATCH] Add API to get dynamic info. --- examples/SharedMemory/PhysicsClientC_API.cpp | 25 +++++++++++++-- examples/SharedMemory/PhysicsClientC_API.h | 3 +- .../SharedMemory/PhysicsClientExample.cpp | 10 ++++-- .../PhysicsClientSharedMemory.cpp | 9 ++++++ .../PhysicsServerCommandProcessor.cpp | 32 +++++++++++++++++++ examples/SharedMemory/SharedMemoryCommands.h | 7 ++++ examples/SharedMemory/SharedMemoryPublic.h | 3 ++ 7 files changed, 83 insertions(+), 6 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index be95bab46..92c1692b0 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1214,9 +1214,30 @@ int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointInd return cl->getJointInfo(bodyIndex, jointIndex, *info); } -int b3GetDynamicInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, struct b3DynamicInfo* info) +b3SharedMemoryCommandHandle b3GetDynamicInfoCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex) { - return 0; + PhysicsClient* cl = (PhysicsClient* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + command->m_type = CMD_GET_DYNAMIC_INFO; + command->m_getDynamicInfoArgs.m_bodyUniqueId = bodyUniqueId; + command->m_getDynamicInfoArgs.m_linkIndex = linkIndex; + return (b3SharedMemoryCommandHandle) command; +} + +int b3GetDynamicInfo(b3SharedMemoryStatusHandle statusHandle, struct b3DynamicInfo* info) +{ + const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; + const b3DynamicInfo &dynamicInfo = status->m_dynamicInfo; + btAssert(status->m_type == CMD_GET_DYNAMIC_INFO); + if (status->m_type != CMD_GET_DYNAMIC_INFO_COMPLETED) + return false; + + info->m_mass = dynamicInfo.m_mass; + info->m_lateralFrictionCoeff = dynamicInfo.m_lateralFrictionCoeff; + return true; } b3SharedMemoryCommandHandle b3InitResetDynamicInfo(b3PhysicsClientHandle physClient) diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 482c6f4d4..2d9b16630 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -77,8 +77,9 @@ int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyIndex); ///given a body and joint index, return the joint information. See b3JointInfo in SharedMemoryPublic.h int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointIndex, struct b3JointInfo* info); +b3SharedMemoryCommandHandle b3GetDynamicInfoCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex); ///given a body unique id and link index, return the dynamic information. See b3DynamicInfo in SharedMemoryPublic.h -int b3GetDynamicInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, struct b3DynamicInfo* info); +int b3GetDynamicInfo(b3SharedMemoryStatusHandle statusHandle, struct b3DynamicInfo* info); b3SharedMemoryCommandHandle b3InitResetDynamicInfo(b3PhysicsClientHandle physClient); int b3ResetDynamicInfoSetMass(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double mass); diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index b744a2d0b..86e03c0c9 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/examples/SharedMemory/PhysicsClientExample.cpp @@ -54,8 +54,8 @@ protected: int m_canvasDepthIndex; int m_canvasSegMaskIndex; - float m_lightPos[3]; - float m_specularCoeff; + btScalar m_lightPos[3]; + btScalar m_specularCoeff; void createButton(const char* name, int id, bool isTrigger ); @@ -281,7 +281,11 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId) b3RequestCameraImageSetCameraMatrices(commandHandle, viewMatrix,projectionMatrix); b3RequestCameraImageSetPixelResolution(commandHandle, camVisualizerWidth,camVisualizerHeight); - b3RequestCameraImageSetLightDirection(commandHandle, m_lightPos); + float lightPos[3]; + lightPos[0] = m_lightPos[0]; + lightPos[1] = m_lightPos[1]; + lightPos[2] = m_lightPos[2]; + b3RequestCameraImageSetLightDirection(commandHandle, lightPos); b3RequestCameraImageSetLightSpecularCoeff(commandHandle, m_specularCoeff); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 27262685f..6c8bb1f83 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -1039,6 +1039,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { b3Warning("Removing body failed"); break; } + case CMD_GET_DYNAMIC_INFO_COMPLETED: + { + break; + } + case CMD_GET_DYNAMIC_INFO_FAILED: + { + b3Warning("Request dynamic info failed"); + break; + } default: { b3Error("Unknown server status %d\n", serverCmd.m_type); btAssert(0); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 30672dae5..6190cad6d 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -3955,6 +3955,38 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm break; }; + case CMD_GET_DYNAMIC_INFO: + { + int bodyUniqueId = clientCmd.m_getDynamicInfoArgs.m_bodyUniqueId; + int linkIndex = clientCmd.m_getDynamicInfoArgs.m_linkIndex; + InteralBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); + if (body && body->m_multiBody) + { + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_GET_DYNAMIC_INFO_COMPLETED; + + btMultiBody* mb = body->m_multiBody; + if (linkIndex == -1) + { + serverCmd.m_dynamicInfo.m_mass = mb->getBaseMass(); + serverCmd.m_dynamicInfo.m_lateralFrictionCoeff = mb->getBaseCollider()->getFriction(); + } + else + { + serverCmd.m_dynamicInfo.m_mass = mb->getLinkMass(linkIndex); + serverCmd.m_dynamicInfo.m_lateralFrictionCoeff = mb->getLinkCollider(linkIndex)->getFriction(); + } + hasStatus = true; + } + else + { + b3Warning("The dynamic info requested is not available"); + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_GET_DYNAMIC_INFO_FAILED; + hasStatus = true; + } + break; + } case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS: { BT_PROFILE("CMD_SEND_PHYSICS_SIMULATION_PARAMETERS"); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 5e339430a..b798d896f 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -122,6 +122,12 @@ struct ResetDynamicInfoArgs double m_lateralFriction; }; +struct GetDynamicInfoArgs +{ + int m_bodyUniqueId; + int m_linkIndex; +}; + struct SetJointFeedbackArgs { int m_bodyUniqueId; @@ -739,6 +745,7 @@ struct SharedMemoryCommand struct FileArgs m_fileArguments; struct SdfRequestInfoArgs m_sdfRequestInfoArgs; struct ResetDynamicInfoArgs m_resetDynamicInfoArgs; + struct GetDynamicInfoArgs m_getDynamicInfoArgs; struct InitPoseArgs m_initPoseArgs; struct SendPhysicsSimulationParameters m_physSimParamArgs; struct BulletDataStreamArgs m_dataStreamArguments; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 7602e309e..6a08a944a 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -57,6 +57,7 @@ enum EnumSharedMemoryClientCommand CMD_REQUEST_OPENGL_VISUALIZER_CAMERA, CMD_REMOVE_BODY, CMD_RESET_DYNAMIC_INFO, + CMD_GET_DYNAMIC_INFO, CMD_PROFILE_TIMING, //don't go beyond this command! CMD_MAX_CLIENT_COMMANDS, @@ -141,6 +142,8 @@ enum EnumSharedMemoryServerStatus CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_COMPLETED, CMD_REMOVE_BODY_COMPLETED, CMD_REMOVE_BODY_FAILED, + CMD_GET_DYNAMIC_INFO_COMPLETED, + CMD_GET_DYNAMIC_INFO_FAILED, //don't go beyond 'CMD_MAX_SERVER_COMMANDS! CMD_MAX_SERVER_COMMANDS };