examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp - master - redgpu/bullet

examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp
2 655 строк78 KB
Erwin Coumans
revert linkNames in createMultiBody, we cannot pass pointers pointing to local memory (storing the string) around into shared memory or over the network.
20 сен 2021, 22:22
20 сен 2021, 22:2287605ca
#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());
	b3LoadMJCFCommandSetFlags(command, URDF_USE_IMPLICIT_CYLINDER);
	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, const btVector3& basePosition, const 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;
}

int b3RobotSimulatorClientAPI_NoDirect::computeDofCount(int bodyUniqueId) const
{
	if (!isConnected())
	{
		b3Warning("Not connected");
		return 0;
	}
	return b3ComputeDofCount(m_data->m_physicsClientHandle, bodyUniqueId);
}

int b3RobotSimulatorClientAPI_NoDirect::calculateMassMatrix(int bodyUniqueId, const double* jointPositions, int numJointPositions, double* massMatrix, int flags)
{
	if (!isConnected())
	{
		b3Warning("Not connected");
		return 0;
	}

	b3SharedMemoryStatusHandle statusHandle;
	int statusType;
	b3SharedMemoryCommandHandle commandHandle =
		b3CalculateMassMatrixCommandInit(m_data->m_physicsClientHandle, bodyUniqueId, jointPositions, numJointPositions);
	b3CalculateMassMatrixSetFlags(commandHandle, flags);
	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
	statusType = b3GetStatusType(statusHandle);
	if (statusType == CMD_CALCULATED_MASS_MATRIX_COMPLETED)
	{
		int dofCount;
		b3GetStatusMassMatrix(m_data->m_physicsClientHandle, statusHandle, &dofCount, NULL);
		if (dofCount)
		{
			b3GetStatusMassMatrix(m_data->m_physicsClientHandle, statusHandle, NULL, massMatrix);
			return dofCount;
		}
	}
	
	return 0;
}

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(const 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(const 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::createVisualShape(int shapeType, struct b3RobotSimulatorCreateVisualShapeArgs& 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 = b3CreateVisualShapeCommandInit(sm);

	if (shapeType == GEOM_SPHERE && args.m_radius > 0)
	{
		shapeIndex = b3CreateVisualShapeAddSphere(command, args.m_radius);
	}
	if (shapeType == GEOM_BOX)
	{
		double halfExtents[3];
		scalarToDouble3(args.m_halfExtents, halfExtents);
		shapeIndex = b3CreateVisualShapeAddBox(command, halfExtents);
	}
	if (shapeType == GEOM_CAPSULE && args.m_radius > 0 && args.m_height >= 0)
	{
		shapeIndex = b3CreateVisualShapeAddCapsule(command, args.m_radius, args.m_height);
	}
	if (shapeType == GEOM_CYLINDER && args.m_radius > 0 && args.m_height >= 0)
	{
		shapeIndex = b3CreateVisualShapeAddCylinder(command, args.m_radius, args.m_height);
	}
	if (shapeType == GEOM_MESH && args.m_fileName)
	{
		double meshScale[3];
		scalarToDouble3(args.m_meshScale, meshScale);
		shapeIndex = b3CreateVisualShapeAddMesh(command, args.m_fileName, meshScale);
	}
	if (shapeType == GEOM_PLANE)
	{
		double planeConstant = 0;
		double planeNormal[3];
		scalarToDouble3(args.m_planeNormal, planeNormal);
		shapeIndex = b3CreateVisualShapeAddPlane(command, planeNormal, planeConstant);
	}
	if (shapeIndex >= 0 && args.m_flags)
	{
		b3CreateVisualSetFlag(command, shapeIndex, args.m_flags);
	}

	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
	statusType = b3GetStatusType(statusHandle);
	if (statusType == CMD_CREATE_VISUAL_SHAPE_COMPLETED)
	{
		int uid = b3GetStatusVisualShapeUniqueId(statusHandle);
		return uid;
	}
	return -1;
}

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_HEIGHTFIELD)
	{
		double meshScale[3];
		scalarToDouble3(args.m_meshScale, meshScale);
		if (args.m_fileName)
		{
			shapeIndex = b3CreateCollisionShapeAddHeightfield(command, args.m_fileName, meshScale, args.m_heightfieldTextureScaling);
		}
		else
		{
			if (args.m_heightfieldData.size() && args.m_numHeightfieldRows>0 && args.m_numHeightfieldColumns>0)
			{
				shapeIndex = b3CreateCollisionShapeAddHeightfield2(sm, command, meshScale, args.m_heightfieldTextureScaling,
					&args.m_heightfieldData[0],
					args.m_numHeightfieldRows,
					args.m_numHeightfieldColumns,
					args.m_replaceHeightfieldIndex);
			}
		}
		

	}

	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);
}

void b3RobotSimulatorClientAPI_NoDirect::removeState(int stateUniqueId)
{
	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
	if (sm == 0)
	{
		b3Warning("Not connected");
		return;
	}

	if (stateUniqueId >= 0)
	{
		b3SharedMemoryStatusHandle statusHandle;
		int statusType;
		if (b3CanSubmitCommand(sm))
		{
			statusHandle = b3SubmitClientCommandAndWaitStatus(sm, b3InitRemoveStateCommand(sm, stateUniqueId));
			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);
}