Merge pull request #748 from erwincoumans/master

avoid MT crash in VR/physics server due to printf from separate thread.

examples/CommonInterfaces/CommonGraphicsAppInterface.h

@@ -66,9 +66,9 @@ struct CommonGraphicsApp
 		m_mouseYpos(0.f),
 		m_mouseInitialized(false)
 	{
-		m_backgroundColorRGB[0] = 0.9;
+		m_backgroundColorRGB[0] = 0.7;
-		m_backgroundColorRGB[1] = 0.9;
+		m_backgroundColorRGB[1] = 0.7;
-		m_backgroundColorRGB[2] = 1;
+		m_backgroundColorRGB[2] = 0.8;
 	}
 	virtual ~CommonGraphicsApp()
 	{

examples/ExampleBrowser/OpenGLExampleBrowser.cpp

@@ -123,6 +123,7 @@ static bool enable_experimental_opencl = false;
 
 int gDebugDrawFlags = 0;
 static bool pauseSimulation=false;
+static bool singleStepSimulation = false;
 int midiBaseIndex = 176;
 extern bool gDisableDeactivation;
 
@@ -227,6 +228,12 @@ void MyKeyboardCallback(int key, int state)
 	{
 		pauseSimulation = !pauseSimulation;
 	}
+	if (key == 'o' && state)
+	{
+		singleStepSimulation = true;
+	}
+
+
 #ifndef NO_OPENGL3
 	if (key=='s' && state)
 	{
@@ -490,7 +497,7 @@ void	MyComboBoxCallback(int comboId, const char* item)
 void MyGuiPrintf(const char* msg)
 {
 	printf("b3Printf: %s\n",msg);
-	if (gui2)
+	if (!gDisableDemoSelection)
 	{
 		gui2->textOutput(msg);
 		gui2->forceUpdateScrollBars();
@@ -502,7 +509,7 @@ void MyGuiPrintf(const char* msg)
 void MyStatusBarPrintf(const char* msg)
 {
 	printf("b3Printf: %s\n", msg);
-	if (gui2)
+	if (!gDisableDemoSelection)
 	{
 		bool isLeft = true;
 		gui2->setStatusBarMessage(msg,isLeft);
@@ -513,7 +520,7 @@ void MyStatusBarPrintf(const char* msg)
 void MyStatusBarError(const char* msg)
 {
 	printf("Warning: %s\n", msg);
-	if (gui2)
+	if (!gDisableDemoSelection)
 	{
 		bool isLeft = false;
 		gui2->setStatusBarMessage(msg,isLeft);
@@ -1124,12 +1131,12 @@ void OpenGLExampleBrowser::update(float deltaTime)
 		
 		if (sCurrentDemo)
 		{
-			if (!pauseSimulation)
+			if (!pauseSimulation || singleStepSimulation)
 			{
+				singleStepSimulation = false;
 				//printf("---------------------------------------------------\n");
 				//printf("Framecount = %d\n",frameCount);
 				
-
 				if (gFixedTimeStep>0)
 				{
 					sCurrentDemo->stepSimulation(gFixedTimeStep);

examples/ExampleBrowser/OpenGLGuiHelper.cpp

@@ -84,7 +84,10 @@ public:
 
 	virtual void	drawContactPoint(const btVector3& PointOnB,const btVector3& normalOnB,btScalar distance,int lifeTime,const btVector3& color)
 	{
-        drawLine(PointOnB,PointOnB+normalOnB,color);
+        drawLine(PointOnB,PointOnB+normalOnB*distance,color);
+		btVector3 red(0.3, 1., 0.3);
+		drawLine(PointOnB, PointOnB + normalOnB*0.01, red);
+		
 	}
      
 

examples/RoboticsLearning/GripperGraspExample.cpp

@@ -58,7 +58,7 @@ public:
     
     virtual void physicsDebugDraw(int debugDrawMode)
     {
-        
+		m_robotSim.debugDraw(debugDrawMode);
     }
     virtual void    initPhysics()
     {
@@ -81,6 +81,15 @@ public:
                 slider.m_maxVal=1;
                 m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
             }
+			{
+				b3RobotSimLoadFileArgs args("");
+				b3RobotSimLoadFileResults results;
+				args.m_fileName = "cube_small.urdf";
+				args.m_startPosition.setValue(0, 0, .107);
+				args.m_startOrientation.setEulerZYX(0, 0, 0);
+				args.m_useMultiBody = true;
+				m_robotSim.loadFile(args, results);
+			}
 
             {
                 b3RobotSimLoadFileArgs args("");
@@ -129,15 +138,7 @@ public:
                     }
                 }
             }
-            {
+           
-                b3RobotSimLoadFileArgs args("");
-                b3RobotSimLoadFileResults results;
-                args.m_fileName = "cube_small.urdf";
-                args.m_startPosition.setValue(0,0,.107);
-                args.m_startOrientation.setEulerZYX(0,0,0);
-                args.m_useMultiBody = false;
-                m_robotSim.loadFile(args,results);
-            }
             if (1)
             {
                 b3RobotSimLoadFileArgs args("");
@@ -186,6 +187,7 @@ public:
 		m_robotSim.renderScene();
 
 		//m_app->m_renderer->renderScene();
+		
     }
 
 	

examples/RoboticsLearning/b3RobotSimAPI.cpp

@@ -7,15 +7,15 @@
 //#include "../CommonInterfaces/CommonExampleInterface.h"
 #include "../CommonInterfaces/CommonGUIHelperInterface.h"
 #include "../SharedMemory/PhysicsServerSharedMemory.h"
+#include "../SharedMemory/PhysicsServerSharedMemory.h"
 #include "../SharedMemory/PhysicsClientC_API.h"
+#include "../SharedMemory/PhysicsDirectC_API.h"
+#include "../SharedMemory/PhysicsDirect.h"
+
 #include <string>
-
-
-
-
-
-
 #include "../Utils/b3Clock.h"
+
+
 #include "../MultiThreading/b3ThreadSupportInterface.h"
 
 
@@ -406,24 +406,27 @@ public:
 
 
 
-
-
-
 struct b3RobotSimAPI_InternalData
 {
 	//GUIHelperInterface* m_guiHelper;
 	PhysicsServerSharedMemory	m_physicsServer;
 	b3PhysicsClientHandle m_physicsClient;
 
+
 	b3ThreadSupportInterface* m_threadSupport;
 	RobotSimArgs m_args[MAX_ROBOT_NUM_THREADS];
 	MultiThreadedOpenGLGuiHelper2* m_multiThreadedHelper;
+	PhysicsDirect* m_clientServerDirect;
 
+	bool m_useMultiThreading;
 	bool m_connected;
 
 	b3RobotSimAPI_InternalData()
-		:m_multiThreadedHelper(0),
+		:m_threadSupport(0),
+		m_multiThreadedHelper(0),
+		m_clientServerDirect(0),
 		m_physicsClient(0),
+		m_useMultiThreading(false),
 		m_connected(false)
 	{
 	}
@@ -464,6 +467,9 @@ b3RobotSimAPI::~b3RobotSimAPI()
 
 void b3RobotSimAPI::processMultiThreadedGraphicsRequests()
 {
+	if (0==m_data->m_multiThreadedHelper)
+		return;
+
 	switch (m_data->m_multiThreadedHelper->getCriticalSection()->getSharedParam(1))
 	{
 	case eRobotSimGUIHelperCreateCollisionShapeGraphicsObject:
@@ -563,27 +569,6 @@ void b3RobotSimAPI::processMultiThreadedGraphicsRequests()
 		}
 	}
 	
-
-
-
-	#if 0
-	if (m_options == PHYSICS_SERVER_USE_RTC_CLOCK)
-	{
-		btClock rtc;
-		btScalar endTime = rtc.getTimeMilliseconds() + deltaTime*btScalar(800);
-
-		while (rtc.getTimeMilliseconds()<endTime)
-		{
-			m_physicsServer.processClientCommands();
-		}
-	} else
-	{
-        //for (int i=0;i<10;i++)
-			m_physicsServer.processClientCommands();
-	}
-	#endif
-
-	
 }
 
 b3SharedMemoryStatusHandle b3RobotSimAPI::submitClientCommandAndWaitStatusMultiThreaded(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle)
@@ -733,18 +718,20 @@ bool b3RobotSimAPI::loadFile(const struct b3RobotSimLoadFileArgs& args, b3RobotS
 
 bool b3RobotSimAPI::connect(GUIHelperInterface* guiHelper)
 {
-	m_data->m_multiThreadedHelper  = new MultiThreadedOpenGLGuiHelper2(guiHelper->getAppInterface(),guiHelper);
+	if (m_data->m_useMultiThreading)
+	{
+		m_data->m_multiThreadedHelper = new MultiThreadedOpenGLGuiHelper2(guiHelper->getAppInterface(), guiHelper);
 
-	MultiThreadedOpenGLGuiHelper2* guiHelperWrapper = new MultiThreadedOpenGLGuiHelper2(guiHelper->getAppInterface(),guiHelper);
+		MultiThreadedOpenGLGuiHelper2* guiHelperWrapper = new MultiThreadedOpenGLGuiHelper2(guiHelper->getAppInterface(), guiHelper);
 
 
 
 
 		m_data->m_threadSupport = createRobotSimThreadSupport(MAX_ROBOT_NUM_THREADS);
 
-	for (int i=0;i<m_data->m_threadSupport->getNumTasks();i++)
+		for (int i = 0; i < m_data->m_threadSupport->getNumTasks(); i++)
 		{
-		RobotSimThreadLocalStorage* storage = (RobotSimThreadLocalStorage*) m_data->m_threadSupport->getThreadLocalMemory(i);
+			RobotSimThreadLocalStorage* storage = (RobotSimThreadLocalStorage*)m_data->m_threadSupport->getThreadLocalMemory(i);
 			b3Assert(storage);
 			storage->threadId = i;
 			//storage->m_sharedMem = data->m_sharedMem;
@@ -753,54 +740,66 @@ bool b3RobotSimAPI::connect(GUIHelperInterface* guiHelper)
 
 
 
-	for (int w=0;w<MAX_ROBOT_NUM_THREADS;w++)
+		for (int w = 0; w < MAX_ROBOT_NUM_THREADS; w++)
 		{
 			m_data->m_args[w].m_cs = m_data->m_threadSupport->createCriticalSection();
-		m_data->m_args[w].m_cs->setSharedParam(0,eRobotSimIsUnInitialized);
+			m_data->m_args[w].m_cs->setSharedParam(0, eRobotSimIsUnInitialized);
 			int numMoving = 0;
 			m_data->m_args[w].m_positions.resize(numMoving);
 			m_data->m_args[w].m_physicsServerPtr = &m_data->m_physicsServer;
 			int index = 0;
 
-		m_data->m_threadSupport->runTask(B3_THREAD_SCHEDULE_TASK, (void*) &m_data->m_args[w], w);
+			m_data->m_threadSupport->runTask(B3_THREAD_SCHEDULE_TASK, (void*)&m_data->m_args[w], w);
-		while (m_data->m_args[w].m_cs->getSharedParam(0)==eRobotSimIsUnInitialized)
+			while (m_data->m_args[w].m_cs->getSharedParam(0) == eRobotSimIsUnInitialized)
 			{
 			}
 		}
 
-	m_data->m_args[0].m_cs->setSharedParam(1,eRobotSimGUIHelperIdle);
+		m_data->m_args[0].m_cs->setSharedParam(1, eRobotSimGUIHelperIdle);
 		m_data->m_multiThreadedHelper->setCriticalSection(m_data->m_args[0].m_cs);
 
-	m_data->m_connected = m_data->m_physicsServer.connectSharedMemory( m_data->m_multiThreadedHelper);
+		bool serverConnected = m_data->m_physicsServer.connectSharedMemory(m_data->m_multiThreadedHelper);
-
+		b3Assert(serverConnected);
-		b3Assert(m_data->m_connected);
 
 		m_data->m_physicsClient = b3ConnectSharedMemory(SHARED_MEMORY_KEY);
-	int canSubmit = b3CanSubmitCommand(m_data->m_physicsClient);
+	}
-	b3Assert(canSubmit);
+	else
-	return m_data->m_connected && canSubmit;
+	{
+		m_data->m_clientServerDirect = new PhysicsDirect();
+		bool connected = m_data->m_clientServerDirect->connect(guiHelper);
+		m_data->m_physicsClient = (b3PhysicsClientHandle)m_data->m_clientServerDirect;
+
+	}
+
+	//client connected
+	m_data->m_connected = b3CanSubmitCommand(m_data->m_physicsClient);
+	
+	b3Assert(m_data->m_connected);
+	return m_data->m_connected && m_data->m_connected;
 }
 
 void b3RobotSimAPI::disconnect()
 {
-
+	if (m_data->m_useMultiThreading)
-	for (int i=0;i<MAX_ROBOT_NUM_THREADS;i++)
+	{
+		for (int i = 0; i < MAX_ROBOT_NUM_THREADS; i++)
 		{
 			m_data->m_args[i].m_cs->lock();
-		m_data->m_args[i].m_cs->setSharedParam(0,eRequestTerminateRobotSim);
+			m_data->m_args[i].m_cs->setSharedParam(0, eRequestTerminateRobotSim);
 			m_data->m_args[i].m_cs->unlock();
 		}
 		int numActiveThreads = MAX_ROBOT_NUM_THREADS;
 
 		while (numActiveThreads)
 		{
-		int arg0,arg1;
+			int arg0, arg1;
-                    if (m_data->m_threadSupport->isTaskCompleted(&arg0,&arg1,0))
+			if (m_data->m_threadSupport->isTaskCompleted(&arg0, &arg1, 0))
 			{
 				numActiveThreads--;
-                            printf("numActiveThreads = %d\n",numActiveThreads);
+				printf("numActiveThreads = %d\n", numActiveThreads);
 
-                    } else
+			}
+			else
 			{
 			}
 		};
@@ -809,18 +808,34 @@ void b3RobotSimAPI::disconnect()
 
 		delete m_data->m_threadSupport;
 		m_data->m_threadSupport = 0;
-
+	}
 	b3DisconnectSharedMemory(m_data->m_physicsClient);
 	m_data->m_physicsServer.disconnectSharedMemory(true);
+	
 	m_data->m_connected = false;
 }
 
+
+void b3RobotSimAPI::debugDraw(int debugDrawMode)
+{
+	if (m_data->m_clientServerDirect)
+	{
+		m_data->m_clientServerDirect->debugDraw(debugDrawMode);
+	}
+}
 void b3RobotSimAPI::renderScene()
 {
+	if (m_data->m_useMultiThreading)
+	{
 		if (m_data->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface())
 		{
 			m_data->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->writeTransforms();
 		}
+	}
 
+	if (m_data->m_clientServerDirect)
+	{
+		m_data->m_clientServerDirect->renderScene();
+	}
 	m_data->m_physicsServer.renderScene();
 }

examples/RoboticsLearning/b3RobotSimAPI.h

@@ -95,6 +95,7 @@ public:
 	void setGravity(const b3Vector3& gravityAcceleration);
 
 	void renderScene();
+	void debugDraw(int debugDrawMode);
 };
 
 #endif //B3_ROBOT_SIM_API_H

examples/SharedMemory/PhysicsDirect.cpp

@@ -69,9 +69,27 @@ PhysicsDirect::~PhysicsDirect()
 bool PhysicsDirect::connect()
 {
 	m_data->m_commandProcessor->setGuiHelper(&m_data->m_noGfx);
+
 	return true;
 }
 
+// return true if connection succesfull, can also check 'isConnected'
+bool PhysicsDirect::connect(struct GUIHelperInterface* guiHelper)
+{
+	m_data->m_commandProcessor->setGuiHelper(guiHelper);
+	
+	return true;
+}
+
+void PhysicsDirect::renderScene()
+{
+	m_data->m_commandProcessor->renderScene();
+}
+void PhysicsDirect::debugDraw(int debugDrawMode)
+{
+	m_data->m_commandProcessor->physicsDebugDraw(debugDrawMode);
+}
+
 ////todo: rename to 'disconnect'
 void PhysicsDirect::disconnectSharedMemory()
 {

examples/SharedMemory/PhysicsDirect.h

@@ -30,6 +30,7 @@ public:
     virtual ~PhysicsDirect();
 
 	// return true if connection succesfull, can also check 'isConnected'
+	//it is OK to pass a null pointer for the gui helper
     virtual bool connect();
 	
 	////todo: rename to 'disconnect'
@@ -63,6 +64,11 @@ public:
 
 	virtual void getCachedCameraImage(b3CameraImageData* cameraData);
 
+	//those 2 APIs are for internal use for visualization
+	virtual bool connect(struct GUIHelperInterface* guiHelper);
+	virtual void renderScene();
+	virtual void debugDraw(int debugDrawMode);
+
 };
 
 #endif //PHYSICS_DIRECT_H

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -2445,7 +2445,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
 			loadUrdf("plane.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, true, &bodyId, &bufferServerToClient[0], bufferServerToClient.size());
 		}
 
-		m_data->m_dynamicsWorld->stepSimulation(dtInSec);
+		m_data->m_dynamicsWorld->stepSimulation(dtInSec,10,1./240.);
 	}
 }
 

examples/SharedMemory/PhysicsServerExample.cpp

@@ -872,7 +872,22 @@ void PhysicsServerExample::renderScene()
 		static int frameCount=0;
 		frameCount++;
 		char bla[1024];
-		sprintf(bla,"VR sub-title text test, frame %d", frameCount/2);
+
+		static btScalar prevTime = m_clock.getTimeSeconds();
+		btScalar curTime = m_clock.getTimeSeconds();
+		static btScalar deltaTime = 0.f;
+		static int count = 10;
+		if (count-- < 0)
+		{
+			count = 10;
+			deltaTime = curTime - prevTime;
+		}
+		if (deltaTime == 0)
+			deltaTime = 1000;
+
+		prevTime = curTime;
+		
+		sprintf(bla,"VR sub-title text test,fps = %f, frame %d", 1./deltaTime, frameCount/2);
 		float pos[4];
 		m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->getCameraTargetPosition(pos);
 		btTransform viewTr;

src/LinearMath/btIDebugDraw.h

@@ -166,9 +166,9 @@ class	btIDebugDraw
 	virtual void drawTransform(const btTransform& transform, btScalar orthoLen)
 	{
 		btVector3 start = transform.getOrigin();
-		drawLine(start, start+transform.getBasis() * btVector3(orthoLen, 0, 0), btVector3(0.7f,0,0));
+		drawLine(start, start+transform.getBasis() * btVector3(orthoLen, 0, 0), btVector3(1.f,0.3,0.3));
-		drawLine(start, start+transform.getBasis() * btVector3(0, orthoLen, 0), btVector3(0,0.7f,0));
+		drawLine(start, start+transform.getBasis() * btVector3(0, orthoLen, 0), btVector3(0.3,1.f, 0.3));
-		drawLine(start, start+transform.getBasis() * btVector3(0, 0, orthoLen), btVector3(0,0,0.7f));
+		drawLine(start, start+transform.getBasis() * btVector3(0, 0, orthoLen), btVector3(0.3, 0.3,1.f));
 	}
 
 	virtual void drawArc(const btVector3& center, const btVector3& normal, const btVector3& axis, btScalar radiusA, btScalar radiusB, btScalar minAngle, btScalar maxAngle,