* Adjustment: Initial CMake reworking.

Engine/lib/bullet/examples/SharedMemory/BodyJointInfoUtility.h

@@ -0,0 +1,77 @@
+#ifndef BODY_JOINT_INFO_UTILITY_H
+#define BODY_JOINT_INFO_UTILITY_H
+
+#include "Bullet3Common/b3Logging.h"
+
+namespace Bullet
+{
+	class btMultiBodyDoubleData;
+	class btMultiBodyFloatData;
+};
+
+inline char* strDup(const char* const str)
+{
+#ifdef _WIN32
+	return _strdup(str);
+#else
+	return strdup(str);
+#endif
+}
+
+template <typename T, typename U> void addJointInfoFromMultiBodyData(const T* mb, U* bodyJoints, bool verboseOutput)
+{
+	if (mb->m_baseName) 
+	{
+		if (verboseOutput) 
+		{
+			b3Printf("mb->m_baseName = %s\n", mb->m_baseName);
+		}
+	}
+	int qOffset = 7;
+	int uOffset = 6;
+
+	for (int link = 0; link < mb->m_numLinks; link++) 
+	{
+		{
+			b3JointInfo info;
+			info.m_flags = 0;
+			info.m_jointIndex = link;
+			info.m_qIndex =
+				(0 < mb->m_links[link].m_posVarCount) ? qOffset : -1;
+			info.m_uIndex =
+				(0 < mb->m_links[link].m_dofCount) ? uOffset : -1;
+
+			if (mb->m_links[link].m_linkName) {
+				if (verboseOutput) {
+					b3Printf("mb->m_links[%d].m_linkName = %s\n", link,
+								mb->m_links[link].m_linkName);
+				}
+				info.m_linkName = strDup(mb->m_links[link].m_linkName);
+			}
+			if (mb->m_links[link].m_jointName) {
+				if (verboseOutput) {
+					b3Printf("mb->m_links[%d].m_jointName = %s\n", link,
+								mb->m_links[link].m_jointName);
+				}
+				info.m_jointName = strDup(mb->m_links[link].m_jointName);
+			}
+
+			info.m_jointType = mb->m_links[link].m_jointType;
+			info.m_jointDamping = mb->m_links[link].m_jointDamping;
+			info.m_jointFriction = mb->m_links[link].m_jointFriction;
+
+			if ((mb->m_links[link].m_jointType == eRevoluteType) ||
+				(mb->m_links[link].m_jointType == ePrismaticType)) {
+				info.m_flags |= JOINT_HAS_MOTORIZED_POWER;
+			}
+			bodyJoints->m_jointInfo.push_back(info);
+		}
+		qOffset += mb->m_links[link].m_posVarCount;
+		uOffset += mb->m_links[link].m_dofCount;
+	}
+	
+}
+
+
+
+#endif //BODY_JOINT_INFO_UTILITY_H

Engine/lib/bullet/examples/SharedMemory/IKTrajectoryHelper.cpp

@@ -0,0 +1,206 @@
+#include "IKTrajectoryHelper.h"
+#include "BussIK/Node.h"
+#include "BussIK/Tree.h"
+#include "BussIK/Jacobian.h"
+#include "BussIK/VectorRn.h"
+#include "BussIK/MatrixRmn.h"
+#include "Bullet3Common/b3AlignedObjectArray.h"
+#include "BulletDynamics/Featherstone/btMultiBody.h"
+
+
+#define RADIAN(X)	((X)*RadiansToDegrees)
+
+//use BussIK and Reflexxes to convert from Cartesian endeffector future target to
+//joint space positions at each real-time (simulation) step
+struct IKTrajectoryHelperInternalData
+{
+    VectorR3 m_endEffectorTargetPosition;
+    VectorRn m_nullSpaceVelocity;
+    
+    b3AlignedObjectArray<Node*> m_ikNodes;
+    Jacobian* m_ikJacobian;
+    
+    IKTrajectoryHelperInternalData()
+    {
+        m_endEffectorTargetPosition.SetZero();
+        m_nullSpaceVelocity.SetZero();
+    }
+};
+
+
+IKTrajectoryHelper::IKTrajectoryHelper()
+{
+    m_data = new IKTrajectoryHelperInternalData;
+}
+
+IKTrajectoryHelper::~IKTrajectoryHelper()
+{
+    delete m_data;
+}
+
+
+bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
+                                   const double endEffectorTargetOrientation[4],
+                                   const double endEffectorWorldPosition[3],
+                                   const double endEffectorWorldOrientation[4],
+               const double* q_current, int numQ,int endEffectorIndex,
+               double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, const double dampIk[6])
+{
+	bool useAngularPart = (ikMethod==IK2_VEL_DLS_WITH_ORIENTATION || ikMethod==IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE) ? true : false;
+
+	m_data->m_ikJacobian = new Jacobian(useAngularPart,numQ);
+	
+//    Reset(m_ikTree,m_ikJacobian);
+
+    m_data->m_ikJacobian->Reset();
+
+    bool UseJacobianTargets1 = false;
+    
+    if ( UseJacobianTargets1 ) {
+        m_data->m_ikJacobian->SetJtargetActive();
+    }
+    else {
+        m_data->m_ikJacobian->SetJendActive();
+    }
+    VectorR3 targets;
+    targets.Set(endEffectorTargetPosition[0],endEffectorTargetPosition[1],endEffectorTargetPosition[2]);
+    m_data->m_ikJacobian->ComputeJacobian(&targets);						// Set up Jacobian and deltaS vectors
+    
+    // Set one end effector world position from Bullet
+    VectorRn deltaS(3);
+    for (int i = 0; i < 3; ++i)
+    {
+        deltaS.Set(i,dampIk[i]*(endEffectorTargetPosition[i]-endEffectorWorldPosition[i]));
+    }
+    
+    // Set one end effector world orientation from Bullet
+    VectorRn deltaR(3);
+    if (useAngularPart)
+    {
+        btQuaternion startQ(endEffectorWorldOrientation[0],endEffectorWorldOrientation[1],endEffectorWorldOrientation[2],endEffectorWorldOrientation[3]);
+        btQuaternion endQ(endEffectorTargetOrientation[0],endEffectorTargetOrientation[1],endEffectorTargetOrientation[2],endEffectorTargetOrientation[3]);
+        btQuaternion deltaQ = endQ*startQ.inverse();
+        float angle = deltaQ.getAngle();
+        btVector3 axis = deltaQ.getAxis();
+        if (angle > PI) {
+            angle -= 2.0*PI;
+        }
+        else if (angle < -PI) {
+            angle += 2.0*PI;
+        }
+        float angleDot = angle;
+        btVector3 angularVel = angleDot*axis.normalize();
+        for (int i = 0; i < 3; ++i)
+        {
+            deltaR.Set(i,dampIk[i+3]*angularVel[i]);
+        }
+    }
+    
+    {
+        
+		if (useAngularPart)
+		{
+			VectorRn deltaC(6);
+			MatrixRmn completeJacobian(6,numQ);
+			for (int i = 0; i < 3; ++i)
+			{
+				deltaC.Set(i,deltaS[i]);
+				deltaC.Set(i+3,deltaR[i]);
+				for (int j = 0; j < numQ; ++j)
+				{
+					completeJacobian.Set(i,j,linear_jacobian[i*numQ+j]);
+					completeJacobian.Set(i+3,j,angular_jacobian[i*numQ+j]);
+				}
+			}
+			m_data->m_ikJacobian->SetDeltaS(deltaC);
+			m_data->m_ikJacobian->SetJendTrans(completeJacobian);
+		} else
+		{
+			VectorRn deltaC(3);
+			MatrixRmn completeJacobian(3,numQ);
+			for (int i = 0; i < 3; ++i)
+			{
+				deltaC.Set(i,deltaS[i]);
+				for (int j = 0; j < numQ; ++j)
+				{
+					completeJacobian.Set(i,j,linear_jacobian[i*numQ+j]);
+				}
+			}
+			m_data->m_ikJacobian->SetDeltaS(deltaC);
+			m_data->m_ikJacobian->SetJendTrans(completeJacobian);
+		}
+    }
+    
+    // Calculate the change in theta values
+    switch (ikMethod) {
+        case IK2_JACOB_TRANS:
+            m_data->m_ikJacobian->CalcDeltaThetasTranspose();		// Jacobian transpose method
+            break;
+		case IK2_DLS:
+        case IK2_VEL_DLS:
+		case IK2_VEL_DLS_WITH_ORIENTATION:
+            m_data->m_ikJacobian->CalcDeltaThetasDLS();			// Damped least squares method
+            break;
+        case IK2_VEL_DLS_WITH_NULLSPACE:
+        case IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE:
+            assert(m_data->m_nullSpaceVelocity.GetLength()==numQ);
+            m_data->m_ikJacobian->CalcDeltaThetasDLSwithNullspace(m_data->m_nullSpaceVelocity);
+            break;
+        case IK2_DLS_SVD:
+            m_data->m_ikJacobian->CalcDeltaThetasDLSwithSVD();
+            break;
+        case IK2_PURE_PSEUDO:
+            m_data->m_ikJacobian->CalcDeltaThetasPseudoinverse();	// Pure pseudoinverse method
+            break;
+        case IK2_SDLS:
+            m_data->m_ikJacobian->CalcDeltaThetasSDLS();			// Selectively damped least squares method
+            break;
+        default:
+            m_data->m_ikJacobian->ZeroDeltaThetas();
+            break;
+    }
+    
+    // Use for velocity IK, update theta dot
+    //m_data->m_ikJacobian->UpdateThetaDot();
+    
+    // Use for position IK, incrementally update theta
+    //m_data->m_ikJacobian->UpdateThetas();
+    
+    // Apply the change in the theta values
+    //m_data->m_ikJacobian->UpdatedSClampValue(&targets);
+    
+    for (int i=0;i<numQ;i++)
+    {
+        // Use for velocity IK
+        q_new[i] = m_data->m_ikJacobian->dTheta[i] + q_current[i];
+        
+        // Use for position IK
+        //q_new[i] = m_data->m_ikNodes[i]->GetTheta();
+    }
+    return true;
+}
+
+bool IKTrajectoryHelper::computeNullspaceVel(int numQ, const double* q_current, const double* lower_limit, const double* upper_limit, const double* joint_range, const double* rest_pose)
+{
+    m_data->m_nullSpaceVelocity.SetLength(numQ);
+    m_data->m_nullSpaceVelocity.SetZero();
+    double stayCloseToZeroGain = 0.1;
+    double stayAwayFromLimitsGain = 10.0;
+
+    // Stay close to zero
+    for (int i = 0; i < numQ; ++i)
+    {
+        m_data->m_nullSpaceVelocity[i] = stayCloseToZeroGain * (rest_pose[i]-q_current[i]);
+    }
+
+    // Stay away from joint limits
+    for (int i = 0; i < numQ; ++i) {
+        if (q_current[i] > upper_limit[i]) {
+            m_data->m_nullSpaceVelocity[i] += stayAwayFromLimitsGain * (upper_limit[i] - q_current[i]) / joint_range[i];
+        }
+        if (q_current[i] < lower_limit[i]) {
+            m_data->m_nullSpaceVelocity[i] += stayAwayFromLimitsGain * (lower_limit[i] - q_current[i]) / joint_range[i];
+        }
+    }
+    return true;
+}

Engine/lib/bullet/examples/SharedMemory/IKTrajectoryHelper.h

@@ -0,0 +1,36 @@
+#ifndef IK_TRAJECTORY_HELPER_H
+#define IK_TRAJECTORY_HELPER_H
+
+enum IK2_Method
+{
+    IK2_JACOB_TRANS=0,
+    IK2_PURE_PSEUDO,
+    IK2_DLS,
+    IK2_SDLS ,
+    IK2_DLS_SVD,
+    IK2_VEL_DLS,
+	IK2_VEL_DLS_WITH_ORIENTATION,
+    IK2_VEL_DLS_WITH_NULLSPACE,
+    IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE,
+};
+
+
+class IKTrajectoryHelper
+{
+    struct IKTrajectoryHelperInternalData* m_data;
+    
+public:
+    IKTrajectoryHelper();
+    virtual ~IKTrajectoryHelper();
+    
+	bool computeIK(const double endEffectorTargetPosition[3],
+		const double endEffectorTargetOrientation[4],
+		const double endEffectorWorldPosition[3],
+		const double endEffectorWorldOrientation[4],
+		const double* q_old, int numQ, int endEffectorIndex,
+		double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, const double dampIk[6]);
+    
+    bool computeNullspaceVel(int numQ, const double* q_current, const double* lower_limit, const double* upper_limit, const double* joint_range, const double* rest_pose);
+    
+};
+#endif //IK_TRAJECTORY_HELPER_H

Engine/lib/bullet/examples/SharedMemory/InProcessMemory.cpp

@@ -0,0 +1,49 @@
+#include "InProcessMemory.h"
+
+
+#include "LinearMath/btHashMap.h"
+
+struct InProcessMemoryInternalData
+{
+	btHashMap<btHashInt, void*> m_memoryPointers;
+
+};
+
+
+InProcessMemory::InProcessMemory()
+{
+	m_data = new InProcessMemoryInternalData;
+}
+
+InProcessMemory::~InProcessMemory()
+{
+	for (int i=0;i<m_data->m_memoryPointers.size();i++)
+	{
+		void** ptrptr = m_data->m_memoryPointers.getAtIndex(i);
+		if (ptrptr)
+		{
+			void* ptr = *ptrptr;
+			free(ptr);
+		}
+	}
+	delete m_data;
+}
+
+void*	InProcessMemory::allocateSharedMemory(int key, int size, bool allowCreation)
+{
+	void** ptrptr = m_data->m_memoryPointers[key];
+	if (ptrptr)
+	{
+		return *ptrptr;
+	}
+
+	void* ptr = malloc(size);
+	m_data->m_memoryPointers.insert(key,ptr);
+	return ptr;
+}
+
+void	InProcessMemory::releaseSharedMemory(int /*key*/, int /*size*/)
+{
+	//we don't release the memory here, but in the destructor instead,
+	//so multiple users could 'share' the memory given some key
+}

Engine/lib/bullet/examples/SharedMemory/InProcessMemory.h

@@ -0,0 +1,19 @@
+#ifndef IN_PROCESS_MEMORY_H
+#define IN_PROCESS_MEMORY_H
+
+#include "SharedMemoryInterface.h"
+
+class InProcessMemory : public SharedMemoryInterface
+{
+	struct InProcessMemoryInternalData* m_data;
+
+public:
+
+	InProcessMemory();
+	virtual ~InProcessMemory();
+
+	virtual void*	allocateSharedMemory(int key, int size, bool allowCreation);
+	virtual void	releaseSharedMemory(int key, int size);
+};
+
+#endif

Engine/lib/bullet/examples/SharedMemory/PhysicsClient.cpp

@@ -0,0 +1,6 @@
+#include "PhysicsClient.h"
+
+PhysicsClient::~PhysicsClient()
+{
+}
+

Engine/lib/bullet/examples/SharedMemory/PhysicsClient.h

@@ -0,0 +1,53 @@
+#ifndef BT_PHYSICS_CLIENT_API_H
+#define BT_PHYSICS_CLIENT_API_H
+
+//#include "SharedMemoryCommands.h"
+#include "LinearMath/btVector3.h"
+
+class PhysicsClient {
+public:
+    virtual ~PhysicsClient();
+
+    // return true if connection succesfull, can also check 'isConnected'
+    virtual bool connect() = 0;
+
+    virtual void disconnectSharedMemory() = 0;
+
+    virtual bool isConnected() const = 0;
+
+    // return non-null if there is a status, nullptr otherwise
+    virtual const struct SharedMemoryStatus* processServerStatus() = 0;
+
+    virtual struct SharedMemoryCommand* getAvailableSharedMemoryCommand() = 0;
+
+    virtual bool canSubmitCommand() const = 0;
+
+    virtual bool submitClientCommand(const struct SharedMemoryCommand& command) = 0;
+
+	virtual int getNumBodies() const = 0;
+
+	virtual int getBodyUniqueId(int serialIndex) const = 0;
+
+	virtual bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const = 0;
+	
+    virtual int getNumJoints(int bodyUniqueId) const = 0;
+
+    virtual bool getJointInfo(int bodyUniqueId, int jointIndex, struct b3JointInfo& info) const = 0;
+
+    virtual void setSharedMemoryKey(int key) = 0;
+
+    virtual void uploadBulletFileToSharedMemory(const char* data, int len) = 0;
+
+    virtual int getNumDebugLines() const = 0;
+
+    virtual const float* getDebugLinesFrom() const = 0;
+    virtual const float* getDebugLinesTo() const = 0;
+    virtual const float* getDebugLinesColor() const = 0;
+
+	virtual void getCachedCameraImage(struct b3CameraImageData* cameraData)=0;
+	
+	virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData)=0;
+	
+};
+
+#endif  // BT_PHYSICS_CLIENT_API_H

Engine/lib/bullet/examples/SharedMemory/PhysicsClientC_API.h

@@ -0,0 +1,225 @@
+#ifndef PHYSICS_CLIENT_C_API_H
+#define PHYSICS_CLIENT_C_API_H
+
+//#include "SharedMemoryBlock.h"
+#include "SharedMemoryPublic.h"
+
+#define B3_DECLARE_HANDLE(name) typedef struct name##__ { int unused; } *name
+
+B3_DECLARE_HANDLE(b3PhysicsClientHandle);
+B3_DECLARE_HANDLE(b3SharedMemoryCommandHandle);
+B3_DECLARE_HANDLE(b3SharedMemoryStatusHandle);
+
+
+#ifdef __cplusplus
+extern "C" { 
+#endif
+
+///b3ConnectSharedMemory will connect to a physics server over shared memory, so
+///make sure to start the server first.
+///and a way to spawn an OpenGL 3D GUI physics server and connect (b3CreateInProcessPhysicsServerAndConnect)
+b3PhysicsClientHandle b3ConnectSharedMemory(int key);
+
+///b3DisconnectSharedMemory will disconnect the client from the server and cleanup memory.
+void	b3DisconnectSharedMemory(b3PhysicsClientHandle physClient);
+
+///There can only be 1 outstanding command. Check if a command can be send.
+int	b3CanSubmitCommand(b3PhysicsClientHandle physClient);
+
+///blocking submit command and wait for status
+b3SharedMemoryStatusHandle b3SubmitClientCommandAndWaitStatus(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle);
+
+///In general it is better to use b3SubmitClientCommandAndWaitStatus. b3SubmitClientCommand is a non-blocking submit
+///command, which requires checking for the status manually, using b3ProcessServerStatus. Also, before sending the
+///next command, make sure to check if you can send a command using 'b3CanSubmitCommand'.
+int	b3SubmitClientCommand(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle);
+
+///non-blocking check status
+b3SharedMemoryStatusHandle	b3ProcessServerStatus(b3PhysicsClientHandle physClient);
+
+/// Get the physics server return status type. See EnumSharedMemoryServerStatus in SharedMemoryPublic.h for error codes.
+int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle);
+
+int b3GetStatusBodyIndices(b3SharedMemoryStatusHandle statusHandle, int* bodyIndicesOut, int bodyIndicesCapacity);
+
+int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle);
+
+int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
+                           int* bodyUniqueId,
+                           int* numDegreeOfFreedomQ,
+                           int* numDegreeOfFreedomU,
+                           const double* rootLocalInertialFrame[],
+                           const double* actualStateQ[],
+                           const double* actualStateQdot[],
+                           const double* jointReactionForces[]);
+
+///return the total number of bodies in the simulation
+int	b3GetNumBodies(b3PhysicsClientHandle physClient);
+
+/// return the body unique id, given the index in range [0 , b3GetNumBodies() )
+int b3GetBodyUniqueId(b3PhysicsClientHandle physClient, int serialIndex);
+
+///given a body unique id, return the body information. See b3BodyInfo in SharedMemoryPublic.h
+int b3GetBodyInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, struct b3BodyInfo* info);
+
+///give a unique body index (after loading the body) return the number of joints.
+int	b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyIndex);
+
+///given a body and joint index, return the joint information. See b3JointInfo in SharedMemoryPublic.h
+int	b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointIndex, struct b3JointInfo* info);
+    
+b3SharedMemoryCommandHandle b3CreateJoint(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info);
+
+///Request debug lines for debug visualization. The flags in debugMode are the same as used in Bullet
+///See btIDebugDraw::DebugDrawModes in Bullet/src/LinearMath/btIDebugDraw.h
+b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode);
+
+///Get the pointers to the debug line information, after b3InitRequestDebugLinesCommand returns
+///status CMD_DEBUG_LINES_COMPLETED
+void    b3GetDebugLines(b3PhysicsClientHandle physClient, struct b3DebugLines* lines);
+
+///request an image from a simulated camera, using a software renderer.
+b3SharedMemoryCommandHandle b3InitRequestCameraImage(b3PhysicsClientHandle physClient);
+void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle command, float viewMatrix[16], float projectionMatrix[16]);
+void b3RequestCameraImageSetViewMatrix(b3SharedMemoryCommandHandle command, const float cameraPosition[3], const float cameraTargetPosition[3], const float cameraUp[3]);
+void b3RequestCameraImageSetViewMatrix2(b3SharedMemoryCommandHandle commandHandle, const float cameraTargetPosition[3], float distance, float yaw, float pitch, float roll, int upAxis);
+void b3RequestCameraImageSetProjectionMatrix(b3SharedMemoryCommandHandle command, float left, float right, float bottom, float top, float nearVal, float farVal);
+void b3RequestCameraImageSetFOVProjectionMatrix(b3SharedMemoryCommandHandle command, float fov, float aspect, float nearVal, float farVal);
+void b3RequestCameraImageSetPixelResolution(b3SharedMemoryCommandHandle command, int width, int height );
+void b3RequestCameraImageSelectRenderer(b3SharedMemoryCommandHandle commandHandle, int renderer);
+void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImageData* imageData);
+
+///request an contact point information
+b3SharedMemoryCommandHandle b3InitRequestContactPointInformation(b3PhysicsClientHandle physClient);
+void b3SetContactFilterBodyA(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdA);
+void b3SetContactFilterBodyB(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueIdB);
+void b3GetContactPointInformation(b3PhysicsClientHandle physClient, struct b3ContactInformation* contactPointData);
+
+b3SharedMemoryCommandHandle	b3InitPhysicsParamCommand(b3PhysicsClientHandle physClient);
+int	b3PhysicsParamSetGravity(b3SharedMemoryCommandHandle commandHandle, double gravx,double gravy, double gravz);
+int	b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandHandle, double timeStep);
+int	b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultContactERP);
+int	b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int numSubSteps);
+int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation);
+int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations);
+
+
+
+b3SharedMemoryCommandHandle	b3InitStepSimulationCommand(b3PhysicsClientHandle physClient);
+
+b3SharedMemoryCommandHandle	b3InitResetSimulationCommand(b3PhysicsClientHandle physClient);
+
+///Load a robot from a URDF file. Status type will CMD_URDF_LOADING_COMPLETED.
+///Access the robot from the unique body index, through b3GetStatusBodyIndex(statusHandle);
+b3SharedMemoryCommandHandle	b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName);
+
+int	b3LoadUrdfCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ);
+int	b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW);
+int	b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
+int	b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase);
+
+
+///compute the forces to achieve an acceleration, given a state q and qdot using inverse dynamics
+b3SharedMemoryCommandHandle	b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex,
+	const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations);
+
+int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandle,
+	int* bodyUniqueId,
+	int* dofCount,
+	double* jointForces);
+
+b3SharedMemoryCommandHandle	b3CalculateJacobianCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, const double* localPosition, const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations);
+
+int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, double* linearJacobian, double* angularJacobian);
+
+///compute the joint positions to move the end effector to a desired target using inverse kinematics
+b3SharedMemoryCommandHandle	b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex);
+void b3CalculateInverseKinematicsAddTargetPurePosition(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[3]);
+void b3CalculateInverseKinematicsAddTargetPositionWithOrientation(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[3], const double targetOrientation[4]);
+void b3CalculateInverseKinematicsPosWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[3], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose);
+void b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[3], const double targetOrientation[4], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose);
+int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle,
+	int* bodyUniqueId,
+	int* dofCount,
+	double* jointPositions);
+
+b3SharedMemoryCommandHandle	b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
+int	b3LoadSdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
+
+b3SharedMemoryCommandHandle	b3SaveWorldCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
+
+
+///The b3JointControlCommandInit method is obsolete, use b3JointControlCommandInit2 instead
+b3SharedMemoryCommandHandle  b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode);
+
+    
+///Set joint motor control variables such as desired position/angle, desired velocity,
+///applied joint forces, dependent on the control mode (CONTROL_MODE_VELOCITY or CONTROL_MODE_TORQUE)
+b3SharedMemoryCommandHandle  b3JointControlCommandInit2(b3PhysicsClientHandle physClient, int bodyUniqueId, int controlMode);
+
+///Only use when controlMode is CONTROL_MODE_POSITION_VELOCITY_PD
+int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, int qIndex, double value);
+int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
+int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
+
+///Only use when controlMode is CONTROL_MODE_VELOCITY
+int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); /* find a better name for dof/q/u indices, point to b3JointInfo */
+int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int dofIndex,  double value);
+///Only use if when controlMode is CONTROL_MODE_TORQUE,
+int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandle, int  dofIndex, double value);
+    
+
+///the creation of collision shapes and rigid bodies etc is likely going to change,
+///but good to have a b3CreateBoxShapeCommandInit for now
+
+///create a box of size (1,1,1) at world origin (0,0,0) at orientation quat (0,0,0,1)
+///after that, you can optionally adjust the initial position, orientation and size
+b3SharedMemoryCommandHandle b3CreateBoxShapeCommandInit(b3PhysicsClientHandle physClient);
+int	b3CreateBoxCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ);
+int	b3CreateBoxCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW);
+int	b3CreateBoxCommandSetHalfExtents(b3SharedMemoryCommandHandle commandHandle, double halfExtentsX,double halfExtentsY,double halfExtentsZ);
+int	b3CreateBoxCommandSetMass(b3SharedMemoryCommandHandle commandHandle, double mass);
+int	b3CreateBoxCommandSetCollisionShapeType(b3SharedMemoryCommandHandle commandHandle, int collisionShapeType);
+int	b3CreateBoxCommandSetColorRGBA(b3SharedMemoryCommandHandle commandHandle, double red,double green,double blue, double alpha);
+
+
+///b3CreatePoseCommandInit will initialize (teleport) the pose of a body/robot. You can individually set the base position,
+///base orientation and joint angles. This will set all velocities of base and joints to zero.
+///This is not a robot control command using actuators/joint motors, but manual repositioning the robot.
+b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyIndex);
+int	b3CreatePoseCommandSetBasePosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ);
+int	b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW);
+int	b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHandle, int numJointPositions, const double* jointPositions);
+int	b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle,  int jointIndex, double jointPosition);
+
+///We are currently not reading the sensor information from the URDF file, and programmatically assign sensors.
+///This is rather inconsistent, to mix programmatical creation with loading from file.
+b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId);
+int b3CreateSensorEnable6DofJointForceTorqueSensor(b3SharedMemoryCommandHandle commandHandle, int jointIndex, int enable);
+///b3CreateSensorEnableIMUForLink is not implemented yet.
+///For now, if the IMU is located in the root link, use the root world transform to mimic an IMU.
+int b3CreateSensorEnableIMUForLink(b3SharedMemoryCommandHandle commandHandle, int linkIndex, int enable);
+
+b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient,int bodyUniqueId);
+void b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, struct b3JointSensorState *state);
+void b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int linkIndex, struct b3LinkState *state);
+
+b3SharedMemoryCommandHandle b3PickBody(b3PhysicsClientHandle physClient, double rayFromWorldX,
+                                       double rayFromWorldY, double rayFromWorldZ,
+                                       double rayToWorldX, double rayToWorldY, double rayToWorldZ);
+b3SharedMemoryCommandHandle b3MovePickedBody(b3PhysicsClientHandle physClient, double rayFromWorldX,
+                                             double rayFromWorldY, double rayFromWorldZ,
+                                             double rayToWorldX, double rayToWorldY,
+                                             double rayToWorldZ);
+b3SharedMemoryCommandHandle b3RemovePickingConstraint(b3PhysicsClientHandle physClient);
+
+/// Apply external force at the body (or link) center of mass, in world space/Cartesian coordinates.
+b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient);
+void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double force[3], const double position[3], int flags);
+void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double torque[3], int flags);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif //PHYSICS_CLIENT_C_API_H

