bullet3/examples/SharedMemory/PhysicsServerExample.cpp

//todo(erwincoumans): re-use the upcoming b3RobotSimAPI here

#include "PhysicsServerExample.h"


#include "../CommonInterfaces/Common2dCanvasInterface.h"
#include "PhysicsServerSharedMemory.h"
#include "Bullet3Common/b3CommandLineArgs.h"
#include "SharedMemoryCommon.h"
#include "Bullet3Common/b3Matrix3x3.h"
#include "../Utils/b3Clock.h"
#include "../MultiThreading/b3ThreadSupportInterface.h"
#include "SharedMemoryPublic.h"
//#define BT_ENABLE_VR
#ifdef BT_ENABLE_VR
#include "../RenderingExamples/TinyVRGui.h"
#endif//BT_ENABLE_VR


#include "../CommonInterfaces/CommonParameterInterface.h"
#include "../Importers/ImportURDFDemo/urdfStringSplit.h"

//@todo(erwincoumans) those globals are hacks for a VR demo, move this to Python/pybullet!
bool gEnablePicking=true;
bool gEnableTeleporting=true;
bool gEnableRendering= true;
bool gActivedVRRealTimeSimulation = false;

bool gEnableSyncPhysicsRendering= true;
bool gEnableUpdateDebugDrawLines = true;
static int gCamVisualizerWidth = 228;
static int gCamVisualizerHeight = 192;

static bool gEnableDefaultKeyboardShortcuts = true;
static bool gEnableDefaultMousePicking = true;





btScalar gVRTeleportRotZ = 0;

extern int gInternalSimFlags;
extern bool gResetSimulation;
int gGraspingController = -1;
extern btScalar simTimeScalingFactor;
bool gBatchUserDebugLines = true;


const char* startFileNameVR = "0_VRDemoSettings.txt";

#include <vector>

static void loadCurrentSettingsVR(b3CommandLineArgs& args)
{
	//int currentEntry = 0;
	FILE* f = fopen(startFileNameVR, "r");
	if (f)
	{
		char oneline[1024];
		char* argv[] = { 0,&oneline[0] };
		
		while (fgets(oneline, 1024, f) != NULL)
		{
			char *pos;
			if ((pos = strchr(oneline, '\n')) != NULL)
				*pos = '\0';
			args.addArgs(2, argv);
		}
		fclose(f);
	}
	
};

//remember the settings (you don't want to re-tune again and again...)


static void saveCurrentSettingsVR(const btVector3& VRTeleportPos1)
{
	FILE* f = fopen(startFileNameVR, "w");
	if (f)
	{
		fprintf(f, "--camPosX= %f\n", VRTeleportPos1[0]);
		fprintf(f, "--camPosY= %f\n", VRTeleportPos1[1]);
		fprintf(f, "--camPosZ= %f\n", VRTeleportPos1[2]);
		fprintf(f, "--camRotZ= %f\n", gVRTeleportRotZ);
		fclose(f);
	}
};
bool gDebugRenderToggle  = false;
void	MotionThreadFunc(void* userPtr,void* lsMemory);
void*	MotionlsMemoryFunc();
void	MotionlsMemoryReleaseFunc(void* ptr);
#define MAX_MOTION_NUM_THREADS 1
enum
	{
		numCubesX = 20,
		numCubesY = 20
	};


enum TestExampleBrowserCommunicationEnums
{
	eRequestTerminateMotion= 13,
	eMotionIsUnInitialized,
	eMotionIsInitialized,
	eMotionInitializationFailed,
	eMotionHasTerminated
};

enum MultiThreadedGUIHelperCommunicationEnums
{
	eGUIHelperIdle= 13,
	eGUIHelperRegisterTexture,
	eGUIHelperRegisterGraphicsShape,
	eGUIHelperRegisterGraphicsInstance,
	eGUIHelperCreateCollisionShapeGraphicsObject,
	eGUIHelperCreateCollisionObjectGraphicsObject,
	eGUIHelperCreateRigidBodyGraphicsObject,
	eGUIHelperRemoveAllGraphicsInstances,
	eGUIHelperCopyCameraImageData,
	eGUIHelperDisplayCameraImageData,
	eGUIHelperAutogenerateGraphicsObjects,
	eGUIUserDebugAddText,
	eGUIUserDebugAddLine,
	eGUIUserDebugAddParameter,
	eGUIUserDebugRemoveItem,
	eGUIUserDebugRemoveAllItems,
	eGUIDumpFramesToVideo,
	eGUIHelperRemoveGraphicsInstance,
	eGUIHelperChangeGraphicsInstanceRGBAColor,
	eGUIHelperChangeGraphicsInstanceSpecularColor,
	eGUIHelperSetVisualizerFlag,
	eGUIHelperChangeGraphicsInstanceTextureId,
	eGUIHelperGetShapeIndexFromInstance,
	eGUIHelperChangeTexture,
	eGUIHelperRemoveTexture,
};


#include <stdio.h>
//#include "BulletMultiThreaded/PlatformDefinitions.h"

#ifndef _WIN32
#include "../MultiThreading/b3PosixThreadSupport.h"

b3ThreadSupportInterface* createMotionThreadSupport(int numThreads)
{
	b3PosixThreadSupport::ThreadConstructionInfo constructionInfo("MotionThreads",
                                                                MotionThreadFunc,
                                                                MotionlsMemoryFunc,
																MotionlsMemoryReleaseFunc,
                                                                numThreads);
    b3ThreadSupportInterface* threadSupport = new b3PosixThreadSupport(constructionInfo);

	return threadSupport;

}


#elif defined( _WIN32)
#include "../MultiThreading/b3Win32ThreadSupport.h"

b3ThreadSupportInterface* createMotionThreadSupport(int numThreads)
{
	b3Win32ThreadSupport::Win32ThreadConstructionInfo threadConstructionInfo("MotionThreads",MotionThreadFunc,MotionlsMemoryFunc,MotionlsMemoryReleaseFunc,numThreads);
	b3Win32ThreadSupport* threadSupport = new b3Win32ThreadSupport(threadConstructionInfo);
	return threadSupport;

}
#endif

enum MyMouseCommandType
{
	MyMouseMove = 1,
	MyMouseButtonDown,
	MyMouseButtonUp

};
struct MyMouseCommand
{
	btVector3 m_rayFrom;
	btVector3 m_rayTo;
	int m_type;
};

struct	MotionArgs
{
	MotionArgs()
		:
		m_debugDrawFlags(0),
		m_physicsServerPtr(0)
	{
		for (int i=0;i<MAX_VR_CONTROLLERS;i++)
		{
			m_vrControllerEvents[i].m_numButtonEvents = 0;
			m_vrControllerEvents[i].m_numMoveEvents = 0;
			for (int b=0;b<MAX_VR_BUTTONS;b++)
			{
				m_vrControllerEvents[i].m_buttons[b]=0;
			}
			m_vrControllerPos[i].setValue(0,0,0);
			m_vrControllerOrn[i].setValue(0,0,0,1);
			m_isVrControllerPicking[i] = false;
			m_isVrControllerDragging[i] = false;
			m_isVrControllerReleasing[i] = false;
			m_isVrControllerTeleporting[i] = false;
		}
	}
	b3CriticalSection* m_cs;
	b3CriticalSection* m_cs2;
	b3CriticalSection* m_cs3;
	b3CriticalSection* m_csGUI;

	int m_debugDrawFlags;

	btAlignedObjectArray<MyMouseCommand> m_mouseCommands;

	b3VRControllerEvent m_vrControllerEvents[MAX_VR_CONTROLLERS];
	b3VRControllerEvent m_sendVrControllerEvents[MAX_VR_CONTROLLERS];

	btAlignedObjectArray<b3KeyboardEvent> m_keyboardEvents;
	btAlignedObjectArray<b3KeyboardEvent> m_sendKeyEvents;
	btAlignedObjectArray<b3MouseEvent> m_allMouseEvents;
	btAlignedObjectArray<b3MouseEvent> m_sendMouseEvents;
	PhysicsServerSharedMemory*	m_physicsServerPtr;
	b3AlignedObjectArray<b3Vector3> m_positions;

	btVector3 m_vrControllerPos[MAX_VR_CONTROLLERS];
	btQuaternion m_vrControllerOrn[MAX_VR_CONTROLLERS];
	bool m_isVrControllerPicking[MAX_VR_CONTROLLERS];
	bool m_isVrControllerDragging[MAX_VR_CONTROLLERS];
	bool m_isVrControllerReleasing[MAX_VR_CONTROLLERS];
	bool m_isVrControllerTeleporting[MAX_VR_CONTROLLERS];
	
};

struct MotionThreadLocalStorage
{
	int threadId;
};


float clampedDeltaTime  = 0.2;


void	MotionThreadFunc(void* userPtr,void* lsMemory)
{
	printf("MotionThreadFunc thread started\n");
	//MotionThreadLocalStorage* localStorage = (MotionThreadLocalStorage*) lsMemory;

	MotionArgs* args = (MotionArgs*) userPtr;
	//int workLeft = true;
	b3Clock clock;
	clock.reset();
	b3Clock sleepClock;
	bool init = true;
	if (init)
	{

		unsigned int cachedSharedParam = eMotionIsInitialized;

		args->m_cs->lock();
		args->m_cs->setSharedParam(0,eMotionIsInitialized);
		args->m_cs->unlock();


		double deltaTimeInSeconds = 0;
		int numCmdSinceSleep1ms = 0;
		unsigned long long int prevTime = clock.getTimeMicroseconds();
		
		

		do
		{
			{
				b3Clock::usleep(0);
			}

			{
				

				if (sleepClock.getTimeMilliseconds()>1)
				{
					sleepClock.reset();
					numCmdSinceSleep1ms = 0;
				}
			}

			unsigned long long int curTime = clock.getTimeMicroseconds();
			unsigned long long int dtMicro = curTime - prevTime;
			prevTime = curTime;
#if 1
			double dt = double(dtMicro)/1000000.;
#else
			double dt = double(clock.getTimeMicroseconds())/1000000.;
			clock.reset();
#endif
			deltaTimeInSeconds+= dt;
			
			{

				//process special controller commands, such as
				//VR controller button press/release and controller motion

				for (int c=0;c<MAX_VR_CONTROLLERS;c++)
				{
				
					btVector3 from = args->m_vrControllerPos[c];
					btMatrix3x3 mat(args->m_vrControllerOrn[c]);
				
					btScalar pickDistance = 1000.;
					btVector3 to = from+mat.getColumn(0)*pickDistance;
//					btVector3 toY = from+mat.getColumn(1)*pickDistance;
//					btVector3 toZ = from+mat.getColumn(2)*pickDistance;

					if (args->m_isVrControllerTeleporting[c])
					{
						args->m_isVrControllerTeleporting[c] = false;
						args->m_physicsServerPtr->pickBody(from, to);
						args->m_physicsServerPtr->removePickingConstraint();
					}

//					if (!gEnableKukaControl)
					{
						if (args->m_isVrControllerPicking[c])
						{
							args->m_isVrControllerPicking[c]  = false;
							args->m_isVrControllerDragging[c] = true;
							args->m_physicsServerPtr->pickBody(from, to);
							//printf("PICK!\n");
						}
					}

					 if (args->m_isVrControllerDragging[c])
					 {
						 args->m_physicsServerPtr->movePickedBody(from, to);
						// printf(".");
					 }
				
					if (args->m_isVrControllerReleasing[c])
					{
						args->m_isVrControllerDragging[c] = false;
						args->m_isVrControllerReleasing[c] = false;
						args->m_physicsServerPtr->removePickingConstraint();
						//printf("Release pick\n");
					}
				}

				//don't simulate over a huge timestep if we had some interruption (debugger breakpoint etc)
				if (deltaTimeInSeconds>clampedDeltaTime)
				{
					deltaTimeInSeconds = clampedDeltaTime;
					//b3Warning("Clamp deltaTime from %f to %f",deltaTimeInSeconds, clampedDeltaTime);
				}
				
				

				args->m_csGUI->lock();

				int numSendVrControllers = 0;
				for (int i=0;i<MAX_VR_CONTROLLERS;i++)
				{
					if (args->m_vrControllerEvents[i].m_numButtonEvents+args->m_vrControllerEvents[i].m_numMoveEvents)
					{
						args->m_sendVrControllerEvents[numSendVrControllers++] =
							args->m_vrControllerEvents[i];


						if (args->m_vrControllerEvents[i].m_numButtonEvents)
						{
							for (int b=0;b<MAX_VR_BUTTONS;b++)
							{
								args->m_vrControllerEvents[i].m_buttons[b] &= eButtonIsDown;
							}
						}
						args->m_vrControllerEvents[i].m_numMoveEvents = 0;
						args->m_vrControllerEvents[i].m_numButtonEvents = 0;
					}
				}

				args->m_sendKeyEvents.resize(args->m_keyboardEvents.size());
				for (int i=0;i<args->m_keyboardEvents.size();i++)
				{
					args->m_sendKeyEvents[i] = args->m_keyboardEvents[i];
					if (args->m_keyboardEvents[i].m_keyState&eButtonReleased)
					{
						args->m_keyboardEvents[i].m_keyState = 0;
					} else
					{
						args->m_keyboardEvents[i].m_keyState &= ~eButtonTriggered;
					}
				}
				//remove the 'released' events
				for (int i=args->m_keyboardEvents.size()-1;i>=0;i--)
				{
					if (args->m_keyboardEvents[i].m_keyState==0)
					{
						args->m_keyboardEvents.removeAtIndex(i);
					}
				}

				b3KeyboardEvent* keyEvents = args->m_sendKeyEvents.size()? &args->m_sendKeyEvents[0] : 0;

				args->m_csGUI->unlock();

				args->m_csGUI->lock();
				if (gEnableDefaultMousePicking)
				{
					for (int i = 0; i < args->m_mouseCommands.size(); i++)
					{
						switch (args->m_mouseCommands[i].m_type)
						{
						case MyMouseMove:
						{
							args->m_physicsServerPtr->movePickedBody(args->m_mouseCommands[i].m_rayFrom, args->m_mouseCommands[i].m_rayTo);
							break;
						};
						case MyMouseButtonDown:
						{
							args->m_physicsServerPtr->pickBody(args->m_mouseCommands[i].m_rayFrom, args->m_mouseCommands[i].m_rayTo);
							break;
						}
						case MyMouseButtonUp:
						{
							args->m_physicsServerPtr->removePickingConstraint();
							break;
						}

						default:
						{
						}
						}
					}
				}

				
				args->m_sendMouseEvents.resize(args->m_allMouseEvents.size());
				for (int i=0;i<args->m_allMouseEvents.size();i++)
				{
					args->m_sendMouseEvents[i] = args->m_allMouseEvents[i];
				}
				b3MouseEvent* mouseEvents = args->m_sendMouseEvents.size()? &args->m_sendMouseEvents[0] : 0;

				args->m_allMouseEvents.clear();
				args->m_mouseCommands.clear();
				args->m_csGUI->unlock();


				{
					args->m_physicsServerPtr->stepSimulationRealTime(deltaTimeInSeconds, args->m_sendVrControllerEvents,numSendVrControllers, keyEvents, args->m_sendKeyEvents.size(), mouseEvents, args->m_sendMouseEvents.size());
				}
				{
					if (gEnableUpdateDebugDrawLines)
					{
						args->m_csGUI->lock();
							args->m_physicsServerPtr->physicsDebugDraw(args->m_debugDrawFlags);
							gEnableUpdateDebugDrawLines=false;
						args->m_csGUI->unlock();
					}
				}
				deltaTimeInSeconds = 0;
				
			}

			

			{
				args->m_physicsServerPtr->processClientCommands();
				numCmdSinceSleep1ms++;
			}
			
			args->m_cs->lock();
			cachedSharedParam = args->m_cs->getSharedParam(0);
			args->m_cs->unlock();

		} while (cachedSharedParam!=eRequestTerminateMotion);
	} else
	{
		args->m_cs->lock();
		args->m_cs->setSharedParam(0,eMotionInitializationFailed);
		args->m_cs->unlock();
	}

	args->m_physicsServerPtr->disconnectSharedMemory(true);
	//do nothing

}



