Merge pull request #859 from erwincoumans/master

pybullet loadBullet/saveBullet, allow ExampleBrowser with --opengl2 fallback with z-up axis,

data/kiva_shelf/0_Bullet3Demo.txt

@@ -1,7 +0,0 @@
---start_demo_name=R2D2 Grasp
---mouse_move_multiplier=0.400000
---mouse_wheel_multiplier=0.010000
---background_color_red= 0.900000
---background_color_green= 0.900000
---background_color_blue= 1.000000
---fixed_timestep= 0.000000

examples/CommonInterfaces/CommonGUIHelperInterface.h

@@ -69,6 +69,12 @@ struct GUIHelperInterface
 	virtual void drawText3D( const char* txt, float posX, float posZY, float posZ, float size)=0;
 
 
+	
+	virtual int		addUserDebugText3D( const char* txt, const double posisionXYZ[3], const double	textColorRGB[3], double size, double lifeTime)=0;
+	virtual int		addUserDebugLine(const double	debugLineFromXYZ[3], const double	debugLineToXYZ[3], const double	debugLineColorRGB[3], double lineWidth, double lifeTime )=0;
+	virtual void	removeUserDebugItem( int debugItemUniqueId)=0;
+	virtual void	removeAllUserDebugItems( )=0;
+
 };
 
 
@@ -142,6 +148,21 @@ struct DummyGUIHelper : public GUIHelperInterface
 	{
 	}
 
+	virtual int		addUserDebugText3D( const char* txt, const double positionXYZ[3], const double	textColorRGB[3], double size, double lifeTime)
+	{
+		return -1;
+	}
+	virtual int		addUserDebugLine(const double	debugLineFromXYZ[3], const double	debugLineToXYZ[3], const double	debugLineColorRGB[3], double lineWidth, double lifeTime )
+	{
+		return -1;
+	}
+	virtual void	removeUserDebugItem( int debugItemUniqueId)
+	{
+	}
+	virtual void	removeAllUserDebugItems( )
+	{
+	}
+
 };
 
 #endif //GUI_HELPER_INTERFACE_H

examples/ExampleBrowser/GL_ShapeDrawer.cpp

@@ -782,7 +782,7 @@ GL_ShapeDrawer::~GL_ShapeDrawer()
 	}
 }
 
