pybullet, more robust multi-server connections

Windows shared memory: allow to use custom key.
Improve GUI performance on Windows, submit letters in text as a batch (fewer draw-calls)
quadruped.py: first try to connect to SHARED_MEMORY, if it fails (<0) use GUI
increase Chrome about://tracing json export capacity (press 'p' in Example Browser)
UDP physics server: add --port and --sharedMemoryKey command-line arguments
PhysicsServerExample: add --sharedMemoryKey command-line option (for VR example too)
ExampleBrowser: sleep a few milliseconds if rendering is too fast, use --minUpdateTimeMicroSecs=0 to disable

examples/ExampleBrowser/InProcessExampleBrowser.cpp

@@ -224,6 +224,8 @@ enum TestExampleBrowserCommunicationEnums
 	eExampleBrowserHasTerminated
 };
 
+static double gMinUpdateTimeMicroSecs = 4000.;
+
 void	ExampleBrowserThreadFunc(void* userPtr,void* lsMemory)
 {
 	printf("ExampleBrowserThreadFunc started\n");
@@ -254,8 +256,20 @@ void	ExampleBrowserThreadFunc(void* userPtr,void* lsMemory)
 		do
 		{
 			float deltaTimeInSeconds = clock.getTimeMicroseconds()/1000000.f;
-			clock.reset();
+			{
-			exampleBrowser->update(deltaTimeInSeconds);
+				if (deltaTimeInSeconds > 0.1)
+				{
+					deltaTimeInSeconds = 0.1;
+				}
+				if (deltaTimeInSeconds < (gMinUpdateTimeMicroSecs/1e6))
+				{
+					clock.usleep(gMinUpdateTimeMicroSecs/10.);
+				} else
+				{
+					clock.reset();
+					exampleBrowser->update(deltaTimeInSeconds);
+				}
+			}
 
 		} while (!exampleBrowser->requestedExit() && (args->m_cs->getSharedParam(0)!=eRequestTerminateExampleBrowser));
 	} else

examples/ExampleBrowser/OpenGLExampleBrowser.cpp

@@ -163,7 +163,7 @@ FILE* gTimingFile = 0;
 #define __STDC_FORMAT_MACROS
 #endif //__STDC_FORMAT_MACROS
 #include <inttypes.h>
-#define BT_TIMING_CAPACITY 65536
+#define BT_TIMING_CAPACITY 16*65536
 static bool m_firstTiming = true;
 
 
@@ -266,7 +266,10 @@ struct btTimings
 			return;
 		}
 
-		
+		if (m_timings[0].size()==0)
+		{
+			m_timings[0].resize(BT_TIMING_CAPACITY);
+		}
 
 		int slot = m_numTimings++;
 
@@ -279,7 +282,7 @@ struct btTimings
 
 	int m_numTimings;
 	int m_activeBuffer;
-	btTiming m_timings[2][BT_TIMING_CAPACITY];
+	btAlignedObjectArray<btTiming> m_timings[1];
 };
 
 btTimings gTimings[BT_MAX_THREAD_COUNT];
@@ -729,11 +732,13 @@ void	MyComboBoxCallback(int comboId, const char* item)
 
 }
 