Engine/lib/bullet/examples/SharedMemory/PhysicsClientExample.h

@@ -0,0 +1,13 @@
+#ifndef PHYSICS_CLIENT_EXAMPLE_H
+#define PHYSICS_CLIENT_EXAMPLE_H
+
+enum ClientExampleOptions
+{
+	eCLIENTEXAMPLE_LOOPBACK=1,
+	eCLIENTEXAMPLE_DIRECT=2,
+	eCLIENTEXAMPLE_SERVER=3,
+};
+
+class CommonExampleInterface*    PhysicsClientCreateFunc(struct CommonExampleOptions& options);
+
+#endif//PHYSICS_CLIENT_EXAMPLE_H

Engine/lib/bullet/examples/SharedMemory/PhysicsClientSharedMemory.cpp

@@ -0,0 +1,834 @@
+#include "PhysicsClientSharedMemory.h"
+#include "PosixSharedMemory.h"
+#include "Win32SharedMemory.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "LinearMath/btVector3.h"
+#include <string.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"
+
+
+
+
+
+struct BodyJointInfoCache
+{
+	std::string m_baseName;
+	btAlignedObjectArray<b3JointInfo> m_jointInfo;
+};
+
+struct PhysicsClientSharedMemoryInternalData {
+    SharedMemoryInterface* m_sharedMemory;
+	bool	m_ownsSharedMemory;
+    SharedMemoryBlock* m_testBlock1;
+   
+	btHashMap<btHashInt,BodyJointInfoCache*> m_bodyJointMap;
+
+    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<int> m_bodyIdsRequestInfo;
+    SharedMemoryStatus m_tempBackupServerStatus;
+    
+    SharedMemoryStatus m_lastServerStatus;
+
+    int m_counter;
+    
+    bool m_isConnected;
+    bool m_waitingForServer;
+    bool m_hasLastServerStatus;
+    int m_sharedMemoryKey;
+    bool m_verboseOutput;
+
+    PhysicsClientSharedMemoryInternalData()
+        : m_sharedMemory(0),
+		  m_ownsSharedMemory(false),
+          m_testBlock1(0),
+		  m_counter(0),
+		  m_cachedCameraPixelsWidth(0),
+		  m_cachedCameraPixelsHeight(0),
+		  
+          m_isConnected(false),
+          m_waitingForServer(false),
+          m_hasLastServerStatus(false),
+          m_sharedMemoryKey(SHARED_MEMORY_KEY),
+          m_verboseOutput(false) {}
+
+    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;
+		info.m_baseName = bodyJoints->m_baseName.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;
+	
+}
+
+bool PhysicsClientSharedMemory::getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo& info) const
+{
+	BodyJointInfoCache** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
+	if (bodyJointsPtr && *bodyJointsPtr)
+	{
+		BodyJointInfoCache* bodyJoints = *bodyJointsPtr;
+		info = bodyJoints->m_jointInfo[jointIndex];
+        return true;
+	}
+    return false;
+}
+
+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();
+    }
+	if (m_data->m_ownsSharedMemory)
+	{
+	    delete m_data->m_sharedMemory;
+	}
+    delete m_data;
+}
+
+void PhysicsClientSharedMemory::setSharedMemoryKey(int key) { m_data->m_sharedMemoryKey = key; }
+
+
+void PhysicsClientSharedMemory::setSharedMemoryInterface(class SharedMemoryInterface* 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; }
+
+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) {
+            b3Error("Error: 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 {
+        b3Error("Cannot connect to shared memory");
+        return false;
+    }
+    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_dataStreamArguments.m_streamChunkLength);
+    bf.setFileDNAisMemoryDNA();
+    bf.parse(false);
+    
+    
+    BodyJointInfoCache* bodyJoints = new BodyJointInfoCache;
+    m_data->m_bodyJointMap.insert(bodyUniqueId,bodyJoints);
+    
+    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");
+    }
+}
+
+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) {
+        btAssert(m_data->m_testBlock1->m_numServerCommands ==
+                 m_data->m_testBlock1->m_numProcessedServerCommands + 1);
+
+        const SharedMemoryStatus& serverCmd = m_data->m_testBlock1->m_serverCommands[0];
+        m_data->m_lastServerStatus = serverCmd;
+
+        EnumSharedMemoryServerStatus s = (EnumSharedMemoryServerStatus)serverCmd.m_type;
+        // consume the command
+
+        switch (serverCmd.m_type) {
+            case CMD_CLIENT_COMMAND_COMPLETED: {
+                if (m_data->m_verboseOutput) {
+                    b3Printf("Server completed command");
+                }
+                break;
+            }
+            case CMD_SDF_LOADING_COMPLETED: {
+                
+                if (m_data->m_verboseOutput) {
+                    b3Printf("Server loading the SDF OK\n");
+                }
+
+                break;
+            }
+            case CMD_URDF_LOADING_COMPLETED: {
+                
+                if (m_data->m_verboseOutput) {
+                    b3Printf("Server loading the URDF OK\n");
+                }
+
+                if (serverCmd.m_dataStreamArguments.m_streamChunkLength > 0) {
+                    bParse::btBulletFile bf(
+                        this->m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor,
+                        serverCmd.m_dataStreamArguments.m_streamChunkLength);
+                    bf.setFileDNAisMemoryDNA();
+                    bf.parse(false);
+					int bodyUniqueId = serverCmd.m_dataStreamArguments.m_bodyUniqueId;
+
+					BodyJointInfoCache* bodyJoints = new BodyJointInfoCache;
+                    m_data->m_bodyJointMap.insert(bodyUniqueId,bodyJoints);
+
+                    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];
+
+							addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput);
+                        } else 
+						{
+                            Bullet::btMultiBodyFloatData* mb =
+                                (Bullet::btMultiBodyFloatData*)bf.m_multiBodies[i];
+
+							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: {
+                if (m_data->m_verboseOutput) {
+                    b3Printf("Server received desired state");
+                }
+                break;
+            }
+            case CMD_STEP_FORWARD_SIMULATION_COMPLETED: {
+                if (m_data->m_verboseOutput) {
+                    b3Printf("Server completed step simulation");
+                }
+                break;
+            }
+            case CMD_URDF_LOADING_FAILED: {
+                if (m_data->m_verboseOutput) {
+                    b3Printf("Server failed loading the URDF...\n");
+                }
+                
+                break;
+            }
+            
+            case 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_SDF_LOADING_FAILED: {
+                if (m_data->m_verboseOutput) {
+                    b3Printf("Server failed loading the SDF...\n");
+                }
+                
+                break;
+            }
+
+            case 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: {
+                if (m_data->m_verboseOutput) {
+                    b3Printf("Server failed receiving bullet data stream\n");
+                }
+
+                break;
+            }
+
+            case 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;
+                if (m_data->m_verboseOutput) {
+                    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,
+                                    command.m_sendActualStateArgs.m_actualStateQ[i]);
+                        } else {
+                            sprintf(msg, "%s%f", msg,
+                                    command.m_sendActualStateArgs.m_actualStateQ[i]);
+                        }
+                    }
+                    sprintf(msg, "%s]", msg);
+                }
+                if (m_data->m_verboseOutput) {
+                    b3Printf(msg);
+                }
+
+                {
+                    sprintf(msg, "U=[");
+
+                    for (int i = 0; i < numU; i++) {
+                        if (i < numU - 1) {
+                            sprintf(msg, "%s%f,", msg,
+                                    command.m_sendActualStateArgs.m_actualStateQdot[i]);
+                        } else {
+                            sprintf(msg, "%s%f", msg,
+                                    command.m_sendActualStateArgs.m_actualStateQdot[i]);
+                        }
+                    }
+                    sprintf(msg, "%s]", msg);
+                }
+                if (m_data->m_verboseOutput) {
+                    b3Printf(msg);
+                }
+
+                if (m_data->m_verboseOutput) {
+                    b3Printf("\n");
+                }
+                break;
+            }
+            case CMD_RESET_SIMULATION_COMPLETED: {
+                if (m_data->m_verboseOutput) {
+                    b3Printf("CMD_RESET_SIMULATION_COMPLETED clean data\n");
+                }
+				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)
+					{
+						BodyJointInfoCache* bodyJoints = *bodyJointsPtr;
+						for (int j=0;j<bodyJoints->m_jointInfo.size();j++) {
+							if (bodyJoints->m_jointInfo[j].m_jointName)
+							{
+								free(bodyJoints->m_jointInfo[j].m_jointName);
+							}
+							if (bodyJoints->m_jointInfo[j].m_linkName)
+							{
+								free(bodyJoints->m_jointInfo[j].m_linkName);
+							}
+						}
+						delete (*bodyJointsPtr);
+					}
+				}
+				m_data->m_bodyJointMap.clear();
+                
+                break;
+            }
+            case 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:
+			{
+
+				break;
+			}
+            case 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:
+            {
+				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:
+            {
+                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_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:
+                {
+                    b3Warning("Contact Point Information Request failed");
+                    break;
+                }
+
+			case CMD_SAVE_WORLD_COMPLETED:
+				break;
+					
+			case CMD_SAVE_WORLD_FAILED:
+			{
+				b3Warning("Saving world  failed");
+				break;
+			}
+			case CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED:
+			{
+                    break;
+                }
+            case CMD_CALCULATE_INVERSE_KINEMATICS_FAILED:
+                {
+                    b3Warning("Calculate Inverse Kinematics Request failed");
+                    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)
+        {
+            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_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
+            {
+                m_data->m_lastServerStatus = m_data->m_tempBackupServerStatus;
+            }
+        }
+        
+        if (serverCmd.m_type == CMD_CONTACT_POINT_INFORMATION_COMPLETED)
+        {
+            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_CAMERA_IMAGE_COMPLETED)
+		{
+			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)) {
+            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) {
+            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 {
+    return (m_data->m_isConnected && !m_data->m_waitingForServer);
+}
+
+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];
+        }
+    }
+}
+
+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;
+
+}
+
+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(); }

Engine/lib/bullet/examples/SharedMemory/PhysicsClientSharedMemory.h

@@ -0,0 +1,61 @@
+#ifndef BT_PHYSICS_CLIENT_SHARED_MEMORY_API_H
+#define BT_PHYSICS_CLIENT_SHARED_MEMORY_API_H
+
+#include "PhysicsClient.h"
+
+//#include "SharedMemoryCommands.h"
+#include "LinearMath/btVector3.h"
+
+class PhysicsClientSharedMemory : public PhysicsClient {
+    struct PhysicsClientSharedMemoryInternalData* m_data;
+
+protected:
+	virtual void setSharedMemoryInterface(class SharedMemoryInterface* sharedMem);
+    void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd);
+
+        
+public:
+    PhysicsClientSharedMemory();
+    virtual ~PhysicsClientSharedMemory();
+
+    // return true if connection succesfull, can also check 'isConnected'
+    virtual bool connect();
+
+    virtual void disconnectSharedMemory();
+
+    virtual bool isConnected() const;
+
+    // return non-null if there is a status, nullptr otherwise
+    virtual const struct SharedMemoryStatus* processServerStatus();
+
+    virtual struct SharedMemoryCommand* getAvailableSharedMemoryCommand();
+
+    virtual bool canSubmitCommand() const;
+
+    virtual bool submitClientCommand(const struct SharedMemoryCommand& command);
+
+	virtual int getNumBodies() const;
+
+	virtual int getBodyUniqueId(int serialIndex) const;
+
+	virtual bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const;
+
+    virtual int getNumJoints(int bodyUniqueId) const;
+
+    virtual bool getJointInfo(int bodyUniqueId, int jointIndex, struct b3JointInfo& info) const;
+
+    virtual void setSharedMemoryKey(int key);
+
+    virtual void uploadBulletFileToSharedMemory(const char* data, int len);
+
+    virtual int getNumDebugLines() const;
+
+    virtual const float* getDebugLinesFrom() const;
+    virtual const float* getDebugLinesTo() const;
+    virtual const float* getDebugLinesColor() const;
+	virtual void getCachedCameraImage(struct b3CameraImageData* cameraData);
+	
+	virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData);
+};
+
+#endif  // BT_PHYSICS_CLIENT_API_H

Engine/lib/bullet/examples/SharedMemory/PhysicsDirect.cpp

@@ -0,0 +1,578 @@
+#include "PhysicsDirect.h"
+
+#include "PhysicsClientSharedMemory.h"
+#include "../CommonInterfaces/CommonGUIHelperInterface.h"
+#include "SharedMemoryCommands.h"
+#include "PhysicsServerCommandProcessor.h"
+#include "LinearMath/btHashMap.h"
+#include "LinearMath/btAlignedObjectArray.h"
+#include "../../Extras/Serialize/BulletFileLoader/btBulletFile.h"
+#include "../../Extras/Serialize/BulletFileLoader/autogenerated/bullet.h"
+#include "BodyJointInfoUtility.h"
+#include <string>
+
+struct BodyJointInfoCache2
+{
+	std::string m_baseName;
+	btAlignedObjectArray<b3JointInfo> m_jointInfo;
+};
+
+struct PhysicsDirectInternalData
+{
+	DummyGUIHelper m_noGfx;
+
+	SharedMemoryCommand m_command;
+	SharedMemoryStatus m_serverStatus;
+	bool m_hasStatus;
+	bool m_verboseOutput;
+	
+	btAlignedObjectArray<TmpFloat3> m_debugLinesFrom;
+    btAlignedObjectArray<TmpFloat3> m_debugLinesTo;
+    btAlignedObjectArray<TmpFloat3> m_debugLinesColor;
+
+	btHashMap<btHashInt,BodyJointInfoCache2*> m_bodyJointMap;
+	
+	char    m_bulletStreamDataServerToClient[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE];
+
+	int m_cachedCameraPixelsWidth;
+	int m_cachedCameraPixelsHeight;
+	btAlignedObjectArray<unsigned char> m_cachedCameraPixelsRGBA;
+	btAlignedObjectArray<float> m_cachedCameraDepthBuffer;
+	btAlignedObjectArray<int> m_cachedSegmentationMask;
+	
+    btAlignedObjectArray<b3ContactPointData> m_cachedContactPoints;
+    
+
+
+	PhysicsServerCommandProcessor* m_commandProcessor;
+
+	PhysicsDirectInternalData()
+		:m_hasStatus(false),
+		m_verboseOutput(false)
+	{
+	}
+};
+
+PhysicsDirect::PhysicsDirect()
+{
+	m_data = new PhysicsDirectInternalData;
+	m_data->m_commandProcessor = new PhysicsServerCommandProcessor;
+	
+	
+}
+
+PhysicsDirect::~PhysicsDirect()
+{
+	delete m_data->m_commandProcessor;
+	delete m_data;
+}
+
+
+// return true if connection succesfull, can also check 'isConnected'
+bool PhysicsDirect::connect()
+{
+	m_data->m_commandProcessor->setGuiHelper(&m_data->m_noGfx);
+
+	return true;
+}
+
+// return true if connection succesfull, can also check 'isConnected'
+bool PhysicsDirect::connect(struct GUIHelperInterface* guiHelper)
+{
+	m_data->m_commandProcessor->setGuiHelper(guiHelper);
+	
+	return true;
+}
+
+void PhysicsDirect::renderScene()
+{
+	m_data->m_commandProcessor->renderScene();
+}
+void PhysicsDirect::debugDraw(int debugDrawMode)
+{
+	m_data->m_commandProcessor->physicsDebugDraw(debugDrawMode);
+}
+
+////todo: rename to 'disconnect'
+void PhysicsDirect::disconnectSharedMemory()
+{
+	m_data->m_commandProcessor->setGuiHelper(0);
+}
+
+bool PhysicsDirect::isConnected() const
+{
+	return true;
+}
+
+// return non-null if there is a status, nullptr otherwise
+const  SharedMemoryStatus* PhysicsDirect::processServerStatus()
+{
+	SharedMemoryStatus* stat = 0;
+	if (m_data->m_hasStatus)
+	{
+		stat = &m_data->m_serverStatus;
+		m_data->m_hasStatus = false;
+	}
+	return stat;
+}
+
+SharedMemoryCommand* PhysicsDirect::getAvailableSharedMemoryCommand()
+{
+	return &m_data->m_command;
+}
+
+bool PhysicsDirect::canSubmitCommand() const
+{
+	return true;
+}
+
+bool PhysicsDirect::processDebugLines(const struct SharedMemoryCommand& orgCommand)
+{
+	SharedMemoryCommand command = orgCommand;
+
+	const SharedMemoryStatus& serverCmd = m_data->m_serverStatus;
+
+	do
+	{
+
+		bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
+		m_data->m_hasStatus = hasStatus;
+		if (hasStatus)
+		{
+			btAssert(m_data->m_serverStatus.m_type == 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_bulletStreamDataServerToClient[0];
+			float* linesTo =
+				(float*)(&m_data->m_bulletStreamDataServerToClient[0] + 
+						numLines * 3 * sizeof(float));
+			float* linesColor =
+				(float*)(&m_data->m_bulletStreamDataServerToClient[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;
+			}
+
+			if (serverCmd.m_sendDebugLinesArgs.m_numRemainingDebugLines > 0)
+			{
+				command.m_type = CMD_REQUEST_DEBUG_LINES;
+				command.m_requestDebugLinesArguments.m_startingLineIndex =
+					serverCmd.m_sendDebugLinesArgs.m_numDebugLines +
+					serverCmd.m_sendDebugLinesArgs.m_startingLineIndex;
+			}
+		}
+
+	} while (serverCmd.m_sendDebugLinesArgs.m_numRemainingDebugLines > 0);
+	
+	return m_data->m_hasStatus;
+}
+
+bool PhysicsDirect::processContactPointData(const struct SharedMemoryCommand& orgCommand)
+{
+    SharedMemoryCommand command = orgCommand;
+    
+    const SharedMemoryStatus& serverCmd = m_data->m_serverStatus;
+    
+    do
+    {
+        bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
+        m_data->m_hasStatus = hasStatus;
+        if (hasStatus)
+        {
+            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_bulletStreamDataServerToClient[0];
+            
+            for (int i=0;i<numContactsCopied;i++)
+            {
+                m_data->m_cachedContactPoints[startContactIndex+i] = contactData[i];
+            }
+            
+            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;
+                
+            }
+
+        }
+    } while (serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints > 0 && serverCmd.m_sendContactPointArgs.m_numContactPointsCopied);
+    
+    return m_data->m_hasStatus;
+
+}
+
+
+bool PhysicsDirect::processCamera(const struct SharedMemoryCommand& orgCommand)
+{
+	SharedMemoryCommand command = orgCommand;
+
+	const SharedMemoryStatus& serverCmd = m_data->m_serverStatus;
+
+	do
+	{
+
+		bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
+		m_data->m_hasStatus = hasStatus;
+		if (hasStatus)
+		{
+			btAssert(m_data->m_serverStatus.m_type == 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_cachedSegmentationMask.resize(numTotalPixels);
+			m_data->m_cachedCameraPixelsRGBA.resize(numTotalPixels*numBytesPerPixel);
+                
+                
+			unsigned char* rgbaPixelsReceived =
+                (unsigned char*)&m_data->m_bulletStreamDataServerToClient[0];
+			
+			float* depthBuffer = (float*)&(m_data->m_bulletStreamDataServerToClient[serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*4]);
+			int* segmentationMaskBuffer = (int*)&(m_data->m_bulletStreamDataServerToClient[serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*8]);
+			
+          //  printf("pixel = %d\n", rgbaPixelsReceived[0]);
+                
+			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_cachedSegmentationMask[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];
+			}
+
+			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;
+				
+			} else
+			{
+				m_data->m_cachedCameraPixelsWidth = serverCmd.m_sendPixelDataArguments.m_imageWidth;
+				m_data->m_cachedCameraPixelsHeight = serverCmd.m_sendPixelDataArguments.m_imageHeight;
+			}	
+		}
+	}  while (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0 && serverCmd.m_sendPixelDataArguments.m_numPixelsCopied);
+	
+	return m_data->m_hasStatus;
+
+
+}
+
+
+void PhysicsDirect::processBodyJointInfo(int bodyUniqueId, const SharedMemoryStatus& serverCmd)
+{
+    bParse::btBulletFile bf(
+        &m_data->m_bulletStreamDataServerToClient[0],
+        serverCmd.m_dataStreamArguments.m_streamChunkLength);
+    bf.setFileDNAisMemoryDNA();
+    bf.parse(false);
+	
+    BodyJointInfoCache2* bodyJoints = new BodyJointInfoCache2;
+    m_data->m_bodyJointMap.insert(bodyUniqueId,bodyJoints);
+
+    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");
+    }
+}
+
+bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& command)
+{
+	if (command.m_type==CMD_REQUEST_DEBUG_LINES)
+	{			
+		return processDebugLines(command);
+	}
+
+	if (command.m_type==CMD_REQUEST_CAMERA_IMAGE_DATA)
+	{
+		return processCamera(command);
+	}
+    if (command.m_type == CMD_REQUEST_CONTACT_POINT_INFORMATION)
+    {
+        return processContactPointData(command);
+    }
+
+	bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
+	m_data->m_hasStatus = hasStatus;
+	if (hasStatus)
+	{
+		const SharedMemoryStatus& serverCmd = m_data->m_serverStatus;
+
+		switch (m_data->m_serverStatus.m_type)
+		{
+			case CMD_RESET_SIMULATION_COMPLETED:
+			{
+				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++)
+				{
+					BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap.getAtIndex(i);
+					if (bodyJointsPtr && *bodyJointsPtr)
+					{
+						BodyJointInfoCache2* bodyJoints = *bodyJointsPtr;
+						for (int j=0;j<bodyJoints->m_jointInfo.size();j++) {
+							if (bodyJoints->m_jointInfo[j].m_jointName)
+							{
+								free(bodyJoints->m_jointInfo[j].m_jointName);
+							}
+							if (bodyJoints->m_jointInfo[j].m_linkName)
+							{
+								free(bodyJoints->m_jointInfo[j].m_linkName);
+							}
+						}
+						delete (*bodyJointsPtr);
+					}
+				}
+				m_data->m_bodyJointMap.clear();
+                
+				break;
+			}
+			case CMD_SDF_LOADING_COMPLETED:
+            {
+                //we'll stream further info from the physics server
+                //so serverCmd will be invalid, make a copy
+                
+                
+                int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies;
+                for (int i=0;i<numBodies;i++)
+                {
+                    int bodyUniqueId = serverCmd.m_sdfLoadedArgs.m_bodyUniqueIds[i];
+                    SharedMemoryCommand infoRequestCommand;
+                    infoRequestCommand.m_type = CMD_REQUEST_BODY_INFO;
+                    infoRequestCommand.m_sdfRequestInfoArgs.m_bodyUniqueId = bodyUniqueId;
+                    SharedMemoryStatus infoStatus;
+                    bool hasStatus = m_data->m_commandProcessor->processCommand(infoRequestCommand,infoStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
+                    if (hasStatus)
+                    {
+                        processBodyJointInfo(bodyUniqueId, infoStatus);
+                    }
+                }
+                break;
+            }
+			case CMD_URDF_LOADING_COMPLETED:
+			{
+				
+				if (serverCmd.m_dataStreamArguments.m_streamChunkLength > 0) 
+				{
+				    int bodyIndex = serverCmd.m_dataStreamArguments.m_bodyUniqueId;
+                    processBodyJointInfo(bodyIndex,serverCmd);
+				}
+                break;
+            }
+			
+		 default:
+			 {
+				// b3Error("Unknown server status type");
+			 }
+		};
+										  
+		
+	}
+	return hasStatus;
+}
+
+int PhysicsDirect::getNumBodies() const
+{
+	return m_data->m_bodyJointMap.size();
+}
+
+
+int PhysicsDirect::getBodyUniqueId(int serialIndex) const
+{
+	if ((serialIndex >= 0) && (serialIndex < getNumBodies()))
+	{
+		return m_data->m_bodyJointMap.getKeyAtIndex(serialIndex).getUid1();
+	}
+	return -1;
+}
+
+bool PhysicsDirect::getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const
+{
+	BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyUniqueId];
+	if (bodyJointsPtr && *bodyJointsPtr)
+	{
+		BodyJointInfoCache2* bodyJoints = *bodyJointsPtr;
+		info.m_baseName = bodyJoints->m_baseName.c_str();
+		return true;
+	}
+	
+	return false;
+}
+
+int PhysicsDirect::getNumJoints(int bodyIndex) const
+{
+	BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyIndex];
+	if (bodyJointsPtr && *bodyJointsPtr)
+	{
+		BodyJointInfoCache2* bodyJoints = *bodyJointsPtr;
+		return bodyJoints->m_jointInfo.size(); 
+	}
+	btAssert(0);
+	return 0;
+}
+
+bool PhysicsDirect::getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const
+{
+	BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap[bodyIndex];
+	if (bodyJointsPtr && *bodyJointsPtr)
+	{
+		BodyJointInfoCache2* bodyJoints = *bodyJointsPtr;
+        if (jointIndex < bodyJoints->m_jointInfo.size())
+        {
+            info = bodyJoints->m_jointInfo[jointIndex];
+            return true;
+        }
+	}
+    return false;
+}
+
+///todo: move this out of the
+void PhysicsDirect::setSharedMemoryKey(int key)
+{
+	//m_data->m_physicsServer->setSharedMemoryKey(key);
+	//m_data->m_physicsClient->setSharedMemoryKey(key);
+}
+
+void PhysicsDirect::uploadBulletFileToSharedMemory(const char* data, int len)
+{
+	//m_data->m_physicsClient->uploadBulletFileToSharedMemory(data,len);
+}
+
+int PhysicsDirect::getNumDebugLines() const
+{
+	return m_data->m_debugLinesFrom.size();
+}
+
+const float* PhysicsDirect::getDebugLinesFrom() const
+{
+	if (getNumDebugLines())
+	{
+		return &m_data->m_debugLinesFrom[0].m_x;
+	}
+	return 0;
+}
+const float* PhysicsDirect::getDebugLinesTo() const
+{
+	if (getNumDebugLines())
+	{
+		return &m_data->m_debugLinesTo[0].m_x;
+	}
+	return 0;
+}
+const float* PhysicsDirect::getDebugLinesColor() const
+{
+	if (getNumDebugLines())
+	{
+		return &m_data->m_debugLinesColor[0].m_x;
+	}
+	return 0;
+}
+
+void PhysicsDirect::getCachedCameraImage(b3CameraImageData* cameraData)
+{
+	if (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_cachedSegmentationMask.size()? &m_data->m_cachedSegmentationMask[0] : 0;
+	}
+}
+
+void PhysicsDirect::getCachedContactPointInformation(struct b3ContactInformation* contactPointData)
+{
+    contactPointData->m_numContactPoints = m_data->m_cachedContactPoints.size();
+    contactPointData->m_contactPointData = contactPointData->m_numContactPoints? &m_data->m_cachedContactPoints[0] : 0;
+    
+}
+