void*	MotionlsMemoryFunc()
{
	//don't create local store memory, just return 0
	return new MotionThreadLocalStorage;
}

void	MotionlsMemoryReleaseFunc(void* ptr)
{
	MotionThreadLocalStorage* p = (MotionThreadLocalStorage*) ptr;
	delete p;
}



struct UserDebugDrawLine
{
	double	m_debugLineFromXYZ[3];
	double	m_debugLineToXYZ[3];
	double	m_debugLineColorRGB[3];
	double	m_lineWidth;
	
	double m_lifeTime;
	int m_itemUniqueId;
	int m_trackingVisualShapeIndex;
	int m_replaceItemUid;
};

struct UserDebugParameter
{
	char m_text[1024];
	double m_rangeMin;
	double m_rangeMax;
	btScalar m_value;
	int m_itemUniqueId;
};

struct UserDebugText
{
	char m_text[1024];
	double m_textPositionXYZ1[3];
	double m_textColorRGB[3];
	double textSize;

	double m_lifeTime;
	int m_itemUniqueId;
	double m_textOrientation[4];
	int m_trackingVisualShapeIndex;
	int m_optionFlags;

};


  struct LineSegment
{
        btVector3 m_from;
        btVector3 m_to;
};

struct ColorWidth
{
        btVector3FloatData m_color;
        int width;
        int getHash() const
        {
                unsigned char r = (unsigned char) m_color.m_floats[0]*255;
                unsigned char g = (unsigned char) m_color.m_floats[1]*255;
                unsigned char b = (unsigned char) m_color.m_floats[2]*255;
                unsigned char w = width;
                return r+(256*g)+(256*256*b)+(256*256*256*w);
        }
        bool equals(const ColorWidth& other) const
        {
                bool same = ((width == other.width) && (m_color.m_floats[0] == other.m_color.m_floats[0]) &&
                        (m_color.m_floats[1] == other.m_color.m_floats[1]) &&
                        (m_color.m_floats[2] == other.m_color.m_floats[2]));
                return same;
        }
};



ATTRIBUTE_ALIGNED16( class )MultithreadedDebugDrawer : public btIDebugDraw
{
	struct GUIHelperInterface* m_guiHelper;
	int m_debugMode;

	btAlignedObjectArray< btAlignedObjectArray<unsigned int> > m_sortedIndices;
	btAlignedObjectArray< btAlignedObjectArray<btVector3FloatData> > m_sortedLines;
	btHashMap<ColorWidth,int> m_hashedLines;


	public:
	void drawDebugDrawerLines()
	{
		if (m_hashedLines.size())
		{
			for (int i=0;i<m_hashedLines.size();i++)
			{
				ColorWidth cw = m_hashedLines.getKeyAtIndex(i);
				int index = *m_hashedLines.getAtIndex(i);
				int stride = sizeof(btVector3FloatData);
				const float* positions = &m_sortedLines[index][0].m_floats[0];
				int numPoints = m_sortedLines[index].size();
				const unsigned int* indices = &m_sortedIndices[index][0];
				int numIndices = m_sortedIndices[index].size();
				m_guiHelper->getRenderInterface()->drawLines(positions,cw.m_color.m_floats,numPoints, stride, indices,numIndices,cw.width);
			}
		}
	}
	MultithreadedDebugDrawer(GUIHelperInterface* guiHelper)
		:m_guiHelper(guiHelper),
		m_debugMode(0)
	{
	}
	virtual ~MultithreadedDebugDrawer()
	{
	}
	virtual void	drawLine(const btVector3& from,const btVector3& to,const btVector3& color)
	{
		{
			ColorWidth cw;
			color.serializeFloat(cw.m_color);
			cw.width = 1;
			int index = -1;

			int* indexPtr = m_hashedLines.find(cw);
			if (indexPtr)
			{
				index = *indexPtr;
			} else
			{
				index = m_sortedLines.size();
				m_sortedLines.expand();
				m_sortedIndices.expand();
				m_hashedLines.insert(cw,index);
			}
			btAssert(index>=0);
			if (index>=0)
			{
				btVector3FloatData from1,toX1;
				m_sortedIndices[index].push_back(m_sortedLines[index].size());
				from.serializeFloat(from1);
				m_sortedLines[index].push_back(from1);
				m_sortedIndices[index].push_back(m_sortedLines[index].size());
				to.serializeFloat(toX1);
				m_sortedLines[index].push_back(toX1);
			}
		}
	}

	virtual void	drawContactPoint(const btVector3& PointOnB,const btVector3& normalOnB,btScalar distance,int lifeTime,const btVector3& color)
	{
		drawLine(PointOnB,PointOnB+normalOnB*distance,color);
		btVector3 ncolor(0, 0, 0);
		drawLine(PointOnB, PointOnB + normalOnB*0.01, ncolor);
	}

	virtual void	reportErrorWarning(const char* warningString)
	{
	}
	virtual void	draw3dText(const btVector3& location,const char* textString)
	{
	}
	virtual void	setDebugMode(int debugMode)
	{
		m_debugMode = debugMode;
	}

	virtual int		getDebugMode() const
	{
		return m_debugMode;
	}

	virtual void clearLines()
	{
		m_hashedLines.clear();
		m_sortedIndices.clear();
		m_sortedLines.clear();
	}
	virtual void flushLines()
	{
		
	}
};

class MultiThreadedOpenGLGuiHelper : public GUIHelperInterface
{
//	CommonGraphicsApp* m_app;
	
	b3CriticalSection* m_cs;
	b3CriticalSection* m_cs2;
	b3CriticalSection* m_cs3;
	b3CriticalSection* m_csGUI;
	

public:

    
   MultithreadedDebugDrawer* m_debugDraw;
   void drawDebugDrawerLines()
   {
	   if (m_debugDraw)
	   {
		   m_debugDraw->drawDebugDrawerLines();
	   }
   }
	GUIHelperInterface* m_childGuiHelper;

	int m_uidGenerator;
	const unsigned char* m_texels;
	int m_textureWidth;
	int m_textureHeight;


	int m_shapeIndex;
	const float* m_position;
	const float* m_quaternion;
	const float* m_color;
	const float* m_scaling;

	const float* m_vertices;
	int m_numvertices;
	const int* m_indices;
	int m_numIndices;
	int m_primitiveType;
	int m_textureId;
	int m_instanceId;
    bool m_skipGraphicsUpdate;
    
	void mainThreadRelease()
	{
		BT_PROFILE("mainThreadRelease");

		getCriticalSection()->setSharedParam(1,eGUIHelperIdle);
		getCriticalSection3()->lock();
		getCriticalSection2()->unlock();
		getCriticalSection()->lock();
		getCriticalSection2()->lock();
		getCriticalSection()->unlock();
		getCriticalSection3()->unlock();

	}

	void workerThreadWait()
	{
        BT_PROFILE("workerThreadWait");

        if (m_skipGraphicsUpdate)
        {
            getCriticalSection()->setSharedParam(1,eGUIHelperIdle);
            m_cs->unlock();
            return;
        }
		m_cs2->lock();
		m_cs->unlock();
		m_cs2->unlock();
		m_cs3->lock();
		m_cs3->unlock();

		
		
		while (m_cs->getSharedParam(1)!=eGUIHelperIdle)
		{
			b3Clock::usleep(0);
		}
	}

	MultiThreadedOpenGLGuiHelper(CommonGraphicsApp* app, GUIHelperInterface* guiHelper, int skipGraphicsUpdate)
		:
	//m_app(app),
		m_cs(0),
		m_cs2(0),
		m_cs3(0),
		m_csGUI(0),
		m_debugDraw(0),
		m_uidGenerator(0),
		m_texels(0),
        m_shapeIndex(-1),
        m_textureId(-1),
        m_instanceId(-1),
        m_skipGraphicsUpdate(skipGraphicsUpdate)
	{
		m_childGuiHelper = guiHelper;

	}

	virtual ~MultiThreadedOpenGLGuiHelper()
	{
		//delete m_childGuiHelper;
		if (m_debugDraw)
		{
			delete m_debugDraw;
			m_debugDraw = 0;
		}
		
		for (int i=0;i<m_userDebugParams.size();i++)
		{
			delete m_userDebugParams[i];
		}
		m_userDebugParams.clear();
	}

	void setCriticalSection(b3CriticalSection* cs)
	{
		m_cs = cs;
	}

	b3CriticalSection* getCriticalSection()
	{
		return m_cs;
	}


	void setCriticalSection2(b3CriticalSection* cs)
	{
		m_cs2 = cs;
	}

	b3CriticalSection* getCriticalSection2()
	{
		return m_cs2;
	}

	void setCriticalSection3(b3CriticalSection* cs)
	{
		m_cs3 = cs;
	}

	void setCriticalSectionGUI(b3CriticalSection* cs)
	{
		m_csGUI  = cs;
	}


	b3CriticalSection* getCriticalSection3()
	{
		return m_cs3;
	}
	btRigidBody* m_body;
	btVector3 m_color3;
	virtual void createRigidBodyGraphicsObject(btRigidBody* body,const btVector3& color)
	{
		m_body = body;
		m_color3 = color;
		m_cs->lock();
		m_cs->setSharedParam(1,eGUIHelperCreateRigidBodyGraphicsObject);
		workerThreadWait();

	}

	btCollisionObject* m_obj;
	btVector3 m_color2;

	virtual void createCollisionObjectGraphicsObject(btCollisionObject* obj,const btVector3& color)
	{
		m_obj = obj;
		m_color2 = color;
		m_cs->lock();
		m_cs->setSharedParam(1,eGUIHelperCreateCollisionObjectGraphicsObject);
		workerThreadWait();

	}

	btCollisionShape* m_colShape;
	virtual void createCollisionShapeGraphicsObject(btCollisionShape* collisionShape)
	{
		m_colShape = collisionShape;
		m_cs->lock();
		m_cs->setSharedParam(1,eGUIHelperCreateCollisionShapeGraphicsObject);
		workerThreadWait();

	}

	virtual void syncPhysicsToGraphics(const btDiscreteDynamicsWorld* rbWorld)
	{
	    //this check is to prevent a crash, in case we removed all graphics instances, but there are still physics objects.
	    //the check will be obsolete, once we have a better/safer way of synchronizing physics->graphics transforms
        if ( m_childGuiHelper->getRenderInterface() && m_childGuiHelper->getRenderInterface()->getTotalNumInstances()>0)
        {
            m_childGuiHelper->syncPhysicsToGraphics(rbWorld);
        }
	}

	virtual void render(const btDiscreteDynamicsWorld* rbWorld) 
	{
		m_childGuiHelper->render(0);
	}

	virtual void createPhysicsDebugDrawer( btDiscreteDynamicsWorld* rbWorld)
	{
		btAssert(rbWorld);
		if (m_debugDraw)
		{
			delete m_debugDraw;
			m_debugDraw = 0;
		}

		m_debugDraw = new MultithreadedDebugDrawer(this);

		rbWorld->setDebugDrawer(m_debugDraw );

		//m_childGuiHelper->createPhysicsDebugDrawer(rbWorld);
	}

	int m_removeTextureUid;

	virtual void removeTexture(int textureUid)
	{
		m_removeTextureUid = textureUid;
		m_cs->lock();
		m_cs->setSharedParam(1, eGUIHelperRemoveTexture);

		workerThreadWait();
	}

