2015-07-23 01:06:05 +00:00
# include "PhysicsClientC_API.h"
2015-09-25 05:50:34 +00:00
# include "PhysicsClientSharedMemory.h"
2015-07-23 01:06:05 +00:00
# include "Bullet3Common/b3Scalar.h"
2016-06-16 18:48:37 +00:00
# include "Bullet3Common/b3Vector3.h"
2016-08-02 18:12:23 +00:00
# include "Bullet3Common/b3Matrix3x3.h"
2017-01-12 05:39:22 +00:00
# include "Bullet3Common/b3Transform.h"
2016-08-02 18:12:23 +00:00
2015-07-31 06:22:44 +00:00
# include <string.h>
2015-09-17 06:09:10 +00:00
# include "SharedMemoryCommands.h"
2015-07-31 06:22:44 +00:00
2015-09-17 06:09:10 +00:00
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadSdfCommandInit ( b3PhysicsClientHandle physClient , const char * sdfFileName )
2016-06-04 02:03:56 +00:00
{
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
b3Assert ( cl ) ;
b3Assert ( cl - > canSubmitCommand ( ) ) ;
struct SharedMemoryCommand * command = cl - > getAvailableSharedMemoryCommand ( ) ;
b3Assert ( command ) ;
2018-09-01 20:49:56 +00:00
return b3LoadSdfCommandInit2 ( ( b3SharedMemoryCommandHandle ) command , sdfFileName ) ;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadSdfCommandInit2 ( b3SharedMemoryCommandHandle commandHandle , const char * sdfFileName )
{
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
2016-06-04 02:03:56 +00:00
command - > m_type = CMD_LOAD_SDF ;
int len = strlen ( sdfFileName ) ;
if ( len < MAX_SDF_FILENAME_LENGTH )
{
2018-09-01 20:49:56 +00:00
strcpy ( command - > m_sdfArguments . m_sdfFileName , sdfFileName ) ;
}
else
2016-06-04 02:03:56 +00:00
{
command - > m_sdfArguments . m_sdfFileName [ 0 ] = 0 ;
}
command - > m_updateFlags = SDF_ARGS_FILE_NAME ;
2018-09-01 20:49:56 +00:00
return ( b3SharedMemoryCommandHandle ) command ;
2016-06-04 02:03:56 +00:00
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3SaveWorldCommandInit ( b3PhysicsClientHandle physClient , const char * sdfFileName )
2016-10-13 06:03:36 +00:00
{
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 ;
}
2015-09-17 06:09:10 +00:00
2015-07-31 06:22:44 +00:00
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadBulletCommandInit ( b3PhysicsClientHandle physClient , const char * fileName )
2016-11-12 02:07:42 +00:00
{
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 ;
}
2017-12-28 18:05:51 +00:00
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3SaveBulletCommandInit ( b3PhysicsClientHandle physClient , const char * fileName )
2016-11-12 02:07:42 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadMJCFCommandInit ( b3PhysicsClientHandle physClient , const char * fileName )
2016-11-12 02:07:42 +00:00
{
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
b3Assert ( cl ) ;
b3Assert ( cl - > canSubmitCommand ( ) ) ;
if ( cl - > canSubmitCommand ( ) )
{
struct SharedMemoryCommand * command = cl - > getAvailableSharedMemoryCommand ( ) ;
b3Assert ( command ) ;
2018-09-01 20:49:56 +00:00
return b3LoadMJCFCommandInit2 ( ( b3SharedMemoryCommandHandle ) command , fileName ) ;
2016-11-12 02:07:42 +00:00
}
return 0 ;
}
2018-09-01 20:49:56 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadMJCFCommandInit2 ( b3SharedMemoryCommandHandle commandHandle , const char * fileName )
{
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3LoadMJCFCommandSetFlags ( b3SharedMemoryCommandHandle commandHandle , int flags )
2017-05-10 22:01:25 +00:00
{
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 ;
}
}
2016-11-12 02:07:42 +00:00
2018-01-08 03:56:46 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3LoadSoftBodyCommandInit ( b3PhysicsClientHandle physClient , const char * fileName )
2016-10-17 20:01:04 +00:00
{
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
b3Assert ( cl ) ;
b3Assert ( cl - > canSubmitCommand ( ) ) ;
2018-01-08 03:56:46 +00:00
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 ;
2016-10-17 20:01:04 +00:00
}
2018-01-08 03:24:37 +00:00
B3_SHARED_API int b3LoadSoftBodySetScale ( b3SharedMemoryCommandHandle commandHandle , double scale )
2016-11-01 23:45:10 +00:00
{
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
2018-01-08 03:24:37 +00:00
b3Assert ( command - > m_type = = CMD_LOAD_SOFT_BODY ) ;
command - > m_loadSoftBodyArguments . m_scale = scale ;
command - > m_updateFlags | = LOAD_SOFT_BODY_UPDATE_SCALE ;
2016-11-01 23:45:10 +00:00
return 0 ;
}
2018-01-08 03:24:37 +00:00
B3_SHARED_API int b3LoadSoftBodySetMass ( b3SharedMemoryCommandHandle commandHandle , double mass )
2016-11-01 23:45:10 +00:00
{
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
2018-01-08 03:24:37 +00:00
b3Assert ( command - > m_type = = CMD_LOAD_SOFT_BODY ) ;
command - > m_loadSoftBodyArguments . m_mass = mass ;
command - > m_updateFlags | = LOAD_SOFT_BODY_UPDATE_MASS ;
2016-11-01 23:45:10 +00:00
return 0 ;
}
2018-01-08 03:24:37 +00:00
B3_SHARED_API int b3LoadSoftBodySetCollisionMargin ( b3SharedMemoryCommandHandle commandHandle , double collisionMargin )
2016-11-01 23:45:10 +00:00
{
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
2018-01-08 03:24:37 +00:00
b3Assert ( command - > m_type = = CMD_LOAD_SOFT_BODY ) ;
command - > m_loadSoftBodyArguments . m_collisionMargin = collisionMargin ;
command - > m_updateFlags | = LOAD_SOFT_BODY_UPDATE_COLLISION_MARGIN ;
2016-11-01 23:45:10 +00:00
return 0 ;
}
2018-08-30 04:12:13 +00:00
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 b3LoadUrdfCommandInit2 ( b3SharedMemoryCommandHandle commandHandle , const char * urdfFileName )
{
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
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 ;
}
2017-09-21 06:18:18 +00:00
B3_SHARED_API int b3LoadUrdfCommandSetUseMultiBody ( b3SharedMemoryCommandHandle commandHandle , int useMultiBody )
2016-08-16 23:57:48 +00:00
{
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 ;
}
2015-07-31 06:22:44 +00:00
2017-09-21 06:18:18 +00:00
B3_SHARED_API int b3LoadUrdfCommandSetGlobalScaling ( b3SharedMemoryCommandHandle commandHandle , double globalScaling )
2017-08-14 21:59:41 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3LoadSdfCommandSetUseMultiBody ( b3SharedMemoryCommandHandle commandHandle , int useMultiBody )
2016-08-17 00:56:30 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3LoadSdfCommandSetUseGlobalScaling ( b3SharedMemoryCommandHandle commandHandle , double globalScaling )
2017-08-14 21:59:41 +00:00
{
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 ;
}
2017-09-21 06:18:18 +00:00
B3_SHARED_API int b3LoadUrdfCommandSetUseFixedBase ( b3SharedMemoryCommandHandle commandHandle , int useFixedBase )
2015-09-17 06:09:10 +00:00
{
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
b3Assert ( command ) ;
b3Assert ( command - > m_type = = CMD_LOAD_URDF ) ;
2016-11-04 20:15:10 +00:00
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 ;
2015-09-17 06:09:10 +00:00
}
2015-07-31 06:22:44 +00:00
2017-09-21 06:18:18 +00:00
B3_SHARED_API int b3LoadUrdfCommandSetFlags ( b3SharedMemoryCommandHandle commandHandle , int flags )
2017-03-27 15:30:20 +00:00
{
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 ;
}
2017-09-21 06:18:18 +00:00
B3_SHARED_API int b3LoadUrdfCommandSetStartPosition ( b3SharedMemoryCommandHandle commandHandle , double startPosX , double startPosY , double startPosZ )
2015-07-31 06:22:44 +00:00
{
2015-09-17 06:09:10 +00:00
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
2015-07-31 06:22:44 +00:00
b3Assert ( command ) ;
2016-11-04 20:15:10 +00:00
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 ;
2015-07-31 06:22:44 +00:00
}
2015-09-17 06:09:10 +00:00
2017-09-21 06:18:18 +00:00
B3_SHARED_API int b3LoadUrdfCommandSetStartOrientation ( b3SharedMemoryCommandHandle commandHandle , double startOrnX , double startOrnY , double startOrnZ , double startOrnW )
2015-07-31 06:22:44 +00:00
{
2015-09-17 06:09:10 +00:00
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
2015-07-31 06:22:44 +00:00
b3Assert ( command ) ;
2016-11-04 20:15:10 +00:00
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 ;
2015-07-31 06:22:44 +00:00
}
2015-07-23 01:06:05 +00:00
2017-10-05 18:43:14 +00:00
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3InitPhysicsParamCommand ( b3PhysicsClientHandle physClient )
2015-07-23 01:06:05 +00:00
{
2015-09-25 05:50:34 +00:00
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
2015-09-17 06:09:10 +00:00
b3Assert ( cl ) ;
2016-03-10 22:36:46 +00:00
b3Assert ( cl - > canSubmitCommand ( ) ) ;
2015-09-17 06:09:10 +00:00
struct SharedMemoryCommand * command = cl - > getAvailableSharedMemoryCommand ( ) ;
b3Assert ( command ) ;
2018-09-04 06:13:15 +00:00
return b3InitPhysicsParamCommand2 ( ( b3SharedMemoryCommandHandle ) command ) ;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitPhysicsParamCommand2 ( b3SharedMemoryCommandHandle commandHandle )
{
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
2015-07-23 01:06:05 +00:00
command - > m_type = CMD_SEND_PHYSICS_SIMULATION_PARAMETERS ;
command - > m_updateFlags = 0 ;
2018-09-04 06:13:15 +00:00
return ( b3SharedMemoryCommandHandle ) command ;
2015-07-23 01:06:05 +00:00
}
2018-09-04 06:13:15 +00:00
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3PhysicsParamSetGravity ( b3SharedMemoryCommandHandle commandHandle , double gravx , double gravy , double gravz )
2015-07-23 01:06:05 +00:00
{
2015-09-17 06:09:10 +00:00
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
2015-07-23 01:06:05 +00:00
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3PhysicsParamSetRealTimeSimulation ( b3SharedMemoryCommandHandle commandHandle , int enableRealTimeSimulation )
2016-07-18 06:50:11 +00:00
{
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
b3Assert ( command - > m_type = = CMD_SEND_PHYSICS_SIMULATION_PARAMETERS ) ;
2017-10-05 18:43:14 +00:00
command - > m_physSimParamArgs . m_useRealTimeSimulation = ( enableRealTimeSimulation ! = 0 ) ;
2016-07-18 06:50:11 +00:00
command - > m_updateFlags | = SIM_PARAM_UPDATE_REAL_TIME_SIMULATION ;
return 0 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3PhysicsParamSetInternalSimFlags ( b3SharedMemoryCommandHandle commandHandle , int flags )
2016-10-23 14:14:50 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3PhysicsParamSetUseSplitImpulse ( b3SharedMemoryCommandHandle commandHandle , int useSplitImpulse )
2016-12-01 06:24:20 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3PhysicsParamSetSplitImpulsePenetrationThreshold ( b3SharedMemoryCommandHandle commandHandle , double splitImpulsePenetrationThreshold )
2016-12-01 06:24:20 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3PhysicsParamSetContactBreakingThreshold ( b3SharedMemoryCommandHandle commandHandle , double contactBreakingThreshold )
2017-02-16 22:19:09 +00:00
{
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 ;
}
2017-10-05 18:43:14 +00:00
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3PhysicsParamSetMaxNumCommandsPer1ms ( b3SharedMemoryCommandHandle commandHandle , int maxNumCmdPer1ms )
2017-03-01 21:48:57 +00:00
{
2017-10-05 18:43:14 +00:00
//obsolete command
2017-03-01 21:48:57 +00:00
return 0 ;
}
2017-02-16 22:19:09 +00:00
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3PhysicsParamSetEnableFileCaching ( b3SharedMemoryCommandHandle commandHandle , int enableFileCaching )
2017-05-18 02:29:12 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3PhysicsParamSetRestitutionVelocityThreshold ( b3SharedMemoryCommandHandle commandHandle , double restitutionVelocityThreshold )
2017-05-27 01:14:38 +00:00
{
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 ;
}
2017-12-01 18:07:07 +00:00
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 ;
}
2018-01-09 18:10:36 +00:00
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 ;
}
2018-02-17 03:44:33 +00:00
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 ;
}
2017-05-27 01:14:38 +00:00
2018-05-12 02:52:06 +00:00
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3PhysicsParamSetNumSolverIterations ( b3SharedMemoryCommandHandle commandHandle , int numSolverIterations )
2016-10-05 23:31:48 +00:00
{
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 ;
}
2018-05-23 03:26:00 +00:00
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 ;
}
2018-06-12 23:08:46 +00:00
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 ;
}
2017-01-17 02:17:18 +00:00
2018-08-24 06:04:17 +00:00
B3_SHARED_API int b3PhysicsParameterSetConstraintSolverType ( b3SharedMemoryCommandHandle commandHandle , int constraintSolverType )
{
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
b3Assert ( command - > m_type = = CMD_SEND_PHYSICS_SIMULATION_PARAMETERS ) ;
command - > m_physSimParamArgs . m_constraintSolverType = constraintSolverType ;
command - > m_updateFlags | = SIM_PARAM_CONSTRAINT_SOLVER_TYPE ;
return 0 ;
2018-08-26 23:14:36 +00:00
}
B3_SHARED_API int b3PhysicsParameterSetMinimumSolverIslandSize ( b3SharedMemoryCommandHandle commandHandle , int minimumSolverIslandSize )
{
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
b3Assert ( command - > m_type = = CMD_SEND_PHYSICS_SIMULATION_PARAMETERS ) ;
command - > m_physSimParamArgs . m_minimumSolverIslandSize = minimumSolverIslandSize ;
command - > m_updateFlags | = SIM_PARAM_CONSTRAINT_MIN_SOLVER_ISLAND_SIZE ;
return 0 ;
2018-08-24 06:04:17 +00:00
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3PhysicsParamSetCollisionFilterMode ( b3SharedMemoryCommandHandle commandHandle , int filterMode )
2017-01-17 02:17:18 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3PhysicsParamSetTimeStep ( b3SharedMemoryCommandHandle commandHandle , double timeStep )
2015-07-31 06:22:44 +00:00
{
2015-09-17 06:09:10 +00:00
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
2015-07-31 06:22:44 +00:00
b3Assert ( command - > m_type = = CMD_SEND_PHYSICS_SIMULATION_PARAMETERS ) ;
2015-10-24 20:50:11 +00:00
command - > m_updateFlags | = SIM_PARAM_UPDATE_DELTA_TIME ;
2015-07-31 06:22:44 +00:00
command - > m_physSimParamArgs . m_deltaTime = timeStep ;
return 0 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3PhysicsParamSetNumSubSteps ( b3SharedMemoryCommandHandle commandHandle , int numSubSteps )
2016-08-24 21:25:06 +00:00
{
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 ;
}
2016-09-08 22:15:58 +00:00
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3PhysicsParamSetDefaultContactERP ( b3SharedMemoryCommandHandle commandHandle , double defaultContactERP )
2016-09-08 22:15:58 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3PhysicsParamSetDefaultNonContactERP ( b3SharedMemoryCommandHandle commandHandle , double defaultNonContactERP )
2017-06-07 16:37:28 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3PhysicsParamSetDefaultFrictionERP ( b3SharedMemoryCommandHandle commandHandle , double frictionERP )
2017-06-07 16:37:28 +00:00
{
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 ;
}
2016-09-08 22:15:58 +00:00
2018-05-16 20:46:19 +00:00
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 ;
}
2016-09-08 22:15:58 +00:00
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3InitStepSimulationCommand ( b3PhysicsClientHandle physClient )
2015-07-31 06:22:44 +00:00
{
2015-09-25 05:50:34 +00:00
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
2015-09-17 06:09:10 +00:00
b3Assert ( cl ) ;
b3Assert ( cl - > canSubmitCommand ( ) ) ;
struct SharedMemoryCommand * command = cl - > getAvailableSharedMemoryCommand ( ) ;
b3Assert ( command ) ;
2018-08-30 04:12:13 +00:00
return b3InitStepSimulationCommand2 ( ( b3SharedMemoryCommandHandle ) command ) ;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitStepSimulationCommand2 ( b3SharedMemoryCommandHandle commandHandle )
{
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
2015-07-31 06:22:44 +00:00
command - > m_type = CMD_STEP_FORWARD_SIMULATION ;
command - > m_updateFlags = 0 ;
2018-08-30 04:12:13 +00:00
return ( b3SharedMemoryCommandHandle ) command ;
2015-07-31 06:22:44 +00:00
}
2018-08-30 04:12:13 +00:00
2017-09-21 06:18:18 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3InitResetSimulationCommand ( b3PhysicsClientHandle physClient )
2015-08-28 00:51:31 +00:00
{
2015-09-25 05:50:34 +00:00
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
2015-09-17 06:09:10 +00:00
b3Assert ( cl ) ;
b3Assert ( cl - > canSubmitCommand ( ) ) ;
struct SharedMemoryCommand * command = cl - > getAvailableSharedMemoryCommand ( ) ;
b3Assert ( command ) ;
2018-09-04 06:13:15 +00:00
return b3InitResetSimulationCommand2 ( ( b3SharedMemoryCommandHandle ) command ) ;
}
2015-08-28 00:51:31 +00:00
2018-09-04 06:13:15 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3InitResetSimulationCommand2 ( b3SharedMemoryCommandHandle commandHandle )
{
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
b3Assert ( command ) ;
command - > m_type = CMD_RESET_SIMULATION ;
command - > m_updateFlags = 0 ;
return ( b3SharedMemoryCommandHandle ) command ;
2015-08-28 00:51:31 +00:00
}
2015-08-02 21:00:43 +00:00
2018-09-04 06:13:15 +00:00
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3JointControlCommandInit ( b3PhysicsClientHandle physClient , int controlMode )
2016-06-23 15:40:36 +00:00
{
return b3JointControlCommandInit2 ( physClient , 0 , controlMode ) ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3JointControlCommandInit2 ( b3PhysicsClientHandle physClient , int bodyUniqueId , int controlMode )
2015-08-02 21:00:43 +00:00
{
2015-09-25 05:50:34 +00:00
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
2015-09-17 06:09:10 +00:00
b3Assert ( cl ) ;
b3Assert ( cl - > canSubmitCommand ( ) ) ;
struct SharedMemoryCommand * command = cl - > getAvailableSharedMemoryCommand ( ) ;
2015-08-02 21:00:43 +00:00
b3Assert ( command ) ;
2018-09-04 06:13:15 +00:00
return b3JointControlCommandInit2Internal ( ( b3SharedMemoryCommandHandle ) command , bodyUniqueId , controlMode ) ;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3JointControlCommandInit2Internal ( b3SharedMemoryCommandHandle commandHandle , int bodyUniqueId , int controlMode )
{
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
2015-08-02 21:00:43 +00:00
command - > m_type = CMD_SEND_DESIRED_STATE ;
2018-09-04 06:13:15 +00:00
command - > m_sendDesiredStateCommandArgument . m_controlMode = controlMode ;
2016-06-17 01:46:34 +00:00
command - > m_sendDesiredStateCommandArgument . m_bodyUniqueId = bodyUniqueId ;
2015-08-02 21:00:43 +00:00
command - > m_updateFlags = 0 ;
2018-09-04 06:13:15 +00:00
for ( int i = 0 ; i < MAX_DEGREE_OF_FREEDOM ; i + + )
{
command - > m_sendDesiredStateCommandArgument . m_hasDesiredStateFlags [ i ] = 0 ;
}
return ( b3SharedMemoryCommandHandle ) command ;
2015-09-17 06:09:10 +00:00
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3JointControlSetDesiredPosition ( b3SharedMemoryCommandHandle commandHandle , int qIndex , double value )
2015-09-17 06:09:10 +00:00
{
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
b3Assert ( command ) ;
2017-02-04 20:32:42 +00:00
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 ;
}
2015-09-17 06:09:10 +00:00
return 0 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3JointControlSetKp ( b3SharedMemoryCommandHandle commandHandle , int dofIndex , double value )
2015-09-17 06:09:10 +00:00
{
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
b3Assert ( command ) ;
2017-02-04 20:32:42 +00:00
if ( ( dofIndex > = 0 ) & & ( dofIndex < MAX_DEGREE_OF_FREEDOM ) )
2017-01-20 19:48:33 +00:00
{
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 ;
}
2015-09-17 06:09:10 +00:00
return 0 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3JointControlSetKd ( b3SharedMemoryCommandHandle commandHandle , int dofIndex , double value )
2015-09-17 06:09:10 +00:00
{
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
b3Assert ( command ) ;
2017-02-04 20:32:42 +00:00
if ( ( dofIndex > = 0 ) & & ( dofIndex < MAX_DEGREE_OF_FREEDOM ) )
2017-01-20 19:48:33 +00:00
{
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 ;
}
2015-08-02 21:00:43 +00:00
return 0 ;
}
2017-11-22 01:05:28 +00:00
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3JointControlSetDesiredVelocity ( b3SharedMemoryCommandHandle commandHandle , int dofIndex , double value )
2015-08-02 21:00:43 +00:00
{
2015-09-17 06:09:10 +00:00
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
2015-08-02 21:00:43 +00:00
b3Assert ( command ) ;
2017-02-04 20:32:42 +00:00
if ( ( dofIndex > = 0 ) & & ( dofIndex < MAX_DEGREE_OF_FREEDOM ) )
2017-01-20 19:48:33 +00:00
{
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 ;
}
2015-08-02 21:00:43 +00:00
return 0 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3JointControlSetMaximumForce ( b3SharedMemoryCommandHandle commandHandle , int dofIndex , double value )
2015-08-02 21:00:43 +00:00
{
2015-09-17 06:09:10 +00:00
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
2015-08-02 21:00:43 +00:00
b3Assert ( command ) ;
2017-02-04 20:32:42 +00:00
if ( ( dofIndex > = 0 ) & & ( dofIndex < MAX_DEGREE_OF_FREEDOM ) )
2017-01-20 19:48:33 +00:00
{
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 ;
}
2015-08-02 21:00:43 +00:00
return 0 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3JointControlSetDesiredForceTorque ( b3SharedMemoryCommandHandle commandHandle , int dofIndex , double value )
2015-08-02 21:00:43 +00:00
{
2015-09-17 06:09:10 +00:00
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
2015-08-02 21:00:43 +00:00
b3Assert ( command ) ;
2017-02-04 20:32:42 +00:00
if ( ( dofIndex > = 0 ) & & ( dofIndex < MAX_DEGREE_OF_FREEDOM ) )
2017-01-20 19:48:33 +00:00
{
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 ;
}
2015-08-02 21:00:43 +00:00
return 0 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3RequestActualStateCommandInit ( b3PhysicsClientHandle physClient , int bodyUniqueId )
2015-08-02 21:00:43 +00:00
{
2015-09-25 05:50:34 +00:00
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
2015-09-17 06:09:10 +00:00
b3Assert ( cl ) ;
b3Assert ( cl - > canSubmitCommand ( ) ) ;
struct SharedMemoryCommand * command = cl - > getAvailableSharedMemoryCommand ( ) ;
2015-08-02 21:00:43 +00:00
b3Assert ( command ) ;
2018-09-01 20:49:56 +00:00
return b3RequestActualStateCommandInit2 ( ( b3SharedMemoryCommandHandle ) command , bodyUniqueId ) ;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3RequestActualStateCommandInit2 ( b3SharedMemoryCommandHandle commandHandle , int bodyUniqueId )
{
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
command - > m_type = CMD_REQUEST_ACTUAL_STATE ;
2017-03-26 20:06:46 +00:00
command - > m_updateFlags = 0 ;
2015-10-27 22:46:13 +00:00
command - > m_requestActualStateInformationCommandArgument . m_bodyUniqueId = bodyUniqueId ;
2018-09-01 20:49:56 +00:00
return ( b3SharedMemoryCommandHandle ) command ;
2015-08-02 21:00:43 +00:00
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3RequestActualStateCommandComputeLinkVelocity ( b3SharedMemoryCommandHandle commandHandle , int computeLinkVelocity )
2017-03-26 20:06:46 +00:00
{
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 ;
}
2017-09-26 17:05:17 +00:00
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 ;
}
2017-03-26 20:06:46 +00:00
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3GetJointState ( b3PhysicsClientHandle physClient , b3SharedMemoryStatusHandle statusHandle , int jointIndex , b3JointSensorState * state )
2015-09-25 05:42:22 +00:00
{
const SharedMemoryStatus * status = ( const SharedMemoryStatus * ) statusHandle ;
b3Assert ( status ) ;
2015-10-13 18:32:25 +00:00
int bodyIndex = status - > m_sendActualStateArgs . m_bodyUniqueId ;
b3Assert ( bodyIndex > = 0 ) ;
if ( bodyIndex > = 0 )
{
b3JointInfo info ;
2017-03-23 17:29:16 +00:00
bool result = b3GetJointInfo ( physClient , bodyIndex , jointIndex , & info ) ! = 0 ;
2017-01-21 02:13:24 +00:00
if ( result )
{
2017-02-04 20:32:42 +00:00
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 ] ;
2017-02-20 21:18:33 +00:00
} 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 ;
2015-10-13 18:32:25 +00:00
}
2015-09-25 05:42:22 +00:00
}
2017-01-21 02:13:24 +00:00
return 0 ;
2015-09-25 05:42:22 +00:00
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3GetLinkState ( b3PhysicsClientHandle physClient , b3SharedMemoryStatusHandle statusHandle , int linkIndex , b3LinkState * state )
2016-04-24 00:29:46 +00:00
{
const SharedMemoryStatus * status = ( const SharedMemoryStatus * ) statusHandle ;
b3Assert ( status ) ;
int bodyIndex = status - > m_sendActualStateArgs . m_bodyUniqueId ;
b3Assert ( bodyIndex > = 0 ) ;
2017-01-21 02:13:24 +00:00
b3Assert ( linkIndex > = 0 ) ;
2018-09-11 12:44:06 +00:00
int numLinks = status - > m_sendActualStateArgs . m_numLinks ;
b3Assert ( linkIndex < numLinks ) ;
2017-01-21 02:13:24 +00:00
2018-09-11 12:44:06 +00:00
if ( ( bodyIndex > = 0 ) & & ( linkIndex > = 0 ) & & linkIndex < numLinks )
2016-04-24 00:29:46 +00:00
{
2017-01-12 05:39:22 +00:00
b3Transform wlf , com , inertial ;
2016-04-25 16:37:04 +00:00
for ( int i = 0 ; i < 3 ; + + i )
2016-04-24 00:29:46 +00:00
{
state - > m_worldPosition [ i ] = status - > m_sendActualStateArgs . m_linkState [ 7 * linkIndex + i ] ;
2016-04-29 21:45:15 +00:00
state - > m_localInertialPosition [ i ] = status - > m_sendActualStateArgs . m_linkLocalInertialFrames [ 7 * linkIndex + i ] ;
2017-03-26 20:06:46 +00:00
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 ] ;
2016-04-24 00:29:46 +00:00
}
for ( int i = 0 ; i < 4 ; + + i )
{
state - > m_worldOrientation [ i ] = status - > m_sendActualStateArgs . m_linkState [ 7 * linkIndex + 3 + i ] ;
2016-04-29 21:45:15 +00:00
state - > m_localInertialOrientation [ i ] = status - > m_sendActualStateArgs . m_linkLocalInertialFrames [ 7 * linkIndex + 3 + i ] ;
2016-04-24 00:29:46 +00:00
}
2017-01-12 05:39:22 +00:00
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 ] ;
}
2017-01-21 02:13:24 +00:00
return 1 ;
2016-04-24 00:29:46 +00:00
}
2017-01-21 02:13:24 +00:00
return 0 ;
2016-04-24 00:29:46 +00:00
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3CreateCollisionShapeCommandInit ( b3PhysicsClientHandle physClient )
2017-06-03 17:57:56 +00:00
{
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 ;
2017-10-08 01:50:23 +00:00
command - > m_createUserShapeArgs . m_numUserShapes = 0 ;
2017-06-03 17:57:56 +00:00
return ( b3SharedMemoryCommandHandle ) command ;
}
return 0 ;
}
2017-06-19 17:14:26 +00:00
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3CreateCollisionShapeAddSphere ( b3SharedMemoryCommandHandle commandHandle , double radius )
2017-06-03 17:57:56 +00:00
{
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
b3Assert ( command ) ;
2017-10-08 01:50:23 +00:00
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 ) )
2017-06-03 17:57:56 +00:00
{
2017-10-08 01:50:23 +00:00
int shapeIndex = command - > m_createUserShapeArgs . m_numUserShapes ;
2017-06-03 17:57:56 +00:00
if ( shapeIndex < MAX_COMPOUND_COLLISION_SHAPES )
{
2017-10-08 01:50:23 +00:00
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 + + ;
2017-06-19 17:14:26 +00:00
return shapeIndex ;
2017-06-03 17:57:56 +00:00
}
}
2017-06-19 17:14:26 +00:00
return - 1 ;
2017-06-03 17:57:56 +00:00
}
2017-10-08 01:50:23 +00:00
B3_SHARED_API int b3CreateVisualShapeAddSphere ( b3SharedMemoryCommandHandle commandHandle , double radius )
{
return b3CreateCollisionShapeAddSphere ( commandHandle , radius ) ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3CreateCollisionShapeAddBox ( b3SharedMemoryCommandHandle commandHandle , double halfExtents [ 3 ] )
2017-06-03 17:57:56 +00:00
{
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
b3Assert ( command ) ;
2017-10-08 01:50:23 +00:00
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 ) )
2017-06-03 17:57:56 +00:00
{
2017-10-08 01:50:23 +00:00
int shapeIndex = command - > m_createUserShapeArgs . m_numUserShapes ;
2017-06-03 17:57:56 +00:00
if ( shapeIndex < MAX_COMPOUND_COLLISION_SHAPES )
{
2017-10-08 01:50:23 +00:00
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 + + ;
2017-06-19 17:14:26 +00:00
return shapeIndex ;
}
}
return - 1 ;
}
2017-10-08 01:50:23 +00:00
B3_SHARED_API int b3CreateVisualShapeAddBox ( b3SharedMemoryCommandHandle commandHandle , double halfExtents [ /*3*/ ] )
{
return b3CreateCollisionShapeAddBox ( commandHandle , halfExtents ) ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3CreateCollisionShapeAddCapsule ( b3SharedMemoryCommandHandle commandHandle , double radius , double height )
2017-06-19 17:14:26 +00:00
{
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
b3Assert ( command ) ;
2017-10-08 01:50:23 +00:00
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 ) )
2017-06-19 17:14:26 +00:00
{
2017-10-08 01:50:23 +00:00
int shapeIndex = command - > m_createUserShapeArgs . m_numUserShapes ;
2017-06-19 17:14:26 +00:00
if ( shapeIndex < MAX_COMPOUND_COLLISION_SHAPES )
{
2017-10-08 01:50:23 +00:00
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 + + ;
2017-06-19 17:14:26 +00:00
return shapeIndex ;
}
}
return - 1 ;
}
2017-10-08 01:50:23 +00:00
B3_SHARED_API int b3CreateVisualShapeAddCapsule ( b3SharedMemoryCommandHandle commandHandle , double radius , double height )
{
return b3CreateCollisionShapeAddCapsule ( commandHandle , radius , height ) ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3CreateCollisionShapeAddCylinder ( b3SharedMemoryCommandHandle commandHandle , double radius , double height )
2017-06-19 17:14:26 +00:00
{
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
b3Assert ( command ) ;
2017-10-08 01:50:23 +00:00
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 ) )
2017-06-19 17:14:26 +00:00
{
2017-10-08 01:50:23 +00:00
int shapeIndex = command - > m_createUserShapeArgs . m_numUserShapes ;
2017-06-19 17:14:26 +00:00
if ( shapeIndex < MAX_COMPOUND_COLLISION_SHAPES )
{
2017-10-08 01:50:23 +00:00
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 + + ;
2017-06-19 17:14:26 +00:00
return shapeIndex ;
}
}
return - 1 ;
}
2017-10-08 01:50:23 +00:00
B3_SHARED_API int b3CreateVisualShapeAddCylinder ( b3SharedMemoryCommandHandle commandHandle , double radius , double height )
{
return b3CreateCollisionShapeAddCylinder ( commandHandle , radius , height ) ;
}
2017-06-19 17:14:26 +00:00
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3CreateCollisionShapeAddPlane ( b3SharedMemoryCommandHandle commandHandle , double planeNormal [ 3 ] , double planeConstant )
2017-06-19 17:14:26 +00:00
{
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
b3Assert ( command ) ;
2017-10-08 01:50:23 +00:00
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 ) )
2017-06-19 17:14:26 +00:00
{
2017-10-08 01:50:23 +00:00
int shapeIndex = command - > m_createUserShapeArgs . m_numUserShapes ;
2017-06-19 17:14:26 +00:00
if ( shapeIndex < MAX_COMPOUND_COLLISION_SHAPES )
{
2017-10-08 01:50:23 +00:00
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 + + ;
2017-06-19 17:14:26 +00:00
return shapeIndex ;
}
}
return - 1 ;
}
2017-10-08 01:50:23 +00:00
B3_SHARED_API int b3CreateVisualShapeAddPlane ( b3SharedMemoryCommandHandle commandHandle , double planeNormal [ /*3*/ ] , double planeConstant )
{
return b3CreateCollisionShapeAddPlane ( commandHandle , planeNormal , planeConstant ) ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3CreateCollisionShapeAddMesh ( b3SharedMemoryCommandHandle commandHandle , const char * fileName , double meshScale [ 3 ] )
2017-06-19 17:14:26 +00:00
{
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
b3Assert ( command ) ;
2017-10-08 01:50:23 +00:00
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 ) )
2017-06-19 17:14:26 +00:00
{
2017-10-08 01:50:23 +00:00
int shapeIndex = command - > m_createUserShapeArgs . m_numUserShapes ;
2017-06-19 17:14:26 +00:00
if ( shapeIndex < MAX_COMPOUND_COLLISION_SHAPES & & strlen ( fileName ) < VISUAL_SHAPE_MAX_PATH_LEN )
{
2017-10-08 01:50:23 +00:00
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 + + ;
2017-06-19 17:14:26 +00:00
return shapeIndex ;
}
}
return - 1 ;
}
2017-10-08 01:50:23 +00:00
B3_SHARED_API int b3CreateVisualShapeAddMesh ( b3SharedMemoryCommandHandle commandHandle , const char * fileName , double meshScale [ /*3*/ ] )
{
return b3CreateCollisionShapeAddMesh ( commandHandle , fileName , meshScale ) ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3CreateCollisionSetFlag ( b3SharedMemoryCommandHandle commandHandle , int shapeIndex , int flags )
2017-06-19 17:14:26 +00:00
{
2017-06-23 21:43:28 +00:00
2017-06-19 17:14:26 +00:00
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
b3Assert ( command ) ;
2017-10-08 01:50:23 +00:00
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 ) )
2017-06-19 17:14:26 +00:00
{
2017-10-08 01:50:23 +00:00
if ( shapeIndex < command - > m_createUserShapeArgs . m_numUserShapes )
2017-06-19 17:14:26 +00:00
{
2017-10-08 01:50:23 +00:00
command - > m_createUserShapeArgs . m_shapes [ shapeIndex ] . m_collisionFlags | = flags ;
2017-06-03 17:57:56 +00:00
}
}
}
2017-10-08 01:50:23 +00:00
B3_SHARED_API void b3CreateVisualSetFlag ( b3SharedMemoryCommandHandle commandHandle , int shapeIndex , int flags )
{
b3CreateCollisionSetFlag ( commandHandle , shapeIndex , flags ) ;
}
2017-06-03 17:57:56 +00:00
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3CreateCollisionShapeSetChildTransform ( b3SharedMemoryCommandHandle commandHandle , int shapeIndex , double childPosition [ 3 ] , double childOrientation [ 4 ] )
2017-06-03 17:57:56 +00:00
{
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
b3Assert ( command ) ;
2017-10-08 01:50:23 +00:00
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 ) )
2017-06-03 17:57:56 +00:00
{
2017-10-08 01:50:23 +00:00
if ( shapeIndex < command - > m_createUserShapeArgs . m_numUserShapes )
2017-06-03 17:57:56 +00:00
{
2017-10-08 01:50:23 +00:00
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 ] ;
2017-06-03 17:57:56 +00:00
}
}
}
2017-10-08 01:50:23 +00:00
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 ;
}
}
}
2017-06-03 17:57:56 +00:00
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3GetStatusCollisionShapeUniqueId ( b3SharedMemoryStatusHandle statusHandle )
2017-06-03 17:57:56 +00:00
{
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 )
{
2017-10-08 01:50:23 +00:00
return status - > m_createUserShapeResultArgs . m_userShapeUniqueId ;
2017-06-03 17:57:56 +00:00
}
return - 1 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3CreateVisualShapeCommandInit ( b3PhysicsClientHandle physClient )
2017-06-03 17:57:56 +00:00
{
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 ;
2017-10-08 01:50:23 +00:00
command - > m_createUserShapeArgs . m_numUserShapes = 0 ;
2017-06-03 17:57:56 +00:00
return ( b3SharedMemoryCommandHandle ) command ;
}
return 0 ;
}
2017-10-08 01:50:23 +00:00
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3GetStatusVisualShapeUniqueId ( b3SharedMemoryStatusHandle statusHandle )
2017-06-03 17:57:56 +00:00
{
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 )
{
2017-10-08 01:50:23 +00:00
return status - > m_createUserShapeResultArgs . m_userShapeUniqueId ;
2017-06-03 17:57:56 +00:00
}
return - 1 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3CreateMultiBodyCommandInit ( b3PhysicsClientHandle physClient )
2017-06-03 17:57:56 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
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 ] )
2017-06-03 17:57:56 +00:00
{
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 ;
2017-06-19 17:14:26 +00:00
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 ] ;
2017-06-03 17:57:56 +00:00
2017-06-19 17:14:26 +00:00
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 ] ;
2017-06-03 17:57:56 +00:00
2017-06-20 00:13:20 +00:00
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 ;
2017-06-03 17:57:56 +00:00
2017-06-20 00:13:20 +00:00
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 ] ;
2017-06-03 17:57:56 +00:00
2017-06-20 00:13:20 +00:00
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 ] ;
2017-06-03 17:57:56 +00:00
command - > m_createMultiBodyArgs . m_linkCollisionShapeUniqueIds [ baseLinkIndex ] = collisionShapeUnique ;
command - > m_createMultiBodyArgs . m_linkVisualShapeUniqueIds [ baseLinkIndex ] = visualShapeUniqueId ;
2017-06-19 17:14:26 +00:00
command - > m_createMultiBodyArgs . m_linkMasses [ baseLinkIndex ] = mass ;
2017-06-19 20:15:05 +00:00
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 ;
2017-06-03 17:57:56 +00:00
command - > m_createMultiBodyArgs . m_numLinks + + ;
}
return numLinks ;
}
return - 2 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3CreateMultiBodyLink ( b3SharedMemoryCommandHandle commandHandle , double linkMass , double linkCollisionShapeIndex ,
2017-06-19 17:14:26 +00:00
double linkVisualShapeIndex ,
double linkPosition [ 3 ] ,
double linkOrientation [ 4 ] ,
2017-06-19 20:15:05 +00:00
double linkInertialFramePosition [ 3 ] ,
double linkInertialFrameOrientation [ 4 ] ,
2017-06-19 17:14:26 +00:00
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 ;
2017-06-19 20:15:05 +00:00
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 ] ;
2017-06-19 17:14:26 +00:00
2017-06-19 20:15:05 +00:00
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 ] ;
2017-06-19 17:14:26 +00:00
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 ;
2017-06-19 20:15:05 +00:00
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 ] ;
2017-06-19 17:14:26 +00:00
command - > m_createMultiBodyArgs . m_linkMasses [ linkIndex ] = linkMass ;
command - > m_createMultiBodyArgs . m_numLinks + + ;
return numLinks ;
}
}
return - 1 ;
}
2017-06-05 05:04:16 +00:00
//useMaximalCoordinates are disabled by default, enabling them is experimental and not fully supported yet
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3CreateMultiBodyUseMaximalCoordinates ( b3SharedMemoryCommandHandle commandHandle )
2017-06-05 05:04:16 +00:00
{
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 ;
}
}
2017-06-03 17:57:56 +00:00
2018-06-02 18:37:14 +00:00
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 ;
}
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3GetStatusMultiBodyUniqueId ( b3SharedMemoryStatusHandle statusHandle )
2017-06-03 17:57:56 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3CreateBoxShapeCommandInit ( b3PhysicsClientHandle physClient )
2015-08-02 21:00:43 +00:00
{
2015-09-25 05:50:34 +00:00
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
2015-09-17 06:09:10 +00:00
b3Assert ( cl ) ;
b3Assert ( cl - > canSubmitCommand ( ) ) ;
struct SharedMemoryCommand * command = cl - > getAvailableSharedMemoryCommand ( ) ;
2015-08-02 21:00:43 +00:00
b3Assert ( command ) ;
command - > m_type = CMD_CREATE_BOX_COLLISION_SHAPE ;
command - > m_updateFlags = 0 ;
2015-09-17 06:09:10 +00:00
return ( b3SharedMemoryCommandHandle ) command ;
2015-08-02 21:00:43 +00:00
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3CreateBoxCommandSetStartPosition ( b3SharedMemoryCommandHandle commandHandle , double startPosX , double startPosY , double startPosZ )
2015-08-02 21:00:43 +00:00
{
2015-09-17 06:09:10 +00:00
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
2015-08-02 21:00:43 +00:00
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 ;
}
2015-10-27 21:55:46 +00:00
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3CreateBoxCommandSetHalfExtents ( b3SharedMemoryCommandHandle commandHandle , double halfExtentsX , double halfExtentsY , double halfExtentsZ )
2015-10-27 21:55:46 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3CreateBoxCommandSetMass ( b3SharedMemoryCommandHandle commandHandle , double mass )
2015-10-27 21:55:46 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3CreateBoxCommandSetCollisionShapeType ( b3SharedMemoryCommandHandle commandHandle , int collisionShapeType )
2015-10-27 21:55:46 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3CreateBoxCommandSetColorRGBA ( b3SharedMemoryCommandHandle commandHandle , double red , double green , double blue , double alpha )
2015-11-07 01:11:15 +00:00
{
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 ;
}
2015-10-27 21:55:46 +00:00
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3CreateBoxCommandSetStartOrientation ( b3SharedMemoryCommandHandle commandHandle , double startOrnX , double startOrnY , double startOrnZ , double startOrnW )
2015-08-02 21:00:43 +00:00
{
2015-09-17 06:09:10 +00:00
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
2015-08-02 21:00:43 +00:00
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 ;
}
2018-05-31 23:06:15 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3CreatePoseCommandInit ( b3PhysicsClientHandle physClient , int bodyUniqueId )
2015-10-14 05:23:28 +00:00
{
2016-06-24 14:31:17 +00:00
2015-10-14 05:23:28 +00:00
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
b3Assert ( cl ) ;
b3Assert ( cl - > canSubmitCommand ( ) ) ;
struct SharedMemoryCommand * command = cl - > getAvailableSharedMemoryCommand ( ) ;
b3Assert ( command ) ;
2018-09-01 20:49:56 +00:00
return b3CreatePoseCommandInit2 ( ( b3SharedMemoryCommandHandle ) command , bodyUniqueId ) ;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3CreatePoseCommandInit2 ( b3SharedMemoryCommandHandle commandHandle , int bodyUniqueId )
{
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
command - > m_type = CMD_INIT_POSE ;
command - > m_updateFlags = 0 ;
2018-05-31 23:06:15 +00:00
command - > m_initPoseArgs . m_bodyUniqueId = bodyUniqueId ;
2016-06-24 14:31:17 +00:00
//a bit slow, initialing the full range to zero...
2018-09-01 20:49:56 +00:00
for ( int i = 0 ; i < MAX_DEGREE_OF_FREEDOM ; i + + )
{
command - > m_initPoseArgs . m_hasInitialStateQ [ i ] = 0 ;
2017-03-23 17:29:16 +00:00
command - > m_initPoseArgs . m_hasInitialStateQdot [ i ] = 0 ;
2018-09-01 20:49:56 +00:00
}
return commandHandle ;
2015-10-14 05:23:28 +00:00
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3CreatePoseCommandSetBasePosition ( b3SharedMemoryCommandHandle commandHandle , double startPosX , double startPosY , double startPosZ )
2015-10-14 05:23:28 +00:00
{
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 ;
2016-06-24 14:31:17 +00:00
command - > m_initPoseArgs . m_hasInitialStateQ [ 0 ] = 1 ;
command - > m_initPoseArgs . m_hasInitialStateQ [ 1 ] = 1 ;
command - > m_initPoseArgs . m_hasInitialStateQ [ 2 ] = 1 ;
2015-10-14 05:23:28 +00:00
return 0 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3CreatePoseCommandSetBaseOrientation ( b3SharedMemoryCommandHandle commandHandle , double startOrnX , double startOrnY , double startOrnZ , double startOrnW )
2015-10-14 05:23:28 +00:00
{
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 ;
2016-06-24 14:31:17 +00:00
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 ;
2015-10-14 05:23:28 +00:00
return 0 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3CreatePoseCommandSetBaseLinearVelocity ( b3SharedMemoryCommandHandle commandHandle , double linVel [ 3 ] )
2016-11-28 23:11:34 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3CreatePoseCommandSetBaseAngularVelocity ( b3SharedMemoryCommandHandle commandHandle , double angVel [ 3 ] )
2016-11-28 23:11:34 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3CreatePoseCommandSetJointPositions ( b3SharedMemoryCommandHandle commandHandle , int numJointPositions , const double * jointPositions )
2015-10-14 05:23:28 +00:00
{
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 + + )
{
2017-03-23 17:29:16 +00:00
if ( ( i + 7 ) < MAX_DEGREE_OF_FREEDOM )
{
command - > m_initPoseArgs . m_initialStateQ [ i + 7 ] = jointPositions [ i ] ;
command - > m_initPoseArgs . m_hasInitialStateQ [ i + 7 ] = 1 ;
}
2015-10-14 05:23:28 +00:00
}
return 0 ;
}
2018-09-01 20:49:56 +00:00
B3_SHARED_API int b3CreatePoseCommandSetQ ( b3SharedMemoryCommandHandle commandHandle , int numJointPositions , const double * q , const int * hasQ )
{
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 ) < MAX_DEGREE_OF_FREEDOM )
{
command - > m_initPoseArgs . m_initialStateQ [ i ] = q [ i ] ;
command - > m_initPoseArgs . m_hasInitialStateQ [ i ] = hasQ [ i ] ;
}
}
return 0 ;
}
2016-11-28 23:11:34 +00:00
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3CreatePoseCommandSetJointPosition ( b3PhysicsClientHandle physClient , b3SharedMemoryCommandHandle commandHandle , int jointIndex , double jointPosition )
2015-10-14 05:23:28 +00:00
{
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
2015-10-24 20:48:53 +00:00
b3Assert ( command ) ;
b3Assert ( command - > m_type = = CMD_INIT_POSE ) ;
command - > m_updateFlags | = INIT_POSE_HAS_JOINT_STATE ;
2015-10-14 05:23:28 +00:00
b3JointInfo info ;
b3GetJointInfo ( physClient , command - > m_initPoseArgs . m_bodyUniqueId , jointIndex , & info ) ;
2017-09-26 01:14:50 +00:00
//btAssert((info.m_flags & JOINT_HAS_MOTORIZED_POWER) && info.m_qIndex >=0);
2015-10-24 20:48:53 +00:00
if ( ( info . m_flags & JOINT_HAS_MOTORIZED_POWER ) & & info . m_qIndex > = 0 )
{
command - > m_initPoseArgs . m_initialStateQ [ info . m_qIndex ] = jointPosition ;
2016-06-24 14:31:17 +00:00
command - > m_initPoseArgs . m_hasInitialStateQ [ info . m_qIndex ] = 1 ;
2015-10-24 20:48:53 +00:00
}
2015-10-14 05:23:28 +00:00
return 0 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3CreatePoseCommandSetJointVelocities ( b3PhysicsClientHandle physClient , b3SharedMemoryCommandHandle commandHandle , int numJointVelocities , const double * jointVelocities )
2017-03-23 17:29:16 +00:00
{
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 ;
}
2018-09-01 20:49:56 +00:00
B3_SHARED_API int b3CreatePoseCommandSetQdots ( b3SharedMemoryCommandHandle commandHandle , int numJointVelocities , const double * qDots , const int * hasQdots )
{
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 < MAX_DEGREE_OF_FREEDOM )
{
command - > m_initPoseArgs . m_initialStateQdot [ i ] = qDots [ i ] ;
command - > m_initPoseArgs . m_hasInitialStateQdot [ i ] = hasQdots [ i ] ;
}
}
return 0 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3CreatePoseCommandSetJointVelocity ( b3PhysicsClientHandle physClient , b3SharedMemoryCommandHandle commandHandle , int jointIndex , double jointVelocity )
2017-03-23 17:29:16 +00:00
{
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 ) ;
2017-09-26 01:14:50 +00:00
//btAssert((info.m_flags & JOINT_HAS_MOTORIZED_POWER) && info.m_uIndex >=0);
2017-03-23 17:29:16 +00:00
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 ;
}
2015-10-14 05:23:28 +00:00
2015-08-02 21:00:43 +00:00
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3CreateSensorCommandInit ( b3PhysicsClientHandle physClient , int bodyUniqueId )
2015-08-02 21:00:43 +00:00
{
2015-09-25 05:50:34 +00:00
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
2015-09-17 06:09:10 +00:00
b3Assert ( cl ) ;
b3Assert ( cl - > canSubmitCommand ( ) ) ;
struct SharedMemoryCommand * command = cl - > getAvailableSharedMemoryCommand ( ) ;
2015-08-02 21:00:43 +00:00
b3Assert ( command ) ;
2015-09-17 06:09:10 +00:00
2015-08-02 21:00:43 +00:00
command - > m_type = CMD_CREATE_SENSOR ;
command - > m_updateFlags = 0 ;
command - > m_createSensorArguments . m_numJointSensorChanges = 0 ;
2016-06-17 01:46:34 +00:00
command - > m_createSensorArguments . m_bodyUniqueId = bodyUniqueId ;
2015-09-17 06:09:10 +00:00
return ( b3SharedMemoryCommandHandle ) command ;
2015-08-02 21:00:43 +00:00
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3CreateSensorEnable6DofJointForceTorqueSensor ( b3SharedMemoryCommandHandle commandHandle , int jointIndex , int enable )
2015-08-02 21:00:43 +00:00
{
2015-09-17 06:09:10 +00:00
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
2015-08-02 21:00:43 +00:00
b3Assert ( command ) ;
b3Assert ( command - > m_type = = CMD_CREATE_SENSOR ) ;
int curIndex = command - > m_createSensorArguments . m_numJointSensorChanges ;
2015-09-03 21:18:22 +00:00
command - > m_createSensorArguments . m_sensorType [ curIndex ] = SENSOR_FORCE_TORQUE ;
2015-08-02 21:00:43 +00:00
command - > m_createSensorArguments . m_jointIndex [ curIndex ] = jointIndex ;
command - > m_createSensorArguments . m_enableJointForceSensor [ curIndex ] = enable ;
command - > m_createSensorArguments . m_numJointSensorChanges + + ;
return 0 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3CreateSensorEnableIMUForLink ( b3SharedMemoryCommandHandle commandHandle , int linkIndex , int enable )
2015-09-03 21:18:22 +00:00
{
2015-09-17 06:09:10 +00:00
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
2015-09-03 21:18:22 +00:00
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 ;
}
2015-08-02 21:00:43 +00:00
2015-10-27 00:56:21 +00:00
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3DisconnectSharedMemory ( b3PhysicsClientHandle physClient )
2015-07-31 06:22:44 +00:00
{
2015-09-25 05:50:34 +00:00
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
2017-10-04 01:01:53 +00:00
if ( cl )
{
cl - > disconnectSharedMemory ( ) ;
}
2015-07-31 06:22:44 +00:00
delete cl ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryStatusHandle b3ProcessServerStatus ( b3PhysicsClientHandle physClient )
2015-07-31 06:22:44 +00:00
{
2015-09-25 05:50:34 +00:00
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
2016-03-10 22:36:46 +00:00
if ( cl & & cl - > isConnected ( ) )
{
const SharedMemoryStatus * stat = cl - > processServerStatus ( ) ;
return ( b3SharedMemoryStatusHandle ) stat ;
}
return 0 ;
2015-09-17 06:09:10 +00:00
}
2015-09-17 16:37:44 +00:00
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3GetStatusType ( b3SharedMemoryStatusHandle statusHandle )
2015-09-17 06:09:10 +00:00
{
const SharedMemoryStatus * status = ( const SharedMemoryStatus * ) statusHandle ;
2017-03-20 17:58:07 +00:00
//b3Assert(status);
2015-09-17 06:09:10 +00:00
if ( status )
{
return status - > m_type ;
}
2016-03-10 22:36:46 +00:00
return CMD_INVALID_STATUS ;
2015-07-31 06:22:44 +00:00
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3GetStatusBodyIndices ( b3SharedMemoryStatusHandle statusHandle , int * bodyIndicesOut , int bodyIndicesCapacity )
2016-06-13 17:11:28 +00:00
{
int numBodies = 0 ;
const SharedMemoryStatus * status = ( const SharedMemoryStatus * ) statusHandle ;
b3Assert ( status ) ;
if ( status )
{
switch ( status - > m_type )
{
2016-12-31 22:43:15 +00:00
case CMD_MJCF_LOADING_COMPLETED :
2016-11-14 15:39:34 +00:00
case CMD_BULLET_LOADING_COMPLETED :
2016-06-13 17:11:28 +00:00
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3GetStatusBodyIndex ( b3SharedMemoryStatusHandle statusHandle )
2015-10-13 18:32:25 +00:00
{
const SharedMemoryStatus * status = ( const SharedMemoryStatus * ) statusHandle ;
b3Assert ( status ) ;
int bodyId = - 1 ;
2015-09-17 06:09:10 +00:00
2015-10-13 18:32:25 +00:00
if ( status )
{
switch ( status - > m_type )
{
case CMD_URDF_LOADING_COMPLETED :
{
bodyId = status - > m_dataStreamArguments . m_bodyUniqueId ;
break ;
}
2015-10-27 22:46:13 +00:00
case CMD_RIGID_BODY_CREATION_COMPLETED :
{
bodyId = status - > m_rigidBodyCreateArgs . m_bodyUniqueId ;
break ;
}
2017-06-05 05:04:16 +00:00
case CMD_CREATE_MULTI_BODY_COMPLETED :
{
bodyId = status - > m_dataStreamArguments . m_bodyUniqueId ;
break ;
}
2018-01-10 06:47:56 +00:00
case CMD_LOAD_SOFT_BODY_COMPLETED :
{
bodyId = status - > m_loadSoftBodyResultArguments . m_objectUniqueId ;
break ;
}
2015-10-13 18:32:25 +00:00
default :
{
b3Assert ( 0 ) ;
}
} ;
}
return bodyId ;
}
2015-11-05 00:08:28 +00:00
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3RequestCollisionInfoCommandInit ( b3PhysicsClientHandle physClient , int bodyUniqueId )
2017-06-17 01:10:10 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3GetStatusAABB ( b3SharedMemoryStatusHandle statusHandle , int linkIndex , double aabbMin [ 3 ] , double aabbMax [ 3 ] )
2017-06-16 02:46:27 +00:00
{
const SharedMemoryStatus * status = ( const SharedMemoryStatus * ) statusHandle ;
2017-10-10 18:10:42 +00:00
btAssert ( status ) ;
if ( status = = 0 )
return 0 ;
2017-06-17 01:10:10 +00:00
const b3SendCollisionInfoArgs & args = status - > m_sendCollisionInfoArgs ;
btAssert ( status - > m_type = = CMD_REQUEST_COLLISION_INFO_COMPLETED ) ;
if ( status - > m_type ! = CMD_REQUEST_COLLISION_INFO_COMPLETED )
2017-06-16 02:46:27 +00:00
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 )
{
2017-06-17 01:10:10 +00:00
aabbMin [ 0 ] = args . m_linkWorldAABBsMin [ linkIndex * 3 + 0 ] ;
aabbMin [ 1 ] = args . m_linkWorldAABBsMin [ linkIndex * 3 + 1 ] ;
aabbMin [ 2 ] = args . m_linkWorldAABBsMin [ linkIndex * 3 + 2 ] ;
2017-06-16 02:46:27 +00:00
2017-06-17 01:10:10 +00:00
aabbMax [ 0 ] = args . m_linkWorldAABBsMax [ linkIndex * 3 + 0 ] ;
aabbMax [ 1 ] = args . m_linkWorldAABBsMax [ linkIndex * 3 + 1 ] ;
aabbMax [ 2 ] = args . m_linkWorldAABBsMax [ linkIndex * 3 + 2 ] ;
2017-06-16 02:46:27 +00:00
return 1 ;
}
return 0 ;
}
2016-08-10 01:40:12 +00:00
2018-09-01 20:49:56 +00:00
B3_SHARED_API int b3GetStatusActualState2 ( b3SharedMemoryStatusHandle statusHandle ,
int * bodyUniqueId ,
int * numLinks ,
int * numDegreeOfFreedomQ ,
int * numDegreeOfFreedomU ,
const double * rootLocalInertialFrame [ ] ,
const double * actualStateQ [ ] ,
const double * actualStateQdot [ ] ,
const double * jointReactionForces [ ] ,
const double * linkLocalInertialFrames [ ] ,
const double * jointMotorForces [ ] ,
const double * linkStates [ ] ,
const double * linkWorldVelocities [ ] )
{
const SharedMemoryStatus * status = ( const SharedMemoryStatus * ) statusHandle ;
btAssert ( status ) ;
if ( status = = 0 )
return 0 ;
b3GetStatusActualState ( statusHandle , bodyUniqueId , numDegreeOfFreedomQ , numDegreeOfFreedomU ,
rootLocalInertialFrame , actualStateQ , actualStateQdot , jointReactionForces ) ;
const SendActualStateArgs & args = status - > m_sendActualStateArgs ;
if ( numLinks )
{
* numLinks = args . m_numLinks ;
}
if ( linkLocalInertialFrames )
{
* linkLocalInertialFrames = args . m_linkLocalInertialFrames ;
}
if ( jointMotorForces )
{
* jointMotorForces = args . m_jointMotorForce ;
}
if ( linkStates )
{
* linkStates = args . m_linkState ;
}
if ( linkWorldVelocities )
{
* linkWorldVelocities = args . m_linkWorldVelocities ;
}
2018-09-04 06:13:15 +00:00
return 1 ;
2018-09-01 20:49:56 +00:00
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3GetStatusActualState ( b3SharedMemoryStatusHandle statusHandle ,
2015-11-05 00:08:28 +00:00
int * bodyUniqueId ,
int * numDegreeOfFreedomQ ,
int * numDegreeOfFreedomU ,
const double * rootLocalInertialFrame [ ] ,
const double * actualStateQ [ ] ,
const double * actualStateQdot [ ] ,
const double * jointReactionForces [ ] ) {
const SharedMemoryStatus * status = ( const SharedMemoryStatus * ) statusHandle ;
2017-10-10 18:10:42 +00:00
btAssert ( status ) ;
if ( status = = 0 )
return 0 ;
2015-11-05 00:08:28 +00:00
const SendActualStateArgs & args = status - > m_sendActualStateArgs ;
2016-06-14 01:58:52 +00:00
btAssert ( status - > m_type = = CMD_ACTUAL_STATE_UPDATE_COMPLETED ) ;
if ( status - > m_type ! = CMD_ACTUAL_STATE_UPDATE_COMPLETED )
2016-06-13 17:11:28 +00:00
return false ;
2015-11-05 00:08:28 +00:00
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3CanSubmitCommand ( b3PhysicsClientHandle physClient )
2015-07-31 06:22:44 +00:00
{
2015-09-25 05:50:34 +00:00
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
2015-10-27 00:56:21 +00:00
if ( cl )
{
return ( int ) cl - > canSubmitCommand ( ) ;
}
return false ;
2015-07-31 06:22:44 +00:00
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3SubmitClientCommand ( b3PhysicsClientHandle physClient , const b3SharedMemoryCommandHandle commandHandle )
2015-07-31 06:22:44 +00:00
{
2015-09-17 06:09:10 +00:00
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
2015-09-25 05:50:34 +00:00
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
2016-11-04 20:15:10 +00:00
b3Assert ( command ) ;
b3Assert ( cl ) ;
if ( command & & cl )
{
return ( int ) cl - > submitClientCommand ( * command ) ;
}
return - 1 ;
2015-07-31 06:22:44 +00:00
}
2017-02-22 17:33:30 +00:00
# include "../Utils/b3Clock.h"
2017-09-23 02:17:57 +00:00
2017-09-21 06:18:18 +00:00
B3_SHARED_API b3SharedMemoryStatusHandle b3SubmitClientCommandAndWaitStatus ( b3PhysicsClientHandle physClient , const b3SharedMemoryCommandHandle commandHandle )
2015-09-17 16:37:44 +00:00
{
2017-03-26 20:06:46 +00:00
B3_PROFILE ( " b3SubmitClientCommandAndWaitStatus " ) ;
2017-02-22 17:33:30 +00:00
b3Clock clock ;
double startTime = clock . getTimeInSeconds ( ) ;
2017-02-24 23:34:11 +00:00
2017-02-22 17:33:30 +00:00
2016-11-04 20:15:10 +00:00
b3SharedMemoryStatusHandle statusHandle = 0 ;
b3Assert ( commandHandle ) ;
b3Assert ( physClient ) ;
if ( physClient & & commandHandle )
{
2017-02-24 23:34:11 +00:00
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
double timeOutInSeconds = cl - > getTimeOut ( ) ;
2016-11-04 20:15:10 +00:00
{
2017-03-26 20:06:46 +00:00
B3_PROFILE ( " b3SubmitClientCommand " ) ;
b3SubmitClientCommand ( physClient , commandHandle ) ;
}
{
B3_PROFILE ( " b3ProcessServerStatus " ) ;
while ( cl - > isConnected ( ) & & ( statusHandle = = 0 ) & & ( clock . getTimeInSeconds ( ) - startTime < timeOutInSeconds ) )
{
clock . usleep ( 0 ) ;
statusHandle = b3ProcessServerStatus ( physClient ) ;
}
2016-11-04 20:15:10 +00:00
}
return ( b3SharedMemoryStatusHandle ) statusHandle ;
}
return 0 ;
2015-09-17 16:37:44 +00:00
}
2015-08-02 21:00:43 +00:00
2016-09-27 19:13:45 +00:00
///return the total number of bodies in the simulation
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3GetNumBodies ( b3PhysicsClientHandle physClient )
2016-09-27 19:13:45 +00:00
{
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
return cl - > getNumBodies ( ) ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3GetNumUserConstraints ( b3PhysicsClientHandle physClient )
2017-01-23 03:08:31 +00:00
{
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
return cl - > getNumUserConstraints ( ) ;
}
2017-09-25 06:26:06 +00:00
B3_SHARED_API int b3GetUserConstraintInfo ( b3PhysicsClientHandle physClient , int constraintUniqueId , struct b3UserConstraint * info )
2017-01-23 03:08:31 +00:00
{
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
b3UserConstraint constraintInfo1 ;
b3Assert ( physClient ) ;
2017-09-25 06:26:06 +00:00
b3Assert ( info ) ;
2017-01-23 03:08:31 +00:00
b3Assert ( constraintUniqueId > = 0 ) ;
2017-09-25 06:26:06 +00:00
if ( info = = 0 )
2017-01-23 03:08:31 +00:00
return 0 ;
if ( cl - > getUserConstraintInfo ( constraintUniqueId , constraintInfo1 ) )
{
2017-09-25 06:26:06 +00:00
* info = constraintInfo1 ;
2017-01-23 03:08:31 +00:00
return 1 ;
}
return 0 ;
}
2017-05-04 01:25:25 +00:00
/// return the user constraint id, given the index in range [0 , b3GetNumUserConstraints() )
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3GetUserConstraintId ( b3PhysicsClientHandle physClient , int serialIndex )
2017-05-04 01:25:25 +00:00
{
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
return cl - > getUserConstraintId ( serialIndex ) ;
}
2016-09-27 19:13:45 +00:00
/// return the body unique id, given the index in range [0 , b3GetNumBodies() )
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3GetBodyUniqueId ( b3PhysicsClientHandle physClient , int serialIndex )
2016-09-27 19:13:45 +00:00
{
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
return cl - > getBodyUniqueId ( serialIndex ) ;
}
///given a body unique id, return the body information. See b3BodyInfo in SharedMemoryPublic.h
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3GetBodyInfo ( b3PhysicsClientHandle physClient , int bodyUniqueId , struct b3BodyInfo * info )
2016-09-27 19:13:45 +00:00
{
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
return cl - > getBodyInfo ( bodyUniqueId , * info ) ;
}
2018-05-31 23:06:15 +00:00
B3_SHARED_API int b3GetNumJoints ( b3PhysicsClientHandle physClient , int bodyUniqueId )
2015-07-31 06:22:44 +00:00
{
2015-09-25 05:50:34 +00:00
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
2018-05-31 23:06:15 +00:00
return cl - > getNumJoints ( bodyUniqueId ) ;
2015-07-31 06:22:44 +00:00
}
2018-05-31 23:06:15 +00:00
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 )
2015-07-31 06:22:44 +00:00
{
2015-09-25 05:50:34 +00:00
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
2018-05-31 23:06:15 +00:00
return cl - > getJointInfo ( bodyUniqueId , jointIndex , * info ) ;
2015-07-31 06:22:44 +00:00
}
2017-09-23 02:17:57 +00:00
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 ;
}
2017-09-24 01:05:23 +00:00
B3_SHARED_API void b3CustomCommandLoadPlugin ( b3SharedMemoryCommandHandle commandHandle , const char * pluginPath )
2017-09-23 02:17:57 +00:00
{
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 ;
2017-09-24 01:05:23 +00:00
2017-09-23 02:17:57 +00:00
int len = strlen ( pluginPath ) ;
if ( len < MAX_FILENAME_LENGTH )
{
strcpy ( command - > m_customCommandArgs . m_pluginPath , pluginPath ) ;
}
}
}
2017-10-03 22:00:52 +00:00
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 ) ;
}
}
}
2017-09-23 02:17:57 +00:00
B3_SHARED_API int b3GetStatusPluginCommandResult ( b3SharedMemoryStatusHandle statusHandle )
{
int statusUniqueId = - 1 ;
const SharedMemoryStatus * status = ( const SharedMemoryStatus * ) statusHandle ;
b3Assert ( status ) ;
2017-10-10 18:10:42 +00:00
if ( status = = 0 )
return statusUniqueId ;
2017-09-23 02:17:57 +00:00
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 ) ;
2017-10-10 18:10:42 +00:00
if ( status )
2017-09-23 02:17:57 +00:00
{
2017-10-10 18:10:42 +00:00
b3Assert ( status - > m_type = = CMD_CUSTOM_COMMAND_COMPLETED ) ;
if ( status - > m_type = = CMD_CUSTOM_COMMAND_COMPLETED )
{
statusUniqueId = status - > m_customCommandResultArgs . m_pluginUniqueId ;
}
2017-09-23 02:17:57 +00:00
}
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 ;
}
}
2017-09-24 01:05:23 +00:00
B3_SHARED_API void b3CustomCommandExecutePluginCommand ( b3SharedMemoryCommandHandle commandHandle , int pluginUniqueId , const char * textArguments )
2017-09-23 02:17:57 +00:00
{
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 ;
2017-09-24 01:05:23 +00:00
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 ;
2018-09-06 00:58:14 +00:00
int len = textArguments ? strlen ( textArguments ) : 0 ;
2017-09-24 01:05:23 +00:00
2018-09-06 00:58:14 +00:00
if ( len & & len < MAX_FILENAME_LENGTH )
2017-09-23 02:17:57 +00:00
{
2017-09-24 01:05:23 +00:00
strcpy ( command - > m_customCommandArgs . m_arguments . m_text , textArguments ) ;
2017-09-23 02:17:57 +00:00
}
2017-09-24 01:05:23 +00:00
}
}
2017-09-23 02:17:57 +00:00
2017-09-24 01:05:23 +00:00
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 + + ;
}
}
}
2017-09-23 02:17:57 +00:00
2017-09-24 01:05:23 +00:00
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 + + ;
}
2017-09-23 02:17:57 +00:00
}
}
2017-09-24 01:05:23 +00:00
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3GetDynamicsInfoCommandInit ( b3PhysicsClientHandle physClient , int bodyUniqueId , int linkIndex )
2017-05-08 04:09:08 +00:00
{
2017-05-08 05:21:38 +00:00
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
b3Assert ( cl ) ;
b3Assert ( cl - > canSubmitCommand ( ) ) ;
struct SharedMemoryCommand * command = cl - > getAvailableSharedMemoryCommand ( ) ;
b3Assert ( command ) ;
2018-09-01 20:49:56 +00:00
return b3GetDynamicsInfoCommandInit2 ( ( b3SharedMemoryCommandHandle ) command , bodyUniqueId , linkIndex ) ;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3GetDynamicsInfoCommandInit2 ( b3SharedMemoryCommandHandle commandHandle , int bodyUniqueId , int linkIndex )
{
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
2017-05-09 17:31:28 +00:00
command - > m_type = CMD_GET_DYNAMICS_INFO ;
command - > m_getDynamicsInfoArgs . m_bodyUniqueId = bodyUniqueId ;
command - > m_getDynamicsInfoArgs . m_linkIndex = linkIndex ;
2018-09-01 20:49:56 +00:00
return ( b3SharedMemoryCommandHandle ) command ;
2017-05-08 05:21:38 +00:00
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3GetDynamicsInfo ( b3SharedMemoryStatusHandle statusHandle , struct b3DynamicsInfo * info )
2017-05-08 05:21:38 +00:00
{
const SharedMemoryStatus * status = ( const SharedMemoryStatus * ) statusHandle ;
2017-05-09 17:31:28 +00:00
const b3DynamicsInfo & dynamicsInfo = status - > m_dynamicsInfo ;
2017-12-20 22:54:32 +00:00
btAssert ( status - > m_type = = CMD_GET_DYNAMICS_INFO_COMPLETED ) ;
2017-05-09 17:31:28 +00:00
if ( status - > m_type ! = CMD_GET_DYNAMICS_INFO_COMPLETED )
2017-05-08 05:21:38 +00:00
return false ;
2018-01-04 03:17:28 +00:00
if ( info )
{
* info = dynamicsInfo ;
return true ;
}
return false ;
2017-05-08 04:09:08 +00:00
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3InitChangeDynamicsInfo ( b3PhysicsClientHandle physClient )
2017-05-02 05:18:54 +00:00
{
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
b3Assert ( cl ) ;
b3Assert ( cl - > canSubmitCommand ( ) ) ;
struct SharedMemoryCommand * command = cl - > getAvailableSharedMemoryCommand ( ) ;
b3Assert ( command ) ;
2018-09-01 20:49:56 +00:00
return b3InitChangeDynamicsInfo2 ( ( b3SharedMemoryCommandHandle ) command ) ;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitChangeDynamicsInfo2 ( b3SharedMemoryCommandHandle commandHandle )
{
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
2017-05-09 17:44:33 +00:00
command - > m_type = CMD_CHANGE_DYNAMICS_INFO ;
2017-06-07 15:37:42 +00:00
command - > m_changeDynamicsInfoArgs . m_bodyUniqueId = - 1 ;
command - > m_changeDynamicsInfoArgs . m_linkIndex = - 2 ;
2017-05-02 05:18:54 +00:00
command - > m_updateFlags = 0 ;
2018-09-01 20:49:56 +00:00
return commandHandle ;
2017-05-02 05:18:54 +00:00
}
2016-11-14 22:08:05 +00:00
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3ChangeDynamicsInfoSetMass ( b3SharedMemoryCommandHandle commandHandle , int bodyUniqueId , int linkIndex , double mass )
2017-05-02 05:18:54 +00:00
{
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
2017-05-09 17:44:33 +00:00
b3Assert ( command - > m_type = = CMD_CHANGE_DYNAMICS_INFO ) ;
2017-05-04 04:30:42 +00:00
b3Assert ( mass > 0 ) ;
2017-05-09 17:44:33 +00:00
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 ;
2017-05-02 05:18:54 +00:00
return 0 ;
}
2016-11-14 22:08:05 +00:00
2018-01-30 03:16:01 +00:00
B3_SHARED_API int b3ChangeDynamicsInfoSetLocalInertiaDiagonal ( b3SharedMemoryCommandHandle commandHandle , int bodyUniqueId , int linkIndex , double localInertiaDiagonal [ ] )
2017-12-21 00:56:31 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3ChangeDynamicsInfoSetLateralFriction ( b3SharedMemoryCommandHandle commandHandle , int bodyUniqueId , int linkIndex , double lateralFriction )
2017-05-04 04:47:53 +00:00
{
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
2017-05-09 17:44:33 +00:00
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 ;
2017-05-04 04:47:53 +00:00
return 0 ;
}
2016-11-14 22:08:05 +00:00
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3ChangeDynamicsInfoSetSpinningFriction ( b3SharedMemoryCommandHandle commandHandle , int bodyUniqueId , int linkIndex , double friction )
2017-05-27 01:14:38 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3ChangeDynamicsInfoSetRollingFriction ( b3SharedMemoryCommandHandle commandHandle , int bodyUniqueId , int linkIndex , double friction )
2017-05-27 01:14:38 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3ChangeDynamicsInfoSetRestitution ( b3SharedMemoryCommandHandle commandHandle , int bodyUniqueId , int linkIndex , double restitution )
2017-05-27 01:14:38 +00:00
{
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 ;
2017-06-05 05:04:16 +00:00
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3ChangeDynamicsInfoSetLinearDamping ( b3SharedMemoryCommandHandle commandHandle , int bodyUniqueId , double linearDamping )
2017-06-05 05:04:16 +00:00
{
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 ;
}
2017-05-27 01:14:38 +00:00
2017-06-05 05:04:16 +00:00
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3ChangeDynamicsInfoSetAngularDamping ( b3SharedMemoryCommandHandle commandHandle , int bodyUniqueId , double angularDamping )
2017-06-05 05:04:16 +00:00
{
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
b3Assert ( command - > m_type = = CMD_CHANGE_DYNAMICS_INFO ) ;
command - > m_changeDynamicsInfoArgs . m_bodyUniqueId = bodyUniqueId ;
2017-10-19 02:15:35 +00:00
command - > m_changeDynamicsInfoArgs . m_angularDamping = angularDamping ;
2017-06-05 05:04:16 +00:00
command - > m_updateFlags | = CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING ;
return 0 ;
2017-05-27 01:14:38 +00:00
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3ChangeDynamicsInfoSetContactStiffnessAndDamping ( b3SharedMemoryCommandHandle commandHandle , int bodyUniqueId , int linkIndex , double contactStiffness , double contactDamping )
2017-06-07 15:37:42 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3ChangeDynamicsInfoSetFrictionAnchor ( b3SharedMemoryCommandHandle commandHandle , int bodyUniqueId , int linkIndex , int frictionAnchor )
2017-06-07 16:37:28 +00:00
{
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 ;
}
2018-02-17 03:44:33 +00:00
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 ;
}
2018-05-23 03:26:00 +00:00
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 ;
}
2018-06-16 04:26:26 +00:00
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 ;
}
2017-06-07 16:37:28 +00:00
2017-06-07 15:37:42 +00:00
2018-05-31 23:06:15 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand ( b3PhysicsClientHandle physClient , int parentBodyUniqueId , int parentJointIndex , int childBodyUniqueId , int childJointIndex , struct b3JointInfo * info )
2016-08-23 01:14:29 +00:00
{
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
b3Assert ( cl ) ;
b3Assert ( cl - > canSubmitCommand ( ) ) ;
struct SharedMemoryCommand * command = cl - > getAvailableSharedMemoryCommand ( ) ;
b3Assert ( command ) ;
2018-09-04 06:13:15 +00:00
return b3InitCreateUserConstraintCommand2 ( ( b3SharedMemoryCommandHandle ) command , parentBodyUniqueId , parentJointIndex , childBodyUniqueId , childJointIndex , info ) ;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand2 ( b3SharedMemoryCommandHandle commandHandle , int parentBodyUniqueId , int parentJointIndex , int childBodyUniqueId , int childJointIndex , struct b3JointInfo * info )
{
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
command - > m_type = CMD_USER_CONSTRAINT ;
2016-11-14 22:08:05 +00:00
command - > m_updateFlags = USER_CONSTRAINT_ADD_CONSTRAINT ;
2018-09-04 06:13:15 +00:00
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 ;
2016-08-23 01:14:29 +00:00
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3InitChangeUserConstraintCommand ( b3PhysicsClientHandle physClient , int userConstraintUniqueId )
2017-01-12 18:30:46 +00:00
{
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 ;
}
2017-09-25 06:26:06 +00:00
B3_SHARED_API int b3InitChangeUserConstraintSetPivotInB ( b3SharedMemoryCommandHandle commandHandle , double jointChildPivot [ ] )
2017-01-12 18:30:46 +00:00
{
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 ;
2017-09-25 06:26:06 +00:00
command - > m_userConstraintArguments . m_childFrame [ 0 ] = jointChildPivot [ 0 ] ;
command - > m_userConstraintArguments . m_childFrame [ 1 ] = jointChildPivot [ 1 ] ;
command - > m_userConstraintArguments . m_childFrame [ 2 ] = jointChildPivot [ 2 ] ;
2017-01-12 18:30:46 +00:00
return 0 ;
}
2017-09-25 06:26:06 +00:00
B3_SHARED_API int b3InitChangeUserConstraintSetFrameInB ( b3SharedMemoryCommandHandle commandHandle , double jointChildFrameOrn [ ] )
2017-01-12 18:30:46 +00:00
{
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 ;
2017-09-25 06:26:06 +00:00
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 ] ;
2017-01-12 18:30:46 +00:00
return 0 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3InitChangeUserConstraintSetMaxForce ( b3SharedMemoryCommandHandle commandHandle , double maxAppliedForce )
2017-01-17 02:17:18 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3InitChangeUserConstraintSetGearRatio ( b3SharedMemoryCommandHandle commandHandle , double gearRatio )
2017-06-07 20:44:34 +00:00
{
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 ;
2017-01-17 02:17:18 +00:00
2017-06-07 20:44:34 +00:00
return 0 ;
}
2017-01-12 18:30:46 +00:00
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3InitChangeUserConstraintSetGearAuxLink ( b3SharedMemoryCommandHandle commandHandle , int gearAuxLink )
2017-06-24 03:24:04 +00:00
{
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 ;
}
2017-09-27 02:54:36 +00:00
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 ;
}
2017-10-19 02:15:35 +00:00
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 ;
}
2017-06-24 03:24:04 +00:00
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand ( b3PhysicsClientHandle physClient , int userConstraintUniqueId )
2016-11-14 22:08:05 +00:00
{
2017-10-19 02:15:35 +00:00
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
2016-11-14 22:08:05 +00:00
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 ;
}
2017-05-04 04:53:29 +00:00
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveBodyCommand ( b3PhysicsClientHandle physClient , int bodyUniqueId )
2017-05-04 04:53:29 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3GetStatusUserConstraintUniqueId ( b3SharedMemoryStatusHandle statusHandle )
2016-11-14 22:08:05 +00:00
{
const SharedMemoryStatus * status = ( const SharedMemoryStatus * ) statusHandle ;
b3Assert ( status ) ;
2017-10-10 18:10:42 +00:00
if ( status )
2016-11-14 22:08:05 +00:00
{
2017-10-10 18:10:42 +00:00
b3Assert ( status - > m_type = = CMD_USER_CONSTRAINT_COMPLETED ) ;
if ( status & & status - > m_type = = CMD_USER_CONSTRAINT_COMPLETED )
{
return status - > m_userConstraintResultArgs . m_userConstraintUniqueId ;
}
2016-11-14 22:08:05 +00:00
}
return - 1 ;
}
2017-10-19 02:15:35 +00:00
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3PickBody ( b3PhysicsClientHandle physClient , double rayFromWorldX ,
2015-11-05 00:08:28 +00:00
double rayFromWorldY , double rayFromWorldZ ,
double rayToWorldX , double rayToWorldY , double rayToWorldZ )
2015-09-25 05:42:22 +00:00
{
2015-11-05 00:08:28 +00:00
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3MovePickedBody ( b3PhysicsClientHandle physClient , double rayFromWorldX ,
2015-11-05 00:08:28 +00:00
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 ;
2015-09-25 05:42:22 +00:00
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3RemovePickingConstraint ( b3PhysicsClientHandle physClient )
2015-09-25 05:42:22 +00:00
{
2015-11-05 00:08:28 +00:00
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 ;
2015-09-25 05:42:22 +00:00
}
2015-09-17 06:09:10 +00:00
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3CreateRaycastCommandInit ( b3PhysicsClientHandle physClient , double rayFromWorldX ,
2016-12-27 03:40:09 +00:00
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 ;
2018-06-16 19:28:21 +00:00
command - > m_requestRaycastIntersections . m_numCommandRays = 1 ;
command - > m_requestRaycastIntersections . m_numStreamingRays = 0 ;
2018-06-15 14:47:04 +00:00
command - > m_requestRaycastIntersections . m_numThreads = 1 ;
2018-06-16 15:14:00 +00:00
2018-06-16 19:28:21 +00:00
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 ;
2016-12-27 03:40:09 +00:00
return ( b3SharedMemoryCommandHandle ) command ;
2017-04-05 22:21:26 +00:00
}
2016-12-27 03:40:09 +00:00
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3CreateRaycastBatchCommandInit ( b3PhysicsClientHandle physClient )
2017-04-05 22:21:26 +00:00
{
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 ;
2018-06-16 19:28:21 +00:00
command - > m_requestRaycastIntersections . m_numCommandRays = 0 ;
command - > m_requestRaycastIntersections . m_numStreamingRays = 0 ;
2018-06-15 14:47:04 +00:00
command - > m_requestRaycastIntersections . m_numThreads = 1 ;
2017-04-05 22:21:26 +00:00
return ( b3SharedMemoryCommandHandle ) command ;
2016-12-27 03:40:09 +00:00
}
2018-06-15 14:47:04 +00:00
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 ;
}
2018-06-16 19:28:21 +00:00
B3_SHARED_API void b3RaycastBatchAddRay ( b3SharedMemoryCommandHandle commandHandle , const double rayFromWorld [ 3 ] , const double rayToWorld [ 3 ] )
2017-04-05 22:21:26 +00:00
{
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 )
{
2018-06-16 19:28:21 +00:00
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 + + ;
}
2018-06-16 15:14:00 +00:00
}
}
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 ) ;
2018-06-16 19:28:21 +00:00
b3Assert ( numRays < MAX_RAY_INTERSECTION_BATCH_SIZE_STREAMING ) ;
2018-06-16 15:14:00 +00:00
if ( command - > m_type = = CMD_REQUEST_RAY_CAST_INTERSECTIONS )
{
cl - > uploadRaysToSharedMemory ( * command , rayFromWorldArray , rayToWorldArray , numRays ) ;
2017-04-05 22:21:26 +00:00
}
}
2018-06-16 15:14:00 +00:00
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3GetRaycastInformation ( b3PhysicsClientHandle physClient , struct b3RaycastInformation * raycastInfo )
2016-12-27 03:40:09 +00:00
{
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
if ( cl )
{
cl - > getCachedRaycastHits ( raycastInfo ) ;
}
}
2017-01-21 02:13:24 +00:00
///If you re-connected to an existing server, or server changed otherwise, sync the body info
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3InitSyncBodyInfoCommand ( b3PhysicsClientHandle physClient )
2017-01-21 02:13:24 +00:00
{
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 ;
}
2016-12-27 03:40:09 +00:00
2018-06-02 20:40:08 +00:00
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 ;
}
2018-07-03 15:45:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3InitAddUserDataCommand ( b3PhysicsClientHandle physClient , int bodyUniqueId , int linkIndex , int visualShapeIndex , const char * key , UserDataValueType valueType , int valueLength , const void * valueData ) {
2018-06-02 20:40:08 +00:00
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 ;
2018-07-03 15:45:19 +00:00
command - > m_addUserDataRequestArgs . m_visualShapeIndex = visualShapeIndex ;
2018-06-02 20:40:08 +00:00
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 ;
}
2018-07-03 15:45:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveUserDataCommand ( b3PhysicsClientHandle physClient , int userDataId ) {
2018-06-02 20:40:08 +00:00
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 ;
}
2018-07-03 15:45:19 +00:00
B3_SHARED_API int b3GetUserData ( b3PhysicsClientHandle physClient , int userDataId , struct b3UserDataValue * valueOut )
2018-06-02 20:40:08 +00:00
{
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
if ( cl )
{
2018-07-03 15:45:19 +00:00
return cl - > getCachedUserData ( userDataId , * valueOut ) ;
2018-06-02 20:40:08 +00:00
}
return false ;
}
2018-07-03 15:45:19 +00:00
B3_SHARED_API int b3GetUserDataId ( b3PhysicsClientHandle physClient , int bodyUniqueId , int linkIndex , int visualShapeIndex , const char * key )
2018-06-02 20:40:08 +00:00
{
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
if ( cl )
{
2018-07-03 15:45:19 +00:00
return cl - > getCachedUserDataId ( bodyUniqueId , linkIndex , visualShapeIndex , key ) ;
2018-06-02 20:40:08 +00:00
}
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 ) ;
2018-07-03 15:45:19 +00:00
return status - > m_userDataResponseArgs . m_userDataId ;
2018-06-02 20:40:08 +00:00
}
return - 1 ;
}
2018-07-03 15:45:19 +00:00
B3_SHARED_API int b3GetNumUserData ( b3PhysicsClientHandle physClient , int bodyUniqueId )
2018-06-02 20:40:08 +00:00
{
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
if ( cl )
{
2018-07-03 15:45:19 +00:00
return cl - > getNumUserData ( bodyUniqueId ) ;
2018-06-02 20:40:08 +00:00
}
return 0 ;
}
2018-07-03 15:45:19 +00:00
B3_SHARED_API void b3GetUserDataInfo ( b3PhysicsClientHandle physClient , int bodyUniqueId , int userDataIndex , const char * * keyOut , int * userDataIdOut , int * linkIndexOut , int * visualShapeIndexOut )
2018-06-02 20:40:08 +00:00
{
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
if ( cl )
{
2018-07-03 15:45:19 +00:00
cl - > getUserDataInfo ( bodyUniqueId , userDataIndex , keyOut , userDataIdOut , linkIndexOut , visualShapeIndexOut ) ;
2018-06-02 20:40:08 +00:00
}
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand ( b3PhysicsClientHandle physClient , int debugMode )
2015-09-17 06:09:10 +00:00
{
2015-09-25 05:50:34 +00:00
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
2015-09-17 06:09:10 +00:00
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3GetDebugLines ( b3PhysicsClientHandle physClient , struct b3DebugLines * lines )
2015-09-17 06:09:10 +00:00
{
2015-09-25 05:50:34 +00:00
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
2015-09-17 06:09:10 +00:00
b3Assert ( lines ) ;
if ( lines )
{
lines - > m_numDebugLines = cl - > getNumDebugLines ( ) ;
lines - > m_linesFrom = cl - > getDebugLinesFrom ( ) ;
lines - > m_linesTo = cl - > getDebugLinesTo ( ) ;
lines - > m_linesColor = cl - > getDebugLinesColor ( ) ;
}
}
2016-05-18 06:57:19 +00:00
2016-11-14 15:39:34 +00:00
/// Add/remove user-specific debug lines and debug text messages
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugDrawAddLine3D ( b3PhysicsClientHandle physClient , double fromXYZ [ 3 ] , double toXYZ [ 3 ] , double colorRGB [ 3 ] , double lineWidth , double lifeTime )
2016-11-14 15:39:34 +00:00
{
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 ;
2017-05-24 16:06:15 +00:00
command - > m_userDebugDrawArgs . m_parentObjectUniqueId = - 1 ;
command - > m_userDebugDrawArgs . m_parentLinkIndex = - 1 ;
Improve debug text/line rendering, can be use to draw frames and text in local coordinate of an object / link.
example:
kuka = p.loadURDF("kuka_iiwa/model.urdf")
p.getNumJoints(kuka)
pybullet.addUserDebugLine([0,0,0],[0,0,0.1],[0,0,1],trackObjectUniqueId=2,trackLinkIndex=6)
pybullet.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],trackObjectUniqueId=2,trackLinkIndex=6)
Also allow to render text using a given orientation (instead of pointing to the camera), example:
pybullet.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],textOrientation=[0,0,0,1], trackObjectUniqueId=2,trackLinkIndex=6)
Add drawTexturedTriangleMesh, for drawing 3d text.
Expose readSingleInstanceTransformToCPU, to extract position/orientation from graphics index.
updateTexture: allow to not flip texels around up axis
2017-05-24 05:05:26 +00:00
command - > m_userDebugDrawArgs . m_optionFlags = 0 ;
2016-11-14 15:39:34 +00:00
return ( b3SharedMemoryCommandHandle ) command ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugDrawAddText3D ( b3PhysicsClientHandle physClient , const char * txt , double positionXYZ [ 3 ] , double colorRGB [ 3 ] , double textSize , double lifeTime )
2016-11-14 15:39:34 +00:00
{
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 ;
2017-05-24 16:06:15 +00:00
command - > m_userDebugDrawArgs . m_parentObjectUniqueId = - 1 ;
command - > m_userDebugDrawArgs . m_parentLinkIndex = - 1 ;
Improve debug text/line rendering, can be use to draw frames and text in local coordinate of an object / link.
example:
kuka = p.loadURDF("kuka_iiwa/model.urdf")
p.getNumJoints(kuka)
pybullet.addUserDebugLine([0,0,0],[0,0,0.1],[0,0,1],trackObjectUniqueId=2,trackLinkIndex=6)
pybullet.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],trackObjectUniqueId=2,trackLinkIndex=6)
Also allow to render text using a given orientation (instead of pointing to the camera), example:
pybullet.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],textOrientation=[0,0,0,1], trackObjectUniqueId=2,trackLinkIndex=6)
Add drawTexturedTriangleMesh, for drawing 3d text.
Expose readSingleInstanceTransformToCPU, to extract position/orientation from graphics index.
updateTexture: allow to not flip texels around up axis
2017-05-24 05:05:26 +00:00
command - > m_userDebugDrawArgs . m_optionFlags = 0 ;
2016-11-14 15:39:34 +00:00
return ( b3SharedMemoryCommandHandle ) command ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3UserDebugTextSetOptionFlags ( b3SharedMemoryCommandHandle commandHandle , int optionFlags )
Improve debug text/line rendering, can be use to draw frames and text in local coordinate of an object / link.
example:
kuka = p.loadURDF("kuka_iiwa/model.urdf")
p.getNumJoints(kuka)
pybullet.addUserDebugLine([0,0,0],[0,0,0.1],[0,0,1],trackObjectUniqueId=2,trackLinkIndex=6)
pybullet.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],trackObjectUniqueId=2,trackLinkIndex=6)
Also allow to render text using a given orientation (instead of pointing to the camera), example:
pybullet.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],textOrientation=[0,0,0,1], trackObjectUniqueId=2,trackLinkIndex=6)
Add drawTexturedTriangleMesh, for drawing 3d text.
Expose readSingleInstanceTransformToCPU, to extract position/orientation from graphics index.
updateTexture: allow to not flip texels around up axis
2017-05-24 05:05:26 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3UserDebugTextSetOrientation ( b3SharedMemoryCommandHandle commandHandle , double orientation [ 4 ] )
Improve debug text/line rendering, can be use to draw frames and text in local coordinate of an object / link.
example:
kuka = p.loadURDF("kuka_iiwa/model.urdf")
p.getNumJoints(kuka)
pybullet.addUserDebugLine([0,0,0],[0,0,0.1],[0,0,1],trackObjectUniqueId=2,trackLinkIndex=6)
pybullet.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],trackObjectUniqueId=2,trackLinkIndex=6)
Also allow to render text using a given orientation (instead of pointing to the camera), example:
pybullet.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],textOrientation=[0,0,0,1], trackObjectUniqueId=2,trackLinkIndex=6)
Add drawTexturedTriangleMesh, for drawing 3d text.
Expose readSingleInstanceTransformToCPU, to extract position/orientation from graphics index.
updateTexture: allow to not flip texels around up axis
2017-05-24 05:05:26 +00:00
{
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 ;
}
2017-10-25 15:15:01 +00:00
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 ;
}
Improve debug text/line rendering, can be use to draw frames and text in local coordinate of an object / link.
example:
kuka = p.loadURDF("kuka_iiwa/model.urdf")
p.getNumJoints(kuka)
pybullet.addUserDebugLine([0,0,0],[0,0,0.1],[0,0,1],trackObjectUniqueId=2,trackLinkIndex=6)
pybullet.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],trackObjectUniqueId=2,trackLinkIndex=6)
Also allow to render text using a given orientation (instead of pointing to the camera), example:
pybullet.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],textOrientation=[0,0,0,1], trackObjectUniqueId=2,trackLinkIndex=6)
Add drawTexturedTriangleMesh, for drawing 3d text.
Expose readSingleInstanceTransformToCPU, to extract position/orientation from graphics index.
updateTexture: allow to not flip texels around up axis
2017-05-24 05:05:26 +00:00
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3UserDebugItemSetParentObject ( b3SharedMemoryCommandHandle commandHandle , int objectUniqueId , int linkIndex )
Improve debug text/line rendering, can be use to draw frames and text in local coordinate of an object / link.
example:
kuka = p.loadURDF("kuka_iiwa/model.urdf")
p.getNumJoints(kuka)
pybullet.addUserDebugLine([0,0,0],[0,0,0.1],[0,0,1],trackObjectUniqueId=2,trackLinkIndex=6)
pybullet.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],trackObjectUniqueId=2,trackLinkIndex=6)
Also allow to render text using a given orientation (instead of pointing to the camera), example:
pybullet.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],textOrientation=[0,0,0,1], trackObjectUniqueId=2,trackLinkIndex=6)
Add drawTexturedTriangleMesh, for drawing 3d text.
Expose readSingleInstanceTransformToCPU, to extract position/orientation from graphics index.
updateTexture: allow to not flip texels around up axis
2017-05-24 05:05:26 +00:00
{
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
b3Assert ( command ) ;
b3Assert ( command - > m_type = = CMD_USER_DEBUG_DRAW ) ;
2017-05-24 16:06:15 +00:00
command - > m_updateFlags | = USER_DEBUG_HAS_PARENT_OBJECT ;
command - > m_userDebugDrawArgs . m_parentObjectUniqueId = objectUniqueId ;
command - > m_userDebugDrawArgs . m_parentLinkIndex = linkIndex ;
Improve debug text/line rendering, can be use to draw frames and text in local coordinate of an object / link.
example:
kuka = p.loadURDF("kuka_iiwa/model.urdf")
p.getNumJoints(kuka)
pybullet.addUserDebugLine([0,0,0],[0,0,0.1],[0,0,1],trackObjectUniqueId=2,trackLinkIndex=6)
pybullet.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],trackObjectUniqueId=2,trackLinkIndex=6)
Also allow to render text using a given orientation (instead of pointing to the camera), example:
pybullet.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],textOrientation=[0,0,0,1], trackObjectUniqueId=2,trackLinkIndex=6)
Add drawTexturedTriangleMesh, for drawing 3d text.
Expose readSingleInstanceTransformToCPU, to extract position/orientation from graphics index.
updateTexture: allow to not flip texels around up axis
2017-05-24 05:05:26 +00:00
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugAddParameter ( b3PhysicsClientHandle physClient , const char * txt , double rangeMin , double rangeMax , double startValue )
2017-01-17 23:42:32 +00:00
{
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 ;
2017-05-24 16:06:15 +00:00
command - > m_userDebugDrawArgs . m_parentObjectUniqueId = - 1 ;
Improve debug text/line rendering, can be use to draw frames and text in local coordinate of an object / link.
example:
kuka = p.loadURDF("kuka_iiwa/model.urdf")
p.getNumJoints(kuka)
pybullet.addUserDebugLine([0,0,0],[0,0,0.1],[0,0,1],trackObjectUniqueId=2,trackLinkIndex=6)
pybullet.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],trackObjectUniqueId=2,trackLinkIndex=6)
Also allow to render text using a given orientation (instead of pointing to the camera), example:
pybullet.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],textOrientation=[0,0,0,1], trackObjectUniqueId=2,trackLinkIndex=6)
Add drawTexturedTriangleMesh, for drawing 3d text.
Expose readSingleInstanceTransformToCPU, to extract position/orientation from graphics index.
updateTexture: allow to not flip texels around up axis
2017-05-24 05:05:26 +00:00
command - > m_userDebugDrawArgs . m_optionFlags = 0 ;
2017-01-17 23:42:32 +00:00
return ( b3SharedMemoryCommandHandle ) command ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugReadParameter ( b3PhysicsClientHandle physClient , int debugItemUniqueId )
2017-01-17 23:42:32 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3GetStatusDebugParameterValue ( b3SharedMemoryStatusHandle statusHandle , double * paramValue )
2017-01-17 23:42:32 +00:00
{
const SharedMemoryStatus * status = ( const SharedMemoryStatus * ) statusHandle ;
2017-10-10 18:10:42 +00:00
if ( status )
2017-01-17 23:42:32 +00:00
{
2017-10-10 18:10:42 +00:00
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 ;
}
2017-01-17 23:42:32 +00:00
}
return 0 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugDrawRemove ( b3PhysicsClientHandle physClient , int debugItemUniqueId )
2016-11-14 15:39:34 +00:00
{
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 ;
2017-01-17 23:42:32 +00:00
command - > m_userDebugDrawArgs . m_itemUniqueId = debugItemUniqueId ;
2016-11-14 15:39:34 +00:00
return ( b3SharedMemoryCommandHandle ) command ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugDrawRemoveAll ( b3PhysicsClientHandle physClient )
2016-11-14 15:39:34 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3GetDebugItemUniqueId ( b3SharedMemoryStatusHandle statusHandle )
2016-11-14 15:39:34 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3InitDebugDrawingCommand ( b3PhysicsClientHandle physClient )
2016-11-21 15:42:11 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3SetDebugObjectColor ( b3SharedMemoryCommandHandle commandHandle , int objectUniqueId , int linkIndex , double objectColorRGB [ 3 ] )
2016-11-21 15:42:11 +00:00
{
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 ] ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3RemoveDebugObjectColor ( b3SharedMemoryCommandHandle commandHandle , int objectUniqueId , int linkIndex )
2016-11-21 15:42:11 +00:00
{
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 ;
}
2016-11-14 15:39:34 +00:00
2016-05-18 06:57:19 +00:00
///request an image from a simulated camera, using a software renderer.
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestCameraImage ( b3PhysicsClientHandle physClient )
2016-05-18 06:57:19 +00:00
{
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
b3Assert ( cl ) ;
b3Assert ( cl - > canSubmitCommand ( ) ) ;
struct SharedMemoryCommand * command = cl - > getAvailableSharedMemoryCommand ( ) ;
b3Assert ( command ) ;
2018-09-04 06:13:15 +00:00
return b3InitRequestCameraImage2 ( ( b3SharedMemoryCommandHandle ) command ) ;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestCameraImage2 ( b3SharedMemoryCommandHandle commandHandle )
{
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
command - > m_type = CMD_REQUEST_CAMERA_IMAGE_DATA ;
2016-05-31 17:23:04 +00:00
command - > m_requestPixelDataArguments . m_startPixelIndex = 0 ;
2018-09-04 06:13:15 +00:00
command - > m_updateFlags = 0 ; //REQUEST_PIXEL_ARGS_USE_HARDWARE_OPENGL;
return ( b3SharedMemoryCommandHandle ) command ;
2016-05-18 06:57:19 +00:00
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3RequestCameraImageSelectRenderer ( b3SharedMemoryCommandHandle commandHandle , int renderer )
2016-07-13 01:16:13 +00:00
{
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
b3Assert ( command ) ;
b3Assert ( command - > m_type = = CMD_REQUEST_CAMERA_IMAGE_DATA ) ;
b3Assert ( renderer > ( 1 < < 15 ) ) ;
2017-05-18 02:29:12 +00:00
if ( renderer > ( 1 < < 15 ) )
{
command - > m_updateFlags | = renderer ;
}
2016-07-13 01:16:13 +00:00
}
2016-05-18 06:57:19 +00:00
2017-12-28 20:37:07 +00:00
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 ;
}
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3RequestCameraImageSetCameraMatrices ( b3SharedMemoryCommandHandle commandHandle , float viewMatrix [ 16 ] , float projectionMatrix [ 16 ] )
2016-05-18 06:57:19 +00:00
{
2016-06-01 18:04:10 +00:00
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 ;
2016-05-18 06:57:19 +00:00
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3RequestCameraImageSetLightDirection ( b3SharedMemoryCommandHandle commandHandle , const float lightDirection [ 3 ] )
2016-08-02 18:12:23 +00:00
{
2016-11-17 23:15:52 +00:00
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
b3Assert ( command ) ;
b3Assert ( command - > m_type = = CMD_REQUEST_CAMERA_IMAGE_DATA ) ;
for ( int i = 0 ; i < 3 ; i + + )
2016-08-02 18:12:23 +00:00
{
2016-11-17 23:15:52 +00:00
command - > m_requestPixelDataArguments . m_lightDirection [ i ] = lightDirection [ i ] ;
}
command - > m_updateFlags | = REQUEST_PIXEL_ARGS_SET_LIGHT_DIRECTION ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3RequestCameraImageSetLightColor ( b3SharedMemoryCommandHandle commandHandle , const float lightColor [ 3 ] )
2016-11-20 20:52:12 +00:00
{
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 ;
}
2016-11-17 23:15:52 +00:00
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3RequestCameraImageSetLightDistance ( b3SharedMemoryCommandHandle commandHandle , float lightDistance )
2016-11-29 21:19:35 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3RequestCameraImageSetLightAmbientCoeff ( b3SharedMemoryCommandHandle commandHandle , float lightAmbientCoeff )
2016-12-06 23:21:35 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3RequestCameraImageSetLightDiffuseCoeff ( b3SharedMemoryCommandHandle commandHandle , float lightDiffuseCoeff )
2016-12-06 23:21:35 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3RequestCameraImageSetLightSpecularCoeff ( b3SharedMemoryCommandHandle commandHandle , float lightSpecularCoeff )
2016-12-06 23:21:35 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3RequestCameraImageSetShadow ( b3SharedMemoryCommandHandle commandHandle , int hasShadow )
2016-11-29 21:19:35 +00:00
{
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 ;
}
2018-03-19 01:45:54 +00:00
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3ComputePositionFromViewMatrix ( const float viewMatrix [ 16 ] , float cameraPosition [ 3 ] , float cameraTargetPosition [ 3 ] , float cameraUp [ 3 ] )
2017-06-01 22:30:37 +00:00
{
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 ] ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3ComputeViewMatrixFromPositions ( const float cameraPosition [ 3 ] , const float cameraTargetPosition [ 3 ] , const float cameraUp [ 3 ] , float viewMatrix [ 16 ] )
2016-11-17 23:15:52 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3ComputeViewMatrixFromYawPitchRoll ( const float cameraTargetPosition [ 3 ] , float distance , float yaw , float pitch , float roll , int upAxis , float viewMatrix [ 16 ] )
2016-11-17 23:15:52 +00:00
{
b3Vector3 camUpVector ;
b3Vector3 camForward ;
b3Vector3 camPos ;
b3Vector3 camTargetPos = b3MakeVector3 ( cameraTargetPosition [ 0 ] , cameraTargetPosition [ 1 ] , cameraTargetPosition [ 2 ] ) ;
b3Vector3 eyePos = b3MakeVector3 ( 0 , 0 , 0 ) ;
2017-06-01 22:30:37 +00:00
b3Scalar yawRad = yaw * b3Scalar ( 0.01745329251994329547 ) ; // rads per deg
b3Scalar pitchRad = pitch * b3Scalar ( 0.01745329251994329547 ) ; // rads per deg
b3Scalar rollRad = 0.0 ;
b3Quaternion eyeRot ;
2016-11-17 23:15:52 +00:00
int forwardAxis ( - 1 ) ;
2017-06-01 22:30:37 +00:00
switch ( upAxis )
2016-08-02 18:12:23 +00:00
{
2016-11-17 23:15:52 +00:00
case 1 :
2017-06-01 22:30:37 +00:00
forwardAxis = 2 ;
camUpVector = b3MakeVector3 ( 0 , 1 , 0 ) ;
eyeRot . setEulerZYX ( rollRad , yawRad , - pitchRad ) ;
2016-11-17 23:15:52 +00:00
break ;
case 2 :
forwardAxis = 1 ;
2017-06-01 22:30:37 +00:00
camUpVector = b3MakeVector3 ( 0 , 0 , 1 ) ;
eyeRot . setEulerZYX ( yawRad , rollRad , pitchRad ) ;
2016-11-17 23:15:52 +00:00
break ;
default :
return ;
2017-06-01 22:30:37 +00:00
} ;
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 ( ) ;
2016-08-02 18:12:23 +00:00
}
2017-06-01 22:30:37 +00:00
2017-06-03 00:40:50 +00:00
eyePos = b3Matrix3x3 ( eyeRot ) * eyePos ;
2017-06-01 22:30:37 +00:00
camUpVector = b3Matrix3x3 ( eyeRot ) * camUpVector ;
2016-08-02 18:12:23 +00:00
camPos = eyePos ;
camPos + = camTargetPos ;
2016-11-17 23:15:52 +00:00
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 ) ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3ComputeProjectionMatrix ( float left , float right , float bottom , float top , float nearVal , float farVal , float projectionMatrix [ 16 ] )
2016-11-17 23:15:52 +00:00
{
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 ) ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3ComputeProjectionMatrixFOV ( float fov , float aspect , float nearVal , float farVal , float projectionMatrix [ 16 ] )
2016-11-17 23:15:52 +00:00
{
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 ) ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3RequestCameraImageSetViewMatrix2 ( b3SharedMemoryCommandHandle commandHandle , const float cameraTargetPosition [ 3 ] , float distance , float yaw , float pitch , float roll , int upAxis )
2016-11-17 23:15:52 +00:00
{
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 ;
2016-08-02 18:12:23 +00:00
}
2016-11-17 23:15:52 +00:00
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3RequestCameraImageSetViewMatrix ( b3SharedMemoryCommandHandle commandHandle , const float cameraPosition [ 3 ] , const float cameraTargetPosition [ 3 ] , const float cameraUp [ 3 ] )
2016-06-16 18:48:37 +00:00
{
2016-11-17 23:15:52 +00:00
float viewMatrix [ 16 ] ;
b3ComputeViewMatrixFromPositions ( cameraPosition , cameraTargetPosition , cameraUp , viewMatrix ) ;
2016-06-16 18:48:37 +00:00
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
b3Assert ( command ) ;
b3Assert ( command - > m_type = = CMD_REQUEST_CAMERA_IMAGE_DATA ) ;
2016-11-17 23:15:52 +00:00
b3ComputeViewMatrixFromPositions ( cameraPosition , cameraTargetPosition , cameraUp , command - > m_requestPixelDataArguments . m_viewMatrix ) ;
2016-06-16 18:48:37 +00:00
command - > m_updateFlags | = REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3RequestCameraImageSetProjectionMatrix ( b3SharedMemoryCommandHandle commandHandle , float left , float right , float bottom , float top , float nearVal , float farVal )
2016-06-16 18:48:37 +00:00
{
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
b3Assert ( command ) ;
b3Assert ( command - > m_type = = CMD_REQUEST_CAMERA_IMAGE_DATA ) ;
2016-11-17 23:15:52 +00:00
b3ComputeProjectionMatrix ( left , right , bottom , top , nearVal , farVal , command - > m_requestPixelDataArguments . m_projectionMatrix ) ;
2016-06-16 18:48:37 +00:00
command - > m_updateFlags | = REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3RequestCameraImageSetFOVProjectionMatrix ( b3SharedMemoryCommandHandle commandHandle , float fov , float aspect , float nearVal , float farVal )
2016-07-08 21:29:58 +00:00
{
2016-11-17 23:15:52 +00:00
2016-07-08 21:29:58 +00:00
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
b3Assert ( command ) ;
b3Assert ( command - > m_type = = CMD_REQUEST_CAMERA_IMAGE_DATA ) ;
2016-11-17 23:15:52 +00:00
b3ComputeProjectionMatrixFOV ( fov , aspect , nearVal , farVal , command - > m_requestPixelDataArguments . m_projectionMatrix ) ;
2016-07-08 21:29:58 +00:00
command - > m_updateFlags | = REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3RequestCameraImageSetPixelResolution ( b3SharedMemoryCommandHandle commandHandle , int width , int height )
2016-06-07 23:11:58 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3GetCameraImageData ( b3PhysicsClientHandle physClient , struct b3CameraImageData * imageData )
2016-05-18 06:57:19 +00:00
{
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
if ( cl )
{
2016-05-31 17:23:04 +00:00
cl - > getCachedCameraImage ( imageData ) ;
2016-05-18 06:57:19 +00:00
}
}
2016-09-01 20:30:07 +00:00
///request an contact point information
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestContactPointInformation ( b3PhysicsClientHandle physClient )
2016-09-01 20:30:07 +00:00
{
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 ;
2016-11-20 01:13:56 +00:00
command - > m_requestContactPointArguments . m_linkIndexAIndexFilter = - 2 ;
command - > m_requestContactPointArguments . m_linkIndexBIndexFilter = - 2 ;
2016-09-01 20:30:07 +00:00
command - > m_updateFlags = 0 ;
return ( b3SharedMemoryCommandHandle ) command ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3SetContactFilterBodyA ( b3SharedMemoryCommandHandle commandHandle , int bodyUniqueIdA )
2016-09-01 20:30:07 +00:00
{
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
b3Assert ( command ) ;
b3Assert ( command - > m_type = = CMD_REQUEST_CONTACT_POINT_INFORMATION ) ;
command - > m_requestContactPointArguments . m_objectAIndexFilter = bodyUniqueIdA ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3SetContactFilterLinkA ( b3SharedMemoryCommandHandle commandHandle , int linkIndexA )
2016-11-20 01:13:56 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3SetContactFilterLinkB ( b3SharedMemoryCommandHandle commandHandle , int linkIndexB )
2016-11-20 01:13:56 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3SetClosestDistanceFilterLinkA ( b3SharedMemoryCommandHandle commandHandle , int linkIndexA )
2016-11-20 01:13:56 +00:00
{
b3SetContactFilterLinkA ( commandHandle , linkIndexA ) ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3SetClosestDistanceFilterLinkB ( b3SharedMemoryCommandHandle commandHandle , int linkIndexB )
2016-11-20 01:13:56 +00:00
{
b3SetContactFilterLinkB ( commandHandle , linkIndexB ) ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3SetContactFilterBodyB ( b3SharedMemoryCommandHandle commandHandle , int bodyUniqueIdB )
2016-09-01 20:30:07 +00:00
{
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
b3Assert ( command ) ;
b3Assert ( command - > m_type = = CMD_REQUEST_CONTACT_POINT_INFORMATION ) ;
command - > m_requestContactPointArguments . m_objectBIndexFilter = bodyUniqueIdB ;
}
2016-11-10 05:01:04 +00:00
///compute the closest points between two bodies
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3InitClosestDistanceQuery ( b3PhysicsClientHandle physClient )
2016-11-10 05:01:04 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3SetClosestDistanceFilterBodyA ( b3SharedMemoryCommandHandle commandHandle , int bodyUniqueIdA )
2016-11-10 05:01:04 +00:00
{
b3SetContactFilterBodyA ( commandHandle , bodyUniqueIdA ) ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3SetClosestDistanceFilterBodyB ( b3SharedMemoryCommandHandle commandHandle , int bodyUniqueIdB )
2016-11-10 05:01:04 +00:00
{
b3SetContactFilterBodyB ( commandHandle , bodyUniqueIdB ) ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3SetClosestDistanceThreshold ( b3SharedMemoryCommandHandle commandHandle , double distance )
2016-11-10 05:01:04 +00:00
{
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)
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3InitAABBOverlapQuery ( b3PhysicsClientHandle physClient , const double aabbMin [ 3 ] , const double aabbMax [ 3 ] )
2016-11-10 05:01:04 +00:00
{
2016-11-10 19:22:22 +00:00
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
b3Assert ( cl ) ;
b3Assert ( cl - > canSubmitCommand ( ) ) ;
struct SharedMemoryCommand * command = cl - > getAvailableSharedMemoryCommand ( ) ;
2016-11-10 05:01:04 +00:00
b3Assert ( command ) ;
2016-11-10 19:22:22 +00:00
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 ] ;
2016-11-10 05:01:04 +00:00
2016-11-10 19:22:22 +00:00
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 ;
2016-11-10 05:01:04 +00:00
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3GetAABBOverlapResults ( b3PhysicsClientHandle physClient , struct b3AABBOverlapData * data )
2016-11-10 05:01:04 +00:00
{
2016-11-10 19:22:22 +00:00
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
if ( cl )
{
cl - > getCachedOverlappingObjects ( data ) ;
}
2016-11-10 05:01:04 +00:00
}
2016-09-01 20:30:07 +00:00
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3GetContactPointInformation ( b3PhysicsClientHandle physClient , struct b3ContactInformation * contactPointData )
2016-09-01 20:30:07 +00:00
{
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
if ( cl )
{
cl - > getCachedContactPointInformation ( contactPointData ) ;
}
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3GetClosestPointInformation ( b3PhysicsClientHandle physClient , struct b3ContactInformation * contactPointInfo )
2016-11-10 05:01:04 +00:00
{
b3GetContactPointInformation ( physClient , contactPointInfo ) ;
}
2016-09-01 20:30:07 +00:00
2018-01-04 23:48:59 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestCollisionShapeInformation ( b3PhysicsClientHandle physClient , int bodyUniqueId , int linkIndex )
2018-01-04 03:17:28 +00:00
{
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
b3Assert ( cl ) ;
b3Assert ( cl - > canSubmitCommand ( ) ) ;
struct SharedMemoryCommand * command = cl - > getAvailableSharedMemoryCommand ( ) ;
b3Assert ( command ) ;
command - > m_type = CMD_REQUEST_COLLISION_SHAPE_INFO ;
2018-01-04 23:48:59 +00:00
command - > m_requestCollisionShapeDataArguments . m_bodyUniqueId = bodyUniqueId ;
2018-01-04 03:17:28 +00:00
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 ) ;
}
}
2016-10-19 05:05:28 +00:00
//request visual shape information
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestVisualShapeInformation ( b3PhysicsClientHandle physClient , int bodyUniqueIdA )
2016-10-19 05:05:28 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3GetVisualShapeInformation ( b3PhysicsClientHandle physClient , struct b3VisualShapeInformation * visualShapeInfo )
2016-10-19 05:05:28 +00:00
{
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
if ( cl )
{
cl - > getCachedVisualShapeInformation ( visualShapeInfo ) ;
}
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3CreateChangeTextureCommandInit ( b3PhysicsClientHandle physClient , int textureUniqueId , int width , int height , const char * rgbPixels )
2017-07-01 02:11:43 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3InitLoadTexture ( b3PhysicsClientHandle physClient , const char * filename )
2016-10-20 17:56:44 +00:00
{
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
b3Assert ( cl ) ;
b3Assert ( cl - > canSubmitCommand ( ) ) ;
struct SharedMemoryCommand * command = cl - > getAvailableSharedMemoryCommand ( ) ;
b3Assert ( command ) ;
2016-10-21 06:40:30 +00:00
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 ;
}
2016-10-20 17:56:44 +00:00
command - > m_updateFlags = 0 ;
return ( b3SharedMemoryCommandHandle ) command ;
2016-10-21 06:40:30 +00:00
}
2016-10-19 05:05:28 +00:00
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3GetStatusTextureUniqueId ( b3SharedMemoryStatusHandle statusHandle )
2017-06-30 20:35:07 +00:00
{
int uid = - 1 ;
const SharedMemoryStatus * status = ( const SharedMemoryStatus * ) statusHandle ;
2017-10-10 18:10:42 +00:00
if ( status )
2017-06-30 20:35:07 +00:00
{
2017-10-10 18:10:42 +00:00
btAssert ( status - > m_type = = CMD_LOAD_TEXTURE_COMPLETED ) ;
if ( status - > m_type = = CMD_LOAD_TEXTURE_COMPLETED )
{
uid = status - > m_loadTextureResultArguments . m_textureUniqueId ;
}
2017-06-30 20:35:07 +00:00
}
return uid ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3InitUpdateVisualShape ( b3PhysicsClientHandle physClient , int bodyUniqueId , int jointIndex , int shapeIndex , int textureUniqueId )
2016-10-21 06:40:30 +00:00
{
2016-10-20 17:56:44 +00:00
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
b3Assert ( cl ) ;
b3Assert ( cl - > canSubmitCommand ( ) ) ;
struct SharedMemoryCommand * command = cl - > getAvailableSharedMemoryCommand ( ) ;
b3Assert ( command ) ;
command - > m_type = CMD_UPDATE_VISUAL_SHAPE ;
2016-10-21 06:40:30 +00:00
command - > m_updateVisualShapeDataArguments . m_bodyUniqueId = bodyUniqueId ;
2016-10-21 18:55:27 +00:00
command - > m_updateVisualShapeDataArguments . m_jointIndex = jointIndex ;
command - > m_updateVisualShapeDataArguments . m_shapeIndex = shapeIndex ;
2016-10-21 06:40:30 +00:00
command - > m_updateVisualShapeDataArguments . m_textureUniqueId = textureUniqueId ;
2016-10-20 17:56:44 +00:00
command - > m_updateFlags = 0 ;
2017-05-13 16:18:36 +00:00
if ( textureUniqueId > = 0 )
{
command - > m_updateFlags | = CMD_UPDATE_VISUAL_SHAPE_TEXTURE ;
}
2016-10-20 17:56:44 +00:00
return ( b3SharedMemoryCommandHandle ) command ;
}
2016-10-19 05:05:28 +00:00
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3UpdateVisualShapeRGBAColor ( b3SharedMemoryCommandHandle commandHandle , double rgbaColor [ 4 ] )
2017-05-13 16:18:36 +00:00
{
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 ;
}
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3UpdateVisualShapeSpecularColor ( b3SharedMemoryCommandHandle commandHandle , double specularColor [ 3 ] )
2017-06-01 19:32:44 +00:00
{
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 ;
}
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit ( b3PhysicsClientHandle physClient )
2016-06-27 01:18:30 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3ApplyExternalForce ( b3SharedMemoryCommandHandle commandHandle , int bodyUniqueId , int linkId , const double force [ 3 ] , const double position [ 3 ] , int flag )
2016-06-27 01:18:30 +00:00
{
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 + + ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3ApplyExternalTorque ( b3SharedMemoryCommandHandle commandHandle , int bodyUniqueId , int linkId , const double torque [ 3 ] , int flag )
2016-06-27 01:18:30 +00:00
{
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 + + ;
}
2016-08-10 01:40:12 +00:00
///compute the forces to achieve an acceleration, given a state q and qdot using inverse dynamics
2018-05-31 23:06:15 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit ( b3PhysicsClientHandle physClient , int bodyUniqueId ,
2016-08-10 01:40:12 +00:00
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 ;
2018-05-31 23:06:15 +00:00
command - > m_calculateInverseDynamicsArguments . m_bodyUniqueId = bodyUniqueId ;
int numJoints = cl - > getNumJoints ( bodyUniqueId ) ;
2016-08-10 01:40:12 +00:00
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3GetStatusInverseDynamicsJointForces ( b3SharedMemoryStatusHandle statusHandle ,
2016-08-10 01:40:12 +00:00
int * bodyUniqueId ,
int * dofCount ,
double * jointForces )
{
const SharedMemoryStatus * status = ( const SharedMemoryStatus * ) statusHandle ;
2017-10-10 18:10:42 +00:00
if ( status = = 0 )
return false ;
2016-08-10 01:40:12 +00:00
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 ;
2016-09-07 23:00:38 +00:00
}
2018-05-31 23:06:15 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit ( b3PhysicsClientHandle physClient , int bodyUniqueId , int linkIndex , const double * localPosition , const double * jointPositionsQ , const double * jointVelocitiesQdot , const double * jointAccelerations )
2016-09-07 23:00:38 +00:00
{
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 ;
2018-05-31 23:06:15 +00:00
command - > m_calculateJacobianArguments . m_bodyUniqueId = bodyUniqueId ;
2016-09-07 23:00:38 +00:00
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 ] ;
2018-05-31 23:06:15 +00:00
int numJoints = cl - > getNumJoints ( bodyUniqueId ) ;
2016-09-07 23:00:38 +00:00
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 ;
}
2016-11-14 15:39:34 +00:00
2017-09-24 02:58:59 +00:00
B3_SHARED_API int b3GetStatusJacobian ( b3SharedMemoryStatusHandle statusHandle , int * dofCount , double * linearJacobian , double * angularJacobian )
2016-09-07 23:00:38 +00:00
{
const SharedMemoryStatus * status = ( const SharedMemoryStatus * ) statusHandle ;
2017-10-10 18:10:42 +00:00
if ( status = = 0 )
return false ;
2016-09-07 23:00:38 +00:00
btAssert ( status - > m_type = = CMD_CALCULATED_JACOBIAN_COMPLETED ) ;
if ( status - > m_type ! = CMD_CALCULATED_JACOBIAN_COMPLETED )
return false ;
2017-09-24 02:58:59 +00:00
if ( dofCount )
{
* dofCount = status - > m_jacobianResultArgs . m_dofCount ;
}
2016-09-07 23:00:38 +00:00
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 ;
2016-09-11 11:11:51 +00:00
}
2016-09-08 22:15:58 +00:00
2018-05-31 23:06:15 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateMassMatrixCommandInit ( b3PhysicsClientHandle physClient , int bodyUniqueId , const double * jointPositionsQ )
2017-09-30 20:39:11 +00:00
{
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 ;
2018-05-31 23:06:15 +00:00
int numJoints = cl - > getNumJoints ( bodyUniqueId ) ;
2017-09-30 20:39:11 +00:00
for ( int i = 0 ; i < numJoints ; i + + )
{
command - > m_calculateMassMatrixArguments . m_jointPositionsQ [ i ] = jointPositionsQ [ i ] ;
}
return ( b3SharedMemoryCommandHandle ) command ;
}
2017-10-05 15:23:10 +00:00
B3_SHARED_API int b3GetStatusMassMatrix ( b3PhysicsClientHandle physClient , b3SharedMemoryStatusHandle statusHandle , int * dofCount , double * massMatrix )
2017-09-30 20:39:11 +00:00
{
2017-10-05 15:23:10 +00:00
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
b3Assert ( cl ) ;
2017-09-30 20:39:11 +00:00
const SharedMemoryStatus * status = ( const SharedMemoryStatus * ) statusHandle ;
2017-10-10 18:10:42 +00:00
if ( status = = 0 )
return false ;
2017-09-30 20:39:11 +00:00
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 )
{
2017-10-05 15:23:10 +00:00
cl - > getCachedMassMatrix ( status - > m_massMatrixResultArgs . m_dofCount , massMatrix ) ;
2017-09-30 20:39:11 +00:00
}
return true ;
}
2018-09-13 02:30:49 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3CollisionFilterCommandInit ( b3PhysicsClientHandle physClient )
{
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
b3Assert ( cl ) ;
b3Assert ( cl - > canSubmitCommand ( ) ) ;
struct SharedMemoryCommand * command = cl - > getAvailableSharedMemoryCommand ( ) ;
b3Assert ( command ) ;
command - > m_type = CMD_COLLISION_FILTER ;
command - > m_collisionFilterArgs . m_bodyUniqueIdA = - 1 ;
command - > m_collisionFilterArgs . m_bodyUniqueIdB = - 1 ;
command - > m_collisionFilterArgs . m_linkIndexA = - 2 ;
command - > m_collisionFilterArgs . m_linkIndexB = - 2 ;
command - > m_collisionFilterArgs . m_enableCollision = 0 ;
command - > m_updateFlags = 0 ;
return ( b3SharedMemoryCommandHandle ) command ;
}
B3_SHARED_API void b3SetCollisionFilterPair ( b3SharedMemoryCommandHandle commandHandle , int bodyUniqueIdA ,
int bodyUniqueIdB , int linkIndexA , int linkIndexB , int enableCollision )
{
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
b3Assert ( command ) ;
b3Assert ( command - > m_type = = CMD_COLLISION_FILTER ) ;
command - > m_updateFlags = B3_COLLISION_FILTER_PAIR ;
command - > m_collisionFilterArgs . m_bodyUniqueIdA = bodyUniqueIdA ;
command - > m_collisionFilterArgs . m_bodyUniqueIdB = bodyUniqueIdB ;
command - > m_collisionFilterArgs . m_linkIndexA = linkIndexA ;
command - > m_collisionFilterArgs . m_linkIndexB = linkIndexB ;
command - > m_collisionFilterArgs . m_enableCollision = enableCollision ;
}
B3_SHARED_API void b3SetCollisionFilterGroupMask ( b3SharedMemoryCommandHandle commandHandle , int bodyUniqueIdA ,
int linkIndexA , int collisionFilterGroup , int collisionFilterMask )
{
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
b3Assert ( command ) ;
b3Assert ( command - > m_type = = CMD_COLLISION_FILTER ) ;
command - > m_updateFlags = B3_COLLISION_FILTER_GROUP_MASK ;
command - > m_collisionFilterArgs . m_bodyUniqueIdA = bodyUniqueIdA ;
command - > m_collisionFilterArgs . m_linkIndexA = linkIndexA ;
command - > m_collisionFilterArgs . m_collisionFilterGroup = collisionFilterGroup ;
command - > m_collisionFilterArgs . m_collisionFilterMask = collisionFilterMask ;
}
2016-09-08 22:15:58 +00:00
///compute the joint positions to move the end effector to a desired target using inverse kinematics
2018-05-31 23:06:15 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit ( b3PhysicsClientHandle physClient , int bodyUniqueId )
2016-09-08 22:15:58 +00:00
{
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 ;
2018-05-31 23:06:15 +00:00
command - > m_calculateInverseKinematicsArguments . m_bodyUniqueId = bodyUniqueId ;
2016-09-22 20:27:09 +00:00
return ( b3SharedMemoryCommandHandle ) command ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3CalculateInverseKinematicsAddTargetPurePosition ( b3SharedMemoryCommandHandle commandHandle , int endEffectorLinkIndex , const double targetPosition [ 3 ] )
2016-09-22 20:27:09 +00:00
{
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 ] ;
2017-10-25 04:06:44 +00:00
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 ;
2016-09-22 20:27:09 +00:00
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3CalculateInverseKinematicsAddTargetPositionWithOrientation ( b3SharedMemoryCommandHandle commandHandle , int endEffectorLinkIndex , const double targetPosition [ 3 ] , const double targetOrientation [ 4 ] )
2016-09-22 20:27:09 +00:00
{
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 ;
2016-09-08 22:15:58 +00:00
command - > m_calculateInverseKinematicsArguments . m_targetPosition [ 0 ] = targetPosition [ 0 ] ;
command - > m_calculateInverseKinematicsArguments . m_targetPosition [ 1 ] = targetPosition [ 1 ] ;
command - > m_calculateInverseKinematicsArguments . m_targetPosition [ 2 ] = targetPosition [ 2 ] ;
2016-09-20 00:04:05 +00:00
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 ] ;
2016-09-08 22:15:58 +00:00
}
2017-09-21 23:40:19 +00:00
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 )
2016-09-30 07:05:00 +00:00
{
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 ] ;
2016-09-30 08:03:40 +00:00
command - > m_calculateInverseKinematicsArguments . m_restPose [ i ] = restPose [ i ] ;
2016-09-30 07:05:00 +00:00
}
}
2017-09-21 23:40:19 +00:00
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 )
2016-09-30 07:05:00 +00:00
{
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 ] ;
2016-09-30 08:03:40 +00:00
command - > m_calculateInverseKinematicsArguments . m_restPose [ i ] = restPose [ i ] ;
2016-09-30 07:05:00 +00:00
}
}
2016-09-22 20:27:09 +00:00
2018-05-31 23:06:15 +00:00
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3CalculateInverseKinematicsSetJointDamping ( b3SharedMemoryCommandHandle commandHandle , int numDof , const double * jointDampingCoeff )
2017-02-03 19:08:44 +00:00
{
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 ] ;
}
}
2017-10-19 21:00:53 +00:00
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3GetStatusInverseKinematicsJointPositions ( b3SharedMemoryStatusHandle statusHandle ,
2016-09-08 22:15:58 +00:00
int * bodyUniqueId ,
int * dofCount ,
double * jointPositions )
{
const SharedMemoryStatus * status = ( const SharedMemoryStatus * ) statusHandle ;
2017-10-10 18:10:42 +00:00
btAssert ( status ) ;
if ( status = = 0 )
return 0 ;
2016-09-08 22:15:58 +00:00
btAssert ( status - > m_type = = CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED ) ;
if ( status - > m_type ! = CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED )
2017-10-10 18:10:42 +00:00
return 0 ;
2016-09-08 22:15:58 +00:00
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 ] ;
}
}
2017-10-10 18:10:42 +00:00
return 1 ;
2016-11-04 20:15:10 +00:00
}
2016-12-27 03:40:09 +00:00
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3RequestVREventsCommandInit ( b3PhysicsClientHandle physClient )
2016-12-27 03:40:09 +00:00
{
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
b3Assert ( cl ) ;
b3Assert ( cl - > canSubmitCommand ( ) ) ;
struct SharedMemoryCommand * command = cl - > getAvailableSharedMemoryCommand ( ) ;
b3Assert ( command ) ;
command - > m_type = CMD_REQUEST_VR_EVENTS_DATA ;
2017-04-08 05:53:36 +00:00
command - > m_updateFlags = VR_DEVICE_CONTROLLER ;
2016-12-27 03:40:09 +00:00
return ( b3SharedMemoryCommandHandle ) command ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3VREventsSetDeviceTypeFilter ( b3SharedMemoryCommandHandle commandHandle , int deviceTypeFilter )
2017-04-08 05:53:36 +00:00
{
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
b3Assert ( command ) ;
if ( command - > m_type = = CMD_REQUEST_VR_EVENTS_DATA )
{
command - > m_updateFlags = deviceTypeFilter ;
}
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3GetVREventsData ( b3PhysicsClientHandle physClient , struct b3VREventsData * vrEventsData )
2016-12-27 03:40:09 +00:00
{
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
if ( cl )
{
cl - > getCachedVREvents ( vrEventsData ) ;
}
}
2017-01-06 01:41:58 +00:00
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3SetVRCameraStateCommandInit ( b3PhysicsClientHandle physClient )
2017-01-06 01:41:58 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3SetVRCameraRootPosition ( b3SharedMemoryCommandHandle commandHandle , double rootPos [ 3 ] )
2017-01-06 01:41:58 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3SetVRCameraRootOrientation ( b3SharedMemoryCommandHandle commandHandle , double rootOrn [ 4 ] )
2017-01-06 01:41:58 +00:00
{
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 ] ;
2017-05-17 23:29:30 +00:00
command - > m_vrCameraStateArguments . m_rootOrientation [ 3 ] = rootOrn [ 3 ] ;
2017-01-06 01:41:58 +00:00
return 0 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3SetVRCameraTrackingObject ( b3SharedMemoryCommandHandle commandHandle , int objectUniqueId )
2017-01-06 01:41:58 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3SetVRCameraTrackingObjectFlag ( b3SharedMemoryCommandHandle commandHandle , int flag )
2017-05-18 02:29:12 +00:00
{
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 ;
}
2017-03-02 20:33:22 +00:00
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3RequestKeyboardEventsCommandInit ( b3PhysicsClientHandle physClient )
2017-03-02 20:33:22 +00:00
{
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
b3Assert ( cl ) ;
b3Assert ( cl - > canSubmitCommand ( ) ) ;
struct SharedMemoryCommand * command = cl - > getAvailableSharedMemoryCommand ( ) ;
b3Assert ( command ) ;
2018-09-04 06:13:15 +00:00
return b3RequestKeyboardEventsCommandInit2 ( ( b3SharedMemoryCommandHandle ) command ) ;
}
2017-03-02 20:33:22 +00:00
2018-09-04 06:13:15 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3RequestKeyboardEventsCommandInit2 ( b3SharedMemoryCommandHandle commandHandle )
{
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
2017-03-02 20:33:22 +00:00
command - > m_type = CMD_REQUEST_KEYBOARD_EVENTS_DATA ;
command - > m_updateFlags = 0 ;
return ( b3SharedMemoryCommandHandle ) command ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3GetKeyboardEventsData ( b3PhysicsClientHandle physClient , struct b3KeyboardEventsData * keyboardEventsData )
2017-03-02 20:33:22 +00:00
{
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
if ( cl )
{
cl - > getCachedKeyboardEvents ( keyboardEventsData ) ;
}
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3RequestMouseEventsCommandInit ( b3PhysicsClientHandle physClient )
2017-06-17 20:29:14 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3GetMouseEventsData ( b3PhysicsClientHandle physClient , struct b3MouseEventsData * mouseEventsData )
2017-06-17 20:29:14 +00:00
{
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
if ( cl )
{
cl - > getCachedMouseEvents ( mouseEventsData ) ;
}
}
2017-03-02 20:33:22 +00:00
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3ProfileTimingCommandInit ( b3PhysicsClientHandle physClient , const char * name )
2017-05-05 00:51:40 +00:00
{
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 ;
}
2018-06-16 13:19:49 +00:00
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 ( ) ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3SetProfileTimingDuractionInMicroSeconds ( b3SharedMemoryCommandHandle commandHandle , int duration )
2017-05-05 00:51:40 +00:00
{
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 ;
}
}
2017-03-02 20:33:22 +00:00
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3StateLoggingCommandInit ( b3PhysicsClientHandle physClient )
2017-02-17 18:47:55 +00:00
{
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
b3Assert ( cl ) ;
b3Assert ( cl - > canSubmitCommand ( ) ) ;
struct SharedMemoryCommand * command = cl - > getAvailableSharedMemoryCommand ( ) ;
b3Assert ( command ) ;
2017-02-17 22:25:53 +00:00
command - > m_type = CMD_STATE_LOGGING ;
2017-02-17 18:47:55 +00:00
command - > m_updateFlags = 0 ;
2017-02-17 22:25:53 +00:00
command - > m_stateLoggingArguments . m_numBodyUniqueIds = 0 ;
2017-04-08 18:48:12 +00:00
command - > m_stateLoggingArguments . m_deviceFilterType = VR_DEVICE_CONTROLLER ;
2017-02-17 18:47:55 +00:00
return ( b3SharedMemoryCommandHandle ) command ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3StateLoggingStart ( b3SharedMemoryCommandHandle commandHandle , int loggingType , const char * fileName )
2017-02-17 18:47:55 +00:00
{
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
b3Assert ( command ) ;
2017-02-17 22:25:53 +00:00
b3Assert ( command - > m_type = = CMD_STATE_LOGGING ) ;
if ( command - > m_type = = CMD_STATE_LOGGING )
2017-02-17 18:47:55 +00:00
{
2017-02-17 22:25:53 +00:00
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 ;
2017-02-17 18:47:55 +00:00
}
return 0 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3GetStatusLoggingUniqueId ( b3SharedMemoryStatusHandle statusHandle )
2017-02-17 22:25:53 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3StateLoggingAddLoggingObjectUniqueId ( b3SharedMemoryCommandHandle commandHandle , int objectUniqueId )
2017-02-17 22:25:53 +00:00
{
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 ;
}
2017-03-04 23:30:57 +00:00
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3StateLoggingSetLinkIndexA ( b3SharedMemoryCommandHandle commandHandle , int linkIndexA )
2017-04-02 22:45:48 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3StateLoggingSetLinkIndexB ( b3SharedMemoryCommandHandle commandHandle , int linkIndexB )
2017-04-02 22:45:48 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3StateLoggingSetBodyAUniqueId ( b3SharedMemoryCommandHandle commandHandle , int bodyAUniqueId )
2017-04-02 22:45:48 +00:00
{
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
b3Assert ( command ) ;
b3Assert ( command - > m_type = CMD_STATE_LOGGING ) ;
if ( command - > m_type = = CMD_STATE_LOGGING )
{
2017-04-04 17:38:25 +00:00
command - > m_updateFlags | = STATE_LOGGING_FILTER_BODY_UNIQUE_ID_A ;
command - > m_stateLoggingArguments . m_bodyUniqueIdA = bodyAUniqueId ;
2017-04-02 22:45:48 +00:00
}
return 0 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3StateLoggingSetBodyBUniqueId ( b3SharedMemoryCommandHandle commandHandle , int bodyBUniqueId )
2017-04-02 22:45:48 +00:00
{
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
b3Assert ( command ) ;
b3Assert ( command - > m_type = CMD_STATE_LOGGING ) ;
if ( command - > m_type = = CMD_STATE_LOGGING )
{
2017-04-04 17:38:25 +00:00
command - > m_updateFlags | = STATE_LOGGING_FILTER_BODY_UNIQUE_ID_B ;
command - > m_stateLoggingArguments . m_bodyUniqueIdB = bodyBUniqueId ;
2017-04-02 22:45:48 +00:00
}
return 0 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3StateLoggingSetMaxLogDof ( b3SharedMemoryCommandHandle commandHandle , int maxLogDof )
2017-03-04 23:30:57 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3StateLoggingSetLogFlags ( b3SharedMemoryCommandHandle commandHandle , int logFlags )
2017-08-31 02:41:15 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3StateLoggingSetDeviceTypeFilter ( b3SharedMemoryCommandHandle commandHandle , int deviceTypeFilter )
2017-04-08 18:48:12 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3StateLoggingStop ( b3SharedMemoryCommandHandle commandHandle , int loggingUid )
2017-02-17 18:47:55 +00:00
{
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
b3Assert ( command ) ;
2017-02-17 22:25:53 +00:00
b3Assert ( command - > m_type = = CMD_STATE_LOGGING ) ;
if ( command - > m_type = = CMD_STATE_LOGGING )
2017-02-17 18:47:55 +00:00
{
2017-02-17 22:25:53 +00:00
command - > m_updateFlags | = STATE_LOGGING_STOP_LOG ;
command - > m_stateLoggingArguments . m_loggingUniqueId = loggingUid ;
2017-02-17 18:47:55 +00:00
}
return 0 ;
}
2017-02-22 01:36:54 +00:00
///configure the 3D OpenGL debug visualizer (enable/disable GUI widgets, shadows, position camera etc)
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3InitConfigureOpenGLVisualizer ( b3PhysicsClientHandle physClient )
2017-02-22 01:36:54 +00:00
{
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
b3Assert ( cl ) ;
b3Assert ( cl - > canSubmitCommand ( ) ) ;
struct SharedMemoryCommand * command = cl - > getAvailableSharedMemoryCommand ( ) ;
b3Assert ( command ) ;
2018-09-04 06:13:15 +00:00
return b3InitConfigureOpenGLVisualizer2 ( ( b3SharedMemoryCommandHandle ) command ) ;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitConfigureOpenGLVisualizer2 ( b3SharedMemoryCommandHandle commandHandle )
{
struct SharedMemoryCommand * command = ( struct SharedMemoryCommand * ) commandHandle ;
command - > m_type = CMD_CONFIGURE_OPENGL_VISUALIZER ;
command - > m_updateFlags = 0 ;
return ( b3SharedMemoryCommandHandle ) command ;
2017-02-22 01:36:54 +00:00
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3ConfigureOpenGLVisualizerSetVisualizationFlags ( b3SharedMemoryCommandHandle commandHandle , int flag , int enabled )
2017-02-22 01:36:54 +00:00
{
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 ;
}
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3ConfigureOpenGLVisualizerSetViewMatrix ( b3SharedMemoryCommandHandle commandHandle , float cameraDistance , float cameraPitch , float cameraYaw , const float cameraTargetPosition [ 3 ] )
2017-02-22 01:36:54 +00:00
{
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 ] ;
}
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestOpenGLVisualizerCameraCommand ( b3PhysicsClientHandle physClient )
2017-04-10 18:03:41 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API int b3GetStatusOpenGLVisualizerCamera ( b3SharedMemoryStatusHandle statusHandle , b3OpenGLVisualizerCameraInfo * camera )
2017-04-10 18:03:41 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3SetTimeOut ( b3PhysicsClientHandle physClient , double timeOutInSeconds )
2017-02-24 23:34:11 +00:00
{
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
b3Assert ( cl ) ;
if ( cl )
{
cl - > setTimeOut ( timeOutInSeconds ) ;
}
}
2017-08-28 02:34:00 +00:00
2017-09-21 23:40:19 +00:00
B3_SHARED_API double b3GetTimeOut ( b3PhysicsClientHandle physClient )
2017-02-24 23:34:11 +00:00
{
PhysicsClient * cl = ( PhysicsClient * ) physClient ;
b3Assert ( cl ) ;
if ( cl )
{
return cl - > getTimeOut ( ) ;
}
return - 1 ;
}
2017-05-30 04:55:58 +00:00
2018-05-31 23:06:15 +00:00
B3_SHARED_API b3SharedMemoryCommandHandle b3SetAdditionalSearchPath ( b3PhysicsClientHandle physClient , const char * path )
2017-08-28 02:34:00 +00:00
{
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 ;
}
2017-09-21 23:40:19 +00:00
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 ] )
2017-05-30 04:55:58 +00:00
{
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 ] ;
}
2017-09-21 23:40:19 +00:00
B3_SHARED_API void b3InvertTransform ( const double pos [ 3 ] , const double orn [ 4 ] , double outPos [ 3 ] , double outOrn [ 4 ] )
2017-05-30 04:55:58 +00:00
{
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 ] ;
}