Engine/lib/bullet/examples/SharedMemory/PhysicsDirect.h

@@ -0,0 +1,84 @@
+#ifndef PHYSICS_DIRECT_H
+#define PHYSICS_DIRECT_H
+
+//#include "SharedMemoryCommands.h"
+
+
+#include "PhysicsClient.h"
+#include "LinearMath/btVector3.h"
+
+///todo: the PhysicsClient API was designed with shared memory in mind, 
+///now it become more general we need to move out the shared memory specifics away
+///for example naming [disconnectSharedMemory -> disconnect] [ move setSharedMemoryKey to shared memory specific subclass ]
+///PhysicsDirect executes the commands directly, without transporting them or having a separate server executing commands
+class PhysicsDirect : public PhysicsClient 
+{
+protected:
+
+	struct PhysicsDirectInternalData* m_data;
+
+	bool processDebugLines(const struct SharedMemoryCommand& orgCommand);
+
+	bool processCamera(const struct SharedMemoryCommand& orgCommand);
+
+    bool processContactPointData(const struct SharedMemoryCommand& orgCommand);
+
+    void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd);
+    
+public:
+
+    PhysicsDirect();
+    
+    virtual ~PhysicsDirect();
+
+	// return true if connection succesfull, can also check 'isConnected'
+	//it is OK to pass a null pointer for the gui helper
+    virtual bool connect();
+	
+	////todo: rename to 'disconnect'
+    virtual void disconnectSharedMemory();
+
+    virtual bool isConnected() const;
+
+    // return non-null if there is a status, nullptr otherwise
+    virtual const  SharedMemoryStatus* processServerStatus();
+
+    virtual  SharedMemoryCommand* getAvailableSharedMemoryCommand();
+
+    virtual bool canSubmitCommand() const;
+
+    virtual bool submitClientCommand(const struct SharedMemoryCommand& command);
+
+	virtual int getNumBodies() const;
+
+	virtual int getBodyUniqueId(int serialIndex) const;
+
+	virtual bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const;
+
+    virtual int getNumJoints(int bodyIndex) const;
+
+    virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;
+
+	///todo: move this out of the
+    virtual void setSharedMemoryKey(int key);
+
+    void uploadBulletFileToSharedMemory(const char* data, int len);
+
+    virtual int getNumDebugLines() const;
+
+    virtual const float* getDebugLinesFrom() const;
+    virtual const float* getDebugLinesTo() const;
+    virtual const float* getDebugLinesColor() const;
+
+	virtual void getCachedCameraImage(b3CameraImageData* cameraData);
+
+    virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData);
+
+	//those 2 APIs are for internal use for visualization
+	virtual bool connect(struct GUIHelperInterface* guiHelper);
+	virtual void renderScene();
+	virtual void debugDraw(int debugDrawMode);
+
+};
+
+#endif //PHYSICS_DIRECT_H

Engine/lib/bullet/examples/SharedMemory/PhysicsDirectC_API.cpp

@@ -0,0 +1,14 @@
+#include "PhysicsDirectC_API.h"
+
+#include "PhysicsDirect.h"
+
+
+
+//think more about naming. The b3ConnectPhysicsLoopback
+b3PhysicsClientHandle b3ConnectPhysicsDirect()
+{
+	PhysicsDirect* direct = new PhysicsDirect();
+	bool connected = direct->connect();
+	return (b3PhysicsClientHandle  )direct;
+}
+

Engine/lib/bullet/examples/SharedMemory/PhysicsDirectC_API.h

@@ -0,0 +1,19 @@
+#ifndef PHYSICS_DIRECT_C_API_H
+#define PHYSICS_DIRECT_C_API_H
+
+#include "PhysicsClientC_API.h"
+
+#ifdef __cplusplus
+extern "C" { 
+#endif
+
+
+///think more about naming. Directly execute commands without transport (no shared memory, UDP, socket, grpc etc)
+b3PhysicsClientHandle b3ConnectPhysicsDirect();
+
+    
+#ifdef __cplusplus
+}
+#endif
+
+#endif //PHYSICS_DIRECT_C_API_H

Engine/lib/bullet/examples/SharedMemory/PhysicsLoopBack.cpp

@@ -0,0 +1,142 @@
+#include "PhysicsLoopBack.h"
+#include "PhysicsServerSharedMemory.h"
+#include "PhysicsClientSharedMemory.h"
+#include "../CommonInterfaces/CommonGUIHelperInterface.h"
+
+struct PhysicsLoopBackInternalData
+{
+	PhysicsClientSharedMemory* m_physicsClient;
+	PhysicsServerSharedMemory* m_physicsServer;
+	DummyGUIHelper m_noGfx;
+	
+
+	PhysicsLoopBackInternalData()
+		:m_physicsClient(0),
+		m_physicsServer(0)
+	{
+	}
+};
+
+PhysicsLoopBack::PhysicsLoopBack()
+{
+	m_data = new PhysicsLoopBackInternalData;
+	m_data->m_physicsServer = new PhysicsServerSharedMemory();
+	m_data->m_physicsClient = new PhysicsClientSharedMemory();
+}
+
+PhysicsLoopBack::~PhysicsLoopBack()
+{
+	delete m_data->m_physicsClient;
+	delete m_data->m_physicsServer;
+	delete m_data;
+}
+
+
+// return true if connection succesfull, can also check 'isConnected'
+bool PhysicsLoopBack::connect()
+{
+	m_data->m_physicsServer->connectSharedMemory(&m_data->m_noGfx);
+	m_data->m_physicsClient->connect();
+	return m_data->m_physicsClient->isConnected();
+}
+
+////todo: rename to 'disconnect'
+void PhysicsLoopBack::disconnectSharedMemory()
+{
+	m_data->m_physicsClient->disconnectSharedMemory();
+	m_data->m_physicsServer->disconnectSharedMemory(true);
+}
+
+bool PhysicsLoopBack::isConnected() const
+{
+	return m_data->m_physicsClient->isConnected();
+}
+
+// return non-null if there is a status, nullptr otherwise
+const  SharedMemoryStatus* PhysicsLoopBack::processServerStatus()
+{
+	m_data->m_physicsServer->processClientCommands();
+	return m_data->m_physicsClient->processServerStatus();
+}
+
+SharedMemoryCommand* PhysicsLoopBack::getAvailableSharedMemoryCommand()
+{
+	return m_data->m_physicsClient->getAvailableSharedMemoryCommand();
+}
+
+bool PhysicsLoopBack::canSubmitCommand() const
+{
+	return m_data->m_physicsClient->canSubmitCommand();
+}
+
+bool PhysicsLoopBack::submitClientCommand(const struct SharedMemoryCommand& command)
+{
+	return  m_data->m_physicsClient->submitClientCommand(command);
+}
+
+int PhysicsLoopBack::getNumBodies() const
+{
+	return m_data->m_physicsClient->getNumBodies();
+}
+
+int PhysicsLoopBack::getBodyUniqueId(int serialIndex) const
+{
+	return m_data->m_physicsClient->getBodyUniqueId(serialIndex);
+}
+
+bool PhysicsLoopBack::getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const
+{
+	return m_data->m_physicsClient->getBodyInfo(bodyUniqueId, info);
+}
+
+int PhysicsLoopBack::getNumJoints(int bodyIndex) const
+{
+	return m_data->m_physicsClient->getNumJoints(bodyIndex);
+}
+
+bool PhysicsLoopBack::getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const
+{
+	return m_data->m_physicsClient->getJointInfo(bodyIndex,jointIndex,info);
+}
+
+///todo: move this out of the
+void PhysicsLoopBack::setSharedMemoryKey(int key)
+{
+	m_data->m_physicsServer->setSharedMemoryKey(key);
+	m_data->m_physicsClient->setSharedMemoryKey(key);
+}
+
+void PhysicsLoopBack::uploadBulletFileToSharedMemory(const char* data, int len)
+{
+	m_data->m_physicsClient->uploadBulletFileToSharedMemory(data,len);
+}
+
+int PhysicsLoopBack::getNumDebugLines() const
+{
+	return m_data->m_physicsClient->getNumDebugLines();
+}
+
+const float* PhysicsLoopBack::getDebugLinesFrom() const
+{
+	return m_data->m_physicsClient->getDebugLinesFrom();
+}
+
+const float* PhysicsLoopBack::getDebugLinesTo() const
+{
+	return m_data->m_physicsClient->getDebugLinesTo();
+}
+
+const float* PhysicsLoopBack::getDebugLinesColor() const
+{
+	return m_data->m_physicsClient->getDebugLinesColor();
+}
+
+void PhysicsLoopBack::getCachedCameraImage(struct b3CameraImageData* cameraData)
+{
+	return m_data->m_physicsClient->getCachedCameraImage(cameraData);
+}
+
+void PhysicsLoopBack::getCachedContactPointInformation(struct b3ContactInformation* contactPointData)
+{
+    return m_data->m_physicsClient->getCachedContactPointInformation(contactPointData);
+}

Engine/lib/bullet/examples/SharedMemory/PhysicsLoopBack.h

@@ -0,0 +1,67 @@
+#ifndef PHYSICS_LOOP_BACK_H
+#define PHYSICS_LOOP_BACK_H
+
+//#include "SharedMemoryCommands.h"
+
+
+#include "PhysicsClient.h"
+#include "LinearMath/btVector3.h"
+
+///todo: the PhysicsClient API was designed with shared memory in mind, 
+///now it become more general we need to move out the shared memory specifics away
+///for example naming [disconnectSharedMemory -> disconnect] [ move setSharedMemoryKey to shared memory specific subclass ]
+
+class PhysicsLoopBack : public PhysicsClient 
+{
+	struct PhysicsLoopBackInternalData* m_data;
+
+public:
+
+	PhysicsLoopBack();
+
+	virtual ~PhysicsLoopBack();
+
+	  // return true if connection succesfull, can also check 'isConnected'
+    virtual bool connect();
+
+	////todo: rename to 'disconnect'
+    virtual void disconnectSharedMemory();
+
+    virtual bool isConnected() const;
+
+    // return non-null if there is a status, nullptr otherwise
+    virtual const  SharedMemoryStatus* processServerStatus();
+
+    virtual  SharedMemoryCommand* getAvailableSharedMemoryCommand();
+
+    virtual bool canSubmitCommand() const;
+
+    virtual bool submitClientCommand(const struct SharedMemoryCommand& command);
+
+	virtual int getNumBodies() const;
+
+	virtual int getBodyUniqueId(int serialIndex) const;
+
+	virtual bool getBodyInfo(int bodyUniqueId, struct b3BodyInfo& info) const;
+
+    virtual int getNumJoints(int bodyIndex) const;
+
+    virtual bool getJointInfo(int bodyIndex, int jointIndex, struct b3JointInfo& info) const;
+
+	///todo: move this out of the
+    virtual void setSharedMemoryKey(int key);
+
+    void uploadBulletFileToSharedMemory(const char* data, int len);
+
+    virtual int getNumDebugLines() const;
+
+    virtual const float* getDebugLinesFrom() const;
+    virtual const float* getDebugLinesTo() const;
+    virtual const float* getDebugLinesColor() const;
+	virtual void getCachedCameraImage(struct b3CameraImageData* cameraData);
+	
+	virtual void getCachedContactPointInformation(struct b3ContactInformation* contactPointData);
+
+};
+
+#endif //PHYSICS_LOOP_BACK_H

Engine/lib/bullet/examples/SharedMemory/PhysicsLoopBackC_API.cpp

@@ -0,0 +1,15 @@
+#include "PhysicsLoopBackC_API.h"
+
+#include "PhysicsLoopBack.h"
+
+
+
+//think more about naming. The b3ConnectPhysicsLoopback
+b3PhysicsClientHandle b3ConnectPhysicsLoopback(int key)
+{
+	PhysicsLoopBack* loopBack = new PhysicsLoopBack();
+	loopBack->setSharedMemoryKey(key);
+	bool connected = loopBack->connect();
+	return (b3PhysicsClientHandle  )loopBack;
+}
+

Engine/lib/bullet/examples/SharedMemory/PhysicsLoopBackC_API.h

@@ -0,0 +1,19 @@
+#ifndef PHYSICS_LOOPBACK_C_API_H
+#define PHYSICS_LOOPBACK_C_API_H
+
+#include "PhysicsClientC_API.h"
+
+#ifdef __cplusplus
+extern "C" { 
+#endif
+
+
+///think more about naming. The b3ConnectPhysicsLoopback
+b3PhysicsClientHandle b3ConnectPhysicsLoopback(int key);
+
+    
+#ifdef __cplusplus
+}
+#endif
+
+#endif //PHYSICS_LOOPBACK_C_API_H

Engine/lib/bullet/examples/SharedMemory/PhysicsServer.cpp

@@ -0,0 +1,5 @@
+#include "PhysicsServer.h"
+
+PhysicsServer::~PhysicsServer()
+{
+}

Engine/lib/bullet/examples/SharedMemory/PhysicsServer.h

@@ -0,0 +1,46 @@
+#ifndef PHYSICS_SERVER_H
+#define PHYSICS_SERVER_H
+
+#include "LinearMath/btVector3.h"
+
+
+
+class PhysicsServer
+{
+	
+
+public:
+	
+	virtual ~PhysicsServer();
+	
+	virtual void setSharedMemoryKey(int key)=0;
+	
+	virtual bool connectSharedMemory( struct GUIHelperInterface* guiHelper)=0;
+
+	virtual void disconnectSharedMemory (bool deInitializeSharedMemory)=0;
+
+	virtual void processClientCommands()=0;
+
+//	virtual bool	supportsJointMotor(class btMultiBody* body, int linkIndex)=0;
+
+	//@todo(erwincoumans) Should we have shared memory commands for picking objects?
+	///The pickBody method will try to pick the first body along a ray, return true if succeeds, false otherwise
+	virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)=0;
+	virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)=0;
+	virtual void removePickingConstraint()=0;
+
+	//for physicsDebugDraw and renderScene are mainly for debugging purposes
+	//and for physics visualization. The idea is that physicsDebugDraw can also send wireframe
+	//to a physics client, over shared memory
+	virtual void    physicsDebugDraw(int debugDrawFlags)=0;
+	virtual void    renderScene()=0;
+
+	virtual void enableCommandLogging(bool enable, const char* fileName)=0;
+	virtual void replayFromLogFile(const char* fileName)=0;
+	
+};
+
+
+#endif //PHYSICS_SERVER_H
+
+

Engine/lib/bullet/examples/SharedMemory/PhysicsServerCommandProcessor.h

@@ -0,0 +1,63 @@
+#ifndef PHYSICS_SERVER_COMMAND_PROCESSOR_H
+#define PHYSICS_SERVER_COMMAND_PROCESSOR_H
+
+#include "LinearMath/btVector3.h"
+
+struct SharedMemLines
+{
+	btVector3 m_from;
+	btVector3 m_to;
+	btVector3 m_color;
+};
+
+///todo: naming. Perhaps PhysicsSdkCommandprocessor?
+class PhysicsServerCommandProcessor
+{
+
+	struct PhysicsServerCommandProcessorInternalData* m_data;
+
+protected:
+
+
+    
+
+    bool loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody);
+
+	bool loadUrdf(const char* fileName, const class btVector3& pos, const class btQuaternion& orn,
+                             bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes);
+
+	bool	supportsJointMotor(class btMultiBody* body, int linkIndex);
+	
+	int createBodyInfoStream(int bodyUniqueId, char* bufferServerToClient, int bufferSizeInBytes);
+	void deleteCachedInverseDynamicsBodies();
+
+public:
+	PhysicsServerCommandProcessor();
+	virtual ~PhysicsServerCommandProcessor();
+
+	void	createJointMotors(class btMultiBody* body);
+	
+	virtual void createEmptyDynamicsWorld();
+	virtual void deleteDynamicsWorld();
+	
+
+	virtual bool processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes );
+
+	virtual void renderScene();
+	virtual void   physicsDebugDraw(int debugDrawFlags);
+	virtual void setGuiHelper(struct GUIHelperInterface* guiHelper);
+	
+	//@todo(erwincoumans) Should we have shared memory commands for picking objects?
+	///The pickBody method will try to pick the first body along a ray, return true if succeeds, false otherwise
+	virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld);
+	virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld);
+	virtual void removePickingConstraint();
+	
+	void enableCommandLogging(bool enable, const char* fileName);
+	void replayFromLogFile(const char* fileName);
+	void replayLogCommand(char* bufferServerToClient, int bufferSizeInBytes );
+	void stepSimulationRealTime(double dtInSec);
+	void applyJointDamping(int bodyUniqueId);
+};
+
+#endif //PHYSICS_SERVER_COMMAND_PROCESSOR_H

Engine/lib/bullet/examples/SharedMemory/PhysicsServerExample.h

@@ -0,0 +1,15 @@
+#ifndef PHYSICS_SERVER_EXAMPLE_H
+#define PHYSICS_SERVER_EXAMPLE_H
+
+enum PhysicsServerOptions
+{
+	PHYSICS_SERVER_ENABLE_COMMAND_LOGGING=1,
+	PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG=2,
+	PHYSICS_SERVER_USE_RTC_CLOCK = 4,
+};
+
+class CommonExampleInterface*    PhysicsServerCreateFunc(struct CommonExampleOptions& options);
+
+#endif //PHYSICS_SERVER_EXAMPLE_H
+
+

Engine/lib/bullet/examples/SharedMemory/PhysicsServerSharedMemory.cpp