-void GL_ShapeDrawer::drawSceneInternal(const btDiscreteDynamicsWorld* dynamicsWorld, int pass)
+void GL_ShapeDrawer::drawSceneInternal(const btDiscreteDynamicsWorld* dynamicsWorld, int pass, int cameraUpAxis)
 {
 
 	btAssert(dynamicsWorld);
@@ -849,7 +849,12 @@ void GL_ShapeDrawer::drawSceneInternal(const btDiscreteDynamicsWorld* dynamicsWo
 		//if (!(getDebugMode()& btIDebugDraw::DBG_DrawWireframe))
 		int debugMode = 0;//getDebugMode()
 		//btVector3 m_sundirection(-1,-1,-1);
+		
 		btVector3 m_sundirection(btVector3(1,-2,1)*1000);
+		if (cameraUpAxis==2)
+		{
+			m_sundirection = btVector3(1,1,-2)*1000;
+		}
 				
 		switch(pass)
 		{
@@ -861,9 +866,12 @@ void GL_ShapeDrawer::drawSceneInternal(const btDiscreteDynamicsWorld* dynamicsWo
 
 }
 
-void GL_ShapeDrawer::drawScene(const btDiscreteDynamicsWorld* dynamicsWorld, bool useShadows)
+//this GL_ShapeDrawer will be removed, in the meanwhile directly access this global 'useShadoMaps'
+extern bool useShadowMap;
+void GL_ShapeDrawer::drawScene(const btDiscreteDynamicsWorld* dynamicsWorld, bool useShadows1, int cameraUpAxis)
 {
 
+	bool useShadows = useShadowMap;
 		GLfloat light_ambient[] = { btScalar(0.2), btScalar(0.2), btScalar(0.2), btScalar(1.0) };
 	GLfloat light_diffuse[] = { btScalar(1.0), btScalar(1.0), btScalar(1.0), btScalar(1.0) };
 	GLfloat light_specular[] = { btScalar(1.0), btScalar(1.0), btScalar(1.0), btScalar(1.0 )};
@@ -897,7 +905,7 @@ void GL_ShapeDrawer::drawScene(const btDiscreteDynamicsWorld* dynamicsWorld, boo
 		{
 			glClear(GL_STENCIL_BUFFER_BIT);
 			glEnable(GL_CULL_FACE);
-			drawSceneInternal(dynamicsWorld,0);
+			drawSceneInternal(dynamicsWorld,0, cameraUpAxis);
 
 			glDisable(GL_LIGHTING);
 			glDepthMask(GL_FALSE);
@@ -907,10 +915,10 @@ void GL_ShapeDrawer::drawScene(const btDiscreteDynamicsWorld* dynamicsWorld, boo
 			glStencilFunc(GL_ALWAYS,1,0xFFFFFFFFL);
 			glFrontFace(GL_CCW);
 			glStencilOp(GL_KEEP,GL_KEEP,GL_INCR);
-			drawSceneInternal(dynamicsWorld,1);
+			drawSceneInternal(dynamicsWorld,1,cameraUpAxis);
 			glFrontFace(GL_CW);
 			glStencilOp(GL_KEEP,GL_KEEP,GL_DECR);
-			drawSceneInternal(dynamicsWorld,1);
+			drawSceneInternal(dynamicsWorld,1,cameraUpAxis);
 			glFrontFace(GL_CCW);
 
 			glPolygonMode(GL_FRONT,GL_FILL);
@@ -929,7 +937,7 @@ void GL_ShapeDrawer::drawScene(const btDiscreteDynamicsWorld* dynamicsWorld, boo
 			glStencilFunc( GL_NOTEQUAL, 0, 0xFFFFFFFFL );
 			glStencilOp( GL_KEEP, GL_KEEP, GL_KEEP );
 			glDisable(GL_LIGHTING);
-			drawSceneInternal(dynamicsWorld,2);
+			drawSceneInternal(dynamicsWorld,2,cameraUpAxis);
 			glEnable(GL_LIGHTING);
 			glDepthFunc(GL_LESS);
 			glDisable(GL_STENCIL_TEST);
@@ -938,6 +946,6 @@ void GL_ShapeDrawer::drawScene(const btDiscreteDynamicsWorld* dynamicsWorld, boo
 		else
 		{
 			glDisable(GL_CULL_FACE);
-			drawSceneInternal(dynamicsWorld,0);
+			drawSceneInternal(dynamicsWorld,0,cameraUpAxis);
 		}
 }

examples/ExampleBrowser/GL_ShapeDrawer.h

@@ -44,7 +44,7 @@ protected:
 
 	ShapeCache*							cache(btConvexShape*);
 
-	virtual void drawSceneInternal(const btDiscreteDynamicsWorld* world, int pass);
+	virtual void drawSceneInternal(const btDiscreteDynamicsWorld* world, int pass, int cameraUpAxis);
 
 public:
 		GL_ShapeDrawer();
@@ -53,7 +53,7 @@ public:
 
 		
 
-		virtual void drawScene(const btDiscreteDynamicsWorld* world, bool useShadows);
+		virtual void drawScene(const btDiscreteDynamicsWorld* world, bool useShadows,  int cameraUpAxis);
 
 		///drawOpenGL might allocate temporary memoty, stores pointer in shape userpointer
 		virtual void		drawOpenGL(btScalar* m, const btCollisionShape* shape, const btVector3& color,int	debugMode,const btVector3& worldBoundsMin,const btVector3& worldBoundsMax);

examples/ExampleBrowser/OpenGLGuiHelper.cpp

@@ -338,7 +338,7 @@ void OpenGLGuiHelper::render(const btDiscreteDynamicsWorld* rbWorld)
 	if (m_data->m_gl2ShapeDrawer && rbWorld)
 	{
 		m_data->m_gl2ShapeDrawer->enableTexture(true);
-		m_data->m_gl2ShapeDrawer->drawScene(rbWorld,true);
+		m_data->m_gl2ShapeDrawer->drawScene(rbWorld,true, m_data->m_glApp->getUpAxis());
 	}
 }
 void OpenGLGuiHelper::createPhysicsDebugDrawer(btDiscreteDynamicsWorld* rbWorld)

examples/ExampleBrowser/OpenGLGuiHelper.h

@@ -55,9 +55,27 @@ struct OpenGLGuiHelper : public GUIHelperInterface
     
     virtual void drawText3D( const char* txt, float posX, float posY, float posZ, float size);
 
+	virtual int		addUserDebugText3D( const char* txt, const double positionXYZ[3], const double	textColorRGB[3], double size, double lifeTime)
+	{
+		return -1;
+	}
+	virtual int		addUserDebugLine(const double	debugLineFromXYZ[3], const double	debugLineToXYZ[3], const double	debugLineColorRGB[3], double lineWidth, double lifeTime )
+	{
+		return -1;
+	}
+	virtual void	removeUserDebugItem( int debugItemUniqueId)
+	{
+	}
+	virtual void	removeAllUserDebugItems( )
+	{
+	}
+
+
 	void renderInternalGl2(int  pass, const btDiscreteDynamicsWorld* dynamicsWorld);
 
 	void setVRMode(bool vrMode);
+
+
 };
 
 #endif //OPENGL_GUI_HELPER_H

examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp

@@ -0,0 +1,236 @@
+
+#include "ImportMJCFSetup.h"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
+//#define TEST_MULTIBODY_SERIALIZATION 1
+
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "Bullet3Common/b3FileUtils.h"
+
+#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
+#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
+#include "../CommonInterfaces/CommonParameterInterface.h"
+#include "../../Utils/b3ResourcePath.h"
+
+#include "../CommonInterfaces/CommonMultiBodyBase.h"
+
+#include "../ImportURDFDemo/MyMultiBodyCreator.h"
+
+class ImportMJCFSetup : public CommonMultiBodyBase
+{
+    char m_fileName[1024];
+
+    struct ImportMJCFInternalData* m_data;
+	bool m_useMultiBody;
+	btAlignedObjectArray<std::string* > m_nameMemory;
+	btScalar m_grav;
+	int m_upAxis;
+public:
+    ImportMJCFSetup(struct GUIHelperInterface* helper, int option, const char* fileName);
+    virtual ~ImportMJCFSetup();
+
+	virtual void initPhysics();
+	virtual void stepSimulation(float deltaTime);
+
+    void setFileName(const char* mjcfFileName);
+
+	virtual void resetCamera()
+	{
+		float dist = 3.5;
+		float pitch = -136;
+		float yaw = 28;
+		float targetPos[3]={0.47,0,-0.64};
+		m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
+	}
+};
+
+
+static btAlignedObjectArray<std::string> gMCFJFileNameArray;
+
+
+#define MAX_NUM_MOTORS 1024
+
+struct ImportMJCFInternalData
+{
+    ImportMJCFInternalData()
+    :m_numMotors(0),
+    m_mb(0)
+    {
+		for (int i=0;i<MAX_NUM_MOTORS;i++)
+		{
+			m_jointMotors[i] = 0;
+			m_generic6DofJointMotors[i] = 0;
+		}
+    }
+
+    
+    btScalar m_motorTargetVelocities[MAX_NUM_MOTORS];
+    btMultiBodyJointMotor* m_jointMotors [MAX_NUM_MOTORS];
+	btGeneric6DofSpring2Constraint* m_generic6DofJointMotors [MAX_NUM_MOTORS];
+    int m_numMotors;
+    btMultiBody* m_mb;
+    btRigidBody* m_rb;
+
+};
+
+
+ImportMJCFSetup::ImportMJCFSetup(struct GUIHelperInterface* helper, int option, const char* fileName)
+	:CommonMultiBodyBase(helper),
+	m_grav(0),
+	m_upAxis(2)
+{
+	m_data = new ImportMJCFInternalData;
+
+	if (option==1)
+	{
+		m_useMultiBody = true;
+	} else
+	{
+		m_useMultiBody = false;
+	}
+
+	static int count = 0;
+	if (fileName)
+	{
+		setFileName(fileName);
+	} else
+	{
+		gMCFJFileNameArray.clear();
+		
+
+
+		//load additional MJCF file names from file
+
+		FILE* f = fopen("mjcf_files.txt","r");
+		if (f)
+		{
+			int result;
+			//warning: we don't avoid string buffer overflow in this basic example in fscanf
+			char fileName[1024];
+			do
+			{
+				result = fscanf(f,"%s",fileName);
+                b3Printf("mjcf_files.txt entry %s",fileName);
+				if (result==1)
+				{
+					gMCFJFileNameArray.push_back(fileName);
+				}
+			} while (result==1);
+
+			fclose(f);
+		}
+		
+		if (gMCFJFileNameArray.size()==0)
+		{
+			gMCFJFileNameArray.push_back("quadruped/quadruped.mjcf");
+
+		}
+
+		int numFileNames = gMCFJFileNameArray.size();
+
+		if (count>=numFileNames)
+		{
+			count=0;
+		}
+		sprintf(m_fileName,"%s",gMCFJFileNameArray[count++].c_str());
+	}
+}
+
+ImportMJCFSetup::~ImportMJCFSetup()
+{
+	for (int i=0;i<m_nameMemory.size();i++)
+	{
+		delete m_nameMemory[i];
+	}
+	m_nameMemory.clear();
+    delete m_data;
+}
+
+static btVector4 colors[4] =
+{
+	btVector4(1,0,0,1),
+	btVector4(0,1,0,1),
+	btVector4(0,1,1,1),
+	btVector4(1,1,0,1),
+};
+
+
+static btVector3 selectColor()
+{
+
+	static int curColor = 0;
+	btVector4 color = colors[curColor];
+	curColor++;
+	curColor&=3;
+	return color;
+}
+
+void ImportMJCFSetup::setFileName(const char* mjcfFileName)
+{
+    memcpy(m_fileName,mjcfFileName,strlen(mjcfFileName)+1);
+}
+
+
+
+
+void ImportMJCFSetup::initPhysics()
+{
+
+	
+	m_guiHelper->setUpAxis(m_upAxis);
+	
+	this->createEmptyDynamicsWorld();
+	//m_dynamicsWorld->getSolverInfo().m_numIterations = 100;
+    m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+    m_dynamicsWorld->getDebugDrawer()->setDebugMode(
+    btIDebugDraw::DBG_DrawConstraints
+    +btIDebugDraw::DBG_DrawContactPoints
+    +btIDebugDraw::DBG_DrawAabb
+        );//+btIDebugDraw::DBG_DrawConstraintLimits);
+
+
+	if (m_guiHelper->getParameterInterface())
+	{
+		SliderParams slider("Gravity", &m_grav);
+		slider.m_minVal = -10;
+		slider.m_maxVal = 10;
+		m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
+	}
+	
+
+		
+}
+
+
+
+void ImportMJCFSetup::stepSimulation(float deltaTime)
+{
+	if (m_dynamicsWorld)
+	{
+		btVector3 gravity(0, 0, 0);
+		gravity[m_upAxis] = m_grav;
+		m_dynamicsWorld->setGravity(gravity);
+
+        for (int i=0;i<m_data->m_numMotors;i++)
+        {
+			if (m_data->m_jointMotors[i])
+			{
+				m_data->m_jointMotors[i]->setVelocityTarget(m_data->m_motorTargetVelocities[i]);
+			}
+			if (m_data->m_generic6DofJointMotors[i])
+			{
+				GenericConstraintUserInfo* jointInfo = (GenericConstraintUserInfo*)m_data->m_generic6DofJointMotors[i]->getUserConstraintPtr();
+				m_data->m_generic6DofJointMotors[i]->setTargetVelocity(jointInfo->m_jointAxisIndex,m_data->m_motorTargetVelocities[i]);
+				//jointInfo->
+			}
+        }
+
+		//the maximal coordinates/iterative MLCP solver requires a smallish timestep to converge
+		m_dynamicsWorld->stepSimulation(deltaTime,10,1./240.);
+	}
+}
+
+class CommonExampleInterface*    ImportMJCFCreateFunc(struct CommonExampleOptions& options)
+{
+
+	return new ImportMJCFSetup(options.m_guiHelper, options.m_option,options.m_fileName);
+}

examples/Importers/ImportMJCFDemo/ImportMJCFSetup.h

@@ -0,0 +1,8 @@
+#ifndef IMPORT_MJCF_SETUP_H
+#define IMPORT_MJCF_SETUP_H
+
+
+class CommonExampleInterface*    ImportMJCFCreateFunc(struct CommonExampleOptions& options);
+
+
+#endif //IMPORT_MJCF_SETUP_H

examples/OpenGLWindow/SimpleOpenGL2App.cpp

@@ -70,6 +70,11 @@ struct SimpleOpenGL2AppInternalData
 {
 	GLuint m_fontTextureId;
 	GLuint m_largeFontTextureId;
+	int m_upAxis;
+	SimpleOpenGL2AppInternalData()
+		:m_upAxis(1)
+	{
+	}
 	
 };
 static GLuint BindFont2(const CTexFont *_Font)
@@ -268,10 +273,11 @@ void SimpleOpenGL2App::drawGrid(DrawGridData data)
 }
 void SimpleOpenGL2App::setUpAxis(int axis)
 {
+	this->m_data->m_upAxis = axis;
 }
 int SimpleOpenGL2App::getUpAxis() const
 {
-	return 1;
+	return this->m_data->m_upAxis;
 }
 	
 void SimpleOpenGL2App::swapBuffer()

examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp

@@ -33,6 +33,7 @@ void SimpleOpenGL2Renderer::updateCamera(int upAxis)
     float projection[16];
     float view[16];
     m_camera.setAspectRatio((float)m_width/(float)m_height);
+	m_camera.setCameraUpAxis(upAxis);
     m_camera.update();
     m_camera.getCameraProjectionMatrix(projection);
     m_camera.getCameraViewMatrix(view);

examples/Raycast/RaytestDemo.cpp

@@ -200,7 +200,6 @@ void	RaytestDemo::initPhysics()
 		btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
 		btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia);
 		btRigidBody* body = new btRigidBody(rbInfo);
-		body->setRollingFriction(1);
 		body->setFriction(1);
 		//add the body to the dynamics world
 		m_dynamicsWorld->addRigidBody(body);
@@ -269,6 +268,7 @@ void	RaytestDemo::initPhysics()
 			rbInfo.m_startWorldTransform = startTransform;
 			btRigidBody* body = new btRigidBody(rbInfo);
 			body->setRollingFriction(0.03);
+			body->setSpinningFriction(0.03);
 			body->setFriction(1);
 			body->setAnisotropicFriction(colShape->getAnisotropicRollingFrictionDirection(),btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
 		

examples/RoboticsLearning/b3RobotSimAPI.cpp

@@ -401,6 +401,21 @@ public:
     
 	virtual void drawText3D( const char* txt, float posX, float posZY, float posZ, float size)
 	{
+	}
+
+		virtual int		addUserDebugText3D( const char* txt, const double positionXYZ[3], const double	textColorRGB[3], double size, double lifeTime)
+	{
+		return -1;
+	}
+	virtual int		addUserDebugLine(const double	debugLineFromXYZ[3], const double	debugLineToXYZ[3], const double	debugLineColorRGB[3], double lineWidth, double lifeTime )
+	{
+		return -1;
+	}
+	virtual void	removeUserDebugItem( int debugItemUniqueId)
+	{
+	}
+	virtual void	removeAllUserDebugItems( )
+	{
 	}
 };
 

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -79,6 +79,87 @@ b3SharedMemoryCommandHandle	b3LoadUrdfCommandInit(b3PhysicsClientHandle physClie
 	return 0;
 }
 
+b3SharedMemoryCommandHandle	b3LoadBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName)
+{
+	PhysicsClient* cl = (PhysicsClient*)physClient;
+	b3Assert(cl);
+	b3Assert(cl->canSubmitCommand());
+
+	if (cl->canSubmitCommand())
+	{
+		struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+		b3Assert(command);
+		command->m_type = CMD_LOAD_BULLET;
+		int len = strlen(fileName);
+		if (len < MAX_URDF_FILENAME_LENGTH)
+		{
+			strcpy(command->m_fileArguments.m_fileName, fileName);
+		}
+		else
+		{
+			command->m_fileArguments.m_fileName[0] = 0;
+		}
+		command->m_updateFlags = 0;
+
+		return (b3SharedMemoryCommandHandle)command;
+	}
+	return 0;
+}
+
+b3SharedMemoryCommandHandle	b3SaveBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName)
+{
+	PhysicsClient* cl = (PhysicsClient*)physClient;
+	b3Assert(cl);
+	b3Assert(cl->canSubmitCommand());
+
+	if (cl->canSubmitCommand())
+	{
+		struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+		b3Assert(command);
+		command->m_type = CMD_SAVE_BULLET;
+		int len = strlen(fileName);
+		if (len < MAX_URDF_FILENAME_LENGTH)
+		{
+			strcpy(command->m_fileArguments.m_fileName, fileName);
+		}
+		else
+		{
+			command->m_fileArguments.m_fileName[0] = 0;
+		}
+		command->m_updateFlags = 0;
+
+		return (b3SharedMemoryCommandHandle)command;
+	}
+	return 0;
+}
+b3SharedMemoryCommandHandle	b3LoadMJCFCommandInit(b3PhysicsClientHandle physClient, const char* fileName)
+{
+	PhysicsClient* cl = (PhysicsClient*)physClient;
+	b3Assert(cl);
+	b3Assert(cl->canSubmitCommand());
+
+	if (cl->canSubmitCommand())
+	{
+		struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+		b3Assert(command);
+		command->m_type = CMD_LOAD_MJCF;
+		int len = strlen(fileName);
+		if (len < MAX_URDF_FILENAME_LENGTH)
+		{
+			strcpy(command->m_fileArguments.m_fileName, fileName);
+		}
+		else
+		{
+			command->m_fileArguments.m_fileName[0] = 0;
+		}
+		command->m_updateFlags = 0;
+
+		return (b3SharedMemoryCommandHandle)command;
+	}
+	return 0;
+}
+
+
 b3SharedMemoryCommandHandle	b3LoadBunnyCommandInit(b3PhysicsClientHandle physClient)
 {
     PhysicsClient* cl = (PhysicsClient* ) physClient;
@@ -712,6 +793,7 @@ int b3GetStatusBodyIndices(b3SharedMemoryStatusHandle statusHandle, int* bodyInd
 	{
 			switch (status->m_type)
 			{
+				case CMD_BULLET_LOADING_COMPLETED:
 				case CMD_SDF_LOADING_COMPLETED:
 				{
 				    int i,maxBodies;
@@ -981,6 +1063,107 @@ void    b3GetDebugLines(b3PhysicsClientHandle physClient, struct b3DebugLines* l
     
 }
 
+
+/// Add/remove user-specific debug lines and debug text messages
+b3SharedMemoryCommandHandle b3InitUserDebugDrawAddLine3D(b3PhysicsClientHandle physClient, double fromXYZ[3], double toXYZ[3], double colorRGB[3], double lineWidth, double lifeTime)
+{
+	PhysicsClient* cl = (PhysicsClient* ) physClient;
+	b3Assert(cl);
+    b3Assert(cl->canSubmitCommand());
+    struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+    b3Assert(command);
+    command->m_type =CMD_USER_DEBUG_DRAW;
+	command->m_updateFlags = USER_DEBUG_HAS_LINE; //USER_DEBUG_HAS_TEXT
+	
+	command->m_userDebugDrawArgs.m_debugLineFromXYZ[0] = fromXYZ[0];
+	command->m_userDebugDrawArgs.m_debugLineFromXYZ[1] = fromXYZ[1];
+	command->m_userDebugDrawArgs.m_debugLineFromXYZ[2] = fromXYZ[2];
+
+	command->m_userDebugDrawArgs.m_debugLineToXYZ[0] = toXYZ[0];
+	command->m_userDebugDrawArgs.m_debugLineToXYZ[1] = toXYZ[1];
+	command->m_userDebugDrawArgs.m_debugLineToXYZ[2] = toXYZ[2];
+	
+	command->m_userDebugDrawArgs.m_debugLineColorRGB[0] = colorRGB[0];
+	command->m_userDebugDrawArgs.m_debugLineColorRGB[1] = colorRGB[1];
+	command->m_userDebugDrawArgs.m_debugLineColorRGB[2] = colorRGB[2];
+		
+	command->m_userDebugDrawArgs.m_lineWidth = lineWidth;
+	command->m_userDebugDrawArgs.m_lifeTime = lifeTime;
+
+	return (b3SharedMemoryCommandHandle) command;
+}
+
+b3SharedMemoryCommandHandle b3InitUserDebugDrawAddText3D(b3PhysicsClientHandle physClient, const char* txt, double positionXYZ[3], double colorRGB[3], double textSize, double lifeTime)
+{
+
+	PhysicsClient* cl = (PhysicsClient* ) physClient;
+	b3Assert(cl);
+    b3Assert(cl->canSubmitCommand());
+    struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+    b3Assert(command);
+    command->m_type =CMD_USER_DEBUG_DRAW;
+	command->m_updateFlags = USER_DEBUG_HAS_TEXT;
+	
+	int len = strlen(txt);
+    if (len<MAX_FILENAME_LENGTH)
+    {
+		strcpy(command->m_userDebugDrawArgs.m_text,txt);
+    } else
+    {
+        command->m_userDebugDrawArgs.m_text[0] = 0;
+    }
+	command->m_userDebugDrawArgs.m_textPositionXYZ[0] = positionXYZ[0];
+	command->m_userDebugDrawArgs.m_textPositionXYZ[1] = positionXYZ[1];
+	command->m_userDebugDrawArgs.m_textPositionXYZ[2] = positionXYZ[2];
+
+	command->m_userDebugDrawArgs.m_textColorRGB[0] = colorRGB[0];
+	command->m_userDebugDrawArgs.m_textColorRGB[1] = colorRGB[1];
+	command->m_userDebugDrawArgs.m_textColorRGB[2] = colorRGB[2];
+	
+	command->m_userDebugDrawArgs.m_textSize = textSize;
+
+	command->m_userDebugDrawArgs.m_lifeTime = lifeTime;
+
+	return (b3SharedMemoryCommandHandle) command;
+}
+
+b3SharedMemoryCommandHandle b3InitUserDebugDrawRemove(b3PhysicsClientHandle physClient, int debugItemUniqueId)
+{
+	PhysicsClient* cl = (PhysicsClient* ) physClient;
+	b3Assert(cl);
+    b3Assert(cl->canSubmitCommand());
+    struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+    b3Assert(command);
+    command->m_type =CMD_USER_DEBUG_DRAW;
+	command->m_updateFlags = USER_DEBUG_REMOVE_ONE_ITEM;
+	command->m_userDebugDrawArgs.m_removeItemUniqueId = debugItemUniqueId;
+	return (b3SharedMemoryCommandHandle) command;
+
+}
+
+b3SharedMemoryCommandHandle b3InitUserDebugDrawRemoveAll(b3PhysicsClientHandle physClient)
+{
+	PhysicsClient* cl = (PhysicsClient* ) physClient;
+	b3Assert(cl);
+    b3Assert(cl->canSubmitCommand());
+    struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+    b3Assert(command);
+    command->m_type =CMD_USER_DEBUG_DRAW;
+	command->m_updateFlags = USER_DEBUG_REMOVE_ALL;
+	return (b3SharedMemoryCommandHandle) command;
+}
+
+int b3GetDebugItemUniqueId(b3SharedMemoryStatusHandle statusHandle)
+{
+    const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
+    btAssert(status->m_type == CMD_USER_DEBUG_DRAW_COMPLETED);
+    if (status->m_type != CMD_USER_DEBUG_DRAW_COMPLETED)
+        return -1;
+
+	return status->m_userDebugDrawArgs.m_debugItemUniqueId;
+}
+
+
 ///request an image from a simulated camera, using a software renderer.
 b3SharedMemoryCommandHandle b3InitRequestCameraImage(b3PhysicsClientHandle physClient)
 {
@@ -1546,6 +1729,7 @@ b3SharedMemoryCommandHandle	b3CalculateJacobianCommandInit(b3PhysicsClientHandle
     return (b3SharedMemoryCommandHandle)command;
 }
 
+
 int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, double* linearJacobian, double* angularJacobian)
 {
     const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;

examples/SharedMemory/PhysicsClientC_API.h

@@ -74,14 +74,22 @@ int	b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointInd
     
 b3SharedMemoryCommandHandle b3CreateJoint(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info);
 
-///Request debug lines for debug visualization. The flags in debugMode are the same as used in Bullet
+///Request physics debug lines for debug visualization. The flags in debugMode are the same as used in Bullet
 ///See btIDebugDraw::DebugDrawModes in Bullet/src/LinearMath/btIDebugDraw.h
 b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode);
 
-///Get the pointers to the debug line information, after b3InitRequestDebugLinesCommand returns
+///Get the pointers to the physics debug line information, after b3InitRequestDebugLinesCommand returns
 ///status CMD_DEBUG_LINES_COMPLETED
 void    b3GetDebugLines(b3PhysicsClientHandle physClient, struct b3DebugLines* lines);
 
+/// Add/remove user-specific debug lines and debug text messages
+b3SharedMemoryCommandHandle b3InitUserDebugDrawAddLine3D(b3PhysicsClientHandle physClient, double fromXYZ[3], double toXYZ[3], double colorRGB[3], double lineWidth, double lifeTime);
+b3SharedMemoryCommandHandle b3InitUserDebugDrawAddText3D(b3PhysicsClientHandle physClient, const char* txt, double positionXYZ[3], double colorRGB[3], double textSize, double lifeTime);
+b3SharedMemoryCommandHandle b3InitUserDebugDrawRemove(b3PhysicsClientHandle physClient, int debugItemUniqueId);
+b3SharedMemoryCommandHandle b3InitUserDebugDrawRemoveAll(b3PhysicsClientHandle physClient);
+///All debug items unique Ids are positive: a negative unique Id means failure.
+int b3GetDebugItemUniqueId(b3SharedMemoryStatusHandle statusHandle);
+
 ///request an image from a simulated camera, using a software renderer.
 b3SharedMemoryCommandHandle b3InitRequestCameraImage(b3PhysicsClientHandle physClient);
 void b3RequestCameraImageSetCameraMatrices(b3SharedMemoryCommandHandle command, float viewMatrix[16], float projectionMatrix[16]);
@@ -143,6 +151,11 @@ int	b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHand
 int	b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
 int	b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase);
 
+b3SharedMemoryCommandHandle	b3LoadBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
+b3SharedMemoryCommandHandle	b3SaveBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
+b3SharedMemoryCommandHandle	b3LoadMJCFCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
+
+
 
 ///compute the forces to achieve an acceleration, given a state q and qdot using inverse dynamics
 b3SharedMemoryCommandHandle	b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex,
@@ -243,6 +256,7 @@ b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandl
 void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double force[3], const double position[3], int flags);
 void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double torque[3], int flags);
     
+///experiments of robots interacting with non-rigid objects (such as btSoftBody)
 b3SharedMemoryCommandHandle	b3LoadBunnyCommandInit(b3PhysicsClientHandle physClient);
 int b3LoadBunnySetScale(b3SharedMemoryCommandHandle commandHandle, double scale);
 int b3LoadBunnySetMass(b3SharedMemoryCommandHandle commandHandle, double mass);

examples/SharedMemory/PhysicsClientSharedMemory.cpp

@@ -703,6 +703,35 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
                 b3Warning("Load texture failed");
                 break;
             }
+			case CMD_BULLET_LOADING_COMPLETED:
+			{
+				break;
+			}
+			case CMD_BULLET_LOADING_FAILED:
+			{
+				b3Warning("Load .bullet failed");
+				break;
+			}
+			case CMD_BULLET_SAVING_FAILED:
+			{
+				b3Warning("Save .bullet failed");
+				break;
+			}
+			case CMD_MJCF_LOADING_FAILED:
+			{
+				b3Warning("Load .mjcf failed");
+				break;
+			}
+			case CMD_USER_DEBUG_DRAW_COMPLETED:
+			{
+				break;
+			}
+			case CMD_USER_DEBUG_DRAW_FAILED:
+			{
+				b3Warning("User debug draw failed");
+				break;
+			}
+
             default: {
                 b3Error("Unknown server status %d\n", serverCmd.m_type);
                 btAssert(0);

examples/SharedMemory/PhysicsDirect.cpp

@@ -662,10 +662,19 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
 		}
 		break;
 	}
+	case CMD_BULLET_LOADING_FAILED:
+	{
+		b3Warning("Couldn't load .bullet file");
+		break;
+	}
+	case CMD_BULLET_LOADING_COMPLETED:
+	{
+		break;
+	}
 
 	default:
 	{
-		// b3Error("Unknown server status type");
+		b3Warning("Unknown server status type");
 	}
 	};
 

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -3363,14 +3363,174 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
                     SharedMemoryStatus& serverCmd = serverStatusOut;
                     serverCmd.m_type = CMD_LOAD_TEXTURE_FAILED;
                     
-                    m_data->m_visualConverter.loadTextureFile(clientCmd.m_loadTextureArguments.m_textureFileName);
+                    int uid = m_data->m_visualConverter.loadTextureFile(clientCmd.m_loadTextureArguments.m_textureFileName);
                     
+                    if (uid>=0)
+                    {
                         serverCmd.m_type = CMD_LOAD_TEXTURE_COMPLETED;
+                    } else
+                    {
+                        serverCmd.m_type = CMD_LOAD_TEXTURE_FAILED;
+                    }
                     hasStatus = true;
                     
                     break;
                 }
 
