bullet3/examples/SharedMemory/PhysicsClientSharedMemory.cpp

#include "PhysicsClientSharedMemory.h"
#include "PosixSharedMemory.h"
#include "Win32SharedMemory.h"
#include "Bullet3Common/b3AlignedObjectArray.h"
#include "Bullet3Common/b3Vector3.h"
#include <string.h>
#include "Bullet3Common/b3HashMap.h"
#include "Bullet3Common/b3Logging.h"
#include "../Utils/b3ResourcePath.h"
#include "../../Extras/Serialize/BulletFileLoader/btBulletFile.h"
#include "../../Extras/Serialize/BulletFileLoader/autogenerated/bullet.h"
#include "SharedMemoryBlock.h"
#include "BodyJointInfoUtility.h"
#include "SharedMemoryUserData.h"
#include "LinearMath/btQuickprof.h"

struct BodyJointInfoCache
{
	std::string m_baseName;
	b3AlignedObjectArray<b3JointInfo> m_jointInfo;
	std::string m_bodyName;
	btAlignedObjectArray<int> m_userDataIds;
	int m_numDofs;
	~BodyJointInfoCache()
	{
	}
};

struct PhysicsClientSharedMemoryInternalData
{
	SharedMemoryInterface* m_sharedMemory;
	bool m_ownsSharedMemory;
	SharedMemoryBlock* m_testBlock1;

	btAlignedObjectArray<CProfileSample*> m_profileTimings;
	btHashMap<btHashString, std::string*> m_profileTimingStringArray;

	btHashMap<btHashInt, BodyJointInfoCache*> m_bodyJointMap;
	btHashMap<btHashInt, b3UserConstraint> m_userConstraintInfoMap;

	btAlignedObjectArray<TmpFloat3> m_debugLinesFrom;
	btAlignedObjectArray<TmpFloat3> m_debugLinesTo;
	btAlignedObjectArray<TmpFloat3> m_debugLinesColor;

	int m_cachedCameraPixelsWidth;
	int m_cachedCameraPixelsHeight;
	btAlignedObjectArray<unsigned char> m_cachedCameraPixelsRGBA;
	btAlignedObjectArray<float> m_cachedCameraDepthBuffer;
	btAlignedObjectArray<int> m_cachedSegmentationMaskBuffer;

	btAlignedObjectArray<b3ContactPointData> m_cachedContactPoints;
	btAlignedObjectArray<b3OverlappingObject> m_cachedOverlappingObjects;
	btAlignedObjectArray<b3VisualShapeData> m_cachedVisualShapes;
	btAlignedObjectArray<b3CollisionShapeData> m_cachedCollisionShapes;

	btAlignedObjectArray<b3VRControllerEvent> m_cachedVREvents;
	btAlignedObjectArray<b3KeyboardEvent> m_cachedKeyboardEvents;
	btAlignedObjectArray<b3MouseEvent> m_cachedMouseEvents;
	btAlignedObjectArray<double> m_cachedMassMatrix;
	btAlignedObjectArray<b3RayHitInfo> m_raycastHits;

	btAlignedObjectArray<int> m_bodyIdsRequestInfo;
	btAlignedObjectArray<int> m_constraintIdsRequestInfo;

	btAlignedObjectArray<int> m_userDataIdsRequestInfo;
	btHashMap<btHashInt, SharedMemoryUserData> m_userDataMap;
	btHashMap<SharedMemoryUserDataHashKey, int> m_userDataHandleLookup;

	SharedMemoryStatus m_tempBackupServerStatus;

	SharedMemoryStatus m_lastServerStatus;

	SendActualStateSharedMemoryStorage m_cachedState;
	
	int m_counter;

	bool m_isConnected;
	bool m_waitingForServer;
	bool m_hasLastServerStatus;
	int m_sharedMemoryKey;
	bool m_verboseOutput;
	double m_timeOutInSeconds;

	

	PhysicsClientSharedMemoryInternalData()
		: m_sharedMemory(0),
		  m_ownsSharedMemory(false),
		  m_testBlock1(0),
		  m_cachedCameraPixelsWidth(0),
		  m_cachedCameraPixelsHeight(0),
		  m_counter(0),
		  m_isConnected(false),
		  m_waitingForServer(false),
		  m_hasLastServerStatus(false),
		  m_sharedMemoryKey(SHARED_MEMORY_KEY),
		  m_verboseOutput(false),
		  m_timeOutInSeconds(1e30)
	{
	}

	void processServerStatus();

	bool canSubmitCommand() const;
};

int PhysicsClientSharedMemory::getNumBodies() const
{
	return m_data->m_bodyJointMap.size();
}

int PhysicsClientSharedMemory::getBodyUniqueId(int serialIndex) const
{
	if ((serialIndex >= 0) && (serialIndex < getNumBodies()))
	{
		return m_data->m_bodyJointMap.getKeyAtIndex(serialIndex).getUid1();
	}
	return -1;
}

bool PhysicsClientSharedMemory::getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const
{
	BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
	if (bodyJointsPtr && *bodyJointsPtr)
	{
		BodyJointInfoCache* bodyJoints = *bodyJointsPtr;
		strcpy(info.m_baseName, bodyJoints->m_baseName.c_str());
		strcpy(info.m_bodyName, bodyJoints->m_bodyName.c_str());
		return true;
	}

	return false;
}

int PhysicsClientSharedMemory::getNumJoints(int bodyUniqueId) const
{
	BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
	if (bodyJointsPtr && *bodyJointsPtr)
	{
		BodyJointInfoCache* bodyJoints = *bodyJointsPtr;

		return bodyJoints->m_jointInfo.size();
	}
	return 0;
}

int PhysicsClientSharedMemory::getNumDofs(int bodyUniqueId) const
{
        BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
        if (bodyJointsPtr && *bodyJointsPtr)
        {
                BodyJointInfoCache* bodyJoints = *bodyJointsPtr;
                return bodyJoints->m_numDofs;
        }
        return 0;
}

bool PhysicsClientSharedMemory::getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo& info) const
{
	BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
	if (bodyJointsPtr && *bodyJointsPtr)
	{
		BodyJointInfoCache* bodyJoints = *bodyJointsPtr;
		if ((jointIndex >= 0) && (jointIndex < bodyJoints->m_jointInfo.size()))
		{
			info = bodyJoints->m_jointInfo[jointIndex];
			info.m_qSize = 0;
			info.m_uSize = 0;
			switch (info.m_jointType)
			{
				case eSphericalType:
				{
					info.m_qSize = 4;//quaterion x,y,z,w
					info.m_uSize = 3;
					break;
				}
				case ePlanarType:
				{
					info.m_qSize = 2;
					info.m_uSize = 2;
					break;
				}
				case ePrismaticType:
				case eRevoluteType:
				{
					info.m_qSize = 1;
					info.m_uSize = 1;
					break;
				}

				default:
				{
				}
			}
			return true;
		}
	}
	return false;
}

int PhysicsClientSharedMemory::getNumUserConstraints() const
{
	return m_data->m_userConstraintInfoMap.size();
}

int PhysicsClientSharedMemory::getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const
{
	b3UserConstraint* constraintPtr = m_data->m_userConstraintInfoMap[constraintUniqueId];
	if (constraintPtr)
	{
		info = *constraintPtr;
		return 1;
	}
	return 0;
}

int PhysicsClientSharedMemory::getUserConstraintId(int serialIndex) const
{
	if ((serialIndex >= 0) && (serialIndex < getNumUserConstraints()))
	{
		return m_data->m_userConstraintInfoMap.getKeyAtIndex(serialIndex).getUid1();
	}
	return -1;
}

PhysicsClientSharedMemory::PhysicsClientSharedMemory()

{
	m_data = new PhysicsClientSharedMemoryInternalData;

#ifdef _WIN32
	m_data->m_sharedMemory = new Win32SharedMemoryClient();
#else
	m_data->m_sharedMemory = new PosixSharedMemory();
#endif
	m_data->m_ownsSharedMemory = true;
}

PhysicsClientSharedMemory::~PhysicsClientSharedMemory()
{
	if (m_data->m_isConnected)
	{
		disconnectSharedMemory();
	}
	resetData();

	for (int i = 0; i < m_data->m_profileTimingStringArray.size(); i++)
	{
		std::string** str = m_data->m_profileTimingStringArray.getAtIndex(i);
		if (str)
		{
			delete *str;
		}
	}
	m_data->m_profileTimingStringArray.clear();

	if (m_data->m_ownsSharedMemory)
	{
		delete m_data->m_sharedMemory;
	}
	delete m_data;
}

void PhysicsClientSharedMemory::removeCachedBody(int bodyUniqueId)
{
	BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
	if (bodyJointsPtr && *bodyJointsPtr)
	{
		for (int i = 0; i < (*bodyJointsPtr)->m_userDataIds.size(); i++)
		{
			const int userDataId = (*bodyJointsPtr)->m_userDataIds[i];
			SharedMemoryUserData* userData = m_data->m_userDataMap[userDataId];
			m_data->m_userDataHandleLookup.remove(SharedMemoryUserDataHashKey(userData));
			m_data->m_userDataMap.remove(userDataId);
		}
		delete (*bodyJointsPtr);
		m_data->m_bodyJointMap.remove(bodyUniqueId);
	}
}
void PhysicsClientSharedMemory::resetData()
{
	m_data->m_debugLinesFrom.clear();
	m_data->m_debugLinesTo.clear();
	m_data->m_debugLinesColor.clear();
	for (int i = 0; i < m_data->m_bodyJointMap.size(); i++)
	{
		BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap.getAtIndex(i);
		if (bodyJointsPtr && *bodyJointsPtr)
		{
			delete (*bodyJointsPtr);
		}
	}
	m_data->m_bodyJointMap.clear();
	m_data->m_userConstraintInfoMap.clear();
	m_data->m_userDataHandleLookup.clear();
	m_data->m_userDataMap.clear();
}
void PhysicsClientSharedMemory::setSharedMemoryKey(int key)
{
	m_data->m_sharedMemoryKey = key;
}