	virtual int	registerTexture(const unsigned char* texels, int width, int height)
	{
		m_texels = texels;
		m_textureWidth = width;
		m_textureHeight = height;

		m_cs->lock();
		m_cs->setSharedParam(1,eGUIHelperRegisterTexture);
		
		workerThreadWait();


		return m_textureId;
	}
	virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId)
	{
		m_vertices = vertices;
		m_numvertices = numvertices;
		m_indices = indices;
		m_numIndices = numIndices;
		m_primitiveType = primitiveType;
		m_textureId = textureId;

		m_cs->lock();
		m_cs->setSharedParam(1,eGUIHelperRegisterGraphicsShape);
		workerThreadWait();

		return m_shapeIndex;
	}

	int m_visualizerFlag;
	int m_visualizerEnable;
	 void setVisualizerFlag(int flag, int enable)
    {
		m_visualizerFlag = flag;
		m_visualizerEnable = enable;

		m_cs->lock();
		m_cs->setSharedParam(1,eGUIHelperSetVisualizerFlag);
		workerThreadWait();
    }

    void	setVisualizerFlagCallback(VisualizerFlagCallback callback)
	{
		m_childGuiHelper->setVisualizerFlagCallback(callback);
	}


	virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling) 
	{
		m_shapeIndex = shapeIndex;
		m_position = position;
		m_quaternion = quaternion;
		m_color = color;
		m_scaling = scaling;

		m_cs->lock();
		m_cs->setSharedParam(1,eGUIHelperRegisterGraphicsInstance);
		workerThreadWait();
		return m_instanceId;
	}

    virtual void removeAllGraphicsInstances()
    {
        m_cs->lock();
		m_cs->setSharedParam(1,eGUIHelperRemoveAllGraphicsInstances);
		workerThreadWait();
    }

	int m_graphicsInstanceRemove;
	virtual void removeGraphicsInstance(int graphicsUid)
	{
		m_graphicsInstanceRemove = graphicsUid;
		m_cs->lock();
		m_cs->setSharedParam(1,eGUIHelperRemoveGraphicsInstance);
		workerThreadWait();    
	}
	
	int m_getShapeIndex_instance;
	int getShapeIndex_shapeIndex;
		
	virtual int getShapeIndexFromInstance(int instance) 
	{
		m_getShapeIndex_instance = instance;
		m_cs->lock();
		m_cs->setSharedParam(1,eGUIHelperGetShapeIndexFromInstance);
        getShapeIndex_shapeIndex=-1;
		workerThreadWait();		
		return getShapeIndex_shapeIndex;
	}

	int m_graphicsInstanceChangeTextureId;
	int m_graphicsInstanceChangeTextureShapeIndex;
	virtual void replaceTexture(int shapeIndex, int textureUid)
	{
		m_graphicsInstanceChangeTextureShapeIndex = shapeIndex;
		m_graphicsInstanceChangeTextureId = textureUid;
		m_cs->lock();
		m_cs->setSharedParam(1,eGUIHelperChangeGraphicsInstanceTextureId);
		workerThreadWait();   
	}


	int m_changeTextureUniqueId;
	const unsigned char* m_changeTextureRgbTexels;
	int m_changeTextureWidth;
	int m_changeTextureHeight;

	virtual void changeTexture(int textureUniqueId, const unsigned char* rgbTexels, int width, int height)
	{
		m_changeTextureUniqueId = textureUniqueId;
		m_changeTextureRgbTexels = rgbTexels;
		m_changeTextureWidth = width;
		m_changeTextureHeight = height;
		m_cs->lock();
		m_cs->setSharedParam(1,eGUIHelperChangeTexture);
		workerThreadWait();   
	}

	double m_rgbaColor[4];
	int m_graphicsInstanceChangeColor;
	virtual void changeRGBAColor(int instanceUid, const double rgbaColor[4])
	{
		m_graphicsInstanceChangeColor = instanceUid;
		m_rgbaColor[0] = rgbaColor[0];
		m_rgbaColor[1] = rgbaColor[1];
		m_rgbaColor[2] = rgbaColor[2];
		m_rgbaColor[3] = rgbaColor[3];
		m_cs->lock();
		m_cs->setSharedParam(1,eGUIHelperChangeGraphicsInstanceRGBAColor);
		workerThreadWait();    
	}

	double m_specularColor[3];
	int m_graphicsInstanceChangeSpecular;
	virtual void changeSpecularColor(int instanceUid, const double specularColor[3])
	{
		m_graphicsInstanceChangeSpecular = instanceUid;
		m_specularColor[0] = specularColor[0];
		m_specularColor[1] = specularColor[1];
		m_specularColor[2] = specularColor[2];
		m_cs->lock();
		m_cs->setSharedParam(1,eGUIHelperChangeGraphicsInstanceSpecularColor);
		workerThreadWait(); 
	}

	virtual Common2dCanvasInterface* get2dCanvasInterface()
	{
		return m_childGuiHelper->get2dCanvasInterface();
	}
	
	virtual CommonParameterInterface* getParameterInterface()
	{
		return 0;
	}

	virtual CommonRenderInterface* getRenderInterface()
	{
		return m_childGuiHelper->getRenderInterface();
	}
	
	virtual CommonGraphicsApp* getAppInterface()
	{
		return m_childGuiHelper->getAppInterface();
	}


	virtual void setUpAxis(int axis)
	{
		m_childGuiHelper->setUpAxis(axis);
	}
	virtual void resetCamera(float camDist, float yaw, float pitch, float camPosX,float camPosY, float camPosZ)
	{
	    m_childGuiHelper->resetCamera(camDist,yaw,pitch,camPosX,camPosY,camPosZ);
	}

	virtual bool getCameraInfo(int* width, int* height, float viewMatrix[16], float projectionMatrix[16], float camUp[3], float camForward[3],float hor[3], float vert[3], float* yaw, float* pitch, float* camDist, float camTarget[3]) const
	{
		return m_childGuiHelper->getCameraInfo(width,height,viewMatrix,projectionMatrix,camUp,camForward,hor,vert,yaw,pitch,camDist,camTarget);
	}


	float m_viewMatrix[16];
    float m_projectionMatrix[16];
    unsigned char* m_pixelsRGBA;
    int m_rgbaBufferSizeInPixels;
    float* m_depthBuffer;
    int m_depthBufferSizeInPixels;
     int* m_segmentationMaskBuffer;
    int m_segmentationMaskBufferSizeInPixels;
    int m_startPixelIndex;
    int m_destinationWidth;
    int m_destinationHeight;
    int* m_numPixelsCopied;

	virtual void copyCameraImageData(const float viewMatrix[16], const float projectionMatrix[16], 
                                  unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, 
                                  float* depthBuffer, int depthBufferSizeInPixels,
                                  int* segmentationMaskBuffer, int segmentationMaskBufferSizeInPixels,
                                  int startPixelIndex, int destinationWidth, 
                                  int destinationHeight, int* numPixelsCopied)
	{
	    m_cs->lock();
	    for (int i=0;i<16;i++)
        {
            m_viewMatrix[i] = viewMatrix[i];
            m_projectionMatrix[i] = projectionMatrix[i];
        }
	    m_pixelsRGBA = pixelsRGBA;
        m_rgbaBufferSizeInPixels = rgbaBufferSizeInPixels;
        m_depthBuffer = depthBuffer;
        m_depthBufferSizeInPixels = depthBufferSizeInPixels;
        m_segmentationMaskBuffer = segmentationMaskBuffer;
        m_segmentationMaskBufferSizeInPixels = segmentationMaskBufferSizeInPixels;
        m_startPixelIndex = startPixelIndex;
        m_destinationWidth = destinationWidth;
        m_destinationHeight = destinationHeight;
        m_numPixelsCopied = numPixelsCopied;
	    
		m_cs->setSharedParam(1,eGUIHelperCopyCameraImageData);
		workerThreadWait();
	}
	
	virtual void debugDisplayCameraImageData(const float viewMatrix[16], const float projectionMatrix[16], 
                                  unsigned char* pixelsRGBA, int rgbaBufferSizeInPixels, 
                                  float* depthBuffer, int depthBufferSizeInPixels, 
                                  int* segmentationMaskBuffer, int segmentationMaskBufferSizeInPixels,
		  int startPixelIndex, int destinationWidth, int destinationHeight, int* numPixelsCopied)
	{
		 m_cs->lock();
	    for (int i=0;i<16;i++)
        {
            m_viewMatrix[i] = viewMatrix[i];
            m_projectionMatrix[i] = projectionMatrix[i];
        }
	    m_pixelsRGBA = pixelsRGBA;
        m_rgbaBufferSizeInPixels = rgbaBufferSizeInPixels;
        m_depthBuffer = depthBuffer;
        m_depthBufferSizeInPixels = depthBufferSizeInPixels;
        m_segmentationMaskBuffer = segmentationMaskBuffer;
        m_segmentationMaskBufferSizeInPixels = segmentationMaskBufferSizeInPixels;
        m_startPixelIndex = startPixelIndex;
        m_destinationWidth = destinationWidth;
        m_destinationHeight = destinationHeight;
        m_numPixelsCopied = numPixelsCopied;
	    
		m_cs->setSharedParam(1,eGUIHelperDisplayCameraImageData);
		workerThreadWait();
	}
	
	virtual void setProjectiveTextureMatrices(const float viewMatrix[16], const float projectionMatrix[16])
	{
		if (m_childGuiHelper->getAppInterface() && m_childGuiHelper->getAppInterface()->m_renderer)
		{
			m_childGuiHelper->getAppInterface()->m_renderer->setProjectiveTextureMatrices(viewMatrix, projectionMatrix);
		}
	}
	
	virtual void setProjectiveTexture(bool useProjectiveTexture)
	{
		if (m_childGuiHelper->getAppInterface() && m_childGuiHelper->getAppInterface()->m_renderer)
		{
			m_childGuiHelper->getAppInterface()->m_renderer->setProjectiveTexture(useProjectiveTexture);
		}
	}

	btDiscreteDynamicsWorld* m_dynamicsWorld;

	virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld) 
	{
		m_dynamicsWorld = rbWorld;
		m_cs->lock();
		m_cs->setSharedParam(1, eGUIHelperAutogenerateGraphicsObjects);
		workerThreadWait();
	}
    
	virtual void drawText3D( const char* txt, float posX, float posZY, float posZ, float size)
	{
	}

	virtual void drawText3D( const char* txt, float position[3], float orientation[4], float color[4], float size, int optionFlag)
	{
	}


	btAlignedObjectArray<UserDebugText> m_userDebugText;
	
	UserDebugText m_tmpText;
	int m_resultUserDebugTextUid;
	
	virtual int		addUserDebugText3D( const char* txt, const double positionXYZ[3], const double orientation[4], const double	textColorRGB[3], double size, double lifeTime, int trackingVisualShapeIndex, int optionFlags, int replaceItemUid)
	{
		
		if (replaceItemUid>=0)
		{
			m_tmpText.m_itemUniqueId = replaceItemUid;
		} else
		{
			m_tmpText.m_itemUniqueId = m_uidGenerator++;
		}
		m_tmpText.m_lifeTime = lifeTime;
		m_tmpText.textSize = size;
		//int len = strlen(txt);
		strcpy(m_tmpText.m_text,txt);
		m_tmpText.m_textPositionXYZ1[0] = positionXYZ[0];
		m_tmpText.m_textPositionXYZ1[1] = positionXYZ[1];
		m_tmpText.m_textPositionXYZ1[2] = positionXYZ[2];

		m_tmpText.m_textOrientation[0] = orientation[0];
		m_tmpText.m_textOrientation[1] = orientation[1];
		m_tmpText.m_textOrientation[2] = orientation[2];
		m_tmpText.m_textOrientation[3] = orientation[3];

		m_tmpText.m_textColorRGB[0] = textColorRGB[0];
		m_tmpText.m_textColorRGB[1] = textColorRGB[1];
		m_tmpText.m_textColorRGB[2] = textColorRGB[2];

		m_tmpText.m_trackingVisualShapeIndex = trackingVisualShapeIndex;

		m_tmpText.m_optionFlags = optionFlags;
		m_tmpText.m_textOrientation[0] = orientation[0];
		m_tmpText.m_textOrientation[1] = orientation[1];
		m_tmpText.m_textOrientation[2] = orientation[2];
		m_tmpText.m_textOrientation[3] = orientation[3];

		m_cs->lock();
		m_cs->setSharedParam(1, eGUIUserDebugAddText);
        m_resultUserDebugTextUid=-1;
		workerThreadWait();

		return m_resultUserDebugTextUid;
	}

	btAlignedObjectArray<UserDebugParameter*> m_userDebugParams;
	UserDebugParameter m_tmpParam;

	virtual int		readUserDebugParameter(int itemUniqueId, double* value) 
	{ 
		for (int i=0;i<m_userDebugParams.size();i++)
		{
			if (m_userDebugParams[i]->m_itemUniqueId == itemUniqueId)
			{
				*value = m_userDebugParams[i]->m_value;
				return 1;
			}
		}
		return 0;
	}
	int m_userDebugParamUid;

	virtual int		addUserDebugParameter(const char* txt, double	rangeMin, double	rangeMax, double startValue)
	{
		strcpy(m_tmpParam.m_text,txt);
		m_tmpParam.m_rangeMin = rangeMin;
		m_tmpParam.m_rangeMax = rangeMax;
		m_tmpParam.m_value = startValue;
		m_tmpParam.m_itemUniqueId = m_uidGenerator++;

		m_cs->lock();
		m_cs->setSharedParam(1, eGUIUserDebugAddParameter);
        m_userDebugParamUid=-1;
		workerThreadWait();

		return m_userDebugParamUid;
	}


	btAlignedObjectArray<UserDebugDrawLine> m_userDebugLines;
	UserDebugDrawLine m_tmpLine;
	int m_resultDebugLineUid;
	
	
	virtual int		addUserDebugLine(const double	debugLineFromXYZ[3], const double	debugLineToXYZ[3], const double	debugLineColorRGB[3], double lineWidth, double lifeTime , int trackingVisualShapeIndex, int replaceItemUid)
	{
		m_tmpLine.m_lifeTime = lifeTime;
		m_tmpLine.m_lineWidth = lineWidth;
		
		m_tmpLine.m_itemUniqueId = replaceItemUid<0? m_uidGenerator++ : replaceItemUid;
		m_tmpLine.m_debugLineFromXYZ[0] = debugLineFromXYZ[0];
		m_tmpLine.m_debugLineFromXYZ[1] = debugLineFromXYZ[1];
		m_tmpLine.m_debugLineFromXYZ[2] = debugLineFromXYZ[2];

		m_tmpLine.m_debugLineToXYZ[0] = debugLineToXYZ[0];
		m_tmpLine.m_debugLineToXYZ[1] = debugLineToXYZ[1];
		m_tmpLine.m_debugLineToXYZ[2] = debugLineToXYZ[2];
		
		m_tmpLine.m_debugLineColorRGB[0] = debugLineColorRGB[0];
		m_tmpLine.m_debugLineColorRGB[1] = debugLineColorRGB[1];
		m_tmpLine.m_debugLineColorRGB[2] = debugLineColorRGB[2];
		m_tmpLine.m_trackingVisualShapeIndex = trackingVisualShapeIndex;
		m_tmpLine.m_replaceItemUid = replaceItemUid;
		m_cs->lock();
		m_cs->setSharedParam(1, eGUIUserDebugAddLine);
        m_resultDebugLineUid=-1;
		workerThreadWait();
		return m_resultDebugLineUid;
	}

	int m_removeDebugItemUid;

	virtual void	removeUserDebugItem( int debugItemUniqueId)
	{
		m_removeDebugItemUid = debugItemUniqueId;
		m_cs->lock();
		m_cs->setSharedParam(1, eGUIUserDebugRemoveItem);
		workerThreadWait();

	}	
	virtual void	removeAllUserDebugItems( )
	{
		m_cs->lock();
		m_cs->setSharedParam(1, eGUIUserDebugRemoveAllItems);
		workerThreadWait();
	
	}

	



	const char* m_mp4FileName;
	virtual void	dumpFramesToVideo(const char* mp4FileName)
	{
		m_cs->lock();
		m_mp4FileName = mp4FileName;
		m_cs->setSharedParam(1, eGUIDumpFramesToVideo);
		workerThreadWait();
		m_mp4FileName = 0;
	}

};