+//in case of multi-threading, don't submit messages while the GUI is rendering (causing crashes)
+static bool gBlockGuiMessages = false;
 
 void MyGuiPrintf(const char* msg)
 {
 	printf("b3Printf: %s\n",msg);
-	if (!gDisableDemoSelection)
+	if (!gDisableDemoSelection && !gBlockGuiMessages)
 	{
 		gui2->textOutput(msg);
 		gui2->forceUpdateScrollBars();
@@ -745,7 +750,7 @@ void MyGuiPrintf(const char* msg)
 void MyStatusBarPrintf(const char* msg)
 {
 	printf("b3Printf: %s\n", msg);
-	if (!gDisableDemoSelection)
+	if (!gDisableDemoSelection && !gBlockGuiMessages)
 	{
 		bool isLeft = true;
 		gui2->setStatusBarMessage(msg,isLeft);
@@ -756,7 +761,7 @@ void MyStatusBarPrintf(const char* msg)
 void MyStatusBarError(const char* msg)
 {
 	printf("Warning: %s\n", msg);
-	if (!gDisableDemoSelection)
+	if (!gDisableDemoSelection && !gBlockGuiMessages)
 	{
 		bool isLeft = false;
 		gui2->setStatusBarMessage(msg,isLeft);
@@ -1016,7 +1021,22 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[])
     b3CommandLineArgs args(argc,argv);
     
 	loadCurrentSettings(startFileName, args);
+	if (args.CheckCmdLineFlag("nogui"))
+	{
+		renderGrid = false;
+		renderGui = false;
+	}
+	if (args.CheckCmdLineFlag("tracing"))
+	{
+		m_firstTiming = true;
+		gProfileDisabled = false;//true;
+		b3SetCustomEnterProfileZoneFunc(MyEnterProfileZoneFunc);
+		b3SetCustomLeaveProfileZoneFunc(MyLeaveProfileZoneFunc);
 
+		//also for Bullet 2.x API
+		btSetCustomEnterProfileZoneFunc(MyEnterProfileZoneFunc);
+		btSetCustomLeaveProfileZoneFunc(MyLeaveProfileZoneFunc);
+	}
 	args.GetCmdLineArgument("fixed_timestep",gFixedTimeStep);
 	args.GetCmdLineArgument("png_skip_frames", gPngSkipFrames);	
 	///The OpenCL rigid body pipeline is experimental and 
@@ -1029,6 +1049,7 @@ bool OpenGLExampleBrowser::init(int argc, char* argv[])
 		enable_experimental_opencl = true;
 		gAllExamples->initOpenCLExampleEntries();
 	}
+	
 	if (args.CheckCmdLineFlag("disable_retina"))
 	{
 		gAllowRetina = false;
@@ -1456,7 +1477,11 @@ void OpenGLExampleBrowser::update(float deltaTime)
 			
 			if (m_internalData->m_gui)
 			{
+				gBlockGuiMessages = true;
 				m_internalData->m_gui->draw(s_instancingRenderer->getScreenWidth(), s_instancingRenderer->getScreenHeight());
+				
+
+				gBlockGuiMessages = false;
 			}
 			
             if (sUseOpenGL2)

examples/ExampleBrowser/main.cpp

@@ -19,13 +19,15 @@
 
 #include "LinearMath/btAlignedAllocator.h"
 
+static double gMinUpdateTimeMicroSecs = 1000.;
+
 
 int main(int argc, char* argv[])
 {
 	{
 		b3CommandLineArgs args(argc, argv);
 		b3Clock clock;
-
+		args.GetCmdLineArgument("minUpdateTimeMicroSecs",gMinUpdateTimeMicroSecs);
 
 		ExampleEntriesAll examples;
 		examples.initExampleEntries();
@@ -45,9 +47,18 @@ int main(int argc, char* argv[])
 			do
 			{
 				float deltaTimeInSeconds = clock.getTimeMicroseconds() / 1000000.f;
-				clock.reset();
+				if (deltaTimeInSeconds > 0.1)
-				exampleBrowser->update(deltaTimeInSeconds);
+				{
-
+					deltaTimeInSeconds = 0.1;
+				}
+				if (deltaTimeInSeconds < (gMinUpdateTimeMicroSecs/1e6))
+				{
+					b3Clock::usleep(gMinUpdateTimeMicroSecs/10.);
+				} else
+				{
+					clock.reset();
+					exampleBrowser->update(deltaTimeInSeconds);
+				}
 			} while (!exampleBrowser->requestedExit());
 		}
 		delete exampleBrowser;

examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp

@@ -8,7 +8,6 @@
 #include "Bullet3Common/b3FileUtils.h"
 #include "../../ThirdPartyLibs/stb_image/stb_image.h"
 
-
 bool b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(const std::string& fileName, b3ImportMeshData& meshData)
 {
 

examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp

@@ -447,6 +447,8 @@ static btCollisionShape* createConvexHullFromShapes(std::vector<tinyobj::shape_t
 
 btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, const char* urdfPathPrefix)
 {
+	BT_PROFILE("convertURDFToCollisionShape");
+
 	btCollisionShape* shape = 0;
 
     switch (collision->m_geometry.m_type)
@@ -677,6 +679,7 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
 							
 							if (collision->m_flags & URDF_FORCE_CONCAVE_TRIMESH)
 							{
+								BT_PROFILE("convert trimesh");
 								btTriangleMesh* meshInterface = new btTriangleMesh();
 								for (int i=0;i<glmesh->m_numIndices/3;i++)
 								{
@@ -692,6 +695,8 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
 								shape = trimesh;
 							} else
 							{
+								BT_PROFILE("convert btConvexHullShape");
+
 								btConvexHullShape* convexHull = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3));
 								convexHull->optimizeConvexHull();
 								//convexHull->initializePolyhedralFeatures();
@@ -727,6 +732,7 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
 
 static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MyTexture>& texturesOut)
 {
+	BT_PROFILE("convertURDFToVisualShapeInternal");
 
 	
 	GLInstanceGraphicsShape* glmesh = 0;
@@ -981,6 +987,8 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha
 	//if we have a convex, tesselate into localVertices/localIndices
 	if ((glmesh==0) && convexColShape)
 	{
+		BT_PROFILE("convexColShape");
+
 		btShapeHull* hull = new btShapeHull(convexColShape);
 		hull->buildHull(0.0);
 		{
@@ -1029,7 +1037,7 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha
 	
 	if (glmesh && glmesh->m_numIndices>0 && glmesh->m_numvertices >0)
 	{
-
+		BT_PROFILE("glmesh");
 		int baseIndex = verticesOut.size();
 
 

examples/OpenGLWindow/GLPrimInternalData.h

@@ -13,8 +13,13 @@ struct PrimInternalData
     GLint m_positionAttribute;
     GLint m_textureAttribute;
     GLuint m_vertexBuffer;
-    GLuint m_vertexArrayObject;
+	GLuint m_vertexBuffer2;
+
+	GLuint m_vertexArrayObject;
+	GLuint m_vertexArrayObject2;
+
     GLuint  m_indexBuffer;
+    GLuint  m_indexBuffer2;
     GLuint m_texturehandle;
 };
 

examples/OpenGLWindow/GLPrimitiveRenderer.cpp

@@ -48,10 +48,20 @@ static const char* fragmentShader3D= \
 
 
 static unsigned int s_indexData[6] = {0,1,2,0,2,3};
+#define MAX_VERTICES2 8192
+struct PrimInternalData2
+{
+	PrimInternalData2()
+		:m_numVerticesText(0),
+		m_numVerticesRect(0)
+	{
+	}
+	int m_numVerticesText;
+	int m_numVerticesRect;
+	PrimVertex m_verticesText[MAX_VERTICES2];
+	PrimVertex m_verticesRect[MAX_VERTICES2];
 
-
+};
-   
-
 
 GLPrimitiveRenderer::GLPrimitiveRenderer(int screenWidth, int screenHeight)
 :m_screenWidth(screenWidth),
@@ -59,6 +69,7 @@ m_screenHeight(screenHeight)
 {
     
 	m_data = new PrimInternalData;
+	m_data2 = new PrimInternalData2;
 
     m_data->m_shaderProg = gltLoadShaderPair(vertexShader3D,fragmentShader3D);
 	
@@ -109,6 +120,13 @@ void GLPrimitiveRenderer::loadBufferData()
     glBindBuffer(GL_ARRAY_BUFFER, m_data->m_vertexBuffer);
     glBufferData(GL_ARRAY_BUFFER, 4 * sizeof(PrimVertex), vertexData, GL_DYNAMIC_DRAW);
     
+	glGenVertexArrays(1, &m_data->m_vertexArrayObject2);
+    glBindVertexArray(m_data->m_vertexArrayObject2);
+    glGenBuffers(1, &m_data->m_vertexBuffer2);
+    glBindBuffer(GL_ARRAY_BUFFER, m_data->m_vertexBuffer2);
+    glBufferData(GL_ARRAY_BUFFER,  MAX_VERTICES2 * sizeof(PrimVertex), 0, GL_DYNAMIC_DRAW);
+   
+
     assert(glGetError()==GL_NO_ERROR);
     
     
@@ -116,7 +134,23 @@ void GLPrimitiveRenderer::loadBufferData()
     glGenBuffers(1, &m_data->m_indexBuffer);
     glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, m_data->m_indexBuffer);
     glBufferData(GL_ELEMENT_ARRAY_BUFFER,6*sizeof(int), s_indexData,GL_STATIC_DRAW);
-    
+
+	unsigned int indexData[MAX_VERTICES2*2];
+	int count=0;
+	for (int i=0;i<MAX_VERTICES2;i+=4)
+	{
+		indexData[count++]=i;
+		indexData[count++]=i+1;
+		indexData[count++]=i+2;
+
+		indexData[count++]=i;
+		indexData[count++]=i+2;
+		indexData[count++]=i+3;
+	}
+	glGenBuffers(1, &m_data->m_indexBuffer2);
+    glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, m_data->m_indexBuffer2);
+	glBufferData(GL_ELEMENT_ARRAY_BUFFER,count*sizeof(int), indexData,GL_STATIC_DRAW);
+
     glEnableVertexAttribArray(m_data->m_positionAttribute);
     glEnableVertexAttribArray(m_data->m_colourAttribute);
 	assert(glGetError()==GL_NO_ERROR);
@@ -182,6 +216,7 @@ GLPrimitiveRenderer::~GLPrimitiveRenderer()
 	 glBindTexture(GL_TEXTURE_2D,0);
     glDeleteProgram(m_data->m_shaderProg);
 	delete m_data;
+	delete m_data2;
 }
 
 void GLPrimitiveRenderer::drawLine()
@@ -299,6 +334,180 @@ void GLPrimitiveRenderer::drawTexturedRect3D(const PrimVertex& v0,const PrimVert
 
 }
 
+void GLPrimitiveRenderer::drawTexturedRect3D2Text( bool useRGBA)
+{
+	drawTexturedRect3D2(&m_data2->m_verticesText[0],m_data2->m_numVerticesText,useRGBA);
+	m_data2->m_numVerticesText = 0;
+}
+
+void GLPrimitiveRenderer::drawTexturedRect3D2( PrimVertex* vertices, int numVertices, bool useRGBA)
+{
+	//B3_PROFILE("drawTexturedRect3D2");
+	if (numVertices==0)
+	{
+		return;
+	}
+	//B3_PROFILE("GLPrimitiveRenderer::drawTexturedRect3D");
+
+    assert(glGetError()==GL_NO_ERROR);
+    float identity[16]={1,0,0,0,
+						0,1,0,0,
+						0,0,1,0,
+						0,0,0,1};
+    
+    glUseProgram(m_data->m_shaderProg);
+ 
+	glUniformMatrix4fv(m_data->m_viewmatUniform, 1, false, identity);
+	glUniformMatrix4fv(m_data->m_projMatUniform, 1, false, identity);
+
+    assert(glGetError()==GL_NO_ERROR);
+    
+    glBindBuffer(GL_ARRAY_BUFFER, m_data->m_vertexBuffer2);
+    glBindVertexArray(m_data->m_vertexArrayObject2);
+    
+	bool useFiltering = false;
+	if (useFiltering)
+	{
+		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_LINEAR);
+		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
+	} else
+	{
+		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+	}
+
+	/*   PrimVertex vertexData[4] = {
+		   v0,v1,v2,v3
+		};
+    */
+
+	   glBufferSubData(GL_ARRAY_BUFFER, 0,numVertices * sizeof(PrimVertex), vertices);
+
+
+
+    
+    
+    
+    assert(glGetError()==GL_NO_ERROR);
+    
+    PrimVec2 p( 0.f,0.f);//?b?0.5f * sinf(timeValue), 0.5f * cosf(timeValue) );
+	if (useRGBA)
+	{
+		p.p[0] = 1.f;
+		p.p[1] = 1.f;
+	}
+
+    glUniform2fv(m_data->m_positionUniform, 1, (const GLfloat *)&p);
+    
+	glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
+
+    assert(glGetError()==GL_NO_ERROR);
+    
+    glEnableVertexAttribArray(m_data->m_positionAttribute);
+	assert(glGetError()==GL_NO_ERROR);
+    
+    glEnableVertexAttribArray(m_data->m_colourAttribute);
+	assert(glGetError()==GL_NO_ERROR);
+    
+	glEnableVertexAttribArray(m_data->m_textureAttribute);
+    
+    glVertexAttribPointer(m_data->m_positionAttribute, 4, GL_FLOAT, GL_FALSE, sizeof(PrimVertex), (const GLvoid *)0);
+    glVertexAttribPointer(m_data->m_colourAttribute  , 4, GL_FLOAT, GL_FALSE, sizeof(PrimVertex), (const GLvoid *)sizeof(PrimVec4));
+    glVertexAttribPointer(m_data->m_textureAttribute , 2, GL_FLOAT, GL_FALSE, sizeof(PrimVertex), (const GLvoid *)(sizeof(PrimVec4)+sizeof(PrimVec4)));
+	assert(glGetError()==GL_NO_ERROR);
+    
+    glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, m_data->m_indexBuffer2);
+    
+    //glDrawArrays(GL_TRIANGLE_FAN, 0, 4);
+    int indexCount = (numVertices/4)*6;
+    assert(glGetError()==GL_NO_ERROR);
+    
+	
+    glDrawElements(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, 0);
+    assert(glGetError()==GL_NO_ERROR);
+	
+	
+    glBindVertexArray(0);
+    assert(glGetError()==GL_NO_ERROR);
+
+	glBindBuffer(GL_ARRAY_BUFFER, 0);
+    assert(glGetError()==GL_NO_ERROR);
+
+	glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
+    assert(glGetError()==GL_NO_ERROR);
+
+	//glDisableVertexAttribArray(m_data->m_textureAttribute);
+    assert(glGetError()==GL_NO_ERROR);
+
+	glUseProgram(0);
+   
+    assert(glGetError()==GL_NO_ERROR);
+	
+
+}
+
+void GLPrimitiveRenderer::drawTexturedRect2a(float x0, float y0, float x1, float y1, float color[4], float u0,float v0, float u1, float v1, int useRGBA)
+{
+   
+	   PrimVertex vertexData[4] = {
+        { PrimVec4(-1.f+2.f*x0/float(m_screenWidth), 1.f-2.f*y0/float(m_screenHeight), 0.f, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v0)},
+        { PrimVec4(-1.f+2.f*x0/float(m_screenWidth),  1.f-2.f*y1/float(m_screenHeight), 0.f, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v1)},
+        { PrimVec4( -1.f+2.f*x1/float(m_screenWidth),  1.f-2.f*y1/float(m_screenHeight), 0.f, 1.f ), PrimVec4(color[0], color[1], color[2], color[3]) ,PrimVec2(u1,v1)},
+        { PrimVec4( -1.f+2.f*x1/float(m_screenWidth), 1.f-2.f*y0/float(m_screenHeight), 0.f, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u1,v0)}
+    };
+
+	int sz = m_data2->m_numVerticesText;
+
+	m_data2->m_verticesRect[m_data2->m_numVerticesRect++]=vertexData[0];
+	m_data2->m_verticesRect[m_data2->m_numVerticesRect++]=vertexData[1];
+	m_data2->m_verticesRect[m_data2->m_numVerticesRect++]=vertexData[2];
+	m_data2->m_verticesRect[m_data2->m_numVerticesRect++]=vertexData[3];
+
+
+	if (m_data2->m_numVerticesRect>=MAX_VERTICES2)
+	{
+		flushBatchedRects();
+	}
+
+}
+
+void GLPrimitiveRenderer::flushBatchedRects()
+{
+	if (m_data2->m_numVerticesRect==0)
+		return;
+
+	glActiveTexture(GL_TEXTURE0);
+	assert(glGetError()==GL_NO_ERROR);
+	glBindTexture(GL_TEXTURE_2D,m_data->m_texturehandle);
+	drawTexturedRect3D2(m_data2->m_verticesRect, m_data2->m_numVerticesRect,0);
+	m_data2->m_numVerticesRect=0;
+}
+void GLPrimitiveRenderer::drawTexturedRect2(float x0, float y0, float x1, float y1, float color[4], float u0,float v0, float u1, float v1, int useRGBA)
+{
+   
+	   PrimVertex vertexData[4] = {
+        { PrimVec4(-1.f+2.f*x0/float(m_screenWidth), 1.f-2.f*y0/float(m_screenHeight), 0.f, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v0)},
+        { PrimVec4(-1.f+2.f*x0/float(m_screenWidth),  1.f-2.f*y1/float(m_screenHeight), 0.f, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v1)},
+        { PrimVec4( -1.f+2.f*x1/float(m_screenWidth),  1.f-2.f*y1/float(m_screenHeight), 0.f, 1.f ), PrimVec4(color[0], color[1], color[2], color[3]) ,PrimVec2(u1,v1)},
+        { PrimVec4( -1.f+2.f*x1/float(m_screenWidth), 1.f-2.f*y0/float(m_screenHeight), 0.f, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u1,v0)}
+    };
+
+	int sz = m_data2->m_numVerticesText;
+
+	m_data2->m_verticesText[m_data2->m_numVerticesText++]=vertexData[0];
+	m_data2->m_verticesText[m_data2->m_numVerticesText++]=vertexData[1];
+	m_data2->m_verticesText[m_data2->m_numVerticesText++]=vertexData[2];
+	m_data2->m_verticesText[m_data2->m_numVerticesText++]=vertexData[3];
+
+
+	if (m_data2->m_numVerticesText>=MAX_VERTICES2)
+	{
+		drawTexturedRect3D2(m_data2->m_verticesText, m_data2->m_numVerticesText,useRGBA);
+		m_data2->m_numVerticesText=0;
+	}
+
+}
+
 
 void GLPrimitiveRenderer::drawTexturedRect(float x0, float y0, float x1, float y1, float color[4], float u0,float v0, float u1, float v1, int useRGBA)
 {

examples/OpenGLWindow/GLPrimitiveRenderer.h

@@ -5,6 +5,8 @@
 
 struct PrimVec2
 {
+	PrimVec2()
+	{}
 	PrimVec2(float x, float y)
 	{
 		p[0] = x;
@@ -28,12 +30,21 @@ struct PrimVec4
 	float p[4];
 };
 
-typedef struct
+struct PrimVertex
 {
+	PrimVertex(const PrimVec4 & p, const PrimVec4 & c, const PrimVec2& u)
+		:position(p),
+		colour(c),
+		uv(u)
+	{
+	}
+
+	PrimVertex()
+	{}
     PrimVec4 position;
     PrimVec4 colour;
 	PrimVec2 uv;
-} PrimVertex;
+} ;
 
 
 class GLPrimitiveRenderer
@@ -42,7 +53,7 @@ class GLPrimitiveRenderer
     int m_screenHeight;
 
 	struct PrimInternalData*	m_data;
-    
+    struct PrimInternalData2*	m_data2;
     void loadBufferData();
     
 public:
@@ -56,6 +67,14 @@ public:
     void drawLine();//float from[4], float to[4], float color[4]);
     void setScreenSize(int width, int height);
     
+	void drawTexturedRect2(float x0, float y0, float x1, float y1, float color[4], float u0,float v0, float u1, float v1, int useRGBA=0);
+	void drawTexturedRect2a(float x0, float y0, float x1, float y1, float color[4], float u0,float v0, float u1, float v1, int useRGBA=0);
+	void flushBatchedRects();
+
+	
+	void drawTexturedRect3D2Text(bool useRGBA = true);
+	void drawTexturedRect3D2(PrimVertex* vertices, int numVertices, bool useRGBA = true);
+  
 	PrimInternalData*	getData()
 	{
 		return m_data;

examples/OpenGLWindow/GwenOpenGL3CoreRenderer.h

@@ -161,6 +161,7 @@ public:
 
 	virtual void StartClip()
 	{
+
 		if (m_useTrueTypeFont)
 			sth_flush_draw(m_font);
 		Gwen::Rect rect = ClipRegion();
@@ -181,6 +182,7 @@ public:
 
 	virtual void EndClip()
 	{
+
 		if (m_useTrueTypeFont)
 			sth_flush_draw(m_font);
 		glDisable( GL_SCISSOR_TEST );
@@ -196,16 +198,23 @@ public:
 	}
 	virtual void DrawFilledRect( Gwen::Rect rect )
 	{
+//		BT_PROFILE("GWEN_DrawFilledRect");
 		Translate( rect );
 
-		m_primitiveRenderer->drawRect(rect.x, rect.y+m_yOffset, rect.x+rect.w, rect.y+rect.h+m_yOffset, m_currentColor);
+		m_primitiveRenderer->drawRect(rect.x, rect.y+m_yOffset, 
+			rect.x+rect.w, rect.y+rect.h+m_yOffset, m_currentColor);
+
+		
+//		m_primitiveRenderer->drawTexturedRect2a(rect.x, rect.y+m_yOffset, 
+	//		rect.x+rect.w, rect.y+rect.h+m_yOffset, m_currentColor,0,0,1,1);
 //		m_yOffset+=rect.h+10;
 
 	}
     
     void RenderText( Gwen::Font* pFont, Gwen::Point rasterPos, const Gwen::UnicodeString& text )
     {
-		
+//		BT_PROFILE("GWEN_RenderText");
+
         Gwen::String str = Gwen::Utility::UnicodeToString(text);
         const char* unicodeText = (const char*)str.c_str();
         
@@ -246,32 +255,33 @@ public:
 		
 			glBindTexture(GL_TEXTURE_2D,m_fontTextureId);
 			float width = r.x;
+			
 			while (unicodeText[pos])
 			{
+				
 				int c = unicodeText[pos];
 				r.h = m_currentFont->m_CharHeight;
 				r.w = m_currentFont->m_CharWidth[c]+extraSpacing;
 				Gwen::Rect rect = r;
 				Translate( rect );
 
-				m_primitiveRenderer->drawTexturedRect(rect.x, rect.y+m_yOffset, rect.x+rect.w, rect.y+rect.h+m_yOffset, m_currentColor,m_currentFont->m_CharU0[c],m_currentFont->m_CharV0[c],m_currentFont->m_CharU1[c],m_currentFont->m_CharV1[c]);
+				m_primitiveRenderer->drawTexturedRect2(rect.x, rect.y+m_yOffset, rect.x+rect.w, rect.y+rect.h+m_yOffset, m_currentColor,m_currentFont->m_CharU0[c],m_currentFont->m_CharV0[c],m_currentFont->m_CharU1[c],m_currentFont->m_CharV1[c]);
 
-				//DrawTexturedRect(0,r,m_currentFont->m_CharU0[c],m_currentFont->m_CharV0[c],m_currentFont->m_CharU1[c],m_currentFont->m_CharV1[c]);
-			//	DrawFilledRect(r);
-
-			
-			
 				width += r.w;
 				r.x = width;
 				pos++;
 				
 			}
+			{
+				m_primitiveRenderer->drawTexturedRect3D2Text(false);
+			}
 			glBindTexture(GL_TEXTURE_2D,0);
 		}
 
     }
     Gwen::Point MeasureText( Gwen::Font* pFont, const Gwen::UnicodeString& text )
     {
+//		BT_PROFILE("GWEN_MeasureText");
         Gwen::String str = Gwen::Utility::UnicodeToString(text);
         const char* unicodeText = (const char*)str.c_str();
         
@@ -341,7 +351,7 @@ public:
 	
 	virtual void DrawTexturedRect( Gwen::Texture* pTexture, Gwen::Rect rect, float u1=0.0f, float v1=0.0f, float u2=1.0f, float v2=1.0f )
 	{
-
+//		BT_PROFILE("DrawTexturedRect");
 		Translate( rect );
 
 		//float eraseColor[4] = {0,0,0,0};

examples/SharedMemory/PhysicsClientSharedMemory.cpp

@@ -207,7 +207,7 @@ bool PhysicsClientSharedMemory::connect() {
             m_data->m_isConnected = true;
         }
     } else {
-        b3Error("Cannot connect to shared memory");
+        b3Warning("Cannot connect to shared memory");
         return false;
     }
 #if 0

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -1044,6 +1044,7 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
 bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVector3& pos, const btQuaternion& orn,
                              bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes)
 {
+	BT_PROFILE("loadURDF");
 	btAssert(m_data->m_dynamicsWorld);
 	if (!m_data->m_dynamicsWorld)
 	{

examples/SharedMemory/PhysicsServerExample.cpp

@@ -239,6 +239,10 @@ struct	MotionArgs
 		}
 	}
 	b3CriticalSection* m_cs;
+	b3CriticalSection* m_cs2;
+	b3CriticalSection* m_cs3;
+	b3CriticalSection* m_csGUI;
+
 
 	btAlignedObjectArray<MyMouseCommand> m_mouseCommands;
 
@@ -289,16 +293,21 @@ void	MotionThreadFunc(void* userPtr,void* lsMemory)
 
 		do
 		{
+			BT_PROFILE("loop");
 			deltaTimeInSeconds+= double(clock.getTimeMicroseconds())/1000000.;
+			clock.reset();
 
 			if (deltaTimeInSeconds<(1./5000.))
 			{
 
 				skip++;
 				skip1++;
+				//if (skip1>105)
 				if (skip1>5)
 				{
+					BT_PROFILE("b3Clock::usleep(250)");
 					b3Clock::usleep(250);
+					skip1 = 0;
 				}
 			} else
 			{
@@ -358,9 +367,9 @@ void	MotionThreadFunc(void* userPtr,void* lsMemory)
 					//b3Warning("Clamp deltaTime from %f to %f",deltaTimeInSeconds, clampedDeltaTime);
 				}
 				
-				clock.reset();
+				
 
-				args->m_cs->lock();
+				args->m_csGUI->lock();
 
 				int numSendVrControllers = 0;
 				for (int i=0;i<MAX_VR_CONTROLLERS;i++)
@@ -383,14 +392,16 @@ void	MotionThreadFunc(void* userPtr,void* lsMemory)
 					}
 				}
 
-				args->m_cs->unlock();
+				args->m_csGUI->unlock();
-
+				{
-				args->m_physicsServerPtr->stepSimulationRealTime(deltaTimeInSeconds, args->m_sendVrControllerEvents,numSendVrControllers);
+					BT_PROFILE("stepSimulationRealTime");
+					args->m_physicsServerPtr->stepSimulationRealTime(deltaTimeInSeconds, args->m_sendVrControllerEvents,numSendVrControllers);
+				}
 				deltaTimeInSeconds = 0;
 				
 			}
 
-			args->m_cs->lock();
+			args->m_csGUI->lock();
 			for (int i = 0; i < args->m_mouseCommands.size(); i++)
 			{
 				switch (args->m_mouseCommands[i].m_type)
@@ -418,10 +429,12 @@ void	MotionThreadFunc(void* userPtr,void* lsMemory)
 				}
 			}
 			args->m_mouseCommands.clear();
-			args->m_cs->unlock();
+			args->m_csGUI->unlock();
 
-
+			{
-			args->m_physicsServerPtr->processClientCommands();
+				BT_PROFILE("processClientCommands");
+				args->m_physicsServerPtr->processClientCommands();
+			}
 			
 		} while (args->m_cs->getSharedParam(0)!=eRequestTerminateMotion);
 	} else
@@ -478,9 +491,10 @@ class MultiThreadedOpenGLGuiHelper : public GUIHelperInterface
 	CommonGraphicsApp* m_app;
 	
 	b3CriticalSection* m_cs;
-	
+	b3CriticalSection* m_cs2;
+	b3CriticalSection* m_cs3;
+	b3CriticalSection* m_csGUI;
 
-	
 
 public:
 
@@ -506,11 +520,43 @@ public:
 	int m_textureId;
 	int m_instanceId;
 	
-	
+	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");
+		m_cs2->lock();
+		m_cs->unlock();
+		m_cs2->unlock();
+		m_cs3->lock();
+		m_cs3->unlock();
+
+		
+		
+		while (m_cs->getSharedParam(1)!=eGUIHelperIdle)
+		{
+			b3Clock::usleep(100);
+		}
+	}
 
 	MultiThreadedOpenGLGuiHelper(CommonGraphicsApp* app, GUIHelperInterface* guiHelper)
 		:m_app(app)
 		,m_cs(0),
+		m_cs2(0),
+		m_cs3(0),
+		m_csGUI(0),
 		m_uidGenerator(0),
 		m_texels(0),
 		m_textureId(-1)
@@ -534,6 +580,32 @@ public:
 		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)
@@ -542,11 +614,8 @@ public:
 		m_color3 = color;
 		m_cs->lock();
 		m_cs->setSharedParam(1,eGUIHelperCreateRigidBodyGraphicsObject);
-		m_cs->unlock();
+		workerThreadWait();
-		while (m_cs->getSharedParam(1)!=eGUIHelperIdle)
+
-		{
-			b3Clock::usleep(1000);
-		}
 	}
 
 	btCollisionObject* m_obj;