+				case CMD_LOAD_BULLET:
+				{
+					
+					SharedMemoryStatus& serverCmd = serverStatusOut;
+					btBulletWorldImporter* importer = new btBulletWorldImporter(m_data->m_dynamicsWorld);
+
+					const char* prefix[] = { "", "./", "./data/", "../data/", "../../data/", "../../../data/", "../../../../data/" };
+					int numPrefixes = sizeof(prefix) / sizeof(const char*);
+					char relativeFileName[1024];
+					FILE* f = 0;
+					bool found = false;
+
+					for (int i = 0; !f && i<numPrefixes; i++)
+					{
+						sprintf(relativeFileName, "%s%s", prefix[i], clientCmd.m_fileArguments.m_fileName);
+						f = fopen(relativeFileName, "rb");
+						if (f)
+						{
+							found = true;
+							break;
+						}
+					}
+					if (f)
+					{
+						fclose(f);
+					}
+
+					if (found)
+					{
+						bool ok = importer->loadFile(relativeFileName);
+						if (ok)
+						{
+							
+							
+							int numRb = importer->getNumRigidBodies();
+							serverStatusOut.m_sdfLoadedArgs.m_numBodies = 0;
+
+							for( int i=0;i<numRb;i++)
+							{
+								btCollisionObject* colObj = importer->getRigidBodyByIndex(i);
+
+								if (colObj)
+								{
+									btRigidBody* rb = btRigidBody::upcast(colObj);
+									if (rb)
+									{
+										int bodyUniqueId = m_data->allocHandle();
+										InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId);
+										colObj->setUserIndex2(bodyUniqueId);
+										bodyHandle->m_rigidBody = rb;
+
+										if (serverStatusOut.m_sdfLoadedArgs.m_numBodies<MAX_SDF_BODIES)
+										{
+											serverStatusOut.m_sdfLoadedArgs.m_numBodies++;
+											serverStatusOut.m_sdfLoadedArgs.m_bodyUniqueIds[i] = bodyUniqueId;
+										}
+									}
+								}
+							}
+
+							serverCmd.m_type = CMD_BULLET_LOADING_COMPLETED;
+							m_data->m_guiHelper->autogenerateGraphicsObjects(m_data->m_dynamicsWorld);
+							hasStatus = true;
+							break;
+						}
+					}
+					serverCmd.m_type = CMD_BULLET_LOADING_FAILED;
+					hasStatus = true;
+					break;
+				}
+
+				case CMD_SAVE_BULLET:
+				{
+					SharedMemoryStatus& serverCmd = serverStatusOut;
+					
+					FILE* f = fopen(clientCmd.m_fileArguments.m_fileName, "wb");
+					if (f)
+					{
+						btDefaultSerializer* ser = new btDefaultSerializer();
+						m_data->m_dynamicsWorld->serialize(ser);
+						fwrite(ser->getBufferPointer(), ser->getCurrentBufferSize(), 1, f);
+						fclose(f);
+						serverCmd.m_type = CMD_BULLET_SAVING_COMPLETED;
+						delete ser;
+					}
+					serverCmd.m_type = CMD_BULLET_SAVING_FAILED;
+					hasStatus = true;
+					break;
+				}
+
+				case CMD_LOAD_MJCF:
+				{
+					SharedMemoryStatus& serverCmd = serverStatusOut;
+					serverCmd.m_type = CMD_MJCF_LOADING_FAILED;
+					hasStatus = true;
+					break;
+				}
+				
+				case CMD_USER_DEBUG_DRAW:
+					{
+						SharedMemoryStatus& serverCmd = serverStatusOut;
+						serverCmd.m_type = CMD_USER_DEBUG_DRAW_FAILED;
+						hasStatus = true;
+						
+
+						if (clientCmd.m_updateFlags & USER_DEBUG_HAS_TEXT)
+						{
+							int uid = m_data->m_guiHelper->addUserDebugText3D(clientCmd.m_userDebugDrawArgs.m_text,
+								clientCmd.m_userDebugDrawArgs.m_textPositionXYZ,
+								clientCmd.m_userDebugDrawArgs.m_textColorRGB,
+								clientCmd.m_userDebugDrawArgs.m_textSize,
+								clientCmd.m_userDebugDrawArgs.m_lifeTime);
+
+							if (uid>=0)
+							{
+								serverCmd.m_userDebugDrawArgs.m_debugItemUniqueId = uid;
+								serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED;
+							}
+						}
+
+						if (clientCmd.m_updateFlags & USER_DEBUG_HAS_LINE)
+						{
+							int uid = m_data->m_guiHelper->addUserDebugLine(
+								clientCmd.m_userDebugDrawArgs.m_debugLineFromXYZ,
+								clientCmd.m_userDebugDrawArgs.m_debugLineToXYZ,
+								clientCmd.m_userDebugDrawArgs.m_debugLineColorRGB,
+								clientCmd.m_userDebugDrawArgs.m_lineWidth,
+								clientCmd.m_userDebugDrawArgs.m_lifeTime);
+
+							if (uid>=0)
+							{
+								serverCmd.m_userDebugDrawArgs.m_debugItemUniqueId = uid;
+								serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED;
+							}
+						}
+						
+
+						if (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_ALL)
+						{
+							m_data->m_guiHelper->removeAllUserDebugItems();
+							serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED;
+
+						}
+						if (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_ONE_ITEM)
+						{
+							m_data->m_guiHelper->removeUserDebugItem(clientCmd.m_userDebugDrawArgs.m_removeItemUniqueId);
+							serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED;
+
+						}
+
+						break;        
+					}
+		
+
                 default:
                 {
                     b3Error("Unknown command encountered");

examples/SharedMemory/PhysicsServerExample.cpp

@@ -74,6 +74,11 @@ enum MultiThreadedGUIHelperCommunicationEnums
 	eGUIHelperCreateRigidBodyGraphicsObject,
 	eGUIHelperRemoveAllGraphicsInstances,
 	eGUIHelperCopyCameraImageData,
+	eGUIHelperAutogenerateGraphicsObjects,
+	eGUIUserDebugAddText,
+	eGUIUserDebugAddLine,
+	eGUIUserDebugRemoveItem,
+	eGUIUserDebugRemoveAllItems,
 };
 
 #include <stdio.h>
