add single step simulation, using 'o' key. use 'i' key to suspend simulation first

default background color a bit darker, to show debug lines
tweaked contact point rendering a bit

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)
 	{
@@ -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,7 +81,16 @@ 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("");
                 args.m_fileName = "gripper/wsg50_with_r2d2_gripper.sdf";
@@ -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("");
@@ -179,19 +180,20 @@ public:
                 m_robotSim.setJointMotorControl(m_gripperIndex,fingerJointIndices[i],controlArgs);
             }
         }
-	m_robotSim.stepSimulation();
+		m_robotSim.stepSimulation();
     }
     virtual void	renderScene()
     {
 		m_robotSim.renderScene();
 
 		//m_app->m_renderer->renderScene();
+		
     }
 
 	
     virtual void	physicsDebugDraw()
     {
-      	
+		
     }
     virtual bool	mouseMoveCallback(float x,float y)
     {

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,94 +718,124 @@ 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)
-	
-	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++)
 	{
-		RobotSimThreadLocalStorage* storage = (RobotSimThreadLocalStorage*) m_data->m_threadSupport->getThreadLocalMemory(i);
+		m_data->m_multiThreadedHelper = new MultiThreadedOpenGLGuiHelper2(guiHelper->getAppInterface(), guiHelper);
-		b3Assert(storage);
+
-		storage->threadId = i;
+		MultiThreadedOpenGLGuiHelper2* guiHelperWrapper = new MultiThreadedOpenGLGuiHelper2(guiHelper->getAppInterface(), guiHelper);
-		//storage->m_sharedMem = data->m_sharedMem;
-	}
 
 
-		
 
-	for (int w=0;w<MAX_ROBOT_NUM_THREADS;w++)
+
-	{
+		m_data->m_threadSupport = createRobotSimThreadSupport(MAX_ROBOT_NUM_THREADS);
-		m_data->m_args[w].m_cs = m_data->m_threadSupport->createCriticalSection();
+
-		m_data->m_args[w].m_cs->setSharedParam(0,eRobotSimIsUnInitialized);
+		for (int i = 0; i < m_data->m_threadSupport->getNumTasks(); i++)
-		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);
-		while (m_data->m_args[w].m_cs->getSharedParam(0)==eRobotSimIsUnInitialized)
 		{
+			RobotSimThreadLocalStorage* storage = (RobotSimThreadLocalStorage*)m_data->m_threadSupport->getThreadLocalMemory(i);
+			b3Assert(storage);
+			storage->threadId = i;
+			//storage->m_sharedMem = data->m_sharedMem;
 		}
+
+
+
+
+		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);
+			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);
+			while (m_data->m_args[w].m_cs->getSharedParam(0) == eRobotSimIsUnInitialized)
+			{
+			}
+		}
+
+		m_data->m_args[0].m_cs->setSharedParam(1, eRobotSimGUIHelperIdle);
+		m_data->m_multiThreadedHelper->setCriticalSection(m_data->m_args[0].m_cs);
+
+		bool serverConnected = m_data->m_physicsServer.connectSharedMemory(m_data->m_multiThreadedHelper);
+		b3Assert(serverConnected);
+
+		m_data->m_physicsClient = b3ConnectSharedMemory(SHARED_MEMORY_KEY);
+	}
+	else
+	{
+		m_data->m_clientServerDirect = new PhysicsDirect();
+		bool connected = m_data->m_clientServerDirect->connect(guiHelper);
+		m_data->m_physicsClient = (b3PhysicsClientHandle)m_data->m_clientServerDirect;
+
 	}
 
-	m_data->m_args[0].m_cs->setSharedParam(1,eRobotSimGUIHelperIdle);
+	//client connected
-	m_data->m_multiThreadedHelper->setCriticalSection(m_data->m_args[0].m_cs);
+	m_data->m_connected = b3CanSubmitCommand(m_data->m_physicsClient);
-
+	
-	m_data->m_connected = m_data->m_physicsServer.connectSharedMemory( m_data->m_multiThreadedHelper);
+	b3Assert(m_data->m_connected);
-
+	return m_data->m_connected && m_data->m_connected;
-		b3Assert(m_data->m_connected);
-
-	m_data->m_physicsClient = b3ConnectSharedMemory(SHARED_MEMORY_KEY);
-	int canSubmit = b3CanSubmitCommand(m_data->m_physicsClient);
-	b3Assert(canSubmit);
-	return m_data->m_connected && canSubmit;
 }
 
 void b3RobotSimAPI::disconnect()
 {
-
+	if (m_data->m_useMultiThreading)
-	for (int i=0;i<MAX_ROBOT_NUM_THREADS;i++)
 	{
-		m_data->m_args[i].m_cs->lock();
+		for (int i = 0; i < MAX_ROBOT_NUM_THREADS; i++)
-		m_data->m_args[i].m_cs->setSharedParam(0,eRequestTerminateRobotSim);
+		{
-		m_data->m_args[i].m_cs->unlock();
+			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->unlock();
+		}
+		int numActiveThreads = MAX_ROBOT_NUM_THREADS;
+
+		while (numActiveThreads)
+		{
+			int arg0, arg1;
+			if (m_data->m_threadSupport->isTaskCompleted(&arg0, &arg1, 0))
+			{
+				numActiveThreads--;
+				printf("numActiveThreads = %d\n", numActiveThreads);
+
+			}
+			else
+			{
+			}
+		};
+
+		printf("stopping threads\n");
+
+		delete m_data->m_threadSupport;
+		m_data->m_threadSupport = 0;
 	}
-	int numActiveThreads = MAX_ROBOT_NUM_THREADS;
-
-	while (numActiveThreads)
-            {
-		int arg0,arg1;
-                    if (m_data->m_threadSupport->isTaskCompleted(&arg0,&arg1,0))
-                    {
-                            numActiveThreads--;
-                            printf("numActiveThreads = %d\n",numActiveThreads);
-
-                    } else
-                    {
-                    }
-            };
-
-	printf("stopping threads\n");
-
-	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_multiThreadedHelper->m_childGuiHelper->getRenderInterface())
+	if (m_data->m_useMultiThreading)
 	{
-		m_data->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->writeTransforms();
+		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

@@ -29,9 +29,10 @@ public:
     
     virtual ~PhysicsDirect();
 
-	  // return true if connection succesfull, can also check 'isConnected'
+	// return true if connection succesfull, can also check 'isConnected'
+	//it is OK to pass a null pointer for the gui helper
     virtual bool connect();
-
+	
 	////todo: rename to 'disconnect'
     virtual void disconnectSharedMemory();
 
@@ -62,7 +63,12 @@ public:
     virtual const float* getDebugLinesColor() const;
 
 	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

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,