bullet3

#ifdef BT_ENABLE_PHYSX
#include "PhysXServerCommandProcessor.h"

#include "../../Utils/ChromeTraceUtil.h"

#include <stdio.h>
#include "../SharedMemoryCommands.h"
#include "LinearMath/btQuickprof.h"
#include "Bullet3Common/b3AlignedObjectArray.h"
#include "LinearMath/btMinMax.h"
#include "Bullet3Common/b3FileUtils.h"
#include "Bullet3Common/b3CommandLineArgs.h"
#include "../../Utils/b3ResourcePath.h"
#include "Bullet3Common/b3ResizablePool.h"
#include "PxPhysicsAPI.h"
#include "../Utils/b3BulletDefaultFileIO.h"
#include "PhysXUrdfImporter.h"
#include "PxTolerancesScale.h"
#include "PxDefaultCpuDispatcher.h"
#include "PxDefaultSimulationFilterShader.h"
#include "URDF2PhysX.h"
#include "../b3PluginManager.h"
#include "PxRigidActorExt.h"
#include "LinearMath/btThreads.h"

#define STATIC_EGLRENDERER_PLUGIN
#ifdef STATIC_EGLRENDERER_PLUGIN
#include "../plugins/eglPlugin/eglRendererPlugin.h"
#endif  //STATIC_EGLRENDERER_PLUGIN

//for serialization of data to client
#include "BulletDynamics/Featherstone/btMultiBody.h"
#include "../Extras/Serialize/BulletFileLoader/btBulletFile.h"
#include "LinearMath/btSerializer.h"
#include "PhysXUserData.h"

class MyPhysXErrorCallback : public physx::PxErrorCallback
{
public:
	MyPhysXErrorCallback()
	{
	}
	~MyPhysXErrorCallback()
	{
	}

	virtual void reportError(physx::PxErrorCode::Enum code, const char* message, const char* file, int line)
	{
		b3Printf("%s in file:%s line:%d\n", message, file, line);
	}
};

class CustomProfilerCallback : public physx::PxProfilerCallback
{
public:
	virtual ~CustomProfilerCallback() {}

	virtual void* zoneStart(const char* eventName, bool detached, uint64_t contextId)
	{
		b3EnterProfileZone(eventName);
		return 0;
	}

	virtual void zoneEnd(void* profilerData, const char* eventName, bool detached, uint64_t contextId)
	{
		b3LeaveProfileZone();
	}

};

static CustomProfilerCallback gCustomProfilerCallback;


struct InternalPhysXBodyData
{
	physx::PxArticulationReducedCoordinate*		mArticulation;
	physx::PxRigidDynamic* m_rigidDynamic;
	physx::PxRigidStatic* m_rigidStatic;

	std::string m_bodyName;

	InternalPhysXBodyData()
	{
		clear();
	}
	//physx::PxArticulationJointReducedCoordinate*	gDriveJoint;
	void clear()
	{
		mArticulation = 0;
		m_rigidDynamic = 0;
		m_rigidStatic = 0;

		m_bodyName = "";
	}
};



typedef b3PoolBodyHandle<InternalPhysXBodyData> InternalPhysXBodyHandle;



struct PhysXServerCommandProcessorInternalData : public physx::PxSimulationEventCallback, public physx::PxContactModifyCallback
{
	bool m_isConnected;
	bool m_verboseOutput;
	double m_physicsDeltaTime;
	int m_numSimulationSubSteps;
	btSpinMutex m_taskLock;

	btAlignedObjectArray<b3ContactPointData> m_contactPoints;
	
	

	void onContactModify(physx::PxContactModifyPair* const pairs, physx::PxU32 count)
	{
		for (physx::PxU32 i = 0; i<count; i++)
		{
			//...
		}
	}

	void onContact(const physx::PxContactPairHeader& pairHeader, const physx::PxContactPair* pairs, physx::PxU32 nbPairs)
	{
		B3_PROFILE("onContact");
		//todo: are there really multiple threads calling 'onContact'?
		m_taskLock.lock();

		btAlignedObjectArray<physx::PxContactPairPoint> contacts;
		for (physx::PxU32 i = 0; i < nbPairs; i++)
		{
			physx::PxU32 contactCount = pairs[i].contactCount;
			if (contactCount)
			{
				contacts.resize(contactCount);
				pairs[i].extractContacts(&contacts[0], contactCount);
				for (physx::PxU32 j = 0; j < contactCount; j++)
				{
					const physx::PxContactPairPoint& contact = contacts[i];
					b3ContactPointData srcPt;
					MyPhysXUserData* udA = (MyPhysXUserData*)pairHeader.actors[0]->userData;
					MyPhysXUserData* udB = (MyPhysXUserData*)pairHeader.actors[1]->userData;
					srcPt.m_bodyUniqueIdA = udA->m_bodyUniqueId;
					srcPt.m_linkIndexA = udA->m_linkIndex;
					srcPt.m_bodyUniqueIdB = udB->m_bodyUniqueId;
					srcPt.m_linkIndexB = udB->m_linkIndex;
					srcPt.m_positionOnAInWS[0] = contact.position.x + contact.separation*contact.normal.x;
					srcPt.m_positionOnAInWS[1] = contact.position.y + contact.separation*contact.normal.y;
					srcPt.m_positionOnAInWS[2] = contact.position.z + contact.separation*contact.normal.z;
					srcPt.m_positionOnBInWS[0] = contact.position.x - contact.separation*contact.normal.x;
					srcPt.m_positionOnBInWS[1] = contact.position.y - contact.separation*contact.normal.y;
					srcPt.m_positionOnBInWS[2] = contact.position.z - contact.separation*contact.normal.z;
					srcPt.m_contactNormalOnBInWS[0] = contact.normal.x;
					srcPt.m_contactNormalOnBInWS[1] = contact.normal.y;
					srcPt.m_contactNormalOnBInWS[2] = contact.normal.z;
					srcPt.m_contactDistance = contact.separation;
					srcPt.m_contactFlags = 0;
					srcPt.m_linearFrictionDirection1[0] = 0;
					srcPt.m_linearFrictionDirection1[1] = 0;
					srcPt.m_linearFrictionDirection1[2] = 0;
					srcPt.m_linearFrictionDirection2[0] = 0;
					srcPt.m_linearFrictionDirection2[1] = 0;
					srcPt.m_linearFrictionDirection2[2] = 0;
					
					srcPt.m_linearFrictionForce2 = 0;
					
					srcPt.m_normalForce = contact.impulse.dot(contact.normal);
					//compute friction direction from impulse projected in contact plane using contact normal.
					physx::PxVec3 fric = contact.impulse - contact.normal*srcPt.m_normalForce;
					double fricForce = fric.normalizeSafe();
					if (fricForce)
					{
						srcPt.m_linearFrictionDirection1[0] = fric.x;
						srcPt.m_linearFrictionDirection1[1] = fric.y;
						srcPt.m_linearFrictionDirection1[2] = fric.z;
						srcPt.m_linearFrictionForce1 = fricForce;
					}
					m_contactPoints.push_back(srcPt);
					// std::cout << "Contact: bw " << pairHeader.actors[0]->getName() << " and " << pairHeader.actors[1]->getName() << " | " << contacts[j].position.x << "," << contacts[j].position.y << ","
					// 		  << contacts[j].position.z << std::endl;
				}
			}
		}
		m_taskLock.unlock();
	}

	void onConstraintBreak(physx::PxConstraintInfo* constraints, physx::PxU32 count)
	{
		PX_UNUSED(constraints);
		PX_UNUSED(count);
	}

	void onWake(physx::PxActor** actors, physx::PxU32 count)
	{
		PX_UNUSED(actors);
		PX_UNUSED(count);
	}

	void onSleep(physx::PxActor** actors, physx::PxU32 count)
	{
		PX_UNUSED(actors);
		PX_UNUSED(count);
	}

	void onTrigger(physx::PxTriggerPair* pairs, physx::PxU32 count)
	{
		PX_UNUSED(pairs);
		PX_UNUSED(count);
	}

	void onAdvance(const physx::PxRigidBody* const*, const physx::PxTransform*, const physx::PxU32) 
	{
	}
	

	b3PluginManager m_pluginManager;


	physx::PxDefaultAllocator		m_allocator;
	MyPhysXErrorCallback	m_errorCallback;
	physx::PxFoundation*			m_foundation;
	physx::PxPhysics*				m_physics;
	physx::PxCooking*		m_cooking;
	physx::PxDefaultCpuDispatcher*	m_dispatcher;
	physx::PxScene*				m_scene;
	physx::PxMaterial*				m_material;
	//physx::PxPvd*                  m_pvd;

	b3ResizablePool<InternalPhysXBodyHandle> m_bodyHandles;
	



	b3AlignedObjectArray<int> m_mjcfRecentLoadedBodies;

	int m_profileTimingLoggingUid;
	int m_stateLoggersUniqueId;
	std::string m_profileTimingFileName;
	b3CommandLineArgs	m_commandLineArgs;
	int m_userDebugParametersUid;
	btHashMap<btHashInt, double> m_userDebugParameters;
	btAlignedObjectArray<int> m_graphicsIndexToSegmentationMask;

	PhysXServerCommandProcessorInternalData(PhysXServerCommandProcessor* sdk, int argc, char* argv[])
		: m_isConnected(false),
		  m_verboseOutput(false),
		  m_physicsDeltaTime(1. / 240.),
		  m_numSimulationSubSteps(0),
		m_pluginManager(sdk),
		m_profileTimingLoggingUid(-1),
		m_stateLoggersUniqueId(1),
		m_commandLineArgs(argc,argv),
		m_userDebugParametersUid(0)
	{

		m_foundation = NULL;
		m_physics = NULL;
		m_cooking = NULL;
		m_dispatcher = NULL;
		m_scene = NULL;
		m_material = NULL;
		//m_pvd = NULL;

#ifdef STATIC_EGLRENDERER_PLUGIN
		{
			bool initPlugin = false;
			b3PluginFunctions funcs(initPlugin_eglRendererPlugin, exitPlugin_eglRendererPlugin, executePluginCommand_eglRendererPlugin);
			funcs.m_getRendererFunc = getRenderInterface_eglRendererPlugin;
			int renderPluginId = m_pluginManager.registerStaticLinkedPlugin("eglRendererPlugin", funcs, initPlugin);
			m_pluginManager.selectPluginRenderer(renderPluginId);
		}
#endif  //STATIC_EGLRENDERER_PLUGIN
	}
};

PhysXServerCommandProcessor::PhysXServerCommandProcessor(int argc, char* argv[])
{

	m_data = new PhysXServerCommandProcessorInternalData(this, argc, argv);
}

PhysXServerCommandProcessor::~PhysXServerCommandProcessor()
{
	delete m_data;
}



physx::PxFilterFlags MyPhysXFilter(physx::PxFilterObjectAttributes attributes0, physx::PxFilterData filterData0,
	physx::PxFilterObjectAttributes attributes1, physx::PxFilterData filterData1,
	physx::PxPairFlags& pairFlags, const void* constantBlock, physx::PxU32 constantBlockSize)
{
	PX_UNUSED(attributes0);
	PX_UNUSED(attributes1);
	PX_UNUSED(constantBlock);
	PX_UNUSED(constantBlockSize);
	// if (filterData0.word2 != 0 && filterData0.word2 == filterData1.word2)
	// 	return physx::PxFilterFlag::eKILL;
	pairFlags |= physx::PxPairFlag::eCONTACT_DEFAULT | physx::PxPairFlag::eNOTIFY_TOUCH_FOUND
		| physx::PxPairFlag::eDETECT_DISCRETE_CONTACT | physx::PxPairFlag::eNOTIFY_CONTACT_POINTS | physx::PxPairFlag::eMODIFY_CONTACTS;
	return physx::PxFilterFlag::eDEFAULT;
}