class PhysicsServerExample : public SharedMemoryCommon
{
	PhysicsServerSharedMemory	m_physicsServer;
	b3ThreadSupportInterface* m_threadSupport;
	MotionArgs m_args[MAX_MOTION_NUM_THREADS];
	MultiThreadedOpenGLGuiHelper* m_multiThreadedHelper;
    bool m_wantsShutdown;

    bool m_isConnected;
    btClock m_clock;
	bool m_replay;

	struct Common2dCanvasInterface* m_canvas;
	int m_canvasRGBIndex;
	int m_canvasDepthIndex;
	int m_canvasSegMaskIndex;

//	int m_options;
	
#ifdef BT_ENABLE_VR
	TinyVRGui* m_tinyVrGui;
#endif

public:

	PhysicsServerExample(MultiThreadedOpenGLGuiHelper* helper, CommandProcessorCreationInterface* commandProcessorCreator, SharedMemoryInterface* sharedMem=0, int options=0);

	virtual ~PhysicsServerExample();

	virtual void	initPhysics();

	virtual void	stepSimulation(float deltaTime);

	virtual void updateGraphics();

    void enableCommandLogging()
	{
		m_physicsServer.enableCommandLogging(true,"BulletPhysicsCommandLog.bin");
	}

	void replayFromLogFile()
	{
		m_replay = true;
		m_physicsServer.replayFromLogFile("BulletPhysicsCommandLog.bin");
	}



	virtual void resetCamera()
	{
		float dist = 5;
		float pitch = -35;
		float yaw = 50;
		float targetPos[3]={0,0,0};//-3,2.8,-2.5};
		m_guiHelper->resetCamera(dist,yaw,pitch,targetPos[0],targetPos[1],targetPos[2]);
	}

    virtual bool wantsTermination();
    virtual bool isConnected();
	virtual void	renderScene();
	void drawUserDebugLines();
	virtual void    exitPhysics();

	virtual void	physicsDebugDraw(int debugFlags);

	btVector3	getRayTo(int x,int y);

	virtual void	vrControllerButtonCallback(int controllerId, int button, int state, float pos[4], float orn[4]);
	virtual void	vrControllerMoveCallback(int controllerId, float pos[4], float orn[4], float analogAxis, float auxAnalogAxes[10]);
	virtual void	vrHMDMoveCallback(int controllerId, float pos[4], float orientation[4]);
	virtual void	vrGenericTrackerMoveCallback(int controllerId, float pos[4], float orientation[4]);


	virtual bool	mouseMoveCallback(float x,float y)
	{
		if (m_replay)
			return false;

		CommonRenderInterface* renderer = m_multiThreadedHelper->m_childGuiHelper->getRenderInterface();// m_guiHelper->getRenderInterface();
	
		if (!renderer)
		{
			return false;
		}

		b3MouseEvent event;
		event.m_buttonState = 0;
		event.m_buttonIndex = -1;
		event.m_mousePosX = x;
		event.m_mousePosY = y;
		event.m_eventType = MOUSE_MOVE_EVENT;
		m_args[0].m_csGUI->lock();
		m_args[0].m_allMouseEvents.push_back(event);
		m_args[0].m_csGUI->unlock();

		btVector3 rayTo = getRayTo(int(x), int(y));
		btVector3 rayFrom;
		renderer->getActiveCamera()->getCameraPosition(rayFrom);

		MyMouseCommand cmd;
		cmd.m_rayFrom = rayFrom;
		cmd.m_rayTo = rayTo;
		cmd.m_type = MyMouseMove;
		m_args[0].m_csGUI->lock();
		m_args[0].m_mouseCommands.push_back(cmd);
		m_args[0].m_csGUI->unlock();
		
		return false;
	};

	virtual bool	mouseButtonCallback(int button, int state, float x, float y)
	{
		if (m_replay)
			return false;

		CommonRenderInterface* renderer = m_guiHelper->getRenderInterface();

		if (!renderer)
		{
			return false;
		}

		CommonWindowInterface* window = m_guiHelper->getAppInterface()->m_window;

		b3MouseEvent event;
		event.m_buttonIndex = button;
		event.m_mousePosX = x;
		event.m_mousePosY = y;
		event.m_eventType = MOUSE_BUTTON_EVENT;
		if (state)
		{
			event.m_buttonState = eButtonIsDown + eButtonTriggered;
		} else
		{
			event.m_buttonState = eButtonReleased;
		}
		
		m_args[0].m_csGUI->lock();
		m_args[0].m_allMouseEvents.push_back(event);
		m_args[0].m_csGUI->unlock();

		if (state==1)
		{
			if(button==0 && (!window->isModifierKeyPressed(B3G_ALT) && !window->isModifierKeyPressed(B3G_CONTROL) ))
			{
				btVector3 camPos;
				renderer->getActiveCamera()->getCameraPosition(camPos);

				btVector3 rayFrom = camPos;
				btVector3 rayTo = getRayTo(int(x),int(y));

				MyMouseCommand cmd;
				cmd.m_rayFrom = rayFrom;
				cmd.m_rayTo = rayTo;
				cmd.m_type = MyMouseButtonDown;
				
				m_args[0].m_csGUI->lock();
				m_args[0].m_mouseCommands.push_back(cmd);
				m_args[0].m_csGUI->unlock();
				

			}
		} else
		{
			if (button==0)
			{
				//m_physicsServer.removePickingConstraint();
				MyMouseCommand cmd;
				cmd.m_rayFrom.setValue(0,0,0);
				cmd.m_rayTo.setValue(0, 0, 0);
				cmd.m_type = MyMouseButtonUp;
				
				m_args[0].m_csGUI->lock();
				m_args[0].m_mouseCommands.push_back(cmd);
				m_args[0].m_csGUI->unlock();
				//remove p2p
			}
		}

		//printf("button=%d, state=%d\n",button,state);
		return false;
	}
	virtual bool	keyboardCallback(int key, int state){

		//printf("key=%d, state=%d\n", key,state);
		{
			int keyIndex = -1;
			//is already there?
			for (int i=0;i<m_args[0].m_keyboardEvents.size();i++)
			{
				if (m_args[0].m_keyboardEvents[i].m_keyCode == key)
				{
					keyIndex = i;
					break;
				}
			}

			if (state)
			{
				b3KeyboardEvent ev;
				ev.m_keyCode = key;
				ev.m_keyState = eButtonIsDown+eButtonTriggered;
				m_args[0].m_csGUI->lock();
				if (keyIndex>=0)
				{
					if (0==(m_args[0].m_keyboardEvents[keyIndex].m_keyState & eButtonIsDown))
					{
						m_args[0].m_keyboardEvents[keyIndex] = ev;
					}
				} else
				{
					m_args[0].m_keyboardEvents.push_back(ev);
				}
				m_args[0].m_csGUI->unlock();
			} else
			{
				m_args[0].m_csGUI->lock();
				b3KeyboardEvent ev;
				ev.m_keyCode = key;
				ev.m_keyState = eButtonReleased;
				if (keyIndex>=0)
				{
					m_args[0].m_keyboardEvents[keyIndex]=ev;
				} else
				{
					m_args[0].m_keyboardEvents.push_back(ev);
				}
				m_args[0].m_csGUI->unlock();
			}
		}
		/*printf("m_args[0].m_keyboardEvents.size()=%d\n", m_args[0].m_keyboardEvents.size());
		for (int i=0;i<m_args[0].m_keyboardEvents.size();i++)
		{
			printf("key[%d]=%d state = %d\n",i,m_args[0].m_keyboardEvents[i].m_keyCode,m_args[0].m_keyboardEvents[i].m_keyState);
		}
		*/
		double shift=0.1;

		CommonWindowInterface* window = m_guiHelper->getAppInterface()->m_window;
		if (window->isModifierKeyPressed(B3G_SHIFT))
			shift=0.01;

		btVector3 VRTeleportPos =this->m_physicsServer.getVRTeleportPosition();

		if (gEnableDefaultKeyboardShortcuts)
		{
			if (key=='w' && state)
			{
				VRTeleportPos[0]+=shift;
				m_physicsServer.setVRTeleportPosition(VRTeleportPos);
				saveCurrentSettingsVR(VRTeleportPos);
			}
			if (key=='s' && state)
			{
				VRTeleportPos[0]-=shift;
				m_physicsServer.setVRTeleportPosition(VRTeleportPos);
				saveCurrentSettingsVR(VRTeleportPos);
			}
			if (key=='a' && state)
			{
				VRTeleportPos[1]-=shift;
				m_physicsServer.setVRTeleportPosition(VRTeleportPos);
				saveCurrentSettingsVR(VRTeleportPos);
			}
			if (key=='d' && state)
			{
				VRTeleportPos[1]+=shift;
				m_physicsServer.setVRTeleportPosition(VRTeleportPos);
				saveCurrentSettingsVR(VRTeleportPos);
			}
			if (key=='q' && state)
			{
				VRTeleportPos[2]+=shift;
				m_physicsServer.setVRTeleportPosition(VRTeleportPos);
				saveCurrentSettingsVR(VRTeleportPos);
			}
			if (key=='e' && state)
			{
				VRTeleportPos[2]-=shift;
				m_physicsServer.setVRTeleportPosition(VRTeleportPos);
				saveCurrentSettingsVR(VRTeleportPos);
			}
			if (key=='z' && state)
			{
				gVRTeleportRotZ+=shift;
				btQuaternion VRTeleportOrn = btQuaternion(btVector3(0, 0, 1), gVRTeleportRotZ);
				m_physicsServer.setVRTeleportOrientation(VRTeleportOrn);
				saveCurrentSettingsVR(VRTeleportPos);
			}
		}
		
		
		return false;
	}

	virtual void setSharedMemoryKey(int key)
	{
		m_physicsServer.setSharedMemoryKey(key);
	}

	virtual void	processCommandLineArgs(int argc, char* argv[])
	{
		b3CommandLineArgs args(argc,argv);
		loadCurrentSettingsVR(args);
		int shmemKey;
		
		if (args.GetCmdLineArgument("sharedMemoryKey", shmemKey))
		{
			setSharedMemoryKey(shmemKey);
		}

		btVector3 vrTeleportPos = m_physicsServer.getVRTeleportPosition();

		if (args.GetCmdLineArgument("camPosX", vrTeleportPos[0]))
		{
			printf("camPosX=%f\n", vrTeleportPos[0]);
		}

		if (args.GetCmdLineArgument("camPosY", vrTeleportPos[1]))
		{
			printf("camPosY=%f\n", vrTeleportPos[1]);
		}

		if (args.GetCmdLineArgument("camPosZ", vrTeleportPos[2]))
		{
			printf("camPosZ=%f\n", vrTeleportPos[2]);
		}

		m_physicsServer.setVRTeleportPosition(vrTeleportPos);

		float camRotZ = 0.f;
		if (args.GetCmdLineArgument("camRotZ", camRotZ))
		{
			printf("camRotZ = %f\n", camRotZ);
			btQuaternion ornZ(btVector3(0, 0, 1), camRotZ);
			m_physicsServer.setVRTeleportOrientation(ornZ);
		}


		if (args.CheckCmdLineFlag("realtimesimulation"))
		{
			//gEnableRealTimeSimVR = true;
			m_physicsServer.enableRealTimeSimulation(true);
		}

		if (args.CheckCmdLineFlag("disableDefaultKeyboardShortcuts"))
		{
			gEnableDefaultKeyboardShortcuts = false;
		}
		if (args.CheckCmdLineFlag("enableDefaultKeyboardShortcuts"))
		{
			gEnableDefaultKeyboardShortcuts = true;
		}
		if (args.CheckCmdLineFlag("disableDefaultMousePicking"))
		{
			gEnableDefaultMousePicking = false;
		}
		if (args.CheckCmdLineFlag("enableDefaultMousePicking"))
		{
			gEnableDefaultMousePicking = true;
		}

	}

};


PhysicsServerExample::PhysicsServerExample(MultiThreadedOpenGLGuiHelper* helper,CommandProcessorCreationInterface* commandProcessorCreator, SharedMemoryInterface* sharedMem, int options)
:SharedMemoryCommon(helper),
m_physicsServer(commandProcessorCreator,sharedMem,0),
m_wantsShutdown(false),
m_isConnected(false),
m_replay(false),
m_canvas(0),
m_canvasRGBIndex(-1),
m_canvasDepthIndex(-1),
m_canvasSegMaskIndex(-1)
//m_options(options)
#ifdef BT_ENABLE_VR
,m_tinyVrGui(0)
#endif
{

	m_multiThreadedHelper = helper;
//	b3Printf("Started PhysicsServer\n");
}



PhysicsServerExample::~PhysicsServerExample()
{
	if (m_canvas)
	{
	    if (m_canvasRGBIndex>=0)
            m_canvas->destroyCanvas(m_canvasRGBIndex);
	    if (m_canvasDepthIndex>=0)
            m_canvas->destroyCanvas(m_canvasDepthIndex);
        if (m_canvasSegMaskIndex>=0)
            m_canvas->destroyCanvas(m_canvasSegMaskIndex);
	}

#ifdef BT_ENABLE_VR
	delete m_tinyVrGui;
#endif


	bool deInitializeSharedMemory = true;
	m_physicsServer.disconnectSharedMemory(deInitializeSharedMemory);
    m_isConnected = false;
	delete m_multiThreadedHelper;
}

bool PhysicsServerExample::isConnected()
{
    return m_isConnected;
}