@@ -558,11 +627,7 @@ public:
 		m_color2 = color;
 		m_cs->lock();
 		m_cs->setSharedParam(1,eGUIHelperCreateCollisionObjectGraphicsObject);
-		m_cs->unlock();
+		workerThreadWait();
-		while (m_cs->getSharedParam(1)!=eGUIHelperIdle)
-		{
-			b3Clock::usleep(1000);
-		}
 
 	}
 
@@ -572,11 +637,7 @@ public:
 		m_colShape = collisionShape;
 		m_cs->lock();
 		m_cs->setSharedParam(1,eGUIHelperCreateCollisionShapeGraphicsObject);
-		m_cs->unlock();
+		workerThreadWait();
-		while (m_cs->getSharedParam(1)!=eGUIHelperIdle)
-		{
-			b3Clock::usleep(1000);
-		}
 
 	}
 
@@ -584,7 +645,7 @@ public:
 	{
 	    //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()->getTotalNumInstances()>0)
+        if ( m_childGuiHelper->getRenderInterface() && m_childGuiHelper->getRenderInterface()->getTotalNumInstances()>0)
         {
             m_childGuiHelper->syncPhysicsToGraphics(rbWorld);
         }
@@ -608,11 +669,10 @@ public:
 
 		m_cs->lock();
 		m_cs->setSharedParam(1,eGUIHelperRegisterTexture);
-		m_cs->unlock();
+		
-		while (m_cs->getSharedParam(1)!=eGUIHelperIdle)
+		workerThreadWait();
-		{
+
-			b3Clock::usleep(1000);
+
-		}
 		return m_textureId;
 	}
 	virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId)