bool PhysXServerCommandProcessor::connect()
{
	if (m_data->m_isConnected)
	{
		printf("already connected\n");
		return true;
	}

	int result = 0;
	{
		
		m_data->m_foundation = PxCreateFoundation(PX_PHYSICS_VERSION, m_data->m_allocator, m_data->m_errorCallback);
		// This call should be performed after PVD is initialized, otherwise it will have no effect.
		PxSetProfilerCallback(&gCustomProfilerCallback);

		m_data->m_physics = PxCreatePhysics(PX_PHYSICS_VERSION, *m_data->m_foundation, physx::PxTolerancesScale(), true, 0);
		m_data->m_cooking = PxCreateCooking(PX_PHYSICS_VERSION, *m_data->m_foundation, physx::PxCookingParams(physx::PxTolerancesScale()));

		
		physx::PxU32 numCores = 1;
		m_data->m_commandLineArgs.GetCmdLineArgument("numCores", numCores);
		printf("PhysX numCores=%d\n", numCores);
		m_data->m_dispatcher = physx::PxDefaultCpuDispatcherCreate(numCores == 0 ? 0 : numCores - 1);
		
		physx::PxSceneDesc sceneDesc(m_data->m_physics->getTolerancesScale());
		sceneDesc.gravity = physx::PxVec3(0.0f, -9.81f, 0.0f);

		
		sceneDesc.solverType = physx::PxSolverType::ePGS;
		std::string solver;
		m_data->m_commandLineArgs.GetCmdLineArgument("solver", solver);
		
		if (solver=="tgs")
		{
			sceneDesc.solverType = physx::PxSolverType::eTGS;
			printf("PhysX using TGS\n");
		}
		else
		{
			printf("PhysX using PGS\n");
		}
		
		sceneDesc.cpuDispatcher = m_data->m_dispatcher;
		
		//todo: add some boolean, to allow enable/disable of this contact filtering
		bool enableContactCallback = false;
		m_data->m_commandLineArgs.GetCmdLineArgument("enableContactCallback", enableContactCallback);

		if (enableContactCallback)
		{
			sceneDesc.filterShader = MyPhysXFilter;
			sceneDesc.simulationEventCallback = this->m_data;
			sceneDesc.contactModifyCallback = this->m_data;
			printf("PhysX enableContactCallback\n");
		}
		else
		{
			sceneDesc.filterShader = physx::PxDefaultSimulationFilterShader;
		}
		
		
		
		
		m_data->m_scene = m_data->m_physics->createScene(sceneDesc);

		m_data->m_material = m_data->m_physics->createMaterial(0.5f, 0.5f, 0.f);

		PxInitExtensions(*m_data->m_physics, 0);


		//PxRigidStatic* groundPlane = PxCreatePlane(*gPhysics, PxPlane(0, 1, 0, 0), *gMaterial);
		//gScene->addActor(*groundPlane);

		result = 1;
	}
	if (result == 1)
	{
		m_data->m_isConnected = true;
		return true;
	}

	return false;
}

void PhysXServerCommandProcessor::resetSimulation()
{
	//gArticulation->release();
	m_data->m_scene->release();
	m_data->m_dispatcher->release();
	m_data->m_cooking->release();
	m_data->m_physics->release();
	//PxPvdTransport* transport = gPvd->getTransport();
	//gPvd->release();
	//transport->release();
	PxCloseExtensions();
	
	m_data->m_foundation->release();
}

void PhysXServerCommandProcessor::disconnect()
{
	resetSimulation();

	m_data->m_isConnected = false;
}

bool PhysXServerCommandProcessor::isConnected() const
{
	return m_data->m_isConnected;
}


bool PhysXServerCommandProcessor::processCustomCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
	bool hasStatus = true;

	SharedMemoryStatus& serverCmd = serverStatusOut;
	serverCmd.m_type = CMD_CUSTOM_COMMAND_FAILED;
	serverCmd.m_customCommandResultArgs.m_pluginUniqueId = -1;

	if (clientCmd.m_updateFlags & CMD_CUSTOM_COMMAND_LOAD_PLUGIN)
	{
		//pluginPath could be registered or load from disk
		const char* postFix = "";
		if (clientCmd.m_updateFlags & CMD_CUSTOM_COMMAND_LOAD_PLUGIN_POSTFIX)
		{
			postFix = clientCmd.m_customCommandArgs.m_postFix;
		}

		int pluginUniqueId = m_data->m_pluginManager.loadPlugin(clientCmd.m_customCommandArgs.m_pluginPath, postFix);
		if (pluginUniqueId >= 0)
		{
			serverCmd.m_customCommandResultArgs.m_pluginUniqueId = pluginUniqueId;
			serverCmd.m_type = CMD_CUSTOM_COMMAND_COMPLETED;
		}
	}
	if (clientCmd.m_updateFlags & CMD_CUSTOM_COMMAND_UNLOAD_PLUGIN)
	{
		m_data->m_pluginManager.unloadPlugin(clientCmd.m_customCommandArgs.m_pluginUniqueId);
		serverCmd.m_type = CMD_CUSTOM_COMMAND_COMPLETED;
	}
	if (clientCmd.m_updateFlags & CMD_CUSTOM_COMMAND_EXECUTE_PLUGIN_COMMAND)
	{
		int result = m_data->m_pluginManager.executePluginCommand(clientCmd.m_customCommandArgs.m_pluginUniqueId, &clientCmd.m_customCommandArgs.m_arguments);
		serverCmd.m_customCommandResultArgs.m_executeCommandResult = result;
		serverCmd.m_type = CMD_CUSTOM_COMMAND_COMPLETED;
	}
	return hasStatus;
}


bool PhysXServerCommandProcessor::processStateLoggingCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
	BT_PROFILE("CMD_STATE_LOGGING");

	serverStatusOut.m_type = CMD_STATE_LOGGING_FAILED;
	bool hasStatus = true;

	if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_PROFILE_TIMINGS)
	{
		if (m_data->m_profileTimingLoggingUid < 0)
		{
			b3ChromeUtilsStartTimings();
			m_data->m_profileTimingFileName = clientCmd.m_stateLoggingArguments.m_fileName;
			int loggerUid = m_data->m_stateLoggersUniqueId++;
			serverStatusOut.m_type = CMD_STATE_LOGGING_START_COMPLETED;
			serverStatusOut.m_stateLoggingResultArgs.m_loggingUniqueId = loggerUid;
			m_data->m_profileTimingLoggingUid = loggerUid;
		}
	}

	if ((clientCmd.m_updateFlags & STATE_LOGGING_STOP_LOG) && clientCmd.m_stateLoggingArguments.m_loggingUniqueId >= 0)
	{
		if (clientCmd.m_stateLoggingArguments.m_loggingUniqueId == m_data->m_profileTimingLoggingUid)
		{
			serverStatusOut.m_type = CMD_STATE_LOGGING_COMPLETED;
			b3ChromeUtilsStopTimingsAndWriteJsonFile(m_data->m_profileTimingFileName.c_str());
			m_data->m_profileTimingLoggingUid = -1;
		}
	}


	return hasStatus;
}


bool PhysXServerCommandProcessor::processInitPoseCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
	bool hasStatus = true;

	BT_PROFILE("CMD_INIT_POSE");

	if (m_data->m_verboseOutput)
	{
		b3Printf("Server Init Pose not implemented yet");
	}
	int bodyUniqueId = clientCmd.m_initPoseArgs.m_bodyUniqueId;
	InternalPhysXBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);

	btVector3 baseLinVel(0, 0, 0);
	btVector3 baseAngVel(0, 0, 0);

	if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_LINEAR_VELOCITY)
	{
		baseLinVel.setValue(clientCmd.m_initPoseArgs.m_initialStateQdot[0],
			clientCmd.m_initPoseArgs.m_initialStateQdot[1],
			clientCmd.m_initPoseArgs.m_initialStateQdot[2]);
	}
	if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_ANGULAR_VELOCITY)
	{
		baseAngVel.setValue(clientCmd.m_initPoseArgs.m_initialStateQdot[3],
			clientCmd.m_initPoseArgs.m_initialStateQdot[4],
			clientCmd.m_initPoseArgs.m_initialStateQdot[5]);
	}
	btVector3 basePos(0, 0, 0);
	if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION)
	{
		basePos = btVector3(
			clientCmd.m_initPoseArgs.m_initialStateQ[0],
			clientCmd.m_initPoseArgs.m_initialStateQ[1],
			clientCmd.m_initPoseArgs.m_initialStateQ[2]);
	}
	btQuaternion baseOrn(0, 0, 0, 1);
	if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_ORIENTATION)
	{
		baseOrn.setValue(clientCmd.m_initPoseArgs.m_initialStateQ[3],
			clientCmd.m_initPoseArgs.m_initialStateQ[4],
			clientCmd.m_initPoseArgs.m_initialStateQ[5],
			clientCmd.m_initPoseArgs.m_initialStateQ[6]);
	}
	if (body && body->mArticulation)
	{
		physx::PxArticulationCache* c = body->mArticulation->createCache();
		body->mArticulation->copyInternalStateToCache(*c, physx::PxArticulationCache::ePOSITION | physx::PxArticulationCache::eVELOCITY);// physx::PxArticulationCache::eALL);
		physx::PxArticulationLink* physxLinks[64];
		physx::PxU32 bufferSize = 64;
		physx::PxU32 startIndex = 0;
		int numLinks2 = body->mArticulation->getLinks(physxLinks, bufferSize, startIndex);

		btAlignedObjectArray<int> dofStarts;
		dofStarts.resize(numLinks2);
		dofStarts[0] = 0; //We know that the root link does not have a joint
		//cache positions in PhysX may be reshuffled, see
		//http://gameworksdocs.nvidia.com/PhysX/4.0/documentation/PhysXGuide/Manual/Articulations.html

		for (int  i = 1; i < numLinks2; ++i)
		{
			int  llIndex = physxLinks[i]->getLinkIndex();
			int  dofs = physxLinks[i]->getInboundJointDof();

			dofStarts[llIndex] = dofs;
		}

		int count = 0;
		for (int i = 1; i < numLinks2; ++i)
		{
			int  dofs = dofStarts[i];
			dofStarts[i] = count;
			count += dofs;
		}

		if (numLinks2 > 0)
		{
			int dofs = physxLinks[0]->getInboundJointDof();
			physx::PxTransform pt = physxLinks[0]->getGlobalPose();
			physx::PxVec3 linVel = physxLinks[0]->getLinearVelocity();
			physx::PxVec3 angVel = physxLinks[0]->getAngularVelocity();
			

			if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_LINEAR_VELOCITY)
			{
				physxLinks[0]->setLinearVelocity(physx::PxVec3(baseLinVel[0], baseLinVel[1], baseLinVel[2]));
			}

			if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_ANGULAR_VELOCITY)
			{
				physxLinks[0]->setAngularVelocity(physx::PxVec3(baseAngVel[0], baseAngVel[1], baseAngVel[2]));
			}

			if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION)
			{
				pt.p.x = basePos[0];
				pt.p.y = basePos[0];
				pt.p.z = basePos[0];
				physxLinks[0]->setGlobalPose(pt);
			}
			if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_ORIENTATION)
			{
				btAssert(clientCmd.m_initPoseArgs.m_hasInitialStateQ[3] &&
					clientCmd.m_initPoseArgs.m_hasInitialStateQ[4] &&
					clientCmd.m_initPoseArgs.m_hasInitialStateQ[5] &&
					clientCmd.m_initPoseArgs.m_hasInitialStateQ[6]);
				pt.q.x = baseOrn[0];
				pt.q.y = baseOrn[1];
				pt.q.z = baseOrn[2];
				pt.q.w = baseOrn[3];
				physxLinks[0]->setGlobalPose(pt);
			}
		}
		if (clientCmd.m_updateFlags & INIT_POSE_HAS_JOINT_STATE)
		{
			int uDofIndex = 6;
			int posVarCountIndex = 7;
			
			//skip 'root' link
			for (int i = 1; i < numLinks2; i++)
			{
				int physxCacheLinkIndex = physxLinks[i]->getLinkIndex();
				int dofs = physxLinks[i]->getInboundJointDof();
				int posVarCount = dofs;//??
				bool hasPosVar = posVarCount > 0;

				for (int j = 0; j < posVarCount; j++)
				{
					if (clientCmd.m_initPoseArgs.m_hasInitialStateQ[posVarCountIndex + j] == 0)
					{
						hasPosVar = false;
						break;
					}
				}
				if (hasPosVar)
				{
					if (posVarCount == 1)
					{
						c->jointPosition[dofStarts[physxCacheLinkIndex]] = clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex];
					}
					if (posVarCount == 3)
					{
						btQuaternion q(
							clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex],
							clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex + 1],
							clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex + 2],
							clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex + 3]);
						q.normalize();
						//mb->setJointPosMultiDof(i, &q[0]);
						//double vel[6] = { 0, 0, 0, 0, 0, 0 };
						//mb->setJointVelMultiDof(i, vel);
					}
				}

				bool hasVel = true;
				for (int j = 0; j < posVarCount; j++)
				{
					if (clientCmd.m_initPoseArgs.m_hasInitialStateQdot[uDofIndex + j] == 0)
					{
						hasVel = false;
						break;
					}
				}

				if (hasVel)
				{
					if (posVarCount == 1)
					{
						btScalar vel = clientCmd.m_initPoseArgs.m_initialStateQdot[uDofIndex];
						c->jointVelocity[dofStarts[physxCacheLinkIndex]] = vel;
					}
					if (posVarCount == 3)
					{
						//mb->setJointVelMultiDof(i, &clientCmd.m_initPoseArgs.m_initialStateQdot[uDofIndex]);
					}
				}
				
				posVarCountIndex += dofs;
				uDofIndex += dofs;// mb->getLink(i).m_dofCount;
			}
		}

		body->mArticulation->applyCache(*c, physx::PxArticulationCache::ePOSITION| physx::PxArticulationCache::eVELOCITY);
		body->mArticulation->releaseCache(*c);
		
	}

	SharedMemoryStatus& serverCmd = serverStatusOut;
	serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
	return hasStatus;
}