@@ -0,0 +1,311 @@
+#include "PhysicsServerSharedMemory.h"
+#include "PosixSharedMemory.h"
+#include "Win32SharedMemory.h"
+
+#include "../CommonInterfaces/CommonRenderInterface.h"
+
+#include "btBulletDynamicsCommon.h"
+
+#include "LinearMath/btTransform.h"
+
+#include "Bullet3Common/b3Logging.h"
+#include "../CommonInterfaces/CommonGUIHelperInterface.h"
+#include "SharedMemoryBlock.h"
+
+#include "PhysicsServerCommandProcessor.h"
+
+
+
+struct PhysicsServerSharedMemoryInternalData
+{
+	
+	///end handle management
+	
+	
+	SharedMemoryInterface* m_sharedMemory;
+	bool m_ownsSharedMemory;
+
+    SharedMemoryBlock* m_testBlock1;
+	int m_sharedMemoryKey;
+	bool m_isConnected;
+	bool m_verboseOutput;
+	PhysicsServerCommandProcessor* m_commandProcessor;
+	
+	PhysicsServerSharedMemoryInternalData()
+		:m_sharedMemory(0),
+		m_ownsSharedMemory(false),
+		m_testBlock1(0),
+		m_sharedMemoryKey(SHARED_MEMORY_KEY),
+		m_isConnected(false),
+		m_verboseOutput(false),
+		m_commandProcessor(0)
+		
+	{
+    
+	}
+
+	SharedMemoryStatus& createServerStatus(int statusType, int sequenceNumber, int timeStamp)
+	{
+		SharedMemoryStatus& serverCmd =m_testBlock1->m_serverCommands[0];
+		serverCmd .m_type = statusType;
+		serverCmd.m_sequenceNumber = sequenceNumber;
+		serverCmd.m_timeStamp = timeStamp;
+		return serverCmd;
+	}
+	void submitServerStatus(SharedMemoryStatus& status)
+	{
+		m_testBlock1->m_numServerCommands++;
+	}
+
+};
+
+
+PhysicsServerSharedMemory::PhysicsServerSharedMemory(SharedMemoryInterface* sharedMem)
+{
+	m_data = new PhysicsServerSharedMemoryInternalData();
+	if (sharedMem)
+	{
+		m_data->m_sharedMemory = sharedMem;
+		m_data->m_ownsSharedMemory = false;
+	} else
+	{
+#ifdef _WIN32
+	m_data->m_sharedMemory = new Win32SharedMemoryServer();
+#else
+	m_data->m_sharedMemory = new PosixSharedMemory();
+#endif
+	m_data->m_ownsSharedMemory = true;
+	}
+
+	m_data->m_commandProcessor = new PhysicsServerCommandProcessor;
+
+
+}
+
+PhysicsServerSharedMemory::~PhysicsServerSharedMemory()
+{
+	m_data->m_commandProcessor->deleteDynamicsWorld();
+	delete m_data->m_commandProcessor;
+	delete m_data;
+}
+
+void PhysicsServerSharedMemory::resetDynamicsWorld()
+{
+	m_data->m_commandProcessor->deleteDynamicsWorld();
+	m_data->m_commandProcessor ->createEmptyDynamicsWorld();
+}
+void PhysicsServerSharedMemory::setSharedMemoryKey(int key)
+{
+	m_data->m_sharedMemoryKey = key;
+}
+
+
+bool PhysicsServerSharedMemory::connectSharedMemory( struct GUIHelperInterface* guiHelper)
+{
+	
+	m_data->m_commandProcessor->setGuiHelper(guiHelper);
+
+	
+	bool allowCreation = true;
+	
+
+    if (m_data->m_isConnected)
+    {
+        b3Warning("connectSharedMemory, while already connected");
+        return m_data->m_isConnected;
+    }
+    
+    
+	int counter = 0;
+	do 
+	{
+
+		m_data->m_testBlock1 = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE,allowCreation);
+		if (m_data->m_testBlock1)
+		{
+			int magicId =m_data->m_testBlock1->m_magicId;
+			if (m_data->m_verboseOutput)
+			{
+				b3Printf("magicId = %d\n", magicId);
+			}
+        
+			if (m_data->m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER)
+			{
+				InitSharedMemoryBlock(m_data->m_testBlock1);
+				if (m_data->m_verboseOutput)
+				{
+					b3Printf("Created and initialized shared memory block\n");
+				}
+				m_data->m_isConnected = true;
+			} else
+			{
+				m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE);
+				m_data->m_testBlock1 = 0;
+				m_data->m_isConnected = false;
+			}
+		} else
+		{
+			b3Error("Cannot connect to shared memory");
+			m_data->m_isConnected = false;
+		}
+	} while (counter++ < 10 && !m_data->m_isConnected);
+
+	if (!m_data->m_isConnected)
+	{
+		b3Error("Server cannot connect to shared memory.\n");
+	}
+	
+	return m_data->m_isConnected;
+}
+
+
+void PhysicsServerSharedMemory::disconnectSharedMemory(bool deInitializeSharedMemory)
+{
+	m_data->m_commandProcessor->setGuiHelper(0);
+
+	if (m_data->m_verboseOutput)
+	{
+		b3Printf("releaseSharedMemory1\n");
+	}
+	if (m_data->m_testBlock1)
+	{
+		if (m_data->m_verboseOutput)
+		{
+			b3Printf("m_testBlock1\n");
+		}
+		if (deInitializeSharedMemory)
+		{
+			m_data->m_testBlock1->m_magicId = 0;
+			if (m_data->m_verboseOutput)
+			{
+				b3Printf("De-initialized shared memory, magic id = %d\n",m_data->m_testBlock1->m_magicId);
+			}
+		}
+		btAssert(m_data->m_sharedMemory);
+		m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE);
+	}
+	if (m_data->m_sharedMemory)
+	{
+		if (m_data->m_verboseOutput)
+		{
+			b3Printf("m_sharedMemory\n");
+		}
+		if (m_data->m_ownsSharedMemory)
+		{
+			delete m_data->m_sharedMemory;
+		}
+		m_data->m_sharedMemory = 0;
+		m_data->m_testBlock1 = 0;
+	}
+}
+
+void PhysicsServerSharedMemory::releaseSharedMemory()
+{
+	if (m_data->m_verboseOutput)
+	{
+		b3Printf("releaseSharedMemory1\n");
+	}
+    if (m_data->m_testBlock1)
+    {
+		if (m_data->m_verboseOutput)
+		{
+			b3Printf("m_testBlock1\n");
+		}
+        m_data->m_testBlock1->m_magicId = 0;
+		if (m_data->m_verboseOutput)
+		{
+			b3Printf("magic id = %d\n",m_data->m_testBlock1->m_magicId);
+		}
+        btAssert(m_data->m_sharedMemory);
+		m_data->m_sharedMemory->releaseSharedMemory(	m_data->m_sharedMemoryKey
+, SHARED_MEMORY_SIZE);
+    }
+    if (m_data->m_sharedMemory)
+    {
+		if (m_data->m_verboseOutput)
+		{
+			b3Printf("m_sharedMemory\n");
+		}
+		if (m_data->m_ownsSharedMemory)
+		{
+	        delete m_data->m_sharedMemory;
+		}
+        m_data->m_sharedMemory = 0;
+        m_data->m_testBlock1 = 0;
+    }
+}
+
+
+void PhysicsServerSharedMemory::stepSimulationRealTime(double dtInSec)
+{
+	m_data->m_commandProcessor->stepSimulationRealTime(dtInSec);
+}
+
+
+void PhysicsServerSharedMemory::processClientCommands()
+{
+	if (m_data->m_isConnected && m_data->m_testBlock1)
+    {
+        m_data->m_commandProcessor->replayLogCommand(&m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
+        
+        ///we ignore overflow of integer for now
+        if (m_data->m_testBlock1->m_numClientCommands> m_data->m_testBlock1->m_numProcessedClientCommands)
+        {
+           
+
+            //until we implement a proper ring buffer, we assume always maximum of 1 outstanding commands
+            btAssert(m_data->m_testBlock1->m_numClientCommands==m_data->m_testBlock1->m_numProcessedClientCommands+1);
+            
+			const SharedMemoryCommand& clientCmd =m_data->m_testBlock1->m_clientCommands[0];
+
+			m_data->m_testBlock1->m_numProcessedClientCommands++;
+			//todo, timeStamp 
+			int timeStamp = 0;
+			SharedMemoryStatus& serverStatusOut = m_data->createServerStatus(CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);
+			bool hasStatus = m_data->m_commandProcessor->processCommand(clientCmd, serverStatusOut,&m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
+			if (hasStatus)
+			{
+				m_data->submitServerStatus(serverStatusOut);
+			}
+			       
+        }
+    }
+}
+
+void PhysicsServerSharedMemory::renderScene()
+{
+	m_data->m_commandProcessor->renderScene();
+
+	
+	
+}
+
+void    PhysicsServerSharedMemory::physicsDebugDraw(int debugDrawFlags)
+{
+	m_data->m_commandProcessor->physicsDebugDraw(debugDrawFlags);
+}
+
+
+bool PhysicsServerSharedMemory::pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)
+{
+	return m_data->m_commandProcessor->pickBody(rayFromWorld,rayToWorld);
+}
+
+bool PhysicsServerSharedMemory::movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)
+{
+	return m_data->m_commandProcessor->movePickedBody(rayFromWorld,rayToWorld);
+}
+void PhysicsServerSharedMemory::removePickingConstraint()
+{
+	m_data->m_commandProcessor->removePickingConstraint();
+}
+
+void PhysicsServerSharedMemory::enableCommandLogging(bool enable, const char* fileName)
+{
+	m_data->m_commandProcessor->enableCommandLogging(enable,fileName);
+}
+
+void PhysicsServerSharedMemory::replayFromLogFile(const char* fileName)
+{
+	m_data->m_commandProcessor->replayFromLogFile(fileName);
+}

Engine/lib/bullet/examples/SharedMemory/PhysicsServerSharedMemory.h

@@ -0,0 +1,55 @@
+#ifndef PHYSICS_SERVER_SHARED_MEMORY_H
+#define PHYSICS_SERVER_SHARED_MEMORY_H
+
+#include "PhysicsServer.h"
+ 
+class PhysicsServerSharedMemory : public PhysicsServer
+{
+	struct PhysicsServerSharedMemoryInternalData* m_data;
+
+protected:
+
+	
+	void	releaseSharedMemory();
+	
+	
+public:
+	PhysicsServerSharedMemory(class SharedMemoryInterface* sharedMem=0);
+	virtual ~PhysicsServerSharedMemory();
+
+	virtual void setSharedMemoryKey(int key);
+	
+	//todo: implement option to allocated shared memory from client 
+	virtual bool connectSharedMemory( struct GUIHelperInterface* guiHelper);
+
+	virtual void disconnectSharedMemory (bool deInitializeSharedMemory);
+
+	virtual void processClientCommands();
+
+	virtual void stepSimulationRealTime(double dtInSec);
+
+	//bool	supportsJointMotor(class btMultiBody* body, int linkIndex);
+
+	//@todo(erwincoumans) Should we have shared memory commands for picking objects?
+	///The pickBody method will try to pick the first body along a ray, return true if succeeds, false otherwise
+	virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld);
+	virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld);
+	virtual void removePickingConstraint();
+
+	//for physicsDebugDraw and renderScene are mainly for debugging purposes
+	//and for physics visualization. The idea is that physicsDebugDraw can also send wireframe
+	//to a physics client, over shared memory
+	void    physicsDebugDraw(int debugDrawFlags);
+	void    renderScene();
+
+	void enableCommandLogging(bool enable, const char* fileName);
+	void replayFromLogFile(const char* fileName);
+	
+	void resetDynamicsWorld();
+
+};
+
+
+#endif //PHYSICS_SERVER_EXAMPLESHARED_MEMORY_H
+
+

Engine/lib/bullet/examples/SharedMemory/PosixSharedMemory.cpp

@@ -0,0 +1,111 @@
+#include "PosixSharedMemory.h"
+#include "Bullet3Common/b3Logging.h"
+#include "LinearMath/btScalar.h" //for btAssert
+
+//Windows implementation is in Win32SharedMemory.cpp
+#ifndef _WIN32
+#define TEST_SHARED_MEMORY
+#endif//_WIN32
+
+#include <stddef.h>
+
+#ifdef TEST_SHARED_MEMORY
+
+#include <sys/shm.h>
+#include <sys/ipc.h>
+
+#endif
+
+struct PosixSharedMemoryInteralData
+{
+	bool m_createdSharedMemory;
+	int m_sharedMemoryId;
+	void* m_sharedMemoryPtr;
+	
+	PosixSharedMemoryInteralData()
+	:m_createdSharedMemory(false),
+	m_sharedMemoryId(-1),
+	m_sharedMemoryPtr(0)
+	{
+	}
+};
+
+PosixSharedMemory::PosixSharedMemory()
+{
+    
+    m_internalData = new PosixSharedMemoryInteralData;
+}
+
+PosixSharedMemory::~PosixSharedMemory()
+{
+    delete m_internalData;
+}
+
+struct btPointerCaster
+{
+    union
+    {
+        void* ptr;
+        ptrdiff_t integer;
+    };
+};
+
+void*   PosixSharedMemory::allocateSharedMemory(int key, int size,  bool allowCreation)
+{
+#ifdef TEST_SHARED_MEMORY
+    int flags = (allowCreation ? IPC_CREAT : 0) | 0666;
+    int id = shmget((key_t) key, (size_t) size,flags);
+    if (id < 0)
+    {
+        b3Error("shmget error");
+    } else
+    {
+        btPointerCaster result;
+        result.ptr = shmat(id,0,0);
+        if (result.integer == -1)
+        {
+            b3Error("shmat returned -1");
+        } else
+        {
+			m_internalData->m_createdSharedMemory = allowCreation;
+			m_internalData->m_sharedMemoryId = id;
+			m_internalData->m_sharedMemoryPtr = result.ptr;
+            return result.ptr;
+        }
+    }
+#else
+    //not implemented yet
+    btAssert(0);
+#endif
+    return 0;
+}
+void PosixSharedMemory::releaseSharedMemory(int key, int size)
+{
+#ifdef TEST_SHARED_MEMORY
+    if (m_internalData->m_sharedMemoryId < 0)
+    {
+        b3Error("PosixSharedMemory::releaseSharedMemory: shared memory id is not set");
+    } else
+    {
+		if (m_internalData->m_createdSharedMemory)
+		{
+			int result = shmctl(m_internalData->m_sharedMemoryId,IPC_RMID,0);
+			if (result == -1)
+			{
+				b3Error("PosixSharedMemory::releaseSharedMemory: shmat returned -1");
+			} else
+			{
+				b3Printf("PosixSharedMemory::releaseSharedMemory removed shared memory");
+			}
+			m_internalData->m_createdSharedMemory = false;
+			m_internalData->m_sharedMemoryId = -1;
+		}
+		if (m_internalData->m_sharedMemoryPtr)
+		{
+			shmdt(m_internalData->m_sharedMemoryPtr);
+			m_internalData->m_sharedMemoryPtr  = 0;
+			b3Printf("PosixSharedMemory::releaseSharedMemory detached shared memory\n");
+		}
+    }
+#endif
+}

Engine/lib/bullet/examples/SharedMemory/PosixSharedMemory.h

@@ -0,0 +1,21 @@
+#ifndef POSIX_SHARED_MEMORY_H
+#define POSIX_SHARED_MEMORY_H
+
+#include "SharedMemoryInterface.h"
+
+
+
+class PosixSharedMemory : public SharedMemoryInterface
+{
+
+	struct PosixSharedMemoryInteralData* m_internalData;
+    
+public:
+    PosixSharedMemory();
+    virtual ~PosixSharedMemory();
+
+    virtual void*   allocateSharedMemory(int key, int size, bool allowCreation);
+    virtual void releaseSharedMemory(int key, int size);
+};
+
+#endif //

Engine/lib/bullet/examples/SharedMemory/RobotControlExample.cpp