@@ -270,6 +275,30 @@ void*	MotionlsMemoryFunc()
 
 
 
+struct UserDebugDrawLine
+{
+	double	m_debugLineFromXYZ[3];
+	double	m_debugLineToXYZ[3];
+	double	m_debugLineColorRGB[3];
+	double	m_lineWidth;
+	
+	double m_lifeTime;
+	int m_itemUniqueId;
+};
+
+struct UserDebugText
+{
+	char m_text[1024];
+	double m_textPositionXYZ[3];
+	double m_textColorRGB[3];
+	double textSize;
+
+	double m_lifeTime;
+	int m_itemUniqueId;
+};
+
+
+
 class MultiThreadedOpenGLGuiHelper : public GUIHelperInterface
 {
 	CommonGraphicsApp* m_app;
@@ -277,10 +306,12 @@ class MultiThreadedOpenGLGuiHelper : public GUIHelperInterface
 	b3CriticalSection* m_cs;
 	
 	
+
 public:
 
 	GUIHelperInterface* m_childGuiHelper;
 
+	int m_uidGenerator;
 	const unsigned char* m_texels;
 	int m_textureWidth;
 	int m_textureHeight;
@@ -304,6 +335,7 @@ public:
 	MultiThreadedOpenGLGuiHelper(CommonGraphicsApp* app, GUIHelperInterface* guiHelper)
 		:m_app(app)
 		,m_cs(0),
+		m_uidGenerator(0),
 		m_texels(0),
 		m_textureId(-1)
 	{
@@ -530,13 +562,113 @@ public:
 	}
 	
 
+	btDiscreteDynamicsWorld* m_dynamicsWorld;
+
 	virtual void autogenerateGraphicsObjects(btDiscreteDynamicsWorld* rbWorld) 
 	{
+		m_dynamicsWorld = rbWorld;
+		m_cs->lock();
+		m_cs->setSharedParam(1, eGUIHelperAutogenerateGraphicsObjects);
+		m_cs->unlock();
+		while (m_cs->getSharedParam(1) != eGUIHelperIdle)
+		{
+			b3Clock::usleep(1000);
+		}
 	}
     
 	virtual void drawText3D( const char* txt, float posX, float posZY, float posZ, float size)
 	{
 	}
+
+
+
+
+	btAlignedObjectArray<UserDebugText> m_userDebugText;
+	
+	UserDebugText m_tmpText;
+
+	virtual int		addUserDebugText3D( const char* txt, const double positionXYZ[3], const double	textColorRGB[3], double size, double lifeTime)
+	{
+		
+		m_tmpText.m_itemUniqueId = m_uidGenerator++;
+		m_tmpText.m_lifeTime = lifeTime;
+		m_tmpText.textSize = size;
+		int len = strlen(txt);
+		strcpy(m_tmpText.m_text,txt);
+		m_tmpText.m_textPositionXYZ[0] = positionXYZ[0];
+		m_tmpText.m_textPositionXYZ[1] = positionXYZ[1];
+		m_tmpText.m_textPositionXYZ[2] = positionXYZ[2];
+		m_tmpText.m_textColorRGB[0] = textColorRGB[0];
+		m_tmpText.m_textColorRGB[1] = textColorRGB[1];
+		m_tmpText.m_textColorRGB[2] = textColorRGB[2];
+
+		m_cs->lock();
+		m_cs->setSharedParam(1, eGUIUserDebugAddText);
+		m_cs->unlock();
+		while (m_cs->getSharedParam(1) != eGUIHelperIdle)
+		{
+			b3Clock::usleep(150);
+		}
+
+		return m_userDebugText[m_userDebugText.size()-1].m_itemUniqueId;
+	}
+
+	btAlignedObjectArray<UserDebugDrawLine> m_userDebugLines;
+	UserDebugDrawLine m_tmpLine;
+
+	virtual int		addUserDebugLine(const double	debugLineFromXYZ[3], const double	debugLineToXYZ[3], const double	debugLineColorRGB[3], double lineWidth, double lifeTime )
+	{
+		m_tmpLine.m_lifeTime = lifeTime;
+		m_tmpLine.m_lineWidth = lineWidth;
+		m_tmpLine.m_itemUniqueId = m_uidGenerator++;
+		m_tmpLine.m_debugLineFromXYZ[0] = debugLineFromXYZ[0];
+		m_tmpLine.m_debugLineFromXYZ[1] = debugLineFromXYZ[1];
+		m_tmpLine.m_debugLineFromXYZ[2] = debugLineFromXYZ[2];
+
+		m_tmpLine.m_debugLineToXYZ[0] = debugLineToXYZ[0];
+		m_tmpLine.m_debugLineToXYZ[1] = debugLineToXYZ[1];
+		m_tmpLine.m_debugLineToXYZ[2] = debugLineToXYZ[2];
+		
+		m_tmpLine.m_debugLineColorRGB[0] = debugLineColorRGB[0];
+		m_tmpLine.m_debugLineColorRGB[1] = debugLineColorRGB[1];
+		m_tmpLine.m_debugLineColorRGB[2] = debugLineColorRGB[2];
+		
+		m_cs->lock();
+		m_cs->setSharedParam(1, eGUIUserDebugAddLine);
+		m_cs->unlock();
+		while (m_cs->getSharedParam(1) != eGUIHelperIdle)
+		{
+			b3Clock::usleep(150);
+		}
+		return m_userDebugLines[m_userDebugLines.size()-1].m_itemUniqueId;
+	}
+
+	int m_removeDebugItemUid;
+
+	virtual void	removeUserDebugItem( int debugItemUniqueId)
+	{
+		m_removeDebugItemUid = debugItemUniqueId;
+		m_cs->lock();
+		m_cs->setSharedParam(1, eGUIUserDebugRemoveItem);
+		m_cs->unlock();
+		while (m_cs->getSharedParam(1) != eGUIHelperIdle)
+		{
+			b3Clock::usleep(150);
+		}
+
+	}	
+	virtual void	removeAllUserDebugItems( )
+	{
+		m_cs->lock();
+		m_cs->setSharedParam(1, eGUIUserDebugRemoveAllItems);
+		m_cs->unlock();
+		while (m_cs->getSharedParam(1) != eGUIHelperIdle)
+		{
+			b3Clock::usleep(150);
+		}
+	
+	}
+
 };
 
 