bool PhysXServerCommandProcessor::processSendDesiredStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
	bool hasStatus = true;
	BT_PROFILE("CMD_SEND_DESIRED_STATE");
	
	int bodyUniqueId = clientCmd.m_sendDesiredStateCommandArgument.m_bodyUniqueId;
	InternalPhysXBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
	if (body && body->mArticulation)
	{
		physx::PxArticulationLink* physxLinks[64];
		physx::PxU32 bufferSize = 64;
		physx::PxU32 startIndex = 0;
		int numLinks2 = body->mArticulation->getLinks(physxLinks, bufferSize, startIndex);
		//http://gameworksdocs.nvidia.com/PhysX/4.0/documentation/PhysXGuide/Manual/Articulations.html


		switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode)
		{
		case CONTROL_MODE_VELOCITY:
		{
			if (m_data->m_verboseOutput)
			{
				b3Printf("Using CONTROL_MODE_VELOCITY");
			}

			int numMotors = 0;
			//find the joint motors and apply the desired velocity and maximum force/torque
			{
				int dofIndex = 6;  //skip the 3 linear + 3 angular degree of freedom entries of the base
				for (int link = 1; link < numLinks2; link++)
				{
					int dofs = physxLinks[link]->getInboundJointDof();
					physx::PxReal stiffness = 10.f;
					physx::PxReal damping = 0.1f;
					physx::PxReal forceLimit = PX_MAX_F32;

					if (dofs == 1)
					{
						physx::PxArticulationJointReducedCoordinate* joint = static_cast<physx::PxArticulationJointReducedCoordinate*>(physxLinks[link]->getInboundJoint());
						btScalar desiredVelocity = 0.f;
						bool hasDesiredVelocity = false;
						physx::PxReal stiffness = 10.f;

						if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_QDOT) != 0)
						{
							desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex];
							btScalar kd = 0.1f;
							if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_KD) != 0)
							{
								kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[dofIndex];
							}
							joint->setDriveVelocity(physx::PxArticulationAxis::eTWIST, desiredVelocity);
							physx::PxReal damping = kd;
							stiffness = 0;
							joint->setDriveTarget(physx::PxArticulationAxis::eTWIST, 0.f);
							physx::PxReal forceLimit = 1000000.f;
							if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_MAX_FORCE) != 0)
							{
								forceLimit = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex];
							}
							joint->setDrive(physx::PxArticulationAxis::eTWIST, stiffness, damping, forceLimit);
						}
					}
					dofIndex += dofs;
				}
			}
			break;
		}

		case CONTROL_MODE_POSITION_VELOCITY_PD:
		{
			if (m_data->m_verboseOutput)
			{
				b3Printf("Using CONTROL_MODE_VELOCITY");
			}

			int numMotors = 0;
			//find the joint motors and apply the desired velocity and maximum force/torque
			{
				int dofIndex = 6;  //skip the 3 linear + 3 angular degree of freedom entries of the base
				int posIndex = 7;  //skip 3 positional and 4 orientation (quaternion) positional degrees of freedom of the base
				for (int link = 1; link < numLinks2; link++)
				{
					int dofs = physxLinks[link]->getInboundJointDof();
					physx::PxReal stiffness = 10.f;
					physx::PxReal damping = 0.1f;
					physx::PxReal forceLimit = PX_MAX_F32;
					

					if (dofs == 1)
					{
						physx::PxArticulationJointReducedCoordinate* joint = static_cast<physx::PxArticulationJointReducedCoordinate*>(physxLinks[link]->getInboundJoint());
						btScalar desiredVelocity = 0.f;
						bool hasDesiredVelocity = false;
						physx::PxReal stiffness = 10.f;
						btScalar kd = 0.1f;
						btScalar kp = 0.f;
						
						if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_QDOT) != 0)
						{
							desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex];
						}

						btScalar desiredPosition = 0.f;
						if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[posIndex] & SIM_DESIRED_STATE_HAS_Q) != 0)
						{
							desiredPosition = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex];


							if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_KD) != 0)
							{
								kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[dofIndex];
							}
							physx::PxReal damping = kd;

							if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_KP) != 0)
							{
								kp = clientCmd.m_sendDesiredStateCommandArgument.m_Kp[dofIndex];
								stiffness = kp;
							}

							joint->setDriveVelocity(physx::PxArticulationAxis::eTWIST, desiredVelocity);



							physx::PxReal forceLimit = 1000000.f;
							if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_MAX_FORCE) != 0)
							{
								forceLimit = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex];
							}
							bool isAcceleration = false;

							joint->setDriveTarget(physx::PxArticulationAxis::eTWIST, desiredPosition);
							joint->setDrive(physx::PxArticulationAxis::eTWIST, stiffness, damping, forceLimit, isAcceleration);
						}
					}

					dofIndex += dofs;
					posIndex += dofs;
				}
			}
			break;
		}
		default:
		{
		}
		}

	}
	serverStatusOut.m_type = CMD_DESIRED_STATE_RECEIVED_COMPLETED;
	return hasStatus;
}



bool PhysXServerCommandProcessor::processChangeDynamicsInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
	bool hasStatus = true;
	BT_PROFILE("CMD_CHANGE_DYNAMICS_INFO");

	int bodyUniqueId = clientCmd.m_changeDynamicsInfoArgs.m_bodyUniqueId;
	int linkIndex = clientCmd.m_changeDynamicsInfoArgs.m_linkIndex;
	double mass = clientCmd.m_changeDynamicsInfoArgs.m_mass;
	double lateralFriction = clientCmd.m_changeDynamicsInfoArgs.m_lateralFriction;
	double spinningFriction = clientCmd.m_changeDynamicsInfoArgs.m_spinningFriction;
	double rollingFriction = clientCmd.m_changeDynamicsInfoArgs.m_rollingFriction;
	double restitution = clientCmd.m_changeDynamicsInfoArgs.m_restitution;
	btVector3 newLocalInertiaDiagonal(clientCmd.m_changeDynamicsInfoArgs.m_localInertiaDiagonal[0],
		clientCmd.m_changeDynamicsInfoArgs.m_localInertiaDiagonal[1],
		clientCmd.m_changeDynamicsInfoArgs.m_localInertiaDiagonal[2]);

	btAssert(bodyUniqueId >= 0);

	InternalPhysXBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);

	if (body->mArticulation)
	{

		physx::PxArticulationLink* physxLinks[64];
		physx::PxU32 bufferSize = 64;
		physx::PxU32 startIndex = 0;
		int physxLinkIndex = linkIndex + 1;
		
		int numLinks2 = body->mArticulation->getLinks(physxLinks, bufferSize, startIndex);

		if (physxLinkIndex >= 0 && physxLinkIndex < numLinks2)
		{
			if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS)
			{
				physx::PxArticulationLink* childLink = physxLinks[physxLinkIndex];
				physx::PxRigidBodyExt::updateMassAndInertia(*childLink, mass);
			}
		}
	}
	if (body->m_rigidDynamic)
	{
		//body->m_rigidDynamic->setMass(mass);
		//also update inertia
		if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS)
		{
			physx::PxRigidBodyExt::updateMassAndInertia(*body->m_rigidDynamic, mass);
		}
	}
	if (body->m_rigidStatic)
	{
		
	}