void PhysicsClientSharedMemory::setSharedMemoryInterface(class SharedMemoryInterface* sharedMem)
{
	if (sharedMem)
	{
		if (m_data->m_sharedMemory && m_data->m_ownsSharedMemory)
		{
			delete m_data->m_sharedMemory;
		}
		m_data->m_ownsSharedMemory = false;
		m_data->m_sharedMemory = sharedMem;
	};
}

void PhysicsClientSharedMemory::disconnectSharedMemory()
{
	if (m_data->m_isConnected && m_data->m_sharedMemory)
	{
		m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE);
	}
	m_data->m_isConnected = false;
}

bool PhysicsClientSharedMemory::isConnected() const
{
	return m_data->m_isConnected && (m_data->m_testBlock1->m_magicId == SHARED_MEMORY_MAGIC_NUMBER);
}

bool PhysicsClientSharedMemory::connect()
{
	/// server always has to create and initialize shared memory
	bool allowCreation = false;
	m_data->m_testBlock1 = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(
		m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE, allowCreation);

	if (m_data->m_testBlock1)
	{
		if (m_data->m_testBlock1->m_magicId != SHARED_MEMORY_MAGIC_NUMBER)
		{
			//there is no chance people are still using this software 100 years from now ;-)
			if ((m_data->m_testBlock1->m_magicId < 211705023) &&
				(m_data->m_testBlock1->m_magicId >= 201705023))
			{
				b3Error("Error: physics server version mismatch (expected %d got %d)\n", SHARED_MEMORY_MAGIC_NUMBER, m_data->m_testBlock1->m_magicId);
			}
			else
			{
				b3Error("Error connecting to shared memory: please start server before client\n");
			}
			m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey,
														SHARED_MEMORY_SIZE);
			m_data->m_testBlock1 = 0;
			return false;
		}
		else
		{
			if (m_data->m_verboseOutput)
			{
				b3Printf("Connected to existing shared memory, status OK.\n");
			}
			m_data->m_isConnected = true;
		}
	}
	else
	{
		//b3Warning("Cannot connect to shared memory");
		return false;
	}
#if 0
	if (m_data->m_isConnected)
	{
		//get all existing bodies and body info...

		SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
		//now transfer the information of the individual objects etc.
		command.m_type = CMD_REQUEST_BODY_INFO;
		command.m_sdfRequestInfoArgs.m_bodyUniqueId = 37;
		submitClientCommand(command);

		double startTime = clock.getTimeInSeconds();
		double timeOutInSeconds = 10;

		const SharedMemoryStatus* status = 0;

		while ((status == 0) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds))
		{
			status = processServerStatus();
		
		}


		//submitClientCommand(command);


	}
#endif
	return true;
}

///todo(erwincoumans) refactor this: merge with PhysicsDirect::processBodyJointInfo
void PhysicsClientSharedMemory::processBodyJointInfo(int bodyUniqueId, const SharedMemoryStatus& serverCmd)
{
	bParse::btBulletFile bf(
		&this->m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0],
		serverCmd.m_numDataStreamBytes);
	bf.setFileDNAisMemoryDNA();
	bf.parse(false);

	BodyJointInfoCache* bodyJoints = new BodyJointInfoCache;
	m_data->m_bodyJointMap.insert(bodyUniqueId, bodyJoints);
	bodyJoints->m_bodyName = serverCmd.m_dataStreamArguments.m_bodyName;

	for (int i = 0; i < bf.m_multiBodies.size(); i++)
	{
		int flag = bf.getFlags();
		if ((flag & bParse::FD_DOUBLE_PRECISION) != 0)
		{
			Bullet::btMultiBodyDoubleData* mb =
				(Bullet::btMultiBodyDoubleData*)bf.m_multiBodies[i];

			bodyJoints->m_baseName = mb->m_baseName;
			addJointInfoFromMultiBodyData(mb, bodyJoints, m_data->m_verboseOutput);
		}
		else
		{
			Bullet::btMultiBodyFloatData* mb =
				(Bullet::btMultiBodyFloatData*)bf.m_multiBodies[i];
			bodyJoints->m_baseName = mb->m_baseName;
			addJointInfoFromMultiBodyData(mb, bodyJoints, m_data->m_verboseOutput);
		}
	}
	if (bf.ok())
	{
		if (m_data->m_verboseOutput)
		{
			b3Printf("Received robot description ok!\n");
		}
	}
	else
	{
		b3Warning("Robot description not received");
	}
}

template <typename T, typename U>
void addJointInfoFromConstraint(int linkIndex, const T* con, U* bodyJoints, bool verboseOutput)
{
	b3JointInfo info;
	info.m_jointName[0] = 0;
	info.m_linkName[0] = 0;
	info.m_flags = 0;
	info.m_jointIndex = linkIndex;
	info.m_qIndex = linkIndex + 7;
	info.m_uIndex = linkIndex + 6;
	//derive type from limits

	if (con->m_typeConstraintData.m_name)
	{
		strcpy(info.m_jointName, con->m_typeConstraintData.m_name);

		//info.m_linkName = strDup(con->m_typeConstraintData.m_name);
	}

	btVector3 linearLowerLimit(con->m_linearLowerLimit.m_floats[0], con->m_linearLowerLimit.m_floats[1], con->m_linearLowerLimit.m_floats[2]);
	btVector3 linearUpperLimit(con->m_linearUpperLimit.m_floats[0], con->m_linearUpperLimit.m_floats[1], con->m_linearUpperLimit.m_floats[2]);
	btVector3 angularLowerLimit(con->m_angularLowerLimit.m_floats[0], con->m_angularLowerLimit.m_floats[1], con->m_angularLowerLimit.m_floats[2]);
	btVector3 angularUpperLimit(con->m_angularUpperLimit.m_floats[0], con->m_angularUpperLimit.m_floats[1], con->m_angularUpperLimit.m_floats[2]);

	//very simple, rudimentary extraction of constaint type, from limits
	info.m_jointType = eFixedType;
	info.m_jointDamping = 0;      //mb->m_links[link].m_jointDamping;
	info.m_jointFriction = 0;     //mb->m_links[link].m_jointFriction;
	info.m_jointLowerLimit = 0;   //mb->m_links[link].m_jointLowerLimit;
	info.m_jointUpperLimit = 0;   //mb->m_links[link].m_jointUpperLimit;
	info.m_jointMaxForce = 0;     //mb->m_links[link].m_jointMaxForce;
	info.m_jointMaxVelocity = 0;  //mb->m_links[link].m_jointMaxVelocity;

	if (linearLowerLimit.isZero() && linearUpperLimit.isZero() && angularLowerLimit.isZero() && angularUpperLimit.isZero())
	{
		info.m_jointType = eFixedType;
	}
	else
	{
		if (linearLowerLimit.isZero() && linearUpperLimit.isZero())
		{
			info.m_jointType = eRevoluteType;
			btVector3 limitRange = angularLowerLimit.absolute() + angularUpperLimit.absolute();
			int limitAxis = limitRange.maxAxis();
			info.m_jointLowerLimit = angularLowerLimit[limitAxis];
			info.m_jointUpperLimit = angularUpperLimit[limitAxis];
		}
		else
		{
			info.m_jointType = ePrismaticType;
			btVector3 limitRange = linearLowerLimit.absolute() + linearUpperLimit.absolute();
			int limitAxis = limitRange.maxAxis();
			info.m_jointLowerLimit = linearLowerLimit[limitAxis];
			info.m_jointUpperLimit = linearUpperLimit[limitAxis];
		}
	}

	//if (mb->m_links[link].m_linkName) {

	if ((info.m_jointType == eRevoluteType) ||
		(info.m_jointType == ePrismaticType))
	{
		info.m_flags |= JOINT_HAS_MOTORIZED_POWER;
	}
	bodyJoints->m_jointInfo.push_back(info);
};