void	PhysicsServerExample::initPhysics()
{
	///for this testing we use Z-axis up
	int upAxis = 2;
	m_guiHelper->setUpAxis(upAxis);


	m_threadSupport = createMotionThreadSupport(MAX_MOTION_NUM_THREADS);
		
		
		m_isConnected = m_physicsServer.connectSharedMemory( m_guiHelper);
		

		for (int i=0;i<m_threadSupport->getNumTasks();i++)
		{
			MotionThreadLocalStorage* storage = (MotionThreadLocalStorage*) m_threadSupport->getThreadLocalMemory(i);
			b3Assert(storage);
			storage->threadId = i;
			//storage->m_sharedMem = data->m_sharedMem;
		}


		

		for (int w=0;w<MAX_MOTION_NUM_THREADS;w++)
		{
			m_args[w].m_cs = m_threadSupport->createCriticalSection();
			m_args[w].m_cs2 = m_threadSupport->createCriticalSection();
			m_args[w].m_cs3 = m_threadSupport->createCriticalSection();
			m_args[w].m_csGUI = m_threadSupport->createCriticalSection();
			m_args[w].m_cs->lock();
			m_args[w].m_cs->setSharedParam(0,eMotionIsUnInitialized);
			m_args[w].m_cs->unlock();
			int numMoving = 0;
 			m_args[w].m_positions.resize(numMoving);
			m_args[w].m_physicsServerPtr = &m_physicsServer;
			//int index = 0;
			
			m_threadSupport->runTask(B3_THREAD_SCHEDULE_TASK, (void*) &this->m_args[w], w);
			bool isUninitialized = true;	
			
			while (isUninitialized)
			{
				m_args[w].m_cs->lock();
				isUninitialized = (m_args[w].m_cs->getSharedParam(0)==eMotionIsUnInitialized);
				m_args[w].m_cs->unlock();
#ifdef _WIN32
				b3Clock::usleep(1000);
#endif
			}
		}

		m_args[0].m_cs->setSharedParam(1,eGUIHelperIdle);
		m_multiThreadedHelper->setCriticalSection(m_args[0].m_cs);
		m_multiThreadedHelper->setCriticalSection2(m_args[0].m_cs2);
		m_multiThreadedHelper->setCriticalSection3(m_args[0].m_cs3);
		m_multiThreadedHelper->setCriticalSectionGUI(m_args[0].m_csGUI);
		
		m_args[0].m_cs2->lock();




	{
		m_canvas = m_guiHelper->get2dCanvasInterface();
		if (m_canvas)
		{
	
			
			m_canvasRGBIndex = m_canvas->createCanvas("Synthetic Camera RGB data",gCamVisualizerWidth, gCamVisualizerHeight, 8,55);
			m_canvasDepthIndex = m_canvas->createCanvas("Synthetic Camera Depth data",gCamVisualizerWidth, gCamVisualizerHeight,8,75+gCamVisualizerHeight);
			m_canvasSegMaskIndex = m_canvas->createCanvas("Synthetic Camera Segmentation Mask",gCamVisualizerWidth, gCamVisualizerHeight,8,95+gCamVisualizerHeight*2);

			for (int i=0;i<gCamVisualizerWidth;i++)
			{
				for (int j=0;j<gCamVisualizerHeight;j++)
				{
					unsigned char red=255;
					unsigned char green=255;
					unsigned char blue=255;
					unsigned char alpha=255;
					if (i==j)
					{
						red = 0;
						green=0;
						blue=0;
					}
					m_canvas->setPixel(m_canvasRGBIndex,i,j,red,green,blue,alpha);
					if (m_canvasSegMaskIndex>=0)
						m_canvas->setPixel(m_canvasDepthIndex,i,j,red,green,blue,alpha);
					if (m_canvasSegMaskIndex>=0)
						m_canvas->setPixel(m_canvasSegMaskIndex,i,j,red,green,blue,alpha);
					
				}
			}
			m_canvas->refreshImageData(m_canvasRGBIndex);
	
			if (m_canvasDepthIndex>=0)
				m_canvas->refreshImageData(m_canvasDepthIndex);
			if (m_canvasSegMaskIndex>=0)
				m_canvas->refreshImageData(m_canvasSegMaskIndex);
			
			
		}

	}
}


void    PhysicsServerExample::exitPhysics()
{
		for (int i=0;i<MAX_MOTION_NUM_THREADS;i++)
		{
			m_args[i].m_cs2->unlock();
			m_args[i].m_cs->lock();
			m_args[i].m_cs->setSharedParam(0,eRequestTerminateMotion);
			m_args[i].m_cs->unlock();
		}
		int numActiveThreads = MAX_MOTION_NUM_THREADS;

		while (numActiveThreads)
                {
			int arg0,arg1;
                        if (m_threadSupport->isTaskCompleted(&arg0,&arg1,0))
                        {
                                numActiveThreads--;
                                printf("numActiveThreads = %d\n",numActiveThreads);

                        } else
                        {
							b3Clock::usleep(0);
                        }
						//we need to call 'stepSimulation' to make sure that
						//other threads get out of blocking state (workerThreadWait)
						stepSimulation(0);
                };

		printf("stopping threads\n");

		m_threadSupport->deleteCriticalSection(m_args[0].m_cs);
		m_threadSupport->deleteCriticalSection(m_args[0].m_cs2);
		m_threadSupport->deleteCriticalSection(m_args[0].m_cs3);
		m_threadSupport->deleteCriticalSection(m_args[0].m_csGUI);

		delete m_threadSupport;   
		m_threadSupport = 0;

		//m_physicsServer.resetDynamicsWorld();
		
}



bool PhysicsServerExample::wantsTermination()
{
    return m_wantsShutdown;
}

