Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge remote-tracking branch 'upstream/master'

Browse Source
This commit is contained in:
yunfeibai
2017-09-27 15:20:11 -07:00
parent 113e103bc2 8e08a9ff16
commit 85be3b43dd
75 changed files with 6253 additions and 767 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
examples/SharedMemory/CMakeLists.txt
View File

@@ -1,5 +1,4 @@
SET(SharedMemory_SRCS
IKTrajectoryHelper.cpp
IKTrajectoryHelper.h
@@ -41,6 +40,7 @@ SET(SharedMemory_SRCS
TinyRendererVisualShapeConverter.h
SharedMemoryCommands.h
SharedMemoryPublic.h
b3PluginManager.cpp
../TinyRenderer/geometry.cpp
../TinyRenderer/model.cpp
../TinyRenderer/tgaimage.cpp

687
examples/SharedMemory/PhysicsClientC_API.cpp
View File

File diff suppressed because it is too large Load Diff

525
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -10,6 +10,14 @@ B3_DECLARE_HANDLE(b3PhysicsClientHandle);
B3_DECLARE_HANDLE(b3SharedMemoryCommandHandle);
B3_DECLARE_HANDLE(b3SharedMemoryStatusHandle);
#ifdef _WIN32
#define B3_SHARED_API __declspec(dllexport)
#elif defined (__GNUC__)
#define B3_SHARED_API __attribute__((visibility("default")))
#else
#define B3_SHARED_API
#endif
///There are several connection methods, see following header files:
#include "PhysicsClientSharedMemory_C_API.h"
@@ -33,30 +41,42 @@ extern "C" {
///b3DisconnectSharedMemory will disconnect the client from the server and cleanup memory.
void b3DisconnectSharedMemory(b3PhysicsClientHandle physClient);
B3_SHARED_API void b3DisconnectSharedMemory(b3PhysicsClientHandle physClient);
///There can only be 1 outstanding command. Check if a command can be send.
int b3CanSubmitCommand(b3PhysicsClientHandle physClient);
B3_SHARED_API int b3CanSubmitCommand(b3PhysicsClientHandle physClient);
///blocking submit command and wait for status
b3SharedMemoryStatusHandle b3SubmitClientCommandAndWaitStatus(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle);
B3_SHARED_API b3SharedMemoryStatusHandle b3SubmitClientCommandAndWaitStatus(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle);
///In general it is better to use b3SubmitClientCommandAndWaitStatus. b3SubmitClientCommand is a non-blocking submit
///command, which requires checking for the status manually, using b3ProcessServerStatus. Also, before sending the
///next command, make sure to check if you can send a command using 'b3CanSubmitCommand'.
int b3SubmitClientCommand(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle);
B3_SHARED_API int b3SubmitClientCommand(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle);
///non-blocking check status
b3SharedMemoryStatusHandle b3ProcessServerStatus(b3PhysicsClientHandle physClient);
B3_SHARED_API b3SharedMemoryStatusHandle b3ProcessServerStatus(b3PhysicsClientHandle physClient);
/// Get the physics server return status type. See EnumSharedMemoryServerStatus in SharedMemoryPublic.h for error codes.
int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle);
B3_SHARED_API int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle);
int b3GetStatusBodyIndices(b3SharedMemoryStatusHandle statusHandle, int* bodyIndicesOut, int bodyIndicesCapacity);
///Plugin system, load and unload a plugin, execute a command
B3_SHARED_API b3SharedMemoryCommandHandle b3CreateCustomCommand(b3PhysicsClientHandle physClient);
B3_SHARED_API void b3CustomCommandLoadPlugin(b3SharedMemoryCommandHandle commandHandle, const char* pluginPath);
B3_SHARED_API int b3GetStatusPluginUniqueId(b3SharedMemoryStatusHandle statusHandle);
B3_SHARED_API int b3GetStatusPluginCommandResult(b3SharedMemoryStatusHandle statusHandle);
int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle);
B3_SHARED_API void b3CustomCommandUnloadPlugin(b3SharedMemoryCommandHandle commandHandle, int pluginUniqueId);
B3_SHARED_API void b3CustomCommandExecutePluginCommand(b3SharedMemoryCommandHandle commandHandle, int pluginUniqueId, const char* textArguments);
B3_SHARED_API void b3CustomCommandExecuteAddIntArgument(b3SharedMemoryCommandHandle commandHandle, int intVal);
B3_SHARED_API void b3CustomCommandExecuteAddFloatArgument(b3SharedMemoryCommandHandle commandHandle, float floatVal);
int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
B3_SHARED_API int b3GetStatusBodyIndices(b3SharedMemoryStatusHandle statusHandle, int* bodyIndicesOut, int bodyIndicesCapacity);
B3_SHARED_API int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle);
B3_SHARED_API int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId,
int* numDegreeOfFreedomQ,
int* numDegreeOfFreedomU,
@@ -66,315 +86,320 @@ int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
const double* jointReactionForces[]);
b3SharedMemoryCommandHandle b3RequestCollisionInfoCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId);
int b3GetStatusAABB(b3SharedMemoryStatusHandle statusHandle, int linkIndex, double aabbMin[3], double aabbMax[3]);
B3_SHARED_API b3SharedMemoryCommandHandle b3RequestCollisionInfoCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId);
B3_SHARED_API int b3GetStatusAABB(b3SharedMemoryStatusHandle statusHandle, int linkIndex, double aabbMin[/*3*/], double aabbMax[/*3*/]);
///If you re-connected to an existing server, or server changed otherwise, sync the body info and user constraints etc.
b3SharedMemoryCommandHandle b3InitSyncBodyInfoCommand(b3PhysicsClientHandle physClient);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitSyncBodyInfoCommand(b3PhysicsClientHandle physClient);
b3SharedMemoryCommandHandle b3InitRemoveBodyCommand(b3PhysicsClientHandle physClient, int bodyUniqueId);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveBodyCommand(b3PhysicsClientHandle physClient, int bodyUniqueId);
///return the total number of bodies in the simulation
int b3GetNumBodies(b3PhysicsClientHandle physClient);
B3_SHARED_API int b3GetNumBodies(b3PhysicsClientHandle physClient);
/// return the body unique id, given the index in range [0 , b3GetNumBodies() )
int b3GetBodyUniqueId(b3PhysicsClientHandle physClient, int serialIndex);
B3_SHARED_API int b3GetBodyUniqueId(b3PhysicsClientHandle physClient, int serialIndex);
///given a body unique id, return the body information. See b3BodyInfo in SharedMemoryPublic.h
int b3GetBodyInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, struct b3BodyInfo* info);
B3_SHARED_API int b3GetBodyInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, struct b3BodyInfo* info);
///give a unique body index (after loading the body) return the number of joints.
int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyIndex);
B3_SHARED_API int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyId);
///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);
B3_SHARED_API int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointIndex, struct b3JointInfo* info);
b3SharedMemoryCommandHandle b3GetDynamicsInfoCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex);
B3_SHARED_API b3SharedMemoryCommandHandle b3GetDynamicsInfoCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex);
///given a body unique id and link index, return the dynamics information. See b3DynamicsInfo in SharedMemoryPublic.h
int b3GetDynamicsInfo(b3SharedMemoryStatusHandle statusHandle, struct b3DynamicsInfo* info);
B3_SHARED_API int b3GetDynamicsInfo(b3SharedMemoryStatusHandle statusHandle, struct b3DynamicsInfo* info);
b3SharedMemoryCommandHandle b3InitChangeDynamicsInfo(b3PhysicsClientHandle physClient);
int b3ChangeDynamicsInfoSetMass(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double mass);
int b3ChangeDynamicsInfoSetLateralFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double lateralFriction);
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);
int b3ChangeDynamicsInfoSetContactStiffnessAndDamping(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex,double contactStiffness, double contactDamping);
int b3ChangeDynamicsInfoSetFrictionAnchor(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex, int frictionAnchor);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitChangeDynamicsInfo(b3PhysicsClientHandle physClient);
B3_SHARED_API int b3ChangeDynamicsInfoSetMass(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double mass);
B3_SHARED_API int b3ChangeDynamicsInfoSetLateralFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double lateralFriction);
B3_SHARED_API int b3ChangeDynamicsInfoSetSpinningFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction);
B3_SHARED_API int b3ChangeDynamicsInfoSetRollingFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction);
B3_SHARED_API int b3ChangeDynamicsInfoSetRestitution(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double restitution);
B3_SHARED_API int b3ChangeDynamicsInfoSetLinearDamping(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId,double linearDamping);
B3_SHARED_API int b3ChangeDynamicsInfoSetAngularDamping(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId,double angularDamping);
B3_SHARED_API int b3ChangeDynamicsInfoSetContactStiffnessAndDamping(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex,double contactStiffness, double contactDamping);
B3_SHARED_API int b3ChangeDynamicsInfoSetFrictionAnchor(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex, int frictionAnchor);
b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info);
///return a unique id for the user constraint, after successful creation, or -1 for an invalid constraint id
int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle statusHandle);
B3_SHARED_API int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle statusHandle);
///change parameters of an existing user constraint
b3SharedMemoryCommandHandle b3InitChangeUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId);
int b3InitChangeUserConstraintSetPivotInB(b3SharedMemoryCommandHandle commandHandle, double jointChildPivot[3]);
int b3InitChangeUserConstraintSetFrameInB(b3SharedMemoryCommandHandle commandHandle, double jointChildFrameOrn[4]);
int b3InitChangeUserConstraintSetMaxForce(b3SharedMemoryCommandHandle commandHandle, double maxAppliedForce);
int b3InitChangeUserConstraintSetGearRatio(b3SharedMemoryCommandHandle commandHandle, double gearRatio);
int b3InitChangeUserConstraintSetGearAuxLink(b3SharedMemoryCommandHandle commandHandle, int gearAuxLink);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitChangeUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId);
B3_SHARED_API int b3InitChangeUserConstraintSetPivotInB(b3SharedMemoryCommandHandle commandHandle, double jointChildPivot[/*3*/]);
B3_SHARED_API int b3InitChangeUserConstraintSetFrameInB(b3SharedMemoryCommandHandle commandHandle, double jointChildFrameOrn[/*4*/]);
B3_SHARED_API int b3InitChangeUserConstraintSetMaxForce(b3SharedMemoryCommandHandle commandHandle, double maxAppliedForce);
B3_SHARED_API int b3InitChangeUserConstraintSetGearRatio(b3SharedMemoryCommandHandle commandHandle, double gearRatio);
B3_SHARED_API int b3InitChangeUserConstraintSetGearAuxLink(b3SharedMemoryCommandHandle commandHandle, int gearAuxLink);
B3_SHARED_API int b3InitChangeUserConstraintSetRelativePositionTarget(b3SharedMemoryCommandHandle commandHandle, double relativePositionTarget);
B3_SHARED_API int b3InitChangeUserConstraintSetERP(b3SharedMemoryCommandHandle commandHandle, double erp);
b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId);
int b3GetNumUserConstraints(b3PhysicsClientHandle physClient);
int b3GetUserConstraintInfo(b3PhysicsClientHandle physClient, int constraintUniqueId, struct b3UserConstraint* info);
B3_SHARED_API int b3GetNumUserConstraints(b3PhysicsClientHandle physClient);
B3_SHARED_API int b3GetUserConstraintInfo(b3PhysicsClientHandle physClient, int constraintUniqueId, struct b3UserConstraint* info);
/// return the user constraint id, given the index in range [0 , b3GetNumUserConstraints() )
int b3GetUserConstraintId(b3PhysicsClientHandle physClient, int serialIndex);
B3_SHARED_API int b3GetUserConstraintId(b3PhysicsClientHandle physClient, int serialIndex);
///Request physics debug lines for debug visualization. The flags in debugMode are the same as used in Bullet
///See btIDebugDraw::DebugDrawModes in Bullet/src/LinearMath/btIDebugDraw.h
b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode);
///Get the pointers to the physics debug line information, after b3InitRequestDebugLinesCommand returns
///status CMD_DEBUG_LINES_COMPLETED
void b3GetDebugLines(b3PhysicsClientHandle physClient, struct b3DebugLines* lines);
B3_SHARED_API void b3GetDebugLines(b3PhysicsClientHandle physClient, struct b3DebugLines* lines);
///configure the 3D OpenGL debug visualizer (enable/disable GUI widgets, shadows, position camera etc)
b3SharedMemoryCommandHandle b3InitConfigureOpenGLVisualizer(b3PhysicsClientHandle physClient);
void b3ConfigureOpenGLVisualizerSetVisualizationFlags(b3SharedMemoryCommandHandle commandHandle, int flag, int enabled);
void b3ConfigureOpenGLVisualizerSetViewMatrix(b3SharedMemoryCommandHandle commandHandle, float cameraDistance, float cameraPitch, float cameraYaw, const float cameraTargetPosition[3]);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitConfigureOpenGLVisualizer(b3PhysicsClientHandle physClient);
B3_SHARED_API void b3ConfigureOpenGLVisualizerSetVisualizationFlags(b3SharedMemoryCommandHandle commandHandle, int flag, int enabled);
B3_SHARED_API void b3ConfigureOpenGLVisualizerSetViewMatrix(b3SharedMemoryCommandHandle commandHandle, float cameraDistance, float cameraPitch, float cameraYaw, const float cameraTargetPosition[/*3*/]);
b3SharedMemoryCommandHandle b3InitRequestOpenGLVisualizerCameraCommand(b3PhysicsClientHandle physClient);
int b3GetStatusOpenGLVisualizerCamera(b3SharedMemoryStatusHandle statusHandle, struct b3OpenGLVisualizerCameraInfo* camera);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestOpenGLVisualizerCameraCommand(b3PhysicsClientHandle physClient);
B3_SHARED_API int b3GetStatusOpenGLVisualizerCamera(b3SharedMemoryStatusHandle statusHandle, struct b3OpenGLVisualizerCameraInfo* camera);
/// Add/remove user-specific debug lines and debug text messages
b3SharedMemoryCommandHandle b3InitUserDebugDrawAddLine3D(b3PhysicsClientHandle physClient, double fromXYZ[3], double toXYZ[3], double colorRGB[3], double lineWidth, double lifeTime);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugDrawAddLine3D(b3PhysicsClientHandle physClient, double fromXYZ[/*3*/], double toXYZ[/*3*/], double colorRGB[/*3*/], double lineWidth, double lifeTime);
b3SharedMemoryCommandHandle b3InitUserDebugDrawAddText3D(b3PhysicsClientHandle physClient, const char* txt, double positionXYZ[3], double colorRGB[3], double textSize, double lifeTime);
void b3UserDebugTextSetOptionFlags(b3SharedMemoryCommandHandle commandHandle, int optionFlags);
void b3UserDebugTextSetOrientation(b3SharedMemoryCommandHandle commandHandle, double orientation[4]);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugDrawAddText3D(b3PhysicsClientHandle physClient, const char* txt, double positionXYZ[/*3*/], double colorRGB[/*3*/], double textSize, double lifeTime);
B3_SHARED_API void b3UserDebugTextSetOptionFlags(b3SharedMemoryCommandHandle commandHandle, int optionFlags);
B3_SHARED_API void b3UserDebugTextSetOrientation(b3SharedMemoryCommandHandle commandHandle, double orientation[/*4*/]);
void b3UserDebugItemSetParentObject(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex);
B3_SHARED_API void b3UserDebugItemSetParentObject(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex);
b3SharedMemoryCommandHandle b3InitUserDebugAddParameter(b3PhysicsClientHandle physClient, const char* txt, double rangeMin, double rangeMax, double startValue);
b3SharedMemoryCommandHandle b3InitUserDebugReadParameter(b3PhysicsClientHandle physClient, int debugItemUniqueId);
int b3GetStatusDebugParameterValue(b3SharedMemoryStatusHandle statusHandle, double* paramValue);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugAddParameter(b3PhysicsClientHandle physClient, const char* txt, double rangeMin, double rangeMax, double startValue);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugReadParameter(b3PhysicsClientHandle physClient, int debugItemUniqueId);
B3_SHARED_API int b3GetStatusDebugParameterValue(b3SharedMemoryStatusHandle statusHandle, double* paramValue);
b3SharedMemoryCommandHandle b3InitUserDebugDrawRemove(b3PhysicsClientHandle physClient, int debugItemUniqueId);
b3SharedMemoryCommandHandle b3InitUserDebugDrawRemoveAll(b3PhysicsClientHandle physClient);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugDrawRemove(b3PhysicsClientHandle physClient, int debugItemUniqueId);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugDrawRemoveAll(b3PhysicsClientHandle physClient);
b3SharedMemoryCommandHandle b3InitDebugDrawingCommand(b3PhysicsClientHandle physClient);
void b3SetDebugObjectColor(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex, double objectColorRGB[3]);
void b3RemoveDebugObjectColor(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitDebugDrawingCommand(b3PhysicsClientHandle physClient);
B3_SHARED_API void b3SetDebugObjectColor(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex, double objectColorRGB[/*3*/]);
B3_SHARED_API void b3RemoveDebugObjectColor(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex);
///All debug items unique Ids are positive: a negative unique Id means failure.
int b3GetDebugItemUniqueId(b3SharedMemoryStatusHandle statusHandle);
B3_SHARED_API int b3GetDebugItemUniqueId(b3SharedMemoryStatusHandle statusHandle);
///request an image from a simulated camera, using a software renderer.
b3SharedMemoryCommandHandle b3InitRequestCameraImage(b3PhysicsClientHandle physClient);
void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle command, float viewMatrix[16], float projectionMatrix[16]);
void b3RequestCameraImageSetPixelResolution(b3SharedMemoryCommandHandle command, int width, int height );
void b3RequestCameraImageSetLightDirection(b3SharedMemoryCommandHandle commandHandle, const float lightDirection[3]);
void b3RequestCameraImageSetLightColor(b3SharedMemoryCommandHandle commandHandle, const float lightColor[3]);
void b3RequestCameraImageSetLightDistance(b3SharedMemoryCommandHandle commandHandle, float lightDistance);
void b3RequestCameraImageSetLightAmbientCoeff(b3SharedMemoryCommandHandle commandHandle, float lightAmbientCoeff);
void b3RequestCameraImageSetLightDiffuseCoeff(b3SharedMemoryCommandHandle commandHandle, float lightDiffuseCoeff);
void b3RequestCameraImageSetLightSpecularCoeff(b3SharedMemoryCommandHandle commandHandle, float lightSpecularCoeff);
void b3RequestCameraImageSetShadow(b3SharedMemoryCommandHandle commandHandle, int hasShadow);
void b3RequestCameraImageSelectRenderer(b3SharedMemoryCommandHandle commandHandle, int renderer);
void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImageData* imageData);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestCameraImage(b3PhysicsClientHandle physClient);
B3_SHARED_API void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle commandHandle, float viewMatrix[/*16*/], float projectionMatrix[/*16*/]);
B3_SHARED_API void b3RequestCameraImageSetPixelResolution(b3SharedMemoryCommandHandle commandHandle, int width, int height );
B3_SHARED_API void b3RequestCameraImageSetLightDirection(b3SharedMemoryCommandHandle commandHandle, const float lightDirection[/*3*/]);
B3_SHARED_API void b3RequestCameraImageSetLightColor(b3SharedMemoryCommandHandle commandHandle, const float lightColor[/*3*/]);
B3_SHARED_API void b3RequestCameraImageSetLightDistance(b3SharedMemoryCommandHandle commandHandle, float lightDistance);
B3_SHARED_API void b3RequestCameraImageSetLightAmbientCoeff(b3SharedMemoryCommandHandle commandHandle, float lightAmbientCoeff);
B3_SHARED_API void b3RequestCameraImageSetLightDiffuseCoeff(b3SharedMemoryCommandHandle commandHandle, float lightDiffuseCoeff);
B3_SHARED_API void b3RequestCameraImageSetLightSpecularCoeff(b3SharedMemoryCommandHandle commandHandle, float lightSpecularCoeff);
B3_SHARED_API void b3RequestCameraImageSetShadow(b3SharedMemoryCommandHandle commandHandle, int hasShadow);
B3_SHARED_API void b3RequestCameraImageSelectRenderer(b3SharedMemoryCommandHandle commandHandle, int renderer);
B3_SHARED_API void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImageData* imageData);
///compute a view matrix, helper function for b3RequestCameraImageSetCameraMatrices
void b3ComputeViewMatrixFromPositions(const float cameraPosition[3], const float cameraTargetPosition[3], const float cameraUp[3], float viewMatrix[16]);
void b3ComputeViewMatrixFromYawPitchRoll(const float cameraTargetPosition[3], float distance, float yaw, float pitch, float roll, int upAxis, float viewMatrix[16]);
void b3ComputePositionFromViewMatrix(const float viewMatrix[16], float cameraPosition[3], float cameraTargetPosition[3], float cameraUp[3]);
B3_SHARED_API void b3ComputeViewMatrixFromPositions(const float cameraPosition[/*3*/], const float cameraTargetPosition[/*3*/], const float cameraUp[/*3*/], float viewMatrix[/*16*/]);
B3_SHARED_API void b3ComputeViewMatrixFromYawPitchRoll(const float cameraTargetPosition[/*3*/], float distance, float yaw, float pitch, float roll, int upAxis, float viewMatrix[/*16*/]);
B3_SHARED_API void b3ComputePositionFromViewMatrix(const float viewMatrix[/*16*/], float cameraPosition[/*3*/], float cameraTargetPosition[/*3*/], float cameraUp[/*3*/]);
///compute a projection matrix, helper function for b3RequestCameraImageSetCameraMatrices
void b3ComputeProjectionMatrix(float left, float right, float bottom, float top, float nearVal, float farVal, float projectionMatrix[16]);
void b3ComputeProjectionMatrixFOV(float fov, float aspect, float nearVal, float farVal, float projectionMatrix[16]);
B3_SHARED_API void b3ComputeProjectionMatrix(float left, float right, float bottom, float top, float nearVal, float farVal, float projectionMatrix[/*16*/]);
B3_SHARED_API void b3ComputeProjectionMatrixFOV(float fov, float aspect, float nearVal, float farVal, float projectionMatrix[/*16*/]);
/* obsolete, please use b3ComputeViewProjectionMatrices */
void b3RequestCameraImageSetViewMatrix(b3SharedMemoryCommandHandle command, const float cameraPosition[3], const float cameraTargetPosition[3], const float cameraUp[3]);
B3_SHARED_API void b3RequestCameraImageSetViewMatrix(b3SharedMemoryCommandHandle commandHandle, const float cameraPosition[/*3*/], const float cameraTargetPosition[/*3*/], const float cameraUp[/*3*/]);
/* obsolete, please use b3ComputeViewProjectionMatrices */
void b3RequestCameraImageSetViewMatrix2(b3SharedMemoryCommandHandle commandHandle, const float cameraTargetPosition[3], float distance, float yaw, float pitch, float roll, int upAxis);
B3_SHARED_API void b3RequestCameraImageSetViewMatrix2(b3SharedMemoryCommandHandle commandHandle, const float cameraTargetPosition[/*3*/], float distance, float yaw, float pitch, float roll, int upAxis);
/* obsolete, please use b3ComputeViewProjectionMatrices */
void b3RequestCameraImageSetProjectionMatrix(b3SharedMemoryCommandHandle command, float left, float right, float bottom, float top, float nearVal, float farVal);
B3_SHARED_API void b3RequestCameraImageSetProjectionMatrix(b3SharedMemoryCommandHandle commandHandle, float left, float right, float bottom, float top, float nearVal, float farVal);
/* obsolete, please use b3ComputeViewProjectionMatrices */
void b3RequestCameraImageSetFOVProjectionMatrix(b3SharedMemoryCommandHandle command, float fov, float aspect, float nearVal, float farVal);
B3_SHARED_API void b3RequestCameraImageSetFOVProjectionMatrix(b3SharedMemoryCommandHandle commandHandle, float fov, float aspect, float nearVal, float farVal);
///request an contact point information
b3SharedMemoryCommandHandle b3InitRequestContactPointInformation(b3PhysicsClientHandle physClient);
void b3SetContactFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA);
void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB);
void b3SetContactFilterLinkA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA);
void b3SetContactFilterLinkB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB);
void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointInfo);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestContactPointInformation(b3PhysicsClientHandle physClient);
B3_SHARED_API void b3SetContactFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA);
B3_SHARED_API void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB);
B3_SHARED_API void b3SetContactFilterLinkA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA);
B3_SHARED_API void b3SetContactFilterLinkB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB);
B3_SHARED_API void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointData);
///compute the closest points between two bodies
b3SharedMemoryCommandHandle b3InitClosestDistanceQuery(b3PhysicsClientHandle physClient);
void b3SetClosestDistanceFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA);
void b3SetClosestDistanceFilterLinkA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA);
void b3SetClosestDistanceFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB);
void b3SetClosestDistanceFilterLinkB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB);
void b3SetClosestDistanceThreshold(b3SharedMemoryCommandHandle commandHandle, double distance);
void b3GetClosestPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointInfo);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitClosestDistanceQuery(b3PhysicsClientHandle physClient);
B3_SHARED_API void b3SetClosestDistanceFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA);
B3_SHARED_API void b3SetClosestDistanceFilterLinkA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA);
B3_SHARED_API void b3SetClosestDistanceFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB);
B3_SHARED_API void b3SetClosestDistanceFilterLinkB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB);
B3_SHARED_API void b3SetClosestDistanceThreshold(b3SharedMemoryCommandHandle commandHandle, double distance);
B3_SHARED_API void b3GetClosestPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointInfo);
///get all the bodies that touch a given axis aligned bounding box specified in world space (min and max coordinates)
b3SharedMemoryCommandHandle b3InitAABBOverlapQuery(b3PhysicsClientHandle physClient, const double aabbMin[3],const double aabbMax[3]);
void b3GetAABBOverlapResults(b3PhysicsClientHandle physClient, struct b3AABBOverlapData* data);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitAABBOverlapQuery(b3PhysicsClientHandle physClient, const double aabbMin[/*3*/],const double aabbMax[/*3*/]);
B3_SHARED_API void b3GetAABBOverlapResults(b3PhysicsClientHandle physClient, struct b3AABBOverlapData* data);
//request visual shape information
b3SharedMemoryCommandHandle b3InitRequestVisualShapeInformation(b3PhysicsClientHandle physClient, int bodyUniqueIdA);
void b3GetVisualShapeInformation(b3PhysicsClientHandle physClient, struct b3VisualShapeInformation* visualShapeInfo);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestVisualShapeInformation(b3PhysicsClientHandle physClient, int bodyUniqueIdA);
B3_SHARED_API void b3GetVisualShapeInformation(b3PhysicsClientHandle physClient, struct b3VisualShapeInformation* visualShapeInfo);
b3SharedMemoryCommandHandle b3InitLoadTexture(b3PhysicsClientHandle physClient, const char* filename);
int b3GetStatusTextureUniqueId(b3SharedMemoryStatusHandle statusHandle);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitLoadTexture(b3PhysicsClientHandle physClient, const char* filename);
B3_SHARED_API int b3GetStatusTextureUniqueId(b3SharedMemoryStatusHandle statusHandle);
b3SharedMemoryCommandHandle b3CreateChangeTextureCommandInit(b3PhysicsClientHandle physClient, int textureUniqueId, int width, int height, const char* rgbPixels);
B3_SHARED_API b3SharedMemoryCommandHandle b3CreateChangeTextureCommandInit(b3PhysicsClientHandle physClient, int textureUniqueId, int width, int height, const char* rgbPixels);
b3SharedMemoryCommandHandle b3InitUpdateVisualShape(b3PhysicsClientHandle physClient, int bodyUniqueId, int jointIndex, int shapeIndex, int textureUniqueId);
void b3UpdateVisualShapeRGBAColor(b3SharedMemoryCommandHandle commandHandle, double rgbaColor[4]);
void b3UpdateVisualShapeSpecularColor(b3SharedMemoryCommandHandle commandHandle, double specularColor[3]);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitUpdateVisualShape(b3PhysicsClientHandle physClient, int bodyUniqueId, int jointIndex, int shapeIndex, int textureUniqueId);
B3_SHARED_API void b3UpdateVisualShapeRGBAColor(b3SharedMemoryCommandHandle commandHandle, double rgbaColor[/*4*/]);
B3_SHARED_API void b3UpdateVisualShapeSpecularColor(b3SharedMemoryCommandHandle commandHandle, double specularColor[/*3*/]);
b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient);
int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx,double gravy, double gravz);
int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double timeStep);
int b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultContactERP);
int b3PhysicsParamSetDefaultNonContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultNonContactERP);
int b3PhysicsParamSetDefaultFrictionERP(b3SharedMemoryCommandHandle commandHandle, double frictionERP);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient);
B3_SHARED_API int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx,double gravy, double gravz);
B3_SHARED_API int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double timeStep);
B3_SHARED_API int b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultContactERP);
B3_SHARED_API int b3PhysicsParamSetDefaultNonContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultNonContactERP);
B3_SHARED_API int b3PhysicsParamSetDefaultFrictionERP(b3SharedMemoryCommandHandle commandHandle, double frictionERP);
int b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int numSubSteps);
int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation);
int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations);
int b3PhysicsParamSetCollisionFilterMode(b3SharedMemoryCommandHandle commandHandle, int filterMode);
B3_SHARED_API int b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int numSubSteps);
B3_SHARED_API int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation);
B3_SHARED_API int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations);
B3_SHARED_API int b3PhysicsParamSetCollisionFilterMode(b3SharedMemoryCommandHandle commandHandle, int filterMode);
int b3PhysicsParamSetUseSplitImpulse(b3SharedMemoryCommandHandle commandHandle, int useSplitImpulse);
int b3PhysicsParamSetSplitImpulsePenetrationThreshold(b3SharedMemoryCommandHandle commandHandle, double splitImpulsePenetrationThreshold);
int b3PhysicsParamSetContactBreakingThreshold(b3SharedMemoryCommandHandle commandHandle, double contactBreakingThreshold);
int b3PhysicsParamSetMaxNumCommandsPer1ms(b3SharedMemoryCommandHandle commandHandle, int maxNumCmdPer1ms);
int b3PhysicsParamSetEnableFileCaching(b3SharedMemoryCommandHandle commandHandle, int enableFileCaching);
int b3PhysicsParamSetRestitutionVelocityThreshold(b3SharedMemoryCommandHandle commandHandle, double restitutionVelocityThreshold);
B3_SHARED_API int b3PhysicsParamSetUseSplitImpulse(b3SharedMemoryCommandHandle commandHandle, int useSplitImpulse);
B3_SHARED_API int b3PhysicsParamSetSplitImpulsePenetrationThreshold(b3SharedMemoryCommandHandle commandHandle, double splitImpulsePenetrationThreshold);
B3_SHARED_API int b3PhysicsParamSetContactBreakingThreshold(b3SharedMemoryCommandHandle commandHandle, double contactBreakingThreshold);
B3_SHARED_API int b3PhysicsParamSetMaxNumCommandsPer1ms(b3SharedMemoryCommandHandle commandHandle, int maxNumCmdPer1ms);
B3_SHARED_API int b3PhysicsParamSetEnableFileCaching(b3SharedMemoryCommandHandle commandHandle, int enableFileCaching);
B3_SHARED_API int b3PhysicsParamSetRestitutionVelocityThreshold(b3SharedMemoryCommandHandle commandHandle, double restitutionVelocityThreshold);
//b3PhysicsParamSetInternalSimFlags is for internal/temporary/easter-egg/experimental demo purposes
//Use at own risk: magic things may or my not happen when calling this API
int b3PhysicsParamSetInternalSimFlags(b3SharedMemoryCommandHandle commandHandle, int flags);
B3_SHARED_API int b3PhysicsParamSetInternalSimFlags(b3SharedMemoryCommandHandle commandHandle, int flags);
b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle physClient);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle physClient);
b3SharedMemoryCommandHandle b3InitResetSimulationCommand(b3PhysicsClientHandle physClient);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitResetSimulationCommand(b3PhysicsClientHandle physClient);
///Load a robot from a URDF file. Status type will CMD_URDF_LOADING_COMPLETED.
///Access the robot from the unique body index, through b3GetStatusBodyIndex(statusHandle);
b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName);
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName);
int b3LoadUrdfCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ);
int b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW);
int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase);
int b3LoadUrdfCommandSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags);
int b3LoadUrdfCommandSetGlobalScaling(b3SharedMemoryCommandHandle commandHandle, double globalScaling);
B3_SHARED_API int b3LoadUrdfCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ);
B3_SHARED_API int b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW);
B3_SHARED_API int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
B3_SHARED_API int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase);
B3_SHARED_API int b3LoadUrdfCommandSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags);
B3_SHARED_API int b3LoadUrdfCommandSetGlobalScaling(b3SharedMemoryCommandHandle commandHandle, double globalScaling);
b3SharedMemoryCommandHandle b3LoadBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
b3SharedMemoryCommandHandle b3SaveBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
b3SharedMemoryCommandHandle b3LoadMJCFCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
void b3LoadMJCFCommandSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags);
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
B3_SHARED_API b3SharedMemoryCommandHandle b3SaveBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadMJCFCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
B3_SHARED_API void b3LoadMJCFCommandSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags);
///compute the forces to achieve an acceleration, given a state q and qdot using inverse dynamics
b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex,
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex,
const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations);
int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandle,
B3_SHARED_API int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId,
int* dofCount,
double* jointForces);
b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, const double* localPosition, const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations);
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, const double* localPosition, const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations);
int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, double* linearJacobian, double* angularJacobian);
B3_SHARED_API int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle,
int* dofCount,
double* linearJacobian,
double* angularJacobian);
///compute the joint positions to move the end effector to a desired target using inverse kinematics
b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex);
void b3CalculateInverseKinematicsAddTargetPurePosition(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[3]);
void b3CalculateInverseKinematicsAddTargetPositionWithOrientation(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[3], const double targetOrientation[4]);
void b3CalculateInverseKinematicsPosWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[3], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose);
void b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[3], const double targetOrientation[4], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose);
void b3CalculateInverseKinematicsSetJointDamping(b3SharedMemoryCommandHandle commandHandle, int numDof, const double* jointDampingCoeff);
int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle,
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex);
B3_SHARED_API void b3CalculateInverseKinematicsAddTargetPurePosition(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[/*3*/]);
B3_SHARED_API void b3CalculateInverseKinematicsAddTargetPositionWithOrientation(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[/*3*/], const double targetOrientation[/*4*/]);
B3_SHARED_API void b3CalculateInverseKinematicsPosWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[/*3*/], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose);
B3_SHARED_API void b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[/*3*/], const double targetOrientation[/*4*/], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose);
B3_SHARED_API void b3CalculateInverseKinematicsSetJointDamping(b3SharedMemoryCommandHandle commandHandle, int numDof, const double* jointDampingCoeff);
B3_SHARED_API int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId,
int* dofCount,
double* jointPositions);
b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
int b3LoadSdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
int b3LoadSdfCommandSetUseGlobalScaling(b3SharedMemoryCommandHandle commandHandle, double globalScaling);
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
B3_SHARED_API int b3LoadSdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
B3_SHARED_API int b3LoadSdfCommandSetUseGlobalScaling(b3SharedMemoryCommandHandle commandHandle, double globalScaling);
b3SharedMemoryCommandHandle b3SaveWorldCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
B3_SHARED_API b3SharedMemoryCommandHandle b3SaveWorldCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
///The b3JointControlCommandInit method is obsolete, use b3JointControlCommandInit2 instead
b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode);
B3_SHARED_API b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode);
///Set joint motor control variables such as desired position/angle, desired velocity,
///applied joint forces, dependent on the control mode (CONTROL_MODE_VELOCITY or CONTROL_MODE_TORQUE)
b3SharedMemoryCommandHandle b3JointControlCommandInit2(b3PhysicsClientHandle physClient, int bodyUniqueId, int controlMode);
B3_SHARED_API b3SharedMemoryCommandHandle b3JointControlCommandInit2(b3PhysicsClientHandle physClient, int bodyUniqueId, int controlMode);
///Only use when controlMode is CONTROL_MODE_POSITION_VELOCITY_PD
int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, int qIndex, double value);
int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
B3_SHARED_API int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, int qIndex, double value);
B3_SHARED_API int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
B3_SHARED_API int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
///Only use when controlMode is CONTROL_MODE_VELOCITY
int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); /* find a better name for dof/q/u indices, point to b3JointInfo */
int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
B3_SHARED_API int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); /* find a better name for dof/q/u indices, point to b3JointInfo */
B3_SHARED_API int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
///Only use if when controlMode is CONTROL_MODE_TORQUE,
int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
B3_SHARED_API int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
///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);
int b3CreateCollisionShapeAddSphere(b3SharedMemoryCommandHandle commandHandle,double radius);
int b3CreateCollisionShapeAddBox(b3SharedMemoryCommandHandle commandHandle,double halfExtents[3]);
int b3CreateCollisionShapeAddCapsule(b3SharedMemoryCommandHandle commandHandle,double radius, double height);
int b3CreateCollisionShapeAddCylinder(b3SharedMemoryCommandHandle commandHandle,double radius, double height);
int b3CreateCollisionShapeAddPlane(b3SharedMemoryCommandHandle commandHandle, double planeNormal[3], double planeConstant);
int b3CreateCollisionShapeAddMesh(b3SharedMemoryCommandHandle commandHandle,const char* fileName, double meshScale[3]);
void b3CreateCollisionSetFlag(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, int flags);
B3_SHARED_API b3SharedMemoryCommandHandle b3CreateCollisionShapeCommandInit(b3PhysicsClientHandle physClient);
B3_SHARED_API int b3CreateCollisionShapeAddSphere(b3SharedMemoryCommandHandle commandHandle,double radius);
B3_SHARED_API int b3CreateCollisionShapeAddBox(b3SharedMemoryCommandHandle commandHandle,double halfExtents[/*3*/]);
B3_SHARED_API int b3CreateCollisionShapeAddCapsule(b3SharedMemoryCommandHandle commandHandle,double radius, double height);
B3_SHARED_API int b3CreateCollisionShapeAddCylinder(b3SharedMemoryCommandHandle commandHandle,double radius, double height);
B3_SHARED_API int b3CreateCollisionShapeAddPlane(b3SharedMemoryCommandHandle commandHandle, double planeNormal[/*3*/], double planeConstant);
B3_SHARED_API int b3CreateCollisionShapeAddMesh(b3SharedMemoryCommandHandle commandHandle,const char* fileName, double meshScale[/*3*/]);
B3_SHARED_API void b3CreateCollisionSetFlag(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, int flags);
void b3CreateCollisionShapeSetChildTransform(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, double childPosition[3], double childOrientation[4]);
B3_SHARED_API void b3CreateCollisionShapeSetChildTransform(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, double childPosition[/*3*/], double childOrientation[/*4*/]);
int b3GetStatusCollisionShapeUniqueId(b3SharedMemoryStatusHandle statusHandle);
B3_SHARED_API int b3GetStatusCollisionShapeUniqueId(b3SharedMemoryStatusHandle statusHandle);
b3SharedMemoryCommandHandle b3CreateVisualShapeCommandInit(b3PhysicsClientHandle physClient);
int b3GetStatusVisualShapeUniqueId(b3SharedMemoryStatusHandle statusHandle);
B3_SHARED_API b3SharedMemoryCommandHandle b3CreateVisualShapeCommandInit(b3PhysicsClientHandle physClient);
B3_SHARED_API int b3GetStatusVisualShapeUniqueId(b3SharedMemoryStatusHandle statusHandle);
b3SharedMemoryCommandHandle b3CreateMultiBodyCommandInit(b3PhysicsClientHandle physClient);
int b3CreateMultiBodyBase(b3SharedMemoryCommandHandle commandHandle, double mass, int collisionShapeUnique, int visualShapeUniqueId, double basePosition[3], double baseOrientation[4] , double baseInertialFramePosition[3], double baseInertialFrameOrientation[4]);
B3_SHARED_API b3SharedMemoryCommandHandle b3CreateMultiBodyCommandInit(b3PhysicsClientHandle physClient);
B3_SHARED_API int b3CreateMultiBodyBase(b3SharedMemoryCommandHandle commandHandle, double mass, int collisionShapeUnique, int visualShapeUniqueId, double basePosition[/*3*/], double baseOrientation[/*4*/] , double baseInertialFramePosition[/*3*/], double baseInertialFrameOrientation[/*4*/]);
int b3CreateMultiBodyLink(b3SharedMemoryCommandHandle commandHandle, double linkMass, double linkCollisionShapeIndex,
B3_SHARED_API int b3CreateMultiBodyLink(b3SharedMemoryCommandHandle commandHandle, double linkMass, double linkCollisionShapeIndex,
double linkVisualShapeIndex,
double linkPosition[3],
double linkOrientation[4],
double linkInertialFramePosition[3],
double linkInertialFrameOrientation[4],
double linkPosition[/*3*/],
double linkOrientation[/*4*/],
double linkInertialFramePosition[/*3*/],
double linkInertialFrameOrientation[/*4*/],
int linkParentIndex,
int linkJointType,
double linkJointAxis[3]);
double linkJointAxis[/*3*/]);
//useMaximalCoordinates are disabled by default, enabling them is experimental and not fully supported yet
void b3CreateMultiBodyUseMaximalCoordinates(b3SharedMemoryCommandHandle commandHandle);
B3_SHARED_API void b3CreateMultiBodyUseMaximalCoordinates(b3SharedMemoryCommandHandle commandHandle);
//int b3CreateMultiBodyAddLink(b3SharedMemoryCommandHandle commandHandle, int jointType, int parentLinkIndex, double linkMass, int linkCollisionShapeUnique, int linkVisualShapeUniqueId);
@@ -382,119 +407,121 @@ void b3CreateMultiBodyUseMaximalCoordinates(b3SharedMemoryCommandHandle commandH
///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);
int b3CreateBoxCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ);
int b3CreateBoxCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW);
int b3CreateBoxCommandSetHalfExtents(b3SharedMemoryCommandHandle commandHandle, double halfExtentsX,double halfExtentsY,double halfExtentsZ);
int b3CreateBoxCommandSetMass(b3SharedMemoryCommandHandle commandHandle, double mass);
int b3CreateBoxCommandSetCollisionShapeType(b3SharedMemoryCommandHandle commandHandle, int collisionShapeType);
int b3CreateBoxCommandSetColorRGBA(b3SharedMemoryCommandHandle commandHandle, double red,double green,double blue, double alpha);
B3_SHARED_API b3SharedMemoryCommandHandle b3CreateBoxShapeCommandInit(b3PhysicsClientHandle physClient);
B3_SHARED_API int b3CreateBoxCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ);
B3_SHARED_API int b3CreateBoxCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW);
B3_SHARED_API int b3CreateBoxCommandSetHalfExtents(b3SharedMemoryCommandHandle commandHandle, double halfExtentsX,double halfExtentsY,double halfExtentsZ);
B3_SHARED_API int b3CreateBoxCommandSetMass(b3SharedMemoryCommandHandle commandHandle, double mass);
B3_SHARED_API int b3CreateBoxCommandSetCollisionShapeType(b3SharedMemoryCommandHandle commandHandle, int collisionShapeType);
B3_SHARED_API int b3CreateBoxCommandSetColorRGBA(b3SharedMemoryCommandHandle commandHandle, double red,double green,double blue, double alpha);
///b3CreatePoseCommandInit will initialize (teleport) the pose of a body/robot. You can individually set the base position,
///base orientation and joint angles. This will set all velocities of base and joints to zero.
///This is not a robot control command using actuators/joint motors, but manual repositioning the robot.
b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyIndex);
int b3CreatePoseCommandSetBasePosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ);
int b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW);
int b3CreatePoseCommandSetBaseLinearVelocity(b3SharedMemoryCommandHandle commandHandle, double linVel[3]);
int b3CreatePoseCommandSetBaseAngularVelocity(b3SharedMemoryCommandHandle commandHandle, double angVel[3]);
B3_SHARED_API b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyIndex);
B3_SHARED_API int b3CreatePoseCommandSetBasePosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ);
B3_SHARED_API int b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW);
B3_SHARED_API int b3CreatePoseCommandSetBaseLinearVelocity(b3SharedMemoryCommandHandle commandHandle, double linVel[/*3*/]);
B3_SHARED_API int b3CreatePoseCommandSetBaseAngularVelocity(b3SharedMemoryCommandHandle commandHandle, double angVel[/*3*/]);
int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHandle, int numJointPositions, const double* jointPositions);
int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointPosition);
B3_SHARED_API int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHandle, int numJointPositions, const double* jointPositions);
B3_SHARED_API int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointPosition);
int b3CreatePoseCommandSetJointVelocities(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int numJointVelocities, const double* jointVelocities);
int b3CreatePoseCommandSetJointVelocity(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointVelocity);
B3_SHARED_API int b3CreatePoseCommandSetJointVelocities(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int numJointVelocities, const double* jointVelocities);
B3_SHARED_API int b3CreatePoseCommandSetJointVelocity(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointVelocity);
///We are currently not reading the sensor information from the URDF file, and programmatically assign sensors.
///This is rather inconsistent, to mix programmatical creation with loading from file.
b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId);
int b3CreateSensorEnable6DofJointForceTorqueSensor(b3SharedMemoryCommandHandle commandHandle, int jointIndex, int enable);
B3_SHARED_API b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId);
B3_SHARED_API int b3CreateSensorEnable6DofJointForceTorqueSensor(b3SharedMemoryCommandHandle commandHandle, int jointIndex, int enable);
///b3CreateSensorEnableIMUForLink is not implemented yet.
///For now, if the IMU is located in the root link, use the root world transform to mimic an IMU.
int b3CreateSensorEnableIMUForLink(b3SharedMemoryCommandHandle commandHandle, int linkIndex, int enable);
B3_SHARED_API int b3CreateSensorEnableIMUForLink(b3SharedMemoryCommandHandle commandHandle, int linkIndex, int enable);
b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient,int bodyUniqueId);
int b3RequestActualStateCommandComputeLinkVelocity(b3SharedMemoryCommandHandle commandHandle, int computeLinkVelocity);
B3_SHARED_API b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient,int bodyUniqueId);
B3_SHARED_API int b3RequestActualStateCommandComputeLinkVelocity(b3SharedMemoryCommandHandle commandHandle, int computeLinkVelocity);
B3_SHARED_API int b3RequestActualStateCommandComputeForwardKinematics(b3SharedMemoryCommandHandle commandHandle, int computeForwardKinematics);
int b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, struct b3JointSensorState *state);
int b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int linkIndex, struct b3LinkState *state);
b3SharedMemoryCommandHandle b3PickBody(b3PhysicsClientHandle physClient, double rayFromWorldX,
B3_SHARED_API int b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, struct b3JointSensorState *state);
B3_SHARED_API int b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int linkIndex, struct b3LinkState *state);
B3_SHARED_API b3SharedMemoryCommandHandle b3PickBody(b3PhysicsClientHandle physClient, double rayFromWorldX,
double rayFromWorldY, double rayFromWorldZ,
double rayToWorldX, double rayToWorldY, double rayToWorldZ);
b3SharedMemoryCommandHandle b3MovePickedBody(b3PhysicsClientHandle physClient, double rayFromWorldX,
B3_SHARED_API b3SharedMemoryCommandHandle b3MovePickedBody(b3PhysicsClientHandle physClient, double rayFromWorldX,
double rayFromWorldY, double rayFromWorldZ,
double rayToWorldX, double rayToWorldY,
double rayToWorldZ);
b3SharedMemoryCommandHandle b3RemovePickingConstraint(b3PhysicsClientHandle physClient);
B3_SHARED_API b3SharedMemoryCommandHandle b3RemovePickingConstraint(b3PhysicsClientHandle physClient);
b3SharedMemoryCommandHandle b3CreateRaycastCommandInit(b3PhysicsClientHandle physClient, double rayFromWorldX,
B3_SHARED_API b3SharedMemoryCommandHandle b3CreateRaycastCommandInit(b3PhysicsClientHandle physClient, double rayFromWorldX,
double rayFromWorldY, double rayFromWorldZ,
double rayToWorldX, double rayToWorldY, double rayToWorldZ);
b3SharedMemoryCommandHandle b3CreateRaycastBatchCommandInit(b3PhysicsClientHandle physClient);
void b3RaycastBatchAddRay(b3SharedMemoryCommandHandle commandHandle, const double rayFromWorld[3], const double rayToWorld[3]);
B3_SHARED_API b3SharedMemoryCommandHandle b3CreateRaycastBatchCommandInit(b3PhysicsClientHandle physClient);
B3_SHARED_API void b3RaycastBatchAddRay(b3SharedMemoryCommandHandle commandHandle, const double rayFromWorld[/*3*/], const double rayToWorld[/*3*/]);
void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastInformation* raycastInfo);
B3_SHARED_API void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastInformation* raycastInfo);
/// Apply external force at the body (or link) center of mass, in world space/Cartesian coordinates.
b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient);
void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double force[3], const double position[3], int flags);
void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double torque[3], int flags);
B3_SHARED_API b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient);
B3_SHARED_API void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double force[/*3*/], const double position[/*3*/], int flag);
B3_SHARED_API void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double torque[/*3*/], int flag);
///experiments of robots interacting with non-rigid objects (such as btSoftBody)
b3SharedMemoryCommandHandle b3LoadBunnyCommandInit(b3PhysicsClientHandle physClient);
int b3LoadBunnySetScale(b3SharedMemoryCommandHandle commandHandle, double scale);
int b3LoadBunnySetMass(b3SharedMemoryCommandHandle commandHandle, double mass);
int b3LoadBunnySetCollisionMargin(b3SharedMemoryCommandHandle commandHandle, double collisionMargin);
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadBunnyCommandInit(b3PhysicsClientHandle physClient);
B3_SHARED_API int b3LoadBunnySetScale(b3SharedMemoryCommandHandle commandHandle, double scale);
B3_SHARED_API int b3LoadBunnySetMass(b3SharedMemoryCommandHandle commandHandle, double mass);
B3_SHARED_API int b3LoadBunnySetCollisionMargin(b3SharedMemoryCommandHandle commandHandle, double collisionMargin);
b3SharedMemoryCommandHandle b3RequestVREventsCommandInit(b3PhysicsClientHandle physClient);
void b3VREventsSetDeviceTypeFilter(b3SharedMemoryCommandHandle commandHandle, int deviceTypeFilter);
B3_SHARED_API b3SharedMemoryCommandHandle b3RequestVREventsCommandInit(b3PhysicsClientHandle physClient);
B3_SHARED_API void b3VREventsSetDeviceTypeFilter(b3SharedMemoryCommandHandle commandHandle, int deviceTypeFilter);
void b3GetVREventsData(b3PhysicsClientHandle physClient, struct b3VREventsData* vrEventsData);
B3_SHARED_API void b3GetVREventsData(b3PhysicsClientHandle physClient, struct b3VREventsData* vrEventsData);
b3SharedMemoryCommandHandle b3SetVRCameraStateCommandInit(b3PhysicsClientHandle physClient);
int b3SetVRCameraRootPosition(b3SharedMemoryCommandHandle commandHandle, double rootPos[3]);
int b3SetVRCameraRootOrientation(b3SharedMemoryCommandHandle commandHandle, double rootOrn[4]);
int b3SetVRCameraTrackingObject(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId);
int b3SetVRCameraTrackingObjectFlag(b3SharedMemoryCommandHandle commandHandle, int flag);
B3_SHARED_API b3SharedMemoryCommandHandle b3SetVRCameraStateCommandInit(b3PhysicsClientHandle physClient);
B3_SHARED_API int b3SetVRCameraRootPosition(b3SharedMemoryCommandHandle commandHandle, double rootPos[/*3*/]);
B3_SHARED_API int b3SetVRCameraRootOrientation(b3SharedMemoryCommandHandle commandHandle, double rootOrn[/*4*/]);
B3_SHARED_API int b3SetVRCameraTrackingObject(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId);
B3_SHARED_API int b3SetVRCameraTrackingObjectFlag(b3SharedMemoryCommandHandle commandHandle, int flag);
b3SharedMemoryCommandHandle b3RequestKeyboardEventsCommandInit(b3PhysicsClientHandle physClient);
void b3GetKeyboardEventsData(b3PhysicsClientHandle physClient, struct b3KeyboardEventsData* keyboardEventsData);
B3_SHARED_API b3SharedMemoryCommandHandle b3RequestKeyboardEventsCommandInit(b3PhysicsClientHandle physClient);
B3_SHARED_API void b3GetKeyboardEventsData(b3PhysicsClientHandle physClient, struct b3KeyboardEventsData* keyboardEventsData);
b3SharedMemoryCommandHandle b3RequestMouseEventsCommandInit(b3PhysicsClientHandle physClient);
void b3GetMouseEventsData(b3PhysicsClientHandle physClient, struct b3MouseEventsData* mouseEventsData);
B3_SHARED_API b3SharedMemoryCommandHandle b3RequestMouseEventsCommandInit(b3PhysicsClientHandle physClient);
B3_SHARED_API void b3GetMouseEventsData(b3PhysicsClientHandle physClient, struct b3MouseEventsData* mouseEventsData);
b3SharedMemoryCommandHandle b3StateLoggingCommandInit(b3PhysicsClientHandle physClient);
int b3StateLoggingStart(b3SharedMemoryCommandHandle commandHandle, int loggingType, const char* fileName);
int b3StateLoggingAddLoggingObjectUniqueId(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId);
int b3StateLoggingSetMaxLogDof(b3SharedMemoryCommandHandle commandHandle, int maxLogDof);
int b3StateLoggingSetLinkIndexA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA);
int b3StateLoggingSetLinkIndexB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB);
int b3StateLoggingSetBodyAUniqueId(b3SharedMemoryCommandHandle commandHandle, int bodyAUniqueId);
int b3StateLoggingSetBodyBUniqueId(b3SharedMemoryCommandHandle commandHandle, int bodyBUniqueId);
int b3StateLoggingSetDeviceTypeFilter(b3SharedMemoryCommandHandle commandHandle, int deviceTypeFilter);
int b3StateLoggingSetLogFlags(b3SharedMemoryCommandHandle commandHandle, int logFlags);
B3_SHARED_API b3SharedMemoryCommandHandle b3StateLoggingCommandInit(b3PhysicsClientHandle physClient);
B3_SHARED_API int b3StateLoggingStart(b3SharedMemoryCommandHandle commandHandle, int loggingType, const char* fileName);
B3_SHARED_API int b3StateLoggingAddLoggingObjectUniqueId(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId);
B3_SHARED_API int b3StateLoggingSetMaxLogDof(b3SharedMemoryCommandHandle commandHandle, int maxLogDof);
B3_SHARED_API int b3StateLoggingSetLinkIndexA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA);
B3_SHARED_API int b3StateLoggingSetLinkIndexB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB);
B3_SHARED_API int b3StateLoggingSetBodyAUniqueId(b3SharedMemoryCommandHandle commandHandle, int bodyAUniqueId);
B3_SHARED_API int b3StateLoggingSetBodyBUniqueId(b3SharedMemoryCommandHandle commandHandle, int bodyBUniqueId);
B3_SHARED_API int b3StateLoggingSetDeviceTypeFilter(b3SharedMemoryCommandHandle commandHandle, int deviceTypeFilter);
B3_SHARED_API int b3StateLoggingSetLogFlags(b3SharedMemoryCommandHandle commandHandle, int logFlags);
int b3GetStatusLoggingUniqueId(b3SharedMemoryStatusHandle statusHandle);
int b3StateLoggingStop(b3SharedMemoryCommandHandle commandHandle, int loggingUniqueId);
B3_SHARED_API int b3GetStatusLoggingUniqueId(b3SharedMemoryStatusHandle statusHandle);
B3_SHARED_API int b3StateLoggingStop(b3SharedMemoryCommandHandle commandHandle, int loggingUid);
b3SharedMemoryCommandHandle b3ProfileTimingCommandInit(b3PhysicsClientHandle physClient, const char* name);
void b3SetProfileTimingDuractionInMicroSeconds(b3SharedMemoryCommandHandle commandHandle, int duration);
B3_SHARED_API b3SharedMemoryCommandHandle b3ProfileTimingCommandInit(b3PhysicsClientHandle physClient, const char* name);
B3_SHARED_API void b3SetProfileTimingDuractionInMicroSeconds(b3SharedMemoryCommandHandle commandHandle, int duration);
void b3SetTimeOut(b3PhysicsClientHandle physClient, double timeOutInSeconds);
double b3GetTimeOut(b3PhysicsClientHandle physClient);
B3_SHARED_API void b3SetTimeOut(b3PhysicsClientHandle physClient, double timeOutInSeconds);
B3_SHARED_API double b3GetTimeOut(b3PhysicsClientHandle physClient);
b3SharedMemoryCommandHandle b3SetAdditionalSearchPath(b3PhysicsClientHandle physClient, char* path);
B3_SHARED_API b3SharedMemoryCommandHandle b3SetAdditionalSearchPath(b3PhysicsClientHandle physClient, char* path);
void b3MultiplyTransforms(const double posA[3], const double ornA[4], const double posB[3], const double ornB[4], double outPos[3], double outOrn[4]);
void b3InvertTransform(const double pos[3], const double orn[4], double outPos[3], double outOrn[4]);
B3_SHARED_API void b3MultiplyTransforms(const double posA[/*3*/], const double ornA[/*4*/], const double posB[/*3*/], const double ornB[/*4*/], double outPos[/*3*/], double outOrn[/*4*/]);
B3_SHARED_API void b3InvertTransform(const double pos[/*3*/], const double orn[/*4*/], double outPos[/*3*/], double outOrn[/*4*/]);
#ifdef __cplusplus
}