@@ -0,0 +1,669 @@
+
+
+
+#include "RobotControlExample.h"
+
+#if 0
+
+#include "../CommonInterfaces/CommonParameterInterface.h"
+#include "PhysicsServer.h"
+#include "PhysicsClient.h"
+#include "SharedMemoryCommon.h"
+#include "../Utils/b3Clock.h"
+#include "PhysicsClientC_API.h"
+#include "../Utils/b3ResourcePath.h"
+#include <string>
+
+//const char* blaatnaam = "basename";
+
+struct MyMotorInfo
+{
+	std::string m_jointName;
+	btScalar m_velTarget;
+	btScalar m_posTarget;
+    btScalar m_kp;
+    btScalar m_kd;
+    
+	btScalar m_maxForce;
+	int		m_uIndex;
+	int		m_posIndex;
+
+	int		m_jointIndex;
+	btScalar m_measuredJointPosition;
+	btScalar m_measuredJointVelocity;
+	btVector3 m_measuredJointForce;
+	btVector3 m_measuredJointTorque;
+	
+};
+#define MAX_NUM_MOTORS 128
+
+class RobotControlExample : public SharedMemoryCommon
+{
+	PhysicsServerSharedMemory	m_physicsServer;
+	PhysicsClientSharedMemory	m_physicsClient;
+	b3Clock	m_realtimeClock;
+
+	int	m_sequenceNumberGenerator;
+	bool m_wantsShutdown;
+	   
+	btAlignedObjectArray<SharedMemoryCommand> m_userCommandRequests;
+	
+		
+	void	createButton(const char* name, int id, bool isTrigger );
+	
+public:
+    //@todo, add accessor methods
+	MyMotorInfo m_motorTargetState[MAX_NUM_MOTORS];
+
+	int m_numMotors;
+	int m_option;
+	bool m_verboseOutput;
+
+	RobotControlExample(GUIHelperInterface* helper, int option);
+    
+	virtual ~RobotControlExample();
+    
+	virtual void	initPhysics();
+    
+	virtual void	stepSimulation(float deltaTime);
+    void prepareControlCommand(SharedMemoryCommand& cmd);
+
+    void enqueueCommand(const SharedMemoryCommand& orgCommand)
+	{
+		m_userCommandRequests.push_back(orgCommand);
+		SharedMemoryCommand& cmd = m_userCommandRequests[m_userCommandRequests.size()-1];
+		cmd.m_sequenceNumber = m_sequenceNumberGenerator++;
+		cmd.m_timeStamp = m_realtimeClock.getTimeMicroseconds();
+
+		if (m_verboseOutput)
+		{
+			b3Printf("User put command request %d on queue (queue length = %d)\n",cmd.m_type, m_userCommandRequests.size());
+		}
+	}
+  
+	virtual void resetCamera()
+	{
+		float dist = 5;
+		float pitch = 50;
+		float yaw = 35;
+		float targetPos[3]={0,0,0};//-3,2.8,-2.5};
+		m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
+	}
+    
+    virtual bool wantsTermination();
+    virtual bool isConnected();
+	virtual void	renderScene()
+	{
+		m_physicsServer.renderScene();
+	}
+	virtual void    exitPhysics(){}
+	
+	virtual void	physicsDebugDraw(int debugFlags)
+	{
+		m_physicsServer.physicsDebugDraw(debugFlags);
+		
+	}
+	virtual bool	mouseMoveCallback(float x,float y){return false;};
+	virtual bool	mouseButtonCallback(int button, int state, float x, float y){return false;}
+	virtual bool	keyboardCallback(int key, int state){return false;}
+	
+	virtual void setSharedMemoryKey(int key)
+	{
+		m_physicsServer.setSharedMemoryKey(key);
+		m_physicsClient.setSharedMemoryKey(key);
+	}
+
+};
+
+bool RobotControlExample::isConnected()
+{
+    return m_physicsClient.isConnected();
+}
+
+void MyCallback2(int buttonId, bool buttonState, void* userPtr)
+{
+	RobotControlExample* cl = (RobotControlExample*) userPtr;
+
+	SharedMemoryCommand command;
+
+	switch (buttonId)
+	{
+	case  CMD_LOAD_URDF:
+		{
+			command.m_type =CMD_LOAD_URDF;
+			command.m_updateFlags = URDF_ARGS_FILE_NAME|URDF_ARGS_INITIAL_POSITION|URDF_ARGS_INITIAL_ORIENTATION;
+            sprintf(command.m_urdfArguments.m_urdfFileName,"r2d2.urdf");//kuka_lwr/kuka.urdf");//r2d2.urdf");
+			command.m_urdfArguments.m_initialPosition[0] = 0.0;
+			command.m_urdfArguments.m_initialPosition[1] = 0.0;
+			command.m_urdfArguments.m_initialPosition[2] = 0.0;
+			command.m_urdfArguments.m_initialOrientation[0] = 0.0;
+			command.m_urdfArguments.m_initialOrientation[1] = 0.0;
+			command.m_urdfArguments.m_initialOrientation[2] = 0.0;
+			command.m_urdfArguments.m_initialOrientation[3] = 1.0;
+			command.m_urdfArguments.m_useFixedBase = false;
+			command.m_urdfArguments.m_useMultiBody = true;
+			cl->enqueueCommand(command);
+			break;
+		}
+
+		case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS:
+		{
+		    //#ifdef USE_C_API
+		    b3InitPhysicsParamCommand(&command);
+            b3PhysicsParamSetGravity(&command, 1,1,-10);
+            
+
+//		    #else
+//		    
+//			command.m_type = CMD_SEND_PHYSICS_SIMULATION_PARAMETERS;
+//			command.m_physSimParamArgs.m_gravityAcceleration[0] = 0;
+//			command.m_physSimParamArgs.m_gravityAcceleration[1] = 0;
+//			command.m_physSimParamArgs.m_gravityAcceleration[2] = -10;
+//			command.m_physSimParamArgs.m_updateFlags = SIM_PARAM_UPDATE_GRAVITY;
+//			#endif // USE_C_API
+			
+			cl->enqueueCommand(command);
+			break;
+
+		};
+		case CMD_INIT_POSE:
+		{
+			///@todo: implement this
+			command.m_type = CMD_INIT_POSE;
+			cl->enqueueCommand(command);
+			break;
+		}
+
+			
+	case CMD_CREATE_BOX_COLLISION_SHAPE:
+		{
+			command.m_type =CMD_CREATE_BOX_COLLISION_SHAPE;
+            command.m_updateFlags = BOX_SHAPE_HAS_INITIAL_POSITION;
+            command.m_createBoxShapeArguments.m_initialPosition[0] = 0;
+            command.m_createBoxShapeArguments.m_initialPosition[1] = 0;
+            command.m_createBoxShapeArguments.m_initialPosition[2] = -3;
+            
+            
+			cl->enqueueCommand(command);
+			break;
+		}
+	case CMD_REQUEST_ACTUAL_STATE:
+		{
+			command.m_type =CMD_REQUEST_ACTUAL_STATE;
+			cl->enqueueCommand(command);
+			break;
+		};
+	case CMD_STEP_FORWARD_SIMULATION:
+		{
+			command.m_type =CMD_STEP_FORWARD_SIMULATION;
+			cl->enqueueCommand(command);
+			break;
+		}
+	
+    case CMD_SEND_DESIRED_STATE:
+		{
+			
+			command.m_type =CMD_SEND_DESIRED_STATE;
+			cl->prepareControlCommand(command);
+			cl->enqueueCommand(command);		
+			break;
+		}
+	case CMD_SEND_BULLET_DATA_STREAM:
+		{
+			command.m_type = buttonId;
+			sprintf(command.m_dataStreamArguments.m_bulletFileName,"slope.bullet");
+			command.m_dataStreamArguments.m_streamChunkLength = 0;
+			cl->enqueueCommand(command);
+			break;
+		}
+
+	default:
+		{
+			b3Error("Unknown buttonId");
+			btAssert(0);
+		}
+	};
+}
+
+
+void RobotControlExample::prepareControlCommand(SharedMemoryCommand& command)
+{
+	for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
+	{
+		command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[i] = 0;
+		command.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[i] = 0;
+	}
+
+	switch (m_option)
+	{
+	case ROBOT_VELOCITY_CONTROL:
+		{
+			command.m_sendDesiredStateCommandArgument.m_controlMode = CONTROL_MODE_VELOCITY;
+			for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
+			{
+				command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[i] = 0;
+				command.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[i] = 1000;
+			}
+			for (int i=0;i<m_numMotors;i++)
+			{
+				btScalar targetVel = m_motorTargetState[i].m_velTarget;
+                            
+				int uIndex = m_motorTargetState[i].m_uIndex;
+				
+                command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[uIndex] = targetVel;
+							
+			}
+			break;
+		}
+	case ROBOT_PD_CONTROL:
+	{
+		command.m_sendDesiredStateCommandArgument.m_controlMode = CONTROL_MODE_POSITION_VELOCITY_PD;
+		for (int i=0;i<m_numMotors;i++)
+		{
+			
+			int uIndex = m_motorTargetState[i].m_uIndex;
+
+			command.m_sendDesiredStateCommandArgument.m_Kp[uIndex] = m_motorTargetState[i].m_kp;
+			command.m_sendDesiredStateCommandArgument.m_Kd[uIndex] = m_motorTargetState[i].m_kd;
+			command.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[uIndex] = 10000;//max force
+
+			btScalar targetVel = m_motorTargetState[i].m_velTarget;
+			command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[uIndex] = targetVel;
+
+			int posIndex = m_motorTargetState[i].m_posIndex;
+			btScalar targetPos = m_motorTargetState[i].m_posTarget;
+			command.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex] = targetPos;
+		}
+		break;
+	}
+	case ROBOT_PING_PONG_JOINT_FEEDBACK:
+	{
+
+		command.m_sendDesiredStateCommandArgument.m_controlMode = CONTROL_MODE_VELOCITY;
+		for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
+		{
+			command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[i] = 0;
+			command.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[i] = 1000;
+		}
+		for (int i=0;i<m_numMotors;i++)
+		{
+			btScalar targetVel = m_motorTargetState[i].m_velTarget;
+                            
+			int uIndex = m_motorTargetState[i].m_uIndex;
+            command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[uIndex] = m_motorTargetState[i].m_velTarget;
+							
+		}
+		break;
+	}
+	default:
+		{
+
+			b3Warning("Unknown control mode in RobotControlExample::prepareControlCommand");
+		}
+	};
+	
+}
+void	RobotControlExample::createButton(const char* name, int buttonId, bool isTrigger )
+{
+	ButtonParams button(name,buttonId,  isTrigger);
+	button.m_callback = MyCallback2;
+	button.m_userPointer = this;
+	m_guiHelper->getParameterInterface()->registerButtonParameter(button);
+}
+
+RobotControlExample::RobotControlExample(GUIHelperInterface* helper, int option)
+:SharedMemoryCommon(helper),
+m_wantsShutdown(false),
+m_sequenceNumberGenerator(0),
+m_numMotors(0),
+m_option(option),
+m_verboseOutput(false)
+{
+	
+	bool useServer = true;
+}
+
+
+
+RobotControlExample::~RobotControlExample()
+{
+	bool deInitializeSharedMemory = true;
+    m_physicsClient.disconnectSharedMemory();
+	m_physicsServer.disconnectSharedMemory(deInitializeSharedMemory);
+}
+
+void	RobotControlExample::initPhysics()
+{
+	///for this testing we use Z-axis up
+	int upAxis = 2;
+	m_guiHelper->setUpAxis(upAxis);
+
+  /*  createEmptyDynamicsWorld();
+	//todo: create a special debug drawer that will cache the lines, so we can send the debug info over the wire
+	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+	btVector3 grav(0,0,0);
+	grav[upAxis] = 0;//-9.8;
+	this->m_dynamicsWorld->setGravity(grav);
+    */
+	m_physicsServer.connectSharedMemory( m_guiHelper);
+  
+	if (m_guiHelper && m_guiHelper->getParameterInterface())
+	{
+		bool isTrigger = false;
+		
+		createButton("Load URDF",CMD_LOAD_URDF,  isTrigger);
+		createButton("Step Sim",CMD_STEP_FORWARD_SIMULATION,  isTrigger);
+		createButton("Send Bullet Stream",CMD_SEND_BULLET_DATA_STREAM,  isTrigger);
+		createButton("Get State",CMD_REQUEST_ACTUAL_STATE,  isTrigger);
+		createButton("Send Desired State",CMD_SEND_DESIRED_STATE,  isTrigger);
+		createButton("Create Box Collider",CMD_CREATE_BOX_COLLISION_SHAPE,isTrigger);
+		createButton("Set Physics Params",CMD_SEND_PHYSICS_SIMULATION_PARAMETERS,isTrigger);
+		createButton("Init Pose",CMD_INIT_POSE,isTrigger);
+	} else
+	{
+		/*
+		m_userCommandRequests.push_back(CMD_LOAD_URDF);
+		m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
+		m_userCommandRequests.push_back(CMD_SEND_DESIRED_STATE);
+		m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
+		//m_userCommandRequests.push_back(CMD_SET_JOINT_FEEDBACK);
+		m_userCommandRequests.push_back(CMD_CREATE_BOX_COLLISION_SHAPE);
+		//m_userCommandRequests.push_back(CMD_CREATE_RIGID_BODY);
+		m_userCommandRequests.push_back(CMD_STEP_FORWARD_SIMULATION);
+		m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
+		m_userCommandRequests.push_back(CMD_SHUTDOWN);
+		*/
+	}
+
+	if (!m_physicsClient.connect())
+	{
+		b3Warning("Cannot eonnect to physics client");
+	}
+
+}
+
+
+bool RobotControlExample::wantsTermination()
+{
+    return m_wantsShutdown;
+}
+
+
+
+void	RobotControlExample::stepSimulation(float deltaTime)
+{
+    
+	m_physicsServer.processClientCommands();
+
+	if (m_physicsClient.isConnected())
+    {
+		
+		SharedMemoryStatus status;
+		bool hasStatus = m_physicsClient.processServerStatus(status);
+		if ((m_option==ROBOT_PING_PONG_JOINT_FEEDBACK) && hasStatus && status.m_type == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
+		{
+			//update sensor feedback: joint force/torque data and measured joint positions
+			
+			for (int i=0;i<m_numMotors;i++)
+			{
+				int jointIndex = m_motorTargetState[i].m_jointIndex;
+				int positionIndex = m_motorTargetState[i].m_posIndex;
+				int velocityIndex = m_motorTargetState[i].m_uIndex;
+
+				m_motorTargetState[i].m_measuredJointPosition = status.m_sendActualStateArgs.m_actualStateQ[positionIndex];
+				m_motorTargetState[i].m_measuredJointVelocity = status.m_sendActualStateArgs.m_actualStateQdot[velocityIndex];
+				m_motorTargetState[i].m_measuredJointForce.setValue(status.m_sendActualStateArgs.m_jointReactionForces[6*jointIndex],
+																	status.m_sendActualStateArgs.m_jointReactionForces[6*jointIndex+1],
+																	status.m_sendActualStateArgs.m_jointReactionForces[6*jointIndex+2]);
+				m_motorTargetState[i].m_measuredJointTorque.setValue(status.m_sendActualStateArgs.m_jointReactionForces[6*jointIndex+3],
+																	status.m_sendActualStateArgs.m_jointReactionForces[6*jointIndex+4],
+																	status.m_sendActualStateArgs.m_jointReactionForces[6*jointIndex+5]);
+				
+				if (m_motorTargetState[i].m_measuredJointPosition>0.1)
+				{
+					m_motorTargetState[i].m_velTarget = -1.5;
+				} else
+				{
+					m_motorTargetState[i].m_velTarget = 1.5;
+				}
+				
+				b3Printf("Joint Force (Linear) [%s]=(%f,%f,%f)\n",m_motorTargetState[i].m_jointName.c_str(),m_motorTargetState[i].m_measuredJointForce.x(),m_motorTargetState[i].m_measuredJointForce.y(),m_motorTargetState[i].m_measuredJointForce.z());
+				b3Printf("Joint Torque (Angular) [%s]=(%f,%f,%f)\n",m_motorTargetState[i].m_jointName.c_str(),m_motorTargetState[i].m_measuredJointTorque.x(),m_motorTargetState[i].m_measuredJointTorque.y(),m_motorTargetState[i].m_measuredJointTorque.z());
+
+			}
+
+			
+		}
+
+		if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED)
+		{
+			SharedMemoryCommand sensorCommand;
+			sensorCommand.m_type = CMD_CREATE_SENSOR;
+			sensorCommand.m_createSensorArguments.m_numJointSensorChanges = 0;
+
+			for (int jointIndex=0;jointIndex<m_physicsClient.getNumJoints();jointIndex++)
+			{
+				b3JointInfo info;
+				m_physicsClient.getJointInfo(jointIndex,info);
+				if (m_verboseOutput)
+				{
+					b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
+				}
+				
+                if (info.m_flags & JOINT_HAS_MOTORIZED_POWER)
+                {
+                    if (m_numMotors<MAX_NUM_MOTORS)
+                    {
+
+						switch (m_option)
+						{
+						case ROBOT_VELOCITY_CONTROL:
+							{
+		                        char motorName[1024];
+								sprintf(motorName,"%s q'", info.m_jointName);
+								MyMotorInfo* motorInfo = &m_motorTargetState[m_numMotors];
+								motorInfo->m_jointName = info.m_jointName;
+
+								motorInfo->m_velTarget = 0.f;
+								motorInfo->m_posTarget = 0.f;
+
+								motorInfo->m_uIndex = info.m_uIndex;
+                    
+								SliderParams slider(motorName,&motorInfo->m_velTarget);
+								slider.m_minVal=-4;
+								slider.m_maxVal=4;
+								m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+								m_numMotors++;
+								break;
+							}
+						case ROBOT_PD_CONTROL:
+						{
+							char motorName[1024];
+							
+							MyMotorInfo* motorInfo = &m_motorTargetState[m_numMotors];
+							motorInfo->m_jointName = info.m_jointName;
+							motorInfo->m_velTarget = 0.f;
+							motorInfo->m_posTarget = 0.f;
+							motorInfo->m_uIndex = info.m_uIndex;
+							motorInfo->m_posIndex  = info.m_qIndex;
+                            motorInfo->m_kp = 1;
+                            motorInfo->m_kd = 0;
+                            
+                            {
+                                sprintf(motorName,"%s kp", info.m_jointName);
+                                SliderParams slider(motorName,&motorInfo->m_kp);
+                                slider.m_minVal=0;
+                                slider.m_maxVal=1;
+                                m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+                            }
+                            
+                            {
+                                sprintf(motorName,"%s q", info.m_jointName);
+							SliderParams slider(motorName,&motorInfo->m_posTarget);
+							slider.m_minVal=-SIMD_PI;
+							slider.m_maxVal=SIMD_PI;
+							m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+                            }
+                            {
+                                sprintf(motorName,"%s kd", info.m_jointName);
+                                SliderParams slider(motorName,&motorInfo->m_kd);
+                                slider.m_minVal=0;
+                                slider.m_maxVal=1;
+                                m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+                            }
+                           
+                            {
+                                 sprintf(motorName,"%s q'", info.m_jointName);
+                            SliderParams slider(motorName,&motorInfo->m_velTarget);
+                            slider.m_minVal=-10;
+                            slider.m_maxVal=10;
+                            m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+                            }
+							m_numMotors++;
+							break;
+						}
+						case ROBOT_PING_PONG_JOINT_FEEDBACK:
+						{
+								
+							if (info.m_flags & JOINT_HAS_MOTORIZED_POWER)
+							{
+								if (m_numMotors<MAX_NUM_MOTORS)
+								{
+									MyMotorInfo* motorInfo = &m_motorTargetState[m_numMotors];
+									motorInfo->m_jointName = info.m_jointName;
+									motorInfo->m_velTarget = 0.f;
+									motorInfo->m_posTarget = 0.f;
+									motorInfo->m_uIndex = info.m_uIndex;
+									motorInfo->m_posIndex  = info.m_qIndex;
+									motorInfo->m_jointIndex = jointIndex;
+                                    sensorCommand.m_createSensorArguments.m_sensorType[sensorCommand.m_createSensorArguments.m_numJointSensorChanges] = SENSOR_FORCE_TORQUE;
+									sensorCommand.m_createSensorArguments.m_jointIndex[sensorCommand.m_createSensorArguments.m_numJointSensorChanges] = jointIndex;
+									sensorCommand.m_createSensorArguments.m_enableJointForceSensor[sensorCommand.m_createSensorArguments.m_numJointSensorChanges] = true;
+									sensorCommand.m_createSensorArguments.m_numJointSensorChanges++;
+									m_numMotors++;
+								}
+							}
+						
+							
+
+							 break;
+						}
+						default:
+							{
+								b3Warning("Unknown control mode in RobotControlExample::stepSimulation");
+							}
+						};
+                    }
+                }
+				
+			}
+			
+
+			if (sensorCommand.m_createSensorArguments.m_numJointSensorChanges)
+			{
+				enqueueCommand(sensorCommand);
+			}
+
+		}
+
+		
+		
+		if (m_physicsClient.canSubmitCommand())
+		{
+			if (m_userCommandRequests.size())
+			{
+				if (m_verboseOutput)
+				{
+					b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size());
+				}
+				SharedMemoryCommand cmd = m_userCommandRequests[0];
+
+				//a manual 'pop_front', we don't use 'remove' because it will re-order the commands
+				for (int i=1;i<m_userCommandRequests.size();i++)
+				{
+					m_userCommandRequests[i-1] = m_userCommandRequests[i];
+				}
+
+				m_userCommandRequests.pop_back();
+				if (cmd.m_type == CMD_CREATE_SENSOR)
+				{
+					b3Printf("CMD_CREATE_SENSOR!\n");
+				}
+				if (cmd.m_type == CMD_SEND_BULLET_DATA_STREAM)
+				{
+					char relativeFileName[1024];
+					
+					bool fileFound = b3ResourcePath::findResourcePath(cmd.m_dataStreamArguments.m_bulletFileName,relativeFileName,1024);
+					if (fileFound)
+					{
+						FILE *fp = fopen(relativeFileName, "rb");
+						if (fp)
+						{
+							fseek(fp, 0L, SEEK_END);
+							int mFileLen = ftell(fp);
+							fseek(fp, 0L, SEEK_SET);
+							if (mFileLen<SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE)
+							{
+								char* data = (char*)malloc(mFileLen);
+
+								fread(data, mFileLen, 1, fp);
+								fclose(fp);
+								cmd.m_dataStreamArguments.m_streamChunkLength = mFileLen;
+								m_physicsClient.uploadBulletFileToSharedMemory(data,mFileLen);
+								if (m_verboseOutput)
+								{
+									b3Printf("Loaded bullet data chunks into shared memory\n");
+								}
+								free(data);
+							} else
+							{
+								b3Warning("Bullet file size (%d) exceeds of streaming memory chunk size (%d)\n", mFileLen,SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
+							}
+						} else
+						{
+							b3Warning("Cannot open file %s\n", relativeFileName);
+						}
+					} else
+					{
+						b3Warning("Cannot find file %s\n", cmd.m_dataStreamArguments.m_bulletFileName);
+					}
+
+				}
+				
+				m_physicsClient.submitClientCommand(cmd);
+			} else
+			{
+
+				if (m_numMotors)
+				{
+					SharedMemoryCommand command;
+					command.m_type =CMD_SEND_DESIRED_STATE;
+					prepareControlCommand(command);
+					enqueueCommand(command);		
+
+					command.m_type =CMD_STEP_FORWARD_SIMULATION;
+					enqueueCommand(command);
+
+					command.m_type = CMD_REQUEST_ACTUAL_STATE;
+					enqueueCommand(command);
+				}
+
+			}
+		}
+	}
+}
+
+extern int gSharedMemoryKey;
+
+class CommonExampleInterface*    RobotControlExampleCreateFunc(struct CommonExampleOptions& options)
+{
+	RobotControlExample* example = new RobotControlExample(options.m_guiHelper, options.m_option);
+	if (gSharedMemoryKey>=0)
+	{
+		example->setSharedMemoryKey(gSharedMemoryKey);
+	}
+	return example;
+}
+#endif
+
+

Engine/lib/bullet/examples/SharedMemory/RobotControlExample.h

@@ -0,0 +1,15 @@
+#ifndef ROBOT_CONTROL_EXAMPLE_H
+#define ROBOT_CONTROL_EXAMPLE_H
+
+enum EnumRobotControls
+{
+	ROBOT_VELOCITY_CONTROL=0,
+	ROBOT_PD_CONTROL,
+	ROBOT_PING_PONG_JOINT_FEEDBACK,
+};
+
+class CommonExampleInterface*    RobotControlExampleCreateFunc(struct CommonExampleOptions& options);
+
+#endif //ROBOT_CONTROL_EXAMPLE_H
+
+

Engine/lib/bullet/examples/SharedMemory/SharedMemoryBlock.h

@@ -0,0 +1,54 @@
+#ifndef SHARED_MEMORY_BLOCK_H
+#define SHARED_MEMORY_BLOCK_H
+
+#define SHARED_MEMORY_MAGIC_NUMBER 64738
+#define SHARED_MEMORY_MAX_COMMANDS 4
+
+#include "SharedMemoryCommands.h"
+
+struct SharedMemoryBlock
+{
+	int m_magicId;
+	struct SharedMemoryCommand m_clientCommands[SHARED_MEMORY_MAX_COMMANDS];
+	struct SharedMemoryStatus m_serverCommands[SHARED_MEMORY_MAX_COMMANDS];
+
+	int m_numClientCommands;
+	int m_numProcessedClientCommands;
+
+	int m_numServerCommands;
+	int m_numProcessedServerCommands;
+
+	//m_bulletStreamDataClientToServer is a way for the client to create collision shapes, rigid bodies and constraints
+	//the Bullet data structures are more general purpose than the capabilities of a URDF file.
+	char    m_bulletStreamDataClientToServer[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE];
+
+	//m_bulletStreamDataServerToClient is used to send (debug) data from server to client, for
+	//example to provide all details of a multibody including joint/link names, after loading a URDF file.
+	char    m_bulletStreamDataServerToClientRefactor[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE];
+};
+
+
+
+
+//http://stackoverflow.com/questions/24736304/unable-to-use-inline-in-declaration-get-error-c2054
+#ifdef _WIN32
+__inline
+#else
+inline
+#endif
+void     InitSharedMemoryBlock(struct SharedMemoryBlock* sharedMemoryBlock)
+{
+    sharedMemoryBlock->m_numClientCommands = 0;
+    sharedMemoryBlock->m_numServerCommands = 0;
+    sharedMemoryBlock->m_numProcessedClientCommands=0;
+    sharedMemoryBlock->m_numProcessedServerCommands=0;
+    sharedMemoryBlock->m_magicId = SHARED_MEMORY_MAGIC_NUMBER;
+}
+
+#define SHARED_MEMORY_SIZE sizeof(SharedMemoryBlock)
+
+
+
+
+#endif //SHARED_MEMORY_BLOCK_H
+

Engine/lib/bullet/examples/SharedMemory/SharedMemoryCommands.h