#if 0
	if (body && body->m_multiBody)
	{
		btMultiBody* mb = body->m_multiBody;

		if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ACTIVATION_STATE)
		{
			if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateWakeUp)
			{
				mb->wakeUp();
			}
			if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateSleep)
			{
				mb->goToSleep();
			}
			if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateEnableSleeping)
			{
				mb->setCanSleep(true);
			}
			if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateDisableSleeping)
			{
				mb->setCanSleep(false);
			}
		}

		if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING)
		{
			mb->setLinearDamping(clientCmd.m_changeDynamicsInfoArgs.m_linearDamping);
		}
		if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING)
		{
			mb->setAngularDamping(clientCmd.m_changeDynamicsInfoArgs.m_angularDamping);
		}

		if (linkIndex == -1)
		{
			if (mb->getBaseCollider())
			{
				if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION)
				{
					mb->getBaseCollider()->setRestitution(restitution);
				}

				if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING)
				{
					mb->getBaseCollider()->setContactStiffnessAndDamping(clientCmd.m_changeDynamicsInfoArgs.m_contactStiffness, clientCmd.m_changeDynamicsInfoArgs.m_contactDamping);
				}
				if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION)
				{
					mb->getBaseCollider()->setFriction(lateralFriction);
				}
				if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION)
				{
					mb->getBaseCollider()->setSpinningFriction(spinningFriction);
				}
				if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION)
				{
					mb->getBaseCollider()->setRollingFriction(rollingFriction);
				}

				if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR)
				{
					if (clientCmd.m_changeDynamicsInfoArgs.m_frictionAnchor)
					{
						mb->getBaseCollider()->setCollisionFlags(mb->getBaseCollider()->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR);
					}
					else
					{
						mb->getBaseCollider()->setCollisionFlags(mb->getBaseCollider()->getCollisionFlags() & ~btCollisionObject::CF_HAS_FRICTION_ANCHOR);
					}
				}
			}
			if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS)
			{
				mb->setBaseMass(mass);
				if (mb->getBaseCollider() && mb->getBaseCollider()->getCollisionShape())
				{
					btVector3 localInertia;
					mb->getBaseCollider()->getCollisionShape()->calculateLocalInertia(mass, localInertia);
					mb->setBaseInertia(localInertia);
				}

				//handle switch from static/fixedBase to dynamic and vise-versa
				if (mass > 0)
				{
					bool isDynamic = true;
					if (mb->hasFixedBase())
					{
						int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
						int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);

						m_data->m_dynamicsWorld->removeCollisionObject(mb->getBaseCollider());
						int oldFlags = mb->getBaseCollider()->getCollisionFlags();
						mb->getBaseCollider()->setCollisionFlags(oldFlags & ~btCollisionObject::CF_STATIC_OBJECT);
						mb->setFixedBase(false);
						m_data->m_dynamicsWorld->addCollisionObject(mb->getBaseCollider(), collisionFilterGroup, collisionFilterMask);

					}
				}
				else
				{
					if (!mb->hasFixedBase())
					{
						bool isDynamic = false;
						int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
						int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
						int oldFlags = mb->getBaseCollider()->getCollisionFlags();
						mb->getBaseCollider()->setCollisionFlags(oldFlags | btCollisionObject::CF_STATIC_OBJECT);
						m_data->m_dynamicsWorld->removeCollisionObject(mb->getBaseCollider());
						mb->setFixedBase(true);
						m_data->m_dynamicsWorld->addCollisionObject(mb->getBaseCollider(), collisionFilterGroup, collisionFilterMask);
					}
				}

			}
			if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LOCAL_INERTIA_DIAGONAL)
			{
				mb->setBaseInertia(newLocalInertiaDiagonal);
			}
		}
		else
		{
			if (linkIndex >= 0 && linkIndex < mb->getNumLinks())
			{
				if (mb->getLinkCollider(linkIndex))
				{
					if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION)
					{
						mb->getLinkCollider(linkIndex)->setRestitution(restitution);
					}
					if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION)
					{
						mb->getLinkCollider(linkIndex)->setSpinningFriction(spinningFriction);
					}
					if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION)
					{
						mb->getLinkCollider(linkIndex)->setRollingFriction(rollingFriction);
					}

					if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR)
					{
						if (clientCmd.m_changeDynamicsInfoArgs.m_frictionAnchor)
						{
							mb->getLinkCollider(linkIndex)->setCollisionFlags(mb->getLinkCollider(linkIndex)->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR);
						}
						else
						{
							mb->getLinkCollider(linkIndex)->setCollisionFlags(mb->getLinkCollider(linkIndex)->getCollisionFlags() & ~btCollisionObject::CF_HAS_FRICTION_ANCHOR);
						}
					}

					if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION)
					{
						mb->getLinkCollider(linkIndex)->setFriction(lateralFriction);
					}

					if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING)
					{
						mb->getLinkCollider(linkIndex)->setContactStiffnessAndDamping(clientCmd.m_changeDynamicsInfoArgs.m_contactStiffness, clientCmd.m_changeDynamicsInfoArgs.m_contactDamping);
					}
				}
				if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS)
				{
					mb->getLink(linkIndex).m_mass = mass;
					if (mb->getLinkCollider(linkIndex) && mb->getLinkCollider(linkIndex)->getCollisionShape())
					{
						btVector3 localInertia;
						mb->getLinkCollider(linkIndex)->getCollisionShape()->calculateLocalInertia(mass, localInertia);
						mb->getLink(linkIndex).m_inertiaLocal = localInertia;
					}
				}
				if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LOCAL_INERTIA_DIAGONAL)
				{
					mb->getLink(linkIndex).m_inertiaLocal = newLocalInertiaDiagonal;
				}
			}
		}
	}
	else
	{
		if (body && body->m_rigidBody)
		{
			if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ACTIVATION_STATE)
			{
				if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateEnableSleeping)
				{
					body->m_rigidBody->forceActivationState(ACTIVE_TAG);
				}
				if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateDisableSleeping)
				{
					body->m_rigidBody->forceActivationState(DISABLE_DEACTIVATION);
				}
				if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateWakeUp)
				{
					body->m_rigidBody->forceActivationState(ACTIVE_TAG);
				}
				if (clientCmd.m_changeDynamicsInfoArgs.m_activationState & eActivationStateSleep)
				{
					body->m_rigidBody->forceActivationState(ISLAND_SLEEPING);
				}
			}

			if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING)
			{
				btScalar angDamping = body->m_rigidBody->getAngularDamping();
				body->m_rigidBody->setDamping(clientCmd.m_changeDynamicsInfoArgs.m_linearDamping, angDamping);
			}
			if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING)
			{
				btScalar linDamping = body->m_rigidBody->getLinearDamping();
				body->m_rigidBody->setDamping(linDamping, clientCmd.m_changeDynamicsInfoArgs.m_angularDamping);
			}

			if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING)
			{
				body->m_rigidBody->setContactStiffnessAndDamping(clientCmd.m_changeDynamicsInfoArgs.m_contactStiffness, clientCmd.m_changeDynamicsInfoArgs.m_contactDamping);
			}
			if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_RESTITUTION)
			{
				body->m_rigidBody->setRestitution(restitution);
			}
			if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LATERAL_FRICTION)
			{
				body->m_rigidBody->setFriction(lateralFriction);
			}
			if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_SPINNING_FRICTION)
			{
				body->m_rigidBody->setSpinningFriction(spinningFriction);
			}
			if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ROLLING_FRICTION)
			{
				body->m_rigidBody->setRollingFriction(rollingFriction);
			}

			if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_FRICTION_ANCHOR)
			{
				if (clientCmd.m_changeDynamicsInfoArgs.m_frictionAnchor)
				{
					body->m_rigidBody->setCollisionFlags(body->m_rigidBody->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR);
				}
				else
				{
					body->m_rigidBody->setCollisionFlags(body->m_rigidBody->getCollisionFlags() & ~btCollisionObject::CF_HAS_FRICTION_ANCHOR);
				}
			}

			if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_MASS)
			{
				btVector3 localInertia;
				if (body->m_rigidBody->getCollisionShape())
				{
					body->m_rigidBody->getCollisionShape()->calculateLocalInertia(mass, localInertia);
				}
				body->m_rigidBody->setMassProps(mass, localInertia);
			}
			if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LOCAL_INERTIA_DIAGONAL)
			{
				btScalar orgMass = body->m_rigidBody->getInvMass();
				if (orgMass > 0)
				{
					body->m_rigidBody->setMassProps(mass, newLocalInertiaDiagonal);
				}
			}

			if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_PROCESSING_THRESHOLD)
			{
				body->m_rigidBody->setContactProcessingThreshold(clientCmd.m_changeDynamicsInfoArgs.m_contactProcessingThreshold);
			}

			if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CCD_SWEPT_SPHERE_RADIUS)
			{
				body->m_rigidBody->setCcdSweptSphereRadius(clientCmd.m_changeDynamicsInfoArgs.m_ccdSweptSphereRadius);
				//for a given sphere radius, use a motion threshold of half the radius, before the ccd algorithm is enabled
				body->m_rigidBody->setCcdMotionThreshold(clientCmd.m_changeDynamicsInfoArgs.m_ccdSweptSphereRadius / 2.);
			}
		}
	}

	b3Notification notification;
	notification.m_notificationType = LINK_DYNAMICS_CHANGED;
	notification.m_linkArgs.m_bodyUniqueId = bodyUniqueId;
	notification.m_linkArgs.m_linkIndex = linkIndex;
	m_data->m_pluginManager.addNotification(notification);


#endif
	SharedMemoryStatus& serverCmd = serverStatusOut;
	serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;

	
	return hasStatus;
}

bool PhysXServerCommandProcessor::processRequestPhysicsSimulationParametersCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
	bool hasStatus = true;
	SharedMemoryStatus& serverCmd = serverStatusOut;
	serverCmd.m_type = CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED;

	serverCmd.m_simulationParameterResultArgs.m_allowedCcdPenetration = 0;// m_data->m_dynamicsWorld->getDispatchInfo().m_allowedCcdPenetration;
	serverCmd.m_simulationParameterResultArgs.m_collisionFilterMode = 0;// m_data->m_broadphaseCollisionFilterCallback->m_filterMode;
	serverCmd.m_simulationParameterResultArgs.m_deltaTime = m_data->m_physicsDeltaTime;
	serverCmd.m_simulationParameterResultArgs.m_contactBreakingThreshold = 0;// gContactBreakingThreshold;
	serverCmd.m_simulationParameterResultArgs.m_contactSlop = 0;// m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop;
	serverCmd.m_simulationParameterResultArgs.m_enableSAT = 0;// m_data->m_dynamicsWorld->getDispatchInfo().m_enableSatConvex;

	serverCmd.m_simulationParameterResultArgs.m_defaultGlobalCFM = 0;// m_data->m_dynamicsWorld->getSolverInfo().m_globalCfm;
	serverCmd.m_simulationParameterResultArgs.m_defaultContactERP = 0;// m_data->m_dynamicsWorld->getSolverInfo().m_erp2;
	serverCmd.m_simulationParameterResultArgs.m_defaultNonContactERP = 0;// m_data->m_dynamicsWorld->getSolverInfo().m_erp;
	
	serverCmd.m_simulationParameterResultArgs.m_deterministicOverlappingPairs = 0;// m_data->m_dynamicsWorld->getDispatchInfo().m_deterministicOverlappingPairs;
	serverCmd.m_simulationParameterResultArgs.m_enableConeFriction = 0;// (m_data->m_dynamicsWorld->getSolverInfo().m_solverMode & SOLVER_DISABLE_IMPLICIT_CONE_FRICTION) ? 0 : 1;
	serverCmd.m_simulationParameterResultArgs.m_enableFileCaching = 0;// b3IsFileCachingEnabled();
	serverCmd.m_simulationParameterResultArgs.m_frictionCFM = 0;// m_data->m_dynamicsWorld->getSolverInfo().m_frictionCFM;
	serverCmd.m_simulationParameterResultArgs.m_frictionERP = 0;// m_data->m_dynamicsWorld->getSolverInfo().m_frictionERP;

	physx::PxVec3 grav = m_data->m_scene->getGravity();

	serverCmd.m_simulationParameterResultArgs.m_gravityAcceleration[0] = grav.x;
	serverCmd.m_simulationParameterResultArgs.m_gravityAcceleration[1] = grav.y;
	serverCmd.m_simulationParameterResultArgs.m_gravityAcceleration[2] = grav.z;
	serverCmd.m_simulationParameterResultArgs.m_internalSimFlags = 0;
	serverCmd.m_simulationParameterResultArgs.m_jointFeedbackMode = 0;
	
	serverCmd.m_simulationParameterResultArgs.m_numSimulationSubSteps = 0;// m_data->m_numSimulationSubSteps;
	serverCmd.m_simulationParameterResultArgs.m_numSolverIterations = 0;// m_data->m_dynamicsWorld->getSolverInfo().m_numIterations;
	serverCmd.m_simulationParameterResultArgs.m_restitutionVelocityThreshold = 0;// m_data->m_dynamicsWorld->getSolverInfo().m_restitutionVelocityThreshold;

	serverCmd.m_simulationParameterResultArgs.m_solverResidualThreshold = 0;// m_data->m_dynamicsWorld->getSolverInfo().m_leastSquaresResidualThreshold;
	serverCmd.m_simulationParameterResultArgs.m_splitImpulsePenetrationThreshold = 0;// m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulsePenetrationThreshold;
	serverCmd.m_simulationParameterResultArgs.m_useRealTimeSimulation = 0;// m_data->m_useRealTimeSimulation;
	serverCmd.m_simulationParameterResultArgs.m_useSplitImpulse = 0;// m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulse;

	return hasStatus;
}

bool PhysXServerCommandProcessor::processRequestContactpointInformationCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
	SharedMemoryStatus& serverCmd = serverStatusOut;
	int totalBytesPerContact = sizeof(b3ContactPointData);
	int contactPointStorage = bufferSizeInBytes / totalBytesPerContact - 1;

	b3ContactPointData* contactData = (b3ContactPointData*)bufferServerToClient;

	int startContactPointIndex = clientCmd.m_requestContactPointArguments.m_startingContactPointIndex;
	int numContactPointBatch = btMin(int(m_data->m_contactPoints.size()), contactPointStorage);

	int endContactPointIndex = startContactPointIndex + numContactPointBatch;
	serverCmd.m_sendContactPointArgs.m_numContactPointsCopied = 0;
	for (int i = startContactPointIndex; i < endContactPointIndex; i++)
	{
		const b3ContactPointData& srcPt = m_data->m_contactPoints[i];
		b3ContactPointData& destPt = contactData[serverCmd.m_sendContactPointArgs.m_numContactPointsCopied];
		destPt = srcPt;
		serverCmd.m_sendContactPointArgs.m_numContactPointsCopied++;
	}
	serverCmd.m_sendContactPointArgs.m_startingContactPointIndex = startContactPointIndex;
	serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints = m_data->m_contactPoints.size() - startContactPointIndex - serverCmd.m_sendContactPointArgs.m_numContactPointsCopied;
	serverCmd.m_numDataStreamBytes = totalBytesPerContact * serverCmd.m_sendContactPointArgs.m_numContactPointsCopied;
	serverCmd.m_type = CMD_CONTACT_POINT_INFORMATION_COMPLETED;
	

	return true;
}

bool PhysXServerCommandProcessor::processCreateCollisionShapeCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
	return false;
}


bool PhysXServerCommandProcessor::processSetAdditionalSearchPathCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
	bool hasStatus = true;

	BT_PROFILE("CMD_SET_ADDITIONAL_SEARCH_PATH");
	b3ResourcePath::setAdditionalSearchPath(clientCmd.m_searchPathArgs.m_path);
	serverStatusOut.m_type = CMD_CLIENT_COMMAND_COMPLETED;
	return hasStatus;
}