@@ -916,10 +1048,76 @@ void	PhysicsServerExample::stepSimulation(float deltaTime)
             m_multiThreadedHelper->getCriticalSection()->unlock();
             break;
         }
+	case eGUIHelperAutogenerateGraphicsObjects:
+	{
+		m_multiThreadedHelper->m_childGuiHelper->autogenerateGraphicsObjects(m_multiThreadedHelper->m_dynamicsWorld);
+		m_multiThreadedHelper->getCriticalSection()->lock();
+		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->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->getCriticalSection()->setSharedParam(1, eGUIHelperIdle);
+		m_multiThreadedHelper->getCriticalSection()->unlock();
+			break;
+	}
+	case eGUIUserDebugRemoveItem:
+	{
+		for (int i=0;i<m_multiThreadedHelper->m_userDebugLines.size();i++)
+		{
+			if (m_multiThreadedHelper->m_userDebugLines[i].m_itemUniqueId == m_multiThreadedHelper->m_removeDebugItemUid)
+			{
+				m_multiThreadedHelper->m_userDebugLines.swap(i,m_multiThreadedHelper->m_userDebugLines.size()-1);
+				m_multiThreadedHelper->m_userDebugLines.pop_back();
+				break;
+			}
+		}
+
+
+		for (int i=0;i<m_multiThreadedHelper->m_userDebugText.size();i++)
+		{
+			if (m_multiThreadedHelper->m_userDebugText[i].m_itemUniqueId == m_multiThreadedHelper->m_removeDebugItemUid)
+			{
+				m_multiThreadedHelper->m_userDebugText.swap(i,m_multiThreadedHelper->m_userDebugText.size()-1);
+				m_multiThreadedHelper->m_userDebugText.pop_back();
+				break;
+			}
+		}
+
+		m_multiThreadedHelper->getCriticalSection()->lock();
+		m_multiThreadedHelper->getCriticalSection()->setSharedParam(1, eGUIHelperIdle);
+		m_multiThreadedHelper->getCriticalSection()->unlock();
+			break;
+	}
+	case eGUIUserDebugRemoveAllItems:
+	{
+		m_multiThreadedHelper->m_userDebugLines.clear();
+		m_multiThreadedHelper->m_userDebugText.clear();
+		m_multiThreadedHelper->m_uidGenerator = 0;
+		m_multiThreadedHelper->getCriticalSection()->lock();
+		m_multiThreadedHelper->getCriticalSection()->setSharedParam(1, eGUIHelperIdle);
+		m_multiThreadedHelper->getCriticalSection()->unlock();
+			break;
+	}
 	case eGUIHelperIdle:
+		{
+			break;
+		}
 	default:
 		{
-			
+			btAssert(0);
 		}
 	}
 	
@@ -974,15 +1172,38 @@ void PhysicsServerExample::renderScene()
 	//add array of lines
 
 	//draw all user- 'text3d' messages
-		if (m_guiHelper)
+		if (m_multiThreadedHelper)
 		{
-			btVector3 from(0, 0, 0);
-			btVector3 toX(1, 1, 1);
-			btVector3 color(0, 1, 0);
-			double width = 2;
-			m_guiHelper->getAppInterface()->m_renderer->drawLine(from, toX, color, width);
 			
-			m_guiHelper->getAppInterface()->drawText3D("hi", 1, 1, 1, 1);
+			for (int i=0;i<m_multiThreadedHelper->m_userDebugLines.size();i++)
+			{
+				btVector3 from;
+				from.setValue(	m_multiThreadedHelper->m_userDebugLines[i].m_debugLineFromXYZ[0],
+								m_multiThreadedHelper->m_userDebugLines[i].m_debugLineFromXYZ[1],
+								m_multiThreadedHelper->m_userDebugLines[i].m_debugLineFromXYZ[2]);
+				btVector3 toX;
+				toX.setValue(	m_multiThreadedHelper->m_userDebugLines[i].m_debugLineToXYZ[0],
+								m_multiThreadedHelper->m_userDebugLines[i].m_debugLineToXYZ[1],
+								m_multiThreadedHelper->m_userDebugLines[i].m_debugLineToXYZ[2]);
+				
+				btVector3 color;
+				color.setValue(	m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[0],
+								m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[1],
+								m_multiThreadedHelper->m_userDebugLines[i].m_debugLineColorRGB[2]);
+
+				
+				m_guiHelper->getAppInterface()->m_renderer->drawLine(from, toX, color, m_multiThreadedHelper->m_userDebugLines[i].m_lineWidth);
+			}
+			
+			for (int i=0;i<m_multiThreadedHelper->m_userDebugText.size();i++)
+			{
+				m_guiHelper->getAppInterface()->drawText3D(m_multiThreadedHelper->m_userDebugText[i].m_text,
+					m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ[0],
+					m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ[1],
+					m_multiThreadedHelper->m_userDebugText[i].m_textPositionXYZ[2],
+					m_multiThreadedHelper->m_userDebugText[i].textSize);
+				
+			}
 		}
 
 

examples/SharedMemory/SharedMemoryCommands.h

@@ -67,6 +67,11 @@ struct SdfArgs
     int m_useMultiBody;
 };
 
+struct FileArgs
+{
+	char m_fileName[MAX_URDF_FILENAME_LENGTH];
+};
+
 enum EnumUrdfArgsUpdateFlags
 {
 	URDF_ARGS_FILE_NAME=1,
@@ -495,6 +500,40 @@ struct CreateJointArgs
     int m_jointType;
 };
 
+
+
+enum EnumUserDebugDrawFlags
+{
+    USER_DEBUG_HAS_LINE=1,
+	USER_DEBUG_HAS_TEXT=2,
+	USER_DEBUG_REMOVE_ONE_ITEM=4,
+	USER_DEBUG_REMOVE_ALL=8	
+};
+
+struct UserDebugDrawArgs
+{
+	double	m_debugLineFromXYZ[3];
+	double	m_debugLineToXYZ[3];
+	double	m_debugLineColorRGB[3];
+	double	m_lineWidth;
+	
+	double m_lifeTime;
+	int m_removeItemUniqueId;
+
+	char m_text[MAX_FILENAME_LENGTH];
+	double m_textPositionXYZ[3];
+	double m_textColorRGB[3];
+	double m_textSize;
+};
+
+
+
+struct UserDebugDrawResultArgs
+{
+	int m_debugItemUniqueId;
+};
+
+
 struct SharedMemoryCommand
 {
 	int m_type;
@@ -509,6 +548,7 @@ struct SharedMemoryCommand
     {
         struct UrdfArgs m_urdfArguments;
 		struct SdfArgs m_sdfArguments;
+		struct FileArgs m_fileArguments;
 		struct SdfRequestInfoArgs m_sdfRequestInfoArgs;
 		struct InitPoseArgs m_initPoseArgs;
 		struct SendPhysicsSimulationParameters m_physSimParamArgs;
@@ -530,6 +570,7 @@ struct SharedMemoryCommand
         struct UpdateVisualShapeDataArgs m_updateVisualShapeDataArguments;
         struct LoadTextureArgs m_loadTextureArguments;
 		struct CalculateInverseKinematicsArgs m_calculateInverseKinematicsArguments;
+		struct UserDebugDrawArgs m_userDebugDrawArgs;
         struct LoadBunnyArgs m_loadBunnyArguments;
     };
 };
@@ -578,6 +619,7 @@ struct SharedMemoryStatus
 		struct SendOverlappingObjectsArgs m_sendOverlappingObjectsArgs;
 		struct CalculateInverseKinematicsResultArgs m_inverseKinematicsResultArgs;
 		struct SendVisualShapeDataArgs m_sendVisualShapeArgs;
+		struct UserDebugDrawResultArgs m_userDebugDrawArgs;
 	};
 };
 

examples/SharedMemory/SharedMemoryPublic.h

@@ -7,14 +7,15 @@ enum EnumSharedMemoryClientCommand
 {
     CMD_LOAD_SDF,
 	CMD_LOAD_URDF,
+	CMD_LOAD_BULLET,
+	CMD_SAVE_BULLET,
+	CMD_LOAD_MJCF,
     CMD_LOAD_BUNNY,
 	CMD_SEND_BULLET_DATA_STREAM,
 	CMD_CREATE_BOX_COLLISION_SHAPE,
-//      CMD_DELETE_BOX_COLLISION_SHAPE,
 	CMD_CREATE_RIGID_BODY,
 	CMD_DELETE_RIGID_BODY,
 	CMD_CREATE_SENSOR,///enable or disable joint feedback for force/torque sensors
-//    CMD_REQUEST_SENSOR_MEASUREMENTS,//see CMD_REQUEST_ACTUAL_STATE/CMD_ACTUAL_STATE_UPDATE_COMPLETED
 	CMD_INIT_POSE,
 	CMD_SEND_PHYSICS_SIMULATION_PARAMETERS,
 	CMD_SEND_DESIRED_STATE,//todo: reconsider naming, for example SET_JOINT_CONTROL_VARIABLE?
@@ -39,6 +40,8 @@ enum EnumSharedMemoryClientCommand
 	CMD_REQUEST_VISUAL_SHAPE_INFO,
     CMD_UPDATE_VISUAL_SHAPE,
     CMD_LOAD_TEXTURE,
+	CMD_USER_DEBUG_DRAW,
+
     //don't go beyond this command!
     CMD_MAX_CLIENT_COMMANDS,
     
@@ -56,6 +59,12 @@ enum EnumSharedMemoryServerStatus
         CMD_SDF_LOADING_FAILED,
         CMD_URDF_LOADING_COMPLETED,
         CMD_URDF_LOADING_FAILED,
+		CMD_BULLET_LOADING_COMPLETED,
+		CMD_BULLET_LOADING_FAILED,
+		CMD_BULLET_SAVING_COMPLETED,
+		CMD_BULLET_SAVING_FAILED,
+		CMD_MJCF_LOADING_COMPLETED,
+		CMD_MJCF_LOADING_FAILED,
 		CMD_REQUEST_INTERNAL_DATA_COMPLETED,
 		CMD_REQUEST_INTERNAL_DATA_FAILED,
         CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED,
@@ -93,6 +102,8 @@ enum EnumSharedMemoryServerStatus
         CMD_VISUAL_SHAPE_UPDATE_FAILED,
         CMD_LOAD_TEXTURE_COMPLETED,
         CMD_LOAD_TEXTURE_FAILED,
+		CMD_USER_DEBUG_DRAW_COMPLETED,
+		CMD_USER_DEBUG_DRAW_FAILED,
         //don't go beyond 'CMD_MAX_SERVER_COMMANDS!
         CMD_MAX_SERVER_COMMANDS
 };
@@ -232,6 +243,7 @@ struct b3VisualShapeData
 	char m_meshAssetFileName[VISUAL_SHAPE_MAX_PATH_LEN];
 	double m_localInertiaFrame[7];//pos[3], orn[4]
 	//todo: add more data if necessary (material color etc, although material can be in asset file .obj file)
+    double m_rgbaColor[4];
 };
 
 struct b3VisualShapeInformation

examples/SharedMemory/TinyRendererVisualShapeConverter.cpp

@@ -527,6 +527,10 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const
 			visualShape.m_localInertiaFrame[4] = localInertiaFrame.getRotation()[1];
 			visualShape.m_localInertiaFrame[5] = localInertiaFrame.getRotation()[2];
 			visualShape.m_localInertiaFrame[6] = localInertiaFrame.getRotation()[3];