@@ -0,0 +1,506 @@
+
+#ifndef SHARED_MEMORY_COMMANDS_H
+#define SHARED_MEMORY_COMMANDS_H
+
+//this is a very experimental draft of commands. We will iterate on this API (commands, arguments etc)
+
+#include "SharedMemoryPublic.h"
+
+#ifdef __GNUC__
+	#include <stdint.h>
+	typedef int32_t smInt32_t;
+	typedef int64_t smInt64_t;
+	typedef uint32_t smUint32_t;
+	typedef uint64_t smUint64_t;
+#elif defined(_MSC_VER)
+	typedef __int32 smInt32_t;
+	typedef __int64 smInt64_t;
+	typedef unsigned __int32 smUint32_t;
+	typedef unsigned __int64 smUint64_t;
+#else
+	typedef int smInt32_t;
+	typedef long long int smInt64_t;
+	typedef unsigned int smUint32_t;
+	typedef unsigned long long int smUint64_t;
+#endif
+
+#define SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE (256*1024)
+
+#define SHARED_MEMORY_SERVER_TEST_C
+#define MAX_DEGREE_OF_FREEDOM 128
+#define MAX_NUM_SENSORS 256
+#define MAX_URDF_FILENAME_LENGTH 1024
+#define MAX_SDF_FILENAME_LENGTH 1024
+#define MAX_FILENAME_LENGTH MAX_URDF_FILENAME_LENGTH
+#define MAX_NUM_LINKS MAX_DEGREE_OF_FREEDOM
+#define MAX_SDF_BODIES 500
+
+struct TmpFloat3 
+{
+    float m_x;
+    float m_y;
+    float m_z;
+};
+
+#ifdef _WIN32
+__inline
+#else
+inline
+#endif
+TmpFloat3 CreateTmpFloat3(float x, float y, float z) 
+{
+    TmpFloat3 tmp;
+    tmp.m_x = x;
+    tmp.m_y = y;
+    tmp.m_z = z;
+    return tmp;
+}
+
+enum EnumSdfArgsUpdateFlags
+{
+	SDF_ARGS_FILE_NAME=1,
+};
+
+struct SdfArgs
+{
+	char m_sdfFileName[MAX_URDF_FILENAME_LENGTH];
+    int m_useMultiBody;
+};
+
+enum EnumUrdfArgsUpdateFlags
+{
+	URDF_ARGS_FILE_NAME=1,
+	URDF_ARGS_INITIAL_POSITION=2,
+	URDF_ARGS_INITIAL_ORIENTATION=4,
+	URDF_ARGS_USE_MULTIBODY=8,
+	URDF_ARGS_USE_FIXED_BASE=16,
+};
+
+
+struct UrdfArgs
+{
+	char m_urdfFileName[MAX_URDF_FILENAME_LENGTH];
+	double m_initialPosition[3];
+	double m_initialOrientation[4];
+	int m_useMultiBody;
+	int m_useFixedBase;
+};
+
+
+struct BulletDataStreamArgs
+{
+	char m_bulletFileName[MAX_FILENAME_LENGTH];
+	int m_streamChunkLength;
+	int m_bodyUniqueId;
+};
+
+struct SetJointFeedbackArgs
+{
+	int m_bodyUniqueId;
+	int m_linkId;
+	int m_isEnabled;
+};
+
+enum EnumInitPoseFlags
+{
+    INIT_POSE_HAS_INITIAL_POSITION=1,
+    INIT_POSE_HAS_INITIAL_ORIENTATION=2,
+    INIT_POSE_HAS_JOINT_STATE=4
+};
+
+
+///InitPoseArgs is mainly to initialize (teleport) the robot in a particular position
+///No motors or controls are needed to initialize the pose. It is similar to
+///moving a robot to a starting place, while it is switched off. It is only called
+///at the start of a robot control session. All velocities and control forces are cleared to zero.
+struct InitPoseArgs
+{
+	int m_bodyUniqueId;
+	int m_hasInitialStateQ[MAX_DEGREE_OF_FREEDOM];
+	double m_initialStateQ[MAX_DEGREE_OF_FREEDOM];
+};
+
+
+struct RequestDebugLinesArgs
+{
+	int m_debugMode;
+    int m_startingLineIndex;
+};
+
+struct RequestPixelDataArgs
+{
+	float m_viewMatrix[16];
+	float m_projectionMatrix[16];
+	int m_startPixelIndex;
+	int m_pixelWidth;
+	int m_pixelHeight;
+};
+
+enum EnumRequestPixelDataUpdateFlags
+{
+	REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES=1,
+	REQUEST_PIXEL_ARGS_SET_PIXEL_WIDTH_HEIGHT=4,
+	//don't exceed (1<<15), because this enum is shared with EnumRenderer in SharedMemoryPublic.h
+	
+};
+
+struct RequestContactDataArgs
+{
+    int m_startingContactPointIndex;
+    int m_objectAIndexFilter;
+	int m_objectBIndexFilter;
+};
+
+
+struct SendDebugLinesArgs
+{
+    int m_startingLineIndex;
+	int m_numDebugLines;
+    int m_numRemainingDebugLines;
+};
+
+struct SendPixelDataArgs
+{
+	int m_imageWidth;
+	int m_imageHeight;
+
+    int m_startingPixelIndex;
+    int m_numPixelsCopied;
+    int m_numRemainingPixels;
+};
+
+struct PickBodyArgs
+{
+    double m_rayFromWorld[3];
+    double m_rayToWorld[3];
+};
+
+
+///Controlling a robot involves sending the desired state to its joint motor controllers.
+///The control mode determines the state variables used for motor control.
+struct SendDesiredStateArgs
+{
+	int m_bodyUniqueId;
+	int m_controlMode;
+
+	//PD parameters in case m_controlMode == CONTROL_MODE_POSITION_VELOCITY_PD
+	double m_Kp[MAX_DEGREE_OF_FREEDOM];//indexed by degree of freedom, 6 for base, and then the dofs for each link
+	double m_Kd[MAX_DEGREE_OF_FREEDOM];//indexed by degree of freedom, 6 for base, and then the dofs for each link
+
+    int m_hasDesiredStateFlags[MAX_DEGREE_OF_FREEDOM];
+    
+	//desired state is only written by the client, read-only access by server is expected
+
+	//m_desiredStateQ is indexed by position variables, 
+	//starting with 3 base position variables, 4 base orientation variables (quaternion), then link position variables
+    double m_desiredStateQ[MAX_DEGREE_OF_FREEDOM];
+    
+
+	//m_desiredStateQdot is index by velocity degrees of freedom, 3 linear and 3 angular variables for the base and then link velocity variables
+    double m_desiredStateQdot[MAX_DEGREE_OF_FREEDOM];
+	
+	//m_desiredStateForceTorque is either the actual applied force/torque (in CONTROL_MODE_TORQUE) or
+	//or the maximum applied force/torque for the PD/motor/constraint to reach the desired velocity in CONTROL_MODE_VELOCITY and CONTROL_MODE_POSITION_VELOCITY_PD mode
+	//indexed by degree of freedom, 6 dof base, and then dofs for each link
+    double m_desiredStateForceTorque[MAX_DEGREE_OF_FREEDOM];
+    
+};
+
+enum EnumSimDesiredStateUpdateFlags
+{
+	SIM_DESIRED_STATE_HAS_Q=1,
+	SIM_DESIRED_STATE_HAS_QDOT=2,
+	SIM_DESIRED_STATE_HAS_KD=4,
+	SIM_DESIRED_STATE_HAS_KP=8,
+	SIM_DESIRED_STATE_HAS_MAX_FORCE=16,
+};
+
+
+enum EnumSimParamUpdateFlags
+{
+	SIM_PARAM_UPDATE_DELTA_TIME=1,
+	SIM_PARAM_UPDATE_GRAVITY=2,
+	SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS=4,	
+	SIM_PARAM_UPDATE_NUM_SIMULATION_SUB_STEPS=8,
+	SIM_PARAM_UPDATE_REAL_TIME_SIMULATION = 16,
+	SIM_PARAM_UPDATE_DEFAULT_CONTACT_ERP=32
+};
+
+///Controlling a robot involves sending the desired state to its joint motor controllers.
+///The control mode determines the state variables used for motor control.
+struct SendPhysicsSimulationParameters
+{
+	double m_deltaTime;
+	double m_gravityAcceleration[3];
+	int m_numSimulationSubSteps;
+	int m_numSolverIterations;
+	bool m_allowRealTimeSimulation;
+	double m_defaultContactERP;
+};
+
+struct RequestActualStateArgs
+{
+	int m_bodyUniqueId;
+};
+
+
+
+
+struct SendActualStateArgs
+{
+	int m_bodyUniqueId;
+	int m_numDegreeOfFreedomQ;
+	int m_numDegreeOfFreedomU;
+
+    double m_rootLocalInertialFrame[7];
+    
+	  //actual state is only written by the server, read-only access by client is expected
+    double m_actualStateQ[MAX_DEGREE_OF_FREEDOM];
+    double m_actualStateQdot[MAX_DEGREE_OF_FREEDOM];
+
+    //measured 6DOF force/torque sensors: force[x,y,z] and torque[x,y,z]
+    double m_jointReactionForces[6*MAX_DEGREE_OF_FREEDOM];
+
+    double m_jointMotorForce[MAX_DEGREE_OF_FREEDOM];
+    
+    double m_linkState[7*MAX_NUM_LINKS];
+    double m_linkLocalInertialFrames[7*MAX_NUM_LINKS];
+};
+
+enum EnumSensorTypes
+{
+    SENSOR_FORCE_TORQUE=1,
+    SENSOR_IMU=2,
+};
+
+struct CreateSensorArgs
+{
+    int m_bodyUniqueId;
+    int m_numJointSensorChanges;
+    int m_sensorType[MAX_DEGREE_OF_FREEDOM];
+    
+///todo: clean up the duplication, make sure no-one else is using those members directly (use C-API header instead)
+    int m_jointIndex[MAX_DEGREE_OF_FREEDOM];
+    int m_enableJointForceSensor[MAX_DEGREE_OF_FREEDOM];
+
+    int m_linkIndex[MAX_DEGREE_OF_FREEDOM];
+    int m_enableSensor[MAX_DEGREE_OF_FREEDOM];
+    
+};
+
+typedef  struct SharedMemoryCommand SharedMemoryCommand_t;
+
+enum EnumBoxShapeFlags
+{
+    BOX_SHAPE_HAS_INITIAL_POSITION=1,
+    BOX_SHAPE_HAS_INITIAL_ORIENTATION=2,
+    BOX_SHAPE_HAS_HALF_EXTENTS=4,
+	BOX_SHAPE_HAS_MASS=8,
+	BOX_SHAPE_HAS_COLLISION_SHAPE_TYPE=16,
+	BOX_SHAPE_HAS_COLOR=32,
+};
+///This command will be replaced to allow arbitrary collision shape types
+struct CreateBoxShapeArgs
+{
+    double m_halfExtentsX;
+    double m_halfExtentsY;
+    double m_halfExtentsZ;
+
+	double m_mass;
+	int m_collisionShapeType;//see SharedMemoryPublic.h
+
+    double m_initialPosition[3];
+	double m_initialOrientation[4];
+	double m_colorRGBA[4];
+};
+
+struct SdfLoadedArgs
+{
+    int m_numBodies;
+    int m_bodyUniqueIds[MAX_SDF_BODIES];
+    
+    ///@todo(erwincoumans) load cameras, lights etc
+    //int m_numCameras; 
+    //int m_numLights; 
+};
+
+
+struct SdfRequestInfoArgs
+{
+    int m_bodyUniqueId;
+};
+
+///flags for b3ApplyExternalTorque and b3ApplyExternalForce
+enum EnumExternalForcePrivateFlags
+{
+//    EF_LINK_FRAME=1,
+//    EF_WORLD_FRAME=2,
+    EF_TORQUE=4,
+    EF_FORCE=8,
+};
+
+
+
+struct ExternalForceArgs
+{
+    int m_numForcesAndTorques;
+    int m_bodyUniqueIds[MAX_SDF_BODIES];
+    int m_linkIds[MAX_SDF_BODIES];
+    double m_forcesAndTorques[3*MAX_SDF_BODIES];
+    double m_positions[3*MAX_SDF_BODIES];
+    int m_forceFlags[MAX_SDF_BODIES];
+};
+
+enum EnumSdfRequestInfoFlags
+{
+    SDF_REQUEST_INFO_BODY=1,
+    //SDF_REQUEST_INFO_CAMERA=2,
+};
+
+
+struct CalculateInverseDynamicsArgs
+{
+	int m_bodyUniqueId;
+
+	double m_jointPositionsQ[MAX_DEGREE_OF_FREEDOM];
+	double m_jointVelocitiesQdot[MAX_DEGREE_OF_FREEDOM];
+	double m_jointAccelerations[MAX_DEGREE_OF_FREEDOM];
+};
+
+struct CalculateInverseDynamicsResultArgs
+{
+	int m_bodyUniqueId;
+	int m_dofCount;
+	double m_jointForces[MAX_DEGREE_OF_FREEDOM];
+};
+
+struct CalculateJacobianArgs
+{
+    int m_bodyUniqueId;
+    int m_linkIndex;
+    double m_localPosition[3];
+    double m_jointPositionsQ[MAX_DEGREE_OF_FREEDOM];
+    double m_jointVelocitiesQdot[MAX_DEGREE_OF_FREEDOM];
+    double m_jointAccelerations[MAX_DEGREE_OF_FREEDOM];
+};
+
+struct CalculateJacobianResultArgs
+{
+    int m_dofCount;
+    double m_linearJacobian[3*MAX_DEGREE_OF_FREEDOM];
+    double m_angularJacobian[3*MAX_DEGREE_OF_FREEDOM];
+};
+
+enum EnumCalculateInverseKinematicsFlags
+{
+    IK_HAS_TARGET_POSITION=1,
+	IK_HAS_TARGET_ORIENTATION=2,
+    IK_HAS_NULL_SPACE_VELOCITY=4,
+    //IK_HAS_CURRENT_JOINT_POSITIONS=8,//not used yet
+};
+
+struct CalculateInverseKinematicsArgs
+{
+	int m_bodyUniqueId;
+//	double m_jointPositionsQ[MAX_DEGREE_OF_FREEDOM];
+	double m_targetPosition[3];
+	double m_targetOrientation[4];//orientation represented as quaternion, x,y,z,w
+	int m_endEffectorLinkIndex;
+    double m_lowerLimit[MAX_DEGREE_OF_FREEDOM];
+    double m_upperLimit[MAX_DEGREE_OF_FREEDOM];
+    double m_jointRange[MAX_DEGREE_OF_FREEDOM];
+    double m_restPose[MAX_DEGREE_OF_FREEDOM];
+};
+
+struct CalculateInverseKinematicsResultArgs
+{
+	int m_bodyUniqueId;
+	int m_dofCount;
+	double m_jointPositions[MAX_DEGREE_OF_FREEDOM];
+};
+
+struct CreateJointArgs
+{
+    int m_parentBodyIndex;
+    int m_parentJointIndex;
+    int m_childBodyIndex;
+    int m_childJointIndex;
+    double m_parentFrame[7];
+    double m_childFrame[7];
+    double m_jointAxis[3];
+    int m_jointType;
+};
+
+struct SharedMemoryCommand
+{
+	int m_type;
+	smUint64_t	m_timeStamp;
+	int	m_sequenceNumber;
+	
+	//m_updateFlags is a bit fields to tell which parameters need updating
+    //for example m_updateFlags = SIM_PARAM_UPDATE_DELTA_TIME | SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS;
+    int m_updateFlags;
+
+    union
+    {
+        struct UrdfArgs m_urdfArguments;
+		struct SdfArgs m_sdfArguments;
+		struct SdfRequestInfoArgs m_sdfRequestInfoArgs;
+		struct InitPoseArgs m_initPoseArgs;
+		struct SendPhysicsSimulationParameters m_physSimParamArgs;
+		struct BulletDataStreamArgs	m_dataStreamArguments;
+		struct SendDesiredStateArgs m_sendDesiredStateCommandArgument;
+		struct RequestActualStateArgs m_requestActualStateInformationCommandArgument;
+        struct CreateSensorArgs m_createSensorArguments;
+        struct CreateBoxShapeArgs m_createBoxShapeArguments;
+		struct RequestDebugLinesArgs m_requestDebugLinesArguments;
+		struct RequestPixelDataArgs m_requestPixelDataArguments;
+		struct PickBodyArgs m_pickBodyArguments;
+        struct ExternalForceArgs m_externalForceArguments;
+		struct CalculateInverseDynamicsArgs m_calculateInverseDynamicsArguments;
+        struct CalculateJacobianArgs m_calculateJacobianArguments;
+        struct CreateJointArgs m_createJointArguments;
+        struct RequestContactDataArgs m_requestContactPointArguments;
+		struct CalculateInverseKinematicsArgs m_calculateInverseKinematicsArguments;
+    };
+};
+
+struct RigidBodyCreateArgs
+{
+	int m_bodyUniqueId; 
+};
+
+struct SendContactDataArgs
+{
+    int m_startingContactPointIndex;
+    int m_numContactPointsCopied;
+    int m_numRemainingContactPoints;
+};
+
+struct SharedMemoryStatus
+{
+	int m_type;
+	
+	smUint64_t	m_timeStamp;
+	int	m_sequenceNumber;
+	
+	union
+	{
+		struct BulletDataStreamArgs	m_dataStreamArguments;
+		struct SdfLoadedArgs m_sdfLoadedArgs;
+		struct SendActualStateArgs m_sendActualStateArgs;
+		struct SendDebugLinesArgs m_sendDebugLinesArgs;
+		struct SendPixelDataArgs m_sendPixelDataArguments;
+		struct RigidBodyCreateArgs m_rigidBodyCreateArgs;
+		struct CalculateInverseDynamicsResultArgs m_inverseDynamicsResultArgs;
+        struct CalculateJacobianResultArgs m_jacobianResultArgs;
+		struct SendContactDataArgs m_sendContactPointArgs;
+		struct CalculateInverseKinematicsResultArgs m_inverseKinematicsResultArgs;
+	};
+};
+
+typedef  struct SharedMemoryStatus SharedMemoryStatus_t;
+
+
+
+#endif //SHARED_MEMORY_COMMANDS_H

Engine/lib/bullet/examples/SharedMemory/SharedMemoryCommon.h

@@ -0,0 +1,23 @@
+#ifndef SHARED_MEMORY_COMMON_H
+#define SHARED_MEMORY_COMMON_H
+
+#include "../CommonInterfaces/CommonMultiBodyBase.h"
+
+class SharedMemoryCommon : public CommonExampleInterface
+{
+protected:
+	
+	struct GUIHelperInterface* m_guiHelper;
+	
+public:
+	SharedMemoryCommon(GUIHelperInterface* helper)
+	:m_guiHelper(helper)
+	{
+	}
+
+	virtual void setSharedMemoryKey(int key)=0;
+	virtual bool wantsTermination()=0;
+    virtual bool isConnected()=0;
+};
+#endif//
+

Engine/lib/bullet/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp

@@ -0,0 +1,105 @@
+
+#include "SharedMemoryInProcessPhysicsC_API.h"
+
+#include "PhysicsClientSharedMemory.h"
+#include"../ExampleBrowser/InProcessExampleBrowser.h"
+
+
+class InProcessPhysicsClientSharedMemoryMainThread : public PhysicsClientSharedMemory
+{
+    btInProcessExampleBrowserMainThreadInternalData* m_data;
+    
+public:
+    
+    InProcessPhysicsClientSharedMemoryMainThread(int argc, char* argv[])
+    {
+        int newargc = argc+2;
+        char** newargv = (char**)malloc(sizeof(void*)*newargc);
+        for (int i=0;i<argc;i++)
+            newargv[i] = argv[i];
+        
+        char* t0 = (char*)"--logtostderr";
+        char* t1 = (char*)"--start_demo_name=Physics Server";
+        newargv[argc] = t0;
+        newargv[argc+1] = t1;
+        m_data = btCreateInProcessExampleBrowserMainThread(newargc,newargv);
+        SharedMemoryInterface* shMem = btGetSharedMemoryInterfaceMainThread(m_data);
+        
+        setSharedMemoryInterface(shMem);
+    }
+    
+    virtual ~InProcessPhysicsClientSharedMemoryMainThread()
+    {
+        setSharedMemoryInterface(0);
+        btShutDownExampleBrowserMainThread(m_data);
+    }
+    
+    // return non-null if there is a status, nullptr otherwise
+    virtual const struct SharedMemoryStatus* processServerStatus()
+    {
+        if (btIsExampleBrowserMainThreadTerminated(m_data))
+        {
+            PhysicsClientSharedMemory::disconnectSharedMemory();
+        }
+        
+        btUpdateInProcessExampleBrowserMainThread(m_data);
+        return PhysicsClientSharedMemory::processServerStatus();
+        
+        
+    }
+    
+    virtual bool submitClientCommand(const struct SharedMemoryCommand& command)
+    {
+//        btUpdateInProcessExampleBrowserMainThread(m_data);
+        return PhysicsClientSharedMemory::submitClientCommand(command);
+    }
+    
+};
+
+b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThread(int argc, char* argv[])
+{
+    InProcessPhysicsClientSharedMemoryMainThread* cl = new InProcessPhysicsClientSharedMemoryMainThread(argc, argv);
+    cl->setSharedMemoryKey(SHARED_MEMORY_KEY);
+    cl->connect();
+    return (b3PhysicsClientHandle ) cl;
+}
+
+class InProcessPhysicsClientSharedMemory : public PhysicsClientSharedMemory
+{
+	btInProcessExampleBrowserInternalData* m_data;
+public:
+
+	InProcessPhysicsClientSharedMemory(int argc, char* argv[])
+	{
+		int newargc = argc+2;
+		char** newargv = (char**)malloc(sizeof(void*)*newargc);
+		for (int i=0;i<argc;i++)
+		newargv[i] = argv[i];
+
+		char* t0 = (char*)"--logtostderr";
+		char* t1 = (char*)"--start_demo_name=Physics Server";
+		newargv[argc] = t0;
+		newargv[argc+1] = t1;
+		m_data = btCreateInProcessExampleBrowser(newargc,newargv);
+		SharedMemoryInterface* shMem = btGetSharedMemoryInterface(m_data);
+
+		setSharedMemoryInterface(shMem);
+	}
+
+	virtual ~InProcessPhysicsClientSharedMemory()
+	{
+		setSharedMemoryInterface(0);
+		btShutDownExampleBrowser(m_data);
+	}
+
+};
+
+b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnect(int argc, char* argv[])
+{
+	
+	InProcessPhysicsClientSharedMemory* cl = new InProcessPhysicsClientSharedMemory(argc, argv);
+    cl->setSharedMemoryKey(SHARED_MEMORY_KEY);
+    cl->connect();
+	return (b3PhysicsClientHandle ) cl;
+}
+

Engine/lib/bullet/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.h

@@ -0,0 +1,24 @@
+
+#ifndef IN_PROCESS_PHYSICS_C_API_H
+#define IN_PROCESS_PHYSICS_C_API_H
+
+#include "PhysicsClientC_API.h"
+
+#ifdef __cplusplus
+extern "C" { 
+#endif
+
+
+///think more about naming. The b3ConnectPhysicsLoopback
+b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnect(int argc, char* argv[]);
+
+b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThread(int argc, char* argv[]);
+
+    
+    
+    
+#ifdef __cplusplus
+}
+#endif
+
+#endif //IN_PROCESS_PHYSICS_C_API_H

Engine/lib/bullet/examples/SharedMemory/SharedMemoryInterface.h

@@ -0,0 +1,16 @@
+#ifndef SHARED_MEMORY_INTERFACE_H
+#define SHARED_MEMORY_INTERFACE_H
+
+class SharedMemoryInterface
+{
+	public:
+	virtual ~SharedMemoryInterface()
+	{
+	}
+	
+	virtual void*	allocateSharedMemory(int key, int size, bool allowCreation) =0;
+	virtual void releaseSharedMemory(int key, int size) =0;
+};
+
+#endif
+

Engine/lib/bullet/examples/SharedMemory/SharedMemoryPublic.h

@@ -0,0 +1,227 @@
+#ifndef SHARED_MEMORY_PUBLIC_H
+#define SHARED_MEMORY_PUBLIC_H
+
+#define SHARED_MEMORY_KEY 12347
+
+enum EnumSharedMemoryClientCommand
+{
+    CMD_LOAD_SDF,
+	CMD_LOAD_URDF,
+        CMD_SEND_BULLET_DATA_STREAM,
+        CMD_CREATE_BOX_COLLISION_SHAPE,
+//      CMD_DELETE_BOX_COLLISION_SHAPE,
+      CMD_CREATE_RIGID_BODY,
+      CMD_DELETE_RIGID_BODY,
+    CMD_CREATE_SENSOR,///enable or disable joint feedback for force/torque sensors
+//    CMD_REQUEST_SENSOR_MEASUREMENTS,//see CMD_REQUEST_ACTUAL_STATE/CMD_ACTUAL_STATE_UPDATE_COMPLETED
+        CMD_INIT_POSE,
+        CMD_SEND_PHYSICS_SIMULATION_PARAMETERS,
+        CMD_SEND_DESIRED_STATE,//todo: reconsider naming, for example SET_JOINT_CONTROL_VARIABLE?
+        CMD_REQUEST_ACTUAL_STATE,
+        CMD_REQUEST_DEBUG_LINES,
+    CMD_REQUEST_BODY_INFO,
+    CMD_STEP_FORWARD_SIMULATION,
+    CMD_RESET_SIMULATION,
+    CMD_PICK_BODY,
+    CMD_MOVE_PICKED_BODY,
+    CMD_REMOVE_PICKING_CONSTRAINT_BODY,
+    CMD_REQUEST_CAMERA_IMAGE_DATA,
+    CMD_APPLY_EXTERNAL_FORCE,
+	CMD_CALCULATE_INVERSE_DYNAMICS,
+    CMD_CALCULATE_INVERSE_KINEMATICS,
+    CMD_CALCULATE_JACOBIAN,
+    CMD_CREATE_JOINT,
+    CMD_REQUEST_CONTACT_POINT_INFORMATION,
+	CMD_SAVE_WORLD,
+    //don't go beyond this command!
+    CMD_MAX_CLIENT_COMMANDS,
+    
+};
+
+enum EnumSharedMemoryServerStatus
+{
+        CMD_SHARED_MEMORY_NOT_INITIALIZED=0,
+        CMD_WAITING_FOR_CLIENT_COMMAND,
+        //CMD_CLIENT_COMMAND_COMPLETED is a generic 'completed' status that doesn't need special handling on the client
+        CMD_CLIENT_COMMAND_COMPLETED,
+        //the server will skip unknown command and report a status 'CMD_UNKNOWN_COMMAND_FLUSHED'
+        CMD_UNKNOWN_COMMAND_FLUSHED,
+		CMD_SDF_LOADING_COMPLETED,
+        CMD_SDF_LOADING_FAILED,
+        CMD_URDF_LOADING_COMPLETED,
+        CMD_URDF_LOADING_FAILED,
+        CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED,
+        CMD_BULLET_DATA_STREAM_RECEIVED_FAILED,
+        CMD_BOX_COLLISION_SHAPE_CREATION_COMPLETED,
+        CMD_RIGID_BODY_CREATION_COMPLETED,
+        CMD_SET_JOINT_FEEDBACK_COMPLETED,
+        CMD_ACTUAL_STATE_UPDATE_COMPLETED,
+        CMD_ACTUAL_STATE_UPDATE_FAILED,
+        CMD_DEBUG_LINES_COMPLETED,
+        CMD_DEBUG_LINES_OVERFLOW_FAILED,
+        CMD_DESIRED_STATE_RECEIVED_COMPLETED,
+        CMD_STEP_FORWARD_SIMULATION_COMPLETED,
+        CMD_RESET_SIMULATION_COMPLETED,
+        CMD_CAMERA_IMAGE_COMPLETED,
+        CMD_CAMERA_IMAGE_FAILED,
+        CMD_BODY_INFO_COMPLETED,
+        CMD_BODY_INFO_FAILED,
+		CMD_INVALID_STATUS,
+		CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED,
+		CMD_CALCULATED_INVERSE_DYNAMICS_FAILED,
+        CMD_CALCULATED_JACOBIAN_COMPLETED,
+        CMD_CALCULATED_JACOBIAN_FAILED,
+		CMD_CONTACT_POINT_INFORMATION_COMPLETED,
+		CMD_CONTACT_POINT_INFORMATION_FAILED,
+		CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED,
+		CMD_CALCULATE_INVERSE_KINEMATICS_FAILED,
+		CMD_SAVE_WORLD_COMPLETED,
+		CMD_SAVE_WORLD_FAILED,
+
+		//don't go beyond 'CMD_MAX_SERVER_COMMANDS!
+        CMD_MAX_SERVER_COMMANDS
+};
+
+enum JointInfoFlags
+{
+    JOINT_HAS_MOTORIZED_POWER=1,
+};
+
+enum 
+{
+	COLLISION_SHAPE_TYPE_BOX=1,
+	COLLISION_SHAPE_TYPE_CYLINDER_X,
+	COLLISION_SHAPE_TYPE_CYLINDER_Y,
+	COLLISION_SHAPE_TYPE_CYLINDER_Z,
+	COLLISION_SHAPE_TYPE_CAPSULE_X,
+	COLLISION_SHAPE_TYPE_CAPSULE_Y,
+	COLLISION_SHAPE_TYPE_CAPSULE_Z,
+	COLLISION_SHAPE_TYPE_SPHERE
+};
+
+// copied from btMultiBodyLink.h
+enum JointType {
+    eRevoluteType = 0,
+    ePrismaticType = 1,
+    eFixedType = 2,
+	ePoint2PointType = 3,
+};
+
+struct b3JointInfo
+{
+        char* m_linkName;
+        char* m_jointName;
+        int m_jointType;
+        int m_qIndex;
+        int m_uIndex;
+        int m_jointIndex;
+        int m_flags;
+		double m_jointDamping;
+		double m_jointFriction;
+    double m_parentFrame[7]; // position and orientation (quaternion)
+    double m_childFrame[7]; // ^^^
+    double m_jointAxis[3]; // joint axis in parent local frame
+};
+
+struct b3BodyInfo
+{
+	const char* m_baseName;
+};
+
+
+
+struct b3JointSensorState
+{
+  double m_jointPosition;
+  double m_jointVelocity;
+  double m_jointForceTorque[6];  /* note to roboticists: this is NOT the motor torque/force, but the spatial reaction force vector at joint */
+  double m_jointMotorTorque;
+};
+
+struct b3DebugLines
+{
+    int m_numDebugLines;
+    const float*  m_linesFrom;//float x,y,z times 'm_numDebugLines'.
+    const float*  m_linesTo;//float x,y,z times 'm_numDebugLines'.
+    const float*  m_linesColor;//float red,green,blue times 'm_numDebugLines'.
+};
+
+struct b3CameraImageData
+{
+	int m_pixelWidth;
+	int m_pixelHeight;
+	const unsigned char* m_rgbColorData;//3*m_pixelWidth*m_pixelHeight bytes
+	const float* m_depthValues;//m_pixelWidth*m_pixelHeight floats
+	const int* m_segmentationMaskValues;//m_pixelWidth*m_pixelHeight ints
+};
+
+
+struct b3ContactPointData
+{
+//todo: expose some contact flags, such as telling which fields below are valid
+    int m_contactFlags;
+    int m_bodyUniqueIdA;
+    int m_bodyUniqueIdB;
+    int m_linkIndexA;
+    int m_linkIndexB;
+    double m_positionOnAInWS[3];//contact point location on object A, in world space coordinates
+    double m_positionOnBInWS[3];//contact point location on object A, in world space coordinates
+    double m_contactNormalOnBInWS[3];//the separating contact normal, pointing from object B towards object A
+    double m_contactDistance;//negative number is penetration, positive is distance.
+    
+    double m_normalForce;
+
+//todo: expose the friction forces as well
+//    double m_linearFrictionDirection0[3];
+//    double m_linearFrictionForce0;
+//    double m_linearFrictionDirection1[3];
+//    double m_linearFrictionForce1;
+//    double m_angularFrictionDirection[3];
+//    double m_angularFrictionForce;
+};
+
+
+struct b3ContactInformation
+{
+	int m_numContactPoints;
+	struct b3ContactPointData* m_contactPointData;
+};
+
+///b3LinkState provides extra information such as the Cartesian world coordinates
+///center of mass (COM) of the link, relative to the world reference frame.
+///Orientation is a quaternion x,y,z,w
+///Note: to compute the URDF link frame (which equals the joint frame at joint position 0)
+///use URDF link frame = link COM frame * inertiaFrame.inverse()
+struct b3LinkState
+{
+    double m_worldPosition[3];
+    double m_worldOrientation[4];
+
+    double m_localInertialPosition[3];
+    double m_localInertialOrientation[4];
+};
+
+//todo: discuss and decide about control mode and combinations
+enum {
+    //    POSITION_CONTROL=0,
+    CONTROL_MODE_VELOCITY=0,
+    CONTROL_MODE_TORQUE,
+    CONTROL_MODE_POSITION_VELOCITY_PD,
+};
+
+///flags for b3ApplyExternalTorque and b3ApplyExternalForce
+enum EnumExternalForceFlags
+{
+    EF_LINK_FRAME=1,
+    EF_WORLD_FRAME=2,
+};
+
+///flags to pick the renderer for synthetic camera
+enum EnumRenderer
+{
+    ER_TINY_RENDERER=(1<<16),
+    ER_BULLET_HARDWARE_OPENGL=(1<<17),
+    //ER_FIRE_RAYS=(1<<18),
+};
+
+#endif//SHARED_MEMORY_PUBLIC_H