bool PhysXServerCommandProcessor::processUserDebugDrawCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
	///dummy, so commands don't fail
	bool hasStatus = true;
	BT_PROFILE("CMD_USER_DEBUG_DRAW");
	SharedMemoryStatus& serverCmd = serverStatusOut;
	int uid = 0;
	serverCmd.m_userDebugDrawArgs.m_debugItemUniqueId = uid;
	serverCmd.m_type = CMD_USER_DEBUG_DRAW_FAILED;

	if (clientCmd.m_updateFlags & USER_DEBUG_ADD_PARAMETER)
	{
		int uid = m_data->m_userDebugParametersUid++;
		double value = clientCmd.m_userDebugDrawArgs.m_startValue;
		m_data->m_userDebugParameters.insert(uid, value);
		serverCmd.m_userDebugDrawArgs.m_debugItemUniqueId = uid;
		serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED;
	}
	if (clientCmd.m_updateFlags & USER_DEBUG_READ_PARAMETER)
	{
		double* valPtr = m_data->m_userDebugParameters[clientCmd.m_userDebugDrawArgs.m_itemUniqueId];
		if (valPtr)
		{
			serverCmd.m_userDebugDrawArgs.m_parameterValue = *valPtr;
			serverCmd.m_type = CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED;
		}
	}
	if (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_ALL)
	{
		m_data->m_userDebugParameters.clear();
		serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED;
	}
	if (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_ONE_ITEM)
	{
		m_data->m_userDebugParameters.remove(clientCmd.m_userDebugDrawArgs.m_itemUniqueId);
		serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED;
	}
	return hasStatus;

}

bool PhysXServerCommandProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
	//	BT_PROFILE("processCommand");

	int sz = sizeof(SharedMemoryStatus);
	int sz2 = sizeof(SharedMemoryCommand);

	bool hasStatus = true;

	serverStatusOut.m_type = CMD_INVALID_STATUS;
	serverStatusOut.m_numDataStreamBytes = 0;
	serverStatusOut.m_dataStream = 0;

	//consume the command
	switch (clientCmd.m_type)
	{
		case CMD_REQUEST_INTERNAL_DATA:
		{
			hasStatus = processRequestInternalDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
			break;
		};

		case CMD_SYNC_BODY_INFO:
		{
			hasStatus = processSyncBodyInfoCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
			break;
		}
		case CMD_SYNC_USER_DATA:
		{
			hasStatus = processSyncUserDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
			break;
		}
		case CMD_REQUEST_BODY_INFO:
		{
			hasStatus = processRequestBodyInfoCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
			break;
		}
		case CMD_STEP_FORWARD_SIMULATION:
		{
			hasStatus = processForwardDynamicsCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
			break;
		}

		case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS:
		{
			hasStatus = processSendPhysicsParametersCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
			break;
		};

		case CMD_REQUEST_ACTUAL_STATE:
		{
			hasStatus = processRequestActualStateCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
			break;
		}

		case CMD_RESET_SIMULATION:
		{
			hasStatus = processResetSimulationCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
			break;
		}

		case CMD_LOAD_URDF:
		{
			hasStatus = processLoadURDFCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
			break;
		}
		case CMD_CUSTOM_COMMAND:
		{
			hasStatus = processCustomCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
			break;
		}
		case CMD_STATE_LOGGING:
		{
			hasStatus = processStateLoggingCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
			break;
		}

		case CMD_INIT_POSE:
		{
			hasStatus = processInitPoseCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
			break;
		}

		case CMD_SEND_DESIRED_STATE:
		{
			hasStatus = processSendDesiredStateCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
			break;
		}

		case CMD_CHANGE_DYNAMICS_INFO:
		{
			hasStatus = processChangeDynamicsInfoCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
			break;
		};

		case CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS:
		{
			hasStatus = processRequestPhysicsSimulationParametersCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
			break;
		}

		case CMD_REQUEST_CONTACT_POINT_INFORMATION:
		{
			hasStatus = processRequestContactpointInformationCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
			break;
		}

		case CMD_CREATE_COLLISION_SHAPE:
		{
			hasStatus = processCreateCollisionShapeCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
			break;
		}

		case CMD_SET_ADDITIONAL_SEARCH_PATH:
		{
			hasStatus = processSetAdditionalSearchPathCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
			break;
		}

		case CMD_USER_DEBUG_DRAW:
		{
			hasStatus = processUserDebugDrawCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
			break;
		}

		case CMD_REQUEST_CAMERA_IMAGE_DATA:
		{
			hasStatus = processRequestCameraImageCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
			break;
		}

#if 0
	case CMD_SET_VR_CAMERA_STATE:
		{
			hasStatus = processSetVRCameraStateCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_REQUEST_VR_EVENTS_DATA:
		{
			hasStatus = processRequestVREventsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		};
	case CMD_REQUEST_MOUSE_EVENTS_DATA:
		{
			hasStatus = processRequestMouseEventsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		};
	case CMD_REQUEST_KEYBOARD_EVENTS_DATA:
		{
			hasStatus = processRequestKeyboardEventsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		};

	case CMD_REQUEST_RAY_CAST_INTERSECTIONS:
		{

			hasStatus = processRequestRaycastIntersectionsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		};
	case CMD_REQUEST_DEBUG_LINES:
		{
			hasStatus = processRequestDebugLinesCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}

	
	case CMD_REQUEST_BODY_INFO:
		{
			hasStatus = processRequestBodyInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_SAVE_WORLD:
		{
			hasStatus = processSaveWorldCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_LOAD_SDF:
		{
			hasStatus = processLoadSDFCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	
	case CMD_CREATE_VISUAL_SHAPE:
		{
			hasStatus = processCreateVisualShapeCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_CREATE_MULTI_BODY:
		{
			hasStatus = processCreateMultiBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	
	
	case CMD_LOAD_MJCF:
	{
		hasStatus = processLoadMJCFCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
		break;
	}

	case CMD_LOAD_SOFT_BODY:
		{
			hasStatus = processLoadSoftBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_CREATE_SENSOR:
		{
			hasStatus = processCreateSensorCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_PROFILE_TIMING:
		{
			hasStatus = processProfileTimingCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}

	
	case CMD_REQUEST_COLLISION_INFO:
		{
			hasStatus = processRequestCollisionInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}

	
	
	
	case CMD_GET_DYNAMICS_INFO:
		{
			hasStatus = processGetDynamicsInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}


	
	
	case CMD_CREATE_RIGID_BODY:
		{
			hasStatus = processCreateRigidBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_CREATE_BOX_COLLISION_SHAPE:
		{
			//for backward compatibility, CMD_CREATE_BOX_COLLISION_SHAPE is the same as CMD_CREATE_RIGID_BODY
			hasStatus = processCreateRigidBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_PICK_BODY:
		{
			hasStatus = processPickBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_MOVE_PICKED_BODY:
		{
			hasStatus = processMovePickedBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_REMOVE_PICKING_CONSTRAINT_BODY:
		{
			hasStatus = processRemovePickingConstraintCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_REQUEST_AABB_OVERLAP:
		{
			hasStatus = processRequestAabbOverlapCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_REQUEST_OPENGL_VISUALIZER_CAMERA:
		{
			hasStatus = processRequestOpenGLVisualizeCameraCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_CONFIGURE_OPENGL_VISUALIZER:
		{
			hasStatus = processConfigureOpenGLVisualizerCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	
	case CMD_CALCULATE_INVERSE_DYNAMICS:
		{
			hasStatus = processInverseDynamicsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_CALCULATE_JACOBIAN:
		{
			hasStatus = processCalculateJacobianCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_CALCULATE_MASS_MATRIX:
		{
			hasStatus = processCalculateMassMatrixCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_APPLY_EXTERNAL_FORCE:
		{
			hasStatus = processApplyExternalForceCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_REMOVE_BODY:
	{
		hasStatus = processRemoveBodyCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
		break;
	}
	case CMD_USER_CONSTRAINT:
	{
		hasStatus = processCreateUserConstraintCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
		break;
	}
	case CMD_CALCULATE_INVERSE_KINEMATICS:
	{
		hasStatus = processCalculateInverseKinematicsCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
		break;
	}
	case CMD_REQUEST_VISUAL_SHAPE_INFO:
	{
		hasStatus = processRequestVisualShapeInfoCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
		break;
	}
	case CMD_REQUEST_COLLISION_SHAPE_INFO:
	{
		hasStatus = processRequestCollisionShapeInfoCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
		break;
	}
	case CMD_UPDATE_VISUAL_SHAPE:
	{
		hasStatus = processUpdateVisualShapeCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
		break;
	}
	case CMD_CHANGE_TEXTURE:
	{
		hasStatus = processChangeTextureCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
		break;
	}
	case CMD_LOAD_TEXTURE:
	{
		hasStatus = processLoadTextureCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
		break;
	}
	case CMD_RESTORE_STATE:
	{
		hasStatus = processRestoreStateCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
		break;
	}

	case CMD_SAVE_STATE:
	{
		hasStatus = processSaveStateCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
		break;
	}

	case CMD_LOAD_BULLET:
	{
		hasStatus = processLoadBulletCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
		break;
	}
	case CMD_SAVE_BULLET:
	{
		hasStatus = processSaveBulletCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
		break;
	}
	case CMD_LOAD_MJCF:
	{
		hasStatus = processLoadMJCFCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
		break;
	}

	case CMD_REQUEST_USER_DATA:
	{
		hasStatus = processRequestUserDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
		break;
	}
	case CMD_ADD_USER_DATA:
	{
		hasStatus = processAddUserDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
		break;
	}
	case CMD_REMOVE_USER_DATA:
	{
		hasStatus = processRemoveUserDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
		break;
	}
#endif

	default:
	{
		BT_PROFILE("CMD_UNKNOWN");
		printf("Unknown command encountered: %d", clientCmd.m_type);
		SharedMemoryStatus& serverCmd = serverStatusOut;
		serverCmd.m_type = CMD_UNKNOWN_COMMAND_FLUSHED;
		hasStatus = true;
	}
	};

	return hasStatus;
}



bool PhysXServerCommandProcessor::processRequestCameraImageCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
	bool hasStatus = true;
	BT_PROFILE("CMD_REQUEST_CAMERA_IMAGE_DATA");
	int startPixelIndex = clientCmd.m_requestPixelDataArguments.m_startPixelIndex;
	int width = clientCmd.m_requestPixelDataArguments.m_pixelWidth;
	int height = clientCmd.m_requestPixelDataArguments.m_pixelHeight;
	int numPixelsCopied = 0;

	int oldWidth;
	int oldHeight;
	m_data->m_pluginManager.getRenderInterface()->getWidthAndHeight(oldWidth, oldHeight);

	serverStatusOut.m_type = CMD_CAMERA_IMAGE_FAILED;
	
	if ((clientCmd.m_requestPixelDataArguments.m_startPixelIndex == 0) &&
		(clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_PIXEL_WIDTH_HEIGHT) != 0)
	{
		if (m_data->m_pluginManager.getRenderInterface())
		{

			m_data->m_pluginManager.getRenderInterface()->setWidthAndHeight(clientCmd.m_requestPixelDataArguments.m_pixelWidth,
				clientCmd.m_requestPixelDataArguments.m_pixelHeight);
		}
	}
	int flags = 0;
	if (clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_FLAGS)
	{
		flags = clientCmd.m_requestPixelDataArguments.m_flags;
	}
	if (m_data->m_pluginManager.getRenderInterface())
	{
		m_data->m_pluginManager.getRenderInterface()->setFlags(flags);
	}

	int numTotalPixels = width * height;
	int numRemainingPixels = numTotalPixels - startPixelIndex;

	if (numRemainingPixels > 0)
	{
		int totalBytesPerPixel = 4 + 4 + 4;  //4 for rgb, 4 for depth, 4 for segmentation mask
		int maxNumPixels = bufferSizeInBytes / totalBytesPerPixel - 1;
		unsigned char* pixelRGBA = (unsigned char*)bufferServerToClient;
		int numRequestedPixels = btMin(maxNumPixels, numRemainingPixels);

		float* depthBuffer = (float*)(bufferServerToClient + numRequestedPixels * 4);
		int* segmentationMaskBuffer = (int*)(bufferServerToClient + numRequestedPixels * 8);

		serverStatusOut.m_numDataStreamBytes = numRequestedPixels * totalBytesPerPixel;
		float viewMat[16];
		float projMat[16];
		float projTextureViewMat[16];
		float projTextureProjMat[16];
		
		if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES) == 0)
		{
			b3OpenGLVisualizerCameraInfo tmpCamResult;
			bool result = m_data->m_pluginManager.getRenderInterface()->getCameraInfo(
				&tmpCamResult.m_width,
				&tmpCamResult.m_height,
				tmpCamResult.m_viewMatrix,
				tmpCamResult.m_projectionMatrix,
				tmpCamResult.m_camUp,
				tmpCamResult.m_camForward,
				tmpCamResult.m_horizontal,
				tmpCamResult.m_vertical,
				&tmpCamResult.m_yaw,
				&tmpCamResult.m_pitch,
				&tmpCamResult.m_dist,
				tmpCamResult.m_target);
			if (result)
			{
				for (int i = 0; i < 16; i++)
				{
					viewMat[i] = tmpCamResult.m_viewMatrix[i];
					projMat[i] = tmpCamResult.m_projectionMatrix[i];
				}
			}
			else
			{
				//failed
				m_data->m_pluginManager.getRenderInterface()->setWidthAndHeight(oldWidth, oldHeight);
				return hasStatus;
			}
		}
		else
		{
			for (int i = 0; i < 16; i++)
			{
				viewMat[i] = clientCmd.m_requestPixelDataArguments.m_viewMatrix[i];
				projMat[i] = clientCmd.m_requestPixelDataArguments.m_projectionMatrix[i];
			}
		}
		
		{
			if (m_data->m_pluginManager.getRenderInterface())
			{
				if (clientCmd.m_requestPixelDataArguments.m_startPixelIndex == 0)
				{
					//   printf("-------------------------------\nRendering\n");

					if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_LIGHT_DIRECTION) != 0)
					{
						m_data->m_pluginManager.getRenderInterface()->setLightDirection(clientCmd.m_requestPixelDataArguments.m_lightDirection[0], clientCmd.m_requestPixelDataArguments.m_lightDirection[1], clientCmd.m_requestPixelDataArguments.m_lightDirection[2]);
					}

					if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_LIGHT_COLOR) != 0)
					{
						m_data->m_pluginManager.getRenderInterface()->setLightColor(clientCmd.m_requestPixelDataArguments.m_lightColor[0], clientCmd.m_requestPixelDataArguments.m_lightColor[1], clientCmd.m_requestPixelDataArguments.m_lightColor[2]);
					}

					if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_LIGHT_DISTANCE) != 0)
					{
						m_data->m_pluginManager.getRenderInterface()->setLightDistance(clientCmd.m_requestPixelDataArguments.m_lightDistance);
					}

					if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_SHADOW) != 0)
					{
						m_data->m_pluginManager.getRenderInterface()->setShadow((clientCmd.m_requestPixelDataArguments.m_hasShadow != 0));
					}

					if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_AMBIENT_COEFF) != 0)
					{
						m_data->m_pluginManager.getRenderInterface()->setLightAmbientCoeff(clientCmd.m_requestPixelDataArguments.m_lightAmbientCoeff);
					}

					if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_DIFFUSE_COEFF) != 0)
					{
						m_data->m_pluginManager.getRenderInterface()->setLightDiffuseCoeff(clientCmd.m_requestPixelDataArguments.m_lightDiffuseCoeff);
					}

					if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_SPECULAR_COEFF) != 0)
					{
						m_data->m_pluginManager.getRenderInterface()->setLightSpecularCoeff(clientCmd.m_requestPixelDataArguments.m_lightSpecularCoeff);
					}


					if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES) != 0)
					{
						m_data->m_pluginManager.getRenderInterface()->render(
							clientCmd.m_requestPixelDataArguments.m_viewMatrix,
							clientCmd.m_requestPixelDataArguments.m_projectionMatrix);
					}
					else
					{
						b3OpenGLVisualizerCameraInfo tmpCamResult;
						bool result = m_data->m_pluginManager.getRenderInterface()->getCameraInfo(
							&tmpCamResult.m_width,
							&tmpCamResult.m_height,
							tmpCamResult.m_viewMatrix,
							tmpCamResult.m_projectionMatrix,
							tmpCamResult.m_camUp,
							tmpCamResult.m_camForward,
							tmpCamResult.m_horizontal,
							tmpCamResult.m_vertical,
							&tmpCamResult.m_yaw,
							&tmpCamResult.m_pitch,
							&tmpCamResult.m_dist,
							tmpCamResult.m_target);
						if (result)
						{
							m_data->m_pluginManager.getRenderInterface()->render(tmpCamResult.m_viewMatrix, tmpCamResult.m_projectionMatrix);
						}
						else
						{
							m_data->m_pluginManager.getRenderInterface()->render();
						}
					}
				}
			}

			if (m_data->m_pluginManager.getRenderInterface())
			{
				if ((flags & ER_USE_PROJECTIVE_TEXTURE) != 0)
				{
					m_data->m_pluginManager.getRenderInterface()->setProjectiveTexture(true);
					if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_PROJECTIVE_TEXTURE_MATRICES) != 0)
					{
						for (int i = 0; i < 16; i++)
						{
							projTextureViewMat[i] = clientCmd.m_requestPixelDataArguments.m_projectiveTextureViewMatrix[i];
							projTextureProjMat[i] = clientCmd.m_requestPixelDataArguments.m_projectiveTextureProjectionMatrix[i];
						}
					}
					else  // If no specified matrices for projective texture, then use the camera matrices.
					{
						for (int i = 0; i < 16; i++)
						{
							projTextureViewMat[i] = viewMat[i];
							projTextureProjMat[i] = projMat[i];
						}
					}
					m_data->m_pluginManager.getRenderInterface()->setProjectiveTextureMatrices(projTextureViewMat, projTextureProjMat);
				}
				else
				{
					m_data->m_pluginManager.getRenderInterface()->setProjectiveTexture(false);
				}

				if ((flags & ER_NO_SEGMENTATION_MASK) != 0)
				{
					segmentationMaskBuffer = 0;
				}

				m_data->m_pluginManager.getRenderInterface()->copyCameraImageData(pixelRGBA, numRequestedPixels,
					depthBuffer, numRequestedPixels,
					segmentationMaskBuffer, numRequestedPixels,
					startPixelIndex, &width, &height, &numPixelsCopied);
				m_data->m_pluginManager.getRenderInterface()->setProjectiveTexture(false);
			}

			#if 0
			m_data->m_guiHelper->debugDisplayCameraImageData(clientCmd.m_requestPixelDataArguments.m_viewMatrix,
				clientCmd.m_requestPixelDataArguments.m_projectionMatrix, pixelRGBA, numRequestedPixels,
				depthBuffer, numRequestedPixels,
				segmentationMaskBuffer, numRequestedPixels,
				startPixelIndex, width, height, &numPixelsCopied);
			#endif		
		}

		//each pixel takes 4 RGBA values and 1 float = 8 bytes
	}
	else
	{
	}

	m_data->m_pluginManager.getRenderInterface()->setWidthAndHeight(oldWidth, oldHeight);
	serverStatusOut.m_type = CMD_CAMERA_IMAGE_COMPLETED;

	serverStatusOut.m_sendPixelDataArguments.m_numPixelsCopied = numPixelsCopied;
	serverStatusOut.m_sendPixelDataArguments.m_numRemainingPixels = numRemainingPixels - numPixelsCopied;
	serverStatusOut.m_sendPixelDataArguments.m_startingPixelIndex = startPixelIndex;
	serverStatusOut.m_sendPixelDataArguments.m_imageWidth = width;
	serverStatusOut.m_sendPixelDataArguments.m_imageHeight = height;
	return hasStatus;

}