void	PhysicsServerExample::updateGraphics()
{
	//check if any graphics related tasks are requested
	
	switch (m_multiThreadedHelper->getCriticalSection()->getSharedParam(1))
	{
	case eGUIHelperCreateCollisionShapeGraphicsObject:
	{
		B3_PROFILE("eGUIHelperCreateCollisionShapeGraphicsObject");
		m_multiThreadedHelper->m_childGuiHelper->createCollisionShapeGraphicsObject(m_multiThreadedHelper->m_colShape);
		m_multiThreadedHelper->mainThreadRelease();
		break;
	}
	case eGUIHelperCreateCollisionObjectGraphicsObject:
	{
		B3_PROFILE("eGUIHelperCreateCollisionObjectGraphicsObject");

		m_multiThreadedHelper->m_childGuiHelper->createCollisionObjectGraphicsObject(m_multiThreadedHelper->m_obj,
			m_multiThreadedHelper->m_color2);
		m_multiThreadedHelper->mainThreadRelease();

		break;
	}
	case eGUIHelperCreateRigidBodyGraphicsObject:
	{
		B3_PROFILE("eGUIHelperCreateRigidBodyGraphicsObject");
		m_multiThreadedHelper->m_childGuiHelper->createRigidBodyGraphicsObject(m_multiThreadedHelper->m_body,m_multiThreadedHelper->m_color3);
		m_multiThreadedHelper->mainThreadRelease();
		break;
	}
	case eGUIHelperRegisterTexture:
	{
		B3_PROFILE("eGUIHelperRegisterTexture");
		m_multiThreadedHelper->m_textureId = m_multiThreadedHelper->m_childGuiHelper->registerTexture(m_multiThreadedHelper->m_texels,
						m_multiThreadedHelper->m_textureWidth,m_multiThreadedHelper->m_textureHeight);
		m_multiThreadedHelper->mainThreadRelease();
		break;
	}
	case eGUIHelperRemoveTexture:
	{
		B3_PROFILE("eGUIHelperRemoveTexture");
		m_multiThreadedHelper->m_childGuiHelper->removeTexture(m_multiThreadedHelper->m_removeTextureUid);
		m_multiThreadedHelper->mainThreadRelease();
		break;
	}
	case eGUIHelperRegisterGraphicsShape:
	{
		B3_PROFILE("eGUIHelperRegisterGraphicsShape");
		m_multiThreadedHelper->m_shapeIndex = m_multiThreadedHelper->m_childGuiHelper->registerGraphicsShape(
				m_multiThreadedHelper->m_vertices,
				m_multiThreadedHelper->m_numvertices,
				m_multiThreadedHelper->m_indices,
				m_multiThreadedHelper->m_numIndices,
				m_multiThreadedHelper->m_primitiveType,
				m_multiThreadedHelper->m_textureId);
		m_multiThreadedHelper->mainThreadRelease();
		break;
	}

	case eGUIHelperSetVisualizerFlag:
	{
		B3_PROFILE("eGUIHelperSetVisualizerFlag");
		int flag = m_multiThreadedHelper->m_visualizerFlag;
		int enable = m_multiThreadedHelper->m_visualizerEnable;

		if (flag == COV_ENABLE_RGB_BUFFER_PREVIEW)
		{
			if (enable)
			{
				if (m_canvasRGBIndex<0)
				{
					m_canvasRGBIndex = m_canvas->createCanvas("Synthetic Camera RGB data",gCamVisualizerWidth, gCamVisualizerHeight, 8,55);
				}
			} else
			{
				if (m_canvasRGBIndex>=0)
				{
					m_canvas->destroyCanvas(m_canvasRGBIndex);
					m_canvasRGBIndex = -1;
				}
			}
		}

		if (flag == COV_ENABLE_DEPTH_BUFFER_PREVIEW)
		{
			if (enable)
			{
				if (m_canvasDepthIndex<0)
				{
					m_canvasDepthIndex = m_canvas->createCanvas("Synthetic Camera Depth data",gCamVisualizerWidth, gCamVisualizerHeight,8,75+gCamVisualizerHeight);
				}
			} else
			{
				if (m_canvasDepthIndex>=0)
				{
					m_canvas->destroyCanvas(m_canvasDepthIndex);
					m_canvasDepthIndex = -1;
				}
			}
		}

		if (flag == COV_ENABLE_SEGMENTATION_MARK_PREVIEW)
		{
			if (enable)
			{
				if (m_canvasSegMaskIndex<0)
				{
					m_canvasSegMaskIndex = m_canvas->createCanvas("Synthetic Camera Segmentation Mask",gCamVisualizerWidth, gCamVisualizerHeight,8,95+gCamVisualizerHeight*2);
				}
			} else
			{
				if (m_canvasSegMaskIndex>=0)
				{
					m_canvas->destroyCanvas(m_canvasSegMaskIndex);
					m_canvasSegMaskIndex = -1;
				}
			}
		}
		
		if (flag==COV_ENABLE_VR_TELEPORTING)
		{
			gEnableTeleporting = (enable!=0);
		}

		if (flag == COV_ENABLE_VR_PICKING)
		{
			gEnablePicking = (enable!=0);
		}
    
		if (flag ==COV_ENABLE_SYNC_RENDERING_INTERNAL)
		{
			gEnableSyncPhysicsRendering = (enable!=0);
		}

		if (flag == COV_ENABLE_RENDERING)
		{
			gEnableRendering = (enable!=0);
		}

		if (flag == COV_ENABLE_KEYBOARD_SHORTCUTS)
		{
			gEnableDefaultKeyboardShortcuts = (enable!=0);
		}

		if (flag == COV_ENABLE_MOUSE_PICKING)
		{
			gEnableDefaultMousePicking = (enable!=0);
		}

		m_multiThreadedHelper->m_childGuiHelper->setVisualizerFlag(m_multiThreadedHelper->m_visualizerFlag,m_multiThreadedHelper->m_visualizerEnable);
		m_multiThreadedHelper->mainThreadRelease();
		break;
    }


	case eGUIHelperRegisterGraphicsInstance:
	{
		B3_PROFILE("eGUIHelperRegisterGraphicsInstance");
		m_multiThreadedHelper->m_instanceId = m_multiThreadedHelper->m_childGuiHelper->registerGraphicsInstance(
				m_multiThreadedHelper->m_shapeIndex,
				m_multiThreadedHelper->m_position,
				m_multiThreadedHelper->m_quaternion,
				m_multiThreadedHelper->m_color,
				m_multiThreadedHelper->m_scaling);
		m_multiThreadedHelper->mainThreadRelease();
		break;
	}
	case eGUIHelperRemoveAllGraphicsInstances:
        {
		B3_PROFILE("eGUIHelperRemoveAllGraphicsInstances");
#ifdef BT_ENABLE_VR
			if (m_tinyVrGui)
			{
				delete m_tinyVrGui;
				m_tinyVrGui=0;
			}
#endif //BT_ENABLE_VR
            m_multiThreadedHelper->m_childGuiHelper->removeAllGraphicsInstances();
			if (m_multiThreadedHelper->m_childGuiHelper->getRenderInterface())
			{
				int numRenderInstances;
				numRenderInstances = m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->getTotalNumInstances();
				b3Assert(numRenderInstances==0);
			}
			m_multiThreadedHelper->mainThreadRelease();

            break;
        }
	case eGUIHelperRemoveGraphicsInstance:
	{
		B3_PROFILE("eGUIHelperRemoveGraphicsInstance");
		m_multiThreadedHelper->m_childGuiHelper->removeGraphicsInstance(m_multiThreadedHelper->m_graphicsInstanceRemove);
		m_multiThreadedHelper->mainThreadRelease();
		break;
	}

	case eGUIHelperGetShapeIndexFromInstance:
	{
		B3_PROFILE("eGUIHelperGetShapeIndexFromInstance");
		m_multiThreadedHelper->getShapeIndex_shapeIndex = m_multiThreadedHelper->m_childGuiHelper->getShapeIndexFromInstance(m_multiThreadedHelper->m_getShapeIndex_instance);
		m_multiThreadedHelper->mainThreadRelease();
		break;
	}

	case eGUIHelperChangeGraphicsInstanceTextureId:
	{
		B3_PROFILE("eGUIHelperChangeGraphicsInstanceTextureId");

		m_multiThreadedHelper->m_childGuiHelper->replaceTexture(
			m_multiThreadedHelper->m_graphicsInstanceChangeTextureShapeIndex,
			m_multiThreadedHelper->m_graphicsInstanceChangeTextureId);
		m_multiThreadedHelper->mainThreadRelease();
		break;
	}

	
	case eGUIHelperChangeTexture:
	{
		B3_PROFILE("eGUIHelperChangeTexture");

		m_multiThreadedHelper->m_childGuiHelper->changeTexture(
			m_multiThreadedHelper->m_changeTextureUniqueId,
			m_multiThreadedHelper->m_changeTextureRgbTexels,
			m_multiThreadedHelper->m_changeTextureWidth,
			m_multiThreadedHelper->m_changeTextureHeight);
		m_multiThreadedHelper->mainThreadRelease();
		break;
	}

	case eGUIHelperChangeGraphicsInstanceRGBAColor:
	{
		B3_PROFILE("eGUIHelperChangeGraphicsInstanceRGBAColor");

		m_multiThreadedHelper->m_childGuiHelper->changeRGBAColor(m_multiThreadedHelper->m_graphicsInstanceChangeColor,m_multiThreadedHelper->m_rgbaColor);
		m_multiThreadedHelper->mainThreadRelease();
		break;
	}
	case eGUIHelperChangeGraphicsInstanceSpecularColor:
	{
		B3_PROFILE("eGUIHelperChangeGraphicsInstanceSpecularColor");

		m_multiThreadedHelper->m_childGuiHelper->changeSpecularColor(m_multiThreadedHelper->m_graphicsInstanceChangeSpecular,m_multiThreadedHelper->m_specularColor);
		m_multiThreadedHelper->mainThreadRelease();
		break;

	}
	case eGUIHelperDisplayCameraImageData:
	{
		B3_PROFILE("eGUIHelperDisplayCameraImageData");

		if (m_canvas)
		{

			int numBytesPerPixel= 4;
			int startRGBIndex = m_multiThreadedHelper->m_startPixelIndex*numBytesPerPixel;
			int endRGBIndex = startRGBIndex + (*m_multiThreadedHelper->m_numPixelsCopied*numBytesPerPixel);

			int startDepthIndex = m_multiThreadedHelper->m_startPixelIndex;
			int endDepthIndex = startDepthIndex + (*m_multiThreadedHelper->m_numPixelsCopied);

			int startSegIndex = m_multiThreadedHelper->m_startPixelIndex;
			int endSegIndex = startSegIndex + (*m_multiThreadedHelper->m_numPixelsCopied);

			//btScalar frustumZNear = m_multiThreadedHelper->m_projectionMatrix[14]/(m_multiThreadedHelper->m_projectionMatrix[10]-1);
			//btScalar frustumZFar = m_multiThreadedHelper->m_projectionMatrix[14]/(m_multiThreadedHelper->m_projectionMatrix[10]+1);
			

				for (int i=0;i<gCamVisualizerWidth;i++)
					{
						for (int j=0;j<gCamVisualizerHeight;j++)
						{
                            int xIndex = int(float(i)*(float(m_multiThreadedHelper->m_destinationWidth)/float(gCamVisualizerWidth)));
                            int yIndex = int(float(j)*(float(m_multiThreadedHelper->m_destinationHeight)/float(gCamVisualizerHeight)));
							btClamp(xIndex,0,m_multiThreadedHelper->m_destinationWidth);
							btClamp(yIndex,0,m_multiThreadedHelper->m_destinationHeight);
							int bytesPerPixel = 4; //RGBA
							
							if (m_canvasRGBIndex >=0)
                            {
                                int rgbPixelIndex = (xIndex+yIndex*m_multiThreadedHelper->m_destinationWidth)*bytesPerPixel;
								if (rgbPixelIndex >= startRGBIndex && rgbPixelIndex < endRGBIndex)
								{
									m_canvas->setPixel(m_canvasRGBIndex,i,j,
											m_multiThreadedHelper->m_pixelsRGBA[rgbPixelIndex-startRGBIndex],
											m_multiThreadedHelper->m_pixelsRGBA[rgbPixelIndex+1-startRGBIndex],
											m_multiThreadedHelper->m_pixelsRGBA[rgbPixelIndex+2-startRGBIndex],
													   255); //alpha set to 255
								}
                            }
                            if (m_canvasDepthIndex >=0 && 0!= m_multiThreadedHelper->m_depthBuffer)
                            {
                                int depthPixelIndex = (xIndex+yIndex*m_multiThreadedHelper->m_destinationWidth);
								if (depthPixelIndex >= startDepthIndex && depthPixelIndex < endDepthIndex)
								{
									float depthValue = m_multiThreadedHelper->m_depthBuffer[depthPixelIndex-startDepthIndex];
									//todo: rescale the depthValue to [0..255]
									if (depthValue>-1e20)
									{
										int rgb = 0;
										btScalar frustumZNear = 0.1;
										btScalar frustumZFar = 30;
										btScalar minDepthValue = frustumZNear;//todo: compute more reasonably min/max depth range
										btScalar maxDepthValue = frustumZFar;

										float depth = depthValue;
										btScalar linearDepth = 255.*(2.0 * frustumZNear) / (frustumZFar + frustumZNear - depth * (frustumZFar - frustumZNear));
										btClamp(linearDepth, btScalar(0),btScalar(255));
										rgb =  linearDepth;
										
										m_canvas->setPixel(m_canvasDepthIndex,i,j,
											rgb,
											rgb,
											255, 255); //alpha set to 255
									} else
									{
										m_canvas->setPixel(m_canvasDepthIndex,i,j,
											0,
											0,
											0, 255); //alpha set to 255
									}
								}
                            }

                            if (m_canvasSegMaskIndex>=0 && (0!=m_multiThreadedHelper->m_segmentationMaskBuffer))
                            {
                                int segmentationMaskPixelIndex = (xIndex+yIndex*m_multiThreadedHelper->m_destinationWidth);

								if (segmentationMaskPixelIndex >= startSegIndex && segmentationMaskPixelIndex < endSegIndex)
								{
									int segmentationMask = m_multiThreadedHelper->m_segmentationMaskBuffer[segmentationMaskPixelIndex-startSegIndex];
									btVector4 palette[4] = {btVector4(32,255,32,255),
															btVector4(32,32,255,255),
															btVector4(255,255,32,255),
															btVector4(32,255,255,255)};
									if (segmentationMask>=0)
									{
										int obIndex = segmentationMask&((1<<24)-1);
										int linkIndex = (segmentationMask>>24)-1;
										
										btVector4 rgb = palette[(obIndex+linkIndex)&3];
										
										 m_canvas->setPixel(m_canvasSegMaskIndex,i,j,
											rgb.x(),
											rgb.y(),
											rgb.z(), 255); //alpha set to 255
									} else
									{
										 m_canvas->setPixel(m_canvasSegMaskIndex,i,j,
											0,
											0,
											0, 255); //alpha set to 255
									}
								}
                            }
						}
					}

					if (m_canvasRGBIndex >=0)
                        m_canvas->refreshImageData(m_canvasRGBIndex);
      
					if (m_canvasDepthIndex >=0)
                        m_canvas->refreshImageData(m_canvasDepthIndex);
                    if (m_canvasSegMaskIndex >=0)
                        m_canvas->refreshImageData(m_canvasSegMaskIndex);
                        
                        
		}
		m_multiThreadedHelper->mainThreadRelease();
		break;
	}
    case eGUIHelperCopyCameraImageData:
        {
			B3_PROFILE("eGUIHelperCopyCameraImageData");

			if (m_multiThreadedHelper->m_startPixelIndex == 0)
			{
				m_physicsServer.syncPhysicsToGraphics();
			}

             m_multiThreadedHelper->m_childGuiHelper->copyCameraImageData(m_multiThreadedHelper->m_viewMatrix,
				 m_multiThreadedHelper->m_projectionMatrix,
				 m_multiThreadedHelper->m_pixelsRGBA,
				 m_multiThreadedHelper->m_rgbaBufferSizeInPixels,
				 m_multiThreadedHelper->m_depthBuffer,
				 m_multiThreadedHelper->m_depthBufferSizeInPixels,
				 m_multiThreadedHelper->m_segmentationMaskBuffer,
				 m_multiThreadedHelper->m_segmentationMaskBufferSizeInPixels,
				 m_multiThreadedHelper->m_startPixelIndex, 
				 m_multiThreadedHelper->m_destinationWidth, 
				 m_multiThreadedHelper->m_destinationHeight, 
				 m_multiThreadedHelper->m_numPixelsCopied);

	 		m_multiThreadedHelper->mainThreadRelease();
            break;
        }
	case eGUIHelperAutogenerateGraphicsObjects:
	{
		B3_PROFILE("eGUIHelperAutogenerateGraphicsObjects");

		m_multiThreadedHelper->m_childGuiHelper->autogenerateGraphicsObjects(m_multiThreadedHelper->m_dynamicsWorld);
		m_multiThreadedHelper->mainThreadRelease();
		break;
	}

	case eGUIUserDebugAddText:
	{
		B3_PROFILE("eGUIUserDebugAddText");
		
		bool replaced = false;

		for (int i=0;i<m_multiThreadedHelper->m_userDebugText.size();i++)
		{
			if (m_multiThreadedHelper->m_userDebugText[i].m_itemUniqueId == m_multiThreadedHelper->m_tmpText.m_itemUniqueId)
			{
				m_multiThreadedHelper->m_userDebugText[i] = m_multiThreadedHelper->m_tmpText;
				m_multiThreadedHelper->m_resultUserDebugTextUid = m_multiThreadedHelper->m_tmpText.m_itemUniqueId;
				replaced = true;
			}
		}

		if (!replaced)
		{
			m_multiThreadedHelper->m_userDebugText.push_back(m_multiThreadedHelper->m_tmpText);
			m_multiThreadedHelper->m_resultUserDebugTextUid = m_multiThreadedHelper->m_userDebugText[m_multiThreadedHelper->m_userDebugText.size()-1].m_itemUniqueId;
		}
		m_multiThreadedHelper->mainThreadRelease();
		
		break;
	}
	case eGUIUserDebugAddParameter:
	{
		B3_PROFILE("eGUIUserDebugAddParameter");

		UserDebugParameter* param = new UserDebugParameter(m_multiThreadedHelper->m_tmpParam);
		m_multiThreadedHelper->m_userDebugParams.push_back(param);

		{
        SliderParams slider(param->m_text,&param->m_value);
        slider.m_minVal=param->m_rangeMin;
        slider.m_maxVal=param->m_rangeMax;
		
		if (m_multiThreadedHelper->m_childGuiHelper->getParameterInterface())
	        m_multiThreadedHelper->m_childGuiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
		}

		m_multiThreadedHelper->m_userDebugParamUid = (*m_multiThreadedHelper->m_userDebugParams[m_multiThreadedHelper->m_userDebugParams.size()-1]).m_itemUniqueId;

		//also add actual menu
		m_multiThreadedHelper->mainThreadRelease();
		break;
	}
	case eGUIUserDebugAddLine:
	{
		B3_PROFILE("eGUIUserDebugAddLine");

		if (m_multiThreadedHelper->m_tmpLine.m_replaceItemUid>=0)
		{
			for (int i=0;i<m_multiThreadedHelper->m_userDebugLines.size();i++)
			{
				if (m_multiThreadedHelper->m_userDebugLines[i].m_itemUniqueId == m_multiThreadedHelper->m_tmpLine.m_replaceItemUid)
				{
					m_multiThreadedHelper->m_userDebugLines[i] = m_multiThreadedHelper->m_tmpLine;
					m_multiThreadedHelper->m_resultDebugLineUid = m_multiThreadedHelper->m_tmpLine.m_replaceItemUid;
				}
			}
		} else
		{
			m_multiThreadedHelper->m_userDebugLines.push_back(m_multiThreadedHelper->m_tmpLine);
			m_multiThreadedHelper->m_resultDebugLineUid = m_multiThreadedHelper->m_userDebugLines[m_multiThreadedHelper->m_userDebugLines.size()-1].m_itemUniqueId;
		}
		m_multiThreadedHelper->mainThreadRelease();
		break;
	}
	case eGUIUserDebugRemoveItem:
	{
		B3_PROFILE("eGUIUserDebugRemoveItem");

		for (int i=0;i<m_multiThreadedHelper->m_userDebugLines.size();i++)
		{
			if (m_multiThreadedHelper->m_userDebugLines[i].m_itemUniqueId == m_multiThreadedHelper->m_removeDebugItemUid)
			{
				m_multiThreadedHelper->m_userDebugLines.swap(i,m_multiThreadedHelper->m_userDebugLines.size()-1);
				m_multiThreadedHelper->m_userDebugLines.pop_back();
				break;
			}
		}


		for (int i=0;i<m_multiThreadedHelper->m_userDebugText.size();i++)
		{
			if (m_multiThreadedHelper->m_userDebugText[i].m_itemUniqueId == m_multiThreadedHelper->m_removeDebugItemUid)
			{
				m_multiThreadedHelper->m_userDebugText.swap(i,m_multiThreadedHelper->m_userDebugText.size()-1);
				m_multiThreadedHelper->m_userDebugText.pop_back();
				break;
			}
		}

		m_multiThreadedHelper->mainThreadRelease();
			break;
	}
	case eGUIUserDebugRemoveAllItems:
	{
		B3_PROFILE("eGUIUserDebugRemoveAllItems");

		m_multiThreadedHelper->m_userDebugLines.clear();
		m_multiThreadedHelper->m_userDebugText.clear();
		m_multiThreadedHelper->m_uidGenerator = 0;
		m_multiThreadedHelper->mainThreadRelease();
			break;
	}

	case eGUIDumpFramesToVideo:
	{
		B3_PROFILE("eGUIDumpFramesToVideo");

		m_multiThreadedHelper->m_childGuiHelper->dumpFramesToVideo(m_multiThreadedHelper->m_mp4FileName);
		m_multiThreadedHelper->mainThreadRelease();
		break;
	}

	case eGUIHelperIdle:
		{
			break;
		}
	default:
		{
			btAssert(0);
		}
	}
	

}

void	PhysicsServerExample::stepSimulation(float deltaTime)
{
	BT_PROFILE("PhysicsServerExample::stepSimulation");

	//this->m_physicsServer.processClientCommands();

	for (int i = m_multiThreadedHelper->m_userDebugLines.size()-1;i>=0;i--)
	{
		if (m_multiThreadedHelper->m_userDebugLines[i].m_lifeTime)
		{
			m_multiThreadedHelper->m_userDebugLines[i].m_lifeTime -= deltaTime;
			if (m_multiThreadedHelper->m_userDebugLines[i].m_lifeTime<=0)
			{
				m_multiThreadedHelper->m_userDebugLines.swap(i,m_multiThreadedHelper->m_userDebugLines.size()-1);
				m_multiThreadedHelper->m_userDebugLines.pop_back();
			}
		}
	}

	for (int i = m_multiThreadedHelper->m_userDebugText.size()-1;i>=0;i--)
	{
		if (m_multiThreadedHelper->m_userDebugText[i].m_lifeTime)
		{
			m_multiThreadedHelper->m_userDebugText[i].m_lifeTime -= deltaTime;
			if (m_multiThreadedHelper->m_userDebugText[i].m_lifeTime<=0)
			{
				m_multiThreadedHelper->m_userDebugText.swap(i,m_multiThreadedHelper->m_userDebugText.size()-1);
				m_multiThreadedHelper->m_userDebugText.pop_back();
			}
		}
	}
	updateGraphics();


	
	{
		if (m_multiThreadedHelper->m_childGuiHelper->getRenderInterface())
		{
			m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->writeTransforms();
		}
	}
}

static float vrOffset[16]={1,0,0,0,
							0,1,0,0,
							0,0,1,0,
							0,0,0,0};


extern int gDroppedSimulationSteps;
extern int gNumSteps;
extern double gDtInSec;
extern double gSubStep;
extern btTransform gVRTrackingObjectTr;