@@ -626,11 +686,8 @@ public:
 
 		m_cs->lock();
 		m_cs->setSharedParam(1,eGUIHelperRegisterGraphicsShape);
-		m_cs->unlock();
+		workerThreadWait();
-		while (m_cs->getSharedParam(1)!=eGUIHelperIdle)
+
-		{
-			b3Clock::usleep(1000);
-		}
 		return m_shapeIndex;
 	}
 	virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling) 
@@ -643,11 +700,7 @@ public:
 
 		m_cs->lock();
 		m_cs->setSharedParam(1,eGUIHelperRegisterGraphicsInstance);
-		m_cs->unlock();
+		workerThreadWait();
-		while (m_cs->getSharedParam(1)!=eGUIHelperIdle)
-		{
-			b3Clock::usleep(1000);
-		}
 		return m_instanceId;
 	}
 
@@ -655,11 +708,7 @@ public:
     {
         m_cs->lock();
 		m_cs->setSharedParam(1,eGUIHelperRemoveAllGraphicsInstances);
-		m_cs->unlock();
+		workerThreadWait();
-		while (m_cs->getSharedParam(1)!=eGUIHelperIdle)
-		{
-			b3Clock::usleep(1000);
-		}
     }
 
 	virtual Common2dCanvasInterface* get2dCanvasInterface()
@@ -730,11 +779,7 @@ public:
         m_numPixelsCopied = numPixelsCopied;
 	    
 		m_cs->setSharedParam(1,eGUIHelperCopyCameraImageData);
-		m_cs->unlock();
+		workerThreadWait();
-		while (m_cs->getSharedParam(1)!=eGUIHelperIdle)
-		{
-			b3Clock::usleep(1000);
-		}
 	}
 	
 
@@ -745,11 +790,7 @@ public:
 		m_dynamicsWorld = rbWorld;
 		m_cs->lock();
 		m_cs->setSharedParam(1, eGUIHelperAutogenerateGraphicsObjects);
-		m_cs->unlock();
+		workerThreadWait();
-		while (m_cs->getSharedParam(1) != eGUIHelperIdle)
-		{
-			b3Clock::usleep(1000);
-		}
 	}
     
 	virtual void drawText3D( const char* txt, float posX, float posZY, float posZ, float size)
@@ -780,11 +821,7 @@ public:
 
 		m_cs->lock();
 		m_cs->setSharedParam(1, eGUIUserDebugAddText);
-		m_cs->unlock();
+		workerThreadWait();
-		while (m_cs->getSharedParam(1) != eGUIHelperIdle)
-		{
-			b3Clock::usleep(150);
-		}
 
 		return m_userDebugText[m_userDebugText.size()-1].m_itemUniqueId;
 	}
@@ -811,11 +848,7 @@ public:
 		
 		m_cs->lock();
 		m_cs->setSharedParam(1, eGUIUserDebugAddLine);
-		m_cs->unlock();
+		workerThreadWait();
-		while (m_cs->getSharedParam(1) != eGUIHelperIdle)
-		{
-			b3Clock::usleep(150);
-		}
 		return m_userDebugLines[m_userDebugLines.size()-1].m_itemUniqueId;
 	}
 
@@ -826,22 +859,14 @@ public:
 		m_removeDebugItemUid = debugItemUniqueId;
 		m_cs->lock();
 		m_cs->setSharedParam(1, eGUIUserDebugRemoveItem);
-		m_cs->unlock();
+		workerThreadWait();
-		while (m_cs->getSharedParam(1) != eGUIHelperIdle)
-		{
-			b3Clock::usleep(150);
-		}
 
 	}	
 	virtual void	removeAllUserDebugItems( )
 	{
 		m_cs->lock();
 		m_cs->setSharedParam(1, eGUIUserDebugRemoveAllItems);
-		m_cs->unlock();
+		workerThreadWait();
-		while (m_cs->getSharedParam(1) != eGUIHelperIdle)
-		{
-			b3Clock::usleep(150);
-		}
 	
 	}
 
@@ -934,9 +959,10 @@ public:
 		cmd.m_rayFrom = rayFrom;
 		cmd.m_rayTo = rayTo;
 		cmd.m_type = MyMouseMove;
-		m_args[0].m_cs->lock();
+		m_args[0].m_csGUI->lock();
 		m_args[0].m_mouseCommands.push_back(cmd);
-		m_args[0].m_cs->unlock();
+		m_args[0].m_csGUI->unlock();
+		
 		return false;
 	};
 
@@ -969,10 +995,11 @@ public:
 				cmd.m_rayFrom = rayFrom;
 				cmd.m_rayTo = rayTo;
 				cmd.m_type = MyMouseButtonDown;
-				m_args[0].m_cs->lock();
+				
+				m_args[0].m_csGUI->lock();
 				m_args[0].m_mouseCommands.push_back(cmd);
-				m_args[0].m_cs->unlock();
+				m_args[0].m_csGUI->unlock();
-
+				
 
 			}
 		} else
@@ -984,9 +1011,10 @@ public:
 				cmd.m_rayFrom.setValue(0,0,0);
 				cmd.m_rayTo.setValue(0, 0, 0);
 				cmd.m_type = MyMouseButtonUp;
-				m_args[0].m_cs->lock();
+				
+				m_args[0].m_csGUI->lock();
 				m_args[0].m_mouseCommands.push_back(cmd);