const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus()
{
	// SharedMemoryStatus* stat = 0;

	if (!m_data->m_testBlock1)
	{
		m_data->m_lastServerStatus.m_type = CMD_SHARED_MEMORY_NOT_INITIALIZED;
		return &m_data->m_lastServerStatus;
	}

	if (!m_data->m_waitingForServer)
	{
		return 0;
	}

	if (m_data->m_testBlock1->m_magicId != SHARED_MEMORY_MAGIC_NUMBER)
	{
		m_data->m_lastServerStatus.m_type = CMD_SHARED_MEMORY_NOT_INITIALIZED;
		return &m_data->m_lastServerStatus;
	}

	if (m_data->m_testBlock1->m_numServerCommands >
		m_data->m_testBlock1->m_numProcessedServerCommands)
	{
		B3_PROFILE("processServerCMD");
		btAssert(m_data->m_testBlock1->m_numServerCommands ==
				 m_data->m_testBlock1->m_numProcessedServerCommands + 1);

		const SharedMemoryStatus& serverCmd = m_data->m_testBlock1->m_serverCommands[0];

		if (serverCmd.m_type==CMD_ACTUAL_STATE_UPDATE_COMPLETED)
		{
			SendActualStateSharedMemoryStorage* serverState = (SendActualStateSharedMemoryStorage*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
			m_data->m_cachedState = *serverState;
			//ideally we provided a 'getCachedState' but that would require changing the API, so we store a pointer instead.
			m_data->m_testBlock1->m_serverCommands[0].m_sendActualStateArgs.m_stateDetails = &m_data->m_cachedState;
		}

		m_data->m_lastServerStatus = serverCmd;

		//       EnumSharedMemoryServerStatus s = (EnumSharedMemoryServerStatus)serverCmd.m_type;
		// consume the command

		switch (serverCmd.m_type)
		{
			case CMD_CLIENT_COMMAND_COMPLETED:
			{
				B3_PROFILE("CMD_CLIENT_COMMAND_COMPLETED");

				if (m_data->m_verboseOutput)
				{
					b3Printf("Server completed command");
				}
				break;
			}

			case CMD_MJCF_LOADING_COMPLETED:
			{
				B3_PROFILE("CMD_MJCF_LOADING_COMPLETED");

				if (m_data->m_verboseOutput)
				{
					b3Printf("Server loading the MJCF OK\n");
				}
				break;
			}
			case CMD_SDF_LOADING_COMPLETED:
			{
				B3_PROFILE("CMD_SDF_LOADING_COMPLETED");

				if (m_data->m_verboseOutput)
				{
					b3Printf("Server loading the SDF OK\n");
				}

				break;
			}

			case CMD_CREATE_MULTI_BODY_COMPLETED:
			case CMD_URDF_LOADING_COMPLETED:
			{
				B3_PROFILE("CMD_URDF_LOADING_COMPLETED");

				if (m_data->m_verboseOutput)
				{
					b3Printf("Server loading the URDF OK\n");
				}

				if (serverCmd.m_numDataStreamBytes > 0)
				{
					bParse::btBulletFile bf(
						this->m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor,
						serverCmd.m_numDataStreamBytes);
					bf.setFileDNAisMemoryDNA();
					bf.parse(false);
					int bodyUniqueId = serverCmd.m_dataStreamArguments.m_bodyUniqueId;

					BodyJointInfoCache* bodyJoints = new BodyJointInfoCache;
					m_data->m_bodyJointMap.insert(bodyUniqueId, bodyJoints);
					bodyJoints->m_bodyName = serverCmd.m_dataStreamArguments.m_bodyName;

					for (int i = 0; i < bf.m_constraints.size(); i++)
					{
						int flag = bf.getFlags();

						if ((flag & bParse::FD_DOUBLE_PRECISION) != 0)
						{
							Bullet::btGeneric6DofSpring2ConstraintDoubleData2* con =
								(Bullet::btGeneric6DofSpring2ConstraintDoubleData2*)bf.m_constraints[i];
							addJointInfoFromConstraint(i, con, bodyJoints, m_data->m_verboseOutput);
						}
						else
						{
							Bullet::btGeneric6DofSpring2ConstraintData* con =
								(Bullet::btGeneric6DofSpring2ConstraintData*)bf.m_constraints[i];
							addJointInfoFromConstraint(i, con, bodyJoints, m_data->m_verboseOutput);
						}
					}

					for (int i = 0; i < bf.m_multiBodies.size(); i++)
					{
						int flag = bf.getFlags();

						if ((flag & bParse::FD_DOUBLE_PRECISION) != 0)
						{
							Bullet::btMultiBodyDoubleData* mb =
								(Bullet::btMultiBodyDoubleData*)bf.m_multiBodies[i];
							if (mb->m_baseName)
							{
								bodyJoints->m_baseName = mb->m_baseName;
							}
							addJointInfoFromMultiBodyData(mb, bodyJoints, m_data->m_verboseOutput);
						}
						else
						{
							Bullet::btMultiBodyFloatData* mb =
								(Bullet::btMultiBodyFloatData*)bf.m_multiBodies[i];
							if (mb->m_baseName)
							{
								bodyJoints->m_baseName = mb->m_baseName;
							}
							addJointInfoFromMultiBodyData(mb, bodyJoints, m_data->m_verboseOutput);
						}
					}
					if (bf.ok())
					{
						if (m_data->m_verboseOutput)
						{
							b3Printf("Received robot description ok!\n");
						}
					}
					else
					{
						b3Warning("Robot description not received");
					}
				}
				break;
			}
			case CMD_DESIRED_STATE_RECEIVED_COMPLETED:
			{
				B3_PROFILE("CMD_DESIRED_STATE_RECEIVED_COMPLETED");

				if (m_data->m_verboseOutput)
				{
					b3Printf("Server received desired state");
				}
				break;
			}
			case CMD_STEP_FORWARD_SIMULATION_COMPLETED:
			{
				B3_PROFILE("CMD_STEP_FORWARD_SIMULATION_COMPLETED");

				if (m_data->m_verboseOutput)
				{
					b3Printf("Server completed step simulation");
				}
				break;
			}
			case CMD_URDF_LOADING_FAILED:
			{
				B3_PROFILE("CMD_URDF_LOADING_FAILED");

				if (m_data->m_verboseOutput)
				{
					b3Printf("Server failed loading the URDF...\n");
				}

				break;
			}
			case CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED:
			{
				break;
			}
			case CMD_USER_CONSTRAINT_INFO_COMPLETED:
			{
				B3_PROFILE("CMD_USER_CONSTRAINT_INFO_COMPLETED");

				int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId;
				m_data->m_userConstraintInfoMap.insert(cid, serverCmd.m_userConstraintResultArgs);
				break;
			}
			case CMD_USER_CONSTRAINT_COMPLETED:
			{
				B3_PROFILE("CMD_USER_CONSTRAINT_COMPLETED");
				int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId;
				m_data->m_userConstraintInfoMap.insert(cid, serverCmd.m_userConstraintResultArgs);
				break;
			}
			case CMD_REMOVE_USER_CONSTRAINT_COMPLETED:
			{
				B3_PROFILE("CMD_REMOVE_USER_CONSTRAINT_COMPLETED");
				int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId;
				m_data->m_userConstraintInfoMap.remove(cid);
				break;
			}
			case CMD_CHANGE_USER_CONSTRAINT_COMPLETED:
			{
				B3_PROFILE("CMD_CHANGE_USER_CONSTRAINT_COMPLETED");

				int cid = serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId;
				b3UserConstraint* userConstraintPtr = m_data->m_userConstraintInfoMap[cid];
				if (userConstraintPtr)
				{
					const b3UserConstraint* serverConstraint = &serverCmd.m_userConstraintResultArgs;
					if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_PIVOT_IN_B)
					{
						userConstraintPtr->m_childFrame[0] = serverConstraint->m_childFrame[0];
						userConstraintPtr->m_childFrame[1] = serverConstraint->m_childFrame[1];
						userConstraintPtr->m_childFrame[2] = serverConstraint->m_childFrame[2];
					}
					if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B)
					{
						userConstraintPtr->m_childFrame[3] = serverConstraint->m_childFrame[3];
						userConstraintPtr->m_childFrame[4] = serverConstraint->m_childFrame[4];
						userConstraintPtr->m_childFrame[5] = serverConstraint->m_childFrame[5];
						userConstraintPtr->m_childFrame[6] = serverConstraint->m_childFrame[6];
					}
					if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_MAX_FORCE)
					{
						userConstraintPtr->m_maxAppliedForce = serverConstraint->m_maxAppliedForce;
					}
					if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_RATIO)
					{
						userConstraintPtr->m_gearRatio = serverConstraint->m_gearRatio;
					}
					if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET)
					{
						userConstraintPtr->m_relativePositionTarget = serverConstraint->m_relativePositionTarget;
					}
					if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_ERP)
					{
						userConstraintPtr->m_erp = serverConstraint->m_erp;
					}
					if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK)
					{
						userConstraintPtr->m_gearAuxLink = serverConstraint->m_gearAuxLink;
					}
				}
				break;
			}

			case CMD_USER_CONSTRAINT_FAILED:
			{
				B3_PROFILE("CMD_USER_CONSTRAINT_FAILED");
				b3Warning("createConstraint failed");
				break;
			}
			case CMD_REMOVE_USER_CONSTRAINT_FAILED:
			{
				B3_PROFILE("CMD_REMOVE_USER_CONSTRAINT_FAILED");
				b3Warning("removeConstraint failed");
				break;
			}
			case CMD_CHANGE_USER_CONSTRAINT_FAILED:
			{
				B3_PROFILE("CMD_CHANGE_USER_CONSTRAINT_FAILED");
				//b3Warning("changeConstraint failed");
				break;
			}
			case CMD_ACTUAL_STATE_UPDATE_FAILED:
			{
				B3_PROFILE("CMD_ACTUAL_STATE_UPDATE_FAILED");
				b3Warning("request actual state failed");
				break;
			}
			case CMD_BODY_INFO_COMPLETED:
			{
				B3_PROFILE("CMD_BODY_INFO_COMPLETED");
				if (m_data->m_verboseOutput)
				{
					b3Printf("Received body info\n");
				}
				int bodyUniqueId = serverCmd.m_dataStreamArguments.m_bodyUniqueId;
				processBodyJointInfo(bodyUniqueId, serverCmd);

				break;
			}
			case CMD_MJCF_LOADING_FAILED:
			{
				B3_PROFILE("CMD_MJCF_LOADING_FAILED");
				if (m_data->m_verboseOutput)
				{
					b3Printf("Server failed loading the MJCF...\n");
				}
				break;
			}
			case CMD_SDF_LOADING_FAILED:
			{
				B3_PROFILE("CMD_SDF_LOADING_FAILED");
				if (m_data->m_verboseOutput)
				{
					b3Printf("Server failed loading the SDF...\n");
				}

				break;
			}

			case CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED:
			{
				B3_PROFILE("CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED");
				if (m_data->m_verboseOutput)
				{
					b3Printf("Server received bullet data stream OK\n");
				}

				break;
			}
			case CMD_BULLET_DATA_STREAM_RECEIVED_FAILED:
			{
				B3_PROFILE("CMD_BULLET_DATA_STREAM_RECEIVED_FAILED");
				if (m_data->m_verboseOutput)
				{
					b3Printf("Server failed receiving bullet data stream\n");
				}

				break;
			}

			case CMD_ACTUAL_STATE_UPDATE_COMPLETED:
			{
				B3_PROFILE("CMD_ACTUAL_STATE_UPDATE_COMPLETED");
				
				if (m_data->m_verboseOutput)
				{
					b3Printf("Received actual state\n");

					SharedMemoryStatus& command = m_data->m_testBlock1->m_serverCommands[0];

					int numQ = command.m_sendActualStateArgs.m_numDegreeOfFreedomQ;
					int numU = command.m_sendActualStateArgs.m_numDegreeOfFreedomU;
					b3Printf("size Q = %d, size U = %d\n", numQ, numU);
					char msg[1024];
					{
						sprintf(msg, "Q=[");

						for (int i = 0; i < numQ; i++)
						{
							if (i < numQ - 1)
							{
								sprintf(msg, "%s%f,", msg,
									m_data->m_cachedState.m_actualStateQ[i]);
							}
							else
							{
								sprintf(msg, "%s%f", msg,
									m_data->m_cachedState.m_actualStateQ[i]);
							}
						}
						sprintf(msg, "%s]", msg);
					}
					b3Printf(msg);

					sprintf(msg, "U=[");

					for (int i = 0; i < numU; i++)
					{
						if (i < numU - 1)
						{
							sprintf(msg, "%s%f,", msg,
								m_data->m_cachedState.m_actualStateQdot[i]);
						}
						else
						{
							sprintf(msg, "%s%f", msg,
								m_data->m_cachedState.m_actualStateQdot[i]);
						}
					}
					sprintf(msg, "%s]", msg);

					b3Printf(msg);
					b3Printf("\n");
				}
				break;
			}
			case CMD_RESET_SIMULATION_COMPLETED:
			{
				B3_PROFILE("CMD_RESET_SIMULATION_COMPLETED");
				if (m_data->m_verboseOutput)
				{
					b3Printf("CMD_RESET_SIMULATION_COMPLETED clean data\n");
				}
				resetData();

				break;
			}
			case CMD_DEBUG_LINES_COMPLETED:
			{
				B3_PROFILE("CMD_DEBUG_LINES_COMPLETED");
				if (m_data->m_verboseOutput)
				{
					b3Printf("Success receiving %d debug lines",
							 serverCmd.m_sendDebugLinesArgs.m_numDebugLines);
				}

				int numLines = serverCmd.m_sendDebugLinesArgs.m_numDebugLines;
				float* linesFrom =
					(float*)&m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0];
				float* linesTo =
					(float*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0] +
							 numLines * 3 * sizeof(float));
				float* linesColor =
					(float*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0] +
							 2 * numLines * 3 * sizeof(float));

				m_data->m_debugLinesFrom.resize(serverCmd.m_sendDebugLinesArgs.m_startingLineIndex +
												numLines);
				m_data->m_debugLinesTo.resize(serverCmd.m_sendDebugLinesArgs.m_startingLineIndex +
											  numLines);
				m_data->m_debugLinesColor.resize(
					serverCmd.m_sendDebugLinesArgs.m_startingLineIndex + numLines);

				for (int i = 0; i < numLines; i++)
				{
					TmpFloat3 from = CreateTmpFloat3(linesFrom[i * 3], linesFrom[i * 3 + 1],
													 linesFrom[i * 3 + 2]);
					TmpFloat3 to =
						CreateTmpFloat3(linesTo[i * 3], linesTo[i * 3 + 1], linesTo[i * 3 + 2]);
					TmpFloat3 color = CreateTmpFloat3(linesColor[i * 3], linesColor[i * 3 + 1],
													  linesColor[i * 3 + 2]);

					m_data
						->m_debugLinesFrom[serverCmd.m_sendDebugLinesArgs.m_startingLineIndex + i] =
						from;
					m_data->m_debugLinesTo[serverCmd.m_sendDebugLinesArgs.m_startingLineIndex + i] =
						to;
					m_data->m_debugLinesColor[serverCmd.m_sendDebugLinesArgs.m_startingLineIndex +
											  i] = color;
				}

				break;
			}
			case CMD_RIGID_BODY_CREATION_COMPLETED:
			{
				B3_PROFILE("CMD_RIGID_BODY_CREATION_COMPLETED");

				break;
			}
			case CMD_DEBUG_LINES_OVERFLOW_FAILED:
			{
				B3_PROFILE("CMD_DEBUG_LINES_OVERFLOW_FAILED");
				b3Warning("Error receiving debug lines");
				m_data->m_debugLinesFrom.resize(0);
				m_data->m_debugLinesTo.resize(0);
				m_data->m_debugLinesColor.resize(0);

				break;
			}

			case CMD_CAMERA_IMAGE_COMPLETED:
			{
				B3_PROFILE("CMD_CAMERA_IMAGE_COMPLETED");
				if (m_data->m_verboseOutput)
				{
					b3Printf("Camera image OK\n");
				}

				int numBytesPerPixel = 4;  //RGBA
				int numTotalPixels = serverCmd.m_sendPixelDataArguments.m_startingPixelIndex +
									 serverCmd.m_sendPixelDataArguments.m_numPixelsCopied +
									 serverCmd.m_sendPixelDataArguments.m_numRemainingPixels;

				m_data->m_cachedCameraPixelsWidth = 0;
				m_data->m_cachedCameraPixelsHeight = 0;

				int numPixels = serverCmd.m_sendPixelDataArguments.m_imageWidth * serverCmd.m_sendPixelDataArguments.m_imageHeight;

				m_data->m_cachedCameraPixelsRGBA.reserve(numPixels * numBytesPerPixel);
				m_data->m_cachedCameraDepthBuffer.resize(numTotalPixels);
				m_data->m_cachedSegmentationMaskBuffer.resize(numTotalPixels);
				m_data->m_cachedCameraPixelsRGBA.resize(numTotalPixels * numBytesPerPixel);

				unsigned char* rgbaPixelsReceived =
					(unsigned char*)&m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0];
				//  printf("pixel = %d\n", rgbaPixelsReceived[0]);

				float* depthBuffer = (float*)&(m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[serverCmd.m_sendPixelDataArguments.m_numPixelsCopied * 4]);
				int* segmentationMaskBuffer = (int*)&(m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[serverCmd.m_sendPixelDataArguments.m_numPixelsCopied * 8]);

				for (int i = 0; i < serverCmd.m_sendPixelDataArguments.m_numPixelsCopied; i++)
				{
					m_data->m_cachedCameraDepthBuffer[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex] = depthBuffer[i];
				}

				for (int i = 0; i < serverCmd.m_sendPixelDataArguments.m_numPixelsCopied; i++)
				{
					m_data->m_cachedSegmentationMaskBuffer[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex] = segmentationMaskBuffer[i];
				}

				for (int i = 0; i < serverCmd.m_sendPixelDataArguments.m_numPixelsCopied * numBytesPerPixel; i++)
				{
					m_data->m_cachedCameraPixelsRGBA[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex * numBytesPerPixel] = rgbaPixelsReceived[i];
				}

				break;
			}

			case CMD_CAMERA_IMAGE_FAILED:
			{
				B3_PROFILE("CMD_CAMERA_IMAGE_FAILED");
				b3Warning("Camera image FAILED\n");
				break;
			}
			case CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED:
			{
				break;
			}
			case CMD_CALCULATED_INVERSE_DYNAMICS_FAILED:
			{
				b3Warning("Inverse Dynamics computations failed");
				break;
			}
			case CMD_REQUEST_AABB_OVERLAP_FAILED:
			{
				b3Warning("Overlapping object query failed");
				break;
			}

			case CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED:
			{
				B3_PROFILE("m_raycastHits");
				if (m_data->m_verboseOutput)
				{
					b3Printf("Raycast completed");
				}
				m_data->m_raycastHits.clear();
				b3RayHitInfo* rayHits = (b3RayHitInfo*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
				for (int i = 0; i < serverCmd.m_raycastHits.m_numRaycastHits; i++)
				{
					m_data->m_raycastHits.push_back(rayHits[i]);
				}
				break;
			}

			case CMD_REQUEST_VR_EVENTS_DATA_COMPLETED:
			{
				B3_PROFILE("CMD_REQUEST_VR_EVENTS_DATA_COMPLETED");
				if (m_data->m_verboseOutput)
				{
					b3Printf("Request VR Events completed");
				}
				m_data->m_cachedVREvents.clear();
				for (int i = 0; i < serverCmd.m_sendVREvents.m_numVRControllerEvents; i++)
				{
					m_data->m_cachedVREvents.push_back(serverCmd.m_sendVREvents.m_controllerEvents[i]);
				}
				break;
			}

			case CMD_REQUEST_KEYBOARD_EVENTS_DATA_COMPLETED:
			{
				B3_PROFILE("CMD_REQUEST_KEYBOARD_EVENTS_DATA_COMPLETED");
				if (m_data->m_verboseOutput)
				{
					b3Printf("Request keyboard events completed");
				}
				m_data->m_cachedKeyboardEvents.resize(serverCmd.m_sendKeyboardEvents.m_numKeyboardEvents);
				for (int i = 0; i < serverCmd.m_sendKeyboardEvents.m_numKeyboardEvents; i++)
				{
					m_data->m_cachedKeyboardEvents[i] = serverCmd.m_sendKeyboardEvents.m_keyboardEvents[i];
				}
				break;
			}

			case CMD_REQUEST_MOUSE_EVENTS_DATA_COMPLETED:
			{
				B3_PROFILE("CMD_REQUEST_MOUSE_EVENTS_DATA_COMPLETED");
				if (m_data->m_verboseOutput)
				{
					b3Printf("Request mouse events completed");
				}
				m_data->m_cachedMouseEvents.resize(serverCmd.m_sendMouseEvents.m_numMouseEvents);
				for (int i = 0; i < serverCmd.m_sendMouseEvents.m_numMouseEvents; i++)
				{
					m_data->m_cachedMouseEvents[i] = serverCmd.m_sendMouseEvents.m_mouseEvents[i];
				}
				break;
			}

			case CMD_REQUEST_AABB_OVERLAP_COMPLETED:
			{
				B3_PROFILE("CMD_REQUEST_AABB_OVERLAP_COMPLETED");
				if (m_data->m_verboseOutput)
				{
					b3Printf("Overlapping object request completed");
				}

				int startOverlapIndex = serverCmd.m_sendOverlappingObjectsArgs.m_startingOverlappingObjectIndex;
				int numOverlapCopied = serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied;
				m_data->m_cachedOverlappingObjects.resize(startOverlapIndex + numOverlapCopied);
				b3OverlappingObject* objects = (b3OverlappingObject*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;

				for (int i = 0; i < numOverlapCopied; i++)
				{
					m_data->m_cachedOverlappingObjects[startOverlapIndex + i] = objects[i];
				}

				break;
			}
			case CMD_CONTACT_POINT_INFORMATION_COMPLETED:
			{
				B3_PROFILE("CMD_CONTACT_POINT_INFORMATION_COMPLETED");
				if (m_data->m_verboseOutput)
				{
					b3Printf("Contact Point Information Request OK\n");
				}
				int startContactIndex = serverCmd.m_sendContactPointArgs.m_startingContactPointIndex;
				int numContactsCopied = serverCmd.m_sendContactPointArgs.m_numContactPointsCopied;

				m_data->m_cachedContactPoints.resize(startContactIndex + numContactsCopied);

				b3ContactPointData* contactData = (b3ContactPointData*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;

				for (int i = 0; i < numContactsCopied; i++)
				{
					m_data->m_cachedContactPoints[startContactIndex + i] = contactData[i];
				}

				break;
			}
			case CMD_CONTACT_POINT_INFORMATION_FAILED:
			{
				B3_PROFILE("CMD_CONTACT_POINT_INFORMATION_FAILED");
				b3Warning("Contact Point Information Request failed");
				break;
			}

			case CMD_SAVE_WORLD_COMPLETED:
			{
				B3_PROFILE("CMD_SAVE_WORLD_COMPLETED");
				break;
			}

			case CMD_SAVE_WORLD_FAILED:
			{
				B3_PROFILE("CMD_SAVE_WORLD_FAILED");
				b3Warning("Saving world  failed");
				break;
			}
			case CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED:
			{
				B3_PROFILE("CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED");
				break;
			}
			case CMD_CALCULATE_INVERSE_KINEMATICS_FAILED:
			{
				B3_PROFILE("CMD_CALCULATE_INVERSE_KINEMATICS_FAILED");
				b3Warning("Calculate Inverse Kinematics Request failed");
				break;
			}
			case CMD_VISUAL_SHAPE_INFO_COMPLETED:
			{
				B3_PROFILE("CMD_VISUAL_SHAPE_INFO_COMPLETED");
				if (m_data->m_verboseOutput)
				{
					b3Printf("Visual Shape Information Request OK\n");
				}
				int startVisualShapeIndex = serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex;
				int numVisualShapesCopied = serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied;
				m_data->m_cachedVisualShapes.resize(startVisualShapeIndex + numVisualShapesCopied);
				b3VisualShapeData* shapeData = (b3VisualShapeData*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
				for (int i = 0; i < numVisualShapesCopied; i++)
				{
					m_data->m_cachedVisualShapes[startVisualShapeIndex + i] = shapeData[i];
				}

				break;
			}
			case CMD_VISUAL_SHAPE_INFO_FAILED:
			{
				b3Warning("Visual Shape Info Request failed");
				break;
			}
			case CMD_VISUAL_SHAPE_UPDATE_COMPLETED:
			{
				break;
			}
			case CMD_VISUAL_SHAPE_UPDATE_FAILED:
			{
				b3Warning("Visual Shape Update failed");
				break;
			}
			case CMD_LOAD_TEXTURE_COMPLETED:
			{
				break;
			}
			case CMD_LOAD_TEXTURE_FAILED:
			{
				b3Warning("Load texture failed");
				break;
			}
			case CMD_BULLET_LOADING_COMPLETED:
			{
				break;
			}
			case CMD_BULLET_LOADING_FAILED:
			{
				b3Warning("Load .bullet failed");
				break;
			}
			case CMD_BULLET_SAVING_FAILED:
			{
				b3Warning("Save .bullet failed");
				break;
			}
			case CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED:
			case CMD_USER_DEBUG_DRAW_COMPLETED:
			{
				break;
			}
			case CMD_USER_DEBUG_DRAW_FAILED:
			{
				b3Warning("User debug draw failed");
				break;
			}

			case CMD_SYNC_BODY_INFO_COMPLETED:
			{
				break;
			}
			case CMD_STATE_LOGGING_START_COMPLETED:
			{
				break;
			};
			case CMD_STATE_LOGGING_COMPLETED:
			{
				break;
			};

			case CMD_STATE_LOGGING_FAILED:
			{
				b3Warning("State Logging failed");
				break;
			}
			case CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_FAILED:
			{
				b3Warning("Request visualizer camera failed");
				break;
			}
			case CMD_REQUEST_OPENGL_VISUALIZER_CAMERA_COMPLETED:
			{
				break;
			}
			case CMD_REMOVE_BODY_COMPLETED:
			{
				break;
			}
			case CMD_REMOVE_BODY_FAILED:
			{
				b3Warning("Removing body failed");
				break;
			}
			case CMD_GET_DYNAMICS_INFO_COMPLETED:
			{
				break;
			}
			case CMD_GET_DYNAMICS_INFO_FAILED:
			{
				b3Warning("Request dynamics info failed");
				break;
			}
			case CMD_CREATE_COLLISION_SHAPE_FAILED:
			{
				b3Warning("Request createCollisionShape failed");
				break;
			}
			case CMD_CREATE_COLLISION_SHAPE_COMPLETED:
			case CMD_CREATE_VISUAL_SHAPE_COMPLETED:
			{
				break;
			}

			case CMD_CREATE_MULTI_BODY_FAILED:
			{
				b3Warning("Request createMultiBody failed");
				break;
			}
			case CMD_CREATE_VISUAL_SHAPE_FAILED:
			{
				b3Warning("Request createVisualShape failed");
				break;
			}
			case CMD_REQUEST_COLLISION_INFO_COMPLETED:
			{
				break;
			}
			case CMD_REQUEST_COLLISION_INFO_FAILED:
			{
				b3Warning("Request getCollisionInfo failed");
				break;
			}
			case CMD_CUSTOM_COMMAND_COMPLETED:
			{
				break;
			}
			case CMD_CALCULATED_JACOBIAN_COMPLETED:
			{
				break;
			}
			case CMD_CALCULATED_JACOBIAN_FAILED:
			{
				b3Warning("jacobian calculation failed");
				break;
			}
			case CMD_CUSTOM_COMMAND_FAILED:
			{
				b3Warning("custom plugin command failed");
				break;
			}

			case CMD_CALCULATED_MASS_MATRIX_FAILED:
			{
				b3Warning("calculate mass matrix failed");
				break;
			}
			case CMD_CALCULATED_MASS_MATRIX_COMPLETED:
			{
				double* matrixData = (double*)&this->m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0];
				m_data->m_cachedMassMatrix.resize(serverCmd.m_massMatrixResultArgs.m_dofCount * serverCmd.m_massMatrixResultArgs.m_dofCount);
				for (int i = 0; i < serverCmd.m_massMatrixResultArgs.m_dofCount * serverCmd.m_massMatrixResultArgs.m_dofCount; i++)
				{
					m_data->m_cachedMassMatrix[i] = matrixData[i];
				}
				break;
			}
			case CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS_COMPLETED:
			{
				break;
			}
			case CMD_SAVE_STATE_COMPLETED:
			{
				break;
			}
			case CMD_RESTORE_STATE_FAILED:
			{
				b3Warning("restoreState failed");
				break;
			}
			case CMD_RESTORE_STATE_COMPLETED:
			{
				break;
			}
			case CMD_BULLET_SAVING_COMPLETED:
			{
				break;
			}
			case CMD_COLLISION_SHAPE_INFO_FAILED:
			{
				b3Warning("getCollisionShapeData failed");
				break;
			}
			case CMD_COLLISION_SHAPE_INFO_COMPLETED:
			{
				B3_PROFILE("CMD_COLLISION_SHAPE_INFO_COMPLETED");
				if (m_data->m_verboseOutput)
				{
					b3Printf("Collision Shape Information Request OK\n");
				}
				int numCollisionShapesCopied = serverCmd.m_sendCollisionShapeArgs.m_numCollisionShapes;
				m_data->m_cachedCollisionShapes.resize(numCollisionShapesCopied);
				b3CollisionShapeData* shapeData = (b3CollisionShapeData*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
				for (int i = 0; i < numCollisionShapesCopied; i++)
				{
					m_data->m_cachedCollisionShapes[i] = shapeData[i];
				}
				break;
			}
			case CMD_LOAD_SOFT_BODY_FAILED:
			{
                            B3_PROFILE("CMD_LOAD_SOFT_BODY_FAILED");

                            if (m_data->m_verboseOutput)
                            {
                                b3Printf("Server failed loading the SoftBody...\n");
                            }
                            break;
			}
			case CMD_LOAD_SOFT_BODY_COMPLETED:
			{
                          B3_PROFILE("CMD_LOAD_SOFT_BODY_COMPLETED");

                          if (m_data->m_verboseOutput)
                          {
                            b3Printf("Server loading the SoftBody OK\n");
                          }
                          
                          b3Assert(serverCmd.m_numDataStreamBytes);
                          if (serverCmd.m_numDataStreamBytes > 0)
                          {
			       bParse::btBulletFile bf(
                                   this->m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor,
                                   serverCmd.m_numDataStreamBytes);
                               bf.setFileDNAisMemoryDNA();
                               bf.parse(false);
                               int bodyUniqueId = serverCmd.m_dataStreamArguments.m_bodyUniqueId;
                               
                               BodyJointInfoCache* bodyJoints = new BodyJointInfoCache;
                               m_data->m_bodyJointMap.insert(bodyUniqueId, bodyJoints);
                               bodyJoints->m_bodyName = serverCmd.m_dataStreamArguments.m_bodyName;
                               bodyJoints->m_baseName = "baseLink";
                                        
                               if (bf.ok())
                               {
                                 if (m_data->m_verboseOutput)
                                 {
                                   b3Printf("Received robot description ok!\n");
                                 }
                               }
                               else
                               {
                                 b3Warning("Robot description not received when loading soft body!");
                               }
                          }
                          break;
			}
			case CMD_SYNC_USER_DATA_FAILED:
			{
				b3Warning("Synchronizing user data failed.");
				break;
			}
			case CMD_REQUEST_USER_DATA_FAILED:
			{
				b3Warning("Requesting user data failed");
				break;
			}
			case CMD_ADD_USER_DATA_FAILED:
			{
				b3Warning("Adding user data failed (do the specified body and link exist?)");
				break;
			}
			case CMD_REMOVE_USER_DATA_FAILED:
			{
				b3Warning("Removing user data failed");
				break;
			}
			case CMD_REQUEST_USER_DATA_COMPLETED:
			case CMD_SYNC_USER_DATA_COMPLETED:
			case CMD_REMOVE_USER_DATA_COMPLETED:
			case CMD_ADD_USER_DATA_COMPLETED:
			case CMD_REMOVE_STATE_FAILED:
			case CMD_REMOVE_STATE_COMPLETED:
			{
				break;
			}
			default:
			{
				b3Error("Unknown server status %d\n", serverCmd.m_type);
				btAssert(0);
			}
		};

		m_data->m_testBlock1->m_numProcessedServerCommands++;
		// we don't have more than 1 command outstanding (in total, either server or client)
		btAssert(m_data->m_testBlock1->m_numProcessedServerCommands ==
				 m_data->m_testBlock1->m_numServerCommands);

		if (m_data->m_testBlock1->m_numServerCommands ==
			m_data->m_testBlock1->m_numProcessedServerCommands)
		{
			m_data->m_waitingForServer = false;
		}
		else
		{
			m_data->m_waitingForServer = true;
		}

		if ((serverCmd.m_type == CMD_SDF_LOADING_COMPLETED) || (serverCmd.m_type == CMD_MJCF_LOADING_COMPLETED) || (serverCmd.m_type == CMD_SYNC_BODY_INFO_COMPLETED))
		{
			B3_PROFILE("CMD_LOADING_COMPLETED");
			int numConstraints = serverCmd.m_sdfLoadedArgs.m_numUserConstraints;
			for (int i = 0; i < numConstraints; i++)
			{
				int constraintUid = serverCmd.m_sdfLoadedArgs.m_userConstraintUniqueIds[i];
				m_data->m_constraintIdsRequestInfo.push_back(constraintUid);
			}
			int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies;
			if (numBodies > 0)
			{
				m_data->m_tempBackupServerStatus = m_data->m_lastServerStatus;

				for (int i = 0; i < numBodies; i++)
				{
					m_data->m_bodyIdsRequestInfo.push_back(serverCmd.m_sdfLoadedArgs.m_bodyUniqueIds[i]);
				}

				int bodyId = m_data->m_bodyIdsRequestInfo[m_data->m_bodyIdsRequestInfo.size() - 1];
				m_data->m_bodyIdsRequestInfo.pop_back();

				SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
				//now transfer the information of the individual objects etc.
				command.m_type = CMD_REQUEST_BODY_INFO;
				command.m_sdfRequestInfoArgs.m_bodyUniqueId = bodyId;
				submitClientCommand(command);
				return 0;
			}
		}

		if (serverCmd.m_type == CMD_SYNC_USER_DATA_COMPLETED)
		{
			B3_PROFILE("CMD_SYNC_USER_DATA_COMPLETED");
			// Remove all cached user data entries.
			for (int i = 0; i < m_data->m_bodyJointMap.size(); i++)
			{
				BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap.getAtIndex(i);
				if (bodyJointsPtr && *bodyJointsPtr)
				{
					(*bodyJointsPtr)->m_userDataIds.clear();
				}
				m_data->m_userDataMap.clear();
				m_data->m_userDataHandleLookup.clear();
			}
			const int numIdentifiers = serverCmd.m_syncUserDataArgs.m_numUserDataIdentifiers;
			if (numIdentifiers > 0)
			{
				m_data->m_tempBackupServerStatus = m_data->m_lastServerStatus;

				const int* identifiers = (int*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
				m_data->m_userDataIdsRequestInfo.reserve(numIdentifiers - 1);
				// Store the identifiers that still need to be requested.
				for (int i = 0; i < numIdentifiers - 1; i++)
				{
					m_data->m_userDataIdsRequestInfo.push_back(identifiers[i]);
				}

				// Request individual user data entries, start with last identifier.
				SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
				command.m_type = CMD_REQUEST_USER_DATA;
				command.m_userDataRequestArgs.m_userDataId = identifiers[numIdentifiers - 1];
				submitClientCommand(command);
				return 0;
			}
		}

		if (serverCmd.m_type == CMD_ADD_USER_DATA_COMPLETED || serverCmd.m_type == CMD_REQUEST_USER_DATA_COMPLETED)
		{
			B3_PROFILE("CMD_ADD_USER_DATA_COMPLETED");
			const UserDataResponseArgs response = serverCmd.m_userDataResponseArgs;
			BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[response.m_bodyUniqueId];
			if (bodyJointsPtr && *bodyJointsPtr)
			{
				const char* dataStream = m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
				SharedMemoryUserData* userData = m_data->m_userDataMap[response.m_userDataId];
				if (userData)
				{
					// Only replace the value.
					userData->replaceValue(dataStream, response.m_valueLength, response.m_valueType);
				}
				else
				{
					// Add a new user data entry.
					const char* key = response.m_key;
					m_data->m_userDataMap.insert(response.m_userDataId, SharedMemoryUserData(key, response.m_bodyUniqueId, response.m_linkIndex, response.m_visualShapeIndex));
					userData = m_data->m_userDataMap[response.m_userDataId];
					userData->replaceValue(dataStream, response.m_valueLength, response.m_valueType);
					m_data->m_userDataHandleLookup.insert(SharedMemoryUserDataHashKey(userData), response.m_userDataId);
					(*bodyJointsPtr)->m_userDataIds.push_back(response.m_userDataId);
				}
			}
		}

		if (serverCmd.m_type == CMD_REQUEST_USER_DATA_COMPLETED)
		{
			if (m_data->m_userDataIdsRequestInfo.size() > 0)
			{
				// Request individual user data entries.
				const int userDataId = m_data->m_userDataIdsRequestInfo[m_data->m_userDataIdsRequestInfo.size() - 1];
				m_data->m_userDataIdsRequestInfo.pop_back();

				SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
				command.m_type = CMD_REQUEST_USER_DATA;
				command.m_userDataRequestArgs.m_userDataId = userDataId;
				submitClientCommand(command);
				return 0;
			}
			m_data->m_lastServerStatus = m_data->m_tempBackupServerStatus;
		}

		if (serverCmd.m_type == CMD_REMOVE_USER_DATA_COMPLETED)
		{
			B3_PROFILE("CMD_REMOVE_USER_DATA_COMPLETED");
			const int userDataId = serverCmd.m_removeUserDataResponseArgs.m_userDataId;
			SharedMemoryUserData* userData = m_data->m_userDataMap[userDataId];
			if (userData)
			{
				BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[userData->m_bodyUniqueId];
				if (bodyJointsPtr && *bodyJointsPtr)
				{
					(*bodyJointsPtr)->m_userDataIds.remove(userDataId);
				}
				m_data->m_userDataHandleLookup.remove(SharedMemoryUserDataHashKey(userData));
				m_data->m_userDataMap.remove(userDataId);
			}
		}

		if (serverCmd.m_type == CMD_REMOVE_BODY_COMPLETED)
		{
			for (int i = 0; i < serverCmd.m_removeObjectArgs.m_numBodies; i++)
			{
				int bodyUniqueId = serverCmd.m_removeObjectArgs.m_bodyUniqueIds[i];
				removeCachedBody(bodyUniqueId);
			}
			for (int i = 0; i < serverCmd.m_removeObjectArgs.m_numUserConstraints; i++)
			{
				int key = serverCmd.m_removeObjectArgs.m_userConstraintUniqueIds[i];
				m_data->m_userConstraintInfoMap.remove(key);
			}
		}

		if (serverCmd.m_type == CMD_USER_CONSTRAINT_INFO_COMPLETED)
		{
			B3_PROFILE("CMD_USER_CONSTRAINT_INFO_COMPLETED");

			if (m_data->m_constraintIdsRequestInfo.size())
			{
				int cid = m_data->m_constraintIdsRequestInfo[m_data->m_constraintIdsRequestInfo.size() - 1];
				m_data->m_constraintIdsRequestInfo.pop_back();
				SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
				command.m_type = CMD_USER_CONSTRAINT;
				command.m_updateFlags = USER_CONSTRAINT_REQUEST_INFO;
				command.m_userConstraintArguments.m_userConstraintUniqueId = cid;
				submitClientCommand(command);
				return 0;
			}
			else
			{
				m_data->m_lastServerStatus = m_data->m_tempBackupServerStatus;
			}
		}

		if (serverCmd.m_type == CMD_BODY_INFO_COMPLETED)
		{
			B3_PROFILE("CMD_BODY_INFO_COMPLETED");
			//are there any bodies left to be processed?
			if (m_data->m_bodyIdsRequestInfo.size())
			{
				int bodyId = m_data->m_bodyIdsRequestInfo[m_data->m_bodyIdsRequestInfo.size() - 1];
				m_data->m_bodyIdsRequestInfo.pop_back();

				SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
				//now transfer the information of the individual objects etc.
				command.m_type = CMD_REQUEST_BODY_INFO;
				command.m_sdfRequestInfoArgs.m_bodyUniqueId = bodyId;
				submitClientCommand(command);
				return 0;
			}
			else
			{
				if (m_data->m_constraintIdsRequestInfo.size())
				{
					int cid = m_data->m_constraintIdsRequestInfo[m_data->m_constraintIdsRequestInfo.size() - 1];
					m_data->m_constraintIdsRequestInfo.pop_back();
					SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
					command.m_type = CMD_USER_CONSTRAINT;
					command.m_updateFlags = USER_CONSTRAINT_REQUEST_INFO;
					command.m_userConstraintArguments.m_userConstraintUniqueId = cid;
					submitClientCommand(command);
					return 0;
				}
				else
				{
					m_data->m_lastServerStatus = m_data->m_tempBackupServerStatus;
				}
			}
		}

		if (serverCmd.m_type == CMD_REQUEST_AABB_OVERLAP_COMPLETED)
		{
			B3_PROFILE("CMD_REQUEST_AABB_OVERLAP_COMPLETED2");
			SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
			if (serverCmd.m_sendOverlappingObjectsArgs.m_numRemainingOverlappingObjects > 0 && serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied)
			{
				command.m_type = CMD_REQUEST_AABB_OVERLAP;
				command.m_requestOverlappingObjectsArgs.m_startingOverlappingObjectIndex = serverCmd.m_sendOverlappingObjectsArgs.m_startingOverlappingObjectIndex + serverCmd.m_sendOverlappingObjectsArgs.m_numOverlappingObjectsCopied;
				submitClientCommand(command);
				return 0;
			}
		}

		if (serverCmd.m_type == CMD_CONTACT_POINT_INFORMATION_COMPLETED)
		{
			B3_PROFILE("CMD_CONTACT_POINT_INFORMATION_COMPLETED2");
			SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
			if (serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints > 0 && serverCmd.m_sendContactPointArgs.m_numContactPointsCopied)
			{
				command.m_type = CMD_REQUEST_CONTACT_POINT_INFORMATION;
				command.m_requestContactPointArguments.m_startingContactPointIndex = serverCmd.m_sendContactPointArgs.m_startingContactPointIndex + serverCmd.m_sendContactPointArgs.m_numContactPointsCopied;
				command.m_requestContactPointArguments.m_objectAIndexFilter = -1;
				command.m_requestContactPointArguments.m_objectBIndexFilter = -1;
				submitClientCommand(command);
				return 0;
			}
		}

		if (serverCmd.m_type == CMD_VISUAL_SHAPE_INFO_COMPLETED)
		{
			B3_PROFILE("CMD_VISUAL_SHAPE_INFO_COMPLETED2");
			SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
			if (serverCmd.m_sendVisualShapeArgs.m_numRemainingVisualShapes > 0 && serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied)
			{
				command.m_type = CMD_REQUEST_VISUAL_SHAPE_INFO;
				command.m_requestVisualShapeDataArguments.m_startingVisualShapeIndex = serverCmd.m_sendVisualShapeArgs.m_startingVisualShapeIndex + serverCmd.m_sendVisualShapeArgs.m_numVisualShapesCopied;
				command.m_requestVisualShapeDataArguments.m_bodyUniqueId = serverCmd.m_sendVisualShapeArgs.m_bodyUniqueId;
				submitClientCommand(command);
				return 0;
			}
		}

		if (serverCmd.m_type == CMD_CAMERA_IMAGE_COMPLETED)
		{
			B3_PROFILE("CMD_CAMERA_IMAGE_COMPLETED2");
			SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];

			if (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0 && serverCmd.m_sendPixelDataArguments.m_numPixelsCopied)
			{
				// continue requesting remaining pixels
				command.m_type = CMD_REQUEST_CAMERA_IMAGE_DATA;
				command.m_requestPixelDataArguments.m_startPixelIndex =
					serverCmd.m_sendPixelDataArguments.m_startingPixelIndex +
					serverCmd.m_sendPixelDataArguments.m_numPixelsCopied;
				submitClientCommand(command);
				return 0;
			}
			else
			{
				m_data->m_cachedCameraPixelsWidth = serverCmd.m_sendPixelDataArguments.m_imageWidth;
				m_data->m_cachedCameraPixelsHeight = serverCmd.m_sendPixelDataArguments.m_imageHeight;
			}
		}

		if ((serverCmd.m_type == CMD_DEBUG_LINES_COMPLETED) &&
			(serverCmd.m_sendDebugLinesArgs.m_numRemainingDebugLines > 0))
		{
			B3_PROFILE("CMD_DEBUG_LINES_COMPLETED2");
			SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];

			// continue requesting debug lines for drawing
			command.m_type = CMD_REQUEST_DEBUG_LINES;
			command.m_requestDebugLinesArguments.m_startingLineIndex =
				serverCmd.m_sendDebugLinesArgs.m_numDebugLines +
				serverCmd.m_sendDebugLinesArgs.m_startingLineIndex;
			submitClientCommand(command);
			return 0;
		}

		return &m_data->m_lastServerStatus;
	}
	else
	{
		if (m_data->m_verboseOutput)
		{
			B3_PROFILE("m_verboseOutput");

			b3Printf("m_numServerStatus  = %d, processed = %d\n",
					 m_data->m_testBlock1->m_numServerCommands,
					 m_data->m_testBlock1->m_numProcessedServerCommands);
		}
	}
	return 0;
}