bool PhysXServerCommandProcessor::processRequestInternalDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
	bool hasStatus = true;
	BT_PROFILE("CMD_REQUEST_INTERNAL_DATA");
	SharedMemoryStatus& serverCmd = serverStatusOut;
	serverCmd.m_type = CMD_REQUEST_INTERNAL_DATA_COMPLETED;
	serverCmd.m_numDataStreamBytes = 0;
	return hasStatus;
}

bool PhysXServerCommandProcessor::processSyncBodyInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
	bool hasStatus = true;
	BT_PROFILE("CMD_SYNC_BODY_INFO");
	int actualNumBodies = 0;
	serverStatusOut.m_sdfLoadedArgs.m_numBodies = 0;
	serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = 0;
	serverStatusOut.m_type = CMD_SYNC_BODY_INFO_COMPLETED;
	return hasStatus;
}

bool PhysXServerCommandProcessor::processSyncUserDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
	bool hasStatus = true;
	BT_PROFILE("CMD_SYNC_USER_DATA");
	int numIdentifiers = 0;
	serverStatusOut.m_syncUserDataArgs.m_numUserDataIdentifiers = numIdentifiers;
	serverStatusOut.m_type = CMD_SYNC_USER_DATA_COMPLETED;
	return hasStatus;
}

struct MyPhysXURDFImporter : public PhysXURDFImporter
{
	b3PluginManager& m_pluginManager;
	
	MyPhysXURDFImporter(struct CommonFileIOInterface* fileIO, double globalScaling, int flags, b3PluginManager& pluginManager)
		:PhysXURDFImporter(fileIO, globalScaling, flags),
		m_pluginManager(pluginManager)
	{

	}

	int convertLinkVisualShapes3(
			int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame,
			const UrdfLink* linkPtr, const UrdfModel* model,
			int collisionObjectUniqueId, int bodyUniqueId, struct  CommonFileIOInterface* fileIO) const
	{

		if (m_pluginManager.getRenderInterface())
		{
			int graphicsUniqueId = m_pluginManager.getRenderInterface()->convertVisualShapes(linkIndex, pathPrefix, localInertiaFrame, linkPtr, model, collisionObjectUniqueId, bodyUniqueId, fileIO);
			return graphicsUniqueId;
		}
		return 0;
	}

};