-				m_args[0].m_cs->unlock();
+				m_args[0].m_csGUI->unlock();
 				//remove p2p
 			}
 		}
@@ -1005,6 +1033,13 @@ public:
 	{
 		b3CommandLineArgs args(argc,argv);
 		loadCurrentSettingsVR(args);
+		int shmemKey;
+		
+		if (args.GetCmdLineArgument("sharedMemoryKey", shmemKey))
+		{
+			setSharedMemoryKey(shmemKey);
+		}
+
 		if (args.GetCmdLineArgument("camPosX", gVRTeleportPos1[0]))
 		{
 			printf("camPosX=%f\n", gVRTeleportPos1[0]);
@@ -1035,7 +1070,7 @@ public:
 
 		if (args.CheckCmdLineFlag("norobotassets"))
 		{
-			gCreateDefaultRobotAssets = false;
+//			gCreateDefaultRobotAssets = false;
 		}
 
 
@@ -1149,6 +1184,10 @@ void	PhysicsServerExample::initPhysics()
 		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->setSharedParam(0,eMotionIsUnInitialized);
 			int numMoving = 0;
  			m_args[w].m_positions.resize(numMoving);
@@ -1165,6 +1204,13 @@ void	PhysicsServerExample::initPhysics()
 
 		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_isConnected = m_physicsServer.connectSharedMemory( m_guiHelper);
 }
 
@@ -1213,8 +1259,35 @@ bool PhysicsServerExample::wantsTermination()
 
 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();
+			}
+		}
+	}
 	//check if any graphics related tasks are requested
 	
 	switch (m_multiThreadedHelper->getCriticalSection()->getSharedParam(1))
@@ -1222,27 +1295,21 @@ void	PhysicsServerExample::stepSimulation(float deltaTime)
 	case eGUIHelperCreateCollisionShapeGraphicsObject:
 	{
 		m_multiThreadedHelper->m_childGuiHelper->createCollisionShapeGraphicsObject(m_multiThreadedHelper->m_colShape);
-		m_multiThreadedHelper->getCriticalSection()->lock();
+		m_multiThreadedHelper->mainThreadRelease();
-		m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle);
-		m_multiThreadedHelper->getCriticalSection()->unlock();
-
 		break;
 	}
 	case eGUIHelperCreateCollisionObjectGraphicsObject:
 	{
 		m_multiThreadedHelper->m_childGuiHelper->createCollisionObjectGraphicsObject(m_multiThreadedHelper->m_obj,
 			m_multiThreadedHelper->m_color2);
-		m_multiThreadedHelper->getCriticalSection()->lock();
+		m_multiThreadedHelper->mainThreadRelease();
-		m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle);
+
-		m_multiThreadedHelper->getCriticalSection()->unlock();
 		break;
 	}
 	case eGUIHelperCreateRigidBodyGraphicsObject:
 	{
 		m_multiThreadedHelper->m_childGuiHelper->createRigidBodyGraphicsObject(m_multiThreadedHelper->m_body,m_multiThreadedHelper->m_color3);
-		m_multiThreadedHelper->getCriticalSection()->lock();
+		m_multiThreadedHelper->mainThreadRelease();
-		m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle);
-		m_multiThreadedHelper->getCriticalSection()->unlock();
 		break;
 	}
 	case eGUIHelperRegisterTexture:
@@ -1250,11 +1317,7 @@ void	PhysicsServerExample::stepSimulation(float deltaTime)
 		
 		m_multiThreadedHelper->m_textureId = m_multiThreadedHelper->m_childGuiHelper->registerTexture(m_multiThreadedHelper->m_texels,
 						m_multiThreadedHelper->m_textureWidth,m_multiThreadedHelper->m_textureHeight);
-
+		m_multiThreadedHelper->mainThreadRelease();
-		m_multiThreadedHelper->getCriticalSection()->lock();
-		m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle);
-		m_multiThreadedHelper->getCriticalSection()->unlock();
-		
 		break;
 	}
 	case eGUIHelperRegisterGraphicsShape:
@@ -1266,10 +1329,7 @@ void	PhysicsServerExample::stepSimulation(float deltaTime)
 				m_multiThreadedHelper->m_numIndices,
 				m_multiThreadedHelper->m_primitiveType,
 				m_multiThreadedHelper->m_textureId);
-
+		m_multiThreadedHelper->mainThreadRelease();
-		m_multiThreadedHelper->getCriticalSection()->lock();
-		m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle);
-		m_multiThreadedHelper->getCriticalSection()->unlock();
 		break;
 	}
 	case eGUIHelperRegisterGraphicsInstance:
@@ -1280,21 +1340,19 @@ void	PhysicsServerExample::stepSimulation(float deltaTime)
 				m_multiThreadedHelper->m_quaternion,
 				m_multiThreadedHelper->m_color,
 				m_multiThreadedHelper->m_scaling);
-
+		m_multiThreadedHelper->mainThreadRelease();
-		m_multiThreadedHelper->getCriticalSection()->lock();
-		m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle);
-		m_multiThreadedHelper->getCriticalSection()->unlock();
 		break;
 	}
 	case eGUIHelperRemoveAllGraphicsInstances:
         {
             m_multiThreadedHelper->m_childGuiHelper->removeAllGraphicsInstances();
-			int numRenderInstances = m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->getTotalNumInstances();
+			if (m_multiThreadedHelper->m_childGuiHelper->getRenderInterface())
-			b3Assert(numRenderInstances==0);
+			{
+				int numRenderInstances = m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->getTotalNumInstances();
+				b3Assert(numRenderInstances==0);
+			}
+			m_multiThreadedHelper->mainThreadRelease();
 
-            m_multiThreadedHelper->getCriticalSection()->lock();
-            m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle);
-            m_multiThreadedHelper->getCriticalSection()->unlock();
             break;
         }
         
@@ -1312,34 +1370,26 @@ void	PhysicsServerExample::stepSimulation(float deltaTime)
                                                                                  m_multiThreadedHelper->m_destinationWidth, 
                                                                                  m_multiThreadedHelper->m_destinationHeight, 
                                                                                  m_multiThreadedHelper->m_numPixelsCopied);
-            m_multiThreadedHelper->getCriticalSection()->lock();
+ 		m_multiThreadedHelper->mainThreadRelease();
-            m_multiThreadedHelper->getCriticalSection()->setSharedParam(1,eGUIHelperIdle);
-            m_multiThreadedHelper->getCriticalSection()->unlock();
             break;
         }
 	case eGUIHelperAutogenerateGraphicsObjects:
 	{
 		m_multiThreadedHelper->m_childGuiHelper->autogenerateGraphicsObjects(m_multiThreadedHelper->m_dynamicsWorld);
-		m_multiThreadedHelper->getCriticalSection()->lock();
+		m_multiThreadedHelper->mainThreadRelease();
-		m_multiThreadedHelper->getCriticalSection()->setSharedParam(1, eGUIHelperIdle);
-		m_multiThreadedHelper->getCriticalSection()->unlock();
 		break;
 	}
 
 	case eGUIUserDebugAddText:
 	{
 		m_multiThreadedHelper->m_userDebugText.push_back(m_multiThreadedHelper->m_tmpText);
-		m_multiThreadedHelper->getCriticalSection()->lock();
+		m_multiThreadedHelper->mainThreadRelease();
-		m_multiThreadedHelper->getCriticalSection()->setSharedParam(1, eGUIHelperIdle);
-		m_multiThreadedHelper->getCriticalSection()->unlock();
 		break;
 	}
 	case eGUIUserDebugAddLine:
 	{
 		m_multiThreadedHelper->m_userDebugLines.push_back(m_multiThreadedHelper->m_tmpLine);
-		m_multiThreadedHelper->getCriticalSection()->lock();
+		m_multiThreadedHelper->mainThreadRelease();
-		m_multiThreadedHelper->getCriticalSection()->setSharedParam(1, eGUIHelperIdle);
-		m_multiThreadedHelper->getCriticalSection()->unlock();
 			break;
 	}
 	case eGUIUserDebugRemoveItem:
@@ -1365,9 +1415,7 @@ void	PhysicsServerExample::stepSimulation(float deltaTime)
 			}
 		}
 
-		m_multiThreadedHelper->getCriticalSection()->lock();
+		m_multiThreadedHelper->mainThreadRelease();
-		m_multiThreadedHelper->getCriticalSection()->setSharedParam(1, eGUIHelperIdle);
-		m_multiThreadedHelper->getCriticalSection()->unlock();
 			break;
 	}
 	case eGUIUserDebugRemoveAllItems:
@@ -1375,9 +1423,7 @@ void	PhysicsServerExample::stepSimulation(float deltaTime)
 		m_multiThreadedHelper->m_userDebugLines.clear();
 		m_multiThreadedHelper->m_userDebugText.clear();
 		m_multiThreadedHelper->m_uidGenerator = 0;
-		m_multiThreadedHelper->getCriticalSection()->lock();
+		m_multiThreadedHelper->mainThreadRelease();
-		m_multiThreadedHelper->getCriticalSection()->setSharedParam(1, eGUIHelperIdle);
-		m_multiThreadedHelper->getCriticalSection()->unlock();
 			break;
 	}
 	case eGUIHelperIdle:
@@ -1670,9 +1716,11 @@ void PhysicsServerExample::renderScene()
 	vrOffset[13]= trInv.getOrigin()[1];
 	vrOffset[14]= trInv.getOrigin()[2];
 
-	this->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->
+	if (m_multiThreadedHelper->m_childGuiHelper->getRenderInterface())
+	{
+		m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()->
 		getActiveCamera()->setVRCameraOffsetTransform(vrOffset);
-
+	}
 	m_physicsServer.renderScene();
 	
 	for (int i=0;i<MAX_VR_CONTROLLERS;i++)
@@ -1939,7 +1987,7 @@ void	PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
 		
 	}
 