20
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -78,7 +78,7 @@ struct PhysicsClientSharedMemoryInternalData {
m_hasLastServerStatus(false),
m_sharedMemoryKey(SHARED_MEMORY_KEY),
m_verboseOutput(false),
m_timeOutInSeconds(5)
m_timeOutInSeconds(30)
{}
void processServerStatus();
@@ -1200,6 +1200,24 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
b3Warning("Request getCollisionInfo failed");
break;
}
case CMD_CUSTOM_COMMAND_COMPLETED:
{
break;
}
case CMD_CALCULATED_JACOBIAN_COMPLETED:
{
break;
}
case CMD_CALCULATED_JACOBIAN_FAILED:
{
b3Warning("jacobian calculation failed");
break;
}
case CMD_CUSTOM_COMMAND_FAILED:
{
b3Warning("custom plugin command failed");
break;
}
default: {
b3Error("Unknown server status %d\n", serverCmd.m_type);

2
examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp
View File

@@ -2,7 +2,7 @@
#include "PhysicsClientSharedMemory.h"
b3PhysicsClientHandle b3ConnectSharedMemory(int key)
B3_SHARED_API b3PhysicsClientHandle b3ConnectSharedMemory(int key)
{
PhysicsClientSharedMemory* cl = new PhysicsClientSharedMemory();
cl->setSharedMemoryKey(key);

2
examples/SharedMemory/PhysicsClientSharedMemory_C_API.h
View File

@@ -7,7 +7,7 @@
extern "C" {
#endif
b3PhysicsClientHandle b3ConnectSharedMemory(int key);
B3_SHARED_API b3PhysicsClientHandle b3ConnectSharedMemory(int key);
#ifdef __cplusplus
}

2
examples/SharedMemory/PhysicsClientTCP_C_API.cpp
View File

@@ -4,7 +4,7 @@
#include "PhysicsDirect.h"
#include <stdio.h>
b3PhysicsClientHandle b3ConnectPhysicsTCP(const char* hostName, int port)
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsTCP(const char* hostName, int port)
{
TcpNetworkedPhysicsProcessor* tcp = new TcpNetworkedPhysicsProcessor(hostName, port);

2
examples/SharedMemory/PhysicsClientTCP_C_API.h
View File

@@ -9,7 +9,7 @@ extern "C" {
///send physics commands using TCP networking
b3PhysicsClientHandle b3ConnectPhysicsTCP(const char* hostName, int port);
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsTCP(const char* hostName, int port);
#ifdef __cplusplus

2
examples/SharedMemory/PhysicsClientUDP_C_API.cpp
View File

@@ -5,7 +5,7 @@
#include <stdio.h>
//think more about naming. The b3ConnectPhysicsLoopback
b3PhysicsClientHandle b3ConnectPhysicsUDP(const char* hostName, int port)
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsUDP(const char* hostName, int port)
{
UdpNetworkedPhysicsProcessor* udp = new UdpNetworkedPhysicsProcessor(hostName, port);

2
examples/SharedMemory/PhysicsClientUDP_C_API.h
View File

@@ -9,7 +9,7 @@ extern "C" {
///send physics commands using UDP networking
b3PhysicsClientHandle b3ConnectPhysicsUDP(const char* hostName, int port);
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsUDP(const char* hostName, int port);
#ifdef __cplusplus

34
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -940,6 +940,40 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
break;
}
case CMD_CUSTOM_COMMAND_COMPLETED:
{
break;
}
case CMD_CUSTOM_COMMAND_FAILED:
{
b3Warning("custom plugin command failed");
break;
}
case CMD_CLIENT_COMMAND_COMPLETED:
{
break;
}
case CMD_CALCULATED_JACOBIAN_COMPLETED:
{
break;
}
case CMD_CALCULATED_JACOBIAN_FAILED:
{
b3Warning("jacobian calculation failed");
break;
}
case CMD_ACTUAL_STATE_UPDATE_COMPLETED:
{
break;
}
case CMD_DESIRED_STATE_RECEIVED_COMPLETED:
{
break;
}
case CMD_STEP_FORWARD_SIMULATION_COMPLETED:
{
break;
}
default:
{
//b3Warning("Unknown server status type");

2
examples/SharedMemory/PhysicsDirectC_API.cpp
View File

@@ -7,7 +7,7 @@
//think more about naming. The b3ConnectPhysicsLoopback
b3PhysicsClientHandle b3ConnectPhysicsDirect()
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsDirect()
{
PhysicsServerCommandProcessor* sdk = new PhysicsServerCommandProcessor;

2
examples/SharedMemory/PhysicsDirectC_API.h
View File

@@ -9,7 +9,7 @@ extern "C" {
///think more about naming. Directly execute commands without transport (no shared memory, UDP, socket, grpc etc)
b3PhysicsClientHandle b3ConnectPhysicsDirect();
B3_SHARED_API b3PhysicsClientHandle b3ConnectPhysicsDirect();
#ifdef __cplusplus

208
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -1,5 +1,6 @@
#include "PhysicsServerCommandProcessor.h"
#include "../Importers/ImportURDFDemo/BulletUrdfImporter.h"
#include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h"
#include "../Importers/ImportURDFDemo/URDF2Bullet.h"
@@ -37,6 +38,8 @@
#include "LinearMath/btRandom.h"
#include "Bullet3Common/b3ResizablePool.h"
#include "../Utils/b3Clock.h"
#include "b3PluginManager.h"
#ifdef B3_ENABLE_TINY_AUDIO
#include "../TinyAudio/b3SoundEngine.h"
#endif
@@ -1053,13 +1056,13 @@ struct GenericRobotStateLogger : public InternalStateLogger
std::string m_fileName;
FILE* m_logFileHandle;
std::string m_structTypes;
btMultiBodyDynamicsWorld* m_dynamicsWorld;
const btMultiBodyDynamicsWorld* m_dynamicsWorld;
btAlignedObjectArray<int> m_bodyIdList;
bool m_filterObjectUniqueId;
int m_maxLogDof;
int m_logFlags;
GenericRobotStateLogger(int loggingUniqueId, const std::string& fileName, btMultiBodyDynamicsWorld* dynamicsWorld, int maxLogDof, int logFlags)
GenericRobotStateLogger(int loggingUniqueId, const std::string& fileName, const btMultiBodyDynamicsWorld* dynamicsWorld, int maxLogDof, int logFlags)
:m_loggingTimeStamp(0),
m_logFileHandle(0),
m_dynamicsWorld(dynamicsWorld),
@@ -1137,7 +1140,7 @@ struct GenericRobotStateLogger : public InternalStateLogger
{
for (int i=0;i<m_dynamicsWorld->getNumMultibodies();i++)
{
btMultiBody* mb = m_dynamicsWorld->getMultiBody(i);
const btMultiBody* mb = m_dynamicsWorld->getMultiBody(i);
int objectUniqueId = mb->getUserIndex2();
if (m_filterObjectUniqueId && m_bodyIdList.findLinearSearch2(objectUniqueId) < 0)
{
@@ -1430,6 +1433,8 @@ struct PhysicsServerCommandProcessorInternalData
b3ResizablePool< InternalBodyHandle > m_bodyHandles;
b3ResizablePool<InternalCollisionShapeHandle> m_userCollisionShapeHandles;
b3PluginManager m_pluginManager;
bool m_allowRealTimeSimulation;
@@ -1484,7 +1489,7 @@ struct PhysicsServerCommandProcessorInternalData
btAlignedObjectArray<int> m_sdfRecentLoadedBodies;
btAlignedObjectArray<InternalStateLogger*> m_stateLoggers;
int m_stateLoggersUniqueId;
int m_profileTimingLoggingUid;
@@ -1512,8 +1517,8 @@ struct PhysicsServerCommandProcessorInternalData
b3HashMap<b3HashString, char*> m_profileEvents;
PhysicsServerCommandProcessorInternalData()
:
PhysicsServerCommandProcessorInternalData(PhysicsCommandProcessorInterface* proc)
:m_pluginManager(proc),
m_allowRealTimeSimulation(false),
m_commandLogger(0),
m_logPlayback(0),
@@ -1537,6 +1542,16 @@ struct PhysicsServerCommandProcessorInternalData
m_pickedConstraint(0),
m_pickingMultiBodyPoint2Point(0)
{
{
//test to statically link a plugin
//#include "plugins/testPlugin/testplugin.h"
//register static plugins:
//m_pluginManager.registerStaticLinkedPlugin("path", initPlugin, exitPlugin, executePluginCommand);
}
m_vrControllerEvents.init();
m_bodyHandles.exitHandles();
@@ -1643,8 +1658,9 @@ void PhysicsServerCommandProcessor::setGuiHelper(struct GUIHelperInterface* guiH
PhysicsServerCommandProcessor::PhysicsServerCommandProcessor()
:m_data(0)
{
m_data = new PhysicsServerCommandProcessorInternalData();
m_data = new PhysicsServerCommandProcessorInternalData(this);
createEmptyDynamicsWorld();
@@ -1666,13 +1682,23 @@ PhysicsServerCommandProcessor::~PhysicsServerCommandProcessor()
delete m_data;
}
void preTickCallback(btDynamicsWorld *world, btScalar timeStep)
{
PhysicsServerCommandProcessor* proc = (PhysicsServerCommandProcessor*) world->getWorldUserInfo();
bool isPreTick = true;
proc->tickPlugins(timeStep, isPreTick);
}
void logCallback(btDynamicsWorld *world, btScalar timeStep)
{
//handle the logging and playing sounds
PhysicsServerCommandProcessor* proc = (PhysicsServerCommandProcessor*) world->getWorldUserInfo();
proc->processCollisionForces(timeStep);
proc->logObjectStates(timeStep);
bool isPreTick = false;
proc->tickPlugins(timeStep, isPreTick);
}
@@ -1781,6 +1807,12 @@ void PhysicsServerCommandProcessor::processCollisionForces(btScalar timeStep)
#endif//B3_ENABLE_TINY_AUDIO
}
void PhysicsServerCommandProcessor::tickPlugins(btScalar timeStep, bool isPreTick)
{
m_data->m_pluginManager.tickPlugins(timeStep, isPreTick);
}
void PhysicsServerCommandProcessor::logObjectStates(btScalar timeStep)
{
for (int i=0;i<m_data->m_stateLoggers.size();i++)
@@ -2144,7 +2176,11 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
{
m_data->m_guiHelper->createPhysicsDebugDrawer(m_data->m_dynamicsWorld);
}
m_data->m_dynamicsWorld->setInternalTickCallback(logCallback,this);
bool isPreTick=false;
m_data->m_dynamicsWorld->setInternalTickCallback(logCallback,this,isPreTick);
isPreTick = true;
m_data->m_dynamicsWorld->setInternalTickCallback(preTickCallback,this,isPreTick);
#ifdef B3_ENABLE_TINY_AUDIO
m_data->m_soundEngine.init(16,true);
@@ -2961,27 +2997,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
bool hasStatus = false;
{
///we ignore overflow of integer for now
{
//until we implement a proper ring buffer, we assume always maximum of 1 outstanding commands
//const SharedMemoryCommand& clientCmd =m_data->m_testBlock1->m_clientCommands[0];
#if 1
if (m_data->m_commandLogger)
{
m_data->m_commandLogger->logCommand(clientCmd);
}
#endif
//m_data->m_testBlock1->m_numProcessedClientCommands++;
//no timestamp yet
//int timeStamp = 0;
//catch uninitialized cases
serverStatusOut.m_type = CMD_INVALID_STATUS;
serverStatusOut.m_numDataStreamBytes = 0;
serverStatusOut.m_dataStream = 0;
@@ -2989,32 +3009,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
//consume the command
switch (clientCmd.m_type)
{
#if 0
case CMD_SEND_BULLET_DATA_STREAM:
{
if (m_data->m_verboseOutput)
{
b3Printf("Processed CMD_SEND_BULLET_DATA_STREAM length %d",clientCmd.m_dataStreamArguments.m_streamChunkLength);
}
btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_data->m_dynamicsWorld);
m_data->m_worldImporters.push_back(worldImporter);
bool completedOk = worldImporter->loadFileFromMemory(m_data->m_testBlock1->m_bulletStreamDataClientToServer,clientCmd.m_dataStreamArguments.m_streamChunkLength);
if (completedOk)
{
SharedMemoryStatus& status = m_data->createServerStatus(CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
m_data->submitServerStatus(status);
} else
{
SharedMemoryStatus& status = m_data->createServerStatus(CMD_BULLET_DATA_STREAM_RECEIVED_FAILED,clientCmd.m_sequenceNumber,timeStamp);
m_data->submitServerStatus(status);
}
break;
}
#endif
case CMD_STATE_LOGGING:
{
BT_PROFILE("CMD_STATE_LOGGING");
@@ -5210,6 +5205,17 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
btAlignedObjectArray<btVector3> omega;
btAlignedObjectArray<btVector3> linVel;
bool computeForwardKinematics = ((clientCmd.m_updateFlags & ACTUAL_STATE_COMPUTE_FORWARD_KINEMATICS)!=0);
if (computeForwardKinematics)
{
B3_PROFILE("compForwardKinematics");
btAlignedObjectArray<btQuaternion> world_to_local;
btAlignedObjectArray<btVector3> local_origin;
world_to_local.resize(mb->getNumLinks()+1);
local_origin.resize(mb->getNumLinks()+1);
mb->forwardKinematics(world_to_local,local_origin);
}
bool computeLinkVelocities = ((clientCmd.m_updateFlags & ACTUAL_STATE_COMPUTE_LINKVELOCITY)!=0);
if (computeLinkVelocities)
{
@@ -6623,30 +6629,60 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
if (tree)
{
int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6;
const int num_dofs = bodyHandle->m_multiBody->getNumDofs();
btInverseDynamics::vecx nu(num_dofs+baseDofs), qdot(num_dofs + baseDofs), q(num_dofs + baseDofs), joint_force(num_dofs + baseDofs);
for (int i = 0; i < num_dofs; i++)
const int numDofs = bodyHandle->m_multiBody->getNumDofs();
btInverseDynamics::vecx q(numDofs + baseDofs);
btInverseDynamics::vecx qdot(numDofs + baseDofs);
btInverseDynamics::vecx nu(numDofs + baseDofs);
btInverseDynamics::vecx joint_force(numDofs + baseDofs);
for (int i = 0; i < numDofs; i++)
{
q[i + baseDofs] = clientCmd.m_calculateJacobianArguments.m_jointPositionsQ[i];
qdot[i + baseDofs] = clientCmd.m_calculateJacobianArguments.m_jointVelocitiesQdot[i];
nu[i+baseDofs] = clientCmd.m_calculateJacobianArguments.m_jointAccelerations[i];
nu[i + baseDofs] = clientCmd.m_calculateJacobianArguments.m_jointAccelerations[i];
}
// Set the gravity to correspond to the world gravity
btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity());
if (-1 != tree->setGravityInWorldFrame(id_grav) &&
-1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force))
{
serverCmd.m_jacobianResultArgs.m_dofCount = num_dofs;
serverCmd.m_jacobianResultArgs.m_dofCount = numDofs + baseDofs;
// Set jacobian value
tree->calculateJacobians(q);
btInverseDynamics::mat3x jac_t(3, num_dofs);
tree->getBodyJacobianTrans(clientCmd.m_calculateJacobianArguments.m_linkIndex+1, &jac_t);
btInverseDynamics::mat3x jac_t(3, numDofs + baseDofs);
btInverseDynamics::mat3x jac_r(3, numDofs + baseDofs);
// Note that inverse dynamics uses zero-based indexing of bodies, not starting from -1 for the base link.
tree->getBodyJacobianTrans(clientCmd.m_calculateJacobianArguments.m_linkIndex + 1, &jac_t);
tree->getBodyJacobianRot(clientCmd.m_calculateJacobianArguments.m_linkIndex + 1, &jac_r);
// Update the translational jacobian based on the desired local point.
// v_pt = v_frame + w x pt
// v_pt = J_t * qd + (J_r * qd) x pt
// v_pt = J_t * qd - pt x (J_r * qd)
// v_pt = J_t * qd - pt_x * J_r * qd)
// v_pt = (J_t - pt_x * J_r) * qd
// J_t_new = J_t - pt_x * J_r
btInverseDynamics::vec3 localPosition;
for (int i = 0; i < 3; ++i) {
localPosition(i) = clientCmd.m_calculateJacobianArguments.m_localPosition[i];
}
// Only calculate if the localPosition is non-zero.
if (btInverseDynamics::maxAbs(localPosition) > 0.0) {
btInverseDynamics::mat33 skewCrossProduct;
btInverseDynamics::skew(localPosition, &skewCrossProduct);
btInverseDynamics::mat3x jac_l(3, numDofs + baseDofs);
btInverseDynamics::mul(skewCrossProduct, jac_r, &jac_l);
btInverseDynamics::mat3x jac_t_new(3, numDofs + baseDofs);
btInverseDynamics::sub(jac_t, jac_l, &jac_t_new);
jac_t = jac_t_new;
}
// Fill in the result into the shared memory.
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < num_dofs; ++j)
for (int j = 0; j < (numDofs + baseDofs); ++j)
{
serverCmd.m_jacobianResultArgs.m_linearJacobian[i*num_dofs+j] = jac_t(i,j);
int element = (numDofs + baseDofs) * i + j;
serverCmd.m_jacobianResultArgs.m_linearJacobian[element] = jac_t(i,j);
serverCmd.m_jacobianResultArgs.m_angularJacobian[element] = jac_r(i,j);
}
}
serverCmd.m_type = CMD_CALCULATED_JACOBIAN_COMPLETED;
@@ -7242,6 +7278,15 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
userConstraintPtr->m_mbConstraint->setGearRatio(clientCmd.m_userConstraintArguments.m_gearRatio);
}
if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET)
{
userConstraintPtr->m_mbConstraint->setRelativePositionTarget(clientCmd.m_userConstraintArguments.m_relativePositionTarget);
}
if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_ERP)
{
userConstraintPtr->m_mbConstraint->setErp(clientCmd.m_userConstraintArguments.m_erp);
}
if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK)
{
userConstraintPtr->m_mbConstraint->setGearAuxLink(clientCmd.m_userConstraintArguments.m_gearAuxLink);
@@ -7370,7 +7415,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
tree->calculateJacobians(q);
btInverseDynamics::mat3x jac_t(3, numDofs);
btInverseDynamics::mat3x jac_r(3,numDofs);
// Note that inverse dynamics uses zero-based indexing of bodies, not starting from -1 for the base link.
// Note that inverse dynamics uses zero-based indexing of bodies, not starting from -1 for the base link.
tree->getBodyJacobianTrans(endEffectorLinkIndex+1, &jac_t);
tree->getBodyJacobianRot(endEffectorLinkIndex+1, &jac_r);
for (int i = 0; i < 3; ++i)
@@ -7968,6 +8013,39 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
break;
}
case CMD_CUSTOM_COMMAND:
{
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_CUSTOM_COMMAND_FAILED;
serverCmd.m_customCommandResultArgs.m_pluginUniqueId = -1;
if (clientCmd.m_updateFlags & CMD_CUSTOM_COMMAND_LOAD_PLUGIN)
{
//pluginPath could be registered or load from disk
int pluginUniqueId = m_data->m_pluginManager.loadPlugin(clientCmd.m_customCommandArgs.m_pluginPath);
if (pluginUniqueId>=0)
{
serverCmd.m_customCommandResultArgs.m_pluginUniqueId = pluginUniqueId;
serverCmd.m_type = CMD_CUSTOM_COMMAND_COMPLETED;
}
}
if (clientCmd.m_updateFlags & CMD_CUSTOM_COMMAND_UNLOAD_PLUGIN)
{
m_data->m_pluginManager.unloadPlugin(clientCmd.m_customCommandArgs.m_pluginUniqueId);
serverCmd.m_type = CMD_CUSTOM_COMMAND_COMPLETED;
}
if (clientCmd.m_updateFlags & CMD_CUSTOM_COMMAND_EXECUTE_PLUGIN_COMMAND)
{
int result = m_data->m_pluginManager.executePluginCommand(clientCmd.m_customCommandArgs.m_pluginUniqueId, &clientCmd.m_customCommandArgs.m_arguments);
serverCmd.m_customCommandResultArgs.m_executeCommandResult = result;
serverCmd.m_type = CMD_CUSTOM_COMMAND_COMPLETED;
}
hasStatus = true;
break;
}
default:
{
BT_PROFILE("CMD_UNKNOWN");
@@ -8186,12 +8264,6 @@ void PhysicsServerCommandProcessor::replayFromLogFile(const char* fileName)
}
btVector3 gVRGripperPos(0.7, 0.3, 0.7);
btQuaternion gVRGripperOrn(0,0,0,1);
btVector3 gVRController2Pos(0,0,0.2);
btQuaternion gVRController2Orn(0,0,0,1);
btScalar gVRGripper2Analog = 0;
btScalar gVRGripperAnalog = 0;
@@ -8214,6 +8286,8 @@ bool PhysicsServerCommandProcessor::isRealTimeSimulationEnabled() const
void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec,const struct b3VRControllerEvent* vrControllerEvents, int numVRControllerEvents, const struct b3KeyboardEvent* keyEvents, int numKeyEvents, const struct b3MouseEvent* mouseEvents, int numMouseEvents)
{
m_data->m_vrControllerEvents.addNewVREvents(vrControllerEvents,numVRControllerEvents);
for (int i=0;i<m_data->m_stateLoggers.size();i++)
{
if (m_data->m_stateLoggers[i]->m_loggingType==STATE_LOGGING_VR_CONTROLLERS)

1
examples/SharedMemory/PhysicsServerCommandProcessor.h
View File

@@ -91,6 +91,7 @@ public:
virtual void replayLogCommand(char* bufferServerToClient, int bufferSizeInBytes );
//logging of object states (position etc)
void tickPlugins(btScalar timeStep, bool isPreTick);
void logObjectStates(btScalar timeStep);
void processCollisionForces(btScalar timeStep);

26
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -109,6 +109,26 @@ struct b3SearchPathfArgs
char m_path[MAX_FILENAME_LENGTH];
};
enum CustomCommandEnum
{
CMD_CUSTOM_COMMAND_LOAD_PLUGIN=1,
CMD_CUSTOM_COMMAND_UNLOAD_PLUGIN=2,
CMD_CUSTOM_COMMAND_EXECUTE_PLUGIN_COMMAND=4,
};
struct b3CustomCommand
{
int m_pluginUniqueId;
b3PluginArguments m_arguments;
char m_pluginPath[MAX_FILENAME_LENGTH];
};
struct b3CustomCommandResultArgs
{
int m_pluginUniqueId;
int m_executeCommandResult;
};
struct BulletDataStreamArgs
{
@@ -658,7 +678,9 @@ enum EnumUserConstraintFlags
USER_CONSTRAINT_CHANGE_MAX_FORCE=32,
USER_CONSTRAINT_REQUEST_INFO=64,
USER_CONSTRAINT_CHANGE_GEAR_RATIO=128,
USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK=256,
USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK=256,
USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET=512,
USER_CONSTRAINT_CHANGE_ERP=1024,
};
@@ -968,6 +990,7 @@ struct SharedMemoryCommand
struct b3RequestCollisionInfoArgs m_requestCollisionInfoArgs;
struct b3ChangeTextureArgs m_changeTextureArgs;
struct b3SearchPathfArgs m_searchPathArgs;
struct b3CustomCommand m_customCommandArgs;
};
};
@@ -1039,6 +1062,7 @@ struct SharedMemoryStatus
struct b3SendCollisionInfoArgs m_sendCollisionInfoArgs;
struct SendMouseEvents m_sendMouseEvents;
struct b3LoadTextureResultArgs m_loadTextureResultArguments;
struct b3CustomCommandResultArgs m_customCommandResultArgs;
};
};

10
examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp
View File

@@ -83,7 +83,7 @@ public:
};
b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThread(int argc, char* argv[])
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThread(int argc, char* argv[])
{
InProcessPhysicsClientSharedMemoryMainThread* cl = new InProcessPhysicsClientSharedMemoryMainThread(argc, argv, 1);
cl->setSharedMemoryKey(SHARED_MEMORY_KEY+1);
@@ -91,7 +91,7 @@ b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThread(int arg
return (b3PhysicsClientHandle ) cl;
}
b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThreadSharedMemory(int argc, char* argv[])
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThreadSharedMemory(int argc, char* argv[])
{
InProcessPhysicsClientSharedMemoryMainThread* cl = new InProcessPhysicsClientSharedMemoryMainThread(argc, argv, 0);
cl->setSharedMemoryKey(SHARED_MEMORY_KEY+1);
@@ -133,7 +133,7 @@ public:
};
b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnect(int argc, char* argv[])
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnect(int argc, char* argv[])
{
InProcessPhysicsClientSharedMemory* cl = new InProcessPhysicsClientSharedMemory(argc, argv, 1);
@@ -141,7 +141,7 @@ b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnect(int argc, char* a
cl->connect();
return (b3PhysicsClientHandle ) cl;
}
b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectSharedMemory(int argc, char* argv[])
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectSharedMemory(int argc, char* argv[])
{
InProcessPhysicsClientSharedMemory* cl = new InProcessPhysicsClientSharedMemory(argc, argv, 0);
@@ -248,7 +248,7 @@ int b3InProcessMouseButtonCallback(b3PhysicsClientHandle clientHandle, int butto
}
b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect(void* guiHelperPtr)
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect(void* guiHelperPtr)
{
GUIHelperInterface* guiHelper = (GUIHelperInterface*) guiHelperPtr;
InProcessPhysicsClientExistingExampleBrowser* cl = new InProcessPhysicsClientExistingExampleBrowser(guiHelper);

10
examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.h
View File

@@ -10,13 +10,13 @@ extern "C" {
///think more about naming. The b3ConnectPhysicsLoopback
b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnect(int argc, char* argv[]);
b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectSharedMemory(int argc, char* argv[]);
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnect(int argc, char* argv[]);
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectSharedMemory(int argc, char* argv[]);
b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThread(int argc, char* argv[]);
b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThreadSharedMemory(int argc, char* argv[]);
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThread(int argc, char* argv[]);
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThreadSharedMemory(int argc, char* argv[]);
b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect(void* guiHelperPtr);
B3_SHARED_API b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect(void* guiHelperPtr);
///ignore the following APIs, they are for internal use for example browser
void b3InProcessRenderSceneInternal(b3PhysicsClientHandle clientHandle);

25
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -5,7 +5,8 @@
///increase the SHARED_MEMORY_MAGIC_NUMBER whenever incompatible changes are made in the structures
///my convention is year/month/day/rev
#define SHARED_MEMORY_MAGIC_NUMBER 201708270
#define SHARED_MEMORY_MAGIC_NUMBER 201709260
//#define SHARED_MEMORY_MAGIC_NUMBER 201708270
//#define SHARED_MEMORY_MAGIC_NUMBER 201707140
//#define SHARED_MEMORY_MAGIC_NUMBER 201706015
//#define SHARED_MEMORY_MAGIC_NUMBER 201706001
@@ -72,6 +73,7 @@ enum EnumSharedMemoryClientCommand
CMD_REQUEST_MOUSE_EVENTS_DATA,
CMD_CHANGE_TEXTURE,
CMD_SET_ADDITIONAL_SEARCH_PATH,
CMD_CUSTOM_COMMAND,
//don't go beyond this command!
CMD_MAX_CLIENT_COMMANDS,
@@ -167,6 +169,9 @@ enum EnumSharedMemoryServerStatus
CMD_REQUEST_COLLISION_INFO_FAILED,
CMD_REQUEST_MOUSE_EVENTS_DATA_COMPLETED,
CMD_CHANGE_TEXTURE_COMMAND_FAILED,
CMD_CUSTOM_COMMAND_COMPLETED,
CMD_CUSTOM_COMMAND_FAILED,
//don't go beyond 'CMD_MAX_SERVER_COMMANDS!
CMD_MAX_SERVER_COMMANDS
};
@@ -242,6 +247,8 @@ struct b3UserConstraint
int m_userConstraintUniqueId;
double m_gearRatio;
int m_gearAuxLink;
double m_relativePositionTarget;
double m_erp;
};
@@ -501,7 +508,8 @@ struct b3VisualShapeInformation
enum eLinkStateFlags
{
ACTUAL_STATE_COMPUTE_LINKVELOCITY=1
ACTUAL_STATE_COMPUTE_LINKVELOCITY=1,
ACTUAL_STATE_COMPUTE_FORWARD_KINEMATICS=2,
};
///b3LinkState provides extra information such as the Cartesian world coordinates
@@ -616,6 +624,19 @@ enum eStateLoggingFlags
STATE_LOG_JOINT_TORQUES = STATE_LOG_JOINT_MOTOR_TORQUES+STATE_LOG_JOINT_USER_TORQUES,
};
#define B3_MAX_PLUGIN_ARG_SIZE 128
#define B3_MAX_PLUGIN_ARG_TEXT_LEN 1024
struct b3PluginArguments
{
char m_text[B3_MAX_PLUGIN_ARG_TEXT_LEN];
int m_numInts;
int m_ints[B3_MAX_PLUGIN_ARG_SIZE];
int m_numFloats;
int m_floats[B3_MAX_PLUGIN_ARG_SIZE];
};
#endif//SHARED_MEMORY_PUBLIC_H

255
examples/SharedMemory/b3PluginManager.cpp Normal file
View File

@@ -0,0 +1,255 @@
#include "b3PluginManager.h"
#include "Bullet3Common/b3HashMap.h"
#include "Bullet3Common/b3ResizablePool.h"
#include "PhysicsClientC_API.h"
#include "PhysicsDirect.h"
#include "plugins/b3PluginContext.h"
#ifdef _WIN32
#define WIN32_LEAN_AND_MEAN
#define VC_EXTRALEAN
#include <windows.h>
typedef HMODULE B3_DYNLIB_HANDLE;
#define B3_DYNLIB_OPEN LoadLibrary
#define B3_DYNLIB_CLOSE FreeLibrary
#define B3_DYNLIB_IMPORT GetProcAddress
#else
#include <dlfcn.h>
typedef void* B3_DYNLIB_HANDLE;
#define B3_DYNLIB_OPEN(path) dlopen(path, RTLD_NOW | RTLD_GLOBAL)
#define B3_DYNLIB_CLOSE dlclose
#define B3_DYNLIB_IMPORT dlsym
#endif
struct b3Plugin
{
B3_DYNLIB_HANDLE m_pluginHandle;
bool m_ownsPluginHandle;
std::string m_pluginPath;
int m_pluginUniqueId;
PFN_INIT m_initFunc;
PFN_EXIT m_exitFunc;
PFN_EXECUTE m_executeCommandFunc;
PFN_TICK m_preTickFunc;
PFN_TICK m_postTickFunc;
void* m_userPointer;
b3Plugin()
:m_pluginHandle(0),
m_ownsPluginHandle(false),
m_pluginUniqueId(-1),
m_initFunc(0),
m_exitFunc(0),
m_executeCommandFunc(0),
m_preTickFunc(0),
m_postTickFunc(0),
m_userPointer(0)
{
}
void clear()
{
if (m_ownsPluginHandle)
{
B3_DYNLIB_CLOSE(m_pluginHandle);
}
m_pluginHandle = 0;
m_initFunc = 0;
m_exitFunc = 0;
m_executeCommandFunc = 0;
m_preTickFunc = 0;
m_postTickFunc = 0;
m_userPointer = 0;
}
};
typedef b3PoolBodyHandle<b3Plugin> b3PluginHandle;
struct b3PluginManagerInternalData
{
b3ResizablePool<b3PluginHandle> m_plugins;
b3HashMap<b3HashString, b3PluginHandle> m_pluginMap;
PhysicsDirect* m_physicsDirect;
};
b3PluginManager::b3PluginManager(class PhysicsCommandProcessorInterface* physSdk)
{
m_data = new b3PluginManagerInternalData;
m_data->m_physicsDirect = new PhysicsDirect(physSdk,false);
}
b3PluginManager::~b3PluginManager()
{
while (m_data->m_pluginMap.size())
{
b3PluginHandle* plugin = m_data->m_pluginMap.getAtIndex(0);
unloadPlugin(plugin->m_pluginUniqueId);
}
delete m_data->m_physicsDirect;
m_data->m_pluginMap.clear();
m_data->m_plugins.exitHandles();
delete m_data;
}
int b3PluginManager::loadPlugin(const char* pluginPath)
{
int pluginUniqueId = -1;
b3Plugin* pluginOrg = m_data->m_pluginMap.find(pluginPath);
if (pluginOrg)
{
//already loaded
pluginUniqueId = pluginOrg->m_pluginUniqueId;
}
else
{
pluginUniqueId = m_data->m_plugins.allocHandle();
b3PluginHandle* plugin = m_data->m_plugins.getHandle(pluginUniqueId);
plugin->m_pluginUniqueId = pluginUniqueId;
B3_DYNLIB_HANDLE pluginHandle = B3_DYNLIB_OPEN(pluginPath);
bool ok = false;
if (pluginHandle)
{
plugin->m_initFunc = (PFN_INIT)B3_DYNLIB_IMPORT(pluginHandle, "initPlugin");
plugin->m_exitFunc = (PFN_EXIT)B3_DYNLIB_IMPORT(pluginHandle, "exitPlugin");
plugin->m_executeCommandFunc = (PFN_EXECUTE)B3_DYNLIB_IMPORT(pluginHandle, "executePluginCommand");
plugin->m_preTickFunc = (PFN_TICK)B3_DYNLIB_IMPORT(pluginHandle, "preTickPluginCallback");
plugin->m_postTickFunc = (PFN_TICK)B3_DYNLIB_IMPORT(pluginHandle, "postTickPluginCallback");
if (plugin->m_initFunc && plugin->m_exitFunc && plugin->m_executeCommandFunc)
{
b3PluginContext context;
context.m_userPointer = plugin->m_userPointer;
context.m_physClient = (b3PhysicsClientHandle) m_data->m_physicsDirect;
int version = plugin->m_initFunc(&context);
//keep the user pointer persistent
plugin->m_userPointer = context.m_userPointer;
if (version == SHARED_MEMORY_MAGIC_NUMBER)
{
ok = true;
plugin->m_ownsPluginHandle = true;
plugin->m_pluginHandle = pluginHandle;
plugin->m_pluginPath = pluginPath;
m_data->m_pluginMap.insert(pluginPath, *plugin);
}
else
{
int expect = SHARED_MEMORY_MAGIC_NUMBER;
b3Warning("Warning: plugin is wrong version: expected %d, got %d\n", expect, version);
}
}
else
{
b3Warning("Loaded plugin but couldn't bind functions");
}
if (!ok)
{
B3_DYNLIB_CLOSE(pluginHandle);
}
}
else
{
b3Warning("Warning: couldn't load plugin %s\n", pluginPath);
}
if (!ok)
{
m_data->m_plugins.freeHandle(pluginUniqueId);
pluginUniqueId = -1;
}
}
return pluginUniqueId;
}
void b3PluginManager::unloadPlugin(int pluginUniqueId)
{
b3PluginHandle* plugin = m_data->m_plugins.getHandle(pluginUniqueId);
if (plugin)
{
b3PluginContext context;
context.m_userPointer = plugin->m_userPointer;
context.m_physClient = (b3PhysicsClientHandle) m_data->m_physicsDirect;
plugin->m_exitFunc(&context);
m_data->m_pluginMap.remove(plugin->m_pluginPath.c_str());
m_data->m_plugins.freeHandle(pluginUniqueId);
}
}
void b3PluginManager::tickPlugins(double timeStep, bool isPreTick)
{
for (int i=0;i<m_data->m_pluginMap.size();i++)
{
b3PluginHandle* plugin = m_data->m_pluginMap.getAtIndex(i);
PFN_TICK tick = isPreTick? plugin->m_preTickFunc : plugin->m_postTickFunc;
if (tick)
{
b3PluginContext context;
context.m_userPointer = plugin->m_userPointer;
context.m_physClient = (b3PhysicsClientHandle) m_data->m_physicsDirect;
int result = tick(&context);
plugin->m_userPointer = context.m_userPointer;
}
}
}
int b3PluginManager::executePluginCommand(int pluginUniqueId, const b3PluginArguments* arguments)
{
int result = -1;
b3PluginHandle* plugin = m_data->m_plugins.getHandle(pluginUniqueId);
if (plugin)
{
b3PluginContext context;
context.m_userPointer = plugin->m_userPointer;
context.m_physClient = (b3PhysicsClientHandle) m_data->m_physicsDirect;
result = plugin->m_executeCommandFunc(&context, arguments);
plugin->m_userPointer = context.m_userPointer;
}
return result;
}
int b3PluginManager::registerStaticLinkedPlugin(const char* pluginPath, PFN_INIT initFunc,PFN_EXIT exitFunc, PFN_EXECUTE executeCommandFunc, PFN_TICK preTickFunc, PFN_TICK postTickFunc)
{
b3Plugin orgPlugin;
int pluginUniqueId = m_data->m_plugins.allocHandle();
b3PluginHandle* pluginHandle = m_data->m_plugins.getHandle(pluginUniqueId);
pluginHandle->m_pluginHandle = 0;
pluginHandle->m_ownsPluginHandle =false;
pluginHandle->m_pluginUniqueId = pluginUniqueId;
pluginHandle->m_executeCommandFunc = executeCommandFunc;
pluginHandle->m_exitFunc = exitFunc;
pluginHandle->m_initFunc = initFunc;
pluginHandle->m_preTickFunc = preTickFunc;
pluginHandle->m_postTickFunc = postTickFunc;
pluginHandle->m_pluginHandle = 0;
pluginHandle->m_pluginPath = pluginPath;
pluginHandle->m_userPointer = 0;
m_data->m_pluginMap.insert(pluginPath, *pluginHandle);
{
b3PluginContext context;
context.m_userPointer = 0;
context.m_physClient = (b3PhysicsClientHandle) m_data->m_physicsDirect;
int result = pluginHandle->m_initFunc(&context);
pluginHandle->m_userPointer = context.m_userPointer;
}
return pluginUniqueId;
}

23
examples/SharedMemory/b3PluginManager.h Normal file
View File

@@ -0,0 +1,23 @@
#ifndef B3_PLUGIN_MANAGER_H
#define B3_PLUGIN_MANAGER_H
#include "plugins/b3PluginAPI.h"
class b3PluginManager
{
struct b3PluginManagerInternalData* m_data;
public:
b3PluginManager(class PhysicsCommandProcessorInterface* physSdk);
virtual ~b3PluginManager();
int loadPlugin(const char* pluginPath);
void unloadPlugin(int pluginUniqueId);
int executePluginCommand(int pluginUniqueId, const struct b3PluginArguments* arguments);
void tickPlugins(double timeStep, bool isPreTick);
int registerStaticLinkedPlugin(const char* pluginPath, PFN_INIT initFunc,PFN_EXIT exitFunc, PFN_EXECUTE executeCommandFunc, PFN_TICK preTickFunc, PFN_TICK postTickFunc);
};
#endif //B3_PLUGIN_MANAGER_H

37
examples/SharedMemory/plugins/b3PluginAPI.h Normal file
View File

@@ -0,0 +1,37 @@
#ifndef B3_PLUGIN_API_H
#define B3_PLUGIN_API_H
#ifdef _WIN32
#define B3_SHARED_API __declspec(dllexport)
#elif defined (__GNUC__)
#define B3_SHARED_API __attribute__((visibility("default")))
#else
#define B3_SHARED_API
#endif
#if defined(_WIN32)
#define B3_API_ENTRY
#define B3_API_CALL __cdecl
#define B3_CALLBACK __cdecl
#else
#define B3_API_ENTRY
#define B3_API_CALL
#define B3_CALLBACK
#endif
#ifdef __cplusplus
extern "C" {
#endif
/* Plugin API */
typedef B3_API_ENTRY int (B3_API_CALL * PFN_INIT)(struct b3PluginContext* context);
typedef B3_API_ENTRY void (B3_API_CALL * PFN_EXIT)(struct b3PluginContext* context);
typedef B3_API_ENTRY int (B3_API_CALL * PFN_EXECUTE)(struct b3PluginContext* context, const struct b3PluginArguments* arguments);
typedef B3_API_ENTRY int (B3_API_CALL * PFN_TICK)(struct b3PluginContext* context);
#ifdef __cplusplus
}
#endif
#endif //B3_PLUGIN_API_H

21
examples/SharedMemory/plugins/b3PluginContext.h Normal file
View File

@@ -0,0 +1,21 @@
#ifndef B3_PLUGIN_CONTEXT_H
#define B3_PLUGIN_CONTEXT_H
#include "../PhysicsClientC_API.h"
struct b3PluginContext
{
b3PhysicsClientHandle m_physClient;
//plugin can modify the m_userPointer to store persistent object pointer (class or struct instance etc)
void* m_userPointer;
};
#endif //B3_PLUGIN_CONTEXT_H

42
examples/SharedMemory/plugins/testPlugin/premake4.lua Normal file
View File

@@ -0,0 +1,42 @@
project ("pybullet_testplugin")
language "C++"
kind "SharedLib"
includedirs {".","../../../../src", "../../../../examples",
"../../../ThirdPartyLibs"}
defines {"PHYSICS_IN_PROCESS_EXAMPLE_BROWSER"}
hasCL = findOpenCL("clew")
links{"BulletFileLoader", "Bullet3Common", "LinearMath"}
if os.is("MacOSX") then
-- targetextension {"so"}
links{"Cocoa.framework","Python"}
end
files {
"testplugin.cpp",
"../../PhysicsClient.cpp",
"../../PhysicsClient.h",
"../../PhysicsClientSharedMemory.cpp",
"../../PhysicsClientSharedMemory.h",
"../../PhysicsClientSharedMemory_C_API.cpp",
"../../PhysicsClientSharedMemory_C_API.h",
"../../PhysicsClientC_API.cpp",
"../../PhysicsClientC_API.h",
"../../Win32SharedMemory.cpp",
"../../Win32SharedMemory.h",
"../../PosixSharedMemory.cpp",
"../../PosixSharedMemory.h",
"../../../Utils/b3Clock.cpp",
"../../../Utils/b3Clock.h",
"../../../Utils/b3ResourcePath.cpp",
"../../../Utils/b3ResourcePath.h",
}

150
examples/SharedMemory/plugins/testPlugin/testplugin.cpp Normal file
View File

@@ -0,0 +1,150 @@
//test plugin, can load a URDF file, example usage on a Windows machine:
/*
import pybullet as p
p.connect(p.GUI)
pluginUid = p.loadPlugin("E:/develop/bullet3/bin/pybullet_testplugin_vs2010_x64_debug.dll")
commandUid = 0
argument = "plane.urdf"
p.executePluginCommand(pluginUid,commandUid,argument)
p.unloadPlugin(pluginUid)
*/
#include "testplugin.h"
#include "../../SharedMemoryPublic.h"
#include "../b3PluginContext.h"
#include <stdio.h>
struct MyClass
{
int m_testData;
MyClass()
:m_testData(42)
{
}
virtual ~MyClass()
{
}
};
B3_SHARED_API int initPlugin(struct b3PluginContext* context)
{
MyClass* obj = new MyClass();
context->m_userPointer = obj;
printf("hi!\n");
return SHARED_MEMORY_MAGIC_NUMBER;
}
B3_SHARED_API int preTickPluginCallback(struct b3PluginContext* context)
{
MyClass* obj = (MyClass* )context->m_userPointer;
{
b3SharedMemoryCommandHandle commandHandle = b3RequestVREventsCommandInit(context->m_physClient);
int deviceTypeFilter = VR_DEVICE_CONTROLLER;
b3VREventsSetDeviceTypeFilter(commandHandle, deviceTypeFilter);
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, commandHandle);
int statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_REQUEST_VR_EVENTS_DATA_COMPLETED)
{
struct b3VREventsData vrEvents;
int i = 0;
b3GetVREventsData(context->m_physClient, &vrEvents);
if (vrEvents.m_numControllerEvents)
{
//this is only for a test, normally you wouldn't print to the console at each simulation substep!
printf("got %d VR controller events!\n", vrEvents.m_numControllerEvents);
}
}
}
{
b3KeyboardEventsData keyboardEventsData;
b3SharedMemoryCommandHandle commandHandle = b3RequestKeyboardEventsCommandInit(context->m_physClient);
b3SubmitClientCommandAndWaitStatus(context->m_physClient, commandHandle);
b3GetKeyboardEventsData(context->m_physClient, &keyboardEventsData);
if (keyboardEventsData.m_numKeyboardEvents)
{
//this is only for a test, normally you wouldn't print to the console at each simulation substep!
printf("got %d keyboard events\n", keyboardEventsData.m_numKeyboardEvents);
}
}
{
b3MouseEventsData mouseEventsData;
b3SharedMemoryCommandHandle commandHandle = b3RequestMouseEventsCommandInit(context->m_physClient);
b3SubmitClientCommandAndWaitStatus(context->m_physClient, commandHandle);
b3GetMouseEventsData(context->m_physClient, &mouseEventsData);
if (mouseEventsData.m_numMouseEvents)
{
//this is only for a test, normally you wouldn't print to the console at each simulation substep!
printf("got %d mouse events\n", mouseEventsData.m_numMouseEvents);
}
}
return 0;
}
B3_SHARED_API int postTickPluginCallback(struct b3PluginContext* context)
{
MyClass* obj = (MyClass* )context->m_userPointer;
obj->m_testData++;
return 0;
}
B3_SHARED_API int executePluginCommand(struct b3PluginContext* context, const struct b3PluginArguments* arguments)
{
printf("text argument:%s\n",arguments->m_text);
printf("int args: [");
for (int i=0;i<arguments->m_numInts;i++)
{
printf("%d", arguments->m_ints[i]);
if ((i+1)<arguments->m_numInts)
{
printf(",");
}
}
printf("]\nfloat args: [");
for (int i=0;i<arguments->m_numFloats;i++)
{
printf("%f", arguments->m_floats[i]);
if ((i+1)<arguments->m_numFloats)
{
printf(",");
}
}
printf("]\n");
MyClass* obj = (MyClass*) context->m_userPointer;
b3SharedMemoryStatusHandle statusHandle;
int statusType = -1;
int bodyUniqueId = -1;
b3SharedMemoryCommandHandle command =
b3LoadUrdfCommandInit(context->m_physClient, arguments->m_text);
statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, command);
statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_URDF_LOADING_COMPLETED)
{
bodyUniqueId = b3GetStatusBodyIndex(statusHandle);
}
return bodyUniqueId;
}
B3_SHARED_API void exitPlugin(struct b3PluginContext* context)
{
MyClass* obj = (MyClass*) context->m_userPointer;
delete obj;
context->m_userPointer = 0;
printf("bye!\n");
}

26
examples/SharedMemory/plugins/testPlugin/testplugin.h Normal file
View File

@@ -0,0 +1,26 @@
#ifndef TEST_PLUGIN_H
#define TEST_PLUGIN_H
#include "../b3PluginAPI.h"
#ifdef __cplusplus
extern "C"
{
#endif
//initPlugin, exitPlugin and executePluginCommand are required, otherwise plugin won't load
B3_SHARED_API int initPlugin(struct b3PluginContext* context);
B3_SHARED_API void exitPlugin(struct b3PluginContext* context);
B3_SHARED_API int executePluginCommand(struct b3PluginContext* context, const struct b3PluginArguments* arguments);
//preTickPluginCallback and postTickPluginCallback are optional.
B3_SHARED_API int preTickPluginCallback(struct b3PluginContext* context);
B3_SHARED_API int postTickPluginCallback(struct b3PluginContext* context);
#ifdef __cplusplus
};
#endif
#endif//#define TEST_PLUGIN_H

42
examples/SharedMemory/plugins/vrSyncPlugin/premake4.lua Normal file
View File

@@ -0,0 +1,42 @@
project ("pybullet_vrSyncPlugin")
language "C++"
kind "SharedLib"
includedirs {".","../../../../src", "../../../../examples",
"../../../ThirdPartyLibs"}
defines {"PHYSICS_IN_PROCESS_EXAMPLE_BROWSER"}
hasCL = findOpenCL("clew")
links{"BulletFileLoader", "Bullet3Common", "LinearMath"}
if os.is("MacOSX") then
-- targetextension {"so"}
links{"Cocoa.framework","Python"}
end
files {
"vrSyncPlugin.cpp",
"../../PhysicsClient.cpp",
"../../PhysicsClient.h",
"../../PhysicsClientSharedMemory.cpp",
"../../PhysicsClientSharedMemory.h",
"../../PhysicsClientSharedMemory_C_API.cpp",
"../../PhysicsClientSharedMemory_C_API.h",
"../../PhysicsClientC_API.cpp",
"../../PhysicsClientC_API.h",
"../../Win32SharedMemory.cpp",
"../../Win32SharedMemory.h",
"../../PosixSharedMemory.cpp",
"../../PosixSharedMemory.h",
"../../../Utils/b3Clock.cpp",
"../../../Utils/b3Clock.h",
"../../../Utils/b3ResourcePath.cpp",
"../../../Utils/b3ResourcePath.h",
}

209
examples/SharedMemory/plugins/vrSyncPlugin/vrSyncPlugin.cpp Normal file
View File

@@ -0,0 +1,209 @@
//vrSyncPlugin plugin, will query the vr controller events
//and set change the user constraint to match the pose
//in Python you can load and configure the plugin like this:
//plugin = p.loadPlugin("e:/develop/bullet3/bin/pybullet_vrSyncPlugin_vs2010_x64_release.dll")
//could also be plugin = p.loadPlugin("vrSyncPlugin.so") on Mac/Linux
//controllerId = 3
#include "vrSyncPlugin.h"
#include "../../SharedMemoryPublic.h"
#include "../b3PluginContext.h"
#include <stdio.h>
struct MyClass
{
int m_testData;
int m_controllerId;
int m_constraintId;
int m_constraintId2;
int m_gripperId;
float m_maxForce;
float m_maxForce2;
MyClass()
:m_testData(42),
m_controllerId(-1),
m_constraintId(-1),
m_constraintId2(-1),
m_gripperId(-1),
m_maxForce(0),
m_maxForce2(0)
{
}
virtual ~MyClass()
{
}
};
B3_SHARED_API int initPlugin(struct b3PluginContext* context)
{
MyClass* obj = new MyClass();
context->m_userPointer = obj;
printf("hi vrSyncPlugin!\n");
return SHARED_MEMORY_MAGIC_NUMBER;
}
B3_SHARED_API int preTickPluginCallback(struct b3PluginContext* context)
{
MyClass* obj = (MyClass* )context->m_userPointer;
if (obj->m_controllerId>=0)
{
b3SharedMemoryCommandHandle commandHandle = b3RequestVREventsCommandInit(context->m_physClient);
int deviceTypeFilter = VR_DEVICE_CONTROLLER;
b3VREventsSetDeviceTypeFilter(commandHandle, deviceTypeFilter);
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, commandHandle);
int statusType = b3GetStatusType(statusHandle);
if (statusType == CMD_REQUEST_VR_EVENTS_DATA_COMPLETED)
{
struct b3VREventsData vrEvents;
int i = 0;
b3GetVREventsData(context->m_physClient, &vrEvents);
if (vrEvents.m_numControllerEvents)
{
for (int n=0;n<vrEvents.m_numControllerEvents;n++)
{
b3VRControllerEvent& event = vrEvents.m_controllerEvents[n];
if (event.m_controllerId ==obj->m_controllerId)
{
if (obj->m_constraintId>=0)
{
//this is basically equivalent to doing this in Python/pybullet:
//p.changeConstraint(pr2_cid, e[POSITION], e[ORIENTATION], maxForce=...)
b3SharedMemoryCommandHandle commandHandle;
int userConstraintUniqueId = obj->m_constraintId;
commandHandle = b3InitChangeUserConstraintCommand(context->m_physClient, userConstraintUniqueId);
double pos[4] = {event.m_pos[0],event.m_pos[1],event.m_pos[2],1};
b3InitChangeUserConstraintSetPivotInB(commandHandle, pos);
double orn[4] = {event.m_orn[0],event.m_orn[1],event.m_orn[2],event.m_orn[3]};
b3InitChangeUserConstraintSetFrameInB(commandHandle, orn);
b3InitChangeUserConstraintSetMaxForce(commandHandle, obj->m_maxForce);
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, commandHandle);
}
// apply the analogue button to close the constraint, using a gear constraint with position target
if (obj->m_constraintId2>=0)
{
//this block is similar to
//p.changeConstraint(c,gearRatio=1, erp=..., relativePositionTarget=relPosTarget, maxForce=...)
//printf("obj->m_constraintId2=%d\n", obj->m_constraintId2);
b3SharedMemoryCommandHandle commandHandle;
commandHandle = b3InitChangeUserConstraintCommand(context->m_physClient, obj->m_constraintId2);
//0 -> open, 1 = closed
double openPos = 1.;
double relPosTarget = openPos - (event.m_analogAxis*openPos);
b3InitChangeUserConstraintSetRelativePositionTarget(commandHandle, relPosTarget);
b3InitChangeUserConstraintSetERP(commandHandle,1);
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, commandHandle);
}
//printf("event.m_analogAxis=%f\n", event.m_analogAxis);
// use the pr2_gripper motors to keep the gripper centered/symmetric around the center axis
if (obj->m_gripperId>=0)
{
//this block is similar to
//b = p.getJointState(pr2_gripper,2)[0]
//print("b = " + str(b))
//p.setJointMotorControl2(pr2_gripper, 0, p.POSITION_CONTROL, targetPosition=b, force=3)
//printf("obj->m_gripperId=%d\n", obj->m_gripperId);
{
b3SharedMemoryCommandHandle cmd_handle =
b3RequestActualStateCommandInit(context->m_physClient, obj->m_gripperId);
b3SharedMemoryStatusHandle status_handle =
b3SubmitClientCommandAndWaitStatus(context->m_physClient, cmd_handle);
int status_type = b3GetStatusType(status_handle);
if (status_type == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
{
//printf("status_type == CMD_ACTUAL_STATE_UPDATE_COMPLETED\n");
b3JointSensorState sensorState;
if (b3GetJointState(context->m_physClient, status_handle, 2, &sensorState))
{
b3SharedMemoryCommandHandle commandHandle;
double targetPosition = sensorState.m_jointPosition;
//printf("targetPosition =%f\n", targetPosition);
if (1)
{
b3JointInfo info;
b3GetJointInfo(context->m_physClient, obj->m_gripperId, 0, &info);
commandHandle = b3JointControlCommandInit2(context->m_physClient, obj->m_gripperId, CONTROL_MODE_POSITION_VELOCITY_PD);
double kp = .1;
double targetVelocity = 0;
double kd = .6;
b3JointControlSetDesiredPosition(commandHandle, info.m_qIndex, targetPosition);
b3JointControlSetKp(commandHandle, info.m_uIndex, kp);
b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex,targetVelocity);
b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, obj->m_maxForce2);
b3SubmitClientCommandAndWaitStatus(context->m_physClient, cmd_handle);
}
} else
{
//printf("???\n");
}
} else
{
//printf("no\n");
}
}
}
}
}
}
}
}
return 0;
}
B3_SHARED_API int executePluginCommand(struct b3PluginContext* context, const struct b3PluginArguments* arguments)
{
MyClass* obj = (MyClass*) context->m_userPointer;
if (arguments->m_numInts>=4 && arguments->m_numFloats >= 2)
{
obj->m_constraintId = arguments->m_ints[1];
obj->m_constraintId2 = arguments->m_ints[2];
obj->m_gripperId = arguments->m_ints[3];
printf("obj->m_constraintId=%d\n", obj->m_constraintId);
obj->m_maxForce = arguments->m_floats[0];
obj->m_maxForce2 = arguments->m_floats[1];
printf("obj->m_maxForce = %f\n", obj->m_maxForce);
obj->m_controllerId = arguments->m_ints[0];
printf("obj->m_controllerId=%d\n", obj->m_controllerId);
b3SharedMemoryCommandHandle command = b3InitSyncBodyInfoCommand(context->m_physClient);
b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, command);
int statusType = b3GetStatusType(statusHandle);
if (statusType != CMD_SYNC_BODY_INFO_COMPLETED)
{
}
}
return 0;
}
B3_SHARED_API void exitPlugin(struct b3PluginContext* context)
{
MyClass* obj = (MyClass*) context->m_userPointer;
delete obj;
context->m_userPointer = 0;
printf("bye vrSyncPlugin!\n");
}