bool PhysXServerCommandProcessor::processLoadURDFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
	BT_PROFILE("CMD_LOAD_URDF");
	serverStatusOut.m_type = CMD_URDF_LOADING_FAILED;
	serverStatusOut.m_numDataStreamBytes = 0;

	const UrdfArgs& urdfArgs = clientCmd.m_urdfArguments;

	btAssert(m_data->m_foundation);
	if (!m_data->m_foundation)
	{
		b3Error("loadUrdf: No valid m_dynamicsWorld");
		return false;
	}

	bool useMultiBody = (clientCmd.m_updateFlags & URDF_ARGS_USE_MULTIBODY) ? (urdfArgs.m_useMultiBody != 0) : true;
	bool useFixedBase = (clientCmd.m_updateFlags & URDF_ARGS_USE_FIXED_BASE) ? (urdfArgs.m_useFixedBase != 0) : false;

	btScalar globalScaling = 1.f;
	if (clientCmd.m_updateFlags & URDF_ARGS_USE_GLOBAL_SCALING)
	{
		globalScaling = urdfArgs.m_globalScaling;
	}

	b3BulletDefaultFileIO fileIO;
	
	btVector3 initialPos(0, 0, 0);
	btQuaternion initialOrn(0, 0, 0, 1);
	if (clientCmd.m_updateFlags & URDF_ARGS_INITIAL_POSITION)
	{
		initialPos[0] = urdfArgs.m_initialPosition[0];
		initialPos[1] = urdfArgs.m_initialPosition[1];
		initialPos[2] = urdfArgs.m_initialPosition[2];
	}
	int urdfFlags = 0;
	if (clientCmd.m_updateFlags & URDF_ARGS_HAS_CUSTOM_URDF_FLAGS)
	{
		urdfFlags = urdfArgs.m_urdfFlags;
	}
	if (clientCmd.m_updateFlags & URDF_ARGS_INITIAL_ORIENTATION)
	{
		initialOrn[0] = urdfArgs.m_initialOrientation[0];
		initialOrn[1] = urdfArgs.m_initialOrientation[1];
		initialOrn[2] = urdfArgs.m_initialOrientation[2];
		initialOrn[3] = urdfArgs.m_initialOrientation[3];
	}


	MyPhysXURDFImporter u2p(&fileIO, globalScaling, urdfArgs.m_urdfFlags, m_data->m_pluginManager);
	
	

	bool loadOk = u2p.loadURDF(urdfArgs.m_urdfFileName, useFixedBase);


	if (loadOk)
	{
		

		for (int m = 0; m < u2p.getNumModels(); m++)
		{
			u2p.activateModel(m);

			btTransform rootTrans;
			rootTrans.setOrigin(initialPos);
			rootTrans.setRotation(initialOrn);
			u2p.setRootTransformInWorld(rootTrans);

			//get a body index
			int bodyUniqueId = m_data->m_bodyHandles.allocHandle();

			InternalPhysXBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId);

			//sd.m_bodyUniqueIds.push_back(bodyUniqueId);

			
			
			u2p.setBodyUniqueId(bodyUniqueId);
			{
				btScalar mass = 0;
				//bodyHandle->m_rootLocalInertialFrame.setIdentity();
				bodyHandle->m_bodyName = u2p.getBodyName();
				btVector3 localInertiaDiagonal(0, 0, 0);
				int urdfLinkIndex = u2p.getRootLinkIndex();
				//u2p.getMassAndInertia2((urdfLinkIndex, mass, localInertiaDiagonal, bodyHandle->m_rootLocalInertialFrame, flags);
			}
			
			physx::PxBase* baseObj = URDF2PhysX(m_data->m_foundation,m_data->m_physics, m_data->m_cooking, m_data->m_scene, u2p, urdfArgs.m_urdfFlags, u2p.getPathPrefix(), rootTrans, &fileIO, useMultiBody);

			physx::PxRigidDynamic* c = baseObj->is<physx::PxRigidDynamic>();
			physx::PxRigidStatic* rigidStatic = baseObj->is<physx::PxRigidStatic>();
			physx::PxRigidDynamic* rigidDynamic = baseObj->is<physx::PxRigidDynamic>();
			physx::PxArticulationReducedCoordinate* articulation = 0;
			if (rigidDynamic)
			{
				bodyHandle->m_rigidDynamic = rigidDynamic;
			}
			if (rigidStatic)
			{
				bodyHandle->m_rigidStatic = rigidStatic;
				serverStatusOut.m_numDataStreamBytes = 0;
				serverStatusOut.m_type = CMD_URDF_LOADING_COMPLETED;
				serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId;
			}
			if ((rigidDynamic == 0) && (rigidStatic == 0))
			{
				articulation = (physx::PxArticulationReducedCoordinate*)baseObj;
				serverStatusOut.m_type = CMD_URDF_LOADING_COMPLETED;
				serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId;
			}
			if (rigidStatic || rigidDynamic || articulation)
			{
				serverStatusOut.m_type = CMD_URDF_LOADING_COMPLETED;
				serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId;
				sprintf(serverStatusOut.m_dataStreamArguments.m_bodyName, "%s", bodyHandle->m_bodyName.c_str());
			}
			
			if (articulation)
			{
				bodyHandle->mArticulation = articulation;
				
				

				btDefaultSerializer ser(bufferSizeInBytes, (unsigned char*)bufferServerToClient);

				ser.startSerialization();

				int len = sizeof(btMultiBodyData); 
				btChunk* chunk = ser.allocate(len, 1);


				btMultiBodyData *mbd = (btMultiBodyData *)chunk->m_oldPtr;
				btTransform rootTrans;
				u2p.getRootTransformInWorld(rootTrans);
				rootTrans.getOrigin().serialize(mbd->m_baseWorldPosition);
				rootTrans.getRotation().serialize(mbd->m_baseWorldOrientation);
				btVector3 zero(0, 0, 0);

				zero.serialize(mbd->m_baseLinearVelocity);
				zero.serialize(mbd->m_baseAngularVelocity);
				
				ser.registerNameForPointer(bodyHandle->m_bodyName.c_str(), bodyHandle->m_bodyName.c_str());
				{
					char *name = (char *)ser.findNameForPointer(bodyHandle->m_bodyName.c_str());
					mbd->m_baseName = (char *)ser.getUniquePointer(name);
					if (mbd->m_baseName)
					{
						ser.serializeName(name);
					}
				}
				mbd->m_numLinks = articulation->getNbLinks()-1;

				if (mbd->m_numLinks)
				{
					int sz = sizeof(btMultiBodyLinkData);
					int numElem = mbd->m_numLinks;
					btChunk *chunk = ser.allocate(sz, numElem);

					physx::PxArticulationLink* physxLinks[64];
					physx::PxU32 bufferSize = 64;
					physx::PxU32 startIndex = 0;
					int numLinks2 = articulation->getLinks(physxLinks, bufferSize, startIndex);
					
					btMultiBodyLinkData *memPtr = (btMultiBodyLinkData *)chunk->m_oldPtr;
					for (int j = 0; j < numElem; j++, memPtr++)
					{
						int i = j + 1;
						
						
						memPtr->m_jointType = 0;//todo 
						memPtr->m_dofCount = physxLinks[i]->getInboundJointDof(); 
						memPtr->m_posVarCount = physxLinks[i]->getInboundJointDof(); //??

						physx::PxVec3 li = physxLinks[i]->getMassSpaceInertiaTensor();
						btVector3 localInertia(li[0], li[1], li[2]);
						localInertia.serialize(memPtr->m_linkInertia);

						memPtr->m_linkMass = physxLinks[i]->getMass();
						memPtr->m_parentIndex = i>0? physxLinks[i]->getInboundJoint()->getParentArticulationLink().getLinkIndex(): -1;
						memPtr->m_jointDamping = 0;//todophysxLinks[i]->getLinearDamping();//??
						memPtr->m_jointFriction = 0;//todo
						memPtr->m_jointLowerLimit = 0;//todogetLink(i).m_jointLowerLimit;
						memPtr->m_jointUpperLimit = 0;//todogetLink(i).m_jointUpperLimit;
						memPtr->m_jointMaxForce = 0;//todogetLink(i).m_jointMaxForce;
						memPtr->m_jointMaxVelocity = 0;//todogetLink(i).m_jointMaxVelocity;

						//getLink(i).m_eVector.serialize(memPtr->m_parentComToThisPivotOffset);
						//getLink(i).m_dVector.serialize(memPtr->m_thisPivotToThisComOffset);
						//getLink(i).m_zeroRotParentToThis.serialize(memPtr->m_zeroRotParentToThis);
						
						
						{
							char *name = (char *)ser.findNameForPointer(physxLinks[i]->getName());
							memPtr->m_linkName = (char *)ser.getUniquePointer(name);
							if (memPtr->m_linkName)
							{
								ser.serializeName(name);
							}
						}
						{
							char *name = (char *)ser.findNameForPointer(physxLinks[i]->getName());
							memPtr->m_jointName = (char *)ser.getUniquePointer(name);
							if (memPtr->m_jointName)
							{
								ser.serializeName(name);
							}
						}
						memPtr->m_linkCollider = (btCollisionObjectData *)ser.getUniquePointer(0);
					}
					ser.finalizeChunk(chunk, btMultiBodyLinkDataName, BT_ARRAY_CODE, (void *)articulation);
				}
				mbd->m_links = mbd->m_numLinks ? (btMultiBodyLinkData *)ser.getUniquePointer((void *)articulation) : 0;

				// Fill padding with zeros to appease msan.
#ifdef BT_USE_DOUBLE_PRECISION
				memset(mbd->m_padding, 0, sizeof(mbd->m_padding));
#endif

				const char* structType = btMultiBodyDataName;
				ser.finalizeChunk(chunk, structType, BT_MULTIBODY_CODE,0);
				int streamSizeInBytes = ser.getCurrentBufferSize();
				serverStatusOut.m_numDataStreamBytes = streamSizeInBytes;
				
			}
		}
#if 0
		btTransform rootTrans;
		rootTrans.setOrigin(pos);
		rootTrans.setRotation(orn);
		u2b.setRootTransformInWorld(rootTrans);
		bool ok = processImportedObjects(fileName, bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, u2b);
		if (ok)
		{
			if (m_data->m_sdfRecentLoadedBodies.size() == 1)
			{
				*bodyUniqueIdPtr = m_data->m_sdfRecentLoadedBodies[0];
			}
			m_data->m_sdfRecentLoadedBodies.clear();
		}
#endif
		return true;
	}
	return false;
}

bool PhysXServerCommandProcessor::processRequestBodyInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
	bool hasStatus = true;
	BT_PROFILE("CMD_REQUEST_BODY_INFO");

	const SdfRequestInfoArgs& sdfInfoArgs = clientCmd.m_sdfRequestInfoArgs;
	//stream info into memory
	int streamSizeInBytes = 0;  //createBodyInfoStream(sdfInfoArgs.m_bodyUniqueId, bufferServerToClient, bufferSizeInBytes);

	serverStatusOut.m_type = CMD_BODY_INFO_COMPLETED;

	
	serverStatusOut.m_numDataStreamBytes = streamSizeInBytes;

	return hasStatus;
}

bool PhysXServerCommandProcessor::processForwardDynamicsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
	bool hasStatus = true;

	BT_PROFILE("CMD_STEP_FORWARD_SIMULATION");
	
	int numArt = m_data->m_scene->getNbArticulations();
	
	{
		B3_PROFILE("clear Contacts");
		m_data->m_contactPoints.clear();
	}
	{
		B3_PROFILE("PhysX_simulate_fetchResults");
		
		m_data->m_scene->simulate(m_data->m_physicsDeltaTime);
		m_data->m_scene->fetchResults(true);
	}
	{
		B3_PROFILE("syncTransform");
		if (m_data->m_pluginManager.getRenderInterface())
		{

			//sync transforms...

			b3AlignedObjectArray<int> usedHandles;
			m_data->m_bodyHandles.getUsedHandles(usedHandles);

			for (int i = 0; i < usedHandles.size(); i++)
			{
				InternalPhysXBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(usedHandles[i]);

				
				physx::PxRigidActor* actor = 0;
				if (bodyHandle->m_rigidDynamic)
					actor = bodyHandle->m_rigidDynamic;
				if (bodyHandle->m_rigidStatic)
					actor = bodyHandle->m_rigidStatic;

				if (actor)
				{
					btTransform tr;
					tr.setIdentity();
					physx::PxTransform pt = actor->getGlobalPose();
					tr.setOrigin(btVector3(pt.p[0], pt.p[1], pt.p[2]));
					tr.setRotation(btQuaternion(pt.q.x, pt.q.y, pt.q.z, pt.q.w));
					btVector3 localScaling(1, 1, 1);//??
					MyPhysXUserData* ud = (MyPhysXUserData*)actor->userData;
					m_data->m_pluginManager.getRenderInterface()->syncTransform(ud->m_graphicsUniqueId, tr, localScaling);
				}

				if (bodyHandle->mArticulation)
				{
					
					physx::PxArticulationLink* physxLinks[64];
					physx::PxU32 bufferSize = 64;
					physx::PxU32 startIndex = 0;
					int numLinks2 = bodyHandle->mArticulation->getLinks(physxLinks, bufferSize, startIndex);

					for (int l = 0; l < numLinks2; l++)
					{
						MyPhysXUserData* ud = (MyPhysXUserData*)physxLinks[l]->userData;
						if (ud)
						{
							btTransform tr;
							tr.setIdentity();
							physx::PxTransform pt = physxLinks[l]->getGlobalPose();
							tr.setOrigin(btVector3(pt.p[0], pt.p[1], pt.p[2]));
							tr.setRotation(btQuaternion(pt.q.x, pt.q.y, pt.q.z, pt.q.w));
							btVector3 localScaling(1, 1, 1);//??
							m_data->m_pluginManager.getRenderInterface()->syncTransform(ud->m_graphicsUniqueId, tr, localScaling);
						}
					}
				}
			}
		}
		
		{
			B3_PROFILE("render");
			//m_data->m_pluginManager.getRenderInterface()->render();
			unsigned char* pixelRGBA = 0;
			int numRequestedPixels = 0;
			float* depthBuffer = 0;
			int* segmentationMaskBuffer = 0;
			int startPixelIndex = 0;
			
			int width = 1024;
			int height = 768;
			m_data->m_pluginManager.getRenderInterface()->getWidthAndHeight(width, height);
			int numPixelsCopied = 0;
			

			m_data->m_pluginManager.getRenderInterface()->copyCameraImageData(pixelRGBA, numRequestedPixels,
				depthBuffer, numRequestedPixels,
				segmentationMaskBuffer, numRequestedPixels,
				startPixelIndex, &width, &height, &numPixelsCopied);
		}
	}

	SharedMemoryStatus& serverCmd = serverStatusOut;
	serverCmd.m_type = CMD_STEP_FORWARD_SIMULATION_COMPLETED;
	return hasStatus;
}