-	m_args[0].m_cs->lock();
+	m_args[0].m_csGUI->lock();
 	m_args[0].m_vrControllerEvents[controllerId].m_controllerId = controllerId;
 	m_args[0].m_vrControllerEvents[controllerId].m_pos[0] = trTotal.getOrigin()[0];
 	m_args[0].m_vrControllerEvents[controllerId].m_pos[1] = trTotal.getOrigin()[1];
@@ -1957,7 +2005,7 @@ void	PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
 		m_args[0].m_vrControllerEvents[controllerId].m_buttons[button]|=eButtonReleased;
 		m_args[0].m_vrControllerEvents[controllerId].m_buttons[button] &= ~eButtonIsDown;
 	}
-	m_args[0].m_cs->unlock();
+	m_args[0].m_csGUI->unlock();
 }
 
 
@@ -2009,7 +2057,7 @@ void	PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[
 		m_args[0].m_vrControllerOrn[controllerId] = trTotal.getRotation();
 	}
 
-	m_args[0].m_cs->lock();
+	m_args[0].m_csGUI->lock();
 	m_args[0].m_vrControllerEvents[controllerId].m_controllerId = controllerId;
 	m_args[0].m_vrControllerEvents[controllerId].m_pos[0] = trTotal.getOrigin()[0];
 	m_args[0].m_vrControllerEvents[controllerId].m_pos[1] = trTotal.getOrigin()[1];
@@ -2020,7 +2068,7 @@ void	PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[
 	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;
-	m_args[0].m_cs->unlock();
+	m_args[0].m_csGUI->unlock();
 
 }
 B3_STANDALONE_EXAMPLE(PhysicsServerCreateFunc)

examples/SharedMemory/Win32SharedMemory.cpp

@@ -4,18 +4,17 @@
 #include "Bullet3Common/b3Scalar.h"
 
 #include <windows.h>
-
+#include <stdio.h>
 //see also https://msdn.microsoft.com/en-us/library/windows/desktop/aa366551%28v=vs.85%29.aspx
 