Engine/lib/bullet/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp

@@ -0,0 +1,731 @@
+/* Copyright (C) 2016 Google
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "TinyRendererVisualShapeConverter.h"
+
+
+
+#include "../Importers/ImportURDFDemo/URDFImporterInterface.h"
+#include "btBulletCollisionCommon.h"
+#include "../Importers/ImportObjDemo/LoadMeshFromObj.h"
+#include "../Importers/ImportSTLDemo/LoadMeshFromSTL.h"
+#include "../Importers/ImportColladaDemo/LoadMeshFromCollada.h"
+#include "BulletCollision/CollisionShapes/btShapeHull.h"//to create a tesselation of a generic btConvexShape
+#include "../CommonInterfaces/CommonGUIHelperInterface.h"
+#include "Bullet3Common/b3FileUtils.h"
+#include <string>
+#include "../Utils/b3ResourcePath.h"
+#include "../TinyRenderer/TinyRenderer.h"
+#include "../OpenGLWindow/SimpleCamera.h"
+#include "../Importers/ImportMeshUtility/b3ImportMeshUtility.h"
+#include <iostream>
+#include <fstream>
+#include "../Importers/ImportURDFDemo/UrdfParser.h"
+
+
+enum MyFileType
+{
+	MY_FILE_STL=1,
+	MY_FILE_COLLADA=2,
+    MY_FILE_OBJ=3,
+};
+
+struct MyTexture2
+{
+	unsigned char* textureData;
+	int m_width;
+	int m_height;
+};
+
+struct TinyRendererObjectArray
+{
+  btAlignedObjectArray<  TinyRenderObjectData*> m_renderObjects;
+};
+
+#define START_WIDTH 640
+#define START_HEIGHT 480
+
+struct TinyRendererVisualShapeConverterInternalData
+{
+	
+    btHashMap<btHashPtr,TinyRendererObjectArray*> m_swRenderInstances;
+    
+   	int m_upAxis;
+	int m_swWidth;
+	int m_swHeight;
+	TGAImage m_rgbColorBuffer;
+	b3AlignedObjectArray<float> m_depthBuffer;
+	b3AlignedObjectArray<int> m_segmentationMaskBuffer;
+	
+	SimpleCamera m_camera;
+	
+	TinyRendererVisualShapeConverterInternalData()
+	:m_upAxis(2),
+	m_swWidth(START_WIDTH),
+	m_swHeight(START_HEIGHT),
+	m_rgbColorBuffer(START_WIDTH,START_HEIGHT,TGAImage::RGB)
+	{
+	    m_depthBuffer.resize(m_swWidth*m_swHeight);
+	    m_segmentationMaskBuffer.resize(m_swWidth*m_swHeight,-1);
+	}
+	
+};
+
+
+
+TinyRendererVisualShapeConverter::TinyRendererVisualShapeConverter()
+{
+	m_data = new TinyRendererVisualShapeConverterInternalData();
+	
+	float dist = 1.5;
+	float pitch = -80;
+	float yaw = 10;
+	float targetPos[3]={0,0,0};
+	m_data->m_camera.setCameraUpAxis(m_data->m_upAxis);
+	resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
+
+
+}
+TinyRendererVisualShapeConverter::~TinyRendererVisualShapeConverter()
+{
+	delete m_data;
+}
+	
+
+
+
+void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MyTexture2>& texturesOut)
+{
+
+	
+	GLInstanceGraphicsShape* glmesh = 0;
+
+	btConvexShape* convexColShape = 0;
+
+	switch (visual->m_geometry.m_type)
+	{
+		case URDF_GEOM_CYLINDER:
+		{
+			btAlignedObjectArray<btVector3> vertices;
+		
+			//int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
+			int numSteps = 32;
+			for (int i = 0; i<numSteps; i++)
+			{
+
+				btScalar cylRadius = visual->m_geometry.m_cylinderRadius;
+				btScalar cylLength = visual->m_geometry.m_cylinderLength;
+				
+				btVector3 vert(cylRadius*btSin(SIMD_2_PI*(float(i) / numSteps)), cylRadius*btCos(SIMD_2_PI*(float(i) / numSteps)), cylLength / 2.);
+				vertices.push_back(vert);
+				vert[2] = -cylLength / 2.;
+				vertices.push_back(vert);
+			}
+
+			btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
+			cylZShape->setMargin(0.001);
+			convexColShape = cylZShape;
+			break;
+		}
+		case URDF_GEOM_BOX:
+		{
+			
+			btVector3 extents = visual->m_geometry.m_boxSize;
+			
+			btBoxShape* boxShape = new btBoxShape(extents*0.5f);
+			//btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5);
+			convexColShape = boxShape;
+			convexColShape->setMargin(0.001);
+			break;
+		}
+		case URDF_GEOM_SPHERE:
+		{
+			btScalar radius = visual->m_geometry.m_sphereRadius;
+			btSphereShape* sphereShape = new btSphereShape(radius);
+			convexColShape = sphereShape;
+			convexColShape->setMargin(0.001);
+			break;
+
+			break;
+		}
+		case URDF_GEOM_MESH:
+		{
+			if (visual->m_name.length())
+			{
+				//b3Printf("visual->name=%s\n", visual->m_name.c_str());
+			}
+			if (1)//visual->m_geometry)
+			{
+				if (visual->m_geometry.m_meshFileName.length())
+				{
+					const char* filename = visual->m_geometry.m_meshFileName.c_str();
+					//b3Printf("mesh->filename=%s\n", filename);
+					char fullPath[1024];
+					int fileType = 0;
+                    
+                    char tmpPathPrefix[1024];
+                    std::string xml_string;
+                    int maxPathLen = 1024;
+                    b3FileUtils::extractPath(filename,tmpPathPrefix,maxPathLen);
+                   
+                    char visualPathPrefix[1024];
+                    sprintf(visualPathPrefix,"%s%s",urdfPathPrefix,tmpPathPrefix);
+                    
+                    
+					sprintf(fullPath, "%s%s", urdfPathPrefix, filename);
+					b3FileUtils::toLower(fullPath);
+					if (strstr(fullPath, ".dae"))
+					{
+						fileType = MY_FILE_COLLADA;
+					}
+					if (strstr(fullPath, ".stl"))
+					{
+						fileType = MY_FILE_STL;
+					}
+                    if (strstr(fullPath,".obj"))
+                    {
+                        fileType = MY_FILE_OBJ;
+                    }
+
+
+					sprintf(fullPath, "%s%s", urdfPathPrefix, filename);
+					FILE* f = fopen(fullPath, "rb");
+					if (f)
+					{
+						fclose(f);
+						
+
+
+						switch (fileType)
+						{
+                            case MY_FILE_OBJ:
+                            {
+                                //glmesh = LoadMeshFromObj(fullPath,visualPathPrefix);
+								b3ImportMeshData meshData;
+								if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fullPath, meshData))
+								{
+									
+									if (meshData.m_textureImage)
+									{
+										MyTexture2 texData;
+										texData.m_width = meshData.m_textureWidth;
+										texData.m_height = meshData.m_textureHeight;
+										texData.textureData = meshData.m_textureImage;
+										texturesOut.push_back(texData);
+									}
+									glmesh = meshData.m_gfxShape;
+								}
+
+								
+                                break;
+                            }
+                           
+						case MY_FILE_STL:
+						{
+							glmesh = LoadMeshFromSTL(fullPath);
+							break;
+						}
+						case MY_FILE_COLLADA:
+						{
+
+							btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes;
+							btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances;
+							btTransform upAxisTrans; upAxisTrans.setIdentity();
+							float unitMeterScaling = 1;
+							int upAxis = 2;
+
+							LoadMeshFromCollada(fullPath,
+								visualShapes,
+								visualShapeInstances,
+								upAxisTrans,
+								unitMeterScaling,
+												upAxis);
+
+							glmesh = new GLInstanceGraphicsShape;
+					//		int index = 0;
+							glmesh->m_indices = new b3AlignedObjectArray<int>();
+							glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
+
+							for (int i = 0; i<visualShapeInstances.size(); i++)
+							{
+								ColladaGraphicsInstance* instance = &visualShapeInstances[i];
+								GLInstanceGraphicsShape* gfxShape = &visualShapes[instance->m_shapeIndex];
+
+								b3AlignedObjectArray<GLInstanceVertex> verts;
+								verts.resize(gfxShape->m_vertices->size());
+
+								int baseIndex = glmesh->m_vertices->size();
+
+								for (int i = 0; i<gfxShape->m_vertices->size(); i++)
+								{
+									verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0];
+									verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1];
+									verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2];
+									verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0];
+									verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1];
+									verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0];
+									verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1];
+									verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2];
+									verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3];
+
+								}
+
+								int curNumIndices = glmesh->m_indices->size();
+								int additionalIndices = gfxShape->m_indices->size();
+								glmesh->m_indices->resize(curNumIndices + additionalIndices);
+								for (int k = 0; k<additionalIndices; k++)
+								{
+									glmesh->m_indices->at(curNumIndices + k) = gfxShape->m_indices->at(k) + baseIndex;
+								}
+
+								//compensate upAxisTrans and unitMeterScaling here
+								btMatrix4x4 upAxisMat;
+								upAxisMat.setIdentity();
+//								upAxisMat.setPureRotation(upAxisTrans.getRotation());
+								btMatrix4x4 unitMeterScalingMat;
+								unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling, unitMeterScaling, unitMeterScaling));
+								btMatrix4x4 worldMat = unitMeterScalingMat*upAxisMat*instance->m_worldTransform;
+								//btMatrix4x4 worldMat = instance->m_worldTransform;
+								int curNumVertices = glmesh->m_vertices->size();
+								int additionalVertices = verts.size();
+								glmesh->m_vertices->reserve(curNumVertices + additionalVertices);
+
+								for (int v = 0; v<verts.size(); v++)
+								{
+									btVector3 pos(verts[v].xyzw[0], verts[v].xyzw[1], verts[v].xyzw[2]);
+									pos = worldMat*pos;
+									verts[v].xyzw[0] = float(pos[0]);
+									verts[v].xyzw[1] = float(pos[1]);
+									verts[v].xyzw[2] = float(pos[2]);
+									glmesh->m_vertices->push_back(verts[v]);
+								}
+							}
+							glmesh->m_numIndices = glmesh->m_indices->size();
+							glmesh->m_numvertices = glmesh->m_vertices->size();
+							//glmesh = LoadMeshFromCollada(fullPath);
+
+							break;
+						}
+						default:
+						{
+                            b3Warning("Error: unsupported file type for Visual mesh: %s\n", fullPath);
+                            btAssert(0);
+						}
+						}
+
+
+						if (glmesh && glmesh->m_vertices && (glmesh->m_numvertices>0))
+						{
+						    //apply the geometry scaling
+						    for (int i=0;i<glmesh->m_vertices->size();i++)
+                            {
+                                glmesh->m_vertices->at(i).xyzw[0] *= visual->m_geometry.m_meshScale[0];
+                                glmesh->m_vertices->at(i).xyzw[1] *= visual->m_geometry.m_meshScale[1];
+                                glmesh->m_vertices->at(i).xyzw[2] *= visual->m_geometry.m_meshScale[2];
+                            }
+						    
+						}
+						else
+						{
+							b3Warning("issue extracting mesh from COLLADA/STL file %s\n", fullPath);
+						}
+
+					}
+					else
+					{
+						b3Warning("mesh geometry not found %s\n", fullPath);
+					}
+
+
+				}
+			}
+
+
+			break;
+		}
+		default:
+		{
+			b3Warning("Error: unknown visual geometry type\n");
+		}
+	}
+
+	//if we have a convex, tesselate into localVertices/localIndices
+	if ((glmesh==0) && convexColShape)
+	{
+		btShapeHull* hull = new btShapeHull(convexColShape);
+		hull->buildHull(0.0);
+		{
+			//	int strideInBytes = 9*sizeof(float);
+			int numVertices = hull->numVertices();
+			int numIndices = hull->numIndices();
+
+			
+			glmesh = new GLInstanceGraphicsShape;
+		//	int index = 0;
+			glmesh->m_indices = new b3AlignedObjectArray<int>();
+			glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
+
+
+			for (int i = 0; i < numVertices; i++)
+			{
+				GLInstanceVertex vtx;
+				btVector3 pos = hull->getVertexPointer()[i];
+				vtx.xyzw[0] = pos.x();
+				vtx.xyzw[1] = pos.y();
+				vtx.xyzw[2] = pos.z();
+				vtx.xyzw[3] = 1.f;
+				pos.normalize();
+				vtx.normal[0] = pos.x();
+				vtx.normal[1] = pos.y();
+				vtx.normal[2] = pos.z();
+				vtx.uv[0] = 0.5f;
+				vtx.uv[1] = 0.5f;
+				glmesh->m_vertices->push_back(vtx);
+			}
+
+			btAlignedObjectArray<int> indices;
+			for (int i = 0; i < numIndices; i++)
+			{
+				glmesh->m_indices->push_back(hull->getIndexPointer()[i]);
+			}
+			
+			glmesh->m_numvertices = glmesh->m_vertices->size();
+			glmesh->m_numIndices = glmesh->m_indices->size();
+		}
+        delete hull;
+		delete convexColShape;
+		convexColShape = 0;
+
+	}
+	
+	if (glmesh && glmesh->m_numIndices>0 && glmesh->m_numvertices >0)
+	{
+
+		int baseIndex = verticesOut.size();
+
+
+
+		for (int i = 0; i < glmesh->m_indices->size(); i++)
+		{
+			indicesOut.push_back(glmesh->m_indices->at(i) + baseIndex);
+		}
+
+		for (int i = 0; i < glmesh->m_vertices->size(); i++)
+		{
+			GLInstanceVertex& v = glmesh->m_vertices->at(i);
+			btVector3 vert(v.xyzw[0],v.xyzw[1],v.xyzw[2]);
+			btVector3 vt = visualTransform*vert;
+			v.xyzw[0] = vt[0];
+			v.xyzw[1] = vt[1];
+			v.xyzw[2] = vt[2];
+			btVector3 triNormal(v.normal[0],v.normal[1],v.normal[2]);
+			triNormal = visualTransform.getBasis()*triNormal;
+			v.normal[0] = triNormal[0];
+			v.normal[1] = triNormal[1];
+			v.normal[2] = triNormal[2];
+			verticesOut.push_back(v);
+		}
+	}
+    delete glmesh;
+    
+}
+
+
+
+void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfModel& model, class btCollisionObject* colObj, int objectIndex)
+{
+    
+	
+	UrdfLink* const* linkPtr = model.m_links.getAtIndex(linkIndex);
+	if (linkPtr)
+	{
+
+		const UrdfLink* link = *linkPtr;
+	
+		for (int v = 0; v < link->m_visualArray.size();v++)
+		{
+			btAlignedObjectArray<MyTexture2> textures;
+			btAlignedObjectArray<GLInstanceVertex> vertices;
+			btAlignedObjectArray<int> indices;
+			btTransform startTrans; startTrans.setIdentity();
+			int graphicsIndex = -1;
+
+			const UrdfVisual& vis = link->m_visualArray[v];
+			btTransform childTrans = vis.m_linkLocalFrame;
+			btHashString matName(vis.m_materialName.c_str());
+			UrdfMaterial *const * matPtr = model.m_materials[matName];
+            
+            float rgbaColor[4] = {1,1,1,1};
+            
+			if (matPtr)
+			{
+				UrdfMaterial *const  mat = *matPtr;
+                for (int i=0;i<4;i++)
+                    rgbaColor[i] = mat->m_rgbaColor[i];
+				//printf("UrdfMaterial %s, rgba = %f,%f,%f,%f\n",mat->m_name.c_str(),mat->m_rgbaColor[0],mat->m_rgbaColor[1],mat->m_rgbaColor[2],mat->m_rgbaColor[3]);
+				//m_data->m_linkColors.insert(linkIndex,mat->m_rgbaColor);
+			}
+			
+			TinyRendererObjectArray** visualsPtr = m_data->m_swRenderInstances[colObj];
+            if (visualsPtr==0)
+            {
+                m_data->m_swRenderInstances.insert(colObj,new TinyRendererObjectArray);
+            }
+            visualsPtr = m_data->m_swRenderInstances[colObj];
+            btAssert(visualsPtr);
+            TinyRendererObjectArray* visuals = *visualsPtr;
+            
+			convertURDFToVisualShape(&vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices,textures);
+
+            if (vertices.size() && indices.size())
+            {
+                TinyRenderObjectData* tinyObj = new TinyRenderObjectData(m_data->m_rgbColorBuffer,m_data->m_depthBuffer, &m_data->m_segmentationMaskBuffer, objectIndex);
+				unsigned char* textureImage=0;
+				int textureWidth=0;
+				int textureHeight=0;
+				if (textures.size())
+				{
+					textureImage = textures[0].textureData;
+					textureWidth = textures[0].m_width;
+					textureHeight = textures[0].m_height;
+				}
+				
+                tinyObj->registerMeshShape(&vertices[0].xyzw[0],vertices.size(),&indices[0],indices.size(),rgbaColor,
+										   textureImage,textureWidth,textureHeight);
+                visuals->m_renderObjects.push_back(tinyObj);
+            }
+			for (int i=0;i<textures.size();i++)
+			{
+				free(textures[i].textureData);
+			}
+		}
+	}
+}
+
+void TinyRendererVisualShapeConverter::setUpAxis(int axis)
+{
+    m_data->m_upAxis = axis;
+    m_data->m_camera.setCameraUpAxis(axis);
+    m_data->m_camera.update();
+}
+void TinyRendererVisualShapeConverter::resetCamera(float camDist, float pitch, float yaw, float camPosX,float camPosY, float camPosZ)
+{
+    m_data->m_camera.setCameraDistance(camDist);
+    m_data->m_camera.setCameraPitch(pitch);
+    m_data->m_camera.setCameraYaw(yaw);
+    m_data->m_camera.setCameraTargetPosition(camPosX,camPosY,camPosZ);
+    m_data->m_camera.setAspectRatio((float)m_data->m_swWidth/(float)m_data->m_swHeight);
+    m_data->m_camera.update();
+
+}
+
+void TinyRendererVisualShapeConverter::clearBuffers(TGAColor& clearColor)
+{
+    for(int y=0;y<m_data->m_swHeight;++y)
+    {
+        for(int x=0;x<m_data->m_swWidth;++x)
+        {
+            m_data->m_rgbColorBuffer.set(x,y,clearColor);
+            m_data->m_depthBuffer[x+y*m_data->m_swWidth] = -1e30f;
+            m_data->m_segmentationMaskBuffer[x+y*m_data->m_swWidth] = -1;
+        }
+    }
+    
+}
+
+void TinyRendererVisualShapeConverter::render() 
+{
+
+    ATTRIBUTE_ALIGNED16(float viewMat[16]);
+    ATTRIBUTE_ALIGNED16(float projMat[16]);
+
+    m_data->m_camera.getCameraProjectionMatrix(projMat);
+    m_data->m_camera.getCameraViewMatrix(viewMat);
+
+	render(viewMat,projMat);
+}    
+
+void TinyRendererVisualShapeConverter::render(const float viewMat[16], const float projMat[16]) 
+{
+    //clear the color buffer
+    TGAColor clearColor;
+    clearColor.bgra[0] = 255;
+    clearColor.bgra[1] = 255;
+    clearColor.bgra[2] = 255;
+    clearColor.bgra[3] = 255;
+    
+    clearBuffers(clearColor);
+
+    
+    ATTRIBUTE_ALIGNED16(btScalar modelMat[16]);
+    
+    
+    btVector3 lightDirWorld(-5,200,-40);
+    switch (m_data->m_upAxis)
+    {
+    case 1:
+            lightDirWorld = btVector3(-50.f,100,30);
+        break;
+    case 2:
+            lightDirWorld = btVector3(-50.f,30,100);
+            break;
+    default:{}
+    };
+    
+    lightDirWorld.normalize();
+    
+  //  printf("num m_swRenderInstances = %d\n", m_data->m_swRenderInstances.size());
+    for (int i=0;i<m_data->m_swRenderInstances.size();i++)
+    {
+        TinyRendererObjectArray** visualArrayPtr = m_data->m_swRenderInstances.getAtIndex(i);
+        if (0==visualArrayPtr)
+            continue;//can this ever happen?
+        TinyRendererObjectArray* visualArray = *visualArrayPtr;
+
+        btHashPtr colObjHash = m_data->m_swRenderInstances.getKeyAtIndex(i);
+        
+        
+        const btCollisionObject* colObj = (btCollisionObject*) colObjHash.getPointer();
+        
+        for (int v=0;v<visualArray->m_renderObjects.size();v++)
+        {
+            
+            TinyRenderObjectData* renderObj = visualArray->m_renderObjects[v];
+            
+        
+            //sync the object transform
+            const btTransform& tr = colObj->getWorldTransform();
+            tr.getOpenGLMatrix(modelMat);
+    
+            for (int i=0;i<4;i++)
+            {
+                for (int j=0;j<4;j++)
+                {
+                    
+                    renderObj->m_projectionMatrix[i][j] = projMat[i+4*j];
+                    renderObj->m_modelMatrix[i][j] = modelMat[i+4*j];
+                    renderObj->m_viewMatrix[i][j] = viewMat[i+4*j];
+                    renderObj->m_localScaling = colObj->getCollisionShape()->getLocalScaling();
+                    renderObj->m_lightDirWorld = lightDirWorld;
+                }
+            }
+            TinyRenderer::renderObject(*renderObj);
+        }
+    }
+	//printf("write tga \n");
+	//m_data->m_rgbColorBuffer.write_tga_file("camera.tga");
+//	printf("flipped!\n");
+	m_data->m_rgbColorBuffer.flip_vertically();
+
+	//flip z-buffer and segmentation Buffer
+	{
+		int half = m_data->m_swHeight>>1;
+		for (int j=0; j<half; j++)
+		{
+			unsigned long l1 = j*m_data->m_swWidth;
+			unsigned long l2 = (m_data->m_swHeight-1-j)*m_data->m_swWidth;
+			for (int i=0;i<m_data->m_swWidth;i++)
+			{
+				btSwap(m_data->m_depthBuffer[l1+i],m_data->m_depthBuffer[l2+i]);
+				btSwap(m_data->m_segmentationMaskBuffer[l1+i],m_data->m_segmentationMaskBuffer[l2+i]);
+			}
+		}
+	}
+}
+
+void TinyRendererVisualShapeConverter::getWidthAndHeight(int& width, int& height)
+{
+    width = m_data->m_swWidth;
+    height = m_data->m_swHeight;
+}
+
+
+void TinyRendererVisualShapeConverter::setWidthAndHeight(int width, int height)
+{
+	m_data->m_swWidth = width;
+	m_data->m_swHeight = height;
+
+	m_data->m_depthBuffer.resize(m_data->m_swWidth*m_data->m_swHeight);
+	m_data->m_segmentationMaskBuffer.resize(m_data->m_swWidth*m_data->m_swHeight);
+	m_data->m_rgbColorBuffer = TGAImage(width, height, TGAImage::RGB);
+	
+		
+}
+
+void TinyRendererVisualShapeConverter::copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, 
+                                                            float* depthBuffer, int depthBufferSizeInPixels,
+                                                            int* segmentationMaskBuffer, int segmentationMaskSizeInPixels,
+                                                            int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied)
+{
+    int w = m_data->m_rgbColorBuffer.get_width();
+    int h = m_data->m_rgbColorBuffer.get_height();
+    
+    if (numPixelsCopied)
+        *numPixelsCopied = 0;
+    
+    if (widthPtr)
+        *widthPtr = w;
+    
+    if (heightPtr)
+        *heightPtr = h;
+    
+    int numTotalPixels = w*h;
+    int numRemainingPixels = numTotalPixels - startPixelIndex;
+    int numBytesPerPixel = 4;//RGBA
+    int numRequestedPixels  = btMin(rgbaBufferSizeInPixels,numRemainingPixels);
+    if (numRequestedPixels)
+    {
+        for (int i=0;i<numRequestedPixels;i++)
+        {
+			if (depthBuffer)
+			{
+				depthBuffer[i] = m_data->m_depthBuffer[i+startPixelIndex];
+			}
+			if (segmentationMaskBuffer)
+            {
+                segmentationMaskBuffer[i] = m_data->m_segmentationMaskBuffer[i+startPixelIndex];
+            }
+			
+            if (pixelsRGBA)
+            {
+                pixelsRGBA[i*numBytesPerPixel] =   m_data->m_rgbColorBuffer.buffer()[(i+startPixelIndex)*3+0];
+                pixelsRGBA[i*numBytesPerPixel+1] = m_data->m_rgbColorBuffer.buffer()[(i+startPixelIndex)*3+1];
+                pixelsRGBA[i*numBytesPerPixel+2] = m_data->m_rgbColorBuffer.buffer()[(i+startPixelIndex)*3+2];
+                pixelsRGBA[i*numBytesPerPixel+3] = 255;
+                
+            }
+        }
+        
+        if (numPixelsCopied)
+            *numPixelsCopied = numRequestedPixels;
+        
+    }    
+}
+
+
+void TinyRendererVisualShapeConverter::resetAll()
+{
+	for (int i=0;i<m_data->m_swRenderInstances.size();i++)
+	{
+		TinyRendererObjectArray** ptrptr = m_data->m_swRenderInstances.getAtIndex(i);
+		if (ptrptr && *ptrptr)
+		{
+			TinyRendererObjectArray* ptr = *ptrptr;
+			delete ptr;
+		}
+	}
+	
+	m_data->m_swRenderInstances.clear();
+}
+

Engine/lib/bullet/examples/SharedMemory/TinyRendererVisualShapeConverter.h

@@ -0,0 +1,39 @@
+#ifndef TINY_RENDERER_VISUAL_SHAPE_CONVERTER_H
+#define TINY_RENDERER_VISUAL_SHAPE_CONVERTER_H
+
+
+#include "../Importers/ImportURDFDemo/LinkVisualShapesConverter.h"
+
+struct TinyRendererVisualShapeConverter : public LinkVisualShapesConverter
+{
+	
+	struct TinyRendererVisualShapeConverterInternalData* m_data;
+	
+	TinyRendererVisualShapeConverter();
+	
+	virtual ~TinyRendererVisualShapeConverter();
+	
+	virtual void convertVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame, const UrdfModel& model, class btCollisionObject* colShape, int objectIndex);
+	
+	void setUpAxis(int axis);
+	
+    void resetCamera(float camDist, float pitch, float yaw, float camPosX,float camPosY, float camPosZ);
+	
+    void clearBuffers(struct TGAColor& clearColor);
+
+	void resetAll();
+
+    void getWidthAndHeight(int& width, int& height);
+	void setWidthAndHeight(int width, int height);
+    
+    void copyCameraImageData(unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, float* depthBuffer, int depthBufferSizeInPixels,int* segmentationMaskBuffer, int segmentationMaskSizeInPixels,  int startPixelIndex, int* widthPtr, int* heightPtr, int* numPixelsCopied);
+    
+	void render();
+	void render(const float viewMat[16], const float projMat[16]);
+	
+};
+
+
+
+
+#endif //TINY_RENDERER_VISUAL_SHAPE_CONVERTER_H

Engine/lib/bullet/examples/SharedMemory/Win32SharedMemory.cpp

@@ -0,0 +1,110 @@
+#ifdef _WIN32
+#include "Win32SharedMemory.h"
+#include "Bullet3Common/b3Logging.h"
+#include "Bullet3Common/b3Scalar.h"
+
+#include <windows.h>
+
+//see also https://msdn.microsoft.com/en-us/library/windows/desktop/aa366551%28v=vs.85%29.aspx
+
+//TCHAR szName[]=TEXT("Global\\MyFileMappingObject2");
+TCHAR szName[]=TEXT("MyFileMappingObject2");
+
+struct Win32SharedMemoryInteralData
+{
+	HANDLE m_hMapFile;
+
+	void*	m_buf;
+
+	Win32SharedMemoryInteralData()
+		:m_hMapFile(0),
+		m_buf(0)
+	{
+	}
+};
+
+Win32SharedMemory::Win32SharedMemory()
+{
+	m_internalData = new Win32SharedMemoryInteralData;
+}
+Win32SharedMemory::~Win32SharedMemory()
+{
+	delete m_internalData;
+}
+
+void*   Win32SharedMemory::allocateSharedMemory(int key, int size, bool allowCreation)
+{
+	b3Assert(m_internalData->m_buf==0);
+
+	m_internalData->m_hMapFile = OpenFileMapping(
+                   FILE_MAP_ALL_ACCESS,   // read/write access
+                   FALSE,                 // do not inherit the name
+                   szName);               // name of mapping object
+
+	if (m_internalData->m_hMapFile==NULL)
+	{
+		 if (allowCreation)
+		 {
+			  m_internalData->m_hMapFile = CreateFileMapping(
+                 INVALID_HANDLE_VALUE,    // use paging file
+                 NULL,                    // default security
+                 PAGE_READWRITE,          // read/write access
+                 0,                       // maximum object size (high-order DWORD)
+                 size,						// maximum object size (low-order DWORD)
+                 szName);                 // name of mapping object
+		 } else
+		 {
+			   b3Error("Could not create file mapping object (%d).\n",GetLastError());
+			 return 0;
+		 }
+
+	}
+
+   m_internalData->m_buf =  MapViewOfFile(m_internalData->m_hMapFile,   // handle to map object
+                        FILE_MAP_ALL_ACCESS, // read/write permission
+                        0,
+                        0,
+                        size);
+
+   if (m_internalData->m_buf == NULL)
+	{
+		b3Error("Could not map view of file (%d).\n",GetLastError());
+		CloseHandle(m_internalData->m_hMapFile);
+		return 0;
+   }
+
+   return m_internalData->m_buf;
+}
+void Win32SharedMemory::releaseSharedMemory(int key, int size)
+{
+
+	if (m_internalData->m_buf)
+	{
+
+		UnmapViewOfFile(m_internalData->m_buf);
+		m_internalData->m_buf=0;
+	}
+
+	if (m_internalData->m_hMapFile)
+	{
+		CloseHandle(m_internalData->m_hMapFile);
+		m_internalData->m_hMapFile = 0;
+	}
+
+}
+
+Win32SharedMemoryServer::Win32SharedMemoryServer()
+{
+}
+Win32SharedMemoryServer::~Win32SharedMemoryServer()
+{
+}
+
+Win32SharedMemoryClient::Win32SharedMemoryClient()
+{
+}
+Win32SharedMemoryClient:: ~Win32SharedMemoryClient()
+{
+}
+
+#endif //_WIN32

Engine/lib/bullet/examples/SharedMemory/Win32SharedMemory.h

@@ -0,0 +1,39 @@
+#ifndef WIN32_SHARED_MEMORY_H
+#define WIN32_SHARED_MEMORY_H
+
+#include "SharedMemoryInterface.h"
+
+
+
+class Win32SharedMemory : public SharedMemoryInterface
+{
+
+	struct Win32SharedMemoryInteralData* m_internalData;
+    
+public:
+    Win32SharedMemory();
+    virtual ~Win32SharedMemory();
+
+    virtual void*   allocateSharedMemory(int key, int size, bool allowCreation);
+    virtual void releaseSharedMemory(int key, int size);
+	
+};
+
+class Win32SharedMemoryServer : public Win32SharedMemory
+{
+public:
+	Win32SharedMemoryServer();
+	virtual ~Win32SharedMemoryServer();
+	
+};
+
+class Win32SharedMemoryClient : public Win32SharedMemory
+{
+public:
+	Win32SharedMemoryClient();
+	virtual ~Win32SharedMemoryClient();
+	
+};
+
+
+#endif //WIN32_SHARED_MEMORY_H

Engine/lib/bullet/examples/SharedMemory/main.cpp

@@ -0,0 +1,100 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2015 Google Inc. http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+
+#include "PhysicsServerExample.h"
+#include "PhysicsClientExample.h"
+#include "Bullet3Common/b3CommandLineArgs.h"
+
+#include "../CommonInterfaces/CommonExampleInterface.h"
+#include "../CommonInterfaces/CommonGUIHelperInterface.h"
+#include "SharedMemoryCommon.h"
+
+
+#include <stdlib.h>
+
+int gSharedMemoryKey = -1;
+
+static SharedMemoryCommon*    example = NULL;
+static bool interrupted = false;
+
+#ifndef _WIN32
+#include <signal.h>
+#include <err.h>
+#include <unistd.h>
+static void cleanup(int signo)
+{
+    if (interrupted) {  // this is the second time, we're hanging somewhere
+      //  if (example) {
+      //      example->abort();
+      //  }
+        b3Printf("Aborting and deleting SharedMemoryCommon object");
+        sleep(1);
+        delete example;
+        errx(EXIT_FAILURE, "aborted example on signal %d", signo);
+    }
+    interrupted = true;
+    warnx("caught signal %d", signo);
+}
+#endif//_WIN32
+
+
+int main(int argc, char* argv[])
+{
+#ifndef _WIN32
+    struct sigaction action;
+    memset(&action, 0x0, sizeof(action));
+    action.sa_handler = cleanup;
+    static const int signos[] = { SIGHUP, SIGINT, SIGQUIT, SIGABRT, SIGSEGV, SIGPIPE, SIGTERM };
+    for (int ii(0); ii < sizeof(signos) / sizeof(*signos); ++ii) {
+        if (0 != sigaction(signos[ii], &action, NULL)) {
+            err(EXIT_FAILURE, "signal %d", signos[ii]);
+        }
+    }
+#endif
+    
+	b3CommandLineArgs args(argc, argv);
+	
+	DummyGUIHelper noGfx;
+
+	CommonExampleOptions options(&noGfx);
+
+	args.GetCmdLineArgument("shared_memory_key", gSharedMemoryKey);
+	
+  	if (args.CheckCmdLineFlag("client"))
+    {
+        example = (SharedMemoryCommon*)PhysicsClientCreateFunc(options);
+    }else
+    {
+//        options.m_option |= PHYSICS_SERVER_ENABLE_COMMAND_LOGGING;
+       // options.m_option |= PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG;
+        
+        example = (SharedMemoryCommon*)PhysicsServerCreateFunc(options);
+    }
+	
+	
+	example->initPhysics();
+	while (example->isConnected() && !(example->wantsTermination() || interrupted))
+	{
+		example->stepSimulation(1.f/60.f);
+	}	
+
+	example->exitPhysics();
+
+	delete example;
+
+	return 0;
+}
+

Engine/lib/bullet/examples/SharedMemory/premake4.lua

@@ -0,0 +1,291 @@
+
+project "App_SharedMemoryPhysics"
+
+if _OPTIONS["ios"] then
+	kind "WindowedApp"
+else	
+	kind "ConsoleApp"
+end
+
+includedirs {".","../../src", "../ThirdPartyLibs",}
+
+links {
+	"Bullet3Common","BulletInverseDynamicsUtils", "BulletInverseDynamics",	"BulletDynamics","BulletCollision", "LinearMath", "BussIK"
+}
+
+language "C++"
+
+myfiles = 
+{
+	"IKTrajectoryHelper.cpp",
+	"IKTrajectoryHelper.h",
+	"PhysicsClient.cpp",
+	"PhysicsClientSharedMemory.cpp",
+	"PhysicsClientExample.cpp",
+	"PhysicsServerExample.cpp",
+	"PhysicsServerSharedMemory.cpp",
+	"PhysicsServerSharedMemory.h",
+	"PhysicsServer.cpp",
+	"PhysicsServer.h",
+	"PhysicsClientC_API.cpp",
+	"SharedMemoryCommands.h",
+	"SharedMemoryPublic.h",
+	"PhysicsServer.cpp",
+	"PosixSharedMemory.cpp",
+	"Win32SharedMemory.cpp",
+	"InProcessMemory.cpp",
+	"PhysicsDirect.cpp",
+	"PhysicsDirect.h",
+	"PhysicsDirectC_API.cpp",
+	"PhysicsDirectC_API.h",
+	"PhysicsLoopBack.cpp",
+	"PhysicsLoopBack.h",
+	"PhysicsLoopBackC_API.cpp",
+	"PhysicsLoopBackC_API.h",
+	"PhysicsServerCommandProcessor.cpp",
+	"PhysicsServerCommandProcessor.h",
+	"TinyRendererVisualShapeConverter.cpp",
+	"TinyRendererVisualShapeConverter.h",
+	"../TinyRenderer/geometry.cpp",
+	"../TinyRenderer/model.cpp",
+	"../TinyRenderer/tgaimage.cpp",
+	"../TinyRenderer/our_gl.cpp",
+	"../TinyRenderer/TinyRenderer.cpp",
+	"../OpenGLWindow/SimpleCamera.cpp",
+	"../OpenGLWindow/SimpleCamera.h",
+	"../Importers/ImportURDFDemo/ConvertRigidBodies2MultiBody.h",
+	"../Importers/ImportURDFDemo/MultiBodyCreationInterface.h",
+	"../Importers/ImportURDFDemo/MyMultiBodyCreator.cpp",
+	"../Importers/ImportURDFDemo/MyMultiBodyCreator.h",
+	"../Importers/ImportURDFDemo/BulletUrdfImporter.cpp",
+	"../Importers/ImportURDFDemo/BulletUrdfImporter.h",
+	"../Importers/ImportURDFDemo/UrdfParser.cpp",
+ 	"../Importers/ImportURDFDemo/urdfStringSplit.cpp",
+	"../Importers/ImportURDFDemo/UrdfParser.cpp",
+	"../Importers/ImportURDFDemo/UrdfParser.h",
+	"../Importers/ImportURDFDemo/URDF2Bullet.cpp",
+	"../Importers/ImportURDFDemo/URDF2Bullet.h",
+	"../Utils/b3ResourcePath.cpp",
+	"../Utils/b3Clock.cpp",	
+	"../../Extras/Serialize/BulletWorldImporter/*",
+	"../../Extras/Serialize/BulletFileLoader/*",	
+	"../Importers/ImportURDFDemo/URDFImporterInterface.h",
+	"../Importers/ImportURDFDemo/URDFJointTypes.h",
+	"../Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp",
+	"../Importers/ImportObjDemo/LoadMeshFromObj.cpp",
+	"../Importers/ImportSTLDemo/ImportSTLSetup.h",
+	"../Importers/ImportSTLDemo/LoadMeshFromSTL.h",
+	"../Importers/ImportColladaDemo/LoadMeshFromCollada.cpp",
+	"../Importers/ImportColladaDemo/ColladaGraphicsInstance.h",
+	"../ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp",	
+	"../ThirdPartyLibs/tinyxml/tinystr.cpp",
+	"../ThirdPartyLibs/tinyxml/tinyxml.cpp",
+	"../ThirdPartyLibs/tinyxml/tinyxmlerror.cpp",
+	"../ThirdPartyLibs/tinyxml/tinyxmlparser.cpp",
+	"../Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
+	"../ThirdPartyLibs/stb_image/stb_image.cpp",     
+}
+
+files {
+	myfiles,
+	"main.cpp",
+}
+
+
+files {
+		"../MultiThreading/b3ThreadSupportInterface.cpp",
+		"../MultiThreading/b3ThreadSupportInterface.h"
+	}
+	if os.is("Windows") then
+
+		files {
+                "../MultiThreading/b3Win32ThreadSupport.cpp",  
+                "../MultiThreading/b3Win32ThreadSupport.h" 
+		}
+		--links {"winmm"}
+		--defines {"__WINDOWS_MM__", "WIN32"}
+	end
+
+	if os.is("Linux") then 
+		files {
+                "../MultiThreading/b3PosixThreadSupport.cpp",  
+                "../MultiThreading/b3PosixThreadSupport.h"    
+        	}
+
+		links {"pthread"}
+	end
+
+	if os.is("MacOSX") then
+		files {
+                "../MultiThreading/b3PosixThreadSupport.cpp",
+                "../MultiThreading/b3PosixThreadSupport.h"    
+                }
+
+		links {"pthread"}
+		--links{"CoreAudio.framework", "coreMIDI.framework", "Cocoa.framework"}
+		--defines {"__MACOSX_CORE__"}
+	end
+
+
+project "App_SharedMemoryPhysics_GUI"
+
+if _OPTIONS["ios"] then
+        kind "WindowedApp"
+else
+        kind "ConsoleApp"
+end
+defines {"B3_USE_STANDALONE_EXAMPLE"}
+
+includedirs {"../../src", "../ThirdPartyLibs"}
+
+links {
+        "BulletInverseDynamicsUtils", "BulletInverseDynamics", "BulletDynamics","BulletCollision", "LinearMath", "OpenGL_Window","Bullet3Common","BussIK"
+}
+	initOpenGL()
+  initGlew()
+
+language "C++"
+
+files {
+        myfiles,
+        "../StandaloneMain/main_opengl_single_example.cpp",
+				"../ExampleBrowser/OpenGLGuiHelper.cpp",
+				"../ExampleBrowser/GL_ShapeDrawer.cpp",
+				"../ExampleBrowser/CollisionShape2TriangleMesh.cpp",
+}
+
+if os.is("Linux") then initX11() end
+
+if os.is("MacOSX") then
+        links{"Cocoa.framework"}
+end
+
+
+files {
+		"../MultiThreading/b3ThreadSupportInterface.cpp",
+		"../MultiThreading/b3ThreadSupportInterface.h"
+	}
+if os.is("Windows") then
+
+	files {
+              "../MultiThreading/b3Win32ThreadSupport.cpp",  
+              "../MultiThreading/b3Win32ThreadSupport.h" 
+	}
+	--links {"winmm"}
+	--defines {"__WINDOWS_MM__", "WIN32"}
+end
+
+if os.is("Linux") then 
+	files {
+              "../MultiThreading/b3PosixThreadSupport.cpp",  
+              "../MultiThreading/b3PosixThreadSupport.h"    
+      	}
+
+	links {"pthread"}
+end
+
+if os.is("MacOSX") then
+	files {
+              "../MultiThreading/b3PosixThreadSupport.cpp",
+              "../MultiThreading/b3PosixThreadSupport.h"    
+              }
+
+	links {"pthread"}
+	--links{"CoreAudio.framework", "coreMIDI.framework", "Cocoa.framework"}
+	--defines {"__MACOSX_CORE__"}
+end
+
+if os.is("Windows") then 
+	project "App_SharedMemoryPhysics_VR"
+	--for now, only enable VR under Windows, until compilation issues are resolved on Mac/Linux
+	defines {"B3_USE_STANDALONE_EXAMPLE","BT_ENABLE_VR"}
+	
+	if _OPTIONS["ios"] then
+		kind "WindowedApp"
+	else	
+		kind "ConsoleApp"
+	end
+	
+	includedirs {
+			".","../../src", "../ThirdPartyLibs",
+			"../ThirdPartyLibs/openvr/headers",
+			"../ThirdPartyLibs/openvr/samples/shared"
+		}
+						
+	links {
+		"BulletInverseDynamicsUtils", "BulletInverseDynamics","Bullet3Common",	"BulletDynamics","BulletCollision", "LinearMath","OpenGL_Window","openvr_api","BussIK"
+	}
+	
+	
+	language "C++"
+	
+	
+		initOpenGL()
+	  initGlew()
+	
+	
+	files
+	{
+		myfiles,
+		 "../StandaloneMain/hellovr_opengl_main.cpp",
+					"../ExampleBrowser/OpenGLGuiHelper.cpp",
+					"../ExampleBrowser/GL_ShapeDrawer.cpp",
+					"../ExampleBrowser/CollisionShape2TriangleMesh.cpp",
+					"../RenderingExamples/TinyVRGui.cpp",
+					"../RenderingExamples/TimeSeriesCanvas.cpp",
+					"../RenderingExamples/TimeSeriesFontData.cpp",
+					"../ThirdPartyLibs/openvr/samples/shared/lodepng.cpp",
+					"../ThirdPartyLibs/openvr/samples/shared/lodepng.h",
+					"../ThirdPartyLibs/openvr/samples/shared/Matrices.cpp",
+					"../ThirdPartyLibs/openvr/samples/shared/Matrices.h",
+					"../ThirdPartyLibs/openvr/samples/shared/pathtools.cpp",
+					"../ThirdPartyLibs/openvr/samples/shared/pathtools.h",
+					"../ThirdPartyLibs/openvr/samples/shared/Vectors.h",
+	}
+	if os.is("Windows") then 
+		libdirs {"../ThirdPartyLibs/openvr/lib/win32"}
+	end
+	
+	if os.is("Linux") then initX11() end
+	
+	if os.is("MacOSX") then
+	        links{"Cocoa.framework"}
+	end
+	
+	
+	files {
+			"../MultiThreading/b3ThreadSupportInterface.cpp",
+			"../MultiThreading/b3ThreadSupportInterface.h"
+		}
+		if os.is("Windows") then
+	
+			files {
+	                "../MultiThreading/b3Win32ThreadSupport.cpp",  
+	                "../MultiThreading/b3Win32ThreadSupport.h" 
+			}
+			--links {"winmm"}
+			--defines {"__WINDOWS_MM__", "WIN32"}
+		end
+	
+		if os.is("Linux") then 
+			files {
+	                "../MultiThreading/b3PosixThreadSupport.cpp",  
+	                "../MultiThreading/b3PosixThreadSupport.h"    
+	        	}
+	
+			links {"pthread"}
+		end
+	
+		if os.is("MacOSX") then
+			files {
+	                "../MultiThreading/b3PosixThreadSupport.cpp",
+	                "../MultiThreading/b3PosixThreadSupport.h"    
+	                }
+	
+			links {"pthread"}
+			--links{"CoreAudio.framework", "coreMIDI.framework", "Cocoa.framework"}
+			--defines {"__MACOSX_CORE__"}
+		end
+
+
+end