bool PhysicsClientSharedMemory::canSubmitCommand() const
{
	if (m_data->m_isConnected && !m_data->m_waitingForServer)
	{
		if (m_data->m_testBlock1->m_magicId == SHARED_MEMORY_MAGIC_NUMBER)
		{
			return true;
		}
		else
		{
			return false;
		}
	}
	return false;
}

struct SharedMemoryCommand* PhysicsClientSharedMemory::getAvailableSharedMemoryCommand()
{
	static int sequence = 0;
	m_data->m_testBlock1->m_clientCommands[0].m_sequenceNumber = sequence++;
	return &m_data->m_testBlock1->m_clientCommands[0];
}

bool PhysicsClientSharedMemory::submitClientCommand(const SharedMemoryCommand& command)
{
	/// at the moment we allow a maximum of 1 outstanding command, so we check for this
	// once the server processed the command and returns a status, we clear the flag
	// "m_data->m_waitingForServer" and allow submitting the next command

	if (!m_data->m_waitingForServer)
	{
		if (&m_data->m_testBlock1->m_clientCommands[0] != &command)
		{
			m_data->m_testBlock1->m_clientCommands[0] = command;
		}
		m_data->m_testBlock1->m_numClientCommands++;
		m_data->m_waitingForServer = true;
		return true;
	}
	return false;
}

