AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-15 06:00:12 +00:00
Code Issues Releases Wiki Activity
2cab56d6ef
bullet3/examples/SharedMemory/PhysicsClientC_API.cpp
Tigran Gasparian 9c7aa3a863 Changes UserData to use global identifiers and makes linkIndex optional. 
This removes the need to specify the body id/link index when retrieving a user data entry.
Additionally, user data can now optionally be set to visual shapes as well.

The following public pybullet APIs have changed (backwards incompatible)
addUserData and getUserDataId
  Makes linkIndex parameter optional (default value is -1)
  Adds optional visualShapeIndex parameter (default value is -1)

getUserData and removeUserData
  Removes required parameters bodyUniqueId and linkIndex

getNumUserData
  Removes required bodyUniqueId parameter

getUserDataInfo
  Removes required linkIndex parameter
  Changes returned tuple from (userDataId, key) to (userDataId, key, bodyUniqueId, linkIndex, visualShapeIndex)
2018-07-03 17:45:19 +02:00

4807 lines
183 KiB
C++
Raw Blame History

#include "PhysicsClientC_API.h"
#include "PhysicsClientSharedMemory.h"
#include "Bullet3Common/b3Scalar.h"
#include "Bullet3Common/b3Vector3.h"
#include "Bullet3Common/b3Matrix3x3.h"
#include "Bullet3Common/b3Transform.h"
#include <string.h>
#include "SharedMemoryCommands.h"
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_LOAD_SDF;
int len = strlen(sdfFileName);
if (len<MAX_SDF_FILENAME_LENGTH)
{
strcpy(command->m_sdfArguments.m_sdfFileName,sdfFileName);
} else
{
command->m_sdfArguments.m_sdfFileName[0] = 0;
}
command->m_updateFlags = SDF_ARGS_FILE_NAME;
return (b3SharedMemoryCommandHandle) command;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3SaveWorldCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_SAVE_WORLD;
int len = strlen(sdfFileName);
if (len<MAX_SDF_FILENAME_LENGTH)
{
strcpy(command->m_sdfArguments.m_sdfFileName,sdfFileName);
} else
{
command->m_sdfArguments.m_sdfFileName[0] = 0;
}
command->m_updateFlags = SDF_ARGS_FILE_NAME;
return (b3SharedMemoryCommandHandle) command;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
if (cl->canSubmitCommand())
{
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_LOAD_URDF;
int len = strlen(urdfFileName);
if (len < MAX_URDF_FILENAME_LENGTH)
{
strcpy(command->m_urdfArguments.m_urdfFileName, urdfFileName);
}
else
{
command->m_urdfArguments.m_urdfFileName[0] = 0;
}
command->m_updateFlags = URDF_ARGS_FILE_NAME;
return (b3SharedMemoryCommandHandle)command;
}
return 0;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
if (cl->canSubmitCommand())
{
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_LOAD_BULLET;
int len = strlen(fileName);
if (len < MAX_URDF_FILENAME_LENGTH)
{
strcpy(command->m_fileArguments.m_fileName, fileName);
}
else
{
command->m_fileArguments.m_fileName[0] = 0;
}
command->m_updateFlags = 0;
return (b3SharedMemoryCommandHandle)command;
}
return 0;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadStateCommandInit(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
if (cl->canSubmitCommand())
{
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_RESTORE_STATE;
command->m_updateFlags = 0;
command->m_loadStateArguments.m_fileName[0] = 0;
command->m_loadStateArguments.m_stateId = -1;
return (b3SharedMemoryCommandHandle)command;
}
return 0;
}
B3_SHARED_API int b3LoadStateSetStateId(b3SharedMemoryCommandHandle commandHandle, int stateId)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_RESTORE_STATE);
if (command->m_type == CMD_RESTORE_STATE)
{
command->m_loadStateArguments.m_stateId = stateId;
command->m_updateFlags |= CMD_LOAD_STATE_HAS_STATEID;
}
return 0;
}
B3_SHARED_API int b3LoadStateSetFileName(b3SharedMemoryCommandHandle commandHandle, const char* fileName)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_RESTORE_STATE);
if (command->m_type == CMD_RESTORE_STATE)
{
int len = strlen(fileName);
if (len < MAX_URDF_FILENAME_LENGTH)
{
strcpy(command->m_loadStateArguments.m_fileName, fileName);
}
else
{
command->m_loadStateArguments.m_fileName[0] = 0;
}
command->m_updateFlags |= CMD_LOAD_STATE_HAS_FILENAME;
}
return 0;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3SaveStateCommandInit(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
if (cl->canSubmitCommand())
{
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_SAVE_STATE;
command->m_updateFlags = 0;
return (b3SharedMemoryCommandHandle)command;
}
return 0;
}
B3_SHARED_API int b3GetStatusGetStateId(b3SharedMemoryStatusHandle statusHandle)
{
int stateId = -1;
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
b3Assert(status);
b3Assert(status->m_type == CMD_SAVE_STATE_COMPLETED);
if (status && status->m_type == CMD_SAVE_STATE_COMPLETED)
{
stateId = status->m_saveStateResultArgs.m_stateId;
}
return stateId;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3SaveBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
if (cl->canSubmitCommand())
{
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_SAVE_BULLET;
int len = strlen(fileName);
if (len < MAX_URDF_FILENAME_LENGTH)
{
strcpy(command->m_fileArguments.m_fileName, fileName);
}
else
{
command->m_fileArguments.m_fileName[0] = 0;
}
command->m_updateFlags = 0;
return (b3SharedMemoryCommandHandle)command;
}
return 0;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadMJCFCommandInit(b3PhysicsClientHandle physClient, const char* fileName)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
if (cl->canSubmitCommand())
{
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_LOAD_MJCF;
int len = strlen(fileName);
if (len < MAX_URDF_FILENAME_LENGTH)
{
strcpy(command->m_mjcfArguments.m_mjcfFileName, fileName);
}
else
{
command->m_mjcfArguments.m_mjcfFileName[0] = 0;
}
command->m_updateFlags = 0;
return (b3SharedMemoryCommandHandle)command;
}
return 0;
}
B3_SHARED_API void b3LoadMJCFCommandSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_LOAD_MJCF);
if (command->m_type == CMD_LOAD_MJCF)
{
command->m_mjcfArguments.m_flags = flags;
command->m_updateFlags |= URDF_ARGS_HAS_CUSTOM_URDF_FLAGS;
}
}
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadSoftBodyCommandInit(b3PhysicsClientHandle physClient, const char* fileName)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
if (cl->canSubmitCommand())
{
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_LOAD_SOFT_BODY;
int len = strlen(fileName);
if (len < MAX_FILENAME_LENGTH)
{
strcpy(command->m_loadSoftBodyArguments.m_fileName, fileName);
}
else
{
command->m_loadSoftBodyArguments.m_fileName[0] = 0;
}
command->m_updateFlags = LOAD_SOFT_BODY_FILE_NAME;
return (b3SharedMemoryCommandHandle) command;
}
return 0;
}
B3_SHARED_API int b3LoadSoftBodySetScale(b3SharedMemoryCommandHandle commandHandle, double scale)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
command->m_loadSoftBodyArguments.m_scale = scale;
command->m_updateFlags |= LOAD_SOFT_BODY_UPDATE_SCALE;
return 0;
}
B3_SHARED_API int b3LoadSoftBodySetMass(b3SharedMemoryCommandHandle commandHandle, double mass)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
command->m_loadSoftBodyArguments.m_mass = mass;
command->m_updateFlags |= LOAD_SOFT_BODY_UPDATE_MASS;
return 0;
}
B3_SHARED_API int b3LoadSoftBodySetCollisionMargin(b3SharedMemoryCommandHandle commandHandle, double collisionMargin)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_LOAD_SOFT_BODY);
command->m_loadSoftBodyArguments.m_collisionMargin = collisionMargin;
command->m_updateFlags |= LOAD_SOFT_BODY_UPDATE_COLLISION_MARGIN;
return 0;
}
B3_SHARED_API int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_LOAD_URDF);
command->m_updateFlags |=URDF_ARGS_USE_MULTIBODY;
command->m_urdfArguments.m_useMultiBody = useMultiBody;
return 0;
}
B3_SHARED_API int b3LoadUrdfCommandSetGlobalScaling(b3SharedMemoryCommandHandle commandHandle, double globalScaling)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_LOAD_URDF);
command->m_updateFlags |=URDF_ARGS_USE_GLOBAL_SCALING;
command->m_urdfArguments.m_globalScaling = globalScaling;
return 0;
}
B3_SHARED_API int b3LoadSdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_LOAD_SDF);
command->m_updateFlags |=URDF_ARGS_USE_MULTIBODY;
command->m_sdfArguments.m_useMultiBody = useMultiBody;
return 0;
}
B3_SHARED_API int b3LoadSdfCommandSetUseGlobalScaling(b3SharedMemoryCommandHandle commandHandle, double globalScaling)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_LOAD_SDF);
command->m_updateFlags |=URDF_ARGS_USE_GLOBAL_SCALING;
command->m_sdfArguments.m_globalScaling = globalScaling;
return 0;
}
B3_SHARED_API int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_LOAD_URDF);
if (command && (command->m_type == CMD_LOAD_URDF))
{
command->m_updateFlags |= URDF_ARGS_USE_FIXED_BASE;
command->m_urdfArguments.m_useFixedBase = useFixedBase;
return 0;
}
return -1;
}
B3_SHARED_API int b3LoadUrdfCommandSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_LOAD_URDF);
if (command && (command->m_type == CMD_LOAD_URDF))
{
command->m_updateFlags |= URDF_ARGS_HAS_CUSTOM_URDF_FLAGS;
command->m_urdfArguments.m_urdfFlags = flags;
}
return 0;
}
B3_SHARED_API int b3LoadUrdfCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
if (command)
{
b3Assert(command->m_type == CMD_LOAD_URDF);
if (command->m_type == CMD_LOAD_URDF)
{
command->m_urdfArguments.m_initialPosition[0] = startPosX;
command->m_urdfArguments.m_initialPosition[1] = startPosY;
command->m_urdfArguments.m_initialPosition[2] = startPosZ;
command->m_updateFlags |= URDF_ARGS_INITIAL_POSITION;
}
return 0;
}
return -1;
}
B3_SHARED_API int b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
if (command)
{
b3Assert(command->m_type == CMD_LOAD_URDF);
if (command->m_type == CMD_LOAD_URDF)
{
command->m_urdfArguments.m_initialOrientation[0] = startOrnX;
command->m_urdfArguments.m_initialOrientation[1] = startOrnY;
command->m_urdfArguments.m_initialOrientation[2] = startOrnZ;
command->m_urdfArguments.m_initialOrientation[3] = startOrnW;
command->m_updateFlags |= URDF_ARGS_INITIAL_ORIENTATION;
}
return 0;
}
return -1;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestPhysicsParamCommand(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS;
command->m_updateFlags = 0;
return (b3SharedMemoryCommandHandle) command;
}
B3_SHARED_API int b3GetStatusPhysicsSimulationParameters(b3SharedMemoryStatusHandle statusHandle,struct b3PhysicsSimulationParameters* params)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
b3Assert(status);
b3Assert(status->m_type == CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED);
if (status && status->m_type == CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED)
{
*params = status->m_simulationParameterResultArgs;
return 1;
}
return 0;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_SEND_PHYSICS_SIMULATION_PARAMETERS;
command->m_updateFlags = 0;
return (b3SharedMemoryCommandHandle) command;
}
B3_SHARED_API int b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx,double gravy, double gravz)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_physSimParamArgs.m_gravityAcceleration[0] = gravx;
command->m_physSimParamArgs.m_gravityAcceleration[1] = gravy;
command->m_physSimParamArgs.m_gravityAcceleration[2] = gravz;
command->m_updateFlags |= SIM_PARAM_UPDATE_GRAVITY;
return 0;
}
B3_SHARED_API int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_physSimParamArgs.m_useRealTimeSimulation = (enableRealTimeSimulation!=0);
command->m_updateFlags |= SIM_PARAM_UPDATE_REAL_TIME_SIMULATION;
return 0;
}
B3_SHARED_API int b3PhysicsParamSetInternalSimFlags(b3SharedMemoryCommandHandle commandHandle, int flags)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_physSimParamArgs.m_internalSimFlags = flags;
command->m_updateFlags |= SIM_PARAM_UPDATE_INTERNAL_SIMULATION_FLAGS;
return 0;
}
B3_SHARED_API int b3PhysicsParamSetUseSplitImpulse(b3SharedMemoryCommandHandle commandHandle, int useSplitImpulse)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_physSimParamArgs.m_useSplitImpulse = useSplitImpulse;
command->m_updateFlags |= SIM_PARAM_UPDATE_USE_SPLIT_IMPULSE;
return 0;
}
B3_SHARED_API int b3PhysicsParamSetSplitImpulsePenetrationThreshold(b3SharedMemoryCommandHandle commandHandle, double splitImpulsePenetrationThreshold)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_physSimParamArgs.m_splitImpulsePenetrationThreshold = splitImpulsePenetrationThreshold;
command->m_updateFlags |= SIM_PARAM_UPDATE_SPLIT_IMPULSE_PENETRATION_THRESHOLD;
return 0;
}
B3_SHARED_API int b3PhysicsParamSetContactBreakingThreshold(b3SharedMemoryCommandHandle commandHandle, double contactBreakingThreshold)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_physSimParamArgs.m_contactBreakingThreshold = contactBreakingThreshold;
command->m_updateFlags |= SIM_PARAM_UPDATE_CONTACT_BREAKING_THRESHOLD;
return 0;
}
B3_SHARED_API int b3PhysicsParamSetMaxNumCommandsPer1ms(b3SharedMemoryCommandHandle commandHandle, int maxNumCmdPer1ms)
{
//obsolete command
return 0;
}
B3_SHARED_API int b3PhysicsParamSetEnableFileCaching(b3SharedMemoryCommandHandle commandHandle, int enableFileCaching)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_physSimParamArgs.m_enableFileCaching= enableFileCaching;
command->m_updateFlags |= SIM_PARAM_ENABLE_FILE_CACHING ;
return 0;
}
B3_SHARED_API int b3PhysicsParamSetRestitutionVelocityThreshold(b3SharedMemoryCommandHandle commandHandle, double restitutionVelocityThreshold)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_physSimParamArgs.m_restitutionVelocityThreshold = restitutionVelocityThreshold;
command->m_updateFlags |= SIM_PARAM_UPDATE_RESTITUTION_VELOCITY_THRESHOLD ;
return 0;
}
B3_SHARED_API int b3PhysicsParamSetEnableConeFriction(b3SharedMemoryCommandHandle commandHandle, int enableConeFriction)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_physSimParamArgs.m_enableConeFriction = enableConeFriction;
command->m_updateFlags |= SIM_PARAM_ENABLE_CONE_FRICTION;
return 0;
}
B3_SHARED_API int b3PhysicsParameterSetDeterministicOverlappingPairs(b3SharedMemoryCommandHandle commandHandle, int deterministicOverlappingPairs)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_physSimParamArgs.m_deterministicOverlappingPairs = deterministicOverlappingPairs;
command->m_updateFlags |= SIM_PARAM_UPDATE_DETERMINISTIC_OVERLAPPING_PAIRS;
return 0;
}
B3_SHARED_API int b3PhysicsParameterSetAllowedCcdPenetration(b3SharedMemoryCommandHandle commandHandle, double allowedCcdPenetration)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_physSimParamArgs.m_allowedCcdPenetration = allowedCcdPenetration;
command->m_updateFlags |= SIM_PARAM_UPDATE_CCD_ALLOWED_PENETRATION ;
return 0;
}
B3_SHARED_API int b3PhysicsParameterSetJointFeedbackMode(b3SharedMemoryCommandHandle commandHandle, int jointFeedbackMode)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_physSimParamArgs.m_jointFeedbackMode = jointFeedbackMode;
command->m_updateFlags |= SIM_PARAM_UPDATE_JOINT_FEEDBACK_MODE;
return 0;
}
B3_SHARED_API int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_physSimParamArgs.m_numSolverIterations = numSolverIterations;
command->m_updateFlags |= SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS;
return 0;
}
B3_SHARED_API int b3PhysicsParamSetSolverResidualThreshold(b3SharedMemoryCommandHandle commandHandle, double solverResidualThreshold)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_physSimParamArgs.m_solverResidualThreshold = solverResidualThreshold;
command->m_updateFlags |= SIM_PARAM_UPDATE_SOLVER_RESIDULAL_THRESHOLD;
return 0;
}
B3_SHARED_API int b3PhysicsParamSetContactSlop(b3SharedMemoryCommandHandle commandHandle, double contactSlop)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_physSimParamArgs.m_contactSlop = contactSlop;
command->m_updateFlags |= SIM_PARAM_UPDATE_CONTACT_SLOP;
return 0;
}
B3_SHARED_API int b3PhysicsParameterSetEnableSAT(b3SharedMemoryCommandHandle commandHandle, int enableSAT)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_physSimParamArgs.m_enableSAT = enableSAT;
command->m_updateFlags |= SIM_PARAM_ENABLE_SAT;
return 0;
}
B3_SHARED_API int b3PhysicsParamSetCollisionFilterMode(b3SharedMemoryCommandHandle commandHandle, int filterMode)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_physSimParamArgs.m_collisionFilterMode = filterMode;
command->m_updateFlags |= SIM_PARAM_UPDATE_COLLISION_FILTER_MODE;
return 0;
}
B3_SHARED_API int b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double timeStep)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_updateFlags |= SIM_PARAM_UPDATE_DELTA_TIME;
command->m_physSimParamArgs.m_deltaTime = timeStep;
return 0;
}
B3_SHARED_API int b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int numSubSteps)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_updateFlags |= SIM_PARAM_UPDATE_NUM_SIMULATION_SUB_STEPS;
command->m_physSimParamArgs.m_numSimulationSubSteps = numSubSteps;
return 0;
}
B3_SHARED_API int b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultContactERP)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_updateFlags |= SIM_PARAM_UPDATE_DEFAULT_CONTACT_ERP;
command->m_physSimParamArgs.m_defaultContactERP = defaultContactERP;
return 0;
}
B3_SHARED_API int b3PhysicsParamSetDefaultNonContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultNonContactERP)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_updateFlags |= SIM_PARAM_UPDATE_DEFAULT_NON_CONTACT_ERP;
command->m_physSimParamArgs.m_defaultNonContactERP = defaultNonContactERP;
return 0;
}
B3_SHARED_API int b3PhysicsParamSetDefaultFrictionERP(b3SharedMemoryCommandHandle commandHandle, double frictionERP)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_updateFlags |= SIM_PARAM_UPDATE_DEFAULT_FRICTION_ERP;
command->m_physSimParamArgs.m_frictionERP = frictionERP;
return 0;
}
B3_SHARED_API int b3PhysicsParamSetDefaultGlobalCFM(b3SharedMemoryCommandHandle commandHandle, double defaultGlobalCFM)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_updateFlags |= SIM_PARAM_UPDATE_DEFAULT_GLOBAL_CFM;
command->m_physSimParamArgs.m_defaultGlobalCFM = defaultGlobalCFM;
return 0;
}
B3_SHARED_API int b3PhysicsParamSetDefaultFrictionCFM(b3SharedMemoryCommandHandle commandHandle, double frictionCFM)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_updateFlags |= SIM_PARAM_UPDATE_DEFAULT_FRICTION_CFM;
command->m_physSimParamArgs.m_frictionCFM = frictionCFM;
return 0;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_STEP_FORWARD_SIMULATION;
command->m_updateFlags = 0;
return (b3SharedMemoryCommandHandle) command;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitResetSimulationCommand(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_RESET_SIMULATION;
command->m_updateFlags = 0;
return (b3SharedMemoryCommandHandle) command;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode)
{
return b3JointControlCommandInit2(physClient,0,controlMode);
}
B3_SHARED_API b3SharedMemoryCommandHandle b3JointControlCommandInit2( b3PhysicsClientHandle physClient, int bodyUniqueId, int controlMode)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_SEND_DESIRED_STATE;
command->m_sendDesiredStateCommandArgument.m_controlMode = controlMode;
command->m_sendDesiredStateCommandArgument.m_bodyUniqueId = bodyUniqueId;
command->m_updateFlags = 0;
for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
{
command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[i] = 0;
}
return (b3SharedMemoryCommandHandle) command;
}
B3_SHARED_API int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, int qIndex, double value)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
if ((qIndex>=0) && (qIndex < MAX_DEGREE_OF_FREEDOM))
{
command->m_sendDesiredStateCommandArgument.m_desiredStateQ[qIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_Q;
command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[qIndex] |= SIM_DESIRED_STATE_HAS_Q;
}
return 0;
}
B3_SHARED_API int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
if ((dofIndex>=0) && (dofIndex < MAX_DEGREE_OF_FREEDOM))
{
command->m_sendDesiredStateCommandArgument.m_Kp[dofIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KP;
command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_KP;
}
return 0;
}
B3_SHARED_API int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
if ((dofIndex>=0) && (dofIndex < MAX_DEGREE_OF_FREEDOM))
{
command->m_sendDesiredStateCommandArgument.m_Kd[dofIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KD;
command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_KD;
}
return 0;
}
B3_SHARED_API int b3JointControlSetMaximumVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double maximumVelocity)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
if ((dofIndex>=0) && (dofIndex < MAX_DEGREE_OF_FREEDOM))
{
command->m_sendDesiredStateCommandArgument.m_rhsClamp[dofIndex] = maximumVelocity;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_RHS_CLAMP;
command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_RHS_CLAMP;
}
return 0;
}
B3_SHARED_API int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
if ((dofIndex>=0) && (dofIndex < MAX_DEGREE_OF_FREEDOM))
{
command->m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_QDOT;
command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_QDOT;
}
return 0;
}
B3_SHARED_API int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
if ((dofIndex>=0) && (dofIndex < MAX_DEGREE_OF_FREEDOM))
{
command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
}
return 0;
}
B3_SHARED_API int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
if ((dofIndex>=0) && (dofIndex < MAX_DEGREE_OF_FREEDOM))
{
command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
}
return 0;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type =CMD_REQUEST_ACTUAL_STATE;
command->m_updateFlags = 0;
command->m_requestActualStateInformationCommandArgument.m_bodyUniqueId = bodyUniqueId;
return (b3SharedMemoryCommandHandle) command;
}
B3_SHARED_API int b3RequestActualStateCommandComputeLinkVelocity(b3SharedMemoryCommandHandle commandHandle, int computeLinkVelocity)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
btAssert(command->m_type == CMD_REQUEST_ACTUAL_STATE);
if (computeLinkVelocity && command->m_type == CMD_REQUEST_ACTUAL_STATE)
{
command->m_updateFlags |= ACTUAL_STATE_COMPUTE_LINKVELOCITY;
}
return 0;
}
B3_SHARED_API int b3RequestActualStateCommandComputeForwardKinematics(b3SharedMemoryCommandHandle commandHandle, int computeForwardKinematics)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
btAssert(command->m_type == CMD_REQUEST_ACTUAL_STATE);
if (computeForwardKinematics && command->m_type == CMD_REQUEST_ACTUAL_STATE)
{
command->m_updateFlags |= ACTUAL_STATE_COMPUTE_FORWARD_KINEMATICS;
}
return 0;
}
B3_SHARED_API int b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, b3JointSensorState *state)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
b3Assert(status);
int bodyIndex = status->m_sendActualStateArgs.m_bodyUniqueId;
b3Assert(bodyIndex>=0);
if (bodyIndex>=0)
{
b3JointInfo info;
bool result = b3GetJointInfo(physClient, bodyIndex,jointIndex, &info)!=0;
if (result)
{
if ((info.m_qIndex>=0) && (info.m_uIndex>=0) && (info.m_qIndex < MAX_DEGREE_OF_FREEDOM) && (info.m_uIndex < MAX_DEGREE_OF_FREEDOM))
{
state->m_jointPosition = status->m_sendActualStateArgs.m_actualStateQ[info.m_qIndex];
state->m_jointVelocity = status->m_sendActualStateArgs.m_actualStateQdot[info.m_uIndex];
} else
{
state->m_jointPosition=0;
state->m_jointVelocity=0;
}
for (int ii(0); ii < 6; ++ii)
{
state->m_jointForceTorque[ii] = status->m_sendActualStateArgs.m_jointReactionForces[6 * jointIndex + ii];
}
state->m_jointMotorTorque = status->m_sendActualStateArgs.m_jointMotorForce[jointIndex];
return 1;
}
}
return 0;
}
B3_SHARED_API int b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int linkIndex, b3LinkState *state)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
b3Assert(status);
int bodyIndex = status->m_sendActualStateArgs.m_bodyUniqueId;
b3Assert(bodyIndex>=0);
b3Assert(linkIndex >= 0);
int numJoints = b3GetNumJoints(physClient,bodyIndex);
b3Assert(linkIndex < numJoints);
if ((bodyIndex>=0) && (linkIndex >= 0) && linkIndex < numJoints)
{
b3Transform wlf,com,inertial;
for (int i = 0; i < 3; ++i)
{
state->m_worldPosition[i] = status->m_sendActualStateArgs.m_linkState[7 * linkIndex + i];
state->m_localInertialPosition[i] = status->m_sendActualStateArgs.m_linkLocalInertialFrames[7 * linkIndex + i];
state->m_worldLinearVelocity[i] = status->m_sendActualStateArgs.m_linkWorldVelocities[6*linkIndex+i];
state->m_worldAngularVelocity[i] = status->m_sendActualStateArgs.m_linkWorldVelocities[6*linkIndex+i+3];
}
for (int i = 0; i < 4; ++i)
{
state->m_worldOrientation[i] = status->m_sendActualStateArgs.m_linkState[7 * linkIndex + 3 + i];
state->m_localInertialOrientation[i] = status->m_sendActualStateArgs.m_linkLocalInertialFrames[7 * linkIndex + 3 + i];
}
com.setOrigin(b3MakeVector3(state->m_worldPosition[0],state->m_worldPosition[1],state->m_worldPosition[2]));
com.setRotation(b3Quaternion(state->m_worldOrientation[0],state->m_worldOrientation[1],state->m_worldOrientation[2],state->m_worldOrientation[3]));
inertial.setOrigin(b3MakeVector3(state->m_localInertialPosition[0],state->m_localInertialPosition[1],state->m_localInertialPosition[2]));
inertial.setRotation(b3Quaternion(state->m_localInertialOrientation[0],state->m_localInertialOrientation[1],state->m_localInertialOrientation[2],state->m_localInertialOrientation[3]));
wlf = com*inertial.inverse();
for (int i = 0; i < 3; ++i)
{
state->m_worldLinkFramePosition[i] = wlf.getOrigin()[i];
}
b3Quaternion wlfOrn = wlf.getRotation();
for (int i = 0; i < 4; ++i)
{
state->m_worldLinkFrameOrientation[i] = wlfOrn[i];
}
return 1;
}
return 0;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3CreateCollisionShapeCommandInit(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
if (cl)
{
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_CREATE_COLLISION_SHAPE;
command->m_updateFlags =0;
command->m_createUserShapeArgs.m_numUserShapes = 0;
return (b3SharedMemoryCommandHandle) command;
}
return 0;
}
B3_SHARED_API int b3CreateCollisionShapeAddSphere(b3SharedMemoryCommandHandle commandHandle,double radius)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert((command->m_type == CMD_CREATE_COLLISION_SHAPE)|| (command->m_type == CMD_CREATE_VISUAL_SHAPE));
if ((command->m_type == CMD_CREATE_COLLISION_SHAPE)|| (command->m_type == CMD_CREATE_VISUAL_SHAPE))
{
int shapeIndex = command->m_createUserShapeArgs.m_numUserShapes;
if (shapeIndex <MAX_COMPOUND_COLLISION_SHAPES)
{
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_type = GEOM_SPHERE;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_collisionFlags = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_visualFlags = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_sphereRadius = radius;
command->m_createUserShapeArgs.m_numUserShapes++;
return shapeIndex;
}
}
return -1;
}
B3_SHARED_API int b3CreateVisualShapeAddSphere(b3SharedMemoryCommandHandle commandHandle,double radius)
{
return b3CreateCollisionShapeAddSphere(commandHandle,radius);
}
B3_SHARED_API int b3CreateCollisionShapeAddBox(b3SharedMemoryCommandHandle commandHandle,double halfExtents[3])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert((command->m_type == CMD_CREATE_COLLISION_SHAPE)|| (command->m_type == CMD_CREATE_VISUAL_SHAPE));
if ((command->m_type == CMD_CREATE_COLLISION_SHAPE)|| (command->m_type == CMD_CREATE_VISUAL_SHAPE))
{
int shapeIndex = command->m_createUserShapeArgs.m_numUserShapes;
if (shapeIndex <MAX_COMPOUND_COLLISION_SHAPES)
{
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_type = GEOM_BOX;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_collisionFlags = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_visualFlags = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_boxHalfExtents[0] = halfExtents[0];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_boxHalfExtents[1] = halfExtents[1];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_boxHalfExtents[2] = halfExtents[2];
command->m_createUserShapeArgs.m_numUserShapes++;
return shapeIndex;
}
}
return -1;
}
B3_SHARED_API int b3CreateVisualShapeAddBox(b3SharedMemoryCommandHandle commandHandle,double halfExtents[/*3*/])
{
return b3CreateCollisionShapeAddBox(commandHandle,halfExtents);
}
B3_SHARED_API int b3CreateCollisionShapeAddCapsule(b3SharedMemoryCommandHandle commandHandle,double radius, double height)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert((command->m_type == CMD_CREATE_COLLISION_SHAPE)|| (command->m_type == CMD_CREATE_VISUAL_SHAPE));
if ((command->m_type == CMD_CREATE_COLLISION_SHAPE)|| (command->m_type == CMD_CREATE_VISUAL_SHAPE))
{
int shapeIndex = command->m_createUserShapeArgs.m_numUserShapes;
if (shapeIndex <MAX_COMPOUND_COLLISION_SHAPES)
{
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_type = GEOM_CAPSULE;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_collisionFlags = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_visualFlags = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_capsuleRadius = radius;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_capsuleHeight = height;
command->m_createUserShapeArgs.m_numUserShapes++;
return shapeIndex;
}
}
return -1;
}
B3_SHARED_API int b3CreateVisualShapeAddCapsule(b3SharedMemoryCommandHandle commandHandle,double radius, double height)
{
return b3CreateCollisionShapeAddCapsule(commandHandle,radius,height);
}
B3_SHARED_API int b3CreateCollisionShapeAddCylinder(b3SharedMemoryCommandHandle commandHandle,double radius, double height)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert((command->m_type == CMD_CREATE_COLLISION_SHAPE)|| (command->m_type == CMD_CREATE_VISUAL_SHAPE));
if ((command->m_type == CMD_CREATE_COLLISION_SHAPE)|| (command->m_type == CMD_CREATE_VISUAL_SHAPE))
{
int shapeIndex = command->m_createUserShapeArgs.m_numUserShapes;
if (shapeIndex <MAX_COMPOUND_COLLISION_SHAPES)
{
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_type = GEOM_CYLINDER;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_collisionFlags = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_visualFlags = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_capsuleRadius = radius;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_capsuleHeight = height;
command->m_createUserShapeArgs.m_numUserShapes++;
return shapeIndex;
}
}
return -1;
}
B3_SHARED_API int b3CreateVisualShapeAddCylinder(b3SharedMemoryCommandHandle commandHandle,double radius, double height)
{
return b3CreateCollisionShapeAddCylinder(commandHandle,radius,height);
}
B3_SHARED_API int b3CreateCollisionShapeAddPlane(b3SharedMemoryCommandHandle commandHandle, double planeNormal[3], double planeConstant)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert((command->m_type == CMD_CREATE_COLLISION_SHAPE)|| (command->m_type == CMD_CREATE_VISUAL_SHAPE));
if ((command->m_type == CMD_CREATE_COLLISION_SHAPE)|| (command->m_type == CMD_CREATE_VISUAL_SHAPE))
{
int shapeIndex = command->m_createUserShapeArgs.m_numUserShapes;
if (shapeIndex <MAX_COMPOUND_COLLISION_SHAPES)
{
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_type = GEOM_PLANE;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_collisionFlags = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_visualFlags = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_planeNormal[0] = planeNormal[0];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_planeNormal[1] = planeNormal[1];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_planeNormal[2] = planeNormal[2];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_planeConstant = planeConstant;
command->m_createUserShapeArgs.m_numUserShapes++;
return shapeIndex;
}
}
return -1;
}
B3_SHARED_API int b3CreateVisualShapeAddPlane(b3SharedMemoryCommandHandle commandHandle, double planeNormal[/*3*/], double planeConstant)
{
return b3CreateCollisionShapeAddPlane(commandHandle,planeNormal,planeConstant);
}
B3_SHARED_API int b3CreateCollisionShapeAddMesh(b3SharedMemoryCommandHandle commandHandle,const char* fileName, double meshScale[3])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert((command->m_type == CMD_CREATE_COLLISION_SHAPE)|| (command->m_type == CMD_CREATE_VISUAL_SHAPE));
if ((command->m_type == CMD_CREATE_COLLISION_SHAPE)|| (command->m_type == CMD_CREATE_VISUAL_SHAPE))
{
int shapeIndex = command->m_createUserShapeArgs.m_numUserShapes;
if (shapeIndex <MAX_COMPOUND_COLLISION_SHAPES && strlen(fileName)<VISUAL_SHAPE_MAX_PATH_LEN)
{
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_type = GEOM_MESH;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_collisionFlags = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_visualFlags = 0;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 0;
strcpy(command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshFileName,fileName);
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[0] = meshScale[0];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[1] = meshScale[1];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshScale[2] = meshScale[2];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_meshFileType = 0;
command->m_createUserShapeArgs.m_numUserShapes++;
return shapeIndex;
}
}
return -1;
}
B3_SHARED_API int b3CreateVisualShapeAddMesh(b3SharedMemoryCommandHandle commandHandle,const char* fileName, double meshScale[/*3*/])
{
return b3CreateCollisionShapeAddMesh(commandHandle,fileName,meshScale);
}
B3_SHARED_API void b3CreateCollisionSetFlag(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, int flags)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert((command->m_type == CMD_CREATE_COLLISION_SHAPE)|| (command->m_type == CMD_CREATE_VISUAL_SHAPE));
if ((command->m_type == CMD_CREATE_COLLISION_SHAPE)|| (command->m_type == CMD_CREATE_VISUAL_SHAPE))
{
if (shapeIndex<command->m_createUserShapeArgs.m_numUserShapes)
{
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_collisionFlags |= flags;
}
}
}
B3_SHARED_API void b3CreateVisualSetFlag(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, int flags)
{
b3CreateCollisionSetFlag(commandHandle,shapeIndex,flags);
}
B3_SHARED_API void b3CreateCollisionShapeSetChildTransform(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, double childPosition[3], double childOrientation[4])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert((command->m_type == CMD_CREATE_COLLISION_SHAPE)|| (command->m_type == CMD_CREATE_VISUAL_SHAPE));
if ((command->m_type == CMD_CREATE_COLLISION_SHAPE)|| (command->m_type == CMD_CREATE_VISUAL_SHAPE))
{
if (shapeIndex<command->m_createUserShapeArgs.m_numUserShapes)
{
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_hasChildTransform = 1;
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_childPosition[0] = childPosition[0];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_childPosition[1] = childPosition[1];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_childPosition[2] = childPosition[2];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_childOrientation[0] = childOrientation[0];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_childOrientation[1] = childOrientation[1];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_childOrientation[2] = childOrientation[2];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_childOrientation[3] = childOrientation[3];
}
}
}
B3_SHARED_API void b3CreateVisualShapeSetChildTransform(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, double childPosition[/*3*/], double childOrientation[/*4*/])
{
b3CreateCollisionShapeSetChildTransform(commandHandle,shapeIndex,childPosition,childOrientation);
}
B3_SHARED_API void b3CreateVisualShapeSetRGBAColor(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, double rgbaColor[/*4*/])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert((command->m_type == CMD_CREATE_COLLISION_SHAPE)|| (command->m_type == CMD_CREATE_VISUAL_SHAPE));
if ((command->m_type == CMD_CREATE_COLLISION_SHAPE)|| (command->m_type == CMD_CREATE_VISUAL_SHAPE))
{
if (shapeIndex<command->m_createUserShapeArgs.m_numUserShapes)
{
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_rgbaColor[0] = rgbaColor[0];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_rgbaColor[1] = rgbaColor[1];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_rgbaColor[2] = rgbaColor[2];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_rgbaColor[3] = rgbaColor[3];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_visualFlags |= GEOM_VISUAL_HAS_RGBA_COLOR;
}
}
}
B3_SHARED_API void b3CreateVisualShapeSetSpecularColor(b3SharedMemoryCommandHandle commandHandle,int shapeIndex, double specularColor[/*3*/])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert((command->m_type == CMD_CREATE_COLLISION_SHAPE)|| (command->m_type == CMD_CREATE_VISUAL_SHAPE));
if ((command->m_type == CMD_CREATE_COLLISION_SHAPE)|| (command->m_type == CMD_CREATE_VISUAL_SHAPE))
{
if (shapeIndex<command->m_createUserShapeArgs.m_numUserShapes)
{
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_specularColor[0] = specularColor[0];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_specularColor[1] = specularColor[1];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_specularColor[2] = specularColor[2];
command->m_createUserShapeArgs.m_shapes[shapeIndex].m_visualFlags |= GEOM_VISUAL_HAS_SPECULAR_COLOR;
}
}
}
B3_SHARED_API int b3GetStatusCollisionShapeUniqueId(b3SharedMemoryStatusHandle statusHandle)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
b3Assert(status);
b3Assert(status->m_type == CMD_CREATE_COLLISION_SHAPE_COMPLETED);
if (status && status->m_type == CMD_CREATE_COLLISION_SHAPE_COMPLETED)
{
return status->m_createUserShapeResultArgs.m_userShapeUniqueId;
}
return -1;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3CreateVisualShapeCommandInit(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
if (cl)
{
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_CREATE_VISUAL_SHAPE;
command->m_updateFlags =0;
command->m_createUserShapeArgs.m_numUserShapes= 0;
return (b3SharedMemoryCommandHandle) command;
}
return 0;
}
B3_SHARED_API int b3GetStatusVisualShapeUniqueId(b3SharedMemoryStatusHandle statusHandle)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
b3Assert(status);
b3Assert(status->m_type == CMD_CREATE_VISUAL_SHAPE_COMPLETED);
if (status && status->m_type == CMD_CREATE_VISUAL_SHAPE_COMPLETED)
{
return status->m_createUserShapeResultArgs.m_userShapeUniqueId;
}
return -1;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3CreateMultiBodyCommandInit(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
if (cl)
{
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_CREATE_MULTI_BODY;
command->m_updateFlags =0;
command->m_createMultiBodyArgs.m_bodyName[0] = 0;
command->m_createMultiBodyArgs.m_baseLinkIndex = -1;
command->m_createMultiBodyArgs.m_numLinks = 0;
return (b3SharedMemoryCommandHandle) command;
}
return 0;
}
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])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CREATE_MULTI_BODY);
if (command->m_type==CMD_CREATE_MULTI_BODY)
{
int numLinks = command->m_createMultiBodyArgs.m_numLinks;
if (numLinks<MAX_CREATE_MULTI_BODY_LINKS)
{
int baseLinkIndex = numLinks;
command->m_updateFlags |=MULTI_BODY_HAS_BASE;
command->m_createMultiBodyArgs.m_baseLinkIndex = baseLinkIndex;
command->m_createMultiBodyArgs.m_linkPositions[baseLinkIndex*3+0]=basePosition[0];
command->m_createMultiBodyArgs.m_linkPositions[baseLinkIndex*3+1]=basePosition[1];
command->m_createMultiBodyArgs.m_linkPositions[baseLinkIndex*3+2]=basePosition[2];
command->m_createMultiBodyArgs.m_linkOrientations[baseLinkIndex*4+0]=baseOrientation[0];
command->m_createMultiBodyArgs.m_linkOrientations[baseLinkIndex*4+1]=baseOrientation[1];
command->m_createMultiBodyArgs.m_linkOrientations[baseLinkIndex*4+2]=baseOrientation[2];
command->m_createMultiBodyArgs.m_linkOrientations[baseLinkIndex*4+3]=baseOrientation[3];
command->m_createMultiBodyArgs.m_linkInertias[baseLinkIndex*3+0] = 0;//unused, is computed automatically. Will add a method to explicitly set it (with a flag), similar to loadURDF etc.
command->m_createMultiBodyArgs.m_linkInertias[baseLinkIndex*3+1] = 0;
command->m_createMultiBodyArgs.m_linkInertias[baseLinkIndex*3+2] = 0;
command->m_createMultiBodyArgs.m_linkInertialFramePositions[baseLinkIndex*3+0] = baseInertialFramePosition[0];
command->m_createMultiBodyArgs.m_linkInertialFramePositions[baseLinkIndex*3+1] = baseInertialFramePosition[1];
command->m_createMultiBodyArgs.m_linkInertialFramePositions[baseLinkIndex*3+2] = baseInertialFramePosition[2];
command->m_createMultiBodyArgs.m_linkInertialFrameOrientations[baseLinkIndex*4+0] = baseInertialFrameOrientation[0];
command->m_createMultiBodyArgs.m_linkInertialFrameOrientations[baseLinkIndex*4+1] = baseInertialFrameOrientation[1];
command->m_createMultiBodyArgs.m_linkInertialFrameOrientations[baseLinkIndex*4+2] = baseInertialFrameOrientation[2];
command->m_createMultiBodyArgs.m_linkInertialFrameOrientations[baseLinkIndex*4+3] = baseInertialFrameOrientation[3];
command->m_createMultiBodyArgs.m_linkCollisionShapeUniqueIds[baseLinkIndex]= collisionShapeUnique;
command->m_createMultiBodyArgs.m_linkVisualShapeUniqueIds[baseLinkIndex] = visualShapeUniqueId;
command->m_createMultiBodyArgs.m_linkMasses[baseLinkIndex] = mass;
command->m_createMultiBodyArgs.m_linkParentIndices[baseLinkIndex] = -2;//no parent
command->m_createMultiBodyArgs.m_linkJointAxis[baseLinkIndex+0]=0;
command->m_createMultiBodyArgs.m_linkJointAxis[baseLinkIndex+1]=0;
command->m_createMultiBodyArgs.m_linkJointAxis[baseLinkIndex+2]=0;
command->m_createMultiBodyArgs.m_linkJointTypes[baseLinkIndex]=-1;
command->m_createMultiBodyArgs.m_numLinks++;
}
return numLinks;
}
return -2;
}
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],
int linkParentIndex,
int linkJointType,
double linkJointAxis[3])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CREATE_MULTI_BODY);
if (command->m_type==CMD_CREATE_MULTI_BODY)
{
int numLinks = command->m_createMultiBodyArgs.m_numLinks;
if (numLinks<MAX_CREATE_MULTI_BODY_LINKS)
{
int linkIndex = numLinks;
command->m_updateFlags |=MULTI_BODY_HAS_BASE;
command->m_createMultiBodyArgs.m_linkPositions[linkIndex*3+0]=linkPosition[0];
command->m_createMultiBodyArgs.m_linkPositions[linkIndex*3+1]=linkPosition[1];
command->m_createMultiBodyArgs.m_linkPositions[linkIndex*3+2]=linkPosition[2];
command->m_createMultiBodyArgs.m_linkOrientations[linkIndex*4+0]=linkOrientation[0];
command->m_createMultiBodyArgs.m_linkOrientations[linkIndex*4+1]=linkOrientation[1];
command->m_createMultiBodyArgs.m_linkOrientations[linkIndex*4+2]=linkOrientation[2];
command->m_createMultiBodyArgs.m_linkOrientations[linkIndex*4+3]=linkOrientation[3];
command->m_createMultiBodyArgs.m_linkInertias[linkIndex*3+0] = linkMass;
command->m_createMultiBodyArgs.m_linkInertias[linkIndex*3+1] = linkMass;
command->m_createMultiBodyArgs.m_linkInertias[linkIndex*3+2] = linkMass;
command->m_createMultiBodyArgs.m_linkInertialFramePositions[linkIndex*3+0] = linkInertialFramePosition[0];
command->m_createMultiBodyArgs.m_linkInertialFramePositions[linkIndex*3+1] = linkInertialFramePosition[1];
command->m_createMultiBodyArgs.m_linkInertialFramePositions[linkIndex*3+2] = linkInertialFramePosition[2];
command->m_createMultiBodyArgs.m_linkInertialFrameOrientations[linkIndex*4+0] = linkInertialFrameOrientation[0];
command->m_createMultiBodyArgs.m_linkInertialFrameOrientations[linkIndex*4+1] = linkInertialFrameOrientation[1];
command->m_createMultiBodyArgs.m_linkInertialFrameOrientations[linkIndex*4+2] = linkInertialFrameOrientation[2];
command->m_createMultiBodyArgs.m_linkInertialFrameOrientations[linkIndex*4+3] = linkInertialFrameOrientation[3];
command->m_createMultiBodyArgs.m_linkCollisionShapeUniqueIds[linkIndex]= linkCollisionShapeIndex;
command->m_createMultiBodyArgs.m_linkVisualShapeUniqueIds[linkIndex] = linkVisualShapeIndex;
command->m_createMultiBodyArgs.m_linkParentIndices[linkIndex] = linkParentIndex;
command->m_createMultiBodyArgs.m_linkJointTypes[linkIndex] = linkJointType;
command->m_createMultiBodyArgs.m_linkJointAxis[3*linkIndex+0] = linkJointAxis[0];
command->m_createMultiBodyArgs.m_linkJointAxis[3*linkIndex+1] = linkJointAxis[1];
command->m_createMultiBodyArgs.m_linkJointAxis[3*linkIndex+2] = linkJointAxis[2];
command->m_createMultiBodyArgs.m_linkMasses[linkIndex] = linkMass;
command->m_createMultiBodyArgs.m_numLinks++;
return numLinks;
}
}
return -1;
}
//useMaximalCoordinates are disabled by default, enabling them is experimental and not fully supported yet
B3_SHARED_API void b3CreateMultiBodyUseMaximalCoordinates(b3SharedMemoryCommandHandle commandHandle)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CREATE_MULTI_BODY);
if (command->m_type==CMD_CREATE_MULTI_BODY)
{
command->m_updateFlags |= MULT_BODY_USE_MAXIMAL_COORDINATES;
}
}
B3_SHARED_API void b3CreateMultiBodySetFlags(b3SharedMemoryCommandHandle commandHandle, int flags)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CREATE_MULTI_BODY);
if (command->m_type==CMD_CREATE_MULTI_BODY)
{
command->m_updateFlags |= MULT_BODY_HAS_FLAGS;
command->m_createMultiBodyArgs.m_flags = flags;
}
}
B3_SHARED_API int b3GetStatusMultiBodyUniqueId(b3SharedMemoryStatusHandle statusHandle)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
b3Assert(status);
b3Assert(status->m_type == CMD_CREATE_MULTI_BODY_COMPLETED);
if (status && status->m_type == CMD_CREATE_MULTI_BODY_COMPLETED)
{
return status->m_createMultiBodyResultArgs.m_bodyUniqueId;
}
return -1;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3CreateBoxShapeCommandInit(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_CREATE_BOX_COLLISION_SHAPE;
command->m_updateFlags =0;
return (b3SharedMemoryCommandHandle) command;
}
B3_SHARED_API int b3CreateBoxCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CREATE_BOX_COLLISION_SHAPE);
command->m_updateFlags |=BOX_SHAPE_HAS_INITIAL_POSITION;
command->m_createBoxShapeArguments.m_initialPosition[0] = startPosX;
command->m_createBoxShapeArguments.m_initialPosition[1] = startPosY;
command->m_createBoxShapeArguments.m_initialPosition[2] = startPosZ;
return 0;
}
B3_SHARED_API int b3CreateBoxCommandSetHalfExtents(b3SharedMemoryCommandHandle commandHandle, double halfExtentsX,double halfExtentsY,double halfExtentsZ)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CREATE_BOX_COLLISION_SHAPE);
command->m_updateFlags |=BOX_SHAPE_HAS_HALF_EXTENTS;
command->m_createBoxShapeArguments.m_halfExtentsX = halfExtentsX;
command->m_createBoxShapeArguments.m_halfExtentsY = halfExtentsY;
command->m_createBoxShapeArguments.m_halfExtentsZ = halfExtentsZ;
return 0;
}
B3_SHARED_API int b3CreateBoxCommandSetMass(b3SharedMemoryCommandHandle commandHandle, double mass)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CREATE_BOX_COLLISION_SHAPE);
command->m_updateFlags |=BOX_SHAPE_HAS_MASS;
command->m_createBoxShapeArguments.m_mass = mass;
return 0;
}
B3_SHARED_API int b3CreateBoxCommandSetCollisionShapeType(b3SharedMemoryCommandHandle commandHandle, int collisionShapeType)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CREATE_BOX_COLLISION_SHAPE);
command->m_updateFlags |=BOX_SHAPE_HAS_COLLISION_SHAPE_TYPE;
command->m_createBoxShapeArguments.m_collisionShapeType = collisionShapeType;
return 0;
}
B3_SHARED_API int b3CreateBoxCommandSetColorRGBA(b3SharedMemoryCommandHandle commandHandle, double red,double green,double blue, double alpha)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CREATE_BOX_COLLISION_SHAPE);
command->m_updateFlags |=BOX_SHAPE_HAS_COLOR;
command->m_createBoxShapeArguments.m_colorRGBA[0] = red;
command->m_createBoxShapeArguments.m_colorRGBA[1] = green;
command->m_createBoxShapeArguments.m_colorRGBA[2] = blue;
command->m_createBoxShapeArguments.m_colorRGBA[3] = alpha;
return 0;
}
B3_SHARED_API int b3CreateBoxCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CREATE_BOX_COLLISION_SHAPE);
command->m_updateFlags |=BOX_SHAPE_HAS_INITIAL_ORIENTATION;
command->m_createBoxShapeArguments.m_initialOrientation[0] = startOrnX;
command->m_createBoxShapeArguments.m_initialOrientation[1] = startOrnY;
command->m_createBoxShapeArguments.m_initialOrientation[2] = startOrnZ;
command->m_createBoxShapeArguments.m_initialOrientation[3] = startOrnW;
return 0;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_INIT_POSE;
command->m_updateFlags =0;
command->m_initPoseArgs.m_bodyUniqueId = bodyUniqueId;
//a bit slow, initialing the full range to zero...
for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
{
command->m_initPoseArgs.m_hasInitialStateQ[i] = 0;
command->m_initPoseArgs.m_hasInitialStateQdot[i] = 0;
}
return (b3SharedMemoryCommandHandle) command;
}
B3_SHARED_API int b3CreatePoseCommandSetBasePosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_INIT_POSE);
command->m_updateFlags |=INIT_POSE_HAS_INITIAL_POSITION;
command->m_initPoseArgs.m_initialStateQ[0] = startPosX;
command->m_initPoseArgs.m_initialStateQ[1] = startPosY;
command->m_initPoseArgs.m_initialStateQ[2] = startPosZ;
command->m_initPoseArgs.m_hasInitialStateQ[0] = 1;
command->m_initPoseArgs.m_hasInitialStateQ[1] = 1;
command->m_initPoseArgs.m_hasInitialStateQ[2] = 1;
return 0;
}
B3_SHARED_API int b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_INIT_POSE);
command->m_updateFlags |=INIT_POSE_HAS_INITIAL_ORIENTATION;
command->m_initPoseArgs.m_initialStateQ[3] = startOrnX;
command->m_initPoseArgs.m_initialStateQ[4] = startOrnY;
command->m_initPoseArgs.m_initialStateQ[5] = startOrnZ;
command->m_initPoseArgs.m_initialStateQ[6] = startOrnW;
command->m_initPoseArgs.m_hasInitialStateQ[3] = 1;
command->m_initPoseArgs.m_hasInitialStateQ[4] = 1;
command->m_initPoseArgs.m_hasInitialStateQ[5] = 1;
command->m_initPoseArgs.m_hasInitialStateQ[6] = 1;
return 0;
}
B3_SHARED_API int b3CreatePoseCommandSetBaseLinearVelocity(b3SharedMemoryCommandHandle commandHandle, double linVel[3])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_INIT_POSE);
command->m_updateFlags |= INIT_POSE_HAS_BASE_LINEAR_VELOCITY;
command->m_initPoseArgs.m_hasInitialStateQdot[0] = 1;
command->m_initPoseArgs.m_hasInitialStateQdot[1] = 1;
command->m_initPoseArgs.m_hasInitialStateQdot[2] = 1;
command->m_initPoseArgs.m_initialStateQdot[0] = linVel[0];
command->m_initPoseArgs.m_initialStateQdot[1] = linVel[1];
command->m_initPoseArgs.m_initialStateQdot[2] = linVel[2];
return 0;
}
B3_SHARED_API int b3CreatePoseCommandSetBaseAngularVelocity(b3SharedMemoryCommandHandle commandHandle, double angVel[3])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_INIT_POSE);
command->m_updateFlags |= INIT_POSE_HAS_BASE_ANGULAR_VELOCITY;
command->m_initPoseArgs.m_hasInitialStateQdot[3] = 1;
command->m_initPoseArgs.m_hasInitialStateQdot[4] = 1;
command->m_initPoseArgs.m_hasInitialStateQdot[5] = 1;
command->m_initPoseArgs.m_initialStateQdot[3] = angVel[0];
command->m_initPoseArgs.m_initialStateQdot[4] = angVel[1];
command->m_initPoseArgs.m_initialStateQdot[5] = angVel[2];
return 0;
}
B3_SHARED_API int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHandle, int numJointPositions, const double* jointPositions)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_INIT_POSE);
command->m_updateFlags |=INIT_POSE_HAS_JOINT_STATE;
for (int i=0;i<numJointPositions;i++)
{
if ((i+7)<MAX_DEGREE_OF_FREEDOM)
{
command->m_initPoseArgs.m_initialStateQ[i+7] = jointPositions[i];
command->m_initPoseArgs.m_hasInitialStateQ[i+7] = 1;
}
}
return 0;
}
B3_SHARED_API int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointPosition)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_INIT_POSE);
command->m_updateFlags |=INIT_POSE_HAS_JOINT_STATE;
b3JointInfo info;
b3GetJointInfo(physClient, command->m_initPoseArgs.m_bodyUniqueId,jointIndex, &info);
//btAssert((info.m_flags & JOINT_HAS_MOTORIZED_POWER) && info.m_qIndex >=0);
if ((info.m_flags & JOINT_HAS_MOTORIZED_POWER) && info.m_qIndex >=0)
{
command->m_initPoseArgs.m_initialStateQ[info.m_qIndex] = jointPosition;
command->m_initPoseArgs.m_hasInitialStateQ[info.m_qIndex] = 1;
}
return 0;
}
B3_SHARED_API int b3CreatePoseCommandSetJointVelocities(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int numJointVelocities, const double* jointVelocities)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_INIT_POSE);
command->m_updateFlags |=INIT_POSE_HAS_JOINT_VELOCITY;
for (int i=0;i<numJointVelocities;i++)
{
if ((i+6)<MAX_DEGREE_OF_FREEDOM)
{
command->m_initPoseArgs.m_initialStateQdot[i+6] = jointVelocities[i];
command->m_initPoseArgs.m_hasInitialStateQdot[i+6] = 1;
}
}
return 0;
}
B3_SHARED_API int b3CreatePoseCommandSetJointVelocity(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointVelocity)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_INIT_POSE);
command->m_updateFlags |=INIT_POSE_HAS_JOINT_VELOCITY;
b3JointInfo info;
b3GetJointInfo(physClient, command->m_initPoseArgs.m_bodyUniqueId,jointIndex, &info);
//btAssert((info.m_flags & JOINT_HAS_MOTORIZED_POWER) && info.m_uIndex >=0);
if ((info.m_flags & JOINT_HAS_MOTORIZED_POWER) && (info.m_uIndex >=0) && (info.m_uIndex<MAX_DEGREE_OF_FREEDOM))
{
command->m_initPoseArgs.m_initialStateQdot[info.m_uIndex] = jointVelocity;
command->m_initPoseArgs.m_hasInitialStateQdot[info.m_uIndex] = 1;
}
return 0;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_CREATE_SENSOR;
command->m_updateFlags = 0;
command->m_createSensorArguments.m_numJointSensorChanges = 0;
command->m_createSensorArguments.m_bodyUniqueId = bodyUniqueId;
return (b3SharedMemoryCommandHandle) command;
}
B3_SHARED_API int b3CreateSensorEnable6DofJointForceTorqueSensor(b3SharedMemoryCommandHandle commandHandle, int jointIndex, int enable)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CREATE_SENSOR);
int curIndex = command->m_createSensorArguments.m_numJointSensorChanges;
command->m_createSensorArguments.m_sensorType[curIndex] = SENSOR_FORCE_TORQUE;
command->m_createSensorArguments.m_jointIndex[curIndex] = jointIndex;
command->m_createSensorArguments.m_enableJointForceSensor[curIndex] = enable;
command->m_createSensorArguments.m_numJointSensorChanges++;
return 0;
}
B3_SHARED_API int b3CreateSensorEnableIMUForLink(b3SharedMemoryCommandHandle commandHandle, int linkIndex, int enable)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CREATE_SENSOR);
int curIndex = command->m_createSensorArguments.m_numJointSensorChanges;
command->m_createSensorArguments.m_sensorType[curIndex] = SENSOR_IMU;
command->m_createSensorArguments.m_linkIndex[curIndex] = linkIndex;
command->m_createSensorArguments.m_enableSensor[curIndex] = enable;
command->m_createSensorArguments.m_numJointSensorChanges++;
return 0;
}
B3_SHARED_API void b3DisconnectSharedMemory(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
if (cl)
{
cl->disconnectSharedMemory();
}
delete cl;
}
B3_SHARED_API b3SharedMemoryStatusHandle b3ProcessServerStatus(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
if (cl && cl->isConnected())
{
const SharedMemoryStatus* stat = cl->processServerStatus();
return (b3SharedMemoryStatusHandle) stat;
}
return 0;
}
B3_SHARED_API int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
//b3Assert(status);
if (status)
{
return status->m_type;
}
return CMD_INVALID_STATUS;
}
B3_SHARED_API int b3GetStatusBodyIndices(b3SharedMemoryStatusHandle statusHandle, int* bodyIndicesOut, int bodyIndicesCapacity)
{
int numBodies = 0;
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
b3Assert(status);
if (status)
{
switch (status->m_type)
{
case CMD_MJCF_LOADING_COMPLETED:
case CMD_BULLET_LOADING_COMPLETED:
case CMD_SDF_LOADING_COMPLETED:
{
int i,maxBodies;
numBodies = status->m_sdfLoadedArgs.m_numBodies;
maxBodies = btMin(bodyIndicesCapacity, numBodies);
for (i=0;i<maxBodies;i++)
{
bodyIndicesOut[i] = status->m_sdfLoadedArgs.m_bodyUniqueIds[i];
}
break;
}
}
}
return numBodies;
}
B3_SHARED_API int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
b3Assert(status);
int bodyId = -1;
if (status)
{
switch (status->m_type)
{
case CMD_URDF_LOADING_COMPLETED:
{
bodyId = status->m_dataStreamArguments.m_bodyUniqueId;
break;
}
case CMD_RIGID_BODY_CREATION_COMPLETED:
{
bodyId = status->m_rigidBodyCreateArgs.m_bodyUniqueId;
break;
}
case CMD_CREATE_MULTI_BODY_COMPLETED:
{
bodyId = status->m_dataStreamArguments.m_bodyUniqueId;
break;
}
case CMD_LOAD_SOFT_BODY_COMPLETED:
{
bodyId = status->m_loadSoftBodyResultArguments.m_objectUniqueId;
break;
}
default:
{
b3Assert(0);
}
};
}
return bodyId;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3RequestCollisionInfoCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type =CMD_REQUEST_COLLISION_INFO;
command->m_updateFlags = 0;
command->m_requestCollisionInfoArgs.m_bodyUniqueId = bodyUniqueId;
return (b3SharedMemoryCommandHandle) command;
}
B3_SHARED_API int b3GetStatusAABB(b3SharedMemoryStatusHandle statusHandle, int linkIndex, double aabbMin[3], double aabbMax[3])
{
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
btAssert(status);
if (status==0)
return 0;
const b3SendCollisionInfoArgs &args = status->m_sendCollisionInfoArgs;
btAssert(status->m_type == CMD_REQUEST_COLLISION_INFO_COMPLETED);
if (status->m_type != CMD_REQUEST_COLLISION_INFO_COMPLETED)
return 0;
if (linkIndex==-1)
{
aabbMin[0] = args.m_rootWorldAABBMin[0];
aabbMin[1] = args.m_rootWorldAABBMin[1];
aabbMin[2] = args.m_rootWorldAABBMin[2];
aabbMax[0] = args.m_rootWorldAABBMax[0];
aabbMax[1] = args.m_rootWorldAABBMax[1];
aabbMax[2] = args.m_rootWorldAABBMax[2];
return 1;
}
if (linkIndex >= 0 && linkIndex < args.m_numLinks)
{
aabbMin[0] = args.m_linkWorldAABBsMin[linkIndex*3+0];
aabbMin[1] = args.m_linkWorldAABBsMin[linkIndex*3+1];
aabbMin[2] = args.m_linkWorldAABBsMin[linkIndex*3+2];
aabbMax[0] = args.m_linkWorldAABBsMax[linkIndex*3+0];
aabbMax[1] = args.m_linkWorldAABBsMax[linkIndex*3+1];
aabbMax[2] = args.m_linkWorldAABBsMax[linkIndex*3+2];
return 1;
}
return 0;
}
B3_SHARED_API int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId,
int* numDegreeOfFreedomQ,
int* numDegreeOfFreedomU,
const double* rootLocalInertialFrame[],
const double* actualStateQ[],
const double* actualStateQdot[],
const double* jointReactionForces[]) {
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
btAssert(status);
if (status==0)
return 0;
const SendActualStateArgs &args = status->m_sendActualStateArgs;
btAssert(status->m_type == CMD_ACTUAL_STATE_UPDATE_COMPLETED);
if (status->m_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED)
return false;
if (bodyUniqueId) {
*bodyUniqueId = args.m_bodyUniqueId;
}
if (numDegreeOfFreedomQ) {
*numDegreeOfFreedomQ = args.m_numDegreeOfFreedomQ;
}
if (numDegreeOfFreedomU) {
*numDegreeOfFreedomU = args.m_numDegreeOfFreedomU;
}
if (rootLocalInertialFrame) {
*rootLocalInertialFrame = args.m_rootLocalInertialFrame;
}
if (actualStateQ) {
*actualStateQ = args.m_actualStateQ;
}
if (actualStateQdot) {
*actualStateQdot = args.m_actualStateQdot;
}
if (jointReactionForces) {
*jointReactionForces = args.m_jointReactionForces;
}
return true;
}
B3_SHARED_API int b3CanSubmitCommand(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
if (cl)
{
return (int)cl->canSubmitCommand();
}
return false;
}
B3_SHARED_API int b3SubmitClientCommand(b3PhysicsClientHandle physClient, const b3SharedMemoryCommandHandle commandHandle)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(command);
b3Assert(cl);
if (command && cl)
{
return (int)cl->submitClientCommand(*command);
}
return -1;
}
#include "../Utils/b3Clock.h"
B3_SHARED_API b3SharedMemoryStatusHandle b3SubmitClientCommandAndWaitStatus(b3PhysicsClientHandle physClient, const b3SharedMemoryCommandHandle commandHandle)
{
B3_PROFILE("b3SubmitClientCommandAndWaitStatus");
b3Clock clock;
double startTime = clock.getTimeInSeconds();
b3SharedMemoryStatusHandle statusHandle = 0;
b3Assert(commandHandle);
b3Assert(physClient);
if (physClient && commandHandle)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
double timeOutInSeconds = cl->getTimeOut();
{
B3_PROFILE("b3SubmitClientCommand");
b3SubmitClientCommand(physClient, commandHandle);
}
{
B3_PROFILE("b3ProcessServerStatus");
while (cl->isConnected() && (statusHandle == 0) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds))
{
clock.usleep(0);
statusHandle = b3ProcessServerStatus(physClient);
}
}
return (b3SharedMemoryStatusHandle)statusHandle;
}
return 0;
}
///return the total number of bodies in the simulation
B3_SHARED_API int b3GetNumBodies(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
return cl->getNumBodies();
}
B3_SHARED_API int b3GetNumUserConstraints(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
return cl->getNumUserConstraints();
}
B3_SHARED_API int b3GetUserConstraintInfo(b3PhysicsClientHandle physClient, int constraintUniqueId, struct b3UserConstraint* info)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3UserConstraint constraintInfo1;
b3Assert(physClient);
b3Assert(info);
b3Assert(constraintUniqueId>=0);
if (info==0)
return 0;
if (cl->getUserConstraintInfo(constraintUniqueId, constraintInfo1))
{
*info = constraintInfo1;
return 1;
}
return 0;
}
/// return the user constraint id, given the index in range [0 , b3GetNumUserConstraints() )
B3_SHARED_API int b3GetUserConstraintId(b3PhysicsClientHandle physClient, int serialIndex)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
return cl->getUserConstraintId(serialIndex);
}
/// return the body unique id, given the index in range [0 , b3GetNumBodies() )
B3_SHARED_API int b3GetBodyUniqueId(b3PhysicsClientHandle physClient, int serialIndex)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
return cl->getBodyUniqueId(serialIndex);
}
///given a body unique id, return the body information. See b3BodyInfo in SharedMemoryPublic.h
B3_SHARED_API int b3GetBodyInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, struct b3BodyInfo* info)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
return cl->getBodyInfo(bodyUniqueId,*info);
}
B3_SHARED_API int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyUniqueId)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
return cl->getNumJoints(bodyUniqueId);
}
B3_SHARED_API int b3ComputeDofCount(b3PhysicsClientHandle physClient, int bodyUniqueId)
{
int nj = b3GetNumJoints(physClient, bodyUniqueId);
int j=0;
int dofCountOrg = 0;
for (j=0;j<nj;j++)
{
struct b3JointInfo info;
b3GetJointInfo(physClient, bodyUniqueId, j, &info);
switch (info.m_jointType)
{
case eRevoluteType:
{
dofCountOrg+=1;
break;
}
case ePrismaticType:
{
dofCountOrg+=1;
break;
}
case eSphericalType:
{
return -1;
}
case ePlanarType:
{
return -2;
}
default:
{
//fixed joint has 0-dof and at the moment, we don't deal with planar, spherical etc
}
}
}
return dofCountOrg;
}
B3_SHARED_API int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, int jointIndex, struct b3JointInfo* info)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
return cl->getJointInfo(bodyUniqueId, jointIndex, *info);
}
B3_SHARED_API b3SharedMemoryCommandHandle b3CreateCustomCommand(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_CUSTOM_COMMAND;
command->m_updateFlags = 0;
return (b3SharedMemoryCommandHandle)command;
}
B3_SHARED_API void b3CustomCommandLoadPlugin(b3SharedMemoryCommandHandle commandHandle, const char* pluginPath)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_CUSTOM_COMMAND);
if (command->m_type == CMD_CUSTOM_COMMAND)
{
command->m_updateFlags |= CMD_CUSTOM_COMMAND_LOAD_PLUGIN;
command->m_customCommandArgs.m_pluginPath[0] = 0;
int len = strlen(pluginPath);
if (len<MAX_FILENAME_LENGTH)
{
strcpy(command->m_customCommandArgs.m_pluginPath, pluginPath);
}
}
}
B3_SHARED_API void b3CustomCommandLoadPluginSetPostFix(b3SharedMemoryCommandHandle commandHandle, const char* postFix)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_CUSTOM_COMMAND);
if (command->m_type == CMD_CUSTOM_COMMAND)
{
command->m_updateFlags |= CMD_CUSTOM_COMMAND_LOAD_PLUGIN_POSTFIX;
command->m_customCommandArgs.m_postFix[0] = 0;
int len = strlen(postFix);
if (len<MAX_FILENAME_LENGTH)
{
strcpy(command->m_customCommandArgs.m_postFix, postFix);
}
}
}
B3_SHARED_API int b3GetStatusPluginCommandResult(b3SharedMemoryStatusHandle statusHandle)
{
int statusUniqueId = -1;
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
b3Assert(status);
if (status==0)
return statusUniqueId;
b3Assert(status->m_type == CMD_CUSTOM_COMMAND_COMPLETED);
if (status->m_type == CMD_CUSTOM_COMMAND_COMPLETED)
{
statusUniqueId = status->m_customCommandResultArgs.m_executeCommandResult;
}
return statusUniqueId;
}
B3_SHARED_API int b3GetStatusPluginUniqueId(b3SharedMemoryStatusHandle statusHandle)
{
int statusUniqueId = -1;
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
b3Assert(status);
if (status)
{
b3Assert(status->m_type == CMD_CUSTOM_COMMAND_COMPLETED);
if (status->m_type == CMD_CUSTOM_COMMAND_COMPLETED)
{
statusUniqueId = status->m_customCommandResultArgs.m_pluginUniqueId;
}
}
return statusUniqueId;
}
B3_SHARED_API void b3CustomCommandUnloadPlugin(b3SharedMemoryCommandHandle commandHandle, int pluginUniqueId)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_CUSTOM_COMMAND);
if (command->m_type == CMD_CUSTOM_COMMAND)
{
command->m_updateFlags |= CMD_CUSTOM_COMMAND_UNLOAD_PLUGIN;
command->m_customCommandArgs.m_pluginUniqueId = pluginUniqueId;
}
}
B3_SHARED_API void b3CustomCommandExecutePluginCommand(b3SharedMemoryCommandHandle commandHandle, int pluginUniqueId, const char* textArguments)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_CUSTOM_COMMAND);
if (command->m_type == CMD_CUSTOM_COMMAND)
{
command->m_updateFlags |= CMD_CUSTOM_COMMAND_EXECUTE_PLUGIN_COMMAND;
command->m_customCommandArgs.m_pluginUniqueId = pluginUniqueId;
command->m_customCommandArgs.m_arguments.m_numInts = 0;
command->m_customCommandArgs.m_arguments.m_numFloats = 0;
command->m_customCommandArgs.m_arguments.m_text[0] = 0;
int len = strlen(textArguments);
if (len<MAX_FILENAME_LENGTH)
{
strcpy(command->m_customCommandArgs.m_arguments.m_text, textArguments);
}
}
}
B3_SHARED_API void b3CustomCommandExecuteAddIntArgument(b3SharedMemoryCommandHandle commandHandle, int intVal)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_CUSTOM_COMMAND);
b3Assert(command->m_updateFlags & CMD_CUSTOM_COMMAND_EXECUTE_PLUGIN_COMMAND);
if (command->m_type == CMD_CUSTOM_COMMAND && (command->m_updateFlags & CMD_CUSTOM_COMMAND_EXECUTE_PLUGIN_COMMAND))
{
int numInts = command->m_customCommandArgs.m_arguments.m_numInts;
if (numInts<B3_MAX_PLUGIN_ARG_SIZE)
{
command->m_customCommandArgs.m_arguments.m_ints[numInts]=intVal;
command->m_customCommandArgs.m_arguments.m_numInts++;
}
}
}
B3_SHARED_API void b3CustomCommandExecuteAddFloatArgument(b3SharedMemoryCommandHandle commandHandle, float floatVal)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_CUSTOM_COMMAND);
b3Assert(command->m_updateFlags & CMD_CUSTOM_COMMAND_EXECUTE_PLUGIN_COMMAND);
if (command->m_type == CMD_CUSTOM_COMMAND && (command->m_updateFlags & CMD_CUSTOM_COMMAND_EXECUTE_PLUGIN_COMMAND))
{
int numFloats = command->m_customCommandArgs.m_arguments.m_numFloats;
if (numFloats<B3_MAX_PLUGIN_ARG_SIZE)
{
command->m_customCommandArgs.m_arguments.m_floats[numFloats]=floatVal;
command->m_customCommandArgs.m_arguments.m_numFloats++;
}
}
}
B3_SHARED_API b3SharedMemoryCommandHandle b3GetDynamicsInfoCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_GET_DYNAMICS_INFO;
command->m_getDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
command->m_getDynamicsInfoArgs.m_linkIndex = linkIndex;
return (b3SharedMemoryCommandHandle) command;
}
B3_SHARED_API int b3GetDynamicsInfo(b3SharedMemoryStatusHandle statusHandle, struct b3DynamicsInfo* info)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
const b3DynamicsInfo &dynamicsInfo = status->m_dynamicsInfo;
btAssert(status->m_type == CMD_GET_DYNAMICS_INFO_COMPLETED);
if (status->m_type != CMD_GET_DYNAMICS_INFO_COMPLETED)
return false;
if (info)
{
*info = dynamicsInfo;
return true;
}
return false;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitChangeDynamicsInfo(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_CHANGE_DYNAMICS_INFO;
command->m_changeDynamicsInfoArgs.m_bodyUniqueId = -1;
command->m_changeDynamicsInfoArgs.m_linkIndex = -2;
command->m_updateFlags = 0;
return (b3SharedMemoryCommandHandle) command;
}
B3_SHARED_API int b3ChangeDynamicsInfoSetMass(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double mass)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
b3Assert(mass > 0);
command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex;
command->m_changeDynamicsInfoArgs.m_mass = mass;
command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_MASS;
return 0;
}
B3_SHARED_API int b3ChangeDynamicsInfoSetLocalInertiaDiagonal(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double localInertiaDiagonal[])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex;
command->m_changeDynamicsInfoArgs.m_localInertiaDiagonal[0] = localInertiaDiagonal[0];
command->m_changeDynamicsInfoArgs.m_localInertiaDiagonal[1] = localInertiaDiagonal[1];
command->m_changeDynamicsInfoArgs.m_localInertiaDiagonal[2] = localInertiaDiagonal[2];
command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_LOCAL_INERTIA_DIAGONAL;
return 0;
}
B3_SHARED_API int b3ChangeDynamicsInfoSetLateralFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double lateralFriction)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex;
command->m_changeDynamicsInfoArgs.m_lateralFriction = lateralFriction;
command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION;
return 0;
}
B3_SHARED_API int b3ChangeDynamicsInfoSetSpinningFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex;
command->m_changeDynamicsInfoArgs.m_spinningFriction = friction;
command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION;
return 0;
}
B3_SHARED_API int b3ChangeDynamicsInfoSetRollingFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double friction)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex;
command->m_changeDynamicsInfoArgs.m_rollingFriction = friction;
command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION;
return 0;
}
B3_SHARED_API int b3ChangeDynamicsInfoSetRestitution(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double restitution)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex;
command->m_changeDynamicsInfoArgs.m_restitution = restitution;
command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_RESTITUTION;
return 0;
}
B3_SHARED_API int b3ChangeDynamicsInfoSetLinearDamping(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId,double linearDamping)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
command->m_changeDynamicsInfoArgs.m_linearDamping = linearDamping;
command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING;
return 0;
}
B3_SHARED_API int b3ChangeDynamicsInfoSetAngularDamping(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId,double angularDamping)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
command->m_changeDynamicsInfoArgs.m_angularDamping = angularDamping;
command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING;
return 0;
}
B3_SHARED_API int b3ChangeDynamicsInfoSetContactStiffnessAndDamping(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex,double contactStiffness, double contactDamping)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex;
command->m_changeDynamicsInfoArgs.m_contactStiffness =contactStiffness;
command->m_changeDynamicsInfoArgs.m_contactDamping = contactDamping;
command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING;
return 0;
}
B3_SHARED_API int b3ChangeDynamicsInfoSetFrictionAnchor(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex, int frictionAnchor)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex;
command->m_changeDynamicsInfoArgs.m_frictionAnchor = frictionAnchor;
command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR;
return 0;
}
B3_SHARED_API int b3ChangeDynamicsInfoSetCcdSweptSphereRadius(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex, double ccdSweptSphereRadius)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex;
command->m_changeDynamicsInfoArgs.m_ccdSweptSphereRadius = ccdSweptSphereRadius;
command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_CCD_SWEPT_SPHERE_RADIUS;
return 0;
}
B3_SHARED_API int b3ChangeDynamicsInfoSetContactProcessingThreshold(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double contactProcessingThreshold)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
command->m_changeDynamicsInfoArgs.m_linkIndex = linkIndex;
command->m_changeDynamicsInfoArgs.m_contactProcessingThreshold = contactProcessingThreshold;
command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_CONTACT_PROCESSING_THRESHOLD;
return 0;
}
B3_SHARED_API int b3ChangeDynamicsInfoSetActivationState(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int activationState)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
command->m_changeDynamicsInfoArgs.m_linkIndex = -1;
command->m_changeDynamicsInfoArgs.m_activationState = activationState;
command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_ACTIVATION_STATE;
return 0;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyUniqueId, int parentJointIndex, int childBodyUniqueId, int childJointIndex, struct b3JointInfo* info)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_USER_CONSTRAINT;
command->m_updateFlags = USER_CONSTRAINT_ADD_CONSTRAINT;
command->m_userConstraintArguments.m_parentBodyIndex = parentBodyUniqueId;
command->m_userConstraintArguments.m_parentJointIndex = parentJointIndex;
command->m_userConstraintArguments.m_childBodyIndex = childBodyUniqueId;
command->m_userConstraintArguments.m_childJointIndex = childJointIndex;
for (int i = 0; i < 7; ++i) {
command->m_userConstraintArguments.m_parentFrame[i] = info->m_parentFrame[i];
command->m_userConstraintArguments.m_childFrame[i] = info->m_childFrame[i];
}
for (int i = 0; i < 3; ++i) {
command->m_userConstraintArguments.m_jointAxis[i] = info->m_jointAxis[i];
}
command->m_userConstraintArguments.m_jointType = info->m_jointType;
return (b3SharedMemoryCommandHandle)command;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitChangeUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_USER_CONSTRAINT;
command->m_updateFlags = USER_CONSTRAINT_CHANGE_CONSTRAINT;
command->m_userConstraintArguments.m_userConstraintUniqueId = userConstraintUniqueId;
return (b3SharedMemoryCommandHandle)command;
}
B3_SHARED_API int b3InitChangeUserConstraintSetPivotInB(b3SharedMemoryCommandHandle commandHandle, double jointChildPivot[])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_USER_CONSTRAINT);
b3Assert(command->m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT);
command->m_updateFlags |= USER_CONSTRAINT_CHANGE_PIVOT_IN_B;
command->m_userConstraintArguments.m_childFrame[0] = jointChildPivot[0];
command->m_userConstraintArguments.m_childFrame[1] = jointChildPivot[1];
command->m_userConstraintArguments.m_childFrame[2] = jointChildPivot[2];
return 0;
}
B3_SHARED_API int b3InitChangeUserConstraintSetFrameInB(b3SharedMemoryCommandHandle commandHandle, double jointChildFrameOrn[])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_USER_CONSTRAINT);
b3Assert(command->m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT);
command->m_updateFlags |= USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B;
command->m_userConstraintArguments.m_childFrame[3] = jointChildFrameOrn[0];
command->m_userConstraintArguments.m_childFrame[4] = jointChildFrameOrn[1];
command->m_userConstraintArguments.m_childFrame[5] = jointChildFrameOrn[2];
command->m_userConstraintArguments.m_childFrame[6] = jointChildFrameOrn[3];
return 0;
}
B3_SHARED_API int b3InitChangeUserConstraintSetMaxForce(b3SharedMemoryCommandHandle commandHandle, double maxAppliedForce)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_USER_CONSTRAINT);
b3Assert(command->m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT);
command->m_updateFlags |=USER_CONSTRAINT_CHANGE_MAX_FORCE;
command->m_userConstraintArguments.m_maxAppliedForce = maxAppliedForce;
return 0;
}
B3_SHARED_API int b3InitChangeUserConstraintSetGearRatio(b3SharedMemoryCommandHandle commandHandle, double gearRatio)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_USER_CONSTRAINT);
b3Assert(command->m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT);
command->m_updateFlags |=USER_CONSTRAINT_CHANGE_GEAR_RATIO;
command->m_userConstraintArguments.m_gearRatio = gearRatio;
return 0;
}
B3_SHARED_API int b3InitChangeUserConstraintSetGearAuxLink(b3SharedMemoryCommandHandle commandHandle, int gearAuxLink)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_USER_CONSTRAINT);
b3Assert(command->m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT);
command->m_updateFlags |=USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK;
command->m_userConstraintArguments.m_gearAuxLink = gearAuxLink;
return 0;
}
B3_SHARED_API int b3InitChangeUserConstraintSetRelativePositionTarget(b3SharedMemoryCommandHandle commandHandle, double relativePositionTarget)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_USER_CONSTRAINT);
b3Assert(command->m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT);
command->m_updateFlags |=USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET;
command->m_userConstraintArguments.m_relativePositionTarget = relativePositionTarget;
return 0;
}
B3_SHARED_API int b3InitChangeUserConstraintSetERP(b3SharedMemoryCommandHandle commandHandle, double erp)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_USER_CONSTRAINT);
b3Assert(command->m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT);
command->m_updateFlags |=USER_CONSTRAINT_CHANGE_ERP;
command->m_userConstraintArguments.m_erp = erp;
return 0;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitGetUserConstraintStateCommand(b3PhysicsClientHandle physClient, int constraintUniqueId)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_USER_CONSTRAINT;
command->m_updateFlags = USER_CONSTRAINT_REQUEST_STATE;
command->m_userConstraintArguments.m_userConstraintUniqueId = constraintUniqueId;
return (b3SharedMemoryCommandHandle)command;
}
B3_SHARED_API int b3GetStatusUserConstraintState(b3SharedMemoryStatusHandle statusHandle, struct b3UserConstraintState* constraintState)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
if (status)
{
btAssert(status->m_type == CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED);
if (status && status->m_type == CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED)
{
int i = 0;
constraintState->m_numDofs = status->m_userConstraintStateResultArgs.m_numDofs;
for (i = 0; i < constraintState->m_numDofs; i++)
{
constraintState->m_appliedConstraintForces[i] = status->m_userConstraintStateResultArgs.m_appliedConstraintForces[i];
}
for (; i < 6; i++)
{
constraintState->m_appliedConstraintForces[i] = 0;
}
return 1;
}
}
return 0;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_USER_CONSTRAINT;
command->m_updateFlags = USER_CONSTRAINT_REMOVE_CONSTRAINT;
command->m_userConstraintArguments.m_userConstraintUniqueId = userConstraintUniqueId;
return (b3SharedMemoryCommandHandle)command;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveBodyCommand(b3PhysicsClientHandle physClient, int bodyUniqueId)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_REMOVE_BODY;
command->m_updateFlags = BODY_DELETE_FLAG;
command->m_removeObjectArgs.m_numBodies = 1;
command->m_removeObjectArgs.m_bodyUniqueIds[0] = bodyUniqueId;
command->m_removeObjectArgs.m_numUserConstraints = 0;
return (b3SharedMemoryCommandHandle)command;
}
B3_SHARED_API int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle statusHandle)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
b3Assert(status);
if (status)
{
b3Assert(status->m_type == CMD_USER_CONSTRAINT_COMPLETED);
if (status && status->m_type == CMD_USER_CONSTRAINT_COMPLETED)
{
return status->m_userConstraintResultArgs.m_userConstraintUniqueId;
}
}
return -1;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3PickBody(b3PhysicsClientHandle physClient, double rayFromWorldX,
double rayFromWorldY, double rayFromWorldZ,
double rayToWorldX, double rayToWorldY, double rayToWorldZ)
{
PhysicsClient *cl = (PhysicsClient *)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand *command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_PICK_BODY;
command->m_pickBodyArguments.m_rayFromWorld[0] = rayFromWorldX;
command->m_pickBodyArguments.m_rayFromWorld[1] = rayFromWorldY;
command->m_pickBodyArguments.m_rayFromWorld[2] = rayFromWorldZ;
command->m_pickBodyArguments.m_rayToWorld[0] = rayToWorldX;
command->m_pickBodyArguments.m_rayToWorld[1] = rayToWorldY;
command->m_pickBodyArguments.m_rayToWorld[2] = rayToWorldZ;
return (b3SharedMemoryCommandHandle)command;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3MovePickedBody(b3PhysicsClientHandle physClient, double rayFromWorldX,
double rayFromWorldY, double rayFromWorldZ,
double rayToWorldX, double rayToWorldY,
double rayToWorldZ)
{
PhysicsClient *cl = (PhysicsClient *)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand *command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_MOVE_PICKED_BODY;
command->m_pickBodyArguments.m_rayFromWorld[0] = rayFromWorldX;
command->m_pickBodyArguments.m_rayFromWorld[1] = rayFromWorldY;
command->m_pickBodyArguments.m_rayFromWorld[2] = rayFromWorldZ;
command->m_pickBodyArguments.m_rayToWorld[0] = rayToWorldX;
command->m_pickBodyArguments.m_rayToWorld[1] = rayToWorldY;
command->m_pickBodyArguments.m_rayToWorld[2] = rayToWorldZ;
return (b3SharedMemoryCommandHandle)command;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3RemovePickingConstraint(b3PhysicsClientHandle physClient)
{
PhysicsClient *cl = (PhysicsClient *)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand *command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_REMOVE_PICKING_CONSTRAINT_BODY;
return (b3SharedMemoryCommandHandle)command;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3CreateRaycastCommandInit(b3PhysicsClientHandle physClient, double rayFromWorldX,
double rayFromWorldY, double rayFromWorldZ,
double rayToWorldX, double rayToWorldY, double rayToWorldZ)
{
PhysicsClient *cl = (PhysicsClient *)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand *command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS;
command->m_requestRaycastIntersections.m_numCommandRays = 1;
command->m_requestRaycastIntersections.m_numStreamingRays = 0;
command->m_requestRaycastIntersections.m_numThreads = 1;
command->m_requestRaycastIntersections.m_numCommandRays = 1;
command->m_requestRaycastIntersections.m_fromToRays[0].m_rayFromPosition[0] = rayFromWorldX;
command->m_requestRaycastIntersections.m_fromToRays[0].m_rayFromPosition[1] = rayFromWorldY;
command->m_requestRaycastIntersections.m_fromToRays[0].m_rayFromPosition[2] = rayFromWorldZ;
command->m_requestRaycastIntersections.m_fromToRays[0].m_rayToPosition[0] = rayToWorldX;
command->m_requestRaycastIntersections.m_fromToRays[0].m_rayToPosition[1] = rayToWorldY;
command->m_requestRaycastIntersections.m_fromToRays[0].m_rayToPosition[2] = rayToWorldZ;
return (b3SharedMemoryCommandHandle)command;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3CreateRaycastBatchCommandInit(b3PhysicsClientHandle physClient)
{
PhysicsClient *cl = (PhysicsClient *)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand *command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS;
command->m_updateFlags = 0;
command->m_requestRaycastIntersections.m_numCommandRays = 0;
command->m_requestRaycastIntersections.m_numStreamingRays = 0;
command->m_requestRaycastIntersections.m_numThreads = 1;
return (b3SharedMemoryCommandHandle)command;
}
B3_SHARED_API void b3RaycastBatchSetNumThreads(b3SharedMemoryCommandHandle commandHandle, int numThreads) {
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS);
command->m_requestRaycastIntersections.m_numThreads = numThreads;
}
B3_SHARED_API void b3RaycastBatchAddRay(b3SharedMemoryCommandHandle commandHandle, const double rayFromWorld[3], const double rayToWorld[3])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS);
if (command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS)
{
int numRays = command->m_requestRaycastIntersections.m_numCommandRays;
if (numRays<MAX_RAY_INTERSECTION_BATCH_SIZE)
{
command->m_requestRaycastIntersections.m_fromToRays[numRays].m_rayFromPosition[0] = rayFromWorld[0];
command->m_requestRaycastIntersections.m_fromToRays[numRays].m_rayFromPosition[1] = rayFromWorld[1];
command->m_requestRaycastIntersections.m_fromToRays[numRays].m_rayFromPosition[2] = rayFromWorld[2];
command->m_requestRaycastIntersections.m_fromToRays[numRays].m_rayToPosition[0] = rayToWorld[0];
command->m_requestRaycastIntersections.m_fromToRays[numRays].m_rayToPosition[1] = rayToWorld[1];
command->m_requestRaycastIntersections.m_fromToRays[numRays].m_rayToPosition[2] = rayToWorld[2];
command->m_requestRaycastIntersections.m_numCommandRays++;
}
}
}
B3_SHARED_API void b3RaycastBatchAddRays(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS);
b3Assert(numRays<MAX_RAY_INTERSECTION_BATCH_SIZE_STREAMING);
if (command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS)
{
cl->uploadRaysToSharedMemory(*command, rayFromWorldArray, rayToWorldArray, numRays);
}
}
B3_SHARED_API void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastInformation* raycastInfo)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
if (cl)
{
cl->getCachedRaycastHits(raycastInfo);
}
}
///If you re-connected to an existing server, or server changed otherwise, sync the body info
B3_SHARED_API b3SharedMemoryCommandHandle b3InitSyncBodyInfoCommand(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type =CMD_SYNC_BODY_INFO;
return (b3SharedMemoryCommandHandle) command;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitSyncUserDataCommand(b3PhysicsClientHandle physClient) {
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_SYNC_USER_DATA;
return (b3SharedMemoryCommandHandle) command;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitAddUserDataCommand(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int visualShapeIndex, const char* key, UserDataValueType valueType, int valueLength, const void *valueData) {
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(strlen(key) < MAX_USER_DATA_KEY_LENGTH);
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_ADD_USER_DATA;
command->m_addUserDataRequestArgs.m_bodyUniqueId = bodyUniqueId;
command->m_addUserDataRequestArgs.m_linkIndex = linkIndex;
command->m_addUserDataRequestArgs.m_visualShapeIndex = visualShapeIndex;
command->m_addUserDataRequestArgs.m_valueType = valueType;
command->m_addUserDataRequestArgs.m_valueLength = valueLength;
strcpy(command->m_addUserDataRequestArgs.m_key, key);
cl->uploadBulletFileToSharedMemory((const char *)valueData, valueLength);
return (b3SharedMemoryCommandHandle) command;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveUserDataCommand(b3PhysicsClientHandle physClient, int userDataId) {
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_REMOVE_USER_DATA;
command->m_removeUserDataRequestArgs.m_userDataId = userDataId;
return (b3SharedMemoryCommandHandle) command;
}
B3_SHARED_API int b3GetUserData(b3PhysicsClientHandle physClient, int userDataId, struct b3UserDataValue *valueOut)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
if (cl)
{
return cl->getCachedUserData(userDataId, *valueOut);
}
return false;
}
B3_SHARED_API int b3GetUserDataId(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, int visualShapeIndex, const char *key)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
if (cl)
{
return cl->getCachedUserDataId(bodyUniqueId, linkIndex, visualShapeIndex, key);
}
return -1;
}
B3_SHARED_API int b3GetUserDataIdFromStatus(b3SharedMemoryStatusHandle statusHandle)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
if (status)
{
btAssert(status->m_type == CMD_ADD_USER_DATA_COMPLETED);
return status->m_userDataResponseArgs.m_userDataId;
}
return -1;
}
B3_SHARED_API int b3GetNumUserData(b3PhysicsClientHandle physClient, int bodyUniqueId)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
if (cl)
{
return cl->getNumUserData(bodyUniqueId);
}
return 0;
}
B3_SHARED_API void b3GetUserDataInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, int userDataIndex, const char **keyOut, int *userDataIdOut, int *linkIndexOut, int *visualShapeIndexOut)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
if (cl)
{
cl->getUserDataInfo(bodyUniqueId, userDataIndex, keyOut, userDataIdOut, linkIndexOut, visualShapeIndexOut);
}
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type =CMD_REQUEST_DEBUG_LINES;
command->m_requestDebugLinesArguments.m_debugMode = debugMode;
command->m_requestDebugLinesArguments.m_startingLineIndex = 0;
return (b3SharedMemoryCommandHandle) command;
}
B3_SHARED_API void b3GetDebugLines(b3PhysicsClientHandle physClient, struct b3DebugLines* lines)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(lines);
if (lines)
{
lines->m_numDebugLines = cl->getNumDebugLines();
lines->m_linesFrom = cl->getDebugLinesFrom();
lines->m_linesTo = cl->getDebugLinesTo();
lines->m_linesColor = cl->getDebugLinesColor();
}
}
/// Add/remove user-specific debug lines and debug text messages
B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugDrawAddLine3D(b3PhysicsClientHandle physClient, double fromXYZ[3], double toXYZ[3], double colorRGB[3], double lineWidth, double lifeTime)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type =CMD_USER_DEBUG_DRAW;
command->m_updateFlags = USER_DEBUG_HAS_LINE; //USER_DEBUG_HAS_TEXT
command->m_userDebugDrawArgs.m_debugLineFromXYZ[0] = fromXYZ[0];
command->m_userDebugDrawArgs.m_debugLineFromXYZ[1] = fromXYZ[1];
command->m_userDebugDrawArgs.m_debugLineFromXYZ[2] = fromXYZ[2];
command->m_userDebugDrawArgs.m_debugLineToXYZ[0] = toXYZ[0];
command->m_userDebugDrawArgs.m_debugLineToXYZ[1] = toXYZ[1];
command->m_userDebugDrawArgs.m_debugLineToXYZ[2] = toXYZ[2];
command->m_userDebugDrawArgs.m_debugLineColorRGB[0] = colorRGB[0];
command->m_userDebugDrawArgs.m_debugLineColorRGB[1] = colorRGB[1];
command->m_userDebugDrawArgs.m_debugLineColorRGB[2] = colorRGB[2];
command->m_userDebugDrawArgs.m_lineWidth = lineWidth;
command->m_userDebugDrawArgs.m_lifeTime = lifeTime;
command->m_userDebugDrawArgs.m_parentObjectUniqueId = -1;
command->m_userDebugDrawArgs.m_parentLinkIndex = -1;
command->m_userDebugDrawArgs.m_optionFlags = 0;
return (b3SharedMemoryCommandHandle) command;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugDrawAddText3D(b3PhysicsClientHandle physClient, const char* txt, double positionXYZ[3], double colorRGB[3], double textSize, double lifeTime)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type =CMD_USER_DEBUG_DRAW;
command->m_updateFlags = USER_DEBUG_HAS_TEXT;
int len = strlen(txt);
if (len<MAX_FILENAME_LENGTH)
{
strcpy(command->m_userDebugDrawArgs.m_text,txt);
} else
{
command->m_userDebugDrawArgs.m_text[0] = 0;
}
command->m_userDebugDrawArgs.m_textPositionXYZ[0] = positionXYZ[0];
command->m_userDebugDrawArgs.m_textPositionXYZ[1] = positionXYZ[1];
command->m_userDebugDrawArgs.m_textPositionXYZ[2] = positionXYZ[2];
command->m_userDebugDrawArgs.m_textColorRGB[0] = colorRGB[0];
command->m_userDebugDrawArgs.m_textColorRGB[1] = colorRGB[1];
command->m_userDebugDrawArgs.m_textColorRGB[2] = colorRGB[2];
command->m_userDebugDrawArgs.m_textSize = textSize;
command->m_userDebugDrawArgs.m_lifeTime = lifeTime;
command->m_userDebugDrawArgs.m_parentObjectUniqueId = -1;
command->m_userDebugDrawArgs.m_parentLinkIndex = -1;
command->m_userDebugDrawArgs.m_optionFlags = 0;
return (b3SharedMemoryCommandHandle) command;
}
B3_SHARED_API void b3UserDebugTextSetOptionFlags(b3SharedMemoryCommandHandle commandHandle, int optionFlags)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_USER_DEBUG_DRAW);
b3Assert(command->m_updateFlags & USER_DEBUG_HAS_TEXT);
command->m_userDebugDrawArgs.m_optionFlags = optionFlags;
command->m_updateFlags |= USER_DEBUG_HAS_OPTION_FLAGS;
}
B3_SHARED_API void b3UserDebugTextSetOrientation(b3SharedMemoryCommandHandle commandHandle, double orientation[4])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_USER_DEBUG_DRAW);
b3Assert(command->m_updateFlags & USER_DEBUG_HAS_TEXT);
command->m_userDebugDrawArgs.m_textOrientation[0] = orientation[0];
command->m_userDebugDrawArgs.m_textOrientation[1] = orientation[1];
command->m_userDebugDrawArgs.m_textOrientation[2] = orientation[2];
command->m_userDebugDrawArgs.m_textOrientation[3] = orientation[3];
command->m_updateFlags |= USER_DEBUG_HAS_TEXT_ORIENTATION;
}
B3_SHARED_API void b3UserDebugItemSetReplaceItemUniqueId(b3SharedMemoryCommandHandle commandHandle, int replaceItemUniqueId)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_USER_DEBUG_DRAW);
command->m_userDebugDrawArgs.m_replaceItemUniqueId = replaceItemUniqueId;
command->m_updateFlags |= USER_DEBUG_HAS_REPLACE_ITEM_UNIQUE_ID;
}
B3_SHARED_API void b3UserDebugItemSetParentObject(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_USER_DEBUG_DRAW);
command->m_updateFlags |= USER_DEBUG_HAS_PARENT_OBJECT;
command->m_userDebugDrawArgs.m_parentObjectUniqueId = objectUniqueId;
command->m_userDebugDrawArgs.m_parentLinkIndex = linkIndex;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugAddParameter(b3PhysicsClientHandle physClient, const char* txt, double rangeMin, double rangeMax, double startValue)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type =CMD_USER_DEBUG_DRAW;
command->m_updateFlags = USER_DEBUG_ADD_PARAMETER;
int len = strlen(txt);
if (len<MAX_FILENAME_LENGTH)
{
strcpy(command->m_userDebugDrawArgs.m_text,txt);
} else
{
command->m_userDebugDrawArgs.m_text[0] = 0;
}
command->m_userDebugDrawArgs.m_rangeMin = rangeMin;
command->m_userDebugDrawArgs.m_rangeMax = rangeMax;
command->m_userDebugDrawArgs.m_startValue = startValue;
command->m_userDebugDrawArgs.m_parentObjectUniqueId = -1;
command->m_userDebugDrawArgs.m_optionFlags = 0;
return (b3SharedMemoryCommandHandle)command;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugReadParameter(b3PhysicsClientHandle physClient, int debugItemUniqueId)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type =CMD_USER_DEBUG_DRAW;
command->m_updateFlags = USER_DEBUG_READ_PARAMETER;
command->m_userDebugDrawArgs.m_itemUniqueId = debugItemUniqueId;
return (b3SharedMemoryCommandHandle) command;
}
B3_SHARED_API int b3GetStatusDebugParameterValue(b3SharedMemoryStatusHandle statusHandle, double* paramValue)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
if (status)
{
btAssert(status->m_type == CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED);
if (paramValue && (status->m_type == CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED))
{
*paramValue = status->m_userDebugDrawArgs.m_parameterValue;
return 1;
}
}
return 0;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugDrawRemove(b3PhysicsClientHandle physClient, int debugItemUniqueId)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type =CMD_USER_DEBUG_DRAW;
command->m_updateFlags = USER_DEBUG_REMOVE_ONE_ITEM;
command->m_userDebugDrawArgs.m_itemUniqueId = debugItemUniqueId;
return (b3SharedMemoryCommandHandle) command;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugDrawRemoveAll(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type =CMD_USER_DEBUG_DRAW;
command->m_updateFlags = USER_DEBUG_REMOVE_ALL;
return (b3SharedMemoryCommandHandle) command;
}
B3_SHARED_API int b3GetDebugItemUniqueId(b3SharedMemoryStatusHandle statusHandle)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
btAssert(status->m_type == CMD_USER_DEBUG_DRAW_COMPLETED);
if (status->m_type != CMD_USER_DEBUG_DRAW_COMPLETED)
return -1;
return status->m_userDebugDrawArgs.m_debugItemUniqueId;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitDebugDrawingCommand(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_USER_DEBUG_DRAW;
command->m_updateFlags = 0;
return (b3SharedMemoryCommandHandle)command;
}
B3_SHARED_API void b3SetDebugObjectColor(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex, double objectColorRGB[3])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_USER_DEBUG_DRAW);
command->m_updateFlags |= USER_DEBUG_SET_CUSTOM_OBJECT_COLOR;
command->m_userDebugDrawArgs.m_objectUniqueId = objectUniqueId;
command->m_userDebugDrawArgs.m_linkIndex = linkIndex;
command->m_userDebugDrawArgs.m_objectDebugColorRGB[0] = objectColorRGB[0];
command->m_userDebugDrawArgs.m_objectDebugColorRGB[1] = objectColorRGB[1];
command->m_userDebugDrawArgs.m_objectDebugColorRGB[2] = objectColorRGB[2];
}
B3_SHARED_API void b3RemoveDebugObjectColor(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId, int linkIndex)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_USER_DEBUG_DRAW);
command->m_updateFlags |= USER_DEBUG_REMOVE_CUSTOM_OBJECT_COLOR;
command->m_userDebugDrawArgs.m_objectUniqueId = objectUniqueId;
command->m_userDebugDrawArgs.m_linkIndex = linkIndex;
}
///request an image from a simulated camera, using a software renderer.
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestCameraImage(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type =CMD_REQUEST_CAMERA_IMAGE_DATA;
command->m_requestPixelDataArguments.m_startPixelIndex = 0;
command->m_updateFlags = 0;//REQUEST_PIXEL_ARGS_USE_HARDWARE_OPENGL;
return (b3SharedMemoryCommandHandle) command;
}
B3_SHARED_API void b3RequestCameraImageSelectRenderer(b3SharedMemoryCommandHandle commandHandle, int renderer)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
b3Assert(renderer>(1<<15));
if (renderer>(1<<15))
{
command->m_updateFlags |= renderer;
}
}
B3_SHARED_API void b3RequestCameraImageSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
if(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA)
{
command->m_requestPixelDataArguments.m_flags = flags;
command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_FLAGS;
}
}
B3_SHARED_API void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle commandHandle, float viewMatrix[16], float projectionMatrix[16])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
for (int i=0;i<16;i++)
{
command->m_requestPixelDataArguments.m_projectionMatrix[i] = projectionMatrix[i];
command->m_requestPixelDataArguments.m_viewMatrix[i] = viewMatrix[i];
}
command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES;
}
B3_SHARED_API void b3RequestCameraImageSetLightDirection(b3SharedMemoryCommandHandle commandHandle, const float lightDirection[3])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
for (int i = 0; i<3; i++)
{
command->m_requestPixelDataArguments.m_lightDirection[i] = lightDirection[i];
}
command->m_updateFlags |= REQUEST_PIXEL_ARGS_SET_LIGHT_DIRECTION;
}
B3_SHARED_API void b3RequestCameraImageSetLightColor(b3SharedMemoryCommandHandle commandHandle, const float lightColor[3])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
for (int i = 0; i<3; i++)
{
command->m_requestPixelDataArguments.m_lightColor[i] = lightColor[i];
}
command->m_updateFlags |= REQUEST_PIXEL_ARGS_SET_LIGHT_COLOR;
}
B3_SHARED_API void b3RequestCameraImageSetLightDistance(b3SharedMemoryCommandHandle commandHandle, float lightDistance)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
command->m_requestPixelDataArguments.m_lightDistance = lightDistance;
command->m_updateFlags |= REQUEST_PIXEL_ARGS_SET_LIGHT_DISTANCE;
}
B3_SHARED_API void b3RequestCameraImageSetLightAmbientCoeff(b3SharedMemoryCommandHandle commandHandle, float lightAmbientCoeff)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
command->m_requestPixelDataArguments.m_lightAmbientCoeff = lightAmbientCoeff;
command->m_updateFlags |= REQUEST_PIXEL_ARGS_SET_AMBIENT_COEFF;
}
B3_SHARED_API void b3RequestCameraImageSetLightDiffuseCoeff(b3SharedMemoryCommandHandle commandHandle, float lightDiffuseCoeff)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
command->m_requestPixelDataArguments.m_lightDiffuseCoeff = lightDiffuseCoeff;
command->m_updateFlags |= REQUEST_PIXEL_ARGS_SET_DIFFUSE_COEFF;
}
B3_SHARED_API void b3RequestCameraImageSetLightSpecularCoeff(b3SharedMemoryCommandHandle commandHandle, float lightSpecularCoeff)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
command->m_requestPixelDataArguments.m_lightSpecularCoeff = lightSpecularCoeff;
command->m_updateFlags |= REQUEST_PIXEL_ARGS_SET_SPECULAR_COEFF;
}
B3_SHARED_API void b3RequestCameraImageSetShadow(b3SharedMemoryCommandHandle commandHandle, int hasShadow)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
command->m_requestPixelDataArguments.m_hasShadow = hasShadow;
command->m_updateFlags |= REQUEST_PIXEL_ARGS_SET_SHADOW;
}
B3_SHARED_API void b3RequestCameraImageSetProjectiveTextureMatrices(b3SharedMemoryCommandHandle commandHandle, float viewMatrix[16], float projectionMatrix[16])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
for (int i=0;i<16;i++)
{
command->m_requestPixelDataArguments.m_projectiveTextureProjectionMatrix[i] = projectionMatrix[i];
command->m_requestPixelDataArguments.m_projectiveTextureViewMatrix[i] = viewMatrix[i];
}
command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_PROJECTIVE_TEXTURE_MATRICES;
}
B3_SHARED_API void b3ComputePositionFromViewMatrix(const float viewMatrix[16], float cameraPosition[3], float cameraTargetPosition[3], float cameraUp[3])
{
b3Matrix3x3 r(viewMatrix[0], viewMatrix[4], viewMatrix[8], viewMatrix[1], viewMatrix[5], viewMatrix[9], viewMatrix[2], viewMatrix[6], viewMatrix[10]);
b3Vector3 p = b3MakeVector3(viewMatrix[12], viewMatrix[13], viewMatrix[14]);
b3Transform t(r,p);
b3Transform tinv = t.inverse();
b3Matrix3x3 basis = tinv.getBasis();
b3Vector3 origin = tinv.getOrigin();
b3Vector3 s = b3MakeVector3(basis[0][0], basis[1][0], basis[2][0]);
b3Vector3 u = b3MakeVector3(basis[0][1], basis[1][1], basis[2][1]);
b3Vector3 f = b3MakeVector3(-basis[0][2], -basis[1][2], -basis[2][2]);
b3Vector3 eye = origin;
cameraPosition[0] = eye[0];
cameraPosition[1] = eye[1];
cameraPosition[2] = eye[2];
b3Vector3 center = f + eye;
cameraTargetPosition[0] = center[0];
cameraTargetPosition[1] = center[1];
cameraTargetPosition[2] = center[2];
cameraUp[0] = u[0];
cameraUp[1] = u[1];
cameraUp[2] = u[2];
}
B3_SHARED_API void b3ComputeViewMatrixFromPositions(const float cameraPosition[3], const float cameraTargetPosition[3], const float cameraUp[3], float viewMatrix[16])
{
b3Vector3 eye = b3MakeVector3(cameraPosition[0], cameraPosition[1], cameraPosition[2]);
b3Vector3 center = b3MakeVector3(cameraTargetPosition[0], cameraTargetPosition[1], cameraTargetPosition[2]);
b3Vector3 up = b3MakeVector3(cameraUp[0], cameraUp[1], cameraUp[2]);
b3Vector3 f = (center - eye).normalized();
b3Vector3 u = up.normalized();
b3Vector3 s = (f.cross(u)).normalized();
u = s.cross(f);
viewMatrix[0 * 4 + 0] = s.x;
viewMatrix[1 * 4 + 0] = s.y;
viewMatrix[2 * 4 + 0] = s.z;
viewMatrix[0 * 4 + 1] = u.x;
viewMatrix[1 * 4 + 1] = u.y;
viewMatrix[2 * 4 + 1] = u.z;
viewMatrix[0 * 4 + 2] = -f.x;
viewMatrix[1 * 4 + 2] = -f.y;
viewMatrix[2 * 4 + 2] = -f.z;
viewMatrix[0 * 4 + 3] = 0.f;
viewMatrix[1 * 4 + 3] = 0.f;
viewMatrix[2 * 4 + 3] = 0.f;
viewMatrix[3 * 4 + 0] = -s.dot(eye);
viewMatrix[3 * 4 + 1] = -u.dot(eye);
viewMatrix[3 * 4 + 2] = f.dot(eye);
viewMatrix[3 * 4 + 3] = 1.f;
}
B3_SHARED_API void b3ComputeViewMatrixFromYawPitchRoll(const float cameraTargetPosition[3], float distance, float yaw, float pitch, float roll, int upAxis, float viewMatrix[16])
{
b3Vector3 camUpVector;
b3Vector3 camForward;
b3Vector3 camPos;
b3Vector3 camTargetPos = b3MakeVector3(cameraTargetPosition[0], cameraTargetPosition[1], cameraTargetPosition[2]);
b3Vector3 eyePos = b3MakeVector3(0, 0, 0);
b3Scalar yawRad = yaw * b3Scalar(0.01745329251994329547);// rads per deg
b3Scalar pitchRad = pitch * b3Scalar(0.01745329251994329547);// rads per deg
b3Scalar rollRad = 0.0;
b3Quaternion eyeRot;
int forwardAxis(-1);
switch (upAxis)
{
case 1:
forwardAxis = 2;
camUpVector = b3MakeVector3(0,1,0);
eyeRot.setEulerZYX(rollRad, yawRad, -pitchRad);
break;
case 2:
forwardAxis = 1;
camUpVector = b3MakeVector3(0,0,1);
eyeRot.setEulerZYX(yawRad, rollRad, pitchRad);
break;
default:
return;
};
eyePos[forwardAxis] = -distance;
camForward = b3MakeVector3(eyePos[0],eyePos[1],eyePos[2]);
if (camForward.length2() < B3_EPSILON)
{
camForward.setValue(1.f,0.f,0.f);
} else
{
camForward.normalize();
}
eyePos = b3Matrix3x3(eyeRot)*eyePos;
camUpVector = b3Matrix3x3(eyeRot)*camUpVector;
camPos = eyePos;
camPos += camTargetPos;
float camPosf[4] = { camPos[0],camPos[1],camPos[2],0 };
float camPosTargetf[4] = { camTargetPos[0],camTargetPos[1],camTargetPos[2],0 };
float camUpf[4] = { camUpVector[0],camUpVector[1],camUpVector[2],0 };
b3ComputeViewMatrixFromPositions(camPosf, camPosTargetf, camUpf,viewMatrix);
}
B3_SHARED_API void b3ComputeProjectionMatrix(float left, float right, float bottom, float top, float nearVal, float farVal, float projectionMatrix[16])
{
projectionMatrix[0 * 4 + 0] = (float(2) * nearVal) / (right - left);
projectionMatrix[0 * 4 + 1] = float(0);
projectionMatrix[0 * 4 + 2] = float(0);
projectionMatrix[0 * 4 + 3] = float(0);
projectionMatrix[1 * 4 + 0] = float(0);
projectionMatrix[1 * 4 + 1] = (float(2) * nearVal) / (top - bottom);
projectionMatrix[1 * 4 + 2] = float(0);
projectionMatrix[1 * 4 + 3] = float(0);
projectionMatrix[2 * 4 + 0] = (right + left) / (right - left);
projectionMatrix[2 * 4 + 1] = (top + bottom) / (top - bottom);
projectionMatrix[2 * 4 + 2] = -(farVal + nearVal) / (farVal - nearVal);
projectionMatrix[2 * 4 + 3] = float(-1);
projectionMatrix[3 * 4 + 0] = float(0);
projectionMatrix[3 * 4 + 1] = float(0);
projectionMatrix[3 * 4 + 2] = -(float(2) * farVal * nearVal) / (farVal - nearVal);
projectionMatrix[3 * 4 + 3] = float(0);
}
B3_SHARED_API void b3ComputeProjectionMatrixFOV(float fov, float aspect, float nearVal, float farVal, float projectionMatrix[16])
{
float yScale = 1.0 / tan((3.141592538 / 180.0) * fov / 2);
float xScale = yScale / aspect;
projectionMatrix[0 * 4 + 0] = xScale;
projectionMatrix[0 * 4 + 1] = float(0);
projectionMatrix[0 * 4 + 2] = float(0);
projectionMatrix[0 * 4 + 3] = float(0);
projectionMatrix[1 * 4 + 0] = float(0);
projectionMatrix[1 * 4 + 1] = yScale;
projectionMatrix[1 * 4 + 2] = float(0);
projectionMatrix[1 * 4 + 3] = float(0);
projectionMatrix[2 * 4 + 0] = 0;
projectionMatrix[2 * 4 + 1] = 0;
projectionMatrix[2 * 4 + 2] = (nearVal + farVal) / (nearVal - farVal);
projectionMatrix[2 * 4 + 3] = float(-1);
projectionMatrix[3 * 4 + 0] = float(0);
projectionMatrix[3 * 4 + 1] = float(0);
projectionMatrix[3 * 4 + 2] = (float(2) * farVal * nearVal) / (nearVal - farVal);
projectionMatrix[3 * 4 + 3] = float(0);
}
B3_SHARED_API void b3RequestCameraImageSetViewMatrix2(b3SharedMemoryCommandHandle commandHandle, const float cameraTargetPosition[3], float distance, float yaw, float pitch, float roll, int upAxis)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
b3ComputeViewMatrixFromYawPitchRoll(cameraTargetPosition, distance, yaw, pitch, roll, upAxis, command->m_requestPixelDataArguments.m_viewMatrix);
command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES;
}
B3_SHARED_API void b3RequestCameraImageSetViewMatrix(b3SharedMemoryCommandHandle commandHandle, const float cameraPosition[3], const float cameraTargetPosition[3], const float cameraUp[3])
{
float viewMatrix[16];
b3ComputeViewMatrixFromPositions(cameraPosition, cameraTargetPosition, cameraUp, viewMatrix);
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
b3ComputeViewMatrixFromPositions(cameraPosition, cameraTargetPosition, cameraUp, command->m_requestPixelDataArguments.m_viewMatrix);
command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES;
}
B3_SHARED_API void b3RequestCameraImageSetProjectionMatrix(b3SharedMemoryCommandHandle commandHandle, float left, float right, float bottom, float top, float nearVal, float farVal)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
b3ComputeProjectionMatrix(left, right, bottom, top, nearVal, farVal, command->m_requestPixelDataArguments.m_projectionMatrix);
command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES;
}
B3_SHARED_API void b3RequestCameraImageSetFOVProjectionMatrix(b3SharedMemoryCommandHandle commandHandle, float fov, float aspect, float nearVal, float farVal)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
b3ComputeProjectionMatrixFOV(fov, aspect, nearVal, farVal, command->m_requestPixelDataArguments.m_projectionMatrix);
command->m_updateFlags |= REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES;
}
B3_SHARED_API void b3RequestCameraImageSetPixelResolution(b3SharedMemoryCommandHandle commandHandle, int width, int height )
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CAMERA_IMAGE_DATA);
command->m_requestPixelDataArguments.m_pixelWidth = width;
command->m_requestPixelDataArguments.m_pixelHeight = height;
command->m_updateFlags |= REQUEST_PIXEL_ARGS_SET_PIXEL_WIDTH_HEIGHT;
}
B3_SHARED_API void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImageData* imageData)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
if (cl)
{
cl->getCachedCameraImage(imageData);
}
}
///request an contact point information
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestContactPointInformation(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type =CMD_REQUEST_CONTACT_POINT_INFORMATION;
command->m_requestContactPointArguments.m_startingContactPointIndex = 0;
command->m_requestContactPointArguments.m_objectAIndexFilter = -1;
command->m_requestContactPointArguments.m_objectBIndexFilter = -1;
command->m_requestContactPointArguments.m_linkIndexAIndexFilter = -2;
command->m_requestContactPointArguments.m_linkIndexBIndexFilter = -2;
command->m_updateFlags = 0;
return (b3SharedMemoryCommandHandle) command;
}
B3_SHARED_API void b3SetContactFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
command->m_requestContactPointArguments.m_objectAIndexFilter = bodyUniqueIdA;
}
B3_SHARED_API void b3SetContactFilterLinkA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
command->m_updateFlags |= CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_A_FILTER;
command->m_requestContactPointArguments.m_linkIndexAIndexFilter= linkIndexA;
}
B3_SHARED_API void b3SetContactFilterLinkB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
command->m_updateFlags |= CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_B_FILTER;
command->m_requestContactPointArguments.m_linkIndexBIndexFilter = linkIndexB;
}
B3_SHARED_API void b3SetClosestDistanceFilterLinkA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA)
{
b3SetContactFilterLinkA(commandHandle, linkIndexA);
}
B3_SHARED_API void b3SetClosestDistanceFilterLinkB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB)
{
b3SetContactFilterLinkB(commandHandle, linkIndexB);
}
B3_SHARED_API void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
command->m_requestContactPointArguments.m_objectBIndexFilter = bodyUniqueIdB;
}
///compute the closest points between two bodies
B3_SHARED_API b3SharedMemoryCommandHandle b3InitClosestDistanceQuery(b3PhysicsClientHandle physClient)
{
b3SharedMemoryCommandHandle commandHandle =b3InitRequestContactPointInformation(physClient);
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
command->m_updateFlags = CMD_REQUEST_CONTACT_POINT_HAS_QUERY_MODE;
command->m_requestContactPointArguments.m_mode = CONTACT_QUERY_MODE_COMPUTE_CLOSEST_POINTS;
return commandHandle;
}
B3_SHARED_API void b3SetClosestDistanceFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA)
{
b3SetContactFilterBodyA(commandHandle,bodyUniqueIdA);
}
B3_SHARED_API void b3SetClosestDistanceFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB)
{
b3SetContactFilterBodyB(commandHandle,bodyUniqueIdB);
}
B3_SHARED_API void b3SetClosestDistanceThreshold(b3SharedMemoryCommandHandle commandHandle, double distance)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION);
command->m_updateFlags += CMD_REQUEST_CONTACT_POINT_HAS_CLOSEST_DISTANCE_THRESHOLD;
command->m_requestContactPointArguments.m_closestDistanceThreshold = distance;
}
///get all the bodies that touch a given axis aligned bounding box specified in world space (min and max coordinates)
B3_SHARED_API b3SharedMemoryCommandHandle b3InitAABBOverlapQuery(b3PhysicsClientHandle physClient, const double aabbMin[3], const double aabbMax[3])
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_REQUEST_AABB_OVERLAP;
command->m_updateFlags = 0;
command->m_requestOverlappingObjectsArgs.m_startingOverlappingObjectIndex = 0;
command->m_requestOverlappingObjectsArgs.m_aabbQueryMin[0] = aabbMin[0];
command->m_requestOverlappingObjectsArgs.m_aabbQueryMin[1] = aabbMin[1];
command->m_requestOverlappingObjectsArgs.m_aabbQueryMin[2] = aabbMin[2];
command->m_requestOverlappingObjectsArgs.m_aabbQueryMax[0] = aabbMax[0];
command->m_requestOverlappingObjectsArgs.m_aabbQueryMax[1] = aabbMax[1];
command->m_requestOverlappingObjectsArgs.m_aabbQueryMax[2] = aabbMax[2];
return (b3SharedMemoryCommandHandle)command;
}
B3_SHARED_API void b3GetAABBOverlapResults(b3PhysicsClientHandle physClient, struct b3AABBOverlapData* data)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
if (cl)
{
cl->getCachedOverlappingObjects(data);
}
}
B3_SHARED_API void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointData)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
if (cl)
{
cl->getCachedContactPointInformation(contactPointData);
}
}
B3_SHARED_API void b3GetClosestPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointInfo)
{
b3GetContactPointInformation(physClient,contactPointInfo);
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestCollisionShapeInformation(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_REQUEST_COLLISION_SHAPE_INFO;
command->m_requestCollisionShapeDataArguments.m_bodyUniqueId = bodyUniqueId;
command->m_requestCollisionShapeDataArguments.m_linkIndex = linkIndex;
command->m_updateFlags = 0;
return (b3SharedMemoryCommandHandle)command;
}
B3_SHARED_API void b3GetCollisionShapeInformation(b3PhysicsClientHandle physClient, struct b3CollisionShapeInformation* collisionShapeInfo)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
if (cl)
{
cl->getCachedCollisionShapeInformation(collisionShapeInfo);
}
}
//request visual shape information
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestVisualShapeInformation(b3PhysicsClientHandle physClient, int bodyUniqueIdA)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_REQUEST_VISUAL_SHAPE_INFO;
command->m_requestVisualShapeDataArguments.m_bodyUniqueId = bodyUniqueIdA;
command->m_requestVisualShapeDataArguments.m_startingVisualShapeIndex = 0;
command->m_updateFlags = 0;
return (b3SharedMemoryCommandHandle) command;
}
B3_SHARED_API void b3GetVisualShapeInformation(b3PhysicsClientHandle physClient, struct b3VisualShapeInformation* visualShapeInfo)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
if (cl)
{
cl->getCachedVisualShapeInformation(visualShapeInfo);
}
}
B3_SHARED_API b3SharedMemoryCommandHandle b3CreateChangeTextureCommandInit(b3PhysicsClientHandle physClient, int textureUniqueId, int width, int height, const char* rgbPixels)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_CHANGE_TEXTURE;
command->m_changeTextureArgs.m_textureUniqueId = textureUniqueId;
command->m_changeTextureArgs.m_width = width;
command->m_changeTextureArgs.m_height = height;
int numPixels = width*height;
cl->uploadBulletFileToSharedMemory(rgbPixels,numPixels*3);
command->m_updateFlags = 0;
return (b3SharedMemoryCommandHandle) command;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitLoadTexture(b3PhysicsClientHandle physClient, const char* filename)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_LOAD_TEXTURE;
int len = strlen(filename);
if (len<MAX_FILENAME_LENGTH)
{
strcpy(command->m_loadTextureArguments.m_textureFileName,filename);
} else
{
command->m_loadTextureArguments.m_textureFileName[0] = 0;
}
command->m_updateFlags = 0;
return (b3SharedMemoryCommandHandle) command;
}
B3_SHARED_API int b3GetStatusTextureUniqueId(b3SharedMemoryStatusHandle statusHandle)
{
int uid = -1;
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
if (status)
{
btAssert(status->m_type == CMD_LOAD_TEXTURE_COMPLETED);
if (status->m_type == CMD_LOAD_TEXTURE_COMPLETED)
{
uid = status->m_loadTextureResultArguments.m_textureUniqueId;
}
}
return uid;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitUpdateVisualShape(b3PhysicsClientHandle physClient, int bodyUniqueId, int jointIndex, int shapeIndex, int textureUniqueId)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_UPDATE_VISUAL_SHAPE;
command->m_updateVisualShapeDataArguments.m_bodyUniqueId = bodyUniqueId;
command->m_updateVisualShapeDataArguments.m_jointIndex = jointIndex;
command->m_updateVisualShapeDataArguments.m_shapeIndex = shapeIndex;
command->m_updateVisualShapeDataArguments.m_textureUniqueId = textureUniqueId;
command->m_updateFlags = 0;
if (textureUniqueId>=0)
{
command->m_updateFlags |= CMD_UPDATE_VISUAL_SHAPE_TEXTURE;
}
return (b3SharedMemoryCommandHandle) command;
}
B3_SHARED_API void b3UpdateVisualShapeRGBAColor(b3SharedMemoryCommandHandle commandHandle, double rgbaColor[4])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_UPDATE_VISUAL_SHAPE);
if (command->m_type == CMD_UPDATE_VISUAL_SHAPE)
{
command->m_updateVisualShapeDataArguments.m_rgbaColor[0] = rgbaColor[0];
command->m_updateVisualShapeDataArguments.m_rgbaColor[1] = rgbaColor[1];
command->m_updateVisualShapeDataArguments.m_rgbaColor[2] = rgbaColor[2];
command->m_updateVisualShapeDataArguments.m_rgbaColor[3] = rgbaColor[3];
command->m_updateFlags |= CMD_UPDATE_VISUAL_SHAPE_RGBA_COLOR;
}
}
B3_SHARED_API void b3UpdateVisualShapeSpecularColor(b3SharedMemoryCommandHandle commandHandle, double specularColor[3])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_UPDATE_VISUAL_SHAPE);
if (command->m_type == CMD_UPDATE_VISUAL_SHAPE)
{
command->m_updateVisualShapeDataArguments.m_specularColor[0] = specularColor[0];
command->m_updateVisualShapeDataArguments.m_specularColor[1] = specularColor[1];
command->m_updateVisualShapeDataArguments.m_specularColor[2] = specularColor[2];
command->m_updateFlags |= CMD_UPDATE_VISUAL_SHAPE_SPECULAR_COLOR;
}
}
B3_SHARED_API b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_APPLY_EXTERNAL_FORCE;
command->m_updateFlags = 0;
command->m_externalForceArguments.m_numForcesAndTorques = 0;
return (b3SharedMemoryCommandHandle) command;
}
B3_SHARED_API void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double force[3], const double position[3], int flag)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_APPLY_EXTERNAL_FORCE);
int index = command->m_externalForceArguments.m_numForcesAndTorques;
command->m_externalForceArguments.m_bodyUniqueIds[index] = bodyUniqueId;
command->m_externalForceArguments.m_linkIds[index] = linkId;
command->m_externalForceArguments.m_forceFlags[index] = EF_FORCE+flag;
for (int i = 0; i < 3; ++i) {
command->m_externalForceArguments.m_forcesAndTorques[index+i] = force[i];
command->m_externalForceArguments.m_positions[index+i] = position[i];
}
command->m_externalForceArguments.m_numForcesAndTorques++;
}
B3_SHARED_API void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double torque[3], int flag)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_APPLY_EXTERNAL_FORCE);
int index = command->m_externalForceArguments.m_numForcesAndTorques;
command->m_externalForceArguments.m_bodyUniqueIds[index] = bodyUniqueId;
command->m_externalForceArguments.m_linkIds[index] = linkId;
command->m_externalForceArguments.m_forceFlags[index] = EF_TORQUE+flag;
for (int i = 0; i < 3; ++i) {
command->m_externalForceArguments.m_forcesAndTorques[index+i] = torque[i];
}
command->m_externalForceArguments.m_numForcesAndTorques++;
}
///compute the forces to achieve an acceleration, given a state q and qdot using inverse dynamics
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId,
const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_CALCULATE_INVERSE_DYNAMICS;
command->m_updateFlags = 0;
command->m_calculateInverseDynamicsArguments.m_bodyUniqueId = bodyUniqueId;
int numJoints = cl->getNumJoints(bodyUniqueId);
for (int i = 0; i < numJoints;i++)
{
command->m_calculateInverseDynamicsArguments.m_jointPositionsQ[i] = jointPositionsQ[i];
command->m_calculateInverseDynamicsArguments.m_jointVelocitiesQdot[i] = jointVelocitiesQdot[i];
command->m_calculateInverseDynamicsArguments.m_jointAccelerations[i] = jointAccelerations[i];
}
return (b3SharedMemoryCommandHandle)command;
}
B3_SHARED_API int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId,
int* dofCount,
double* jointForces)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
if (status==0)
return false;
btAssert(status->m_type == CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED);
if (status->m_type != CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED)
return false;
if (dofCount)
{
*dofCount = status->m_inverseDynamicsResultArgs.m_dofCount;
}
if (bodyUniqueId)
{
*bodyUniqueId = status->m_inverseDynamicsResultArgs.m_bodyUniqueId;
}
if (jointForces)
{
for (int i = 0; i < status->m_inverseDynamicsResultArgs.m_dofCount; i++)
{
jointForces[i] = status->m_inverseDynamicsResultArgs.m_jointForces[i];
}
}
return true;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_CALCULATE_JACOBIAN;
command->m_updateFlags = 0;
command->m_calculateJacobianArguments.m_bodyUniqueId = bodyUniqueId;
command->m_calculateJacobianArguments.m_linkIndex = linkIndex;
command->m_calculateJacobianArguments.m_localPosition[0] = localPosition[0];
command->m_calculateJacobianArguments.m_localPosition[1] = localPosition[1];
command->m_calculateJacobianArguments.m_localPosition[2] = localPosition[2];
int numJoints = cl->getNumJoints(bodyUniqueId);
for (int i = 0; i < numJoints;i++)
{
command->m_calculateJacobianArguments.m_jointPositionsQ[i] = jointPositionsQ[i];
command->m_calculateJacobianArguments.m_jointVelocitiesQdot[i] = jointVelocitiesQdot[i];
command->m_calculateJacobianArguments.m_jointAccelerations[i] = jointAccelerations[i];
}
return (b3SharedMemoryCommandHandle)command;
}
B3_SHARED_API int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, int* dofCount, double* linearJacobian, double* angularJacobian)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
if (status==0)
return false;
btAssert(status->m_type == CMD_CALCULATED_JACOBIAN_COMPLETED);
if (status->m_type != CMD_CALCULATED_JACOBIAN_COMPLETED)
return false;
if (dofCount)
{
*dofCount = status->m_jacobianResultArgs.m_dofCount;
}
if (linearJacobian)
{
for (int i = 0; i < status->m_jacobianResultArgs.m_dofCount*3; i++)
{
linearJacobian[i] = status->m_jacobianResultArgs.m_linearJacobian[i];
}
}
if (angularJacobian)
{
for (int i = 0; i < status->m_jacobianResultArgs.m_dofCount*3; i++)
{
angularJacobian[i] = status->m_jacobianResultArgs.m_angularJacobian[i];
}
}
return true;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateMassMatrixCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, const double* jointPositionsQ)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_CALCULATE_MASS_MATRIX;
command->m_updateFlags = 0;
int numJoints = cl->getNumJoints(bodyUniqueId);
for (int i = 0; i < numJoints; i++)
{
command->m_calculateMassMatrixArguments.m_jointPositionsQ[i] = jointPositionsQ[i];
}
return (b3SharedMemoryCommandHandle)command;
}
B3_SHARED_API int b3GetStatusMassMatrix(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int* dofCount, double* massMatrix)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
if (status==0)
return false;
btAssert(status->m_type == CMD_CALCULATED_MASS_MATRIX_COMPLETED);
if (status->m_type != CMD_CALCULATED_MASS_MATRIX_COMPLETED)
return false;
if (dofCount)
{
*dofCount = status->m_massMatrixResultArgs.m_dofCount;
}
if (massMatrix)
{
cl->getCachedMassMatrix(status->m_massMatrixResultArgs.m_dofCount, massMatrix);
}
return true;
}
///compute the joint positions to move the end effector to a desired target using inverse kinematics
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_CALCULATE_INVERSE_KINEMATICS;
command->m_updateFlags = 0;
command->m_calculateInverseKinematicsArguments.m_bodyUniqueId = bodyUniqueId;
return (b3SharedMemoryCommandHandle)command;
}
B3_SHARED_API void b3CalculateInverseKinematicsAddTargetPurePosition(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[3])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
command->m_updateFlags |= IK_HAS_TARGET_POSITION;
command->m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex = endEffectorLinkIndex;
command->m_calculateInverseKinematicsArguments.m_targetPosition[0] = targetPosition[0];
command->m_calculateInverseKinematicsArguments.m_targetPosition[1] = targetPosition[1];
command->m_calculateInverseKinematicsArguments.m_targetPosition[2] = targetPosition[2];
command->m_calculateInverseKinematicsArguments.m_targetOrientation[0] = 0;
command->m_calculateInverseKinematicsArguments.m_targetOrientation[1] = 0;
command->m_calculateInverseKinematicsArguments.m_targetOrientation[2] = 0;
command->m_calculateInverseKinematicsArguments.m_targetOrientation[3] = 1;
}
B3_SHARED_API void b3CalculateInverseKinematicsAddTargetPositionWithOrientation(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[3], const double targetOrientation[4])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
command->m_updateFlags |= IK_HAS_TARGET_POSITION+IK_HAS_TARGET_ORIENTATION;
command->m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex = endEffectorLinkIndex;
command->m_calculateInverseKinematicsArguments.m_targetPosition[0] = targetPosition[0];
command->m_calculateInverseKinematicsArguments.m_targetPosition[1] = targetPosition[1];
command->m_calculateInverseKinematicsArguments.m_targetPosition[2] = targetPosition[2];
command->m_calculateInverseKinematicsArguments.m_targetOrientation[0] = targetOrientation[0];
command->m_calculateInverseKinematicsArguments.m_targetOrientation[1] = targetOrientation[1];
command->m_calculateInverseKinematicsArguments.m_targetOrientation[2] = targetOrientation[2];
command->m_calculateInverseKinematicsArguments.m_targetOrientation[3] = targetOrientation[3];
}
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)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
command->m_updateFlags |= IK_HAS_TARGET_POSITION+IK_HAS_NULL_SPACE_VELOCITY;
command->m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex = endEffectorLinkIndex;
command->m_calculateInverseKinematicsArguments.m_targetPosition[0] = targetPosition[0];
command->m_calculateInverseKinematicsArguments.m_targetPosition[1] = targetPosition[1];
command->m_calculateInverseKinematicsArguments.m_targetPosition[2] = targetPosition[2];
for (int i = 0; i < numDof; ++i)
{
command->m_calculateInverseKinematicsArguments.m_lowerLimit[i] = lowerLimit[i];
command->m_calculateInverseKinematicsArguments.m_upperLimit[i] = upperLimit[i];
command->m_calculateInverseKinematicsArguments.m_jointRange[i] = jointRange[i];
command->m_calculateInverseKinematicsArguments.m_restPose[i] = restPose[i];
}
}
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)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
command->m_updateFlags |= IK_HAS_TARGET_POSITION+IK_HAS_TARGET_ORIENTATION+IK_HAS_NULL_SPACE_VELOCITY;
command->m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex = endEffectorLinkIndex;
command->m_calculateInverseKinematicsArguments.m_targetPosition[0] = targetPosition[0];
command->m_calculateInverseKinematicsArguments.m_targetPosition[1] = targetPosition[1];
command->m_calculateInverseKinematicsArguments.m_targetPosition[2] = targetPosition[2];
command->m_calculateInverseKinematicsArguments.m_targetOrientation[0] = targetOrientation[0];
command->m_calculateInverseKinematicsArguments.m_targetOrientation[1] = targetOrientation[1];
command->m_calculateInverseKinematicsArguments.m_targetOrientation[2] = targetOrientation[2];
command->m_calculateInverseKinematicsArguments.m_targetOrientation[3] = targetOrientation[3];
for (int i = 0; i < numDof; ++i)
{
command->m_calculateInverseKinematicsArguments.m_lowerLimit[i] = lowerLimit[i];
command->m_calculateInverseKinematicsArguments.m_upperLimit[i] = upperLimit[i];
command->m_calculateInverseKinematicsArguments.m_jointRange[i] = jointRange[i];
command->m_calculateInverseKinematicsArguments.m_restPose[i] = restPose[i];
}
}
B3_SHARED_API void b3CalculateInverseKinematicsSetCurrentPositions(b3SharedMemoryCommandHandle commandHandle, int numDof, const double* currentJointPositions)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
command->m_updateFlags |= IK_HAS_CURRENT_JOINT_POSITIONS;
for (int i = 0; i < numDof; ++i)
{
command->m_calculateInverseKinematicsArguments.m_currentPositions[i] = currentJointPositions[i];
}
}
B3_SHARED_API void b3CalculateInverseKinematicsSetMaxNumIterations(b3SharedMemoryCommandHandle commandHandle, int maxNumIterations)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
command->m_updateFlags |= IK_HAS_MAX_ITERATIONS;
command->m_calculateInverseKinematicsArguments.m_maxNumIterations = maxNumIterations;
}
B3_SHARED_API void b3CalculateInverseKinematicsSetResidualThreshold(b3SharedMemoryCommandHandle commandHandle, double residualThreshold)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
command->m_updateFlags |= IK_HAS_RESIDUAL_THRESHOLD;
command->m_calculateInverseKinematicsArguments.m_residualThreshold = residualThreshold;
}
B3_SHARED_API void b3CalculateInverseKinematicsSetJointDamping(b3SharedMemoryCommandHandle commandHandle, int numDof, const double* jointDampingCoeff)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
command->m_updateFlags |= IK_HAS_JOINT_DAMPING;
for (int i = 0; i < numDof; ++i)
{
command->m_calculateInverseKinematicsArguments.m_jointDamping[i] = jointDampingCoeff[i];
}
}
B3_SHARED_API void b3CalculateInverseKinematicsSelectSolver(b3SharedMemoryCommandHandle commandHandle, int solver)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
command->m_updateFlags |= solver;
}
B3_SHARED_API int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId,
int* dofCount,
double* jointPositions)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
btAssert(status);
if (status==0)
return 0;
btAssert(status->m_type == CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED);
if (status->m_type != CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED)
return 0;
if (dofCount)
{
*dofCount = status->m_inverseKinematicsResultArgs.m_dofCount;
}
if (bodyUniqueId)
{
*bodyUniqueId = status->m_inverseKinematicsResultArgs.m_bodyUniqueId;
}
if (jointPositions)
{
for (int i = 0; i < status->m_inverseKinematicsResultArgs.m_dofCount; i++)
{
jointPositions[i] = status->m_inverseKinematicsResultArgs.m_jointPositions[i];
}
}
return 1;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3RequestVREventsCommandInit(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_REQUEST_VR_EVENTS_DATA;
command->m_updateFlags = VR_DEVICE_CONTROLLER;
return (b3SharedMemoryCommandHandle)command;
}
B3_SHARED_API void b3VREventsSetDeviceTypeFilter(b3SharedMemoryCommandHandle commandHandle, int deviceTypeFilter)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
if (command->m_type == CMD_REQUEST_VR_EVENTS_DATA)
{
command->m_updateFlags = deviceTypeFilter;
}
}
B3_SHARED_API void b3GetVREventsData(b3PhysicsClientHandle physClient, struct b3VREventsData* vrEventsData)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
if (cl)
{
cl->getCachedVREvents(vrEventsData);
}
}
B3_SHARED_API b3SharedMemoryCommandHandle b3SetVRCameraStateCommandInit(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_SET_VR_CAMERA_STATE;
command->m_updateFlags = 0;
return (b3SharedMemoryCommandHandle)command;
}
B3_SHARED_API int b3SetVRCameraRootPosition(b3SharedMemoryCommandHandle commandHandle, double rootPos[3])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_SET_VR_CAMERA_STATE);
command->m_updateFlags |= VR_CAMERA_ROOT_POSITION;
command->m_vrCameraStateArguments.m_rootPosition[0] = rootPos[0];
command->m_vrCameraStateArguments.m_rootPosition[1] = rootPos[1];
command->m_vrCameraStateArguments.m_rootPosition[2] = rootPos[2];
return 0;
}
B3_SHARED_API int b3SetVRCameraRootOrientation(b3SharedMemoryCommandHandle commandHandle, double rootOrn[4])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_SET_VR_CAMERA_STATE);
command->m_updateFlags |= VR_CAMERA_ROOT_ORIENTATION;
command->m_vrCameraStateArguments.m_rootOrientation[0] = rootOrn[0];
command->m_vrCameraStateArguments.m_rootOrientation[1] = rootOrn[1];
command->m_vrCameraStateArguments.m_rootOrientation[2] = rootOrn[2];
command->m_vrCameraStateArguments.m_rootOrientation[3] = rootOrn[3];
return 0;
}
B3_SHARED_API int b3SetVRCameraTrackingObject(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_SET_VR_CAMERA_STATE);
command->m_updateFlags |= VR_CAMERA_ROOT_TRACKING_OBJECT;
command->m_vrCameraStateArguments.m_trackingObjectUniqueId = objectUniqueId;
return 0;
}
B3_SHARED_API int b3SetVRCameraTrackingObjectFlag(b3SharedMemoryCommandHandle commandHandle, int flag)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_SET_VR_CAMERA_STATE);
command->m_updateFlags |= VR_CAMERA_FLAG;
command->m_vrCameraStateArguments.m_trackingObjectFlag = flag;
return 0;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3RequestKeyboardEventsCommandInit(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_REQUEST_KEYBOARD_EVENTS_DATA;
command->m_updateFlags = 0;
return (b3SharedMemoryCommandHandle)command;
}
B3_SHARED_API void b3GetKeyboardEventsData(b3PhysicsClientHandle physClient, struct b3KeyboardEventsData* keyboardEventsData)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
if (cl)
{
cl->getCachedKeyboardEvents(keyboardEventsData);
}
}
B3_SHARED_API b3SharedMemoryCommandHandle b3RequestMouseEventsCommandInit(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_REQUEST_MOUSE_EVENTS_DATA;
command->m_updateFlags = 0;
return (b3SharedMemoryCommandHandle)command;
}
B3_SHARED_API void b3GetMouseEventsData(b3PhysicsClientHandle physClient, struct b3MouseEventsData* mouseEventsData)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
if (cl)
{
cl->getCachedMouseEvents(mouseEventsData);
}
}
B3_SHARED_API b3SharedMemoryCommandHandle b3ProfileTimingCommandInit(b3PhysicsClientHandle physClient, const char* name)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
int len = strlen(name);
if (len>=0 && len < (MAX_FILENAME_LENGTH+1))
{
command->m_type = CMD_PROFILE_TIMING;
strcpy(command->m_profile.m_name,name);
command->m_profile.m_name[len]=0;
} else
{
const char* invalid = "InvalidProfileTimingName";
int len = strlen(invalid);
strcpy(command->m_profile.m_name,invalid);
command->m_profile.m_name[len] = 0;
}
command->m_profile.m_durationInMicroSeconds = 0;
return (b3SharedMemoryCommandHandle)command;
}
B3_SHARED_API void b3PushProfileTiming(b3PhysicsClientHandle physClient, const char* timingName)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
cl->pushProfileTiming(timingName);
}
B3_SHARED_API void b3PopProfileTiming(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
cl->popProfileTiming();
}
B3_SHARED_API void b3SetProfileTimingDuractionInMicroSeconds(b3SharedMemoryCommandHandle commandHandle, int duration)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_PROFILE_TIMING);
if (command->m_type == CMD_PROFILE_TIMING)
{
command->m_profile.m_durationInMicroSeconds = duration;
}
}
B3_SHARED_API b3SharedMemoryCommandHandle b3StateLoggingCommandInit(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_STATE_LOGGING;
command->m_updateFlags = 0;
command->m_stateLoggingArguments.m_numBodyUniqueIds = 0;
command->m_stateLoggingArguments.m_deviceFilterType = VR_DEVICE_CONTROLLER;
return (b3SharedMemoryCommandHandle)command;
}
B3_SHARED_API int b3StateLoggingStart(b3SharedMemoryCommandHandle commandHandle, int loggingType, const char* fileName)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_STATE_LOGGING);
if (command->m_type == CMD_STATE_LOGGING)
{
command->m_updateFlags |= STATE_LOGGING_START_LOG;
int len = strlen(fileName);
if (len < MAX_FILENAME_LENGTH)
{
strcpy(command->m_stateLoggingArguments.m_fileName, fileName);
}
else
{
command->m_stateLoggingArguments.m_fileName[0] = 0;
}
command->m_stateLoggingArguments.m_logType = loggingType;
}
return 0;
}
B3_SHARED_API int b3GetStatusLoggingUniqueId(b3SharedMemoryStatusHandle statusHandle)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
b3Assert(status);
b3Assert(status->m_type == CMD_STATE_LOGGING_START_COMPLETED);
if (status && status->m_type == CMD_STATE_LOGGING_START_COMPLETED)
{
return status->m_stateLoggingResultArgs.m_loggingUniqueId;
}
return -1;
}
B3_SHARED_API int b3StateLoggingAddLoggingObjectUniqueId(b3SharedMemoryCommandHandle commandHandle, int objectUniqueId)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_STATE_LOGGING);
if (command->m_type == CMD_STATE_LOGGING)
{
command->m_updateFlags |= STATE_LOGGING_FILTER_OBJECT_UNIQUE_ID;
if (command->m_stateLoggingArguments.m_numBodyUniqueIds < MAX_SDF_BODIES)
{
command->m_stateLoggingArguments.m_bodyUniqueIds[command->m_stateLoggingArguments.m_numBodyUniqueIds++] = objectUniqueId;
}
}
return 0;
}
B3_SHARED_API int b3StateLoggingSetLinkIndexA(b3SharedMemoryCommandHandle commandHandle, int linkIndexA)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type = CMD_STATE_LOGGING);
if (command->m_type == CMD_STATE_LOGGING)
{
command->m_updateFlags |= STATE_LOGGING_FILTER_LINK_INDEX_A;
command->m_stateLoggingArguments.m_linkIndexA = linkIndexA;
}
return 0;
}
B3_SHARED_API int b3StateLoggingSetLinkIndexB(b3SharedMemoryCommandHandle commandHandle, int linkIndexB)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type = CMD_STATE_LOGGING);
if (command->m_type == CMD_STATE_LOGGING)
{
command->m_updateFlags |= STATE_LOGGING_FILTER_LINK_INDEX_B;
command->m_stateLoggingArguments.m_linkIndexB = linkIndexB;
}
return 0;
}
B3_SHARED_API int b3StateLoggingSetBodyAUniqueId(b3SharedMemoryCommandHandle commandHandle, int bodyAUniqueId)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type = CMD_STATE_LOGGING);
if (command->m_type == CMD_STATE_LOGGING)
{
command->m_updateFlags |= STATE_LOGGING_FILTER_BODY_UNIQUE_ID_A;
command->m_stateLoggingArguments.m_bodyUniqueIdA = bodyAUniqueId;
}
return 0;
}
B3_SHARED_API int b3StateLoggingSetBodyBUniqueId(b3SharedMemoryCommandHandle commandHandle, int bodyBUniqueId)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type = CMD_STATE_LOGGING);
if (command->m_type == CMD_STATE_LOGGING)
{
command->m_updateFlags |= STATE_LOGGING_FILTER_BODY_UNIQUE_ID_B;
command->m_stateLoggingArguments.m_bodyUniqueIdB = bodyBUniqueId;
}
return 0;
}
B3_SHARED_API int b3StateLoggingSetMaxLogDof(b3SharedMemoryCommandHandle commandHandle, int maxLogDof)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_STATE_LOGGING);
if (command->m_type == CMD_STATE_LOGGING)
{
command->m_updateFlags |= STATE_LOGGING_MAX_LOG_DOF;
command->m_stateLoggingArguments.m_maxLogDof = maxLogDof;
}
return 0;
}
B3_SHARED_API int b3StateLoggingSetLogFlags(b3SharedMemoryCommandHandle commandHandle, int logFlags)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_STATE_LOGGING);
if (command->m_type == CMD_STATE_LOGGING)
{
command->m_updateFlags |= STATE_LOGGING_LOG_FLAGS;
command->m_stateLoggingArguments.m_logFlags = logFlags;
}
return 0;
}
B3_SHARED_API int b3StateLoggingSetDeviceTypeFilter(b3SharedMemoryCommandHandle commandHandle, int deviceTypeFilter)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_STATE_LOGGING);
if (command->m_type == CMD_STATE_LOGGING)
{
command->m_updateFlags |= STATE_LOGGING_FILTER_DEVICE_TYPE;
command->m_stateLoggingArguments.m_deviceFilterType = deviceTypeFilter;
}
return 0;
}
B3_SHARED_API int b3StateLoggingStop(b3SharedMemoryCommandHandle commandHandle, int loggingUid)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_STATE_LOGGING);
if (command->m_type == CMD_STATE_LOGGING)
{
command->m_updateFlags |= STATE_LOGGING_STOP_LOG;
command->m_stateLoggingArguments.m_loggingUniqueId = loggingUid;
}
return 0;
}
///configure the 3D OpenGL debug visualizer (enable/disable GUI widgets, shadows, position camera etc)
B3_SHARED_API b3SharedMemoryCommandHandle b3InitConfigureOpenGLVisualizer(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_CONFIGURE_OPENGL_VISUALIZER;
command->m_updateFlags = 0;
return (b3SharedMemoryCommandHandle)command;
}
B3_SHARED_API void b3ConfigureOpenGLVisualizerSetVisualizationFlags(b3SharedMemoryCommandHandle commandHandle, int flag, int enabled)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CONFIGURE_OPENGL_VISUALIZER);
if (command->m_type == CMD_CONFIGURE_OPENGL_VISUALIZER)
{
command->m_updateFlags |= COV_SET_FLAGS;
command->m_configureOpenGLVisualizerArguments.m_setFlag = flag;
command->m_configureOpenGLVisualizerArguments.m_setEnabled = enabled;
}
}
B3_SHARED_API void b3ConfigureOpenGLVisualizerSetViewMatrix(b3SharedMemoryCommandHandle commandHandle, float cameraDistance, float cameraPitch, float cameraYaw, const float cameraTargetPosition[3])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CONFIGURE_OPENGL_VISUALIZER);
if (command->m_type == CMD_CONFIGURE_OPENGL_VISUALIZER)
{
command->m_updateFlags |= COV_SET_CAMERA_VIEW_MATRIX;
command->m_configureOpenGLVisualizerArguments.m_cameraDistance = cameraDistance;
command->m_configureOpenGLVisualizerArguments.m_cameraPitch = cameraPitch;
command->m_configureOpenGLVisualizerArguments.m_cameraYaw = cameraYaw;
command->m_configureOpenGLVisualizerArguments.m_cameraTargetPosition[0] = cameraTargetPosition[0];
command->m_configureOpenGLVisualizerArguments.m_cameraTargetPosition[1] = cameraTargetPosition[1];
command->m_configureOpenGLVisualizerArguments.m_cameraTargetPosition[2] = cameraTargetPosition[2];
}
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestOpenGLVisualizerCameraCommand(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_REQUEST_OPENGL_VISUALIZER_CAMERA;
command->m_updateFlags = 0;
return (b3SharedMemoryCommandHandle)command;
}
B3_SHARED_API int b3GetStatusOpenGLVisualizerCamera(b3SharedMemoryStatusHandle statusHandle, b3OpenGLVisualizerCameraInfo* camera)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
//b3Assert(status);
if (status && status->m_type == CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_COMPLETED)
{
*camera = status->m_visualizerCameraResultArgs;
return 1;
}
return 0;
}
B3_SHARED_API void b3SetTimeOut(b3PhysicsClientHandle physClient, double timeOutInSeconds)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
if (cl)
{
cl->setTimeOut(timeOutInSeconds);
}
}
B3_SHARED_API double b3GetTimeOut(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
if (cl)
{
return cl->getTimeOut();
}
return -1;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3SetAdditionalSearchPath(b3PhysicsClientHandle physClient, const char* path)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_SET_ADDITIONAL_SEARCH_PATH;
command->m_updateFlags = 0;
int len = strlen(path);
if (len<MAX_FILENAME_LENGTH)
{
strcpy(command->m_searchPathArgs.m_path,path);
}
return (b3SharedMemoryCommandHandle)command;
}
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])
{
b3Transform trA;
b3Transform trB;
trA.setOrigin(b3MakeVector3(posA[0],posA[1],posA[2]));
trA.setRotation(b3Quaternion(ornA[0],ornA[1],ornA[2],ornA[3]));
trB.setOrigin(b3MakeVector3(posB[0],posB[1],posB[2]));
trB.setRotation(b3Quaternion(ornB[0],ornB[1],ornB[2],ornB[3]));
b3Transform res = trA*trB;
outPos[0] = res.getOrigin()[0];
outPos[1] = res.getOrigin()[1];
outPos[2] = res.getOrigin()[2];
b3Quaternion orn = res.getRotation();
outOrn[0] = orn[0];
outOrn[1] = orn[1];
outOrn[2] = orn[2];
outOrn[3] = orn[3];
}
B3_SHARED_API void b3InvertTransform(const double pos[3], const double orn[4], double outPos[3], double outOrn[4])
{
b3Transform tr;
tr.setOrigin(b3MakeVector3(pos[0],pos[1],pos[2]));
tr.setRotation(b3Quaternion(orn[0],orn[1],orn[2],orn[3]));
b3Transform trInv = tr.inverse();
outPos[0] = trInv.getOrigin()[0];
outPos[1] = trInv.getOrigin()[1];
outPos[2] = trInv.getOrigin()[2];
b3Quaternion invOrn = trInv.getRotation();
outOrn[0] = invOrn[0];
outOrn[1] = invOrn[1];
outOrn[2] = invOrn[2];
outOrn[3] = invOrn[3];
}
Reference in New Issue View Git Blame Copy Permalink