bool PhysXServerCommandProcessor::processSendPhysicsParametersCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
	bool hasStatus = true;

	BT_PROFILE("CMD_SEND_PHYSICS_SIMULATION_PARAMETERS");

	if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_DELTA_TIME)
	{
		m_data->m_physicsDeltaTime = clientCmd.m_physSimParamArgs.m_deltaTime;
	}

	if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_GRAVITY)
	{
		btVector3 grav(clientCmd.m_physSimParamArgs.m_gravityAcceleration[0],
			clientCmd.m_physSimParamArgs.m_gravityAcceleration[1],
			clientCmd.m_physSimParamArgs.m_gravityAcceleration[2]);
		
		m_data->m_scene->setGravity(physx::PxVec3(grav[0], grav[1], grav[2]));

		if (m_data->m_verboseOutput)
		{
			b3Printf("Updated Gravity: %f,%f,%f", grav[0], grav[1], grav[2]);
		}
	}
#if 0
	if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS)
	{
		
		m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = clientCmd.m_physSimParamArgs.m_numSolverIterations;
	}


	if (clientCmd.m_updateFlags & SIM_PARAM_ENABLE_CONE_FRICTION)
	{
		if (clientCmd.m_physSimParamArgs.m_enableConeFriction)
		{
			m_data->m_dynamicsWorld->getSolverInfo().m_solverMode &=~SOLVER_DISABLE_IMPLICIT_CONE_FRICTION;
		} else
		{
			m_data->m_dynamicsWorld->getSolverInfo().m_solverMode |=SOLVER_DISABLE_IMPLICIT_CONE_FRICTION;
		}
	}
	if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DETERMINISTIC_OVERLAPPING_PAIRS)
	{
		m_data->m_dynamicsWorld->getDispatchInfo().m_deterministicOverlappingPairs = (clientCmd.m_physSimParamArgs.m_deterministicOverlappingPairs!=0);
	}

	if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_CCD_ALLOWED_PENETRATION)
	{
		m_data->m_dynamicsWorld->getDispatchInfo().m_allowedCcdPenetration = clientCmd.m_physSimParamArgs.m_allowedCcdPenetration;
	}
	
	if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_JOINT_FEEDBACK_MODE)
	{
		gJointFeedbackInWorldSpace = (clientCmd.m_physSimParamArgs.m_jointFeedbackMode&JOINT_FEEDBACK_IN_WORLD_SPACE)!=0;
		gJointFeedbackInJointFrame = (clientCmd.m_physSimParamArgs.m_jointFeedbackMode&JOINT_FEEDBACK_IN_JOINT_FRAME)!=0;
	}

	
	if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_REAL_TIME_SIMULATION)
	{
		m_data->m_useRealTimeSimulation = (clientCmd.m_physSimParamArgs.m_useRealTimeSimulation!=0);
	}
					
	//see 
	if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_INTERNAL_SIMULATION_FLAGS)
	{
		//these flags are for internal/temporary/easter-egg/experimental demo purposes, use at own risk
		gInternalSimFlags = clientCmd.m_physSimParamArgs.m_internalSimFlags;
	}

	

	if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_SOLVER_RESIDULAL_THRESHOLD)
	{
		m_data->m_dynamicsWorld->getSolverInfo().m_leastSquaresResidualThreshold = clientCmd.m_physSimParamArgs.m_solverResidualThreshold;
	}
	

	if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_CONTACT_BREAKING_THRESHOLD)
	{
		gContactBreakingThreshold = clientCmd.m_physSimParamArgs.m_contactBreakingThreshold;
	}
	
	if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_CONTACT_SLOP)
	{
		m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = clientCmd.m_physSimParamArgs.m_contactSlop;
	}

	if (clientCmd.m_updateFlags&SIM_PARAM_ENABLE_SAT)
	{
		m_data->m_dynamicsWorld->getDispatchInfo().m_enableSatConvex = clientCmd.m_physSimParamArgs.m_enableSAT!=0;
	}


	if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_COLLISION_FILTER_MODE)
	{
		m_data->m_broadphaseCollisionFilterCallback->m_filterMode = clientCmd.m_physSimParamArgs.m_collisionFilterMode;
	}

	if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_USE_SPLIT_IMPULSE)
	{
		m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulse = clientCmd.m_physSimParamArgs.m_useSplitImpulse;
	}
	if (clientCmd.m_updateFlags &SIM_PARAM_UPDATE_SPLIT_IMPULSE_PENETRATION_THRESHOLD)
	{
		m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulsePenetrationThreshold = clientCmd.m_physSimParamArgs.m_splitImpulsePenetrationThreshold;
	}
					

    if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_NUM_SIMULATION_SUB_STEPS)
    {
        m_data->m_numSimulationSubSteps = clientCmd.m_physSimParamArgs.m_numSimulationSubSteps;
    }

	if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_CONTACT_ERP)
    {
        m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = clientCmd.m_physSimParamArgs.m_defaultContactERP;
    }

	if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_NON_CONTACT_ERP)
    {
        m_data->m_dynamicsWorld->getSolverInfo().m_erp = clientCmd.m_physSimParamArgs.m_defaultNonContactERP;
    }

	if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_FRICTION_ERP)
    {
        m_data->m_dynamicsWorld->getSolverInfo().m_frictionERP = clientCmd.m_physSimParamArgs.m_frictionERP;
    }

    if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_GLOBAL_CFM)
    {
        m_data->m_dynamicsWorld->getSolverInfo().m_globalCfm = clientCmd.m_physSimParamArgs.m_defaultGlobalCFM;
    }

    if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_FRICTION_CFM)
    {
		m_data->m_dynamicsWorld->getSolverInfo().m_frictionCFM = clientCmd.m_physSimParamArgs.m_frictionCFM;
    }

	if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_RESTITUTION_VELOCITY_THRESHOLD)
    {
        m_data->m_dynamicsWorld->getSolverInfo().m_restitutionVelocityThreshold = clientCmd.m_physSimParamArgs.m_restitutionVelocityThreshold;
    }

					

	if (clientCmd.m_updateFlags&SIM_PARAM_ENABLE_FILE_CACHING)
    {
		b3EnableFileCaching(clientCmd.m_physSimParamArgs.m_enableFileCaching);
    }

#endif

	SharedMemoryStatus& serverCmd = serverStatusOut;
	serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
	return hasStatus;
}

bool PhysXServerCommandProcessor::processRequestActualStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
	bool hasStatus = true;
	serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_FAILED;

	int bodyUniqueId = clientCmd.m_requestActualStateInformationCommandArgument.m_bodyUniqueId;
	InternalPhysXBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(bodyUniqueId);

	if (bodyHandle->mArticulation)
	{
		BT_PROFILE("CMD_REQUEST_ACTUAL_STATE");
		if (m_data->m_verboseOutput)
		{
			b3Printf("Sending the actual state (Q,U)");
		}

		{
			SharedMemoryStatus& serverCmd = serverStatusOut;
			serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED;

			serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId;
			serverCmd.m_sendActualStateArgs.m_numLinks = bodyHandle->mArticulation->getNbLinks()-1; //skip base!

			int totalDegreeOfFreedomQ = 0;
			int totalDegreeOfFreedomU = 0;

			if (serverCmd.m_sendActualStateArgs.m_numLinks >= MAX_DEGREE_OF_FREEDOM)
			{
				serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_FAILED;
				hasStatus = true;
				return hasStatus;
			}

			bool computeForwardKinematics = ((clientCmd.m_updateFlags & ACTUAL_STATE_COMPUTE_FORWARD_KINEMATICS) != 0);
			bool computeLinkVelocities = ((clientCmd.m_updateFlags & ACTUAL_STATE_COMPUTE_LINKVELOCITY) != 0);

			if (computeForwardKinematics || computeLinkVelocities)
			{
				//todo:check this
			}

			physx::PxArticulationLink* physxLinks[64];
			physx::PxU32 bufferSize = 64;
			physx::PxU32 startIndex = 0;
			int numLinks2 = bodyHandle->mArticulation->getLinks(physxLinks, bufferSize, startIndex);

			{
				serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[0] = 0;
				serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[1] = 0;
				serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[2] = 0;

				serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[3] = 0;
				serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[4] = 0;
				serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[5] = 0;
				serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[6] = 1;

				physx::PxArticulationLink* l = physxLinks[0];
				physx::PxVec3 pos = l->getGlobalPose().p;
				physx::PxQuat orn = l->getGlobalPose().q;
				//base position in world space, carthesian
				serverCmd.m_sendActualStateArgs.m_actualStateQ[0] = pos[0];
				serverCmd.m_sendActualStateArgs.m_actualStateQ[1] = pos[1];
				serverCmd.m_sendActualStateArgs.m_actualStateQ[2] = pos[2];

				//base orientation, quaternion x,y,z,w, in world space, carthesian
				serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = orn.x;
				serverCmd.m_sendActualStateArgs.m_actualStateQ[4] = orn.y;
				serverCmd.m_sendActualStateArgs.m_actualStateQ[5] = orn.z;
				serverCmd.m_sendActualStateArgs.m_actualStateQ[6] = orn.w;
				totalDegreeOfFreedomQ += 7;  //pos + quaternion

				physx::PxVec3 linVel = l->getLinearVelocity();
				physx::PxVec3 angVel = l->getAngularVelocity();

				//base linear velocity (in world space, carthesian)
				serverCmd.m_sendActualStateArgs.m_actualStateQdot[0] = linVel.x;
				serverCmd.m_sendActualStateArgs.m_actualStateQdot[1] = linVel.y;
				serverCmd.m_sendActualStateArgs.m_actualStateQdot[2] = linVel.z;

				//base angular velocity (in world space, carthesian)
				serverCmd.m_sendActualStateArgs.m_actualStateQdot[3] = angVel.x;
				serverCmd.m_sendActualStateArgs.m_actualStateQdot[4] = angVel.y;
				serverCmd.m_sendActualStateArgs.m_actualStateQdot[5] = angVel.z;
				totalDegreeOfFreedomU += 6;                                      //3 linear and 3 angular DOF
			}

			physx::PxArticulationCache* c = bodyHandle->mArticulation->createCache();
			bodyHandle->mArticulation->copyInternalStateToCache(*c, physx::PxArticulationCache::ePOSITION | physx::PxArticulationCache::eVELOCITY);// physx::PxArticulationCache::eALL);

			btAlignedObjectArray<int> dofStarts;
			dofStarts.resize(numLinks2);
			dofStarts[0] = 0; //We know that the root link does not have a joint
							  //cache positions in PhysX may be reshuffled, see
							  //http://gameworksdocs.nvidia.com/PhysX/4.0/documentation/PhysXGuide/Manual/Articulations.html

			for (int i = 1; i < numLinks2; ++i)
			{
				int  llIndex = physxLinks[i]->getLinkIndex();
				int  dofs = physxLinks[i]->getInboundJointDof();

				dofStarts[llIndex] = dofs;
			}

			int count = 0;
			for (int i = 1; i < numLinks2; ++i)
			{
				int  dofs = dofStarts[i];
				dofStarts[i] = count;
				count += dofs;
			}

			//start at 1, 0 is the base/root, handled above
			for (int l = 1; l < numLinks2; l++)
			{
				int  dofs = physxLinks[l]->getInboundJointDof();
				int  llIndex = physxLinks[l]->getLinkIndex();
				
				for (int d = 0; d < dofs; d++)
				{
					serverCmd.m_sendActualStateArgs.m_actualStateQ[totalDegreeOfFreedomQ++] = c->jointPosition[dofStarts[llIndex + d]];
					serverCmd.m_sendActualStateArgs.m_actualStateQ[totalDegreeOfFreedomU++] = c->jointVelocity[dofStarts[llIndex + d]];
				}
			}

			serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ;
			serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU;

			hasStatus = true;
		}
	}


	return hasStatus;
}

bool PhysXServerCommandProcessor::processResetSimulationCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
	bool hasStatus = true;
	BT_PROFILE("CMD_RESET_SIMULATION");

	resetSimulation();

	SharedMemoryStatus& serverCmd = serverStatusOut;
	serverCmd.m_type = CMD_RESET_SIMULATION_COMPLETED;
	return hasStatus;
}

bool PhysXServerCommandProcessor::receiveStatus(struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
	return false;
}

#endif  //BT_ENABLE_PHYSX