void PhysicsServerExample::drawUserDebugLines()
{
	//static char line0[1024];
	//static char line1[1024];

	//draw all user-debug-lines

	//add array of lines

	//draw all user- 'text3d' messages
	if (m_multiThreadedHelper)
	{


		//if gBatchUserDebugLines is true, batch lines based on color+width, to reduce line draw calls

		btAlignedObjectArray< btAlignedObjectArray<unsigned int> > sortedIndices;
		btAlignedObjectArray< btAlignedObjectArray<btVector3FloatData> > sortedLines;

		btHashMap<ColorWidth,int> hashedLines;

		for (int i = 0; i<m_multiThreadedHelper->m_userDebugLines.size(); i++)
		{

			



			btVector3 from;
			from.setValue(m_multiThreadedHelper->m_userDebugLines[i].m_debugLineFromXYZ[0],
				m_multiThreadedHelper->m_userDebugLines[i].m_debugLineFromXYZ[1],
				m_multiThreadedHelper->m_userDebugLines[i].m_debugLineFromXYZ[2]);
			btVector3 toX;
			toX.setValue(m_multiThreadedHelper->m_userDebugLines[i].m_debugLineToXYZ[0],
				m_multiThreadedHelper->m_userDebugLines[i].m_debugLineToXYZ[1],
				m_multiThreadedHelper->m_userDebugLines[i].m_debugLineToXYZ[2]);

			int graphicsIndex = m_multiThreadedHelper->m_userDebugLines[i].m_trackingVisualShapeIndex;
			if (graphicsIndex>=0)
			{
				CommonRenderInterface* renderer = m_guiHelper->getRenderInterface();
				if (renderer)
				{
					float parentPos[3];
					float parentOrn[4];

					if (renderer->readSingleInstanceTransformToCPU(parentPos,parentOrn,graphicsIndex))
					{
						btTransform parentTrans;
						parentTrans.setOrigin(btVector3(parentPos[0],parentPos[1],parentPos[2]));
						parentTrans.setRotation(btQuaternion(parentOrn[0],parentOrn[1],parentOrn[2],parentOrn[3]));
						from = parentTrans*from;
						toX = parentTrans*toX;
					}
				}
			}

			btVector3 color;
			color.setValue(m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[0],
				m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[1],
				m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[2]);
			ColorWidth cw;
			color.serializeFloat(cw.m_color);
			cw.width = m_multiThreadedHelper->m_userDebugLines[i].m_lineWidth;
			int index = -1;

			if (gBatchUserDebugLines)
			{
				int* indexPtr = hashedLines.find(cw);
				if (indexPtr)
				{
					index = *indexPtr;
				} else
				{
					index = sortedLines.size();
					sortedLines.expand();
					sortedIndices.expand();
					hashedLines.insert(cw,index);
				}
				btAssert(index>=0);
				if (index>=0)
				{
					btVector3FloatData from1,toX1;
					sortedIndices[index].push_back(sortedLines[index].size());
					from.serializeFloat(from1);
					sortedLines[index].push_back(from1);
					sortedIndices[index].push_back(sortedLines[index].size());
					toX.serializeFloat(toX1);
					sortedLines[index].push_back(toX1);
				}
			}
			else
			{
				m_guiHelper->getAppInterface()->m_renderer->drawLine(from, toX, color, m_multiThreadedHelper->m_userDebugLines[i].m_lineWidth);
			}
		}


		if (gBatchUserDebugLines)
		{
			for (int i=0;i<hashedLines.size();i++)
			{
				ColorWidth cw = hashedLines.getKeyAtIndex(i);
				int index = *hashedLines.getAtIndex(i);
				int stride = sizeof(btVector3FloatData);
				const float* positions = &sortedLines[index][0].m_floats[0];
				int numPoints = sortedLines[index].size();
				const unsigned int* indices = &sortedIndices[index][0];
				int numIndices = sortedIndices[index].size();
				m_guiHelper->getAppInterface()->m_renderer->drawLines(positions,cw.m_color.m_floats,numPoints, stride, indices,numIndices,cw.width);
			}
		}

		for (int i = 0; i<m_multiThreadedHelper->m_userDebugText.size(); i++)
		{

			//int optionFlag = 0;//CommonGraphicsApp::eDrawText3D_OrtogonalFaceCamera|CommonGraphicsApp::eDrawText3D_TrueType;
			//int optionFlag = CommonGraphicsApp::eDrawText3D_TrueType;
			float orientation[4] = {0,0,0,1};
			
			//int optionFlag = CommonGraphicsApp::eDrawText3D_OrtogonalFaceCamera;
			int optionFlag = 0;
			
			if (m_multiThreadedHelper->m_userDebugText[i].m_optionFlags&CommonGraphicsApp::eDrawText3D_OrtogonalFaceCamera)
			{
				optionFlag |= CommonGraphicsApp::eDrawText3D_OrtogonalFaceCamera;
			} else
			{
				orientation[0] = (float)m_multiThreadedHelper->m_userDebugText[i].m_textOrientation[0];
				orientation[1] = (float)m_multiThreadedHelper->m_userDebugText[i].m_textOrientation[1];
				orientation[2] = (float)m_multiThreadedHelper->m_userDebugText[i].m_textOrientation[2];
				orientation[3] = (float)m_multiThreadedHelper->m_userDebugText[i].m_textOrientation[3];
				optionFlag |= CommonGraphicsApp::eDrawText3D_TrueType;
				
			}
			
			float colorRGBA[4] = {
				(float)m_multiThreadedHelper->m_userDebugText[i].m_textColorRGB[0],
				(float)m_multiThreadedHelper->m_userDebugText[i].m_textColorRGB[1],
				(float)m_multiThreadedHelper->m_userDebugText[i].m_textColorRGB[2],
				(float)1.};

			float pos[3] = { (float)m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ1[0],
				(float)m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ1[1],
				(float)m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ1[2]};

			int graphicsIndex = m_multiThreadedHelper->m_userDebugText[i].m_trackingVisualShapeIndex;
			if (graphicsIndex>=0)
			{
				CommonRenderInterface* renderer = m_guiHelper->getRenderInterface();
				if (renderer)
				{
					float parentPos[3];
					float parentOrn[4];

					if (renderer->readSingleInstanceTransformToCPU(parentPos,parentOrn,graphicsIndex))
					{
						btTransform parentTrans;
						parentTrans.setOrigin(btVector3(parentPos[0],parentPos[1],parentPos[2]));
						parentTrans.setRotation(btQuaternion(parentOrn[0],parentOrn[1],parentOrn[2],parentOrn[3]));
						btTransform childTr;
						childTr.setOrigin(btVector3(pos[0],pos[1],pos[2]));
						childTr.setRotation(btQuaternion(orientation[0],orientation[1],orientation[2],orientation[3]));

						btTransform siteTr = parentTrans*childTr;
						pos[0] = siteTr.getOrigin()[0];
						pos[1] = siteTr.getOrigin()[1];
						pos[2] = siteTr.getOrigin()[2];
						btQuaternion siteOrn = siteTr.getRotation();
						orientation[0] = siteOrn[0];
						orientation[1] = siteOrn[1];
						orientation[2] = siteOrn[2];
						orientation[3] = siteOrn[3];
					}
				}
			}


			{
				btAlignedObjectArray<std::string> pieces;
				btAlignedObjectArray<std::string> separators;
				separators.push_back("\n");
				urdfStringSplit(pieces,m_multiThreadedHelper->m_userDebugText[i].m_text,separators);
				
				double sz = m_multiThreadedHelper->m_userDebugText[i].textSize;
				
				btTransform tr;
				tr.setIdentity();
				tr.setOrigin(btVector3(pos[0],pos[1],pos[2]));
				tr.setRotation(btQuaternion(orientation[0],orientation[1],orientation[2],orientation[3]));

				//float newpos[3]={pos[0]-float(t)*sz,pos[1],pos[2]};

				for (int t=0;t<pieces.size();t++)
				{
					btTransform offset;
					offset.setIdentity();
					offset.setOrigin(btVector3(0,-float(t)*sz,0));
					btTransform result = tr*offset;
					float newpos[3] = { (float)result.getOrigin()[0],
						(float)result.getOrigin()[1],
						(float)result.getOrigin()[2]};
					
					m_guiHelper->getAppInterface()->drawText3D(pieces[t].c_str(),
						newpos,orientation,colorRGBA,
						sz,optionFlag);
					
				}
			}


			/*m_guiHelper->getAppInterface()->drawText3D(m_multiThreadedHelper->m_userDebugText[i].m_text,
				m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ[0],
				m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ[1],
				m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ[2],
				m_multiThreadedHelper->m_userDebugText[i].textSize);
				*/

		}
	}

}

void PhysicsServerExample::renderScene()
{
	btTransform vrTrans;


		

	B3_PROFILE("PhysicsServerExample::RenderScene");


	if (m_physicsServer.isRealTimeSimulationEnabled())
	{
		
		static int frameCount=0;
		static btScalar prevTime = m_clock.getTimeSeconds();
		frameCount++;
		
		static char line0[1024]={0};
		static char line1[1024]={0};

#if 0

		static btScalar worseFps = 1000000;
		int numFrames = 200;
		static int count = 0;
		count++;
		
			
		if (0 == (count & 1))
		{
			btScalar curTime = m_clock.getTimeSeconds();
			btScalar fps = 1. / (curTime - prevTime);
			prevTime = curTime;
			if (fps < worseFps)
			{
				worseFps = fps;
			}

			if (count > numFrames)
			{
				count = 0;
				sprintf(line0, "fps:%f frame:%d", worseFps, frameCount / 2);
				sprintf(line1, "drop:%d tscale:%f dt:%f, substep %f)", gDroppedSimulationSteps, simTimeScalingFactor,gDtInSec, gSubStep);
				gDroppedSimulationSteps = 0;

				worseFps = 1000000;
			}
		}
#endif

#ifdef BT_ENABLE_VR
		if ((gInternalSimFlags&2 ) && m_tinyVrGui==0)
		{
			ComboBoxParams comboParams;
        comboParams.m_comboboxId = 0;
        comboParams.m_numItems = 0;
        comboParams.m_startItem = 0;
        comboParams.m_callback = 0;//MyComboBoxCallback;
        comboParams.m_userPointer = 0;//this;
        
			m_tinyVrGui = new TinyVRGui(comboParams,this->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface());
			m_tinyVrGui->init();
		}

		if (m_tinyVrGui)
		{

			b3Transform tr;tr.setIdentity();
			btVector3 VRController2Pos = m_physicsServer.getVRTeleportPosition();
			btQuaternion VRController2Orn = m_physicsServer.getVRTeleportOrientation();
			tr.setOrigin(b3MakeVector3(VRController2Pos[0],VRController2Pos[1],VRController2Pos[2]));
			tr.setRotation(b3Quaternion(VRController2Orn[0],VRController2Orn[1],VRController2Orn[2],VRController2Orn[3]));
			tr = tr*b3Transform(b3Quaternion(0,0,-SIMD_HALF_PI),b3MakeVector3(0,0,0));
			b3Scalar dt = 0.01;
			m_tinyVrGui->clearTextArea();
			m_tinyVrGui->grapicalPrintf(line0,0,0,0,0,0,255);
			m_tinyVrGui->grapicalPrintf(line1,0,16,255,255,255,255);

			m_tinyVrGui->tick(dt,tr);
		}
#endif//BT_ENABLE_VR
	}
	///debug rendering
	//m_args[0].m_cs->lock();
	
	//gVRTeleportPos[0] += 0.01;
	btTransform tr2a, tr2;
	tr2a.setIdentity();
	tr2.setIdentity();
	tr2.setOrigin(m_physicsServer.getVRTeleportPosition());
	tr2a.setRotation(m_physicsServer.getVRTeleportOrientation());
	btTransform trTotal = tr2*tr2a;
	btTransform trInv = trTotal.inverse();

	btMatrix3x3 vrOffsetRot;
	vrOffsetRot.setRotation(trInv.getRotation());
	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			vrOffset[i + 4 * j] = vrOffsetRot[i][j];
		}
	}

	vrOffset[12]= trInv.getOrigin()[0];
	vrOffset[13]= trInv.getOrigin()[1];
	vrOffset[14]= trInv.getOrigin()[2];

	if (m_multiThreadedHelper->m_childGuiHelper->getRenderInterface())
	{
		m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->
		getActiveCamera()->setVRCameraOffsetTransform(vrOffset);
	}
	if (gEnableRendering)
	{
		int renderFlags = 0;
		if (!gEnableSyncPhysicsRendering)
		{
			renderFlags|=1;//COV_DISABLE_SYNC_RENDERING;
		}
		m_physicsServer.renderScene(renderFlags);
	}
	
	if (gEnablePicking)
	{
		for (int i=0;i<MAX_VR_CONTROLLERS;i++)
		{
			if (m_args[0].m_isVrControllerPicking[i] || m_args[0].m_isVrControllerDragging[i])
			{
				btVector3 from = m_args[0].m_vrControllerPos[i];
				btMatrix3x3 mat(m_args[0].m_vrControllerOrn[i]);
	
				btVector3 toX = from+mat.getColumn(0);
				btVector3 toY = from+mat.getColumn(1);
				btVector3 toZ = from+mat.getColumn(2);
	
				int width = 2;

	
				btVector4 color;
				color=btVector4(1,0,0,1);
				m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toX,color,width);
				color=btVector4(0,1,0,1);
				m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toY,color,width);
				color=btVector4(0,0,1,1);
				m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toZ,color,width);
	
			}
		}
	}

	if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera())
	{
		if (!m_physicsServer.isRealTimeSimulationEnabled() && !gActivedVRRealTimeSimulation)
		{
			//only activate real-time simulation once (for backward compatibility)
			gActivedVRRealTimeSimulation = true;
			m_physicsServer.enableRealTimeSimulation(1);
		}
	}

	drawUserDebugLines();


	//m_args[0].m_cs->unlock();
}

void    PhysicsServerExample::physicsDebugDraw(int debugDrawFlags)
{
	if (gEnableSyncPhysicsRendering)
	{
		m_physicsServer.syncPhysicsToGraphics();
	}

	drawUserDebugLines();

	if (gEnableRendering)
	{
		///debug rendering
		//m_physicsServer.physicsDebugDraw(debugDrawFlags);
		m_args[0].m_csGUI->lock();
		//draw stuff and flush?
		this->m_multiThreadedHelper->m_debugDraw->drawDebugDrawerLines();
		m_args[0].m_debugDrawFlags = debugDrawFlags;
		gEnableUpdateDebugDrawLines = true;
		m_args[0].m_csGUI->unlock();

	}
}



