text: #include "b3RobotSimulatorClientAPI_NoDirect.h"
  
  #include "PhysicsClientC_API.h"
  #include "b3RobotSimulatorClientAPI_InternalData.h"
  
  #include "SharedMemoryPublic.h"
  #include "Bullet3Common/b3Logging.h"
  
  static void scalarToDouble3(btScalar a[3], double b[3])
  {
  	for (int i = 0; i < 3; i++)
  	{
  		b[i] = a[i];
  	}
  }
  
  static void scalarToDouble4(btScalar a[4], double b[4])
  {
  	for (int i = 0; i < 4; i++)
  	{
  		b[i] = a[i];
  	}
  }
  
  b3RobotSimulatorClientAPI_NoDirect::b3RobotSimulatorClientAPI_NoDirect()
  {
  	m_data = new b3RobotSimulatorClientAPI_InternalData();
  }
  
  b3RobotSimulatorClientAPI_NoDirect::~b3RobotSimulatorClientAPI_NoDirect()
  {
  	delete m_data;
  }
  
  bool b3RobotSimulatorClientAPI_NoDirect::isConnected() const
  {
  	return (m_data->m_physicsClientHandle != 0);
  }
  
  void b3RobotSimulatorClientAPI_NoDirect::setTimeOut(double timeOutInSec)
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return;
  	}
  	b3SetTimeOut(m_data->m_physicsClientHandle, timeOutInSec);
  }
  
  void b3RobotSimulatorClientAPI_NoDirect::disconnect()
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return;
  	}
  #ifndef BT_DISABLE_PHYSICS_DIRECT
  	b3DisconnectSharedMemory(m_data->m_physicsClientHandle);
  	m_data->m_physicsClientHandle = 0;
  #endif
  }
  
  void b3RobotSimulatorClientAPI_NoDirect::syncBodies()
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return;
  	}
  
  	b3SharedMemoryCommandHandle command;
  	b3SharedMemoryStatusHandle statusHandle;
  	int statusType;
  
  	command = b3InitSyncBodyInfoCommand(m_data->m_physicsClientHandle);
  	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
  	statusType = b3GetStatusType(statusHandle);
  }
  
  void b3RobotSimulatorClientAPI_NoDirect::resetSimulation()
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return;
  	}
  	b3SharedMemoryStatusHandle statusHandle;
  	statusHandle = b3SubmitClientCommandAndWaitStatus(
  		m_data->m_physicsClientHandle, b3InitResetSimulationCommand(m_data->m_physicsClientHandle));
  }
  
  void b3RobotSimulatorClientAPI_NoDirect::resetSimulation(int flag)
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return;
  	}
  	b3SharedMemoryStatusHandle statusHandle;
  	b3SharedMemoryCommandHandle command = b3InitResetSimulationCommand(m_data->m_physicsClientHandle);
  	b3InitResetSimulationSetFlags(command, flag);
  	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
  }
  
  bool b3RobotSimulatorClientAPI_NoDirect::canSubmitCommand() const
  {
  	if (!isConnected())
  	{
  		return false;
  	}
  	return (b3CanSubmitCommand(m_data->m_physicsClientHandle) != 0);
  }
  
  void b3RobotSimulatorClientAPI_NoDirect::stepSimulation()
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return;
  	}
  
  	b3SharedMemoryStatusHandle statusHandle;
  	int statusType;
  	if (b3CanSubmitCommand(m_data->m_physicsClientHandle))
  	{
  		statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitStepSimulationCommand(m_data->m_physicsClientHandle));
  		statusType = b3GetStatusType(statusHandle);
  		//btAssert(statusType == CMD_STEP_FORWARD_SIMULATION_COMPLETED);
  	}
  }
  
  void b3RobotSimulatorClientAPI_NoDirect::setGravity(const btVector3& gravityAcceleration)
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return;
  	}
  	btAssert(b3CanSubmitCommand(m_data->m_physicsClientHandle));
  
  	b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
  	b3SharedMemoryStatusHandle statusHandle;
  	b3PhysicsParamSetGravity(command, gravityAcceleration[0], gravityAcceleration[1], gravityAcceleration[2]);
  	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
  	//	btAssert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
  }
  
  btQuaternion b3RobotSimulatorClientAPI_NoDirect::getQuaternionFromEuler(const btVector3& rollPitchYaw)
  {
  	btQuaternion q;
  	q.setEulerZYX(rollPitchYaw[2], rollPitchYaw[1], rollPitchYaw[0]);
  	return q;
  }
  
  btVector3 b3RobotSimulatorClientAPI_NoDirect::getEulerFromQuaternion(const btQuaternion& quat)
  {
  	btScalar roll, pitch, yaw;
  	quat.getEulerZYX(yaw, pitch, roll);
  	btVector3 rpy2 = btVector3(roll, pitch, yaw);
  	return rpy2;
  }
  
  int b3RobotSimulatorClientAPI_NoDirect::loadTexture(const std::string& fileName)
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return -1;
  	}
  	btAssert(b3CanSubmitCommand(m_data->m_physicsClientHandle));
  
  	b3SharedMemoryCommandHandle commandHandle;
  	b3SharedMemoryStatusHandle statusHandle;
  	int statusType;
  
  	{
  		commandHandle = b3InitLoadTexture(m_data->m_physicsClientHandle, fileName.c_str());
  
  		statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
  		statusType = b3GetStatusType(statusHandle);
  		if (statusType == CMD_LOAD_TEXTURE_COMPLETED)
  		{
  			return b3GetStatusTextureUniqueId(statusHandle);
  		}
  	}
  	return -1;
  }
  
  bool b3RobotSimulatorClientAPI_NoDirect::changeVisualShape(const struct b3RobotSimulatorChangeVisualShapeArgs& args)
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return false;
  	}
  
  	int objectUniqueId = args.m_objectUniqueId;
  	int jointIndex = args.m_linkIndex;
  	int shapeIndex = args.m_shapeIndex;
  	int textureUniqueId = args.m_textureUniqueId;
  
  	b3SharedMemoryCommandHandle commandHandle;
  	b3SharedMemoryStatusHandle statusHandle;
  	int statusType;
  
  	commandHandle = b3InitUpdateVisualShape2(m_data->m_physicsClientHandle, objectUniqueId, jointIndex, shapeIndex);
  
  	if (textureUniqueId>=-1)
  	{
  		b3UpdateVisualShapeTexture(commandHandle, textureUniqueId);
  	}
  
  	if (args.m_hasSpecularColor)
  	{
  		double specularColor[3] = {args.m_specularColor[0], args.m_specularColor[1], args.m_specularColor[2]};
  		b3UpdateVisualShapeSpecularColor(commandHandle, specularColor);
  	}
  	if (args.m_hasRgbaColor)
  	{
  		double rgbaColor[4] = {args.m_rgbaColor[0], args.m_rgbaColor[1], args.m_rgbaColor[2], args.m_rgbaColor[3]};
  		b3UpdateVisualShapeRGBAColor(commandHandle, rgbaColor);
  	}
  
  	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
  	statusType = b3GetStatusType(statusHandle);
  
  	return (statusType == CMD_VISUAL_SHAPE_UPDATE_COMPLETED);
  }
  
  int b3RobotSimulatorClientAPI_NoDirect::loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args)
  {
  	int robotUniqueId = -1;
  
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return robotUniqueId;
  	}
  
  	b3SharedMemoryStatusHandle statusHandle;
  	int statusType;
  	b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
  
  	//setting the initial position, orientation and other arguments are optional
  
  	b3LoadUrdfCommandSetFlags(command, args.m_flags);
  
  	b3LoadUrdfCommandSetStartPosition(command, args.m_startPosition[0],
  									  args.m_startPosition[1],
  									  args.m_startPosition[2]);
  	b3LoadUrdfCommandSetStartOrientation(command, args.m_startOrientation[0], args.m_startOrientation[1], args.m_startOrientation[2], args.m_startOrientation[3]);
  	if (args.m_forceOverrideFixedBase)
  	{
  		b3LoadUrdfCommandSetUseFixedBase(command, true);
  	}
  	b3LoadUrdfCommandSetUseMultiBody(command, args.m_useMultiBody);
  	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
  	statusType = b3GetStatusType(statusHandle);
  
  	btAssert(statusType == CMD_URDF_LOADING_COMPLETED);
  	if (statusType == CMD_URDF_LOADING_COMPLETED)
  	{
  		robotUniqueId = b3GetStatusBodyIndex(statusHandle);
  	}
  	return robotUniqueId;
  }
  
  bool b3RobotSimulatorClientAPI_NoDirect::loadMJCF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results)
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return false;
  	}
  
  	b3SharedMemoryStatusHandle statusHandle;
  	int statusType;
  	b3SharedMemoryCommandHandle command;
  
  	command = b3LoadMJCFCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
  	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
  	statusType = b3GetStatusType(statusHandle);
  	if (statusType != CMD_MJCF_LOADING_COMPLETED)
  	{
  		b3Warning("Couldn't load .mjcf file.");
  		return false;
  	}
  	int numBodies = b3GetStatusBodyIndices(statusHandle, 0, 0);
  	if (numBodies)
  	{
  		results.m_uniqueObjectIds.resize(numBodies);
  		int numBodies;
  		numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0], results.m_uniqueObjectIds.size());
  	}
  
  	return true;
  }
  
  bool b3RobotSimulatorClientAPI_NoDirect::savePythonWorld(const std::string& fileName)
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return false;
  	}
  
  	b3SharedMemoryStatusHandle statusHandle;
  	int statusType;
  	b3SharedMemoryCommandHandle command;
  	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
  
  	if (fileName.length() == 0)
  	{
  		return false;
  	}
  
  	command = b3SaveWorldCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
  	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
  	statusType = b3GetStatusType(statusHandle);
  	if (statusType != CMD_SAVE_WORLD_COMPLETED)
  	{
  		return false;
  	}
  	return true;
  }
  
  bool b3RobotSimulatorClientAPI_NoDirect::saveBullet(const std::string& fileName)
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return false;
  	}
  
  	b3SharedMemoryStatusHandle statusHandle;
  	int statusType;
  	b3SharedMemoryCommandHandle command;
  	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
  
  	if (fileName.length() == 0)
  	{
  		return false;
  	}
  
  	command = b3SaveBulletCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
  	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
  	statusType = b3GetStatusType(statusHandle);
  	if (statusType != CMD_BULLET_SAVING_COMPLETED)
  	{
  		return false;
  	}
  	return true;
  }
  
  bool b3RobotSimulatorClientAPI_NoDirect::loadBullet(const std::string& fileName, b3RobotSimulatorLoadFileResults& results)
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return false;
  	}
  
  	b3SharedMemoryStatusHandle statusHandle;
  	int statusType;
  	b3SharedMemoryCommandHandle command;
  
  	command = b3LoadBulletCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
  	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
  	statusType = b3GetStatusType(statusHandle);
  	if (statusType != CMD_BULLET_LOADING_COMPLETED)
  	{
  		return false;
  	}
  	int numBodies = b3GetStatusBodyIndices(statusHandle, 0, 0);
  	if (numBodies)
  	{
  		results.m_uniqueObjectIds.resize(numBodies);
  		int numBodies;
  		numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0], results.m_uniqueObjectIds.size());
  	}
  
  	return true;
  }
  
  bool b3RobotSimulatorClientAPI_NoDirect::loadSDF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results, const struct b3RobotSimulatorLoadSdfFileArgs& args)
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return false;
  	}
  
  	bool statusOk = false;
  
  	b3SharedMemoryStatusHandle statusHandle;
  	int statusType;
  	b3SharedMemoryCommandHandle command = b3LoadSdfCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
  	b3LoadSdfCommandSetUseMultiBody(command, args.m_useMultiBody);
  	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
  	statusType = b3GetStatusType(statusHandle);
  	btAssert(statusType == CMD_SDF_LOADING_COMPLETED);
  	if (statusType == CMD_SDF_LOADING_COMPLETED)
  	{
  		int numBodies = b3GetStatusBodyIndices(statusHandle, 0, 0);
  		if (numBodies)
  		{
  			results.m_uniqueObjectIds.resize(numBodies);
  			int numBodies;
  			numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0], results.m_uniqueObjectIds.size());
  		}
  		statusOk = true;
  	}
  
  	return statusOk;
  }
  
  bool b3RobotSimulatorClientAPI_NoDirect::getBodyInfo(int bodyUniqueId, struct b3BodyInfo* bodyInfo)
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return false;
  	}
  
  	int result = b3GetBodyInfo(m_data->m_physicsClientHandle, bodyUniqueId, bodyInfo);
  	return (result != 0);
  }
  
  bool b3RobotSimulatorClientAPI_NoDirect::getBasePositionAndOrientation(int bodyUniqueId, btVector3& basePosition, btQuaternion& baseOrientation) const
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return false;
  	}
  
  	b3SharedMemoryCommandHandle cmd_handle =
  		b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
  	b3SharedMemoryStatusHandle status_handle =
  		b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, cmd_handle);
  
  	const int status_type = b3GetStatusType(status_handle);
  	const double* actualStateQ;
  
  	if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED)
  	{
  		return false;
  	}
  
  	b3GetStatusActualState(
  		status_handle, 0 /* body_unique_id */,
  		0 /* num_degree_of_freedom_q */, 0 /* num_degree_of_freedom_u */,
  		0 /*root_local_inertial_frame*/, &actualStateQ,
  		0 /* actual_state_q_dot */, 0 /* joint_reaction_forces */);
  
  	basePosition[0] = actualStateQ[0];
  	basePosition[1] = actualStateQ[1];
  	basePosition[2] = actualStateQ[2];
  
  	baseOrientation[0] = actualStateQ[3];
  	baseOrientation[1] = actualStateQ[4];
  	baseOrientation[2] = actualStateQ[5];
  	baseOrientation[3] = actualStateQ[6];
  	return true;
  }
  
  bool b3RobotSimulatorClientAPI_NoDirect::resetBasePositionAndOrientation(int bodyUniqueId, btVector3& basePosition, btQuaternion& baseOrientation)
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return false;
  	}
  
  	b3SharedMemoryCommandHandle commandHandle;
  
  	commandHandle = b3CreatePoseCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
  
  	b3CreatePoseCommandSetBasePosition(commandHandle, basePosition[0], basePosition[1], basePosition[2]);
  	b3CreatePoseCommandSetBaseOrientation(commandHandle, baseOrientation[0], baseOrientation[1],
  										  baseOrientation[2], baseOrientation[3]);
  
  	b3SharedMemoryStatusHandle statusHandle;
  	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
  
  	return true;
  }
  
  bool b3RobotSimulatorClientAPI_NoDirect::getBaseVelocity(int bodyUniqueId, btVector3& baseLinearVelocity, btVector3& baseAngularVelocity) const
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return false;
  	}
  
  	b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
  	b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
  
  	const int status_type = b3GetStatusType(statusHandle);
  	const double* actualStateQdot;
  
  	if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED)
  	{
  		return false;
  	}
  
  	b3GetStatusActualState(statusHandle, 0 /* body_unique_id */,
  						   0 /* num_degree_of_freedom_q */, 0 /* num_degree_of_freedom_u */,
  						   0 /*root_local_inertial_frame*/, 0,
  						   &actualStateQdot, 0 /* joint_reaction_forces */);
  
  	baseLinearVelocity[0] = actualStateQdot[0];
  	baseLinearVelocity[1] = actualStateQdot[1];
  	baseLinearVelocity[2] = actualStateQdot[2];
  
  	baseAngularVelocity[0] = actualStateQdot[3];
  	baseAngularVelocity[1] = actualStateQdot[4];
  	baseAngularVelocity[2] = actualStateQdot[5];
  	return true;
  }
  
  bool b3RobotSimulatorClientAPI_NoDirect::resetBaseVelocity(int bodyUniqueId, const btVector3& linearVelocity, const btVector3& angularVelocity) const
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return false;
  	}
  
  	b3SharedMemoryCommandHandle commandHandle;
  	b3SharedMemoryStatusHandle statusHandle;
  
  	commandHandle = b3CreatePoseCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
  
  	btVector3DoubleData linVelDouble;
  	linearVelocity.serializeDouble(linVelDouble);
  	b3CreatePoseCommandSetBaseLinearVelocity(commandHandle, linVelDouble.m_floats);
  
  	btVector3DoubleData angVelDouble;
  	angularVelocity.serializeDouble(angVelDouble);
  	b3CreatePoseCommandSetBaseAngularVelocity(commandHandle, angVelDouble.m_floats);
  
  	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
  	return true;
  }
  
  void b3RobotSimulatorClientAPI_NoDirect::setInternalSimFlags(int flags)
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return;
  	}
  	{
  		b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
  		b3SharedMemoryStatusHandle statusHandle;
  		b3PhysicsParamSetInternalSimFlags(command, flags);
  		statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
  	}
  }
  
  void b3RobotSimulatorClientAPI_NoDirect::setRealTimeSimulation(bool enableRealTimeSimulation)
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return;
  	}
  
  	b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
  	b3SharedMemoryStatusHandle statusHandle;
  
  	b3PhysicsParamSetRealTimeSimulation(command, enableRealTimeSimulation);
  
  	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
  
  	btAssert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
  }
  
  int b3RobotSimulatorClientAPI_NoDirect::getNumJoints(int bodyUniqueId) const
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return false;
  	}
  	return b3GetNumJoints(m_data->m_physicsClientHandle, bodyUniqueId);
  }
  
  bool b3RobotSimulatorClientAPI_NoDirect::getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo)
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return false;
  	}
  	return (b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, jointInfo) != 0);
  }
  
  int b3RobotSimulatorClientAPI_NoDirect::createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo)
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return -1;
  	}
  	b3SharedMemoryStatusHandle statusHandle;
  	btAssert(b3CanSubmitCommand(m_data->m_physicsClientHandle));
  	if (b3CanSubmitCommand(m_data->m_physicsClientHandle))
  	{
  		statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitCreateUserConstraintCommand(m_data->m_physicsClientHandle, parentBodyIndex, parentJointIndex, childBodyIndex, childJointIndex, jointInfo));
  		int statusType = b3GetStatusType(statusHandle);
  		if (statusType == CMD_USER_CONSTRAINT_COMPLETED)
  		{
  			int userConstraintUid = b3GetStatusUserConstraintUniqueId(statusHandle);
  			return userConstraintUid;
  		}
  	}
  	return -1;
  }
  
  int b3RobotSimulatorClientAPI_NoDirect::changeConstraint(int constraintId, b3RobotUserConstraint* jointInfo)
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return -1;
  	}
  	b3SharedMemoryCommandHandle commandHandle = b3InitChangeUserConstraintCommand(m_data->m_physicsClientHandle, constraintId);
  
  	if (jointInfo->m_userUpdateFlags & USER_CONSTRAINT_CHANGE_MAX_FORCE)
  	{
  		b3InitChangeUserConstraintSetMaxForce(commandHandle, jointInfo->m_maxAppliedForce);
  	}
  
  	if (jointInfo->m_userUpdateFlags & USER_CONSTRAINT_CHANGE_GEAR_RATIO)
  	{
  		b3InitChangeUserConstraintSetGearRatio(commandHandle, jointInfo->m_gearRatio);
  	}
  
  	if (jointInfo->m_userUpdateFlags & USER_CONSTRAINT_CHANGE_ERP)
  	{
  		b3InitChangeUserConstraintSetERP(commandHandle, jointInfo->m_erp);
  	}
  	if (jointInfo->m_userUpdateFlags & USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK)
  	{
  		b3InitChangeUserConstraintSetGearAuxLink(commandHandle, jointInfo->m_gearAuxLink);
  	}
  
  	if (jointInfo->m_userUpdateFlags & USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET)
  	{
  		b3InitChangeUserConstraintSetRelativePositionTarget(commandHandle, jointInfo->m_relativePositionTarget);
  	}
  	
  	if (jointInfo->m_userUpdateFlags & USER_CONSTRAINT_CHANGE_PIVOT_IN_B)
  	{
  		b3InitChangeUserConstraintSetPivotInB(commandHandle, &jointInfo->m_childFrame[0]);
  	}
  	if (jointInfo->m_userUpdateFlags & USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B)
  	{
  		b3InitChangeUserConstraintSetFrameInB(commandHandle, &jointInfo->m_childFrame[3]);
  	}
  
  	b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
  	int statusType = b3GetStatusType(statusHandle);
  	return statusType;
  }
  
  void b3RobotSimulatorClientAPI_NoDirect::removeConstraint(int constraintId)
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return;
  	}
  	b3SharedMemoryCommandHandle commandHandle = b3InitRemoveUserConstraintCommand(m_data->m_physicsClientHandle, constraintId);
  	b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
  	b3GetStatusType(statusHandle);
  }
  
  bool b3RobotSimulatorClientAPI_NoDirect::getConstraintInfo(int constraintUniqueId, struct b3UserConstraint& constraintInfo)
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return false;
  	}
  	if (b3GetUserConstraintInfo(m_data->m_physicsClientHandle, constraintUniqueId, &constraintInfo))
  	{
  		return true;
  	}
  	return false;
  }
  
  bool b3RobotSimulatorClientAPI_NoDirect::getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState* state)
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return false;
  	}
  	b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
  	b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
  	int statusType = b3GetStatusType(statusHandle);
  	if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
  	{
  		if (b3GetJointState(m_data->m_physicsClientHandle, statusHandle, jointIndex, state))
  		{
  			return true;
  		}
  	}
  	return false;
  }
  
  bool b3RobotSimulatorClientAPI_NoDirect::getJointStates(int bodyUniqueId, b3JointStates2& state)
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return false;
  	}
  
  	b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
  	b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
  
  	if (statusHandle)
  	{
  		//   double rootInertialFrame[7];
  		const double* rootLocalInertialFrame;
  		const double* actualStateQ;
  		const double* actualStateQdot;
  		const double* jointReactionForces;
  
  		int stat = b3GetStatusActualState(statusHandle,
  										  &state.m_bodyUniqueId,
  										  &state.m_numDegreeOfFreedomQ,
  										  &state.m_numDegreeOfFreedomU,
  										  &rootLocalInertialFrame,
  										  &actualStateQ,
  										  &actualStateQdot,
  										  &jointReactionForces);
  		if (stat)
  		{
  			state.m_actualStateQ.resize(state.m_numDegreeOfFreedomQ);
  			state.m_actualStateQdot.resize(state.m_numDegreeOfFreedomU);
  
  			for (int i = 0; i < state.m_numDegreeOfFreedomQ; i++)
  			{
  				state.m_actualStateQ[i] = actualStateQ[i];
  			}
  			for (int i = 0; i < state.m_numDegreeOfFreedomU; i++)
  			{
  				state.m_actualStateQdot[i] = actualStateQdot[i];
  			}
  			int numJoints = getNumJoints(bodyUniqueId);
  			state.m_jointReactionForces.resize(6 * numJoints);
  			for (int i = 0; i < numJoints * 6; i++)
  			{
  				state.m_jointReactionForces[i] = jointReactionForces[i];
  			}
  
  			return true;
  		}
  		//rootInertialFrame,
  		//              &state.m_actualStateQ[0],
  		//            &state.m_actualStateQdot[0],
  		//          &state.m_jointReactionForces[0]);
  	}
  	return false;
  }
  
  bool b3RobotSimulatorClientAPI_NoDirect::resetJointState(int bodyUniqueId, int jointIndex, double targetValue)
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return false;
  	}
  	b3SharedMemoryCommandHandle commandHandle;
  	b3SharedMemoryStatusHandle statusHandle;
  	int numJoints;
  
  	numJoints = b3GetNumJoints(m_data->m_physicsClientHandle, bodyUniqueId);
  	if ((jointIndex >= numJoints) || (jointIndex < 0))
  	{
  		return false;
  	}
  
  	commandHandle = b3CreatePoseCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
  
  	b3CreatePoseCommandSetJointPosition(m_data->m_physicsClientHandle, commandHandle, jointIndex,
  										targetValue);
  
  	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
  	return false;
  }
  
  void b3RobotSimulatorClientAPI_NoDirect::setJointMotorControl(int bodyUniqueId, int jointIndex, const b3RobotSimulatorJointMotorArgs& args)
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return;
  	}
  	b3SharedMemoryStatusHandle statusHandle;
  	switch (args.m_controlMode)
  	{
  		case CONTROL_MODE_VELOCITY:
  		{
  			b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_VELOCITY);
  			b3JointInfo jointInfo;
  			b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo);
  			int uIndex = jointInfo.m_uIndex;
  			if (uIndex >= 0)
  			{
  				b3JointControlSetKd(command, uIndex, args.m_kd);
  				b3JointControlSetDesiredVelocity(command, uIndex, args.m_targetVelocity);
  				b3JointControlSetMaximumForce(command, uIndex, args.m_maxTorqueValue);
  				statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
  			}
  			break;
  		}
  		case CONTROL_MODE_POSITION_VELOCITY_PD:
  		{
  			b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_POSITION_VELOCITY_PD);
  			b3JointInfo jointInfo;
  			b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo);
  			int uIndex = jointInfo.m_uIndex;
  			int qIndex = jointInfo.m_qIndex;
  
  			b3JointControlSetDesiredPosition(command, qIndex, args.m_targetPosition);
  			b3JointControlSetKp(command, uIndex, args.m_kp);
  			b3JointControlSetDesiredVelocity(command, uIndex, args.m_targetVelocity);
  			b3JointControlSetKd(command, uIndex, args.m_kd);
  			b3JointControlSetMaximumForce(command, uIndex, args.m_maxTorqueValue);
  			statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
  			break;
  		}
  		case CONTROL_MODE_TORQUE:
  		{
  			b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_TORQUE);
  			b3JointInfo jointInfo;
  			b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo);
  			int uIndex = jointInfo.m_uIndex;
  			if (uIndex >= 0)
  			{
  				b3JointControlSetDesiredForceTorque(command, uIndex, args.m_maxTorqueValue);
  				statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
  			}
  			break;
  		}
  		case CONTROL_MODE_PD:
  		{
  			b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_PD);
  			b3JointInfo jointInfo;
  			b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo);
  			int uIndex = jointInfo.m_uIndex;
  			int qIndex = jointInfo.m_qIndex;
  
  			b3JointControlSetDesiredPosition(command, qIndex, args.m_targetPosition);
  			b3JointControlSetKp(command, uIndex, args.m_kp);
  			b3JointControlSetDesiredVelocity(command, uIndex, args.m_targetVelocity);
  			b3JointControlSetKd(command, uIndex, args.m_kd);
  			b3JointControlSetMaximumForce(command, uIndex, args.m_maxTorqueValue);
  			statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
  			break;
  		}
  		default:
  		{
  			b3Error("Unknown control command in b3RobotSimulationClientAPI::setJointMotorControl");
  		}
  	}
  }
  
  void b3RobotSimulatorClientAPI_NoDirect::setNumSolverIterations(int numIterations)
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return;
  	}
  	b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
  	b3SharedMemoryStatusHandle statusHandle;
  	b3PhysicsParamSetNumSolverIterations(command, numIterations);
  	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
  	btAssert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
  }
  
  void b3RobotSimulatorClientAPI_NoDirect::setContactBreakingThreshold(double threshold)
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return;
  	}
  	b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
  	b3SharedMemoryStatusHandle statusHandle;
  	b3PhysicsParamSetContactBreakingThreshold(command, threshold);
  	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
  	btAssert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
  }
  
  void b3RobotSimulatorClientAPI_NoDirect::setTimeStep(double timeStepInSeconds)
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return;
  	}
  
  	b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
  	b3SharedMemoryStatusHandle statusHandle;
  	int ret;
  	ret = b3PhysicsParamSetTimeStep(command, timeStepInSeconds);
  	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
  }
  
  void b3RobotSimulatorClientAPI_NoDirect::setNumSimulationSubSteps(int numSubSteps)
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return;
  	}
  	b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
  	b3SharedMemoryStatusHandle statusHandle;
  	b3PhysicsParamSetNumSubSteps(command, numSubSteps);
  	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
  	btAssert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
  }
  
  bool b3RobotSimulatorClientAPI_NoDirect::calculateInverseKinematics(const struct b3RobotSimulatorInverseKinematicArgs& args, struct b3RobotSimulatorInverseKinematicsResults& results)
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return false;
  	}
  	btAssert(args.m_endEffectorLinkIndex >= 0);
  	btAssert(args.m_bodyUniqueId >= 0);
  
  	b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(m_data->m_physicsClientHandle, args.m_bodyUniqueId);
  	if ((args.m_flags & B3_HAS_IK_TARGET_ORIENTATION) && (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY))
  	{
  		b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, args.m_endEffectorTargetOrientation, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0], &args.m_restPoses[0]);
  	}
  	else if (args.m_flags & B3_HAS_IK_TARGET_ORIENTATION)
  	{
  		b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, args.m_endEffectorTargetOrientation);
  	}
  	else if (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY)
  	{
  		b3CalculateInverseKinematicsPosWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0], &args.m_restPoses[0]);
  	}
  	else
  	{
  		b3CalculateInverseKinematicsAddTargetPurePosition(command, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition);
  	}
  
  	if (args.m_flags & B3_HAS_JOINT_DAMPING)
  	{
  		b3CalculateInverseKinematicsSetJointDamping(command, args.m_numDegreeOfFreedom, &args.m_jointDamping[0]);
  	}
  
  	if (args.m_flags & B3_HAS_CURRENT_POSITIONS)
  	{
  		b3CalculateInverseKinematicsSetCurrentPositions(command, args.m_numDegreeOfFreedom, &args.m_currentJointPositions[0]);
  	}
  
  	b3SharedMemoryStatusHandle statusHandle;
  	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
  
  	int numPos = 0;
  
  	bool result = b3GetStatusInverseKinematicsJointPositions(statusHandle,
  															 &results.m_bodyUniqueId,
  															 &numPos,
  															 0) != 0;
  	if (result && numPos)
  	{
  		results.m_calculatedJointPositions.resize(numPos);
  		result = b3GetStatusInverseKinematicsJointPositions(statusHandle,
  															&results.m_bodyUniqueId,
  															&numPos,
  															&results.m_calculatedJointPositions[0]) != 0;
  	}
  	return result;
  }
  
  bool b3RobotSimulatorClientAPI_NoDirect::getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian)
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return false;
  	}
  	b3SharedMemoryCommandHandle command = b3CalculateJacobianCommandInit(m_data->m_physicsClientHandle, bodyUniqueId, linkIndex, localPosition, jointPositions, jointVelocities, jointAccelerations);
  	b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
  
  	if (b3GetStatusType(statusHandle) == CMD_CALCULATED_JACOBIAN_COMPLETED)
  	{
  		int dofCount;
  		b3GetStatusJacobian(statusHandle, &dofCount, linearJacobian, angularJacobian);
  		return true;
  	}
  	return false;
  }
  
  bool b3RobotSimulatorClientAPI_NoDirect::getLinkState(int bodyUniqueId, int linkIndex, int computeLinkVelocity, int computeForwardKinematics, b3LinkState* linkState)
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return false;
  	}
  	b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
  
  	if (computeLinkVelocity)
  	{
  		b3RequestActualStateCommandComputeLinkVelocity(command, computeLinkVelocity);
  	}
  
  	if (computeForwardKinematics)
  	{
  		b3RequestActualStateCommandComputeForwardKinematics(command, computeForwardKinematics);
  	}
  
  	b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
  
  	if (b3GetStatusType(statusHandle) == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
  	{
  		b3GetLinkState(m_data->m_physicsClientHandle, statusHandle, linkIndex, linkState);
  		return true;
  	}
  	return false;
  }
  
  void b3RobotSimulatorClientAPI_NoDirect::configureDebugVisualizer(b3ConfigureDebugVisualizerEnum flag, int enable)
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return;
  	}
  	b3SharedMemoryCommandHandle commandHandle = b3InitConfigureOpenGLVisualizer(m_data->m_physicsClientHandle);
  	b3ConfigureOpenGLVisualizerSetVisualizationFlags(commandHandle, flag, enable);
  	b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
  }
  
  void b3RobotSimulatorClientAPI_NoDirect::getVREvents(b3VREventsData* vrEventsData, int deviceTypeFilter)
  {
  	vrEventsData->m_numControllerEvents = 0;
  	vrEventsData->m_controllerEvents = 0;
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return;
  	}
  
  	b3SharedMemoryCommandHandle commandHandle = b3RequestVREventsCommandInit(m_data->m_physicsClientHandle);
  	b3VREventsSetDeviceTypeFilter(commandHandle, deviceTypeFilter);
  	b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
  	b3GetVREventsData(m_data->m_physicsClientHandle, vrEventsData);
  }
  
  void b3RobotSimulatorClientAPI_NoDirect::getKeyboardEvents(b3KeyboardEventsData* keyboardEventsData)
  {
  	keyboardEventsData->m_numKeyboardEvents = 0;
  	keyboardEventsData->m_keyboardEvents = 0;
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return;
  	}
  
  	b3SharedMemoryCommandHandle commandHandle = b3RequestKeyboardEventsCommandInit(m_data->m_physicsClientHandle);
  	b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
  	b3GetKeyboardEventsData(m_data->m_physicsClientHandle, keyboardEventsData);
  }
  
  int b3RobotSimulatorClientAPI_NoDirect::startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const btAlignedObjectArray<int>& objectUniqueIds, int maxLogDof)
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return -1;
  	}
  	int loggingUniqueId = -1;
  	b3SharedMemoryCommandHandle commandHandle;
  	commandHandle = b3StateLoggingCommandInit(m_data->m_physicsClientHandle);
  
  	b3StateLoggingStart(commandHandle, loggingType, fileName.c_str());
  
  	for (int i = 0; i < objectUniqueIds.size(); i++)
  	{
  		int objectUid = objectUniqueIds[i];
  		b3StateLoggingAddLoggingObjectUniqueId(commandHandle, objectUid);
  	}
  
  	if (maxLogDof > 0)
  	{
  		b3StateLoggingSetMaxLogDof(commandHandle, maxLogDof);
  	}
  
  	b3SharedMemoryStatusHandle statusHandle;
  	int statusType;
  	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
  	statusType = b3GetStatusType(statusHandle);
  	if (statusType == CMD_STATE_LOGGING_START_COMPLETED)
  	{
  		loggingUniqueId = b3GetStatusLoggingUniqueId(statusHandle);
  	}
  	return loggingUniqueId;
  }
  
  void b3RobotSimulatorClientAPI_NoDirect::stopStateLogging(int stateLoggerUniqueId)
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return;
  	}
  
  	b3SharedMemoryCommandHandle commandHandle;
  	b3SharedMemoryStatusHandle statusHandle;
  	commandHandle = b3StateLoggingCommandInit(m_data->m_physicsClientHandle);
  	b3StateLoggingStop(commandHandle, stateLoggerUniqueId);
  	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
  }
  
  void b3RobotSimulatorClientAPI_NoDirect::resetDebugVisualizerCamera(double cameraDistance, double cameraPitch, double cameraYaw, const btVector3& targetPos)
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return;
  	}
  
  	b3SharedMemoryCommandHandle commandHandle = b3InitConfigureOpenGLVisualizer(m_data->m_physicsClientHandle);
  	if (commandHandle)
  	{
  		if ((cameraDistance >= 0))
  		{
  			btVector3FloatData camTargetPos;
  			targetPos.serializeFloat(camTargetPos);
  			b3ConfigureOpenGLVisualizerSetViewMatrix(commandHandle, cameraDistance, cameraPitch, cameraYaw, camTargetPos.m_floats);
  		}
  		b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
  	}
  }
  
  void b3RobotSimulatorClientAPI_NoDirect::submitProfileTiming(const std::string& profileName)
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return;
  	}
  
  	b3SharedMemoryCommandHandle commandHandle = b3ProfileTimingCommandInit(m_data->m_physicsClientHandle, profileName.c_str());
  
  	if (profileName.length())
  	{
  		b3SetProfileTimingType(commandHandle, 0);
  	}
  	else
  	{
  		b3SetProfileTimingType(commandHandle, 1);
  	}
  
  	b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
  }
  
  void b3RobotSimulatorClientAPI_NoDirect::loadSoftBody(const std::string& fileName, const struct b3RobotSimulatorLoadSoftBodyArgs& args)
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return;
  	}
  
  	b3SharedMemoryCommandHandle command = b3LoadSoftBodyCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
  	b3LoadSoftBodySetStartPosition(command, args.m_startPosition[0], args.m_startPosition[1], args.m_startPosition[2]);
  	b3LoadSoftBodySetStartOrientation(command, args.m_startOrientation[0], args.m_startOrientation[1], args.m_startOrientation[2], args.m_startOrientation[3]);
  	b3LoadSoftBodySetScale(command, args.m_scale);
  	b3LoadSoftBodySetMass(command, args.m_mass);
  	b3LoadSoftBodySetCollisionMargin(command, args.m_collisionMargin);
  	b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
  }
  
  void b3RobotSimulatorClientAPI_NoDirect::loadDeformableBody(const std::string& fileName, const struct b3RobotSimulatorLoadDeformableBodyArgs& args)
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return;
  	}
  
  	b3SharedMemoryCommandHandle command = b3LoadSoftBodyCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
  	b3LoadSoftBodySetStartPosition(command, args.m_startPosition[0], args.m_startPosition[1], args.m_startPosition[2]);
  	b3LoadSoftBodySetStartOrientation(command, args.m_startOrientation[0], args.m_startOrientation[1], args.m_startOrientation[2], args.m_startOrientation[3]);
  	b3LoadSoftBodySetScale(command, args.m_scale);
  	b3LoadSoftBodySetMass(command, args.m_mass);
  	b3LoadSoftBodySetCollisionMargin(command, args.m_collisionMargin);
  	if (args.m_NeoHookeanMu > 0)
  	{
  		b3LoadSoftBodyAddNeoHookeanForce(command, args.m_NeoHookeanMu, args.m_NeoHookeanLambda, args.m_NeoHookeanDamping);
  	}
  	if (args.m_springElasticStiffness > 0)
  	{
  		b3LoadSoftBodyAddMassSpringForce(command, args.m_springElasticStiffness, args.m_springDampingStiffness);
  	}
  	b3LoadSoftBodySetSelfCollision(command, args.m_useSelfCollision);
  	b3LoadSoftBodyUseFaceContact(command, args.m_useFaceContact);
  	b3LoadSoftBodySetFrictionCoefficient(command, args.m_frictionCoeff);
  	b3LoadSoftBodyUseBendingSprings(command, args.m_useBendingSprings, args.m_springBendingStiffness);
  	b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
  }
  
  void b3RobotSimulatorClientAPI_NoDirect::getMouseEvents(b3MouseEventsData* mouseEventsData)
  {
  	mouseEventsData->m_numMouseEvents = 0;
  	mouseEventsData->m_mouseEvents = 0;
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return;
  	}
  
  	b3SharedMemoryCommandHandle command = b3RequestMouseEventsCommandInit(m_data->m_physicsClientHandle);
  	b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
  	b3GetMouseEventsData(m_data->m_physicsClientHandle, mouseEventsData);
  }
  
  bool b3RobotSimulatorClientAPI_NoDirect::getCameraImage(int width, int height, struct b3RobotSimulatorGetCameraImageArgs args, struct b3CameraImageData& imageData)
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return false;
  	}
  
  	b3SharedMemoryCommandHandle command;
  
  	command = b3InitRequestCameraImage(m_data->m_physicsClientHandle);
  
  	b3RequestCameraImageSetPixelResolution(command, width, height);
  
  	// Check and apply optional arguments
  	if (args.m_viewMatrix && args.m_projectionMatrix)
  	{
  		b3RequestCameraImageSetCameraMatrices(command, args.m_viewMatrix, args.m_projectionMatrix);
  	}
  
  	if (args.m_lightDirection != NULL)
  	{
  		b3RequestCameraImageSetLightDirection(command, args.m_lightDirection);
  	}
  
  	if (args.m_lightColor != NULL)
  	{
  		b3RequestCameraImageSetLightColor(command, args.m_lightColor);
  	}
  
  	if (args.m_lightDistance >= 0)
  	{
  		b3RequestCameraImageSetLightDistance(command, args.m_lightDistance);
  	}
  
  	if (args.m_hasShadow >= 0)
  	{
  		b3RequestCameraImageSetShadow(command, args.m_hasShadow);
  	}
  
  	if (args.m_lightAmbientCoeff >= 0)
  	{
  		b3RequestCameraImageSetLightAmbientCoeff(command, args.m_lightAmbientCoeff);
  	}
  
  	if (args.m_lightDiffuseCoeff >= 0)
  	{
  		b3RequestCameraImageSetLightDiffuseCoeff(command, args.m_lightDiffuseCoeff);
  	}
  
  	if (args.m_lightSpecularCoeff >= 0)
  	{
  		b3RequestCameraImageSetLightSpecularCoeff(command, args.m_lightSpecularCoeff);
  	}
  
  	if (args.m_renderer >= 0)
  	{
  		b3RequestCameraImageSelectRenderer(command, args.m_renderer);
  	}
  
  	// Actually retrieve the image
  	if (b3CanSubmitCommand(m_data->m_physicsClientHandle))
  	{
  		b3SharedMemoryStatusHandle statusHandle;
  		int statusType;
  
  		statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
  		statusType = b3GetStatusType(statusHandle);
  		if (statusType == CMD_CAMERA_IMAGE_COMPLETED)
  		{
  			b3GetCameraImageData(m_data->m_physicsClientHandle, &imageData);
  		}
  	}
  	else
  	{
  		return false;
  	}
  	return true;
  }
  
  bool b3RobotSimulatorClientAPI_NoDirect::calculateInverseDynamics(int bodyUniqueId, double* jointPositions, double* jointVelocities,
  																  double* jointAccelerations, double* jointForcesOutput)
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return false;
  	}
  
  	b3GetNumJoints(m_data->m_physicsClientHandle, bodyUniqueId);
  	b3SharedMemoryStatusHandle statusHandle;
  	int statusType;
  	b3SharedMemoryCommandHandle commandHandle = b3CalculateInverseDynamicsCommandInit(m_data->m_physicsClientHandle, bodyUniqueId, jointPositions,
  																					  jointVelocities, jointAccelerations);
  
  	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
  
  	statusType = b3GetStatusType(statusHandle);
  
  	if (statusType == CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED)
  	{
  		int bodyUniqueId;
  		int dofCount;
  
  		b3GetStatusInverseDynamicsJointForces(statusHandle, &bodyUniqueId, &dofCount, 0);
  
  		if (dofCount)
  		{
  			b3GetStatusInverseDynamicsJointForces(statusHandle, 0, 0, jointForcesOutput);
  			return true;
  		}
  	}
  	return false;
  }
  
  int b3RobotSimulatorClientAPI_NoDirect::getNumBodies() const
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return false;
  	}
  	return b3GetNumBodies(m_data->m_physicsClientHandle);
  }
  
  int b3RobotSimulatorClientAPI_NoDirect::getBodyUniqueId(int bodyId) const
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return false;
  	}
  	return b3GetBodyUniqueId(m_data->m_physicsClientHandle, bodyId);
  }
  
  bool b3RobotSimulatorClientAPI_NoDirect::removeBody(int bodyUniqueId)
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return false;
  	}
  
  	b3SharedMemoryStatusHandle statusHandle;
  	int statusType;
  
  	if (b3CanSubmitCommand(m_data->m_physicsClientHandle))
  	{
  		statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitRemoveBodyCommand(m_data->m_physicsClientHandle, bodyUniqueId));
  		statusType = b3GetStatusType(statusHandle);
  		if (statusType == CMD_REMOVE_BODY_COMPLETED)
  		{
  			return true;
  		}
  		else
  		{
  			b3Warning("getDynamicsInfo did not complete");
  			return false;
  		}
  	}
  	b3Warning("removeBody could not submit command");
  	return false;
  }
  
  bool b3RobotSimulatorClientAPI_NoDirect::getDynamicsInfo(int bodyUniqueId, int linkIndex, b3DynamicsInfo* dynamicsInfo)
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return false;
  	}
  	int status_type = 0;
  	b3SharedMemoryCommandHandle cmd_handle;
  	b3SharedMemoryStatusHandle status_handle;
  	//  struct b3DynamicsInfo info;
  
  	if (bodyUniqueId < 0)
  	{
  		b3Warning("getDynamicsInfo failed; invalid bodyUniqueId");
  		return false;
  	}
  	if (linkIndex < -1)
  	{
  		b3Warning("getDynamicsInfo failed; invalid linkIndex");
  		return false;
  	}
  
  	if (b3CanSubmitCommand(m_data->m_physicsClientHandle))
  	{
  		cmd_handle = b3GetDynamicsInfoCommandInit(m_data->m_physicsClientHandle, bodyUniqueId, linkIndex);
  		status_handle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, cmd_handle);
  		status_type = b3GetStatusType(status_handle);
  		if (status_type == CMD_GET_DYNAMICS_INFO_COMPLETED)
  		{
  			b3GetDynamicsInfo(status_handle, dynamicsInfo);
  			return true;
  		}
  		else
  		{
  			b3Warning("getDynamicsInfo did not complete");
  			return false;
  		}
  	}
  	b3Warning("getDynamicsInfo could not submit command");
  	return false;
  }
  
  bool b3RobotSimulatorClientAPI_NoDirect::changeDynamics(int bodyUniqueId, int linkIndex, struct b3RobotSimulatorChangeDynamicsArgs& args)
  {
  	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
  	if (sm == 0)
  	{
  		b3Warning("Not connected to physics server.");
  		return false;
  	}
  	b3SharedMemoryCommandHandle command = b3InitChangeDynamicsInfo(sm);
  	b3SharedMemoryStatusHandle statusHandle;
  
  	if (args.m_activationState >= 0)
  	{
  		b3ChangeDynamicsInfoSetActivationState(command, bodyUniqueId, args.m_activationState);
  	}
  	if (args.m_mass >= 0)
  	{
  		b3ChangeDynamicsInfoSetMass(command, bodyUniqueId, linkIndex, args.m_mass);
  	}
  
  	if (args.m_lateralFriction >= 0)
  	{
  		b3ChangeDynamicsInfoSetLateralFriction(command, bodyUniqueId, linkIndex, args.m_lateralFriction);
  	}
  
  	if (args.m_spinningFriction >= 0)
  	{
  		b3ChangeDynamicsInfoSetSpinningFriction(command, bodyUniqueId, linkIndex, args.m_spinningFriction);
  	}
  
  	if (args.m_rollingFriction >= 0)
  	{
  		b3ChangeDynamicsInfoSetRollingFriction(command, bodyUniqueId, linkIndex, args.m_rollingFriction);
  	}
  
  	if (args.m_linearDamping >= 0)
  	{
  		b3ChangeDynamicsInfoSetLinearDamping(command, bodyUniqueId, args.m_linearDamping);
  	}
  
  	if (args.m_angularDamping >= 0)
  	{
  		b3ChangeDynamicsInfoSetAngularDamping(command, bodyUniqueId, args.m_angularDamping);
  	}
  
  	if (args.m_restitution >= 0)
  	{
  		b3ChangeDynamicsInfoSetRestitution(command, bodyUniqueId, linkIndex, args.m_restitution);
  	}
  
  	if (args.m_contactStiffness >= 0 && args.m_contactDamping >= 0)
  	{
  		b3ChangeDynamicsInfoSetContactStiffnessAndDamping(command, bodyUniqueId, linkIndex, args.m_contactStiffness, args.m_contactDamping);
  	}
  
  	if (args.m_frictionAnchor >= 0)
  	{
  		b3ChangeDynamicsInfoSetFrictionAnchor(command, bodyUniqueId, linkIndex, args.m_frictionAnchor);
  	}
  	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
  	return true;
  }
  
  int b3RobotSimulatorClientAPI_NoDirect::addUserDebugParameter(const char* paramName, double rangeMin, double rangeMax, double startValue)
  {
  	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
  	if (sm == 0)
  	{
  		b3Warning("Not connected to physics server.");
  		return -1;
  	}
  	b3SharedMemoryCommandHandle commandHandle;
  	b3SharedMemoryStatusHandle statusHandle;
  	int statusType;
  
  	commandHandle = b3InitUserDebugAddParameter(sm, paramName, rangeMin, rangeMax, startValue);
  	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
  	statusType = b3GetStatusType(statusHandle);
  
  	if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED)
  	{
  		int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle);
  		return debugItemUniqueId;
  	}
  	b3Warning("addUserDebugParameter failed.");
  	return -1;
  }
  
  double b3RobotSimulatorClientAPI_NoDirect::readUserDebugParameter(int itemUniqueId)
  {
  	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
  	if (sm == 0)
  	{
  		b3Warning("Not connected to physics server.");
  		return 0;
  	}
  	b3SharedMemoryCommandHandle commandHandle;
  	b3SharedMemoryStatusHandle statusHandle;
  	int statusType;
  
  	commandHandle = b3InitUserDebugReadParameter(sm, itemUniqueId);
  	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
  	statusType = b3GetStatusType(statusHandle);
  
  	if (statusType == CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED)
  	{
  		double paramValue = 0.f;
  		int ok = b3GetStatusDebugParameterValue(statusHandle, &paramValue);
  		if (ok)
  		{
  			return paramValue;
  		}
  	}
  	b3Warning("readUserDebugParameter failed.");
  	return 0;
  }
  
  bool b3RobotSimulatorClientAPI_NoDirect::removeUserDebugItem(int itemUniqueId)
  {
  	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
  	if (sm == 0)
  	{
  		b3Warning("Not connected to physics server.");
  		return false;
  	}
  	b3SharedMemoryCommandHandle commandHandle;
  	b3SharedMemoryStatusHandle statusHandle;
  	int statusType;
  
  	commandHandle = b3InitUserDebugDrawRemove(sm, itemUniqueId);
  	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
  	statusType = b3GetStatusType(statusHandle);
  	return true;
  }
  
  int b3RobotSimulatorClientAPI_NoDirect::addUserDebugText(char* text, double* textPosition, struct b3RobotSimulatorAddUserDebugTextArgs& args)
  {
  	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
  	if (sm == 0)
  	{
  		b3Warning("Not connected to physics server.");
  		return -1;
  	}
  	b3SharedMemoryCommandHandle commandHandle;
  	b3SharedMemoryStatusHandle statusHandle;
  	int statusType;
  
  	commandHandle = b3InitUserDebugDrawAddText3D(sm, text, textPosition, &args.m_colorRGB[0], args.m_size, args.m_lifeTime);
  
  	if (args.m_parentObjectUniqueId >= 0)
  	{
  		b3UserDebugItemSetParentObject(commandHandle, args.m_parentObjectUniqueId, args.m_parentLinkIndex);
  	}
  
  	if (args.m_flags & DEBUG_TEXT_HAS_ORIENTATION)
  	{
  		b3UserDebugTextSetOrientation(commandHandle, &args.m_textOrientation[0]);
  	}
  
  	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
  	statusType = b3GetStatusType(statusHandle);
  
  	if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED)
  	{
  		int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle);
  		return debugItemUniqueId;
  	}
  	b3Warning("addUserDebugText3D failed.");
  	return -1;
  }
  
  int b3RobotSimulatorClientAPI_NoDirect::addUserDebugText(char* text, btVector3& textPosition, struct b3RobotSimulatorAddUserDebugTextArgs& args)
  {
  	double dposXYZ[3];
  	dposXYZ[0] = textPosition.x();
  	dposXYZ[1] = textPosition.y();
  	dposXYZ[2] = textPosition.z();
  
  	return addUserDebugText(text, &dposXYZ[0], args);
  }
  
  int b3RobotSimulatorClientAPI_NoDirect::addUserDebugLine(double* fromXYZ, double* toXYZ, struct b3RobotSimulatorAddUserDebugLineArgs& args)
  {
  	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
  	if (sm == 0)
  	{
  		b3Warning("Not connected to physics server.");
  		return -1;
  	}
  	b3SharedMemoryCommandHandle commandHandle;
  	b3SharedMemoryStatusHandle statusHandle;
  	int statusType;
  
  	commandHandle = b3InitUserDebugDrawAddLine3D(sm, fromXYZ, toXYZ, &args.m_colorRGB[0], args.m_lineWidth, args.m_lifeTime);
  
  	if (args.m_parentObjectUniqueId >= 0)
  	{
  		b3UserDebugItemSetParentObject(commandHandle, args.m_parentObjectUniqueId, args.m_parentLinkIndex);
  	}
  
  	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
  	statusType = b3GetStatusType(statusHandle);
  
  	if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED)
  	{
  		int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle);
  		return debugItemUniqueId;
  	}
  	b3Warning("addUserDebugLine failed.");
  	return -1;
  }
  
  int b3RobotSimulatorClientAPI_NoDirect::addUserDebugLine(btVector3& fromXYZ, btVector3& toXYZ, struct b3RobotSimulatorAddUserDebugLineArgs& args)
  {
  	double dfromXYZ[3];
  	double dtoXYZ[3];
  	dfromXYZ[0] = fromXYZ.x();
  	dfromXYZ[1] = fromXYZ.y();
  	dfromXYZ[2] = fromXYZ.z();
  
  	dtoXYZ[0] = toXYZ.x();
  	dtoXYZ[1] = toXYZ.y();
  	dtoXYZ[2] = toXYZ.z();
  	return addUserDebugLine(&dfromXYZ[0], &dtoXYZ[0], args);
  }
  
  bool b3RobotSimulatorClientAPI_NoDirect::setJointMotorControlArray(int bodyUniqueId, struct b3RobotSimulatorJointMotorArrayArgs& args)
  {
  	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
  	if (sm == 0)
  	{
  		b3Warning("Not connected to physics server.");
  		return false;
  	}
  	b3GetNumJoints(sm, bodyUniqueId);
  
  	b3SharedMemoryCommandHandle commandHandle;
  	b3SharedMemoryStatusHandle statusHandle;
  	//  int statusType;
  	struct b3JointInfo info;
  
  	commandHandle = b3JointControlCommandInit2(sm, bodyUniqueId, args.m_controlMode);
  
  	for (int i = 0; i < args.m_numControlledDofs; i++)
  	{
  		double targetVelocity = 0.0;
  		double targetPosition = 0.0;
  		double force = 100000.0;
  		double kp = 0.1;
  		double kd = 1.0;
  		int jointIndex;
  
  		if (args.m_jointIndices)
  		{
  			jointIndex = args.m_jointIndices[i];
  		}
  		else
  		{
  			jointIndex = i;
  		}
  
  		if (args.m_targetVelocities)
  		{
  			targetVelocity = args.m_targetVelocities[i];
  		}
  
  		if (args.m_targetPositions)
  		{
  			targetPosition = args.m_targetPositions[i];
  		}
  
  		if (args.m_forces)
  		{
  			force = args.m_forces[i];
  		}
  
  		if (args.m_kps)
  		{
  			kp = args.m_kps[i];
  		}
  
  		if (args.m_kds)
  		{
  			kd = args.m_kds[i];
  		}
  
  		b3GetJointInfo(sm, bodyUniqueId, jointIndex, &info);
  
  		switch (args.m_controlMode)
  		{
  			case CONTROL_MODE_VELOCITY:
  			{
  				b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetVelocity);
  				b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
  				b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, force);
  				break;
  			}
  
  			case CONTROL_MODE_TORQUE:
  			{
  				b3JointControlSetDesiredForceTorque(commandHandle, info.m_uIndex, force);
  				break;
  			}
  
  			case CONTROL_MODE_POSITION_VELOCITY_PD:
  			{
  				b3JointControlSetDesiredPosition(commandHandle, info.m_qIndex, targetPosition);
  				b3JointControlSetKp(commandHandle, info.m_uIndex, kp);
  				b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetVelocity);
  				b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
  				b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, force);
  				break;
  			}
  
  			default:
  			{
  			}
  		};
  	}
  	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
  	return true;
  }
  
  bool b3RobotSimulatorClientAPI_NoDirect::getPhysicsEngineParameters(struct b3RobotSimulatorSetPhysicsEngineParameters& args)
  {
  	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
  	if (sm == 0)
  	{
  		b3Warning("Not connected");
  		return false;
  	}
  
  	{
  		b3SharedMemoryCommandHandle command = b3InitRequestPhysicsParamCommand(sm);
  		b3SharedMemoryStatusHandle statusHandle;
  		int statusType;
  
  		statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
  		statusType = b3GetStatusType(statusHandle);
  		if (statusType != CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED)
  		{
  			return false;
  		}
  		b3GetStatusPhysicsSimulationParameters(statusHandle, &args);
  	}
  	return true;
  }
  
  bool b3RobotSimulatorClientAPI_NoDirect::setPhysicsEngineParameter(const struct b3RobotSimulatorSetPhysicsEngineParameters& args)
  {
  	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
  	if (sm == 0)
  	{
  		b3Warning("Not connected");
  		return false;
  	}
  	b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm);
  	b3SharedMemoryStatusHandle statusHandle;
  
  	if (args.m_numSolverIterations >= 0)
  	{
  		b3PhysicsParamSetNumSolverIterations(command, args.m_numSolverIterations);
  	}
  
  	if (args.m_collisionFilterMode >= 0)
  	{
  		b3PhysicsParamSetCollisionFilterMode(command, args.m_collisionFilterMode);
  	}
  
  	if (args.m_numSimulationSubSteps >= 0)
  	{
  		b3PhysicsParamSetNumSubSteps(command, args.m_numSimulationSubSteps);
  	}
  
  	if (args.m_deltaTime >= 0)
  	{
  		b3PhysicsParamSetTimeStep(command, args.m_deltaTime);
  	}
  
  	if (args.m_useSplitImpulse >= 0)
  	{
  		b3PhysicsParamSetUseSplitImpulse(command, args.m_useSplitImpulse);
  	}
  
  	if (args.m_splitImpulsePenetrationThreshold >= 0)
  	{
  		b3PhysicsParamSetSplitImpulsePenetrationThreshold(command, args.m_splitImpulsePenetrationThreshold);
  	}
  
  	if (args.m_contactBreakingThreshold >= 0)
  	{
  		b3PhysicsParamSetContactBreakingThreshold(command, args.m_contactBreakingThreshold);
  	}
  
  	if (args.m_restitutionVelocityThreshold >= 0)
  	{
  		b3PhysicsParamSetRestitutionVelocityThreshold(command, args.m_restitutionVelocityThreshold);
  	}
  
  	if (args.m_enableFileCaching >= 0)
  	{
  		b3PhysicsParamSetEnableFileCaching(command, args.m_enableFileCaching);
  	}
  
  	if (args.m_defaultNonContactERP >= 0)
  	{
  		b3PhysicsParamSetDefaultNonContactERP(command, args.m_defaultNonContactERP);
  	}
  
  	if (args.m_defaultContactERP >= 0)
  	{
  		b3PhysicsParamSetDefaultContactERP(command, args.m_defaultContactERP);
  	}
  
  	if (args.m_frictionERP >= 0)
  	{
  		b3PhysicsParamSetDefaultFrictionERP(command, args.m_frictionERP);
  	}
  
  	if (args.m_solverResidualThreshold >= 0)
  	{
  		b3PhysicsParamSetSolverResidualThreshold(command, args.m_solverResidualThreshold);
  	}
  
  	if (args.m_constraintSolverType >= 0)
  	{
  		b3PhysicsParameterSetConstraintSolverType(command, args.m_constraintSolverType);
  	}
  	if (args.m_minimumSolverIslandSize >= 0)
  	{
  		b3PhysicsParameterSetMinimumSolverIslandSize(command, args.m_minimumSolverIslandSize);
  	}
  
  	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
  	return true;
  }
  
  bool b3RobotSimulatorClientAPI_NoDirect::applyExternalForce(int objectUniqueId, int linkIndex, double* force, double* position, int flags)
  {
  	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
  	if (sm == 0)
  	{
  		b3Warning("Not connected");
  		return false;
  	}
  	b3SharedMemoryCommandHandle command;
  	b3SharedMemoryStatusHandle statusHandle;
  
  	command = b3ApplyExternalForceCommandInit(sm);
  	b3ApplyExternalForce(command, objectUniqueId, linkIndex, force, position, flags);
  	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
  	return true;
  }
  
  bool b3RobotSimulatorClientAPI_NoDirect::applyExternalForce(int objectUniqueId, int linkIndex, btVector3& force, btVector3& position, int flags)
  {
  	double dforce[3];
  	double dposition[3];
  
  	dforce[0] = force.x();
  	dforce[1] = force.y();
  	dforce[2] = force.z();
  
  	dposition[0] = position.x();
  	dposition[1] = position.y();
  	dposition[2] = position.z();
  
  	return applyExternalForce(objectUniqueId, linkIndex, &dforce[0], &dposition[0], flags);
  }
  
  bool b3RobotSimulatorClientAPI_NoDirect::applyExternalTorque(int objectUniqueId, int linkIndex, double* torque, int flags)
  {
  	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
  	if (sm == 0)
  	{
  		b3Warning("Not connected");
  		return false;
  	}
  	b3SharedMemoryCommandHandle command;
  	b3SharedMemoryStatusHandle statusHandle;
  
  	command = b3ApplyExternalForceCommandInit(sm);
  	b3ApplyExternalTorque(command, objectUniqueId, linkIndex, torque, flags);
  	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
  	return true;
  }
  
  bool b3RobotSimulatorClientAPI_NoDirect::applyExternalTorque(int objectUniqueId, int linkIndex, btVector3& torque, int flags)
  {
  	double dtorque[3];
  
  	dtorque[0] = torque.x();
  	dtorque[1] = torque.y();
  	dtorque[2] = torque.z();
  
  	return applyExternalTorque(objectUniqueId, linkIndex, &dtorque[0], flags);
  }
  
  bool b3RobotSimulatorClientAPI_NoDirect::enableJointForceTorqueSensor(int bodyUniqueId, int jointIndex, bool enable)
  {
  	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
  	if (sm == 0)
  	{
  		b3Warning("Not connected");
  		return false;
  	}
  	int numJoints = b3GetNumJoints(sm, bodyUniqueId);
  	if ((jointIndex < 0) || (jointIndex >= numJoints))
  	{
  		b3Warning("Error: invalid jointIndex.");
  		return false;
  	}
  	b3SharedMemoryCommandHandle command;
  	b3SharedMemoryStatusHandle statusHandle;
  	int statusType;
  
  	command = b3CreateSensorCommandInit(sm, bodyUniqueId);
  	b3CreateSensorEnable6DofJointForceTorqueSensor(command, jointIndex, enable);
  	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
  	statusType = b3GetStatusType(statusHandle);
  	if (statusType == CMD_CLIENT_COMMAND_COMPLETED)
  	{
  		return true;
  	}
  	return false;
  }
  
  bool b3RobotSimulatorClientAPI_NoDirect::getDebugVisualizerCamera(struct b3OpenGLVisualizerCameraInfo* cameraInfo)
  {
  	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
  	if (sm == 0)
  	{
  		b3Warning("Not connected");
  		return false;
  	}
  	b3SharedMemoryCommandHandle command;
  	b3SharedMemoryStatusHandle statusHandle;
  	int statusType;
  
  	command = b3InitRequestOpenGLVisualizerCameraCommand(sm);
  	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
  	statusType = b3GetStatusOpenGLVisualizerCamera(statusHandle, cameraInfo);
  
  	if (statusType)
  	{
  		return true;
  	}
  	return false;
  }
  
  bool b3RobotSimulatorClientAPI_NoDirect::getContactPoints(struct b3RobotSimulatorGetContactPointsArgs& args, struct b3ContactInformation* contactInfo)
  {
  	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
  	if (sm == 0)
  	{
  		b3Warning("Not connected");
  		return false;
  	}
  	b3SharedMemoryCommandHandle command;
  	b3SharedMemoryStatusHandle statusHandle;
  	int statusType;
  
  	command = b3InitRequestContactPointInformation(sm);
  
  	if (args.m_bodyUniqueIdA >= 0)
  	{
  		b3SetContactFilterBodyA(command, args.m_bodyUniqueIdA);
  	}
  	if (args.m_bodyUniqueIdB >= 0)
  	{
  		b3SetContactFilterBodyB(command, args.m_bodyUniqueIdB);
  	}
  	if (args.m_linkIndexA >= -1)
  	{
  		b3SetContactFilterLinkA(command, args.m_linkIndexA);
  	}
  	if (args.m_linkIndexB >= -1)
  	{
  		b3SetContactFilterLinkB(command, args.m_linkIndexB);
  	}
  
  	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
  	statusType = b3GetStatusType(statusHandle);
  
  	if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED)
  	{
  		b3GetContactPointInformation(sm, contactInfo);
  		return true;
  	}
  	return false;
  }
  
  bool b3RobotSimulatorClientAPI_NoDirect::getClosestPoints(struct b3RobotSimulatorGetContactPointsArgs& args, double distance, struct b3ContactInformation* contactInfo)
  {
  	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
  	if (sm == 0)
  	{
  		b3Warning("Not connected");
  		return false;
  	}
  	b3SharedMemoryCommandHandle command;
  	b3SharedMemoryStatusHandle statusHandle;
  	int statusType;
  
  	command = b3InitClosestDistanceQuery(sm);
  
  	b3SetClosestDistanceFilterBodyA(command, args.m_bodyUniqueIdA);
  	b3SetClosestDistanceFilterBodyB(command, args.m_bodyUniqueIdB);
  	b3SetClosestDistanceThreshold(command, distance);
  
  	if (args.m_linkIndexA >= -1)
  	{
  		b3SetClosestDistanceFilterLinkA(command, args.m_linkIndexA);
  	}
  	if (args.m_linkIndexB >= -1)
  	{
  		b3SetClosestDistanceFilterLinkB(command, args.m_linkIndexB);
  	}
  
  	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
  	statusType = b3GetStatusType(statusHandle);
  
  	if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED)
  	{
  		b3GetContactPointInformation(sm, contactInfo);
  		return true;
  	}
  	return false;
  }
  
  bool b3RobotSimulatorClientAPI_NoDirect::getOverlappingObjects(double* aabbMin, double* aabbMax, struct b3AABBOverlapData* overlapData)
  {
  	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
  	if (sm == 0)
  	{
  		b3Warning("Not connected");
  		return false;
  	}
  	b3SharedMemoryCommandHandle command;
  	b3SharedMemoryStatusHandle statusHandle;
  	//  int statusType;
  
  	command = b3InitAABBOverlapQuery(sm, aabbMin, aabbMax);
  	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
  	b3GetAABBOverlapResults(sm, overlapData);
  
  	return true;
  }
  
  bool b3RobotSimulatorClientAPI_NoDirect::getOverlappingObjects(btVector3& aabbMin, btVector3& aabbMax, struct b3AABBOverlapData* overlapData)
  {
  	double daabbMin[3];
  	double daabbMax[3];
  
  	daabbMin[0] = aabbMin.x();
  	daabbMin[1] = aabbMin.y();
  	daabbMin[2] = aabbMin.z();
  
  	daabbMax[0] = aabbMax.x();
  	daabbMax[1] = aabbMax.y();
  	daabbMax[2] = aabbMax.z();
  
  	return getOverlappingObjects(&daabbMin[0], &daabbMax[0], overlapData);
  }
  
  bool b3RobotSimulatorClientAPI_NoDirect::getAABB(int bodyUniqueId, int linkIndex, double* aabbMin, double* aabbMax)
  {
  	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
  	if (sm == 0)
  	{
  		b3Warning("Not connected");
  		return false;
  	}
  	b3SharedMemoryCommandHandle command;
  	b3SharedMemoryStatusHandle statusHandle;
  	int statusType;
  
  	if (bodyUniqueId < 0)
  	{
  		b3Warning("Invalid bodyUniqueId");
  		return false;
  	}
  
  	if (linkIndex < -1)
  	{
  		b3Warning("Invalid linkIndex");
  		return false;
  	}
  
  	if (aabbMin == NULL || aabbMax == NULL)
  	{
  		b3Warning("Output AABB matrix is NULL");
  		return false;
  	}
  
  	command = b3RequestCollisionInfoCommandInit(sm, bodyUniqueId);
  	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
  
  	statusType = b3GetStatusType(statusHandle);
  	if (statusType != CMD_REQUEST_COLLISION_INFO_COMPLETED)
  	{
  		return false;
  	}
  	if (b3GetStatusAABB(statusHandle, linkIndex, aabbMin, aabbMax))
  	{
  		return true;
  	}
  	return false;
  }
  
  bool b3RobotSimulatorClientAPI_NoDirect::getAABB(int bodyUniqueId, int linkIndex, btVector3& aabbMin, btVector3& aabbMax)
  {
  	double daabbMin[3];
  	double daabbMax[3];
  
  	bool status = getAABB(bodyUniqueId, linkIndex, &daabbMin[0], &daabbMax[0]);
  
  	aabbMin[0] = (float)daabbMin[0];
  	aabbMin[1] = (float)daabbMin[1];
  	aabbMin[2] = (float)daabbMin[2];
  
  	aabbMax[0] = (float)daabbMax[0];
  	aabbMax[1] = (float)daabbMax[1];
  	aabbMax[2] = (float)daabbMax[2];
  
  	return status;
  }
  
  int b3RobotSimulatorClientAPI_NoDirect::createCollisionShape(int shapeType, struct b3RobotSimulatorCreateCollisionShapeArgs& args)
  {
  	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
  	if (sm == 0)
  	{
  		b3Warning("Not connected");
  		return false;
  	}
  	b3SharedMemoryCommandHandle command;
  	b3SharedMemoryStatusHandle statusHandle;
  	int statusType;
  	int shapeIndex = -1;
  
  	command = b3CreateCollisionShapeCommandInit(sm);
  
  	if (shapeType == GEOM_SPHERE && args.m_radius > 0)
  	{
  		shapeIndex = b3CreateCollisionShapeAddSphere(command, args.m_radius);
  	}
  	if (shapeType == GEOM_BOX)
  	{
  		double halfExtents[3];
  		scalarToDouble3(args.m_halfExtents, halfExtents);
  		shapeIndex = b3CreateCollisionShapeAddBox(command, halfExtents);
  	}
  	if (shapeType == GEOM_CAPSULE && args.m_radius > 0 && args.m_height >= 0)
  	{
  		shapeIndex = b3CreateCollisionShapeAddCapsule(command, args.m_radius, args.m_height);
  	}
  	if (shapeType == GEOM_CYLINDER && args.m_radius > 0 && args.m_height >= 0)
  	{
  		shapeIndex = b3CreateCollisionShapeAddCylinder(command, args.m_radius, args.m_height);
  	}
  	if (shapeType == GEOM_MESH && args.m_fileName)
  	{
  		double meshScale[3];
  		scalarToDouble3(args.m_meshScale, meshScale);
  		shapeIndex = b3CreateCollisionShapeAddMesh(command, args.m_fileName, meshScale);
  	}
  	if (shapeType == GEOM_PLANE)
  	{
  		double planeConstant = 0;
  		double planeNormal[3];
  		scalarToDouble3(args.m_planeNormal, planeNormal);
  		shapeIndex = b3CreateCollisionShapeAddPlane(command, planeNormal, planeConstant);
  	}
  	if (shapeIndex >= 0 && args.m_flags)
  	{
  		b3CreateCollisionSetFlag(command, shapeIndex, args.m_flags);
  	}
  
  	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
  	statusType = b3GetStatusType(statusHandle);
  	if (statusType == CMD_CREATE_COLLISION_SHAPE_COMPLETED)
  	{
  		int uid = b3GetStatusCollisionShapeUniqueId(statusHandle);
  		return uid;
  	}
  	return -1;
  }
  
  int b3RobotSimulatorClientAPI_NoDirect::createMultiBody(struct b3RobotSimulatorCreateMultiBodyArgs& args)
  {
  	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
  	if (sm == 0)
  	{
  		b3Warning("Not connected");
  		return false;
  	}
  	b3SharedMemoryCommandHandle command;
  	b3SharedMemoryStatusHandle statusHandle;
  	int statusType, baseIndex;
  
  	double doubleBasePosition[3];
  	double doubleBaseInertialFramePosition[3];
  	scalarToDouble3(args.m_basePosition.m_floats, doubleBasePosition);
  	scalarToDouble3(args.m_baseInertialFramePosition.m_floats, doubleBaseInertialFramePosition);
  
  	double doubleBaseOrientation[4];
  	double doubleBaseInertialFrameOrientation[4];
  	scalarToDouble4(args.m_baseOrientation, doubleBaseOrientation);
  	scalarToDouble4(args.m_baseInertialFrameOrientation, doubleBaseInertialFrameOrientation);
  
  	command = b3CreateMultiBodyCommandInit(sm);
  
  	if (args.m_useMaximalCoordinates)
  	{
  		b3CreateMultiBodyUseMaximalCoordinates(command);
  	}
  	if (args.m_batchPositions.size())
  	{
  		btAlignedObjectArray<double> positionArray;
  		for (int i = 0; i < args.m_batchPositions.size(); i++)
  		{
  			positionArray.push_back(args.m_batchPositions[i][0]);
  			positionArray.push_back(args.m_batchPositions[i][1]);
  			positionArray.push_back(args.m_batchPositions[i][2]);
  		}
  		b3CreateMultiBodySetBatchPositions(sm, command, &positionArray[0], args.m_batchPositions.size());
  	}
  	baseIndex = b3CreateMultiBodyBase(command, args.m_baseMass, args.m_baseCollisionShapeIndex, args.m_baseVisualShapeIndex,
  									  doubleBasePosition, doubleBaseOrientation, doubleBaseInertialFramePosition, doubleBaseInertialFrameOrientation);
  
  	for (int i = 0; i < args.m_numLinks; i++)
  	{
  		double linkMass = args.m_linkMasses[i];
  		int linkCollisionShapeIndex = args.m_linkCollisionShapeIndices[i];
  		int linkVisualShapeIndex = args.m_linkVisualShapeIndices[i];
  		btVector3 linkPosition = args.m_linkPositions[i];
  		btQuaternion linkOrientation = args.m_linkOrientations[i];
  		btVector3 linkInertialFramePosition = args.m_linkInertialFramePositions[i];
  		btQuaternion linkInertialFrameOrientation = args.m_linkInertialFrameOrientations[i];
  		int linkParentIndex = args.m_linkParentIndices[i];
  		int linkJointType = args.m_linkJointTypes[i];
  		btVector3 linkJointAxis = args.m_linkJointAxes[i];
  
  		double doubleLinkPosition[3];
  		double doubleLinkInertialFramePosition[3];
  		double doubleLinkJointAxis[3];
  		scalarToDouble3(linkPosition.m_floats, doubleLinkPosition);
  		scalarToDouble3(linkInertialFramePosition.m_floats, doubleLinkInertialFramePosition);
  		scalarToDouble3(linkJointAxis.m_floats, doubleLinkJointAxis);
  
  		double doubleLinkOrientation[4];
  		double doubleLinkInertialFrameOrientation[4];
  		scalarToDouble4(linkOrientation, doubleLinkOrientation);
  		scalarToDouble4(linkInertialFrameOrientation, doubleLinkInertialFrameOrientation);
  
  		b3CreateMultiBodyLink(command,
  							  linkMass,
  							  linkCollisionShapeIndex,
  							  linkVisualShapeIndex,
  							  doubleLinkPosition,
  							  doubleLinkOrientation,
  							  doubleLinkInertialFramePosition,
  							  doubleLinkInertialFrameOrientation,
  							  linkParentIndex,
  							  linkJointType,
  							  doubleLinkJointAxis);
  	}
  
  	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
  	statusType = b3GetStatusType(statusHandle);
  	if (statusType == CMD_CREATE_MULTI_BODY_COMPLETED)
  	{
  		int uid = b3GetStatusBodyIndex(statusHandle);
  		return uid;
  	}
  	return -1;
  }
  
  int b3RobotSimulatorClientAPI_NoDirect::getNumConstraints() const
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return -1;
  	}
  	return b3GetNumUserConstraints(m_data->m_physicsClientHandle);
  }
  
  int b3RobotSimulatorClientAPI_NoDirect::getConstraintUniqueId(int serialIndex)
  {
  	if (!isConnected())
  	{
  		b3Warning("Not connected");
  		return -1;
  	}
  	int userConstraintId = -1;
  	userConstraintId = b3GetUserConstraintId(m_data->m_physicsClientHandle, serialIndex);
  	return userConstraintId;
  }
  
  void b3RobotSimulatorClientAPI_NoDirect::setGuiHelper(struct GUIHelperInterface* guiHelper)
  {
  	m_data->m_guiHelper = guiHelper;
  }
  
  struct GUIHelperInterface* b3RobotSimulatorClientAPI_NoDirect::getGuiHelper()
  {
  	return m_data->m_guiHelper;
  }
  
  void b3RobotSimulatorClientAPI_NoDirect::setInternalData(struct b3RobotSimulatorClientAPI_InternalData* data)
  {
  	*m_data = *data;
  }
  
  bool b3RobotSimulatorClientAPI_NoDirect::getCollisionShapeData(int bodyUniqueId, int linkIndex,
  															   b3CollisionShapeInformation& collisionShapeInfo)
  {
  	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
  	if (sm == 0)
  	{
  		b3Warning("Not connected");
  		return false;
  	}
  	b3SharedMemoryCommandHandle command;
  	b3SharedMemoryStatusHandle statusHandle;
  	int statusType;
  
  	{
  		command = b3InitRequestCollisionShapeInformation(sm, bodyUniqueId, linkIndex);
  		statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
  		statusType = b3GetStatusType(statusHandle);
  	}
  
  	btAssert(statusType == CMD_COLLISION_SHAPE_INFO_COMPLETED);
  	if (statusType == CMD_COLLISION_SHAPE_INFO_COMPLETED)
  	{
  		b3GetCollisionShapeInformation(sm, &collisionShapeInfo);
  	}
  	return true;
  }
  
  int b3RobotSimulatorClientAPI_NoDirect::saveStateToMemory()
  {
  	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
  	if (sm == 0)
  	{
  		b3Warning("Not connected");
  		return false;
  	}
  
  	b3SharedMemoryStatusHandle statusHandle;
  	int statusType;
  	b3SharedMemoryCommandHandle command;
  
  	int stateId = -1;
  
  	if (sm == 0)
  	{
  		return -1;
  	}
  
  	command = b3SaveStateCommandInit(sm);
  	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
  	statusType = b3GetStatusType(statusHandle);
  
  	if (statusType != CMD_SAVE_STATE_COMPLETED)
  	{
  		return -1;
  	}
  
  	stateId = b3GetStatusGetStateId(statusHandle);
  	return stateId;
  }
  
  void b3RobotSimulatorClientAPI_NoDirect::restoreStateFromMemory(int stateId)
  {
  	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
  	if (sm == 0)
  	{
  		b3Warning("Not connected");
  		return;
  	}
  
  	int statusType;
  	b3SharedMemoryCommandHandle command;
  	b3SharedMemoryStatusHandle statusHandle;
  	int physicsClientId = 0;
  
  	command = b3LoadStateCommandInit(sm);
  	if (stateId >= 0)
  	{
  		b3LoadStateSetStateId(command, stateId);
  	}
  	//	if (fileName)
  	//	{
  	//		b3LoadStateSetFileName(command, fileName);
  	//	}
  
  	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
  	statusType = b3GetStatusType(statusHandle);
  }
  
  bool b3RobotSimulatorClientAPI_NoDirect::getVisualShapeData(int bodyUniqueId, b3VisualShapeInformation& visualShapeInfo)
  {
  	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
  	if (sm == 0)
  	{
  		b3Warning("Not connected");
  		return false;
  	}
  	b3SharedMemoryCommandHandle commandHandle;
  	b3SharedMemoryStatusHandle statusHandle;
  	int statusType;
  
  	commandHandle = b3InitRequestVisualShapeInformation(sm, bodyUniqueId);
  	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
  	statusType = b3GetStatusType(statusHandle);
  
  	btAssert(statusType == CMD_VISUAL_SHAPE_INFO_COMPLETED);
  	if (statusType == CMD_VISUAL_SHAPE_INFO_COMPLETED)
  	{
  		b3GetVisualShapeInformation(sm, &visualShapeInfo);
  		return true;
  	}
  	return false;
  }
  
  void b3RobotSimulatorClientAPI_NoDirect::setAdditionalSearchPath(const std::string& path)
  {
  	int physicsClientId = 0;
  	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
  	if (sm == 0)
  	{
  		b3Warning("Not connected");
  		return;
  	}
  	if (path.length())
  	{
  		b3SharedMemoryCommandHandle commandHandle;
  		b3SharedMemoryStatusHandle statusHandle;
  		commandHandle = b3SetAdditionalSearchPath(sm, path.c_str());
  		statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
  	}
  }
  
  void b3RobotSimulatorClientAPI_NoDirect::setCollisionFilterGroupMask(int bodyUniqueIdA, int linkIndexA, int collisionFilterGroup, int collisionFilterMask)
  {
      int physicsClientId = 0;
      b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
      if (sm == 0)
      {
          b3Warning("Not connected");
          return;
      }
      
      b3SharedMemoryCommandHandle commandHandle;
      b3SharedMemoryStatusHandle statusHandle;
      int statusType;
      
      commandHandle = b3CollisionFilterCommandInit(sm);
      b3SetCollisionFilterGroupMask(commandHandle, bodyUniqueIdA, linkIndexA, collisionFilterGroup, collisionFilterMask);
      
      statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
      statusType = b3GetStatusType(statusHandle);
  }
  
  