void PhysicsClientSharedMemory::uploadBulletFileToSharedMemory(const char* data, int len)
{
	btAssert(len < SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
	if (len >= SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE)
	{
		b3Warning("uploadBulletFileToSharedMemory %d exceeds max size %d\n", len,
				  SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
	}
	else
	{
		for (int i = 0; i < len; i++)
		{
			//m_data->m_testBlock1->m_bulletStreamDataClientToServer[i] = data[i];
			m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[i] = data[i];
		}
	}
}

void PhysicsClientSharedMemory::uploadRaysToSharedMemory(struct SharedMemoryCommand& command, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays)
{
	int curNumStreamingRays = command.m_requestRaycastIntersections.m_numStreamingRays;
	int newNumRays = curNumStreamingRays + numRays;
	btAssert(newNumRays < MAX_RAY_INTERSECTION_BATCH_SIZE_STREAMING);

	if (newNumRays < MAX_RAY_INTERSECTION_BATCH_SIZE_STREAMING)
	{
		for (int i = 0; i < numRays; i++)
		{
			b3RayData* rayDataStream = (b3RayData*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
			rayDataStream[curNumStreamingRays + i].m_rayFromPosition[0] = rayFromWorldArray[i * 3 + 0];
			rayDataStream[curNumStreamingRays + i].m_rayFromPosition[1] = rayFromWorldArray[i * 3 + 1];
			rayDataStream[curNumStreamingRays + i].m_rayFromPosition[2] = rayFromWorldArray[i * 3 + 2];
			rayDataStream[curNumStreamingRays + i].m_rayToPosition[0] = rayToWorldArray[i * 3 + 0];
			rayDataStream[curNumStreamingRays + i].m_rayToPosition[1] = rayToWorldArray[i * 3 + 1];
			rayDataStream[curNumStreamingRays + i].m_rayToPosition[2] = rayToWorldArray[i * 3 + 2];
			command.m_requestRaycastIntersections.m_numStreamingRays++;
		}
	}
}

void PhysicsClientSharedMemory::getCachedCameraImage(struct b3CameraImageData* cameraData)
{
	cameraData->m_pixelWidth = m_data->m_cachedCameraPixelsWidth;
	cameraData->m_pixelHeight = m_data->m_cachedCameraPixelsHeight;
	cameraData->m_depthValues = m_data->m_cachedCameraDepthBuffer.size() ? &m_data->m_cachedCameraDepthBuffer[0] : 0;
	cameraData->m_rgbColorData = m_data->m_cachedCameraPixelsRGBA.size() ? &m_data->m_cachedCameraPixelsRGBA[0] : 0;
	cameraData->m_segmentationMaskValues = m_data->m_cachedSegmentationMaskBuffer.size() ? &m_data->m_cachedSegmentationMaskBuffer[0] : 0;
}

void PhysicsClientSharedMemory::getCachedContactPointInformation(struct b3ContactInformation* contactPointData)
{
	contactPointData->m_numContactPoints = m_data->m_cachedContactPoints.size();
	contactPointData->m_contactPointData = contactPointData->m_numContactPoints ? &m_data->m_cachedContactPoints[0] : 0;
}

void PhysicsClientSharedMemory::getCachedOverlappingObjects(struct b3AABBOverlapData* overlappingObjects)
{
	overlappingObjects->m_numOverlappingObjects = m_data->m_cachedOverlappingObjects.size();
	overlappingObjects->m_overlappingObjects = m_data->m_cachedOverlappingObjects.size() ? &m_data->m_cachedOverlappingObjects[0] : 0;
}

void PhysicsClientSharedMemory::getCachedVREvents(struct b3VREventsData* vrEventsData)
{
	vrEventsData->m_numControllerEvents = m_data->m_cachedVREvents.size();
	vrEventsData->m_controllerEvents = vrEventsData->m_numControllerEvents ? &m_data->m_cachedVREvents[0] : 0;
}

void PhysicsClientSharedMemory::getCachedKeyboardEvents(struct b3KeyboardEventsData* keyboardEventsData)
{
	keyboardEventsData->m_numKeyboardEvents = m_data->m_cachedKeyboardEvents.size();
	keyboardEventsData->m_keyboardEvents = keyboardEventsData->m_numKeyboardEvents ? &m_data->m_cachedKeyboardEvents[0] : 0;
}

void PhysicsClientSharedMemory::getCachedMouseEvents(struct b3MouseEventsData* mouseEventsData)
{
	mouseEventsData->m_numMouseEvents = m_data->m_cachedMouseEvents.size();
	mouseEventsData->m_mouseEvents = mouseEventsData->m_numMouseEvents ? &m_data->m_cachedMouseEvents[0] : 0;
}

void PhysicsClientSharedMemory::getCachedRaycastHits(struct b3RaycastInformation* raycastHits)
{
	raycastHits->m_numRayHits = m_data->m_raycastHits.size();
	raycastHits->m_rayHits = raycastHits->m_numRayHits ? &m_data->m_raycastHits[0] : 0;
}

void PhysicsClientSharedMemory::getCachedMassMatrix(int dofCountCheck, double* massMatrix)
{
	int sz = dofCountCheck * dofCountCheck;
	if (sz == m_data->m_cachedMassMatrix.size())
	{
		for (int i = 0; i < sz; i++)
		{
			massMatrix[i] = m_data->m_cachedMassMatrix[i];
		}
	}
}

void PhysicsClientSharedMemory::getCachedVisualShapeInformation(struct b3VisualShapeInformation* visualShapesInfo)
{
	visualShapesInfo->m_numVisualShapes = m_data->m_cachedVisualShapes.size();
	visualShapesInfo->m_visualShapeData = visualShapesInfo->m_numVisualShapes ? &m_data->m_cachedVisualShapes[0] : 0;
}

void PhysicsClientSharedMemory::getCachedCollisionShapeInformation(struct b3CollisionShapeInformation* collisionShapesInfo)
{
	collisionShapesInfo->m_numCollisionShapes = m_data->m_cachedCollisionShapes.size();
	collisionShapesInfo->m_collisionShapeData = collisionShapesInfo->m_numCollisionShapes ? &m_data->m_cachedCollisionShapes[0] : 0;
}

const float* PhysicsClientSharedMemory::getDebugLinesFrom() const
{
	if (m_data->m_debugLinesFrom.size())
	{
		return &m_data->m_debugLinesFrom[0].m_x;
	}
	return 0;
}
const float* PhysicsClientSharedMemory::getDebugLinesTo() const
{
	if (m_data->m_debugLinesTo.size())
	{
		return &m_data->m_debugLinesTo[0].m_x;
	}
	return 0;
}
const float* PhysicsClientSharedMemory::getDebugLinesColor() const
{
	if (m_data->m_debugLinesColor.size())
	{
		return &m_data->m_debugLinesColor[0].m_x;
	}
	return 0;
}
int PhysicsClientSharedMemory::getNumDebugLines() const { return m_data->m_debugLinesFrom.size(); }

void PhysicsClientSharedMemory::setTimeOut(double timeOutInSeconds)
{
	m_data->m_timeOutInSeconds = timeOutInSeconds;
}
double PhysicsClientSharedMemory::getTimeOut() const
{
	return m_data->m_timeOutInSeconds;
}

bool PhysicsClientSharedMemory::getCachedUserData(int userDataId, struct b3UserDataValue& valueOut) const
{
	SharedMemoryUserData* userDataPtr = m_data->m_userDataMap[userDataId];
	if (!userDataPtr)
	{
		return false;
	}
	valueOut.m_type = (userDataPtr)->m_type;
	valueOut.m_length = userDataPtr->m_bytes.size();
	valueOut.m_data1 = userDataPtr->m_bytes.size() ? &userDataPtr->m_bytes[0] : 0;
	return true;
}

int PhysicsClientSharedMemory::getCachedUserDataId(int bodyUniqueId, int linkIndex, int visualShapeIndex, const char* key) const
{
	int* userDataId = m_data->m_userDataHandleLookup.find(SharedMemoryUserDataHashKey(key, bodyUniqueId, linkIndex, visualShapeIndex));
	if (!userDataId)
	{
		return -1;
	}
	return *userDataId;
}

int PhysicsClientSharedMemory::getNumUserData(int bodyUniqueId) const
{
	BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
	if (!bodyJointsPtr || !(*bodyJointsPtr))
	{
		return 0;
	}
	return (*bodyJointsPtr)->m_userDataIds.size();
}

void PhysicsClientSharedMemory::getUserDataInfo(int bodyUniqueId, int userDataIndex, const char** keyOut, int* userDataIdOut, int* linkIndexOut, int* visualShapeIndexOut) const
{
	BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
	if (!bodyJointsPtr || !(*bodyJointsPtr) || userDataIndex < 0 || userDataIndex > (*bodyJointsPtr)->m_userDataIds.size())
	{
		*keyOut = 0;
		*userDataIdOut = -1;
		return;
	}
	int userDataId = (*bodyJointsPtr)->m_userDataIds[userDataIndex];
	SharedMemoryUserData* userData = m_data->m_userDataMap[userDataId];

	*userDataIdOut = userDataId;
	*keyOut = userData->m_key.c_str();
	*linkIndexOut = userData->m_linkIndex;
	*visualShapeIndexOut = userData->m_visualShapeIndex;
}

void PhysicsClientSharedMemory::pushProfileTiming(const char* timingName)
{
	std::string** strPtr = m_data->m_profileTimingStringArray[timingName];
	std::string* str = 0;
	if (strPtr)
	{
		str = *strPtr;
	}
	else
	{
		str = new std::string(timingName);
		m_data->m_profileTimingStringArray.insert(timingName, str);
	}
	m_data->m_profileTimings.push_back(new CProfileSample(str->c_str()));
}

void PhysicsClientSharedMemory::popProfileTiming()
{
	if (m_data->m_profileTimings.size())
	{
		CProfileSample* sample = m_data->m_profileTimings[m_data->m_profileTimings.size() - 1];
		m_data->m_profileTimings.pop_back();
		delete sample;
	}
}