btVector3	PhysicsServerExample::getRayTo(int x,int y)
{
	CommonRenderInterface* renderer = m_guiHelper->getRenderInterface();

	if (!renderer)
	{
		btAssert(0);
		return btVector3(0,0,0);
	}
	
	float top = 1.f;
	float bottom = -1.f;
	float nearPlane = 1.f;
	float tanFov = (top-bottom)*0.5f / nearPlane;
	float fov = btScalar(2.0) * btAtan(tanFov);

	btVector3 camPos,camTarget;
	renderer->getActiveCamera()->getCameraPosition(camPos);
	renderer->getActiveCamera()->getCameraTargetPosition(camTarget);
	
	btVector3	rayFrom = camPos;
	btVector3 rayForward = (camTarget-camPos);
	rayForward.normalize();
	float farPlane = 10000.f;
	rayForward*= farPlane;

	btVector3 rightOffset;
	btVector3 cameraUp=btVector3(0,0,0);
	cameraUp[m_guiHelper->getAppInterface()->getUpAxis()]=1;

	btVector3 vertical = cameraUp;

	btVector3 hor;
	hor = rayForward.cross(vertical);
	hor.normalize();
	vertical = hor.cross(rayForward);
	vertical.normalize();

	float tanfov = tanf(0.5f*fov);


	hor *= 2.f * farPlane * tanfov;
	vertical *= 2.f * farPlane * tanfov;

	btScalar aspect;
	float width = float(renderer->getScreenWidth());
	float height = float (renderer->getScreenHeight());

	aspect =  width / height;

	hor*=aspect;


	btVector3 rayToCenter = rayFrom + rayForward;
	btVector3 dHor = hor * 1.f/width;
	btVector3 dVert = vertical * 1.f/height;


	btVector3 rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical;
	rayTo += btScalar(x) * dHor;
	rayTo -= btScalar(y) * dVert;
	return rayTo;
}




void	PhysicsServerExample::vrControllerButtonCallback(int controllerId, int button, int state, float pos[4], float orn[4])
{
	//printf("controllerId %d, button=%d\n",controllerId, button);
	
	if (controllerId<0 || controllerId>=MAX_VR_CONTROLLERS)
		return;

	if (gGraspingController < 0)
	{
		gGraspingController = controllerId;
	}

	btTransform trLocal;
	trLocal.setIdentity();
	trLocal.setRotation(btQuaternion(btVector3(0, 0, 1), SIMD_HALF_PI)*btQuaternion(btVector3(0, 1, 0), SIMD_HALF_PI));

	btTransform trOrg;
	trOrg.setIdentity();
	trOrg.setOrigin(btVector3(pos[0], pos[1], pos[2]));
	trOrg.setRotation(btQuaternion(orn[0], orn[1], orn[2], orn[3]));

	btTransform tr2a;
	tr2a.setIdentity();
	btTransform tr2;
	tr2.setIdentity();



	tr2.setOrigin(m_physicsServer.getVRTeleportPosition());
	tr2a.setRotation(m_physicsServer.getVRTeleportOrientation());


	btTransform trTotal = tr2*tr2a*trOrg*trLocal;


	if (controllerId != gGraspingController)
	{
		if (button == 1 && state == 0)
		{
			
		}
	} else
	{
		if (button == 1)
		{
			if (state == 1)
			{
				gDebugRenderToggle = 1;
			} else
			{
				gDebugRenderToggle = 0;
#if 0//it confuses people, make it into a debug option in a VR GUI?
				if (simTimeScalingFactor==0)
				{
					simTimeScalingFactor = 1;
				} else
				{
					if (simTimeScalingFactor==1)
					{
						simTimeScalingFactor = 0.25;
					}
					else
					{
						simTimeScalingFactor = 0;
					}
				}
#endif
			}
		} else
		{
			
		}
	}


	if (button==32 && state==0)
	{

		if (controllerId == gGraspingController)
		{
		}
		else
		{
//			gEnableKukaControl = !gEnableKukaControl;
		}
	}
	

	if (button==1 && gEnableTeleporting)
	{
		m_args[0].m_isVrControllerTeleporting[controllerId] = true;
	}

	if (controllerId == gGraspingController && (button == 33))
	{
		
	}
	else
	{

		if (button == 33 && gEnablePicking)
		{
			m_args[0].m_isVrControllerPicking[controllerId] = (state != 0);
			m_args[0].m_isVrControllerReleasing[controllerId] = (state == 0);
		}
		

		if ((button == 33) || (button == 1))
		{
//			m_args[0].m_vrControllerPos[controllerId].setValue(pos[0] + gVRTeleportPos[0], pos[1] + gVRTeleportPos[1], pos[2] + gVRTeleportPos[2]);
	//		m_args[0].m_vrControllerOrn[controllerId].setValue(orn[0], orn[1], orn[2], orn[3]);
			m_args[0].m_vrControllerPos[controllerId] = trTotal.getOrigin();
			m_args[0].m_vrControllerOrn[controllerId] = trTotal.getRotation();
		}
		
	}

	m_args[0].m_csGUI->lock();
	m_args[0].m_vrControllerEvents[controllerId].m_controllerId = controllerId;
	m_args[0].m_vrControllerEvents[controllerId].m_deviceType = VR_DEVICE_CONTROLLER;
	m_args[0].m_vrControllerEvents[controllerId].m_pos[0] = trTotal.getOrigin()[0];
	m_args[0].m_vrControllerEvents[controllerId].m_pos[1] = trTotal.getOrigin()[1];
	m_args[0].m_vrControllerEvents[controllerId].m_pos[2] = trTotal.getOrigin()[2];
	m_args[0].m_vrControllerEvents[controllerId].m_orn[0] = trTotal.getRotation()[0];
	m_args[0].m_vrControllerEvents[controllerId].m_orn[1] = trTotal.getRotation()[1];
	m_args[0].m_vrControllerEvents[controllerId].m_orn[2] = trTotal.getRotation()[2];
	m_args[0].m_vrControllerEvents[controllerId].m_orn[3] = trTotal.getRotation()[3];
	m_args[0].m_vrControllerEvents[controllerId].m_numButtonEvents++;
	if (state)
	{
		m_args[0].m_vrControllerEvents[controllerId].m_buttons[button]|=eButtonIsDown+eButtonTriggered;
	} else
	{
		m_args[0].m_vrControllerEvents[controllerId].m_buttons[button]|=eButtonReleased;
		m_args[0].m_vrControllerEvents[controllerId].m_buttons[button] &= ~eButtonIsDown;
	}
	m_args[0].m_csGUI->unlock();
}


void	PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[4], float orn[4], float analogAxis, float auxAnalogAxes[10])
{

	if (controllerId < 0 || controllerId >= MAX_VR_CONTROLLERS)
	{
		printf("Controller Id exceeds max: %d > %d", controllerId, MAX_VR_CONTROLLERS);
		return;
	}

	btTransform trLocal;
	trLocal.setIdentity();
	trLocal.setRotation(btQuaternion(btVector3(0, 0, 1), SIMD_HALF_PI)*btQuaternion(btVector3(0, 1, 0), SIMD_HALF_PI));

	btTransform trOrg;
	trOrg.setIdentity();
	trOrg.setOrigin(btVector3(pos[0], pos[1], pos[2]));
	trOrg.setRotation(btQuaternion(orn[0], orn[1], orn[2], orn[3]));

	btTransform tr2a;
	tr2a.setIdentity();
	btTransform tr2;
	tr2.setIdentity();

	

	tr2.setOrigin(m_physicsServer.getVRTeleportPosition());
	tr2a.setRotation(m_physicsServer.getVRTeleportOrientation());


	btTransform trTotal = tr2*tr2a*trOrg*trLocal;

	if (controllerId == gGraspingController)
	{

	}
	else
	{
		m_args[0].m_vrControllerPos[controllerId] = trTotal.getOrigin();
		m_args[0].m_vrControllerOrn[controllerId] = trTotal.getRotation();
	}

	m_args[0].m_csGUI->lock();
	m_args[0].m_vrControllerEvents[controllerId].m_controllerId = controllerId;
	m_args[0].m_vrControllerEvents[controllerId].m_deviceType = VR_DEVICE_CONTROLLER;
	m_args[0].m_vrControllerEvents[controllerId].m_pos[0] = trTotal.getOrigin()[0];
	m_args[0].m_vrControllerEvents[controllerId].m_pos[1] = trTotal.getOrigin()[1];
	m_args[0].m_vrControllerEvents[controllerId].m_pos[2] = trTotal.getOrigin()[2];
	m_args[0].m_vrControllerEvents[controllerId].m_orn[0] = trTotal.getRotation()[0];
	m_args[0].m_vrControllerEvents[controllerId].m_orn[1] = trTotal.getRotation()[1];
	m_args[0].m_vrControllerEvents[controllerId].m_orn[2] = trTotal.getRotation()[2];
	m_args[0].m_vrControllerEvents[controllerId].m_orn[3] = trTotal.getRotation()[3];
	m_args[0].m_vrControllerEvents[controllerId].m_numMoveEvents++;
	m_args[0].m_vrControllerEvents[controllerId].m_analogAxis = analogAxis;
	for (int i=0;i<10;i++)
	{
		m_args[0].m_vrControllerEvents[controllerId].m_auxAnalogAxis[i] = auxAnalogAxes[i];
	}
	
	m_args[0].m_csGUI->unlock();

}

void	PhysicsServerExample::vrHMDMoveCallback(int controllerId, float pos[4], float orn[4])
{
		if (controllerId < 0 || controllerId >= MAX_VR_CONTROLLERS)
	{
		printf("Controller Id exceeds max: %d > %d", controllerId, MAX_VR_CONTROLLERS);
		return;
	}

	//we may need to add some trLocal transform, to align the camera to our preferences
	btTransform trLocal;
	trLocal.setIdentity();
//	trLocal.setRotation(btQuaternion(btVector3(0, 0, 1), SIMD_HALF_PI)*btQuaternion(btVector3(0, 1, 0), SIMD_HALF_PI));

	btTransform trOrg;
	trOrg.setIdentity();
	trOrg.setOrigin(btVector3(pos[0], pos[1], pos[2]));
	trOrg.setRotation(btQuaternion(orn[0], orn[1], orn[2], orn[3]));

	btTransform tr2a;
	tr2a.setIdentity();
	btTransform tr2;
	tr2.setIdentity();
	tr2.setOrigin(m_physicsServer.getVRTeleportPosition());
	tr2a.setRotation(m_physicsServer.getVRTeleportOrientation());
	btTransform trTotal = tr2*tr2a*trOrg*trLocal;



	m_args[0].m_csGUI->lock();
	m_args[0].m_vrControllerEvents[controllerId].m_controllerId = controllerId;
	m_args[0].m_vrControllerEvents[controllerId].m_deviceType = VR_DEVICE_HMD;
	m_args[0].m_vrControllerEvents[controllerId].m_pos[0] = trTotal.getOrigin()[0];
	m_args[0].m_vrControllerEvents[controllerId].m_pos[1] = trTotal.getOrigin()[1];
	m_args[0].m_vrControllerEvents[controllerId].m_pos[2] = trTotal.getOrigin()[2];
	m_args[0].m_vrControllerEvents[controllerId].m_orn[0] = trTotal.getRotation()[0];
	m_args[0].m_vrControllerEvents[controllerId].m_orn[1] = trTotal.getRotation()[1];
	m_args[0].m_vrControllerEvents[controllerId].m_orn[2] = trTotal.getRotation()[2];
	m_args[0].m_vrControllerEvents[controllerId].m_orn[3] = trTotal.getRotation()[3];
	m_args[0].m_vrControllerEvents[controllerId].m_numMoveEvents++;
	m_args[0].m_csGUI->unlock();
}
void	PhysicsServerExample::vrGenericTrackerMoveCallback(int controllerId, float pos[4], float orn[4])
{
	if (controllerId < 0 || controllerId >= MAX_VR_CONTROLLERS)
	{
		printf("Controller Id exceeds max: %d > %d", controllerId, MAX_VR_CONTROLLERS);
		return;
	}

		//we may need to add some trLocal transform, to align the camera to our preferences
	btTransform trLocal;
	trLocal.setIdentity();
	trLocal.setRotation(btQuaternion(btVector3(0, 0, 1), SIMD_HALF_PI)*btQuaternion(btVector3(0, 1, 0), SIMD_HALF_PI));


	btTransform trOrg;
	trOrg.setIdentity();
	trOrg.setOrigin(btVector3(pos[0], pos[1], pos[2]));
	trOrg.setRotation(btQuaternion(orn[0], orn[1], orn[2], orn[3]));

	btTransform tr2a;
	tr2a.setIdentity();
	btTransform tr2;
	tr2.setIdentity();
	tr2.setOrigin(m_physicsServer.getVRTeleportPosition());
	tr2a.setRotation(m_physicsServer.getVRTeleportOrientation());
	btTransform trTotal = tr2*tr2a*trOrg*trLocal;

	m_args[0].m_csGUI->lock();
	m_args[0].m_vrControllerEvents[controllerId].m_controllerId = controllerId;
	m_args[0].m_vrControllerEvents[controllerId].m_deviceType = VR_DEVICE_GENERIC_TRACKER;
	m_args[0].m_vrControllerEvents[controllerId].m_pos[0] = trTotal.getOrigin()[0];
	m_args[0].m_vrControllerEvents[controllerId].m_pos[1] = trTotal.getOrigin()[1];
	m_args[0].m_vrControllerEvents[controllerId].m_pos[2] = trTotal.getOrigin()[2];
	m_args[0].m_vrControllerEvents[controllerId].m_orn[0] = trTotal.getRotation()[0];
	m_args[0].m_vrControllerEvents[controllerId].m_orn[1] = trTotal.getRotation()[1];
	m_args[0].m_vrControllerEvents[controllerId].m_orn[2] = trTotal.getRotation()[2];
	m_args[0].m_vrControllerEvents[controllerId].m_orn[3] = trTotal.getRotation()[3];
	m_args[0].m_vrControllerEvents[controllerId].m_numMoveEvents++;
	m_args[0].m_csGUI->unlock();
}


extern int gSharedMemoryKey;


class CommonExampleInterface*    PhysicsServerCreateFuncInternal(struct CommonExampleOptions& options)
{

	MultiThreadedOpenGLGuiHelper* guiHelperWrapper = new MultiThreadedOpenGLGuiHelper(options.m_guiHelper->getAppInterface(),options.m_guiHelper, options.m_skipGraphicsUpdate);
	

  	PhysicsServerExample* example = new PhysicsServerExample(guiHelperWrapper, 
		options.m_commandProcessorCreation,
		options.m_sharedMem, 
		options.m_option);

	if (gSharedMemoryKey>=0)
	{
		example->setSharedMemoryKey(gSharedMemoryKey);
	}
	if (options.m_option & PHYSICS_SERVER_ENABLE_COMMAND_LOGGING)
	{
		example->enableCommandLogging();
	}
	if (options.m_option & PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG)
	{
		example->replayFromLogFile();
	}
	return example;

}