+            visualShape.m_rgbaColor[0] = rgbaColor[0];
+            visualShape.m_rgbaColor[1] = rgbaColor[1];
+            visualShape.m_rgbaColor[2] = rgbaColor[2];
+            visualShape.m_rgbaColor[3] = rgbaColor[3];
             
 			convertURDFToVisualShape(&vis, pathPrefix, localInertiaFrame.inverse()*childTrans, vertices, indices,textures, visualShape);
 			m_data->m_visualShapes.push_back(visualShape);
@@ -872,10 +876,14 @@ int TinyRendererVisualShapeConverter::registerTexture(unsigned char* texels, int
     return m_data->m_textures.size()-1;
 }
 
-void TinyRendererVisualShapeConverter::loadTextureFile(const char* filename)
+int TinyRendererVisualShapeConverter::loadTextureFile(const char* filename)
 {
     int width,height,n;
     unsigned char* image=0;
     image = stbi_load(filename, &width, &height, &n, 3);
-    registerTexture(image, width, height);
+    if (image && (width>=0) && (height>=0))
+    {
+        return registerTexture(image, width, height);
+    }
+    return -1;
 }

examples/SharedMemory/TinyRendererVisualShapeConverter.h

@@ -38,7 +38,7 @@ struct TinyRendererVisualShapeConverter : public LinkVisualShapesConverter
 	void render();
 	void render(const float viewMat[16], const float projMat[16]);
     
-    void loadTextureFile(const char* filename);
+    int loadTextureFile(const char* filename);
     int registerTexture(unsigned char* texels, int width, int height);
     void activateShapeTexture(int shapeUniqueId, int textureUniqueId);
     void activateShapeTexture(int objectUniqueId, int jointIndex, int shapeIndex, int textureUniqueId);

examples/SharedMemory/udp/main.cpp

@@ -95,6 +95,7 @@ int main(int argc, char *argv[])
 					switch (event.type)
 					{
                         case ENET_EVENT_TYPE_CONNECT:
+                        {
 						printf("A new client connected from %x:%u.\n",
 							event.peer->address.host,
 							event.peer->address.port);
@@ -103,8 +104,9 @@ int main(int argc, char *argv[])
 						event.peer->data = (void*)"Client information";
 
 						break;
-
+                        }
 					case ENET_EVENT_TYPE_RECEIVE:
+                        {
 						if (gVerboseNetworkMessagesServer)
 						{
 							printf("A packet of length %u containing '%s' was "
@@ -171,8 +173,9 @@ int main(int argc, char *argv[])
 						//enet_host_broadcast(server, 0, event.packet);
 
 						break;
-
+                        }
 					case ENET_EVENT_TYPE_DISCONNECT:
+                        {
 						printf("%s disconnected.\n", event.peer->data);
 
 						/* Reset the peer's client information. */
@@ -181,6 +184,11 @@ int main(int argc, char *argv[])
 
 						break;
                         }
+                    default:
+                        {
+                        
+                        }
+					}
 				}
 				else if (serviceResult > 0)
 				{

examples/SoftDemo/SoftDemo.cpp

@@ -2131,12 +2131,12 @@ void	SoftDemo::initPhysics()
 			for (int j=0;j<NUM_VERTS_Y-1;j++)
 			{
 				gGroundIndices[index++] = j*NUM_VERTS_X+i;
-				gGroundIndices[index++] = j*NUM_VERTS_X+i+1;
 				gGroundIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
+				gGroundIndices[index++] = j*NUM_VERTS_X+i+1;;
 
 				gGroundIndices[index++] = j*NUM_VERTS_X+i;
-				gGroundIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
 				gGroundIndices[index++] = (j+1)*NUM_VERTS_X+i;
+				gGroundIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
 			}
 		}
 

examples/StandaloneMain/main_tinyrenderer_single_example.cpp

@@ -379,6 +379,20 @@ struct TinyRendererGUIHelper : public GUIHelperInterface
     
 	virtual void drawText3D( const char* txt, float posX, float posZY, float posZ, float size)
 	{
+	}
+		virtual int		addUserDebugText3D( const char* txt, const double positionXYZ[3], const double	textColorRGB[3], double size, double lifeTime)
+	{
+		return -1;
+	}
+	virtual int		addUserDebugLine(const double	debugLineFromXYZ[3], const double	debugLineToXYZ[3], const double	debugLineColorRGB[3], double lineWidth, double lifeTime )
+	{
+		return -1;
+	}
+	virtual void	removeUserDebugItem( int debugItemUniqueId)
+	{
+	}
+	virtual void	removeAllUserDebugItems( )
+	{
 	}
 };
 

examples/pybullet/pybullet.c

@@ -197,12 +197,112 @@ static PyObject* pybullet_saveWorld(PyObject* self, PyObject* args) {
 	PyErr_SetString(SpamError, "Cannot execute saveWorld command.");
 	return NULL;
     
+}
+
+#define MAX_SDF_BODIES 512
+static PyObject* pybullet_loadBullet(PyObject* self, PyObject* args)
+{
+	int size = PySequence_Size(args);
+	const char* bulletFileName = "";
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType;
+	b3SharedMemoryCommandHandle command;
+	int i,numBodies;
+	int bodyIndicesOut[MAX_SDF_BODIES];
+	  PyObject* pylist = 0;
+	if (0 == sm) {
+		PyErr_SetString(SpamError, "Not connected to physics server.");
 		return NULL;
 }
+	if (size == 1) {
+		if (!PyArg_ParseTuple(args, "s", &bulletFileName)) return NULL;
+	}
+
+	command = b3LoadBulletCommandInit(sm, bulletFileName);
+	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+	statusType = b3GetStatusType(statusHandle);
+	if (statusType != CMD_BULLET_LOADING_COMPLETED)
+	{
+		PyErr_SetString(SpamError, "Couldn't load .bullet file.");
+		return NULL;
+	}
+
+	numBodies =
+      b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, MAX_SDF_BODIES);
+  if (numBodies > MAX_SDF_BODIES) {
+    PyErr_SetString(SpamError, "loadBullet exceeds body capacity");
+    return NULL;
+  }
+
+  pylist = PyTuple_New(numBodies);
+
+  if (numBodies > 0 && numBodies <= MAX_SDF_BODIES) {
+    for (i = 0; i < numBodies; i++) {
+      PyTuple_SetItem(pylist, i, PyInt_FromLong(bodyIndicesOut[i]));
+    }
+  }
+  return pylist;
+
+	
+}
+
+static PyObject* pybullet_saveBullet(PyObject* self, PyObject* args)
+{
+	int size = PySequence_Size(args);
+	const char* bulletFileName = "";
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType;
+	b3SharedMemoryCommandHandle command;
+
+	if (0 == sm) {
+		PyErr_SetString(SpamError, "Not connected to physics server.");
+		return NULL;
+	}
+	if (size == 1) {
+		if (!PyArg_ParseTuple(args, "s", &bulletFileName)) return NULL;
+	}
+	command = b3SaveBulletCommandInit(sm, bulletFileName);
+	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+	if (statusHandle != CMD_BULLET_SAVING_COMPLETED)
+	{
+		PyErr_SetString(SpamError, "Couldn't save .bullet file.");
+		return NULL;
+	}
+	Py_INCREF(Py_None);
+	return Py_None;
+}
 
 
 
-// Load a URDF file indicating the links and joints of an object
+static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args)
+{
+	int size = PySequence_Size(args);
+	const char* mjcfjFileName = "";
+	b3SharedMemoryStatusHandle statusHandle;
+	int statusType;
+	b3SharedMemoryCommandHandle command;
+
+	if (0 == sm) {
+		PyErr_SetString(SpamError, "Not connected to physics server.");
+		return NULL;
+	}
+	if (size == 1) {
+		if (!PyArg_ParseTuple(args, "s", &mjcfjFileName)) return NULL;
+	}
+	command = b3LoadMJCFCommandInit(sm, mjcfjFileName);
+	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
+	if (statusHandle != CMD_MJCF_LOADING_COMPLETED)
+	{
+		PyErr_SetString(SpamError, "Couldn't load .mjcf file.");
+		return NULL;
+	}
+	Py_INCREF(Py_None);
+	return Py_None;
+}
+
+
+
+// Load a robot from a URDF file (universal robot description format)
 // function can be called without arguments and will default
 // to position (0,0,1) with orientation(0,0,0,1)
 // els(x,y,z) or
@@ -283,7 +383,7 @@ static double pybullet_internalGetFloatFromSequence(PyObject* seq, int index) {
   return v;
 }
 