-//TCHAR szName[]=TEXT("Global\\MyFileMappingObject2");
-TCHAR szName[]=TEXT("MyFileMappingObject2");
 
 struct Win32SharedMemoryInteralData
 {
 	HANDLE m_hMapFile;
 
 	void*	m_buf;
-
+	TCHAR m_szName[1024];
+	
 	Win32SharedMemoryInteralData()
 		:m_hMapFile(0),
 		m_buf(0)
@@ -36,10 +35,18 @@ void*   Win32SharedMemory::allocateSharedMemory(int key, int size, bool allowCre
 {
 	b3Assert(m_internalData->m_buf==0);
 
+#ifdef UNICODE
+	swprintf_s (m_internalData->m_szName,"MyFileMappingObject%d",key);
+#else
+	
+	sprintf(m_internalData->m_szName,"MyFileMappingObject%d",key);	
+#endif
+
+
 	m_internalData->m_hMapFile = OpenFileMapping(
                    FILE_MAP_ALL_ACCESS,   // read/write access
                    FALSE,                 // do not inherit the name
-                   szName);               // name of mapping object
+                   m_internalData->m_szName);               // name of mapping object
 
 	if (m_internalData->m_hMapFile==NULL)
 	{
@@ -51,10 +58,10 @@ void*   Win32SharedMemory::allocateSharedMemory(int key, int size, bool allowCre
                  PAGE_READWRITE,          // read/write access
                  0,                       // maximum object size (high-order DWORD)
                  size,						// maximum object size (low-order DWORD)
-                 szName);                 // name of mapping object
+                 m_internalData->m_szName);                 // name of mapping object
 		 } else
 		 {
-			   b3Error("Could not create file mapping object (%d).\n",GetLastError());
+			   b3Warning("Could not create file mapping object (%d).\n",GetLastError());
 			 return 0;
 		 }
 
@@ -68,7 +75,7 @@ void*   Win32SharedMemory::allocateSharedMemory(int key, int size, bool allowCre
 
    if (m_internalData->m_buf == NULL)
 	{
-		b3Error("Could not map view of file (%d).\n",GetLastError());
+		b3Warning("Could not map view of file (%d).\n",GetLastError());
 		CloseHandle(m_internalData->m_hMapFile);
 		return 0;
    }

examples/SharedMemory/main.cpp

@@ -72,6 +72,7 @@ int main(int argc, char* argv[])
 	CommonExampleOptions options(&noGfx);
 
 	args.GetCmdLineArgument("shared_memory_key", gSharedMemoryKey);
+	args.GetCmdLineArgument("sharedMemoryKey", gSharedMemoryKey);
 	
 // options.m_option |= PHYSICS_SERVER_ENABLE_COMMAND_LOGGING;
 // options.m_option |= PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG;

examples/SharedMemory/udp/main.cpp

@@ -2,13 +2,16 @@
 #include <stdio.h>
 #include <enet/enet.h>
 #include "../../CommonInterfaces/CommonGUIHelperInterface.h"
+#include "Bullet3Common/b3CommandLineArgs.h"
+
 #ifdef NO_SHARED_MEMORY
 	#include "PhysicsServerCommandProcessor.h"
 	typedef PhysicsServerCommandProcessor MyCommandProcessor;
 #else
 	#include "SharedMemoryCommandProcessor.h"
-	typedef SharedMemoryCommandProcessor MyCommandProcessor ;
+	typedef SharedMemoryCommandProcessor MyCommandProcessor;
 #endif //NO_SHARED_MEMORY
+
 #include "SharedMemoryCommands.h"
 #include "Bullet3Common/b3AlignedObjectArray.h"
 #include "PhysicsServerCommandProcessor.h"
@@ -32,11 +35,28 @@ void MySerializeInt(unsigned int sz, unsigned char* output)
 
 int main(int argc, char *argv[])
 {
+	b3CommandLineArgs parseArgs(argc,argv);
 
 	DummyGUIHelper guiHelper;
-	PhysicsCommandProcessorInterface* sm = new MyCommandProcessor;
+	MyCommandProcessor* sm = new MyCommandProcessor;
 	sm->setGuiHelper(&guiHelper);
-	
+
+	int port = 1234;
+	if (parseArgs.GetCmdLineArgument("port",port))
+	{
+		printf("Using UDP port %d\n", port);
+	}
+
+	gVerboseNetworkMessagesServer = parseArgs.CheckCmdLineFlag("verbose");
+
+#ifndef NO_SHARED_MEMORY
+	int key = 0;
+	if (parseArgs.GetCmdLineArgument("sharedMemoryKey",key))
+	{
+		sm->setSharedMemoryKey(key);
+	}
+#endif//NO_SHARED_MEMORY
+
 //	PhysicsDirect* sm = new PhysicsDirect(sdk);
 
 	//PhysicsClientSharedMemory* sm = new PhysicsClientSharedMemory();
@@ -65,7 +85,7 @@ int main(int argc, char *argv[])
 		/* enet_address_set_host (& address, "x.x.x.x"); */
 		address.host = ENET_HOST_ANY;
 		/* Bind the server to port 1234. */
-		address.port = 1234;
+		address.port = port;
 
 		server = enet_host_create(&address,
 			32,   /* number of clients */

examples/StandaloneMain/hellovr_opengl_main.cpp

@@ -10,7 +10,7 @@
 #include "Bullet3Common/b3Transform.h"
 #include "Bullet3Common/b3CommandLineArgs.h"
 
-
+#include "../Utils/b3Clock.h"
 #include "../ExampleBrowser/OpenGLGuiHelper.h"
 #include "../CommonInterfaces/CommonExampleInterface.h"
 #include "../CommonInterfaces/CommonGUIHelperInterface.h"
@@ -156,6 +156,7 @@ private:
 	uint32_t m_nWindowWidth;
 	uint32_t m_nWindowHeight;
 	bool m_hasContext;
+	b3Clock m_clock;
 
 private: // OpenGL bookkeeping
 	int m_iTrackedControllerCount;
@@ -1620,7 +1621,10 @@ void CMainApplication::RenderStereoTargets()
 {
 	B3_PROFILE("CMainApplication::RenderStereoTargets");
 
-	sExample->stepSimulation(1./60.);
+	btScalar dtSec = btScalar(m_clock.getTimeInSeconds());
+	dtSec = b3Min(dtSec,0.1);
+	sExample->stepSimulation(dtSec);
+	m_clock.reset();
 
 	glClearColor( 0.15f, 0.15f, 0.18f, 1.0f ); // nice background color, but not black
 	glEnable( GL_MULTISAMPLE );

examples/StandaloneMain/main_opengl_single_example.cpp

@@ -99,6 +99,7 @@ int main(int argc, char* argv[])
         app->m_instancingRenderer->updateCamera(app->getUpAxis());
 
 		btScalar dtSec = btScalar(clock.getTimeInSeconds());
+		dtSec = b3Min(dtSec,0.1);
 		example->stepSimulation(dtSec);
 	  	clock.reset();
 

examples/pybullet/pybullet.c

@@ -30,9 +30,31 @@ enum eCONNECT_METHOD {
 static PyObject* SpamError;
 
 #define MAX_PHYSICS_CLIENTS 1024
-static b3PhysicsClientHandle sPhysicsClients[MAX_PHYSICS_CLIENTS] = {0};
+static b3PhysicsClientHandle sPhysicsClients1[MAX_PHYSICS_CLIENTS] = {0};
 static int sNumPhysicsClients=0;
 
+b3PhysicsClientHandle getPhysicsClient(int physicsClientId)
+{
+	b3PhysicsClientHandle sm;
+	if ((physicsClientId  <0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients1[physicsClientId])) 
+	{
+		return 0;
+	}
+	sm = sPhysicsClients1[physicsClientId];
+	if (sm)
+	{
+		if (b3CanSubmitCommand(sm))
+		{
+			return sm;
+		}
+		//broken connection?
+		b3DisconnectSharedMemory(sm);
+		sPhysicsClients1[physicsClientId] = 0;
+		sNumPhysicsClients--;
+	}
+	return 0;
+
+}
 
 static double pybullet_internalGetFloatFromSequence(PyObject* seq, int index) {
 	double v = 0.0;
@@ -161,13 +183,12 @@ static PyObject* pybullet_stepSimulation(PyObject* self, PyObject* args, PyObjec
 	{
 		return NULL;
 	}
-	if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) 
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
 	{
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	sm = sPhysicsClients[physicsClientId];
-	
    
   {
     b3SharedMemoryStatusHandle statusHandle;
@@ -281,22 +302,23 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P
     };
   }
 
-
+  if (sm && b3CanSubmitCommand(sm))
-  for (i=0;i<MAX_PHYSICS_CLIENTS;i++)
   {
-	  if (sPhysicsClients[i]==0)
+	  for (i=0;i<MAX_PHYSICS_CLIENTS;i++)
 	  {
-		  freeIndex = i;
+		  if (sPhysicsClients1[i]==0)
-		  break;
+		  {
+			  freeIndex = i;
+			  break;
+		  }
+	  }
+
+	  if (freeIndex>=0)
+	  {
+		  sPhysicsClients1[freeIndex] = sm;
+		  sNumPhysicsClients++;
 	  }
   }
-
-  if (freeIndex>=0)
-  {
-	  sPhysicsClients[freeIndex] = sm;
-	  sNumPhysicsClients++;
-  }
-
   return PyInt_FromLong(freeIndex);
 }
 
@@ -311,20 +333,20 @@ static PyObject* pybullet_disconnectPhysicsServer(PyObject* self,
 	{
 		return NULL;
 	}
-	if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) 
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
 	{
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	sm = sPhysicsClients[physicsClientId];
+	
-
 
   {
     b3DisconnectSharedMemory(sm);
     sm = 0;
   }
 
-  sPhysicsClients[physicsClientId] = 0;
+  sPhysicsClients1[physicsClientId] = 0;
   sNumPhysicsClients--;
 
   Py_INCREF(Py_None);
@@ -342,17 +364,12 @@ static PyObject* pybullet_saveWorld(PyObject* self, PyObject* args,PyObject *key
 	{
 		return NULL;
 	}
-	if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) 
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
 	{
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	sm = sPhysicsClients[physicsClientId];
-	 if (0==sm)
-    {
-        PyErr_SetString(SpamError, "Not connected to physics server.");
-        return NULL;
-    }
    
 
 	{
@@ -397,12 +414,12 @@ static PyObject* pybullet_loadBullet(PyObject* self, PyObject* args,PyObject *ke
 	{
 		return NULL;
 	}
-	if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) 
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
 	{
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	sm = sPhysicsClients[physicsClientId];
 	
 
 	command = b3LoadBulletCommandInit(sm, bulletFileName);
@@ -448,12 +465,12 @@ static PyObject* pybullet_saveBullet(PyObject* self, PyObject* args, PyObject* k
 	{
 		return NULL;
 	}
-	if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) 
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
 	{
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	sm = sPhysicsClients[physicsClientId];
 
 	
 	command = b3SaveBulletCommandInit(sm, bulletFileName);
@@ -485,12 +502,12 @@ static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args, PyObject* key
 	{
 		return NULL;
 	}
-	if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) 
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
 	{
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	sm = sPhysicsClients[physicsClientId];
 
 
 	command = b3LoadMJCFCommandInit(sm, mjcfFileName);
@@ -522,12 +539,12 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar
 		return NULL;
 	}
 
-	if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) 
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
 	{
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	sm = sPhysicsClients[physicsClientId];
 
 	{
 		b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm);
@@ -669,13 +686,12 @@ b3PhysicsClientHandle sm=0;
 	  }
   }
 
-	if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) 
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
 	{
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	sm = sPhysicsClients[physicsClientId];
-
 
   if (strlen(urdfFileName)) {
     // printf("(%f, %f, %f) (%f, %f, %f, %f)\n",
@@ -736,12 +752,12 @@ b3PhysicsClientHandle sm = 0;
 	{
 		return NULL;
 	}
-	if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) 
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
 	{
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	sm = sPhysicsClients[physicsClientId];
 
  
   commandHandle = b3LoadSdfCommandInit(sm, sdfFileName);
@@ -780,12 +796,13 @@ static PyObject* pybullet_resetSimulation(PyObject* self, PyObject* args, PyObje
 	{
 		return NULL;
 	}
-	if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) 
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
 	{
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	sm = sPhysicsClients[physicsClientId];
+
   {
     b3SharedMemoryStatusHandle statusHandle;
     statusHandle = b3SubmitClientCommandAndWaitStatus(
@@ -807,12 +824,13 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) {
   double kd = 1.0;
   int valid = 0;
   int physicsClientId = 0;
-  b3PhysicsClientHandle sm = sPhysicsClients[physicsClientId];
+  b3PhysicsClientHandle sm;
-
+	sm = getPhysicsClient(physicsClientId);
-  if (0 == sm) {
+	if (sm == 0) 
-    PyErr_SetString(SpamError, "Not connected to physics server.");
+	{
-    return NULL;
+		PyErr_SetString(SpamError, "Not connected to physics server.");
-  }
+		return NULL;
+	}
 
   size = PySequence_Size(args);
   if (size == 4) {
@@ -985,12 +1003,12 @@ b3PhysicsClientHandle sm = 0;
 	{
 		return NULL;
 	}
-	if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) 
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
 	{
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	sm = sPhysicsClients[physicsClientId];
 
   {
     int numJoints;
@@ -1066,12 +1084,12 @@ static PyObject* pybullet_setRealTimeSimulation(PyObject* self,
 	{
 		return NULL;
 	}
-	if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) 
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
 	{
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	sm = sPhysicsClients[physicsClientId];
 	
 
   {
@@ -1106,12 +1124,12 @@ b3PhysicsClientHandle sm = 0;
 	{
 		return NULL;
 	}
-	if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) 
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
 	{
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	sm = sPhysicsClients[physicsClientId];
 	
 
   {
@@ -1146,12 +1164,12 @@ static PyObject* pybullet_setGravity(PyObject* self, PyObject* args, PyObject* k
 	{
 		return NULL;
 	}
-	if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) 
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
 	{
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	sm = sPhysicsClients[physicsClientId];
 	
 
     b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm);
@@ -1172,18 +1190,20 @@ static PyObject* pybullet_setTimeStep(PyObject* self, PyObject* args, PyObject*
 	double timeStep = 0.001;
 	int ret;
 	int physicsClientId = 0;
+	b3PhysicsClientHandle sm = 0;
+
 	static char *kwlist[] = { "timeStep", "physicsClientId", NULL };
 	if (!PyArg_ParseTupleAndKeywords(args, keywds, "d|i", kwlist,&timeStep, &physicsClientId))
 	{
 		return NULL;
 	}
-	if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) 
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
 	{
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
 	{
-	b3PhysicsClientHandle sm = sPhysicsClients[physicsClientId];
 
 	b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm);
 	b3SharedMemoryStatusHandle statusHandle;
@@ -1202,18 +1222,20 @@ pybullet_setDefaultContactERP(PyObject* self, PyObject* args,PyObject* keywds)
 {  
 	double defaultContactERP=0.005;
 	int physicsClientId = 0;
+	b3PhysicsClientHandle sm = 0;
+
 	static char *kwlist[] = { "defaultContactERP", "physicsClientId", NULL };
 	if (!PyArg_ParseTupleAndKeywords(args, keywds, "d|i", kwlist,&defaultContactERP, &physicsClientId))
 	{
 		return NULL;
 	}
-	if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) 
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
 	{
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
 	{
-		b3PhysicsClientHandle sm = sPhysicsClients[physicsClientId];
 	    int ret;
        
         b3SharedMemoryStatusHandle statusHandle;
@@ -1366,12 +1388,12 @@ static PyObject* pybullet_getBasePositionAndOrientation(PyObject* self,
 	{
 		return NULL;
 	}
-	if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) 
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
 	{
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	sm = sPhysicsClients[physicsClientId];
 	
   if (0 == pybullet_internalGetBasePositionAndOrientation(
                bodyUniqueId, basePosition, baseOrientation,sm)) {
@@ -1427,12 +1449,12 @@ static PyObject* pybullet_getBaseVelocity(PyObject* self,
 	{
 		return NULL;
 	}
-	if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) 
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
 	{
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	sm = sPhysicsClients[physicsClientId];
 	
 	if (0 == pybullet_internalGetBaseVelocity(
 		bodyUniqueId, baseLinearVelocity, baseAngularVelocity,sm)) {
@@ -1481,12 +1503,12 @@ static PyObject* pybullet_getNumBodies(PyObject* self, PyObject* args, PyObject*
 	{
 		return NULL;
 	}
-	if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) 
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
 	{
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	sm = sPhysicsClients[physicsClientId];
 	
 
  {
@@ -1511,12 +1533,12 @@ b3PhysicsClientHandle sm  = 0;
 	{
 		return NULL;
 	}
-	if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) 
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
 	{
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	sm = sPhysicsClients[physicsClientId];
 	
 
 	{
@@ -1546,12 +1568,12 @@ static PyObject* pybullet_getBodyInfo(PyObject* self, PyObject* args, PyObject*
 	{
 		return NULL;
 	}
-	if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) 
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
 	{
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	sm = sPhysicsClients[physicsClientId];
 
 	{
 		struct b3BodyInfo info;
@@ -1587,12 +1609,12 @@ static PyObject* pybullet_getNumJoints(PyObject* self, PyObject* args, PyObject*
 	{
 		return NULL;
 	}
-	if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) 
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
 	{
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	sm = sPhysicsClients[physicsClientId];
   
     numJoints = b3GetNumJoints(sm, bodyUniqueId);
 
@@ -1619,12 +1641,12 @@ static PyObject* pybullet_resetJointState(PyObject* self, PyObject* args, PyObje
 	{
 		return NULL;
 	}
-	if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) 
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
 	{
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	sm = sPhysicsClients[physicsClientId];
 	
 
     {
@@ -1674,12 +1696,12 @@ static PyObject* pybullet_resetBaseVelocity(PyObject* self, PyObject* args, PyOb
 			return NULL;
 		}
 
-		if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) 
+		sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
 	{
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	sm = sPhysicsClients[physicsClientId];
 
 
 		if (linVelObj || angVelObj)
@@ -1737,12 +1759,12 @@ static PyObject* pybullet_resetBasePositionAndOrientation(PyObject* self,
 	{
 		return NULL;
 	}
-	if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) 
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
 	{
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	sm = sPhysicsClients[physicsClientId];
 	
 	{
       b3SharedMemoryCommandHandle commandHandle;
@@ -1828,12 +1850,12 @@ int physicsClientId = 0;
 	{
 		return NULL;
 	}
-	if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) 
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
 	{
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	sm = sPhysicsClients[physicsClientId];
 	
 
   {
@@ -1909,12 +1931,12 @@ static PyObject* pybullet_getJointState(PyObject* self, PyObject* args,PyObject*
 	{
 		return NULL;
 	}
-	if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) 
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
 	{
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	 sm = sPhysicsClients[physicsClientId];
 	
 
   {
@@ -1994,12 +2016,12 @@ b3PhysicsClientHandle sm = 0;
 	{
 		return NULL;
 	}
-	if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) 
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
 	{
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	sm = sPhysicsClients[physicsClientId];
 	
  {
 	{
@@ -2097,12 +2119,12 @@ static PyObject* pybullet_addUserDebugText(PyObject* self, PyObject* args, PyObj
   {
 	  return NULL;
   }
-   if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) 
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
 	{
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	sm = sPhysicsClients[physicsClientId];
 
 
   res = pybullet_internalSetVectord(textPositionObj,posXYZ);
@@ -2168,13 +2190,12 @@ static PyObject* pybullet_addUserDebugLine(PyObject* self, PyObject* args, PyObj
   {
 	  return NULL;
   }
-  if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) 
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
 	{
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	sm = sPhysicsClients[physicsClientId];
-
 
   res = pybullet_internalSetVectord(lineFromObj,fromXYZ);
   if (!res)
@@ -2228,13 +2249,12 @@ static PyObject* pybullet_removeUserDebugItem(PyObject* self, PyObject* args, Py
 		return NULL;
 	}
 
-	 if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) 
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
 	{
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	sm = sPhysicsClients[physicsClientId];
-
 
 	commandHandle = b3InitUserDebugDrawRemove(sm,itemUniqueId);
 
@@ -2258,12 +2278,12 @@ static PyObject* pybullet_removeAllUserDebugItems(PyObject* self, PyObject* args
 		return NULL;
 	}
 
-  if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) 
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
 	{
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	sm = sPhysicsClients[physicsClientId];
 
 
 	commandHandle = b3InitUserDebugDrawRemoveAll(sm);
@@ -2294,12 +2314,12 @@ static PyObject* pybullet_rayTest(PyObject* self, PyObject* args, PyObject *keyw
 		&rayFromObj, &rayToObj,&physicsClientId))
 		return NULL;
 
-	if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) 
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
 	{
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	sm = sPhysicsClients[physicsClientId];
 
 	pybullet_internalSetVectord(rayFromObj,from);
 	pybullet_internalSetVectord(rayToObj,to);
@@ -2407,13 +2427,12 @@ static PyObject* pybullet_getVREvents(PyObject* self, PyObject* args, PyObject *
 		return NULL;
 	}
 	
-	if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) 
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
 	{
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	sm = sPhysicsClients[physicsClientId];
-
 
 
 	commandHandle = b3RequestVREventsCommandInit(sm);
@@ -2488,12 +2507,12 @@ static PyObject* pybullet_setDebugObjectColor(PyObject* self, PyObject* args, Py
 		&objectUniqueId, &linkIndex, &objectColorRGBObj,&physicsClientId))
 		return NULL;
 
-	  if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) 
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
 	{
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	sm = sPhysicsClients[physicsClientId];
 
 	if (objectColorRGBObj)
 	{
@@ -2531,12 +2550,12 @@ static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args, PyO
 	{
 		return NULL;
 	}
-	if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) 
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
 	{
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	sm = sPhysicsClients[physicsClientId];
 	
 
 	{
@@ -2634,13 +2653,12 @@ static PyObject* pybullet_resetVisualShapeData(PyObject* self, PyObject* args,Py
 	{
 		return NULL;
 	}
-	if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) 
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
 	{
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	sm = sPhysicsClients[physicsClientId];
-	
 
   
     {
@@ -2677,12 +2695,12 @@ static PyObject* pybullet_loadTexture(PyObject* self, PyObject* args, PyObject*
 	{
 		return NULL;
 	}
-	if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) 
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
 	{
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	sm = sPhysicsClients[physicsClientId];
 	
     {
         
@@ -2823,12 +2841,12 @@ static PyObject* pybullet_getOverlappingObjects(PyObject* self, PyObject* args,
 		&aabbMinOb, &aabbMaxOb,&physicsClientId))
 		return NULL;
 
-	  if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) 
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
 	{
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	sm = sPhysicsClients[physicsClientId];
 
 
 	pybullet_internalSetVectord(aabbMinOb, aabbMin);
@@ -2890,12 +2908,12 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py
 	  &bodyUniqueIdA, &bodyUniqueIdB, &distanceThreshold, &linkIndexA, &linkIndexB,&physicsClientId))
 	  return NULL;
 
-    if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) 
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
 	{
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	sm = sPhysicsClients[physicsClientId];
 
   commandHandle = b3InitClosestDistanceQuery(sm);
   b3SetClosestDistanceFilterBodyA(commandHandle, bodyUniqueIdA);
@@ -2939,13 +2957,12 @@ static PyObject* pybullet_removeUserConstraint(PyObject* self, PyObject* args, P
 		return NULL;
 	}
 
-	  if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) 
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
 	{
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	sm = sPhysicsClients[physicsClientId];
-
 
 	commandHandle = b3InitRemoveUserConstraintCommand(sm,userConstraintUniqueId);
 	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
@@ -3017,13 +3034,12 @@ static PyObject* pybullet_createUserConstraint(PyObject* self, PyObject* args, P
 		return NULL;
 	}
 	
-	  if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) 
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
 	{
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	sm = sPhysicsClients[physicsClientId];
-
 
 	pybullet_internalSetVectord(jointAxisObj,jointAxis);
 	pybullet_internalSetVectord(parentFramePositionObj,parentFramePosition);
@@ -3091,14 +3107,12 @@ static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args, Py
 	  &bodyUniqueIdA, &bodyUniqueIdB,&physicsClientId))
 	  return NULL;
 
-  if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) 
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
 	{
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	sm = sPhysicsClients[physicsClientId];
-	 
-
 
   commandHandle = b3InitRequestContactPointInformation(sm);
   b3SetContactFilterBodyA(commandHandle, bodyUniqueIdA);
@@ -3150,12 +3164,12 @@ static PyObject* pybullet_getCameraImage(PyObject* self, PyObject* args, PyObjec
 	{
 		return NULL;
 	}
-	if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) 
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
 	{
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	sm = sPhysicsClients[physicsClientId];
 
 	command = b3InitRequestCameraImage(sm);
 	b3RequestCameraImageSetPixelResolution(command, width, height);
@@ -3478,21 +3492,15 @@ static PyObject* pybullet_renderImageObsolete(PyObject* self, PyObject* args) {
 
   // inialize cmd
   b3SharedMemoryCommandHandle command;
+  b3PhysicsClientHandle sm;
   int physicsClientId = 0;
-  b3PhysicsClientHandle sm=0;
+
-	if (0 == sPhysicsClients[physicsClientId]) 
+  	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
 	{
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	sm = sPhysicsClients[physicsClientId];
-	
-
-  if (0 == sm) {
-    PyErr_SetString(SpamError, "Not connected to physics server.");
-    return NULL;
-  }
-
   command = b3InitRequestCameraImage(sm);
 
   if (size == 2)  // only set camera resolution
@@ -3725,12 +3733,12 @@ static PyObject* pybullet_applyExternalForce(PyObject* self, PyObject* args,PyOb
 	{
 		return NULL;
 	}
-	if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) 
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
 	{
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	sm = sPhysicsClients[physicsClientId];
     {
       PyObject* seq;
       int len, i;
@@ -3793,12 +3801,12 @@ static PyObject* pybullet_applyExternalTorque(PyObject* self, PyObject* args,PyO
 	{
 		return NULL;
 	}
-	if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) 
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
 	{
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	sm = sPhysicsClients[physicsClientId];
 	
     {
       PyObject* seq;
@@ -3982,12 +3990,12 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self,
 	{
 		return NULL;
 	}
-	if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) 
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
 	{
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	sm = sPhysicsClients[physicsClientId];
 	{
 		double pos[3];
 		double ori[4]={0,1.0,0,0};
@@ -4067,12 +4075,12 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self,
 	{
 		return NULL;
 	}
-	if ((physicsClientId<0) || (physicsClientId>=MAX_PHYSICS_CLIENTS) || (0 == sPhysicsClients[physicsClientId])) 
+	sm = getPhysicsClient(physicsClientId);
+	if (sm == 0) 
 	{
 		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 	}
-	sm = sPhysicsClients[physicsClientId];
 	
 	{
       int szObPos = PySequence_Size(objPositionsQ);

examples/pybullet/quadruped.py

@@ -2,11 +2,15 @@ import pybullet as p
 import time
 import math
 
-p.connect(p.SHARED_MEMORY)
+
+physId = p.connect(p.SHARED_MEMORY)
+if (physId<0):
+	p.connect(p.GUI)
+
 p.loadURDF("plane.urdf")
 p.setGravity(0,0,-1)
 p.setRealTimeSimulation(0)
-quadruped = p.loadURDF("quadruped/quadruped.urdf",10,-2,2)
+quadruped = p.loadURDF("quadruped/quadruped.urdf",1,-2,2)
 #p.getNumJoints(1)
 #right front leg
 p.resetJointState(quadruped,0,1.57)
@@ -82,6 +86,7 @@ jump_amp = 0.5
 #jump
 t_end = time.time() + 10
 i=0
+t=0
 while time.time() < t_end:
 	t = time.time()
 	p.setJointMotorControl(quadruped,0,p.POSITION_CONTROL,math.sin(t*speed)*jump_amp+1.57,p_gain)