Merge remote-tracking branch 'origin/master'

README.md

@@ -2,10 +2,11 @@
 [![Travis Build Status](https://api.travis-ci.org/bulletphysics/bullet3.png?branch=master)](https://travis-ci.org/bulletphysics/bullet3)
 [![Appveyor Build status](https://ci.appveyor.com/api/projects/status/6sly9uxajr6xsstq)](https://ci.appveyor.com/project/erwincoumans/bullet3)
 
-# Bullet 2.x including pybullet, Virtual Reality support
+# Bullet Physics SDK
 
-This is the official repository of Bullet 2.x, moved from http://bullet.googlecode.com
+This is the official C++ source code repository of the Bullet Physics SDK: real-time collision detection and multi-physics simulation for games, visual effects, robotics etc.
-It includes the optional experimental Bullet 3 GPU pipeline.
+
+New in Bullet 2.85: pybullet Python bindings, improved support for robotics and VR
 
 The Bullet 2 API will stay default and up-to-date while slowly moving to a new API.
 The steps towards a new API is in a nutshell:
@@ -28,9 +29,9 @@ Some optional demos require OpenGL 2 or OpenGL 3, there are some non-graphical d
 
 https://docs.google.com/document/d/1u9vyzPtrVoVhYqQOGNWUgjRbfwfCdIts_NzmvgiJ144/edit
 
-## Requirements for Bullet 3
+## Requirements for experimental OpenCL GPGPU support
 
-The entire collision detection and rigid body dynamics is executed on the GPU.
+The entire collision detection and rigid body dynamics can be executed on the GPU.
 
 A high-end desktop GPU, such as an AMD Radeon 7970 or NVIDIA GTX 680 or better.
 We succesfully tested the software under Windows, Linux and Mac OSX.
@@ -103,4 +104,4 @@ You can just run it though a terminal/command prompt, or by clicking it.
 You can use mouse picking to grab objects. When holding the ALT or CONTROL key, you have Maya style camera mouse controls.
 Press F1 to create a series of screenshots. Hit ESCAPE to exit the demo app.
 
-See docs folder for further information.
+Check out the docs folder and the Bullet physics forums for further information.

examples/BasicDemo/BasicExample.cpp

@@ -14,7 +14,6 @@ subject to the following restrictions:
 */
 
 
-
 #include "BasicExample.h"
 
 #include "btBulletDynamicsCommon.h"

examples/CommonInterfaces/CommonRigidBodyBase.h

@@ -446,9 +446,15 @@ struct CommonRigidBodyBase : public CommonExampleInterface
 	
 	virtual void renderScene()
 	{
-		m_guiHelper->syncPhysicsToGraphics(m_dynamicsWorld);
+		{
 			
-		m_guiHelper->render(m_dynamicsWorld);
+			m_guiHelper->syncPhysicsToGraphics(m_dynamicsWorld);
+		}
+		
+		{
+			
+			m_guiHelper->render(m_dynamicsWorld);
+		}
 	}
 };
 

examples/ExampleBrowser/CMakeLists.txt

@@ -131,7 +131,7 @@ SET(BulletExampleBrowser_SRCS
 	../SharedMemory/TinyRendererVisualShapeConverter.h
 	../SharedMemory/IKTrajectoryHelper.cpp
 	../SharedMemory/IKTrajectoryHelper.h
-	../RenderingExamples/TinyRendererSetup.cpp
+	
 	../SharedMemory/PhysicsServer.cpp
 	../SharedMemory/PhysicsClientSharedMemory.cpp
 	../SharedMemory/PhysicsClient.cpp
@@ -208,10 +208,16 @@ SET(BulletExampleBrowser_SRCS
 	../MultiThreading/b3PosixThreadSupport.cpp
 	../MultiThreading/b3Win32ThreadSupport.cpp
 	../MultiThreading/b3ThreadSupportInterface.cpp
+	../RenderingExamples/TinyRendererSetup.cpp
 	../RenderingExamples/TimeSeriesCanvas.cpp
 	../RenderingExamples/TimeSeriesCanvas.h
 	../RenderingExamples/TimeSeriesFontData.cpp
 	../RenderingExamples/TimeSeriesFontData.h
+	../RenderingExamples/DynamicTexturedCubeDemo.cpp
+	../RenderingExamples/DynamicTexturedCubeDemo.h
+	../RenderingExamples/TinyVRGui.cpp
+	../RenderingExamples/TinyVRGui.h
+	
 	../RoboticsLearning/GripperGraspExample.cpp
 	../RoboticsLearning/GripperGraspExample.h
 	../RoboticsLearning/b3RobotSimAPI.cpp

examples/ExampleBrowser/ExampleEntries.cpp

@@ -6,7 +6,7 @@
 #include "../RenderingExamples/CoordinateSystemDemo.h"
 #include "../RenderingExamples/RaytracerSetup.h"
 #include "../RenderingExamples/TinyRendererSetup.h"
-
+#include "../RenderingExamples/DynamicTexturedCubeDemo.h"
 #include "../ForkLift/ForkLiftDemo.h"
 #include "../BasicDemo/BasicExample.h"
 #include "../Planar2D/Planar2D.h"
@@ -291,6 +291,9 @@ static ExampleEntry gDefaultExamples[]=
 	ExampleEntry(1,"CoordinateSystemDemo","Show the axis and positive rotation direction around the axis.", CoordinateSystemCreateFunc),
 	ExampleEntry(1,"Time Series", "Render some value(s) in a 2D graph window, shifting to the left", TimeSeriesCreateFunc),
 	ExampleEntry(1,"TinyRenderer", "Very small software renderer.", TinyRendererCreateFunc),
+	ExampleEntry(1,"Dynamic Texture", "Dynamic updated textured applied to a cube.", DynamicTexturedCubeDemoCreateFunc),
+
+		
 
 	//Extended Tutorials Added by Mobeen
 	ExampleEntry(0,"Extended Tutorials"),

examples/ExampleBrowser/OpenGLExampleBrowser.cpp

@@ -1087,7 +1087,7 @@ bool OpenGLExampleBrowser::requestedExit()
 
 void OpenGLExampleBrowser::update(float deltaTime)
 {
-
+		B3_PROFILE("OpenGLExampleBrowser::update");
 		assert(glGetError()==GL_NO_ERROR);
 		s_instancingRenderer->init();
         DrawGridData dg;
@@ -1139,9 +1139,11 @@ void OpenGLExampleBrowser::update(float deltaTime)
 				singleStepSimulation = false;
 				//printf("---------------------------------------------------\n");
 				//printf("Framecount = %d\n",frameCount);
+				B3_PROFILE("sCurrentDemo->stepSimulation");
 
 				if (gFixedTimeStep>0)
 				{
+				
 					sCurrentDemo->stepSimulation(gFixedTimeStep);
 				} else
 				{
@@ -1167,7 +1169,7 @@ void OpenGLExampleBrowser::update(float deltaTime)
                 sCurrentDemo->renderScene();
             }
             {
-				
+				B3_PROFILE("physicsDebugDraw");
 				glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
                 sCurrentDemo->physicsDebugDraw(gDebugDrawFlags);
             }
@@ -1179,6 +1181,7 @@ void OpenGLExampleBrowser::update(float deltaTime)
 			
 			if (gui2 && s_guiHelper && s_guiHelper->getRenderInterface() && s_guiHelper->getRenderInterface()->getActiveCamera())
 			{
+				B3_PROFILE("setStatusBarMessage");
 				char msg[1024];
 				float camDist = s_guiHelper->getRenderInterface()->getActiveCamera()->getCameraDistance();
 				float pitch = s_guiHelper->getRenderInterface()->getActiveCamera()->getCameraPitch();
@@ -1194,6 +1197,7 @@ void OpenGLExampleBrowser::update(float deltaTime)
 		static int toggle = 1;
 		if (renderGui)
 		{
+			B3_PROFILE("renderGui");
 			//            if (!pauseSimulation)
 			//                processProfileData(s_profWindow,false);
 
@@ -1202,7 +1206,7 @@ void OpenGLExampleBrowser::update(float deltaTime)
 
 				saveOpenGLState(s_instancingRenderer->getScreenWidth(), s_instancingRenderer->getScreenHeight());
 			}
-			BT_PROFILE("Draw Gwen GUI");
+			
 			if (m_internalData->m_gui)
 			{
 				m_internalData->m_gui->draw(s_instancingRenderer->getScreenWidth(), s_instancingRenderer->getScreenHeight());
@@ -1237,6 +1241,7 @@ void OpenGLExampleBrowser::update(float deltaTime)
 	
 		if (gui2)
 		{
+			B3_PROFILE("forceUpdateScrollBars");
 			gui2->forceUpdateScrollBars();
 		}
 

examples/ExampleBrowser/OpenGLGuiHelper.cpp

@@ -289,18 +289,25 @@ void OpenGLGuiHelper::syncPhysicsToGraphics(const btDiscreteDynamicsWorld* rbWor
 		return;
 
 	int numCollisionObjects = rbWorld->getNumCollisionObjects();
-	for (int i = 0; i<numCollisionObjects; i++)
 	{
-		btCollisionObject* colObj = rbWorld->getCollisionObjectArray()[i];
+		B3_PROFILE("write all InstanceTransformToCPU");
-		btVector3 pos = colObj->getWorldTransform().getOrigin();
+		for (int i = 0; i<numCollisionObjects; i++)
-		btQuaternion orn = colObj->getWorldTransform().getRotation();
-		int index = colObj->getUserIndex();
-		if (index >= 0)
 		{
-			m_data->m_glApp->m_renderer->writeSingleInstanceTransformToCPU(pos, orn, index);
+			B3_PROFILE("writeSingleInstanceTransformToCPU");
+			btCollisionObject* colObj = rbWorld->getCollisionObjectArray()[i];
+			btVector3 pos = colObj->getWorldTransform().getOrigin();
+			btQuaternion orn = colObj->getWorldTransform().getRotation();
+			int index = colObj->getUserIndex();
+			if (index >= 0)
+			{
+				m_data->m_glApp->m_renderer->writeSingleInstanceTransformToCPU(pos, orn, index);
+			}
 		}
 	}
-	m_data->m_glApp->m_renderer->writeTransforms();
+	{
+		B3_PROFILE("writeTransforms");
+		m_data->m_glApp->m_renderer->writeTransforms();
+	}
 }
 
 

examples/ExampleBrowser/premake4.lua

@@ -54,7 +54,6 @@ project "App_BulletExampleBrowser"
 		"../TinyRenderer/tgaimage.cpp",
 		"../TinyRenderer/our_gl.cpp",
 		"../TinyRenderer/TinyRenderer.cpp",
-		"../RenderingExamples/TinyRendererSetup.cpp",
 		"../SharedMemory/IKTrajectoryHelper.cpp",
 		"../SharedMemory/IKTrajectoryHelper.h",
 		"../SharedMemory/PhysicsClientC_API.cpp",

examples/OpenGLWindow/GLInstancingRenderer.cpp

@@ -413,17 +413,32 @@ void GLInstancingRenderer::writeSingleInstanceTransformToGPU(float* position, fl
 void GLInstancingRenderer::writeTransforms()
 {
 
-	b3Assert(glGetError() ==GL_NO_ERROR);
+	{
+		B3_PROFILE("b3Assert(glGetError() 1");
+		b3Assert(glGetError() ==GL_NO_ERROR);
+	}
+	{
+		B3_PROFILE("glBindBuffer");
+		glBindBuffer(GL_ARRAY_BUFFER, m_data->m_vbo);
+	}
 
+	{
+		B3_PROFILE("glFlush()");
+		//without the flush, the glBufferSubData can spike to really slow (seconds slow)
+		glFlush();
+	}
 
-	glBindBuffer(GL_ARRAY_BUFFER, m_data->m_vbo);
+	{
-	//glFlush();
+		B3_PROFILE("b3Assert(glGetError() 2");
-
+		b3Assert(glGetError() ==GL_NO_ERROR);
-	b3Assert(glGetError() ==GL_NO_ERROR);
+	}
 	
 
 #ifdef B3_DEBUG
 	{
+		
+		//B3_PROFILE("m_data->m_totalNumInstances == totalNumInstances");
+
 		int totalNumInstances= 0;
 		for (int k=0;k<m_graphicsInstances.size();k++)
 		{
@@ -440,14 +455,29 @@ void GLInstancingRenderer::writeTransforms()
 //	int SCALE_BUFFER_SIZE = (totalNumInstances*sizeof(float)*3);
 
 #if 1
+	{
+	//	printf("m_data->m_totalNumInstances = %d\n", m_data->m_totalNumInstances);
+		{
+		B3_PROFILE("glBufferSubData pos");
 	glBufferSubData(	GL_ARRAY_BUFFER,m_data->m_maxShapeCapacityInBytes,m_data->m_totalNumInstances*sizeof(float)*4,
  						&m_data->m_instance_positions_ptr[0]);
+		}
+		{
+			B3_PROFILE("glBufferSubData orn");
 	glBufferSubData(	GL_ARRAY_BUFFER,m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE,m_data->m_totalNumInstances*sizeof(float)*4,
  						&m_data->m_instance_quaternion_ptr[0]);
+		}
+		{
+			B3_PROFILE("glBufferSubData color");
 	glBufferSubData(	GL_ARRAY_BUFFER,m_data->m_maxShapeCapacityInBytes+ POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE, m_data->m_totalNumInstances*sizeof(float)*4,
  						&m_data->m_instance_colors_ptr[0]);
+		}
+		{
+			B3_PROFILE("glBufferSubData scale");
 	glBufferSubData(	GL_ARRAY_BUFFER, m_data->m_maxShapeCapacityInBytes+POSITION_BUFFER_SIZE+ORIENTATION_BUFFER_SIZE+COLOR_BUFFER_SIZE,m_data->m_totalNumInstances*sizeof(float)*3,
 					 	&m_data->m_instance_scale_ptr[0]);
+		}
+	}
 #else
 
 	char* orgBase =  (char*)glMapBuffer( GL_ARRAY_BUFFER,GL_READ_WRITE);
@@ -517,9 +547,15 @@ void GLInstancingRenderer::writeTransforms()
 
 #endif
 
-	glBindBuffer(GL_ARRAY_BUFFER, 0);//m_data->m_vbo);
+	{
+		B3_PROFILE("glBindBuffer 2");
+		glBindBuffer(GL_ARRAY_BUFFER, 0);//m_data->m_vbo);
+	}
 
-    b3Assert(glGetError() ==GL_NO_ERROR);
+	{
+			B3_PROFILE("b3Assert(glGetError() 4");
+		b3Assert(glGetError() ==GL_NO_ERROR);
+	}
 
 }
 
@@ -686,9 +722,13 @@ int GLInstancingRenderer::registerShape(const float* vertices, int numvertices,
 
 
 	glBindBuffer(GL_ARRAY_BUFFER, m_data->m_vbo);
-	char* dest=  (char*)glMapBuffer( GL_ARRAY_BUFFER,GL_WRITE_ONLY);//GL_WRITE_ONLY
 	int vertexStrideInBytes = 9*sizeof(float);
 	int sz = numvertices*vertexStrideInBytes;
+#if 0
+
+	char* dest=  (char*)glMapBuffer( GL_ARRAY_BUFFER,GL_WRITE_ONLY);//GL_WRITE_ONLY
+	
+	
 #ifdef B3_DEBUG
 	int totalUsed = vertexStrideInBytes*gfxObj->m_vertexArrayOffset+sz;
 	b3Assert(totalUsed<m_data->m_maxShapeCapacityInBytes);
@@ -696,6 +736,10 @@ int GLInstancingRenderer::registerShape(const float* vertices, int numvertices,
 
 	memcpy(dest+vertexStrideInBytes*gfxObj->m_vertexArrayOffset,vertices,sz);
 	glUnmapBuffer( GL_ARRAY_BUFFER);
+#else
+	glBufferSubData(	GL_ARRAY_BUFFER,vertexStrideInBytes*gfxObj->m_vertexArrayOffset,sz,
+ 						vertices);
+#endif
 
 	glGenBuffers(1, &gfxObj->m_index_vbo);
 
@@ -1467,6 +1511,8 @@ void GLInstancingRenderer::renderSceneInternal(int renderMode)
 			glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
 			glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
 
+
+
 			float l_ClampColor[] = {1.0, 1.0, 1.0, 1.0};
 			glTexParameterfv(GL_TEXTURE_2D, GL_TEXTURE_BORDER_COLOR, l_ClampColor);
 			glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_BORDER);

examples/OpenGLWindow/GLPrimitiveRenderer.cpp

@@ -223,7 +223,7 @@ void GLPrimitiveRenderer::drawTexturedRect3D(const PrimVertex& v0,const PrimVert
 	bool useFiltering = false;
 	if (useFiltering)
 	{
-		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
+		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_LINEAR);
 		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
 	} else
 	{

examples/OpenGLWindow/Shaders/useShadowMapInstancingPS.glsl

@@ -36,11 +36,9 @@ void main(void)
 		
 	//float bias = 0.005f;
 	
-	float bias = 0.0001*tan(acos(intensity));
-	bias = clamp(bias, 0,0.01);
 	
 
-	float visibility = texture(shadowMap, vec3(ShadowCoord.xy,(ShadowCoord.z-bias)/ShadowCoord.w));
+	float visibility = texture(shadowMap, vec3(ShadowCoord.xy,(ShadowCoord.z)/ShadowCoord.w));
 	
 	intensity = 0.7*intensity  + 0.3*intensity*visibility;
 	

examples/OpenGLWindow/Shaders/useShadowMapInstancingPS.h

@@ -30,9 +30,8 @@ static const char* useShadowMapInstancingFragmentShader= \
 "		\n"
 "	//float bias = 0.005f;\n"
 "	\n"
-"	float bias = 0.0001*tan(acos(intensity));\n"
+"	\n"
-"	bias = clamp(bias, 0,0.01);\n"
+"	float visibility = texture(shadowMap, vec3(ShadowCoord.xy,(ShadowCoord.z)/ShadowCoord.w));\n"
-"	float visibility = texture(shadowMap, vec3(ShadowCoord.xy,(ShadowCoord.z-bias)/ShadowCoord.w));\n"
 "	\n"
 "	intensity = 0.7*intensity  + 0.3*intensity*visibility;\n"
 "	\n"

examples/OpenGLWindow/SimpleOpenGL3App.cpp

@@ -107,7 +107,7 @@ static GLuint BindFont(const CTexFont *_Font)
     glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
     glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
     glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER,GL_NEAREST);
-    glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER,GL_NEAREST);
+    glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER,GL_LINEAR_MIPMAP_LINEAR);
     glBindTexture(GL_TEXTURE_2D, 0);
 
     return TexID;
@@ -173,6 +173,7 @@ SimpleOpenGL3App::SimpleOpenGL3App(	const char* title, int width,int height, boo
     b3Assert(glGetError() ==GL_NO_ERROR);
 
 	m_instancingRenderer = new GLInstancingRenderer(128*1024,128*1024*1024);
+
     m_primRenderer = new GLPrimitiveRenderer(width,height);
     
     m_renderer = m_instancingRenderer ;
@@ -830,7 +831,7 @@ void SimpleOpenGL3App::dumpNextFrameToPng(const char* filename)
                          , 0,GL_RGBA, GL_FLOAT, 0);
 
             glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
-            glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+            glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_LINEAR);
             //glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
             //glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
 

examples/OpenGLWindow/opengl_fontstashcallbacks.cpp

@@ -126,7 +126,7 @@ void InternalOpenGL2RenderCallbacks::updateTexture(sth_texture* texture, sth_gly
 			texture->m_texels = (unsigned char*)malloc(textureWidth*textureHeight);
 			memset(texture->m_texels,0,textureWidth*textureHeight);
 			glTexImage2D(GL_TEXTURE_2D, 0, GL_RED, textureWidth, textureHeight, 0, GL_RED, GL_UNSIGNED_BYTE, texture->m_texels);
-			glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
+			glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_LINEAR);
 			glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
 	    assert(glGetError()==GL_NO_ERROR);
 
@@ -187,7 +187,7 @@ void InternalOpenGL2RenderCallbacks::render(sth_texture* texture)
 	bool useFiltering = false;
 	if (useFiltering)
 	{
-		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
+		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_LINEAR);
 		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
 	} else
 	{

examples/RenderingExamples/DynamicTexturedCubeDemo.cpp

@@ -0,0 +1,152 @@
+
+#include "DynamicTexturedCubeDemo.h"
+#include "Bullet3Common/b3Logging.h"
+#include "../CommonInterfaces/CommonGraphicsAppInterface.h"
+#include "Bullet3Common/b3Quaternion.h"
+#include "Bullet3Common/b3AlignedObjectArray.h"
+#include "../CommonInterfaces/CommonRenderInterface.h"
+#include "../CommonInterfaces/CommonExampleInterface.h"
+#include "../CommonInterfaces/CommonGUIHelperInterface.h"
+#include "GwenGUISupport/GraphingTexture.h"
+
+#include "../CommonInterfaces/Common2dCanvasInterface.h"
+#include "../RenderingExamples/TimeSeriesCanvas.h"
+#include "../RenderingExamples/TimeSeriesFontData.h"
+#include "../Importers/ImportMeshUtility/b3ImportMeshUtility.h"
+#include "../OpenGLWindow/GLInstanceGraphicsShape.h"
+#include "TinyVRGui.h"
+#include "../CommonInterfaces/CommonParameterInterface.h"
+
+class DynamicTexturedCubeDemo : public CommonExampleInterface
+{
+    CommonGraphicsApp* m_app;
+    float m_x;
+    float m_y;
+    float m_z;
+	b3AlignedObjectArray<int> m_movingInstances;
+	
+	
+	TinyVRGui* m_tinyVrGUI;
+
+	enum
+	{
+		numCubesX = 1,
+		numCubesY = 1
+	};
+public:
+    
+    DynamicTexturedCubeDemo(CommonGraphicsApp* app)
+    :m_app(app),
+    m_x(0),
+    m_y(0),
+	m_z(0),
+	m_tinyVrGUI(0)
+    {
+		m_app->setUpAxis(2);
+        
+		 {
+             b3Vector3 extents=b3MakeVector3(100,100,100);
+             extents[m_app->getUpAxis()]=1;
+             
+			 int xres = 20;
+			 int yres = 20;
+			 
+			 b3Vector4 color0=b3MakeVector4(0.1, 0.1, 0.5,1);
+			 b3Vector4 color1=b3MakeVector4(0.6, 0.6, 0.6,1);
+            m_app->registerGrid(xres, yres, color0, color1);
+        }
+		 
+		ComboBoxParams comboParams;
+        comboParams.m_comboboxId = 0;
+        comboParams.m_numItems = 0;
+        comboParams.m_startItem = 0;
+        comboParams.m_callback = 0;//MyComboBoxCallback;
+        comboParams.m_userPointer = 0;//this;
+            
+		
+		m_tinyVrGUI = new TinyVRGui(comboParams,m_app->m_renderer);
+		m_tinyVrGUI->init();
+
+		 m_app->m_renderer->writeTransforms();
+    }
+    virtual ~DynamicTexturedCubeDemo()
+    {
+		delete m_tinyVrGUI;
+        m_app->m_renderer->enableBlend(false);
+    }
+
+    
+    virtual void physicsDebugDraw(int debugDrawMode)
+    {
+        
+    }
+    virtual void    initPhysics()
+    {
+    }
+    virtual void    exitPhysics()
+    {
+		
+    }
+
+    virtual void	stepSimulation(float deltaTime)
+    {
+        static b3Transform tr = b3Transform::getIdentity();
+		static b3Scalar t = 0.f;
+		t+=deltaTime;
+		tr.setOrigin(b3MakeVector3(0.,0.,2.)+b3MakeVector3(0.,0.,0.02*b3Sin(t)));
+
+		m_tinyVrGUI->tick(deltaTime,tr);
+
+        m_app->m_renderer->writeTransforms();
+        
+    }
+    virtual void	renderScene()
+    {
+		
+		m_app->m_renderer->renderScene();
+    }
+
+	
+    virtual void	physicsDebugDraw()
+    {
+      		
+    }
+    virtual bool	mouseMoveCallback(float x,float y)
+    {
+		return false;   
+    }
+    virtual bool	mouseButtonCallback(int button, int state, float x, float y)
+    {
+        return false;   
+    }
+    virtual bool	keyboardCallback(int key, int state)
+    {
+        return false;   
+    }
+    
+	virtual void resetCamera()
+	{
+		float dist = 1.15;
+		float pitch = 396;
+		float yaw = 33.7;
+		float targetPos[3]={-0.5,0.7,1.45};
+		if (m_app->m_renderer  && m_app->m_renderer->getActiveCamera())
+		{
+			m_app->m_renderer->getActiveCamera()->setCameraDistance(dist);
+			m_app->m_renderer->getActiveCamera()->setCameraPitch(pitch);
+			m_app->m_renderer->getActiveCamera()->setCameraYaw(yaw);
+			m_app->m_renderer->getActiveCamera()->setCameraTargetPosition(targetPos[0],targetPos[1],targetPos[2]);
+		}
+	}
+
+};
+
+
+class	CommonExampleInterface*    DynamicTexturedCubeDemoCreateFunc(struct CommonExampleOptions& options)
+{
+	return new DynamicTexturedCubeDemo(options.m_guiHelper->getAppInterface());
+}
+
+
+
+

examples/RenderingExamples/DynamicTexturedCubeDemo.h

@@ -0,0 +1,6 @@
+#ifndef DYNAMIC_TEXTURED_CUBE_DEMO_H
+#define DYNAMIC_TEXTURED_CUBE_DEMO_H
+
+class CommonExampleInterface*    DynamicTexturedCubeDemoCreateFunc(struct CommonExampleOptions& options);
+
+#endif //DYNAMIC_TEXTURED_CUBE_DEMO_H

examples/RenderingExamples/TimeSeriesCanvas.cpp

@@ -104,7 +104,7 @@ void TimeSeriesCanvas::addDataSource(const char* dataSourceLabel, unsigned char
 	m_internalData->m_dataSources.push_back(dataSource);
 
 }
-void TimeSeriesCanvas::setupTimeSeries(float yScale, int ticksPerSecond, int startTime)
+void TimeSeriesCanvas::setupTimeSeries(float yScale, int ticksPerSecond, int startTime, bool clearCanvas)
 {
 	if (0==m_internalData->m_canvasInterface)
 		return;
@@ -113,20 +113,23 @@ void TimeSeriesCanvas::setupTimeSeries(float yScale, int ticksPerSecond, int sta
 	m_internalData->m_ticksPerSecond = ticksPerSecond;
 	m_internalData->m_yScale = yScale;
 	m_internalData->m_dataSources.clear();
-	for (int i=0;i<m_internalData->m_width;i++)
-	{
-		for (int j=0;j<m_internalData->m_height;j++)
-		{
 	
-			m_internalData->m_canvasInterface->setPixel(m_internalData->m_canvasIndex,i,j,
+	if (clearCanvas)
-				m_internalData->m_backgroundRed,
+	{
-				m_internalData->m_backgroundGreen,
+		for (int i=0;i<m_internalData->m_width;i++)
-				m_internalData->m_backgroundBlue,
+		{
-				m_internalData->m_backgroundAlpha);
+			for (int j=0;j<m_internalData->m_height;j++)
+			{
+			
+				m_internalData->m_canvasInterface->setPixel(m_internalData->m_canvasIndex,i,j,
+					m_internalData->m_backgroundRed,
+					m_internalData->m_backgroundGreen,
+					m_internalData->m_backgroundBlue,
+					m_internalData->m_backgroundAlpha);
+			}
 		}
 	}
 	
-	
 	float zeroPixelCoord = m_internalData->m_zero;
 	float pixelsPerUnit = m_internalData->m_pixelsPerUnit;
 	
@@ -207,7 +210,7 @@ void TimeSeriesCanvas::shift1PixelToLeft()
 	int countdown = resetVal;
 
 	//shift pixture one pixel to the left
-	for (int j=0;j<m_internalData->m_height-48;j++)
+	for (int j=50;j<m_internalData->m_height-48;j++)
 	{
 		for (int i=40;i<this->m_internalData->m_width;i++)
 		{

examples/RenderingExamples/TimeSeriesCanvas.h

@@ -6,17 +6,17 @@ class TimeSeriesCanvas
 protected:
 	struct TimeSeriesInternalData* m_internalData;
 	void shift1PixelToLeft();
-	void grapicalPrintf(const char* str,	void* fontData, int rasterposx,int rasterposy,unsigned char red, unsigned char green, unsigned char blue, unsigned char alpha);
 	
 public:
 	
 	TimeSeriesCanvas(struct Common2dCanvasInterface* canvasInterface, int width, int height, const char* windowTitle);
 	virtual ~TimeSeriesCanvas();
 
-	void setupTimeSeries(float yScale, int ticksPerSecond, int startTime);
+	void setupTimeSeries(float yScale, int ticksPerSecond, int startTime, bool clearCanvas=true);
 	void addDataSource(const char* dataSourceLabel, unsigned char red,unsigned char green,unsigned char blue);
 	void insertDataAtCurrentTime(float value, int dataSourceIndex, bool connectToPrevious);
 	float getCurrentTime() const;
+	void grapicalPrintf(const char* str,	void* fontData, int rasterposx,int rasterposy,unsigned char red, unsigned char green, unsigned char blue, unsigned char alpha);
 
 	virtual void nextTick();
 	

examples/RenderingExamples/TinyVRGui.cpp

@@ -0,0 +1,218 @@
+#include "TinyVRGui.h"
+#include "../CommonInterfaces/CommonGUIHelperInterface.h"
+#include "../ExampleBrowser/GwenGUISupport/GraphingTexture.h"
+
+#include "../CommonInterfaces/Common2dCanvasInterface.h"
+#include "../RenderingExamples/TimeSeriesCanvas.h"
+#include "../RenderingExamples/TimeSeriesFontData.h"
+#include "../Importers/ImportMeshUtility/b3ImportMeshUtility.h"
+#include "../OpenGLWindow/GLInstanceGraphicsShape.h"
+
+#include "../CommonInterfaces/CommonRenderInterface.h"
+#include "../CommonInterfaces/CommonParameterInterface.h"
+ 
+
+struct TestCanvasInterface2 : public Common2dCanvasInterface
+{
+	b3AlignedObjectArray<unsigned char>& m_texelsRGB;
+	int m_width;
+	int m_height;
+
+	TestCanvasInterface2(b3AlignedObjectArray<unsigned char>& texelsRGB, int width, int height)
+		:m_width(width),
+		m_height(height),
+		m_texelsRGB(texelsRGB)
+	{
+	}
+
+	virtual ~TestCanvasInterface2() 
+	{}
+	virtual int createCanvas(const char* canvasName, int width, int height)
+	{
+		return 0;
+	}
+	virtual void destroyCanvas(int canvasId)
+	{
+	}
+	virtual void setPixel(int canvasId, int x, int y, unsigned char red, unsigned char green,unsigned char blue, unsigned char alpha)
+	{
+		if (x>=0 && x<m_width && y>=0 && y<m_height)
+		{
+			m_texelsRGB[(x+y*m_width)*3+0] = red;
+			m_texelsRGB[(x+y*m_width)*3+1] = green;
+			m_texelsRGB[(x+y*m_width)*3+2] = blue;
+		}
+	}
+	virtual void getPixel(int canvasId, int x, int y, unsigned char& red, unsigned char& green,unsigned char& blue, unsigned char& alpha)
+	{
+		if (x>=0 && x<m_width && y>=0 && y<m_height)
+		{
+			red = m_texelsRGB[(x+y*m_width)*3+0];
+			green = m_texelsRGB[(x+y*m_width)*3+1];
+			blue = m_texelsRGB[(x+y*m_width)*3+2];
+		}
+	}
+
+	virtual void refreshImageData(int canvasId)
+	{
+
+	}
+
+};
+
+
+
+struct TinyVRGuiInternalData
+{
+	CommonRenderInterface*	m_renderer;
+
+	b3AlignedObjectArray<unsigned char> m_texelsRGB;
+	TestCanvasInterface2* m_testCanvas;
+	TimeSeriesCanvas* m_timeSeries;
+	int m_src;
+	int m_textureId;
+	int m_gfxObjectId;
+
+	TinyVRGuiInternalData()
+		:m_renderer(0),
+		m_testCanvas(0),
+		m_timeSeries(0),
+		m_src(-1),
+		m_textureId(-1),
+		m_gfxObjectId(-1)
+	{
+	}
+};
+
+
+TinyVRGui::TinyVRGui(struct ComboBoxParams& params, struct CommonRenderInterface*	renderer)
+{
+	m_data = new TinyVRGuiInternalData;
+	m_data->m_renderer = renderer;
+	
+
+}
+
+TinyVRGui::~TinyVRGui()
+{
+	delete m_data->m_timeSeries;
+	delete m_data->m_testCanvas;
+	delete m_data;
+}
+	
+bool TinyVRGui::init()
+{
+	 {
+
+			
+			int width = 256;
+			int height = 256;
+			m_data->m_texelsRGB.resize(width*height*3);
+			for (int i=0;i<width;i++)
+				for (int j=0;j<height;j++)
+				{
+					m_data->m_texelsRGB[(i+j*width)*3+0] = 155;
+					m_data->m_texelsRGB[(i+j*width)*3+1] = 155;
+					m_data->m_texelsRGB[(i+j*width)*3+2] = 255;
+				}
+			
+			m_data->m_testCanvas = new TestCanvasInterface2(m_data->m_texelsRGB,width,height);
+			m_data->m_timeSeries = new TimeSeriesCanvas(m_data->m_testCanvas,width,height,"time series");
+			
+			
+			bool clearCanvas = false;
+
+			m_data->m_timeSeries->setupTimeSeries(3,100, 0,clearCanvas);
+			m_data->m_timeSeries->addDataSource("Some sine wave", 255,0,0);
+			m_data->m_timeSeries->addDataSource("Some cosine wave", 0,255,0);
+			m_data->m_timeSeries->addDataSource("Delta Time (*10)", 0,0,255);
+			m_data->m_timeSeries->addDataSource("Tan", 255,0,255);
+			m_data->m_timeSeries->addDataSource("Some cosine wave2", 255,255,0);
+			m_data->m_timeSeries->addDataSource("Empty source2", 255,0,255);
+			
+			m_data->m_textureId = m_data->m_renderer->registerTexture(&m_data->m_texelsRGB[0],width,height);
+
+			{
+				  
+			const char* fileName = "cube.obj";//"textured_sphere_smooth.obj";
+			//fileName = "cube.obj";
+		
+			int shapeId = -1;
+
+			b3ImportMeshData meshData;
+			if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fileName, meshData))
+			{
+						
+
+				shapeId = m_data->m_renderer->registerShape(&meshData.m_gfxShape->m_vertices->at(0).xyzw[0], 
+											meshData.m_gfxShape->m_numvertices, 
+											&meshData.m_gfxShape->m_indices->at(0), 
+											meshData.m_gfxShape->m_numIndices,
+											B3_GL_TRIANGLES,
+											m_data->m_textureId);
+
+				float position[4]={0,0,2,1};
+				float orn[4]={0,0,0,1};
+				float color[4]={1,1,1,1};
+				float scaling[4]={.1,.1,.1,1};
+                
+				m_data->m_gfxObjectId = m_data->m_renderer->registerGraphicsInstance(shapeId,position,orn,color,scaling);
+				m_data->m_renderer->writeTransforms();
+
+						
+						
+				meshData.m_gfxShape->m_scaling[0] = scaling[0];
+				meshData.m_gfxShape->m_scaling[1] = scaling[1];
+				meshData.m_gfxShape->m_scaling[2] = scaling[2];
+                
+						
+				delete meshData.m_gfxShape;
+				delete meshData.m_textureImage;
+			}
+				
+			
+		
+			}
+        }
+
+		 m_data->m_renderer->writeTransforms();
+		 return true;
+}
+
+
+void TinyVRGui::tick(b3Scalar deltaTime, const b3Transform& guiWorldTransform)
+{
+	float time = m_data->m_timeSeries->getCurrentTime();
+	float v = sinf(time);
+	m_data->m_timeSeries->insertDataAtCurrentTime(v,0,true);
+	v = cosf(time);
+	m_data->m_timeSeries->insertDataAtCurrentTime(v,1,true);
+	v = tanf(time);
+	m_data->m_timeSeries->insertDataAtCurrentTime(v,3,true);
+	m_data->m_timeSeries->insertDataAtCurrentTime(deltaTime*10,2,true);
+
+	m_data->m_timeSeries->nextTick();
+
+	m_data->m_renderer->updateTexture(m_data->m_textureId,&m_data->m_texelsRGB[0]);
+	m_data->m_renderer->writeSingleInstanceTransformToCPU(guiWorldTransform.getOrigin(),guiWorldTransform.getRotation(),m_data->m_gfxObjectId);
+    m_data->m_renderer->writeTransforms();
+}
+
+void TinyVRGui::clearTextArea()
+{
+		int width = 256;
+		int height = 50;
+		for (int i=0;i<width;i++)
+			for (int j=0;j<height;j++)
+			{
+				m_data->m_texelsRGB[(i+j*width)*3+0] = 155;
+				m_data->m_texelsRGB[(i+j*width)*3+1] = 155;
+				m_data->m_texelsRGB[(i+j*width)*3+2] = 255;
+			}
+}
+
+void TinyVRGui::grapicalPrintf(const char* str,int rasterposx,int rasterposy,unsigned char red, unsigned char green, unsigned char blue, unsigned char alpha)
+{
+	m_data->m_timeSeries->grapicalPrintf(str,sTimeSeriesFontData,rasterposx,rasterposy,red,green,blue,alpha);
+}
+

examples/RenderingExamples/TinyVRGui.h

@@ -0,0 +1,25 @@
+
+#ifndef TINY_VR_GUI_H
+#define TINY_VR_GUI_H
+
+#include "Bullet3Common/b3Transform.h"
+
+class TinyVRGui
+{
+	struct TinyVRGuiInternalData* m_data;
+
+public:
+
+	TinyVRGui(struct ComboBoxParams& params, struct CommonRenderInterface*	renderer);
+	virtual ~TinyVRGui();
+	
+	bool init();
+	void tick(b3Scalar deltaTime, const b3Transform& guiWorldTransform);
+
+	void clearTextArea();
+	void grapicalPrintf(const char* str,int rasterposx,int rasterposy,unsigned char red, unsigned char green, unsigned char blue, unsigned char alpha);
+
+};
+
+
+#endif //TINY_VR_GUI_H

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -30,7 +30,27 @@ b3SharedMemoryCommandHandle	b3LoadSdfCommandInit(b3PhysicsClientHandle physClien
 	return (b3SharedMemoryCommandHandle) command;
 }
 
+b3SharedMemoryCommandHandle	b3SaveWorldCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName)
+{
+	PhysicsClient* cl = (PhysicsClient* ) physClient;
+    b3Assert(cl);
+    b3Assert(cl->canSubmitCommand());
     
+	struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+    b3Assert(command);
+	command->m_type = CMD_SAVE_WORLD;
+	int len = strlen(sdfFileName);
+	if (len<MAX_SDF_FILENAME_LENGTH)
+	{
+		strcpy(command->m_sdfArguments.m_sdfFileName,sdfFileName);
+	} else
+	{
+		command->m_sdfArguments.m_sdfFileName[0] = 0;
+	}
+	command->m_updateFlags = SDF_ARGS_FILE_NAME;
+	
+	return (b3SharedMemoryCommandHandle) command;
+}
 
 b3SharedMemoryCommandHandle	b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName)
 {

examples/SharedMemory/PhysicsClientC_API.h

@@ -146,6 +146,9 @@ int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle status
 b3SharedMemoryCommandHandle	b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
 int	b3LoadSdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
 
+b3SharedMemoryCommandHandle	b3SaveWorldCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
+
+
 ///The b3JointControlCommandInit method is obsolete, use b3JointControlCommandInit2 instead
 b3SharedMemoryCommandHandle  b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode);
 

examples/SharedMemory/PhysicsClientExample.cpp

@@ -5,6 +5,7 @@
 #include "../CommonInterfaces/Common2dCanvasInterface.h"
 #include "SharedMemoryCommon.h"
 #include "../CommonInterfaces/CommonParameterInterface.h"
+
 #include "PhysicsClientC_API.h"
 #include "PhysicsClient.h"
 //#include "SharedMemoryCommands.h"
@@ -451,6 +452,12 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
                 b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
                 break;
             }
+		case CMD_SAVE_WORLD:
+		{
+                b3SharedMemoryCommandHandle commandHandle = b3SaveWorldCommandInit(m_physicsClientHandle, "saveWorld.py");
+                b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
+			break;
+		}
         default:
         {
             b3Error("Unknown buttonId");
@@ -525,6 +532,7 @@ void	PhysicsClientExample::createButtons()
 
         createButton("Load URDF",CMD_LOAD_URDF,  isTrigger);
         createButton("Load SDF",CMD_LOAD_SDF,  isTrigger);
+		createButton("Save World",CMD_SAVE_WORLD,  isTrigger);
         createButton("Get Camera Image",CMD_REQUEST_CAMERA_IMAGE_DATA,isTrigger);
         createButton("Step Sim",CMD_STEP_FORWARD_SIMULATION,  isTrigger);
         createButton("Send Bullet Stream",CMD_SEND_BULLET_DATA_STREAM,  isTrigger);

examples/SharedMemory/PhysicsClientSharedMemory.cpp

@@ -616,8 +616,26 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
                     break;
                 }
 
+			case CMD_SAVE_WORLD_COMPLETED:
+				break;
+					
+			case CMD_SAVE_WORLD_FAILED:
+			{
+				b3Warning("Saving world  failed");
+				break;
+			}
+			case CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED:
+			{
+                    break;
+                }
+            case CMD_CALCULATE_INVERSE_KINEMATICS_FAILED:
+                {
+                    b3Warning("Calculate Inverse Kinematics Request failed");
+                    break;
+                }
+
             default: {
-                b3Error("Unknown server status\n");
+                b3Error("Unknown server status %d\n", serverCmd.m_type);
                 btAssert(0);
             }
         };

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -31,6 +31,7 @@
 #include "BulletSoftBody/btSoftBodyHelpers.h"
 #include "../SoftDemo/BunnyMesh.h"
 
+
 //@todo(erwincoumans) those globals are hacks for a VR demo, move this to Python/pybullet!
 btVector3 gLastPickPos(0, 0, 0);
 bool gCloseToKuka=false;
@@ -305,6 +306,12 @@ struct CommandLogPlayback
 	}
 };
 
+struct SaveWorldObjectData
+{
+	b3AlignedObjectArray<int> m_bodyUniqueIds;
+	std::string	m_fileName;
+};
+
 struct PhysicsServerCommandProcessorInternalData
 {
 	///handle management
@@ -401,6 +408,8 @@ struct PhysicsServerCommandProcessorInternalData
 	btMultiBody* m_kukaGripperMultiBody;
 	btMultiBodyPoint2Point* m_kukaGripperRevolute1;
 	btMultiBodyPoint2Point* m_kukaGripperRevolute2;
+	
+
 	int m_huskyId;
 	int m_KukaId;
 	int m_sphereId;
@@ -414,7 +423,7 @@ struct PhysicsServerCommandProcessorInternalData
 	btAlignedObjectArray<btMultiBodyJointFeedback*> m_multiBodyJointFeedbacks;
 	btHashMap<btHashPtr, btInverseDynamics::MultiBodyTree*> m_inverseDynamicsBodies;
 	btHashMap<btHashPtr, IKTrajectoryHelper*> m_inverseKinematicsHelpers;
-
+	b3AlignedObjectArray<SaveWorldObjectData> m_saveWorldBodyData;
 
 
 	btAlignedObjectArray<btBulletWorldImporter*> m_worldImporters;
@@ -457,6 +466,10 @@ struct PhysicsServerCommandProcessorInternalData
 		:m_hasGround(false),
 		m_gripperRigidbodyFixed(0),
 		m_gripperMultiBody(0),
+		m_kukaGripperFixed(0),
+		m_kukaGripperMultiBody(0),
+		m_kukaGripperRevolute1(0),
+		m_kukaGripperRevolute2(0),
 		m_allowRealTimeSimulation(false),
 		m_huskyId(-1),
 		m_KukaId(-1),
@@ -817,6 +830,9 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
         {
             b3Printf("loaded %s OK!", fileName);
         }
+		SaveWorldObjectData sd;
+		sd.m_fileName = fileName;
+
 
         for (int m =0; m<u2b.getNumModels();m++)
         {
@@ -830,6 +846,7 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
 
             InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId);
 			
+			sd.m_bodyUniqueIds.push_back(bodyUniqueId);
 
             u2b.setBodyUniqueId(bodyUniqueId);
             {
@@ -860,6 +877,7 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
 			if (mb)
 				mb->setUserIndex2(bodyUniqueId);
 
+			
             if (mb)
             {
                 bodyHandle->m_multiBody = mb;
@@ -904,6 +922,9 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
 			}
 
         }
+		
+		m_data->m_saveWorldBodyData.push_back(sd);
+
     }
     return loadOk;
 }
@@ -936,6 +957,14 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
 		if (bodyUniqueIdPtr)
 			*bodyUniqueIdPtr= bodyUniqueId;
 
+		//quick prototype of 'save world' for crude world editing
+		{
+			SaveWorldObjectData sd;
+			sd.m_fileName = fileName;
+			sd.m_bodyUniqueIds.push_back(bodyUniqueId);
+			m_data->m_saveWorldBodyData.push_back(sd);
+		}
+
         u2b.setBodyUniqueId(bodyUniqueId);
 		InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId);
 		
@@ -1342,6 +1371,154 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
                         hasStatus = true;
                         break;
                     }
+				case CMD_SAVE_WORLD:
+				{
+					///this is a very rudimentary way to save the state of the world, for scene authoring
+					///many todo's, for example save the state of motor controllers etc.
+
+					
+					{
+						//saveWorld(clientCmd.m_sdfArguments.m_sdfFileName);
+
+						FILE* f = fopen(clientCmd.m_sdfArguments.m_sdfFileName,"w");
+						if (f)
+						{
+							char line[1024];
+							{
+								sprintf(line,"import pybullet as p\n");
+								int len = strlen(line);
+								fwrite(line,len,1,f);
+							}
+							{
+								sprintf(line,"p.connect(p.SHARED_MEMORY)\n");
+								int len = strlen(line);
+								fwrite(line,len,1,f);
+							}
+							//for each objects ...
+							for (int i=0;i<m_data->m_saveWorldBodyData.size();i++)
+							{
+								SaveWorldObjectData& sd = m_data->m_saveWorldBodyData[i];
+
+								for (int i=0;i<sd.m_bodyUniqueIds.size();i++)
+								{
+									{
+										int bodyUniqueId = sd.m_bodyUniqueIds[i];
+										InteralBodyData* body = m_data->getHandle(bodyUniqueId);
+										if (body)
+										{
+											 if (body->m_multiBody)
+											 {
+												btMultiBody* mb = body->m_multiBody;
+												
+												btTransform comTr = mb->getBaseWorldTransform();
+												btTransform tr = comTr * body->m_rootLocalInertialFrame.inverse();
+												
+												if (strstr(sd.m_fileName.c_str(),".urdf"))
+												{
+													sprintf(line,"objects = [p.loadURDF(\"%s\", %f,%f,%f,%f,%f,%f,%f)]\n",sd.m_fileName.c_str(),
+														tr.getOrigin()[0],tr.getOrigin()[1],tr.getOrigin()[2],
+														tr.getRotation()[0],tr.getRotation()[1],tr.getRotation()[2],tr.getRotation()[3]);
+													int len = strlen(line);
+													fwrite(line,len,1,f);
+												}
+
+												if (strstr(sd.m_fileName.c_str(),".sdf") && i==0)
+												{
+													sprintf(line,"objects = p.loadSDF(\"%s\")\n",sd.m_fileName.c_str());
+													int len = strlen(line);
+													fwrite(line,len,1,f);
+												}
+
+												if (strstr(sd.m_fileName.c_str(),".sdf") || ((strstr(sd.m_fileName.c_str(),".urdf")) && mb->getNumLinks()) )
+												{
+													sprintf(line,"ob = objects[%d]\n",i);
+													int len = strlen(line);
+													fwrite(line,len,1,f);
+												}
+
+												if (strstr(sd.m_fileName.c_str(),".sdf"))
+												{
+													sprintf(line,"p.resetBasePositionAndOrientation(ob,[%f,%f,%f],[%f,%f,%f,%f])\n",
+														comTr.getOrigin()[0],comTr.getOrigin()[1],comTr.getOrigin()[2],
+														comTr.getRotation()[0],comTr.getRotation()[1],comTr.getRotation()[2],comTr.getRotation()[3]);
+													int len = strlen(line);
+													fwrite(line,len,1,f);
+												}
+
+												if (mb->getNumLinks())
+												{
+													{
+														sprintf(line,"jointPositions=[");
+														int len = strlen(line);
+														fwrite(line,len,1,f);
+													}
+
+													for (int i=0;i<mb->getNumLinks();i++)
+													{
+														btScalar jointPos = mb->getJointPosMultiDof(i)[0];
+														if (i<mb->getNumLinks()-1)
+														{
+															sprintf(line," %f,",jointPos);
+															int len = strlen(line);
+															fwrite(line,len,1,f);
+														} else
+														{
+															sprintf(line," %f ",jointPos);
+															int len = strlen(line);
+															fwrite(line,len,1,f);
+														}
+													}
+
+													{
+														sprintf(line,"]\nfor jointIndex in range (p.getNumJoints(ob)):\n\tp.resetJointState(ob,jointIndex,jointPositions[jointIndex])\n\n");
+														int len = strlen(line);
+														fwrite(line,len,1,f);
+													}
+												}
+											 } else
+											 {
+												 //todo: btRigidBody/btSoftBody etc case
+											 }
+										}
+									}
+									
+								}
+								
+								//for URDF, load at origin, then reposition...
+								
+
+								struct SaveWorldObjectData
+								{
+									b3AlignedObjectArray<int> m_bodyUniqueIds;
+									std::string	m_fileName;
+								};
+							}
+
+							{
+								btVector3 grav=this->m_data->m_dynamicsWorld->getGravity();
+								sprintf(line,"p.setGravity(%f,%f,%f)\n",grav[0],grav[1],grav[2]);
+								int len = strlen(line);
+								fwrite(line,len,1,f);
+							}
+														
+
+							{
+									sprintf(line,"p.stepSimulation()\np.disconnect()\n");
+									int len = strlen(line);
+									fwrite(line,len,1,f);
+							}
+							fclose(f);
+						}
+
+
+						serverStatusOut.m_type = CMD_SAVE_WORLD_COMPLETED;
+						hasStatus = true;
+						break;
+					}
+					serverStatusOut.m_type = CMD_SAVE_WORLD_FAILED;
+					hasStatus = true;
+					break;
+				}
                 case CMD_LOAD_SDF:
                     {
                         const SdfArgs& sdfArgs = clientCmd.m_sdfArguments;
@@ -1846,16 +2023,16 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
                 btVector3 linkLocalInertialOrigin = body->m_linkLocalInertialFrames[l].getOrigin();
                 btQuaternion linkLocalInertialRotation = body->m_linkLocalInertialFrames[l].getRotation();
 
-                btVector3 linkOrigin =  mb->getLink(l).m_cachedWorldTransform.getOrigin();
+                btVector3 linkCOMOrigin =  mb->getLink(l).m_cachedWorldTransform.getOrigin();
-                btQuaternion linkRotation =  mb->getLink(l).m_cachedWorldTransform.getRotation();
+                btQuaternion linkCOMRotation =  mb->getLink(l).m_cachedWorldTransform.getRotation();
 
-                serverCmd.m_sendActualStateArgs.m_linkState[l*7+0] = linkOrigin.getX();
+                serverCmd.m_sendActualStateArgs.m_linkState[l*7+0] = linkCOMOrigin.getX();
-                serverCmd.m_sendActualStateArgs.m_linkState[l*7+1] = linkOrigin.getY();
+                serverCmd.m_sendActualStateArgs.m_linkState[l*7+1] = linkCOMOrigin.getY();
-                serverCmd.m_sendActualStateArgs.m_linkState[l*7+2] = linkOrigin.getZ();
+                serverCmd.m_sendActualStateArgs.m_linkState[l*7+2] = linkCOMOrigin.getZ();
-                serverCmd.m_sendActualStateArgs.m_linkState[l*7+3] = linkRotation.x();
+                serverCmd.m_sendActualStateArgs.m_linkState[l*7+3] = linkCOMRotation.x();
-                serverCmd.m_sendActualStateArgs.m_linkState[l*7+4] = linkRotation.y();
+                serverCmd.m_sendActualStateArgs.m_linkState[l*7+4] = linkCOMRotation.y();
-                serverCmd.m_sendActualStateArgs.m_linkState[l*7+5] = linkRotation.z();
+                serverCmd.m_sendActualStateArgs.m_linkState[l*7+5] = linkCOMRotation.z();
-                serverCmd.m_sendActualStateArgs.m_linkState[l*7+6] = linkRotation.w();
+                serverCmd.m_sendActualStateArgs.m_linkState[l*7+6] = linkCOMRotation.w();
 
                 serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+0] = linkLocalInertialOrigin.getX();
                 serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+1] = linkLocalInertialOrigin.getY();
@@ -2745,11 +2922,13 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 									ikHelperPtr->computeNullspaceVel(numDofs, &q_current[0], &lower_limit[0], &upper_limit[0], &joint_range[0], &rest_pose[0]);
                                 }
                                 
+                                btTransform endEffectorTransformWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform * bodyHandle->m_linkLocalInertialFrames[endEffectorLinkIndex].inverse();
+                               
                                 btVector3DoubleData endEffectorWorldPosition;
                                 btVector3DoubleData endEffectorWorldOrientation;
                                 
-                                btVector3 endEffectorPosWorld =  bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform.getOrigin();
+                                btVector3 endEffectorPosWorld =  endEffectorTransformWorld.getOrigin();
-                                btQuaternion endEffectorOriWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform.getRotation();
+                                btQuaternion endEffectorOriWorld = endEffectorTransformWorld.getRotation();
                                 btVector4 endEffectorOri(endEffectorOriWorld.x(),endEffectorOriWorld.y(),endEffectorOriWorld.z(),endEffectorOriWorld.w());
                                 
                                 endEffectorPosWorld.serializeDouble(endEffectorWorldPosition);
@@ -3000,6 +3179,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
 		gBufferServerToClient.resize(SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
 		int bodyId = 0;
 
+		
 		if (gCreateObjectSimVR >= 0)
 		{
 			gCreateObjectSimVR = -1;
@@ -3189,7 +3369,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
 				//loadUrdf("rook.urdf", btVector3(-1.2, 0, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
 				//loadUrdf("knight.urdf", btVector3(-1.2, 0.2, 0.7), btQuaternion(btVector3(1, 0, 0), SIMD_HALF_PI), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
 
-				loadUrdf("husky/husky.urdf", btVector3(2, -5, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
+				//loadUrdf("husky/husky.urdf", btVector3(2, -5, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
 				m_data->m_huskyId = bodyId;
 
 				m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, -10));

examples/SharedMemory/PhysicsServerExample.cpp

@@ -12,6 +12,12 @@
 #include "Bullet3Common/b3Matrix3x3.h"
 #include "../Utils/b3Clock.h"
 #include "../MultiThreading/b3ThreadSupportInterface.h"
+#ifdef BT_ENABLE_VR
+#include "../RenderingExamples/TinyVRGui.h"
+#endif//BT_ENABLE_VR
+
+
+#include "../CommonInterfaces/CommonParameterInterface.h"
 
 #define MAX_VR_CONTROLLERS 8
 
@@ -546,6 +552,10 @@ class PhysicsServerExample : public SharedMemoryCommon
 	bool m_replay;
 	int m_options;
 	
+#ifdef BT_ENABLE_VR
+	TinyVRGui* m_tinyVrGui;
+#endif
+
 public:
 
 	PhysicsServerExample(MultiThreadedOpenGLGuiHelper* helper, SharedMemoryInterface* sharedMem=0, int options=0);
@@ -677,6 +687,9 @@ m_wantsShutdown(false),
 m_isConnected(false),
 m_replay(false),
 m_options(options)
+#ifdef BT_ENABLE_VR
+,m_tinyVrGui(0)
+#endif
 {
 	m_multiThreadedHelper = helper;
 	b3Printf("Started PhysicsServer\n");
@@ -686,6 +699,9 @@ m_options(options)
 
 PhysicsServerExample::~PhysicsServerExample()
 {
+#ifdef BT_ENABLE_VR
+	delete m_tinyVrGui;
+#endif
 	bool deInitializeSharedMemory = true;
 	m_physicsServer.disconnectSharedMemory(deInitializeSharedMemory);
     m_isConnected = false;
@@ -942,7 +958,73 @@ extern double gSubStep;
 void PhysicsServerExample::renderScene()
 {
 	B3_PROFILE("PhysicsServerExample::RenderScene");
+	static char line0[1024];
+		static char line1[1024];
 
+	if (gEnableRealTimeSimVR)
+	{
+		
+		static int frameCount=0;
+		static btScalar prevTime = m_clock.getTimeSeconds();
+		frameCount++;
+		
+		static btScalar worseFps = 1000000;
+		int numFrames = 200;
+		static int count = 0;
+		count++;
+
+		if (0 == (count & 1))
+		{
+			btScalar curTime = m_clock.getTimeSeconds();
+			btScalar fps = 1. / (curTime - prevTime);
+			prevTime = curTime;
+			if (fps < worseFps)
+			{
+				worseFps = fps;
+			}
+
+			if (count > numFrames)
+			{
+				count = 0;
+				sprintf(line0, "fps:%f frame:%d", worseFps, frameCount / 2);
+				sprintf(line1, "drop:%d tscale:%f dt:%f, substep %f)", gDroppedSimulationSteps, simTimeScalingFactor,gDtInSec, gSubStep);
+				gDroppedSimulationSteps = 0;
+
+				worseFps = 1000000;
+			}
+		}
+
+#ifdef BT_ENABLE_VR
+		if (m_tinyVrGui==0)
+		{
+			ComboBoxParams comboParams;
+        comboParams.m_comboboxId = 0;
+        comboParams.m_numItems = 0;
+        comboParams.m_startItem = 0;
+        comboParams.m_callback = 0;//MyComboBoxCallback;
+        comboParams.m_userPointer = 0;//this;
+        
+			m_tinyVrGui = new TinyVRGui(comboParams,this->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface());
+			m_tinyVrGui->init();
+		}
+
+		if (m_tinyVrGui)
+		{
+
+			b3Transform tr;tr.setIdentity();
+			tr.setOrigin(b3MakeVector3(gVRController2Pos[0],gVRController2Pos[1],gVRController2Pos[2]));
+			tr.setRotation(b3Quaternion(gVRController2Orn[0],gVRController2Orn[1],gVRController2Orn[2],gVRController2Orn[3]));
+			tr = tr*b3Transform(b3Quaternion(0,0,-SIMD_HALF_PI),b3MakeVector3(0,0,0));
+			b3Scalar dt = 0.01;
+			m_tinyVrGui->clearTextArea();
+			
+			m_tinyVrGui->grapicalPrintf(line0,0,0,0,0,0,255);
+			m_tinyVrGui->grapicalPrintf(line1,0,16,255,255,255,255);
+
+			m_tinyVrGui->tick(dt,tr);
+		}
+#endif//BT_ENABLE_VR
+	}
 	///debug rendering
 	//m_args[0].m_cs->lock();
 	
@@ -993,38 +1075,6 @@ void PhysicsServerExample::renderScene()
 		B3_PROFILE("Draw Debug HUD");
 		//some little experiment to add text/HUD to a VR camera (HTC Vive/Oculus Rift)
 
-		static int frameCount=0;
-		static btScalar prevTime = m_clock.getTimeSeconds();
-		frameCount++;
-		static char line0[1024];
-		static char line1[1024];
-
-		static btScalar worseFps = 1000000;
-		int numFrames = 200;
-		static int count = 0;
-		count++;
-
-		if (0 == (count & 1))
-		{
-			btScalar curTime = m_clock.getTimeSeconds();
-			btScalar fps = 1. / (curTime - prevTime);
-			prevTime = curTime;
-			if (fps < worseFps)
-			{
-				worseFps = fps;
-			}
-
-			if (count > numFrames)
-			{
-				count = 0;
-				sprintf(line0, "Graphics FPS (worse) = %f, frame %d", worseFps, frameCount / 2);
-
-				sprintf(line1, "Physics Steps = %d, Drop = %d, time scale=%f, dt %f, Substep %f)", gNumSteps, gDroppedSimulationSteps, simTimeScalingFactor,gDtInSec, gSubStep);
-				gDroppedSimulationSteps = 0;
-
-				worseFps = 1000000;
-			}
-		}
 
 		float pos[4];
 		m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->getCameraTargetPosition(pos);

examples/SharedMemory/SharedMemoryPublic.h

@@ -33,6 +33,7 @@ enum EnumSharedMemoryClientCommand
     CMD_CALCULATE_JACOBIAN,
     CMD_CREATE_JOINT,
     CMD_REQUEST_CONTACT_POINT_INFORMATION,
+	CMD_SAVE_WORLD,
     //don't go beyond this command!
     CMD_MAX_CLIENT_COMMANDS,
     
@@ -75,6 +76,10 @@ enum EnumSharedMemoryServerStatus
 		CMD_CONTACT_POINT_INFORMATION_FAILED,
 		CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED,
 		CMD_CALCULATE_INVERSE_KINEMATICS_FAILED,
+		CMD_SAVE_WORLD_COMPLETED,
+		CMD_SAVE_WORLD_FAILED,
+
+		//don't go beyond 'CMD_MAX_SERVER_COMMANDS!
         CMD_MAX_SERVER_COMMANDS
 };
 

examples/SharedMemory/premake4.lua

@@ -231,6 +231,9 @@ if os.is("Windows") then
 					"../ExampleBrowser/OpenGLGuiHelper.cpp",
 					"../ExampleBrowser/GL_ShapeDrawer.cpp",
 					"../ExampleBrowser/CollisionShape2TriangleMesh.cpp",
+					"../RenderingExamples/TinyVRGui.cpp",
+					"../RenderingExamples/TimeSeriesCanvas.cpp",
+					"../RenderingExamples/TimeSeriesFontData.cpp",
 					"../ThirdPartyLibs/openvr/samples/shared/lodepng.cpp",
 					"../ThirdPartyLibs/openvr/samples/shared/lodepng.h",
 					"../ThirdPartyLibs/openvr/samples/shared/Matrices.cpp",

examples/pybullet/pybullet.c

@@ -112,6 +112,48 @@ static PyObject* pybullet_disconnectPhysicsServer(PyObject* self,
   return Py_None;
 }
 
+static PyObject* pybullet_saveWorld(PyObject* self, PyObject* args) {
+	int size = PySequence_Size(args);
+	const char* worldFileName = "";
+
+	if (0 == sm) {
+    PyErr_SetString(SpamError, "Not connected to physics server.");
+    return NULL;
+  }
+
+   if (size == 1) {
+    if (!PyArg_ParseTuple(args, "s", &worldFileName))
+	{
+		return NULL;
+	}
+	else
+	{
+		b3SharedMemoryCommandHandle	command;
+		b3SharedMemoryStatusHandle statusHandle;
+		int statusType;
+
+		command = b3SaveWorldCommandInit(sm, worldFileName);
+		statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+		statusType = b3GetStatusType(statusHandle);
+		if (statusType != CMD_SAVE_WORLD_COMPLETED) {
+			PyErr_SetString(SpamError, "saveWorld command execution failed.");
+			return NULL;
+		}
+		Py_INCREF(Py_None);
+		return Py_None;
+	}
+  }
+
+
+     
+	PyErr_SetString(SpamError, "Cannot execute saveWorld command.");
+	return NULL;
+    
+    return NULL;
+}
+
+
+
 // Load a URDF file indicating the links and joints of an object
 // function can be called without arguments and will default
 // to position (0,0,1) with orientation(0,0,0,1)
@@ -2082,6 +2124,10 @@ static PyMethodDef SpamMethods[] = {
     {"loadSDF", pybullet_loadSDF, METH_VARARGS,
      "Load multibodies from an SDF file."},
 
+	{"saveWorld", pybullet_saveWorld, METH_VARARGS,
+     "Save an approximate Python file to reproduce the current state of the world: saveWorld"
+	"(filename). (very preliminary and approximately)"},
+
 	{"getNumBodies", pybullet_getNumBodies, METH_VARARGS,
      "Get the number of bodies in the simulation."},
 

examples/pybullet/saveWorld.py

@@ -0,0 +1,8 @@
+import pybullet as p
+import time
+
+p.connect(p.SHARED_MEMORY)
+timestr = time.strftime("%Y%m%d-%H%M%S")
+filename = "saveWorld" + timestr + ".py"
+p.saveWorld(filename)
+p.disconnect()

src/BulletDynamics/Featherstone/btMultiBody.cpp

@@ -28,7 +28,7 @@
 #include "btMultiBodyJointFeedback.h"
 #include "LinearMath/btTransformUtil.h"
 #include "LinearMath/btSerializer.h"
-#include "Bullet3Common/b3Logging.h"
+//#include "Bullet3Common/b3Logging.h"
 // #define INCLUDE_GYRO_TERM 
 
 ///todo: determine if we need these options. If so, make a proper API, otherwise delete those globals

src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp

@@ -1118,7 +1118,7 @@ static void applyJointFeedback(btMultiBodyJacobianData& data, const btMultiBodyS
 }
 #endif
 
-#include "Bullet3Common/b3Logging.h"
+
 void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint& c, btScalar deltaTime)
 {
 #if 1