25
examples/SharedMemory/plugins/vrSyncPlugin/vrSyncPlugin.h Normal file
View File

@@ -0,0 +1,25 @@
#ifndef TEST_PLUGIN_H
#define TEST_PLUGIN_H
#include "../b3PluginAPI.h"
#ifdef __cplusplus
extern "C"
{
#endif
//initPlugin, exitPlugin and executePluginCommand are required, otherwise plugin won't load
B3_SHARED_API int initPlugin(struct b3PluginContext* context);
B3_SHARED_API void exitPlugin(struct b3PluginContext* context);
B3_SHARED_API int executePluginCommand(struct b3PluginContext* context, const struct b3PluginArguments* arguments);
//preTickPluginCallback and postTickPluginCallback are optional.
B3_SHARED_API int preTickPluginCallback(struct b3PluginContext* context);
#ifdef __cplusplus
};
#endif
#endif//#define TEST_PLUGIN_H

8
examples/SharedMemory/premake4.lua
View File

@@ -7,7 +7,7 @@ else
kind "ConsoleApp"
end
includedirs {".","../../src", "../ThirdPartyLibs",}
includedirs {".","../../src", "../ThirdPartyLibs"}
links {
"Bullet3Common","BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision", "LinearMath", "BussIK"
@@ -53,6 +53,8 @@ myfiles =
"SharedMemoryCommandProcessor.h",
"PhysicsServerCommandProcessor.cpp",
"PhysicsServerCommandProcessor.h",
"b3PluginManager.cpp",
"b3PluginManager.h",
"TinyRendererVisualShapeConverter.cpp",
"TinyRendererVisualShapeConverter.h",
"../TinyRenderer/geometry.cpp",
@@ -99,6 +101,7 @@ myfiles =
"../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp",
"../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
"../ThirdPartyLibs/stb_image/stb_image.cpp",
}
files {
@@ -405,4 +408,7 @@ end
include "udp"
include "tcp"
include "plugins/testPlugin"
include "plugins/vrSyncPlugin"

3
examples/SharedMemory/tcp/premake4.lua
View File

@@ -88,6 +88,9 @@ myfiles =
"../SharedMemoryPublic.h",
"../PhysicsServerCommandProcessor.cpp",
"../PhysicsServerCommandProcessor.h",
"../b3PluginManager.cpp",
"../PhysicsDirect.cpp",
"../PhysicsClient.cpp",
"../TinyRendererVisualShapeConverter.cpp",
"../TinyRendererVisualShapeConverter.h",
"../../TinyRenderer/geometry.cpp",

3
examples/SharedMemory/udp/premake4.lua
View File

@@ -79,6 +79,9 @@ myfiles =
"../SharedMemoryPublic.h",
"../PhysicsServerCommandProcessor.cpp",
"../PhysicsServerCommandProcessor.h",
"../b3PluginManager.cpp",
"../PhysicsDirect.cpp",
"../PhysicsClient.cpp",
"../TinyRendererVisualShapeConverter.cpp",
"../TinyRendererVisualShapeConverter.h",
"../../TinyRenderer/geometry.cpp",