-#define MAX_SDF_BODIES 512
+
 
 static PyObject* pybullet_loadSDF(PyObject* self, PyObject* args) {
   const char* sdfFileName = "";
@@ -1293,12 +1393,14 @@ static int pybullet_internalSetMatrix(PyObject* objMat, float matrix[16]) {
 //
 // // Args:
 //  vector - float[3] which will be set by values from objMat
-static int pybullet_internalSetVector(PyObject* objMat, float vector[3]) {
+static int pybullet_internalSetVector(PyObject* objVec, float vector[3]) {
   int i, len;
   PyObject* seq;
+  if (objVec==NULL)
+		return 0;
 
-  seq = PySequence_Fast(objMat, "expected a sequence");
+  seq = PySequence_Fast(objVec, "expected a sequence");
-  len = PySequence_Size(objMat);
+  len = PySequence_Size(objVec);
   if (len == 3) {
     for (i = 0; i < len; i++) {
       vector[i] = pybullet_internalGetFloatFromSequence(seq, i);
@@ -1314,6 +1416,8 @@ static int pybullet_internalSetVector(PyObject* objMat, float vector[3]) {
 static int pybullet_internalSetVectord(PyObject* obVec, double vector[3]) {
     int i, len;
     PyObject* seq;
+    if (obVec==NULL)
+		return 0;
 
     seq = PySequence_Fast(obVec, "expected a sequence");
     len = PySequence_Size(obVec);
@@ -1332,6 +1436,8 @@ static int pybullet_internalSetVectord(PyObject* obVec, double vector[3]) {
 static int pybullet_internalSetVector4(PyObject* obVec, double vector[4]) {
     int i, len;
     PyObject* seq;
+    if (obVec==NULL)
+		return 0;
 
     seq = PySequence_Fast(obVec, "expected a sequence");
     len = PySequence_Size(obVec);
@@ -1346,6 +1452,196 @@ static int pybullet_internalSetVector4(PyObject* obVec, double vector[4]) {
     return 0;
 }
 
+
+
+static PyObject* pybullet_addUserDebugText(PyObject* self, PyObject* args, PyObject *keywds) 
+{
+  int size = PySequence_Size(args);
+  
+  b3SharedMemoryCommandHandle commandHandle;
+  struct b3ContactInformation contactPointData;
+  b3SharedMemoryStatusHandle statusHandle;
+  int statusType;
+  int res = 0;
+
+  PyObject* pyResultList = 0;
+	char* text;
+  double posXYZ[3];
+  double colorRGB[3]={1,1,1};
+
+  
+  PyObject* textPositionObj=0;
+  PyObject* textColorRGBObj=0;
+  double textSize = 1.f;
+  double lifeTime = 0.f;
+  
+  static char *kwlist[] = { "text", "textPosition", "textColorRGB", "textSize", "lifeTime", NULL };
+
+  if (0 == sm) {
+	  PyErr_SetString(SpamError, "Not connected to physics server.");
+	  return NULL;
+  }
+
+  if (!PyArg_ParseTupleAndKeywords(args, keywds, "sO|Odd", kwlist, &text,  &textPositionObj,  &textColorRGBObj,&textSize, &lifeTime))
+  {
+	  return NULL;
+  }
+
+  res = pybullet_internalSetVectord(textPositionObj,posXYZ);
+  if (!res)
+  {
+	  PyErr_SetString(SpamError, "Error converting lineFrom[3]");
+	  return NULL;
+  }
+
+  if (textColorRGBObj)
+  {
+	  res = pybullet_internalSetVectord(textColorRGBObj,colorRGB);
+	  if (!res)
+	  {
+		  PyErr_SetString(SpamError, "Error converting lineTo[3]");
+		  return NULL;
+	  }
+  }
+
+  
+   commandHandle = b3InitUserDebugDrawAddText3D(sm,text,posXYZ,colorRGB,textSize,lifeTime);
+
+  statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
+  statusType = b3GetStatusType(statusHandle);
+  if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED) 
+  {
+    int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle);
+	PyObject* item = PyInt_FromLong(debugItemUniqueId);
+	return item;
+  }
+
+  PyErr_SetString(SpamError, "Error in addUserDebugText.");
+	  return NULL;
+}
+
+
+static PyObject* pybullet_addUserDebugLine(PyObject* self, PyObject* args, PyObject *keywds) 
+{
+  int size = PySequence_Size(args);
+  
+  b3SharedMemoryCommandHandle commandHandle;
+  struct b3ContactInformation contactPointData;
+  b3SharedMemoryStatusHandle statusHandle;
+  int statusType;
+  int res = 0;
+
+  PyObject* pyResultList = 0;
+
+  double fromXYZ[3];
+  double toXYZ[3];
+  double colorRGB[3]={1,1,1};
+
+  PyObject* lineFromObj=0;
+  PyObject* lineToObj=0;
+  PyObject* lineColorRGBObj=0;
+  double lineWidth = 1.f;
+  double lifeTime = 0.f;
+  
+  static char *kwlist[] = { "lineFromXYZ", "lineToXYZ", "lineColorRGB", "lineWidth", "lifeTime", NULL };
+
+  if (0 == sm) {
+	  PyErr_SetString(SpamError, "Not connected to physics server.");
+	  return NULL;
+  }
+
+  if (!PyArg_ParseTupleAndKeywords(args, keywds, "OO|Odd", kwlist, &lineFromObj,  &lineToObj,  &lineColorRGBObj,&lineWidth, &lifeTime))
+  {
+	  return NULL;
+  }
+
+  res = pybullet_internalSetVectord(lineFromObj,fromXYZ);
+  if (!res)
+  {
+	  PyErr_SetString(SpamError, "Error converting lineFrom[3]");
+	  return NULL;
+  }
+
+  res = pybullet_internalSetVectord(lineToObj,toXYZ);
+  if (!res)
+  {
+	  PyErr_SetString(SpamError, "Error converting lineTo[3]");
+	  return NULL;
+  }
+  if (lineColorRGBObj)
+  {
+	  res = pybullet_internalSetVectord(lineColorRGBObj,colorRGB);
+  }
+  
+   commandHandle = b3InitUserDebugDrawAddLine3D(sm,fromXYZ,toXYZ,colorRGB,lineWidth,lifeTime);
+
+  statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
+  statusType = b3GetStatusType(statusHandle);
+  if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED) 
+  {
+    int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle);
+	PyObject* item = PyInt_FromLong(debugItemUniqueId);
+	return item;
+  }
+
+  PyErr_SetString(SpamError, "Error in addUserDebugLine.");
+	  return NULL;
+}
+
+
+
+
+static PyObject* pybullet_removeUserDebugItem(PyObject* self, PyObject* args, PyObject *keywds) 
+{
+	 b3SharedMemoryCommandHandle commandHandle;
+  struct b3ContactInformation contactPointData;
+  b3SharedMemoryStatusHandle statusHandle;
+  int statusType;
+  int itemUniqueId;
+
+	if (0 == sm) {
+	  PyErr_SetString(SpamError, "Not connected to physics server.");
+	  return NULL;
+  }
+
+	if (!PyArg_ParseTuple(args, "i", &itemUniqueId)) {
+			PyErr_SetString(SpamError, "Error parsing user debug item unique id");
+			return NULL;
+		}
+
+	commandHandle = b3InitUserDebugDrawRemove(sm,itemUniqueId);
+
+	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
+	statusType = b3GetStatusType(statusHandle);
+
+	Py_INCREF(Py_None);
+	return Py_None;
+}
+
+static PyObject* pybullet_removeAllUserDebugItems(PyObject* self, PyObject* args, PyObject *keywds) 
+{
+	 b3SharedMemoryCommandHandle commandHandle;
+  struct b3ContactInformation contactPointData;
+  b3SharedMemoryStatusHandle statusHandle;
+  int statusType;
+
+	if (0 == sm) {
+	  PyErr_SetString(SpamError, "Not connected to physics server.");
+	  return NULL;
+  }
+
+	commandHandle = b3InitUserDebugDrawRemoveAll(sm);
+
+	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
+	statusType = b3GetStatusType(statusHandle);
+	  
+
+	Py_INCREF(Py_None);
+	return Py_None;
+}
+	
+
+
 static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args) 
 {
 	int size = PySequence_Size(args);
@@ -1426,7 +1722,7 @@ static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args)
 					PyTuple_SetItem(visualShapeObList, 6, vec);
 				}
 
-				
+		visualShapeInfo.m_visualShapeData[0].m_rgbaColor[0];	
 
 				PyTuple_SetItem(pyResultList, i, visualShapeObList);
 			}
@@ -2515,8 +2811,17 @@ static PyMethodDef SpamMethods[] = {
     {"loadSDF", pybullet_loadSDF, METH_VARARGS,
      "Load multibodies from an SDF file."},
 
+	 { "loadBullet", pybullet_loadBullet, METH_VARARGS,
+	"Restore the full state of the world from a .bullet file." },
+
+	{ "saveBullet", pybullet_saveBullet, METH_VARARGS,
+	"Save the full state of the world to a .bullet file." },
+
+	 { "loadMJCF", pybullet_loadMJCF, METH_VARARGS,
+	"Load multibodies from an MJCF file." },
+
 	{"saveWorld", pybullet_saveWorld, METH_VARARGS,
-     "Save an approximate Python file to reproduce the current state of the world: saveWorld"
+     "Save a approximate Python file to reproduce the current state of the world: saveWorld"
 	"(filename). (very preliminary and approximately)"},
 
 	{"getNumBodies", pybullet_getNumBodies, METH_VARARGS,
@@ -2595,6 +2900,30 @@ static PyMethodDef SpamMethods[] = {
 		"axis-aligned bounding box volume (AABB)."
 		"Input are two vectors defining the AABB in world space [min_x,min_y,min_z],[max_x,max_y,max_z]."
 	 },
+
+
+	 { "addUserDebugLine", (PyCFunction)pybullet_addUserDebugLine, METH_VARARGS | METH_KEYWORDS,
+	 "Add a user debug draw line with lineFrom[3], lineTo[3], lineColorRGB[3], lineWidth, lifeTime. "
+	  "A lifeTime of 0 means permanent until removed. Returns a unique id for the user debug item."
+	 },
+
+
+	 { "addUserDebugText", (PyCFunction)pybullet_addUserDebugText, METH_VARARGS | METH_KEYWORDS,
+	 "Add a user debug draw line with text, textPosition[3], textSize and lifeTime in seconds "
+	 "A lifeTime of 0 means permanent until removed. Returns a unique id for the user debug item."
+	  },
+
+	 { "removeUserDebugItem", (PyCFunction)pybullet_removeUserDebugItem, METH_VARARGS | METH_KEYWORDS,
+	 "remove a user debug draw item, giving its unique id"
+	  },
+
+
+	 { "removeAllUserDebugItems", (PyCFunction)pybullet_removeAllUserDebugItems, METH_VARARGS | METH_KEYWORDS,
+	 "remove all user debug draw items"
+	  },
+
+
+
     {"getVisualShapeData", pybullet_getVisualShapeData, METH_VARARGS,
 	"Return the visual shape information for one object." },