add Obj and STL import demo, work on URDF import

Demos/CommonPhysicsSetup.h

@@ -33,6 +33,8 @@ struct GraphicsPhysicsBridge
 		return 0;
 	}
 	
+	virtual void setUpAxis(int axis)=0;
+	
 };
 
 struct CommonPhysicsSetup

Demos/ConstraintDemo/ConstraintPhysicsSetup.cpp

@@ -22,6 +22,8 @@ void ConstraintPhysicsSetup::stepSimulation(float deltaTime)
 
 void ConstraintPhysicsSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
 {
+	gfxBridge.setUpAxis(1);
+	
 	createEmptyDynamicsWorld();
 
 	gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld);

Demos3/AllBullet2Demos/BulletDemoEntries.h

@@ -16,6 +16,8 @@
 #include "../../Demos/CcdPhysicsDemo/CcdPhysicsSetup.h"
 #include "../../Demos/ConstraintDemo/ConstraintPhysicsSetup.h"
 #include "../ImportURDFDemo/ImportURDFSetup.h"
+#include "../ImportObjDemo/ImportObjSetup.h"
+#include "../ImportSTLDemo/ImportSTLSetup.h"
 
 
 static BulletDemoInterface* MyCcdPhysicsDemoCreateFunc(SimpleOpenGL3App* app)
@@ -41,6 +43,17 @@ static BulletDemoInterface* MyImportUrdfCreateFunc(SimpleOpenGL3App* app)
     CommonPhysicsSetup* physicsSetup = new ImportUrdfDemo();
 	return new BasicDemo(app, physicsSetup);
 }
+static BulletDemoInterface* MyImportObjCreateFunc(SimpleOpenGL3App* app)
+{
+    CommonPhysicsSetup* physicsSetup = new ImportObjDemo(app);
+	return new BasicDemo(app, physicsSetup);
+}
+static BulletDemoInterface* MyImportSTLCreateFunc(SimpleOpenGL3App* app)
+{
+    CommonPhysicsSetup* physicsSetup = new ImportSTLDemo(app);
+	return new BasicDemo(app, physicsSetup);
+}
+
 
 struct BulletDemoEntry
 {
@@ -60,7 +73,11 @@ static BulletDemoEntry allDemos[]=
 	{ 1, "CcdDemo", MyCcdPhysicsDemoCreateFunc },
 	{ 1, "Kinematic", MyKinematicObjectCreateFunc },
 	{ 1, "Constraints", MyConstraintCreateFunc },
+	{0,"File Formats", 0},
+//@todo(erwincoumans)	{ 1, "bullet", MyImportSTLCreateFunc},
+	{ 1, "Wavefront Obj", MyImportObjCreateFunc},
     { 1, "URDF", MyImportUrdfCreateFunc },
+	{ 1, "STL", MyImportSTLCreateFunc},
     
 /*	{1,"ChainDemo",ChainDemo::MyCreateFunc},
 //	{0, "Stress tests", 0 },

Demos3/AllBullet2Demos/main.cpp

@@ -331,8 +331,8 @@ int main(int argc, char* argv[])
 		assert(err==GL_NO_ERROR);
 		app->m_instancingRenderer->init();
         DrawGridData dg;
-//        dg.upAxis = 2;
+        dg.upAxis = app->getUpAxis();
-        
+		
 		app->m_instancingRenderer->updateCamera(dg.upAxis);
         app->drawGrid(dg);
 

Demos3/AllBullet2Demos/premake4.lua

@@ -30,6 +30,10 @@
 		"../../Demos/ConstraintDemo/ConstraintPhysicsSetup.cpp",
   		"../../Demos/ConstraintDemo/ConstraintPhysicsSetup.h",
 		"../ImportURDFDemo/ImportURDFSetup.cpp",
+		"../ImportObjDemo/ImportObjSetup.cpp",
+	 	"../ImportSTLDemo/ImportSTLSetup.cpp",	
+		"../Wavefront/tiny_obj_loader.cpp",
+		"../Wavefront/tiny_obj_loader.h",	
 		"../../btgui/urdf/urdfdom/urdf_parser/src/pose.cpp",
 		"../../btgui/urdf/urdfdom/urdf_parser/src/model.cpp",
                 "../../btgui/urdf/urdfdom/urdf_parser/src/link.cpp",

Demos3/ImportObjDemo/ImportObjSetup.cpp

@@ -0,0 +1,208 @@
+#include "ImportObjSetup.h"
+#include <vector>
+#include "OpenGLWindow/GLInstancingRenderer.h"
+#include"../Wavefront/tiny_obj_loader.h"
+#include "OpenGLWindow/GLInstanceGraphicsShape.h"
+#include "btBulletDynamicsCommon.h"
+#include "OpenGLWindow/SimpleOpenGL3App.h"
+
+ImportObjDemo::ImportObjDemo(SimpleOpenGL3App* app)
+:m_app(app)
+{
+    
+}
+
+ImportObjDemo::~ImportObjDemo()
+{
+    
+}
+
+
+
+static GLInstanceGraphicsShape* gCreateGraphicsShapeFromWavefrontObj(std::vector<tinyobj::shape_t>& shapes)
+{
+	
+	b3AlignedObjectArray<GLInstanceVertex>* vertices = new b3AlignedObjectArray<GLInstanceVertex>;
+	{
+		//		int numVertices = obj->vertexCount;
+		//	int numIndices = 0;
+		b3AlignedObjectArray<int>* indicesPtr = new b3AlignedObjectArray<int>;
+		
+		for (int s=0;s<shapes.size();s++)
+		{
+			tinyobj::shape_t& shape = shapes[s];
+			int faceCount = shape.mesh.indices.size();
+			
+			
+			for (int f=0;f<faceCount;f+=3)
+			{
+				
+				//btVector3 normal(face.m_plane[0],face.m_plane[1],face.m_plane[2]);
+				if (1)
+				{
+					btVector3 normal(0,1,0);
+					int vtxBaseIndex = vertices->size();
+					
+					
+					indicesPtr->push_back(vtxBaseIndex);
+					indicesPtr->push_back(vtxBaseIndex+1);
+					indicesPtr->push_back(vtxBaseIndex+2);
+					
+					GLInstanceVertex vtx0;
+					vtx0.xyzw[0] = shape.mesh.positions[shape.mesh.indices[f]*3+0];
+					vtx0.xyzw[1] = shape.mesh.positions[shape.mesh.indices[f]*3+1];
+					vtx0.xyzw[2] = shape.mesh.positions[shape.mesh.indices[f]*3+2];
+					vtx0.xyzw[3] = 0.f;
+					
+					vtx0.uv[0] = 0.5f;//shape.mesh.positions[shape.mesh.indices[f]*3+2];?
+					vtx0.uv[1] = 0.5f;
+					
+					GLInstanceVertex vtx1;
+					vtx1.xyzw[0] = shape.mesh.positions[shape.mesh.indices[f+1]*3+0];
+					vtx1.xyzw[1] = shape.mesh.positions[shape.mesh.indices[f+1]*3+1];
+					vtx1.xyzw[2] = shape.mesh.positions[shape.mesh.indices[f+1]*3+2];
+					vtx1.xyzw[3]= 0.f;
+					vtx1.uv[0] = 0.5f;//obj->textureList[face->vertex_index[1]]->e[0];
+					vtx1.uv[1] = 0.5f;//obj->textureList[face->vertex_index[1]]->e[1];
+					
+					GLInstanceVertex vtx2;
+					vtx2.xyzw[0] = shape.mesh.positions[shape.mesh.indices[f+2]*3+0];
+					vtx2.xyzw[1] = shape.mesh.positions[shape.mesh.indices[f+2]*3+1];
+					vtx2.xyzw[2] = shape.mesh.positions[shape.mesh.indices[f+2]*3+2];
+					vtx2.xyzw[3] = 0.f;
+					vtx2.uv[0] = 0.5f;
+					vtx2.uv[1] = 0.5f;
+					
+					
+					btVector3 v0(vtx0.xyzw[0],vtx0.xyzw[1],vtx0.xyzw[2]);
+					btVector3 v1(vtx1.xyzw[0],vtx1.xyzw[1],vtx1.xyzw[2]);
+					btVector3 v2(vtx2.xyzw[0],vtx2.xyzw[1],vtx2.xyzw[2]);
+					
+					normal = (v1-v0).cross(v2-v0);
+					normal.normalize();
+					vtx0.normal[0] = normal[0];
+					vtx0.normal[1] = normal[1];
+					vtx0.normal[2] = normal[2];
+					vtx1.normal[0] = normal[0];
+					vtx1.normal[1] = normal[1];
+					vtx1.normal[2] = normal[2];
+					vtx2.normal[0] = normal[0];
+					vtx2.normal[1] = normal[1];
+					vtx2.normal[2] = normal[2];
+					vertices->push_back(vtx0);
+					vertices->push_back(vtx1);
+					vertices->push_back(vtx2);
+				}
+			}
+		}
+		
+		
+		GLInstanceGraphicsShape* gfxShape = new GLInstanceGraphicsShape;
+		gfxShape->m_vertices = vertices;
+		gfxShape->m_numvertices = vertices->size();
+		gfxShape->m_indices = indicesPtr;
+		gfxShape->m_numIndices = indicesPtr->size();
+		for (int i=0;i<4;i++)
+			gfxShape->m_scaling[i] = 1;//bake the scaling into the vertices
+		return gfxShape;
+	}
+}
+
+
+
+
+void ImportObjDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge)
+{
+	this->createEmptyDynamicsWorld();
+	gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld);
+	m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe);
+	const char* fileName = "samurai_monastry.obj";
+    char relativeFileName[1024];
+	const char* prefix[]={"./data/","../data/","../../data/","../../../data/","../../../../data/"};
+	int prefixIndex=-1;
+	{
+		
+		int numPrefixes = sizeof(prefix)/sizeof(char*);
+		
+		for (int i=0;i<numPrefixes;i++)
+		{
+			FILE* f = 0;
+			sprintf(relativeFileName,"%s%s",prefix[i],fileName);
+			f = fopen(relativeFileName,"r");
+			if (f)
+			{
+				fclose(f);
+				prefixIndex = i;
+				break;
+			}
+		}
+	}
+	
+	if (prefixIndex<0)
+		return;
+	
+	btVector3 shift(0,0,0);
+	btVector3 scaling(1,1,1);
+	int index=10;
+	
+	{
+		
+		std::vector<tinyobj::shape_t> shapes;
+		std::string err = tinyobj::LoadObj(shapes, relativeFileName, prefix[prefixIndex]);
+		
+		GLInstanceGraphicsShape* gfxShape = gCreateGraphicsShapeFromWavefrontObj(shapes);
+		
+		btTransform trans;
+		trans.setIdentity();
+		trans.setRotation(btQuaternion(btVector3(1,0,0),SIMD_HALF_PI));
+		
+		btVector3 position = trans.getOrigin();
+		btQuaternion orn = trans.getRotation();
+		
+		btVector3 color(0,0,1);
+		
+		int shapeId = m_app->m_instancingRenderer->registerShape(&gfxShape->m_vertices->at(0).xyzw[0], gfxShape->m_numvertices, &gfxShape->m_indices->at(0), gfxShape->m_numIndices);
+		
+		
+		int id = m_app->m_instancingRenderer->registerGraphicsInstance(shapeId,position,orn,color,scaling);
+
+		/*
+
+		btTriangleMesh* trimeshData = new btTriangleMesh();
+
+		for (int i=0;i<gfxShape->m_numvertices;i++)
+		{
+			for (int j=0;j<3;j++)
+				gfxShape->m_vertices->at(i).xyzw[j] += shift[j];
+		}
+
+		for (int i=0;i<gfxShape->m_numIndices;i+=3)
+		{
+			int index0 = gfxShape->m_indices->at(i);
+			int index1 = gfxShape->m_indices->at(i+1);
+			int index2 = gfxShape->m_indices->at(i+2);
+			
+			btVector3 v0(gfxShape->m_vertices->at(index0).xyzw[0],
+										gfxShape->m_vertices->at(index0).xyzw[1],
+										gfxShape->m_vertices->at(index0).xyzw[2]);
+			btVector3 v1(gfxShape->m_vertices->at(index1).xyzw[0],
+						 gfxShape->m_vertices->at(index1).xyzw[1],
+						 gfxShape->m_vertices->at(index1).xyzw[2]);
+			btVector3 v2(gfxShape->m_vertices->at(index2).xyzw[0],
+						 gfxShape->m_vertices->at(index2).xyzw[1],
+						 gfxShape->m_vertices->at(index2).xyzw[2]);
+			
+			trimeshData->addTriangle(v0,v1,v2);
+		}
+		
+		//btConvexHullShape* convexShape = new btConvexHullShape(&verts[0].x(),verts.size(),sizeof(btVector3));
+		btBvhTriangleMeshShape* shape = new btBvhTriangleMeshShape(trimeshData,true);//meshInterface);
+		
+		btTransform startTrans;startTrans.setIdentity();
+		btRigidBody* body = this->createRigidBody(0,startTrans,shape);
+		//gfxBridge.createCollisionShapeGraphicsObject(shape);
+		btVector3 color(0,0,1);
+		 */
+		//gfxBridge.createRigidBodyGraphicsObject(body,color);
+	}
+}

Demos3/ImportObjDemo/ImportObjSetup.h

@@ -0,0 +1,17 @@
+#ifndef IMPORT_OBJ_SETUP_H
+#define IMPORT_OBJ_SETUP_H
+
+
+#include "../../Demos/CommonRigidBodySetup.h"
+
+class ImportObjDemo : public CommonRigidBodySetup
+{
+	struct SimpleOpenGL3App* m_app;
+public:
+    ImportObjDemo(SimpleOpenGL3App* app);
+    virtual ~ImportObjDemo();
+    
+	virtual void initPhysics(GraphicsPhysicsBridge& gfxBridge);
+};
+
+#endif //IMPORT_OBJ_SETUP_H

Demos3/ImportSTLDemo/ImportSTLSetup.cpp

@@ -0,0 +1,204 @@
+#include "ImportSTLSetup.h"
+#include <vector>
+#include "OpenGLWindow/GLInstancingRenderer.h"
+#include "OpenGLWindow/GLInstanceGraphicsShape.h"
+#include "btBulletDynamicsCommon.h"
+#include "OpenGLWindow/SimpleOpenGL3App.h"
+
+ImportSTLDemo::ImportSTLDemo(SimpleOpenGL3App* app)
+:m_app(app)
+{
+    
+}
+
+ImportSTLDemo::~ImportSTLDemo()
+{
+    
+}
+
+struct MySTLTriangle
+{
+	float normal[3];
+	float vertex0[3];
+	float vertex1[3];
+	float vertex2[3];
+};
+
+GLInstanceGraphicsShape* LoadMeshFromSTL(const char* relativeFileName)
+{
+	GLInstanceGraphicsShape* shape = 0;
+	
+	FILE* file = fopen(relativeFileName,"rb");
+	if (file)
+	{
+		int size=0;
+		if (fseek(file, 0, SEEK_END) || (size = ftell(file)) == EOF || fseek(file, 0, SEEK_SET))
+		{
+			printf("Error: Cannot access file to determine size of %s\n", relativeFileName);
+		} else
+		{
+			if (size)
+			{
+				printf("Open STL file of %d bytes\n",size);
+				char* memoryBuffer = new char[size+1];
+				int actualBytesRead = fread(memoryBuffer,1,size,file);
+				if (actualBytesRead!=size)
+				{
+					printf("Error reading from file %s",relativeFileName);
+				} else
+				{
+					int numTriangles = *(int*)&memoryBuffer[80];
+					
+					if (numTriangles)
+					{
+						
+						shape = new GLInstanceGraphicsShape;
+//						b3AlignedObjectArray<GLInstanceVertex>*	m_vertices;
+//						int				m_numvertices;
+//						b3AlignedObjectArray<int>* 		m_indices;
+//						int				m_numIndices;
+//						float			m_scaling[4];
+						shape->m_scaling[0] = 1;
+						shape->m_scaling[1] = 1;
+						shape->m_scaling[2] = 1;
+						shape->m_scaling[3] = 1;
+						int index = 0;
+						shape->m_indices = new b3AlignedObjectArray<int>();
+						shape->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
+						for (int i=0;i<numTriangles;i++)
+						{
+							char* curPtr = &memoryBuffer[84+i*50];
+							MySTLTriangle* tri = (MySTLTriangle*) curPtr;
+							
+							GLInstanceVertex v0,v1,v2;
+							if (i==numTriangles-2)
+							{
+								printf("!\n");
+							}
+							v0.uv[0] = v1.uv[0] = v2.uv[0] = 0.5;
+							v0.uv[1] = v1.uv[1] = v2.uv[1] = 0.5;
+							for (int v=0;v<3;v++)
+							{
+								v0.xyzw[v] = tri->vertex0[v];
+								v1.xyzw[v] = tri->vertex1[v];
+								v2.xyzw[v] = tri->vertex2[v];
+								v0.normal[v] = v1.normal[v] = v2.normal[v] = tri->normal[v];
+							}
+							v0.xyzw[3] = v1.xyzw[3] = v2.xyzw[3] = 0.f;
+							
+							shape->m_vertices->push_back(v0);
+							shape->m_vertices->push_back(v1);
+							shape->m_vertices->push_back(v2);
+							
+							shape->m_indices->push_back(index++);
+							shape->m_indices->push_back(index++);
+							shape->m_indices->push_back(index++);
+							
+						}
+					}
+				}
+				
+				delete[] memoryBuffer;
+			}
+		}
+		fclose(file);
+	}
+	shape->m_numIndices = shape->m_indices->size();
+	shape->m_numvertices = shape->m_vertices->size();
+	return shape;
+}
+
+
+void ImportSTLDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge)
+{
+	this->createEmptyDynamicsWorld();
+	gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld);
+	m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe);
+
+	const char* fileName = "l_finger_tip.stl";
+    char relativeFileName[1024];
+	const char* prefix[]={"./data/","../data/","../../data/","../../../data/","../../../../data/"};
+	int prefixIndex=-1;
+	{
+		
+		int numPrefixes = sizeof(prefix)/sizeof(char*);
+		
+		for (int i=0;i<numPrefixes;i++)
+		{
+			FILE* f = 0;
+			sprintf(relativeFileName,"%s%s",prefix[i],fileName);
+			f = fopen(relativeFileName,"r");
+			if (f)
+			{
+				fclose(f);
+				prefixIndex = i;
+				break;
+			}
+		}
+	}
+	
+	if (prefixIndex<0)
+		return;
+	
+	btVector3 shift(0,0,0);
+	btVector3 scaling(10,10,10);
+	int index=10;
+	
+	{
+		
+		GLInstanceGraphicsShape* gfxShape = LoadMeshFromSTL(relativeFileName);
+		
+		btTransform trans;
+		trans.setIdentity();
+		trans.setRotation(btQuaternion(btVector3(1,0,0),SIMD_HALF_PI));
+		
+		btVector3 position = trans.getOrigin();
+		btQuaternion orn = trans.getRotation();
+		
+		btVector3 color(0,0,1);
+		
+		int shapeId = m_app->m_instancingRenderer->registerShape(&gfxShape->m_vertices->at(0).xyzw[0], gfxShape->m_numvertices, &gfxShape->m_indices->at(0), gfxShape->m_numIndices);
+		
+		
+		int id = m_app->m_instancingRenderer->registerGraphicsInstance(shapeId,position,orn,color,scaling);
+
+		/*
+
+		btTriangleMesh* trimeshData = new btTriangleMesh();
+
+		for (int i=0;i<gfxShape->m_numvertices;i++)
+		{
+			for (int j=0;j<3;j++)
+				gfxShape->m_vertices->at(i).xyzw[j] += shift[j];
+		}
+
+		for (int i=0;i<gfxShape->m_numIndices;i+=3)
+		{
+			int index0 = gfxShape->m_indices->at(i);
+			int index1 = gfxShape->m_indices->at(i+1);
+			int index2 = gfxShape->m_indices->at(i+2);
+			
+			btVector3 v0(gfxShape->m_vertices->at(index0).xyzw[0],
+										gfxShape->m_vertices->at(index0).xyzw[1],
+										gfxShape->m_vertices->at(index0).xyzw[2]);
+			btVector3 v1(gfxShape->m_vertices->at(index1).xyzw[0],
+						 gfxShape->m_vertices->at(index1).xyzw[1],
+						 gfxShape->m_vertices->at(index1).xyzw[2]);
+			btVector3 v2(gfxShape->m_vertices->at(index2).xyzw[0],
+						 gfxShape->m_vertices->at(index2).xyzw[1],
+						 gfxShape->m_vertices->at(index2).xyzw[2]);
+			
+			trimeshData->addTriangle(v0,v1,v2);
+		}
+		
+		//btConvexHullShape* convexShape = new btConvexHullShape(&verts[0].x(),verts.size(),sizeof(btVector3));
+		btBvhTriangleMeshShape* shape = new btBvhTriangleMeshShape(trimeshData,true);//meshInterface);
+		
+		btTransform startTrans;startTrans.setIdentity();
+		btRigidBody* body = this->createRigidBody(0,startTrans,shape);
+		//gfxBridge.createCollisionShapeGraphicsObject(shape);
+		btVector3 color(0,0,1);
+		 */
+		//gfxBridge.createRigidBodyGraphicsObject(body,color);
+	}
+}

Demos3/ImportSTLDemo/ImportSTLSetup.h

@@ -0,0 +1,17 @@
+#ifndef IMPORT_STL_SETUP_H
+#define IMPORT_STL_SETUP_H
+
+
+#include "../../Demos/CommonRigidBodySetup.h"
+
+class ImportSTLDemo : public CommonRigidBodySetup
+{
+	struct SimpleOpenGL3App* m_app;
+public:
+    ImportSTLDemo(SimpleOpenGL3App* app);
+    virtual ~ImportSTLDemo();
+    
+	virtual void initPhysics(GraphicsPhysicsBridge& gfxBridge);
+};
+
+#endif //IMPORT_OBJ_SETUP_H

Demos3/ImportURDFDemo/ImportURDFSetup.cpp

@@ -106,65 +106,646 @@ const char* urdf_char3 = MSTRINGIFY(<?xml version="1.0"?>
                                    
                                    </robot>);
 
-const char* urdf_char = MSTRINGIFY(<?xml version="1.0"?>
+const char* urdf_char4 = MSTRINGIFY(
-                                   <robot name="materials">
-                                   <link name="base_link">
-                                   <visual>
-                                   <geometry>
-                                   <cylinder length="0.6" radius="0.2"/>
-                                   </geometry>
-                                   <material name="blue">
-                                   <color rgba="0 0 .8 1"/>
-                                   </material>
-                                   </visual>
-                                   </link>
-                                   
-                                   <link name="right_leg">
-                                   <visual>
-                                   <geometry>
-                                   <box size="0.6 .2 .1"/>
-                                   </geometry>
-                                   <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
-                                   <material name="white">
-                                   <color rgba="1 1 1 1"/>
-                                   </material>
-                                   </visual>
-                                   </link>
-                                   
-                                   <joint name="base_to_right_leg" type="fixed">
-                                   <parent link="base_link"/>
-                                   <child link="right_leg"/>
-                                   <origin xyz="0.22 0 .25"/>
-                                   </joint>
-                                   
-                                   <link name="left_leg">
-                                   <visual>
-                                   <geometry>
-                                   <box size="0.6 .2 .1"/>
-                                   </geometry>
-                                   <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
-                                   <material name="white"/>
-                                   </visual>
-                                   </link>
-                                   
-                                   <joint name="base_to_left_leg" type="fixed">
-                                   <parent link="base_link"/>
-                                   <child link="left_leg"/>
-                                   <origin xyz="-0.22 0 .25"/>
-                                   </joint>
-                                   
-                                   </robot>);
 
+								   <?xml version="1.0"?>
+								   <robot name="materials">
+								   <link name="base_link">
+								   <visual>
+								   <geometry>
+								   <cylinder length="0.6" radius="0.2"/>
+								   </geometry>
+								   <material name="blue">
+								   <color rgba="0 0 .8 1"/>
+								   </material>
+								   </visual>
+								   </link>
+								   
+								   <link name="right_leg">
+								   <visual>
+								   <geometry>
+								   <box size="0.6 .2 .1"/>
+								   </geometry>
+								   <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
+								   <material name="white">
+								   <color rgba="1 1 1 1"/>
+								   </material>
+								   </visual>
+								   </link>
+								   
+								   <joint name="base_to_right_leg" type="fixed">
+								   <parent link="base_link"/>
+								   <child link="right_leg"/>
+								   <origin xyz="0.22 0 .25"/>
+								   </joint>
+								   
+								   <link name="left_leg">
+								   <visual>
+								   <geometry>
+								   <box size="0.6 .2 .1"/>
+								   </geometry>
+								   <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
+								   <material name="white"/>
+								   </visual>
+								   </link>
+								   
+								   <joint name="base_to_left_leg" type="fixed">
+								   <parent link="base_link"/>
+								   <child link="left_leg"/>
+								   <origin xyz="-0.22 0 .25"/>
+								   </joint>
+								   
+								   <link name="head">
+								   <visual>
+								   <geometry>
+								   <sphere radius="0.2"/>
+								   </geometry>
+								   <material name="white"/>
+								   </visual>
+								   </link>
+								   
+								   <joint name="head_swivel" type="fixed">
+								   <parent link="base_link"/>
+								   <child link="head"/>
+								   <origin xyz="0 0 0.3"/>
+								   </joint>
+
+								   <link name="box">
+								   <visual>
+								   <geometry>
+								   <box size=".08 .08 .08"/>
+								   </geometry>
+								   <material name="blue"/>
+								   </visual>
+								   </link>
+								   
+								   <joint name="tobox" type="fixed">
+								   <parent link="head"/>
+								   <child link="box"/>
+								   <origin xyz="0 0.1414 0.1414"/>
+								   </joint>
+								   
+								   </robot>
+
+
+);
+
+const char* urdf_char_r2d2 = MSTRINGIFY(
+
+								   <?xml version="1.0"?>
+								   <robot name="visual">
+								   <link name="base_link">
+								   <visual>
+								   <geometry>
+								   <cylinder length="0.6" radius="0.2"/>
+								   </geometry>
+								   <material name="blue">
+								   <color rgba="0 0 .8 1"/>
+								   </material>
+								   </visual>
+								   </link>
+								   
+								   <link name="right_leg">
+								   <visual>
+								   <geometry>
+								   <box size="0.6 .2 .1"/>
+								   </geometry>
+								   <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
+								   <material name="white">
+								   <color rgba="1 1 1 1"/>
+								   </material>
+								   </visual>
+								   </link>
+								   
+								   <joint name="base_to_right_leg" type="fixed">
+								   <parent link="base_link"/>
+								   <child link="right_leg"/>
+								   <origin xyz="0.22 0 .25"/>
+								   </joint>
+								   
+								   <link name="right_base">
+								   <visual>
+								   <geometry>
+								   <box size=".1 0.4 .1"/>
+								   </geometry>
+								   <material name="white"/>
+								   </visual>
+								   </link>
+								   
+								   <joint name="right_base_joint" type="fixed">
+								   <parent link="right_leg"/>
+								   <child link="right_base"/>
+								   <origin xyz="0 0 -0.6"/>
+								   </joint>
+								   
+								   <link name="right_front_wheel">
+								   <visual>
+								   <geometry>
+								   <cylinder length=".1" radius="0.035"/>
+								   </geometry>
+								   <material name="black">
+								   <color rgba="0 0 0 1"/>
+								   </material>
+								   </visual>
+								   </link>
+								   
+								   <joint name="right_front_wheel_joint" type="fixed">
+								   <parent link="right_base"/>
+								   <child link="right_front_wheel"/>
+								   <origin rpy="0 1.57075 0" xyz="0 0.133333333333 -0.085"/>
+								   </joint>
+								   
+								   <link name="right_back_wheel">
+								   <visual>
+								   <geometry>
+								   <cylinder length=".1" radius="0.035"/>
+								   </geometry>
+								   <material name="black"/>
+								   </visual>
+								   </link>
+								   
+								   <joint name="right_back_wheel_joint" type="fixed">
+								   <parent link="right_base"/>
+								   <child link="right_back_wheel"/>
+								   <origin rpy="0 1.57075 0" xyz="0 -0.133333333333 -0.085"/>
+								   </joint>
+								   
+								   <link name="left_leg">
+								   <visual>
+								   <geometry>
+								   <box size="0.6 .2 .1"/>
+								   </geometry>
+								   <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
+								   <material name="white"/>
+								   </visual>
+								   </link>
+								   
+								   <joint name="base_to_left_leg" type="fixed">
+								   <parent link="base_link"/>
+								   <child link="left_leg"/>
+								   <origin xyz="-0.22 0 .25"/>
+								   </joint>
+								   
+								   <link name="left_base">
+								   <visual>
+								   <geometry>
+								   <box size=".1 0.4 .1"/>
+								   </geometry>
+								   <material name="white"/>
+								   </visual>
+								   </link>
+								   
+								   <joint name="left_base_joint" type="fixed">
+								   <parent link="left_leg"/>
+								   <child link="left_base"/>
+								   <origin xyz="0 0 -0.6"/>
+								   </joint>
+								   
+								   <link name="left_front_wheel">
+								   <visual>
+								   <geometry>
+								   <cylinder length=".1" radius="0.035"/>
+								   </geometry>
+								   <material name="black"/>
+								   </visual>
+								   </link>
+								   
+								   <joint name="left_front_wheel_joint" type="fixed">
+								   <parent link="left_base"/>
+								   <child link="left_front_wheel"/>
+								   <origin rpy="0 1.57075 0" xyz="0 0.133333333333 -0.085"/>
+								   </joint>
+								   
+								   <link name="left_back_wheel">
+								   <visual>
+								   <geometry>
+								   <cylinder length=".1" radius="0.035"/>
+								   </geometry>
+								   <material name="black"/>
+								   </visual>
+								   </link>
+								   
+								   <joint name="left_back_wheel_joint" type="fixed">
+								   <parent link="left_base"/>
+								   <child link="left_back_wheel"/>
+								   <origin rpy="0 1.57075 0" xyz="0 -0.133333333333 -0.085"/>
+								   </joint>
+								   
+								   <joint name="gripper_extension" type="fixed">
+								   <parent link="base_link"/>
+								   <child link="gripper_pole"/>
+								   <origin rpy="0 0 1.57075" xyz="0 0.19 .2"/>
+								   </joint>
+								   
+								   <link name="gripper_pole">
+								   <visual>
+								   <geometry>
+								   <cylinder length="0.2" radius=".01"/>
+								   </geometry>
+								   <origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
+								   <material name="Gray">
+								   <color rgba=".7 .7 .7 1"/>
+								   </material>
+								   </visual>
+								   </link>
+								   
+								   <joint name="left_gripper_joint" type="fixed">
+								   <origin rpy="0 0 0" xyz="0.2 0.01 0"/>
+								   <parent link="gripper_pole"/>
+								   <child link="left_gripper"/>
+								   </joint>
+								   
+								   <link name="left_gripper">
+								   <visual>
+								   <origin rpy="0.0 0 0" xyz="0 0 0"/>
+								   <geometry>
+								   <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
+								   </geometry>
+								   </visual>
+								   </link>
+								   
+								   <joint name="left_tip_joint" type="fixed">
+								   <parent link="left_gripper"/>
+								   <child link="left_tip"/>
+								   </joint>
+								   
+								   <link name="left_tip">
+								   <visual>
+								   <origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
+								   <geometry>
+								   <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
+								   </geometry>
+								   </visual>
+								   </link>
+								   
+								   <joint name="right_gripper_joint" type="fixed">
+								   <origin rpy="0 0 0" xyz="0.2 -0.01 0"/>
+								   <parent link="gripper_pole"/>
+								   <child link="right_gripper"/>
+								   </joint>
+								   
+								   <link name="right_gripper">
+								   <visual>
+								   <origin rpy="-3.1415 0 0" xyz="0 0 0"/>
+								   <geometry>
+								   <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
+								   </geometry>
+								   </visual>
+								   </link>
+								   
+								   <joint name="right_tip_joint" type="fixed">
+								   <parent link="right_gripper"/>
+								   <child link="right_tip"/>
+								   </joint>
+								   
+								   <link name="right_tip">
+								   <visual>
+								   <origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
+								   <geometry>
+								   <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
+								   </geometry>
+								   </visual>
+								   </link>
+								   
+								   <link name="head">
+								   <visual>
+								   <geometry>
+								   <sphere radius="0.2"/>
+								   </geometry>
+								   <material name="white"/>
+								   </visual>
+								   </link>
+								   
+								   <joint name="head_swivel" type="fixed">
+								   <parent link="base_link"/>
+								   <child link="head"/>
+								   <origin xyz="0 0 0.3"/>
+								   </joint>
+								   
+								   <link name="box">
+								   <visual>
+								   <geometry>
+								   <box size=".08 .08 .08"/>
+								   </geometry>
+								   <material name="blue"/>
+								   </visual>
+								   </link>
+								   
+								   <joint name="tobox" type="fixed">
+								   <parent link="head"/>
+								   <child link="box"/>
+								   <origin xyz="0 0.1414 0.1414"/>
+								   </joint>
+								   
+								   </robot>
+);
+
+const char* urdf_char = MSTRINGIFY(
+
+								   <?xml version="1.0"?>
+								   <robot name="flexible">
+								   <link name="base_link">
+								   <visual>
+								   <geometry>
+								   <cylinder length="0.6" radius="0.2"/>
+								   </geometry>
+								   <material name="blue">
+								   <color rgba="0 0 .8 1"/>
+								   </material>
+								   </visual>
+								   </link>
+								   
+								   <link name="right_leg">
+								   <visual>
+								   <geometry>
+								   <box size="0.6 .2 .1"/>
+								   </geometry>
+								   <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
+								   <material name="white">
+								   <color rgba="1 1 1 1"/>
+								   </material>
+								   </visual>
+								   </link>
+								   
+								   <joint name="base_to_right_leg" type="fixed">
+								   <parent link="base_link"/>
+								   <child link="right_leg"/>
+								   <origin xyz="0.22 0 .25"/>
+								   </joint>
+								   
+								   <link name="right_base">
+								   <visual>
+								   <geometry>
+								   <box size=".1 0.4 .1"/>
+								   </geometry>
+								   <material name="white"/>
+								   </visual>
+								   </link>
+								   
+								   <joint name="right_base_joint" type="fixed">
+								   <parent link="right_leg"/>
+								   <child link="right_base"/>
+								   <origin xyz="0 0 -0.6"/>
+								   </joint>
+								   
+								   <link name="right_front_wheel">
+								   <visual>
+								   <geometry>
+								   <cylinder length=".1" radius="0.035"/>
+								   </geometry>
+								   <material name="black">
+								   <color rgba="0 0 0 1"/>
+								   </material>
+								   </visual>
+								   </link>
+								   
+								   <joint name="right_front_wheel_joint" type="continuous">
+								   <axis xyz="0 0 1"/>
+								   <parent link="right_base"/>
+								   <child link="right_front_wheel"/>
+								   <origin rpy="0 1.57075 0" xyz="0 0.133333333333 -0.085"/>
+								   <limit effort="100" velocity="100"/>
+								   <joint_properties damping="0.0" friction="0.0"/>
+								   </joint>
+								   
+								   <link name="right_back_wheel">
+								   <visual>
+								   <geometry>
+								   <cylinder length=".1" radius="0.035"/>
+								   </geometry>
+								   <material name="black"/>
+								   </visual>
+								   </link>
+								   
+								   <joint name="right_back_wheel_joint" type="continuous">
+								   <axis xyz="0 0 1"/>
+								   <parent link="right_base"/>
+								   <child link="right_back_wheel"/>
+								   <origin rpy="0 1.57075 0" xyz="0 -0.133333333333 -0.085"/>
+								   <limit effort="100" velocity="100"/>
+								   <joint_properties damping="0.0" friction="0.0"/>
+								   </joint>
+								   
+								   <link name="left_leg">
+								   <visual>
+								   <geometry>
+								   <box size="0.6 .2 .1"/>
+								   </geometry>
+								   <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
+								   <material name="white"/>
+								   </visual>
+								   </link>
+								   
+								   <joint name="base_to_left_leg" type="fixed">
+								   <parent link="base_link"/>
+								   <child link="left_leg"/>
+								   <origin xyz="-0.22 0 .25"/>
+								   </joint>
+								   
+								   <link name="left_base">
+								   <visual>
+								   <geometry>
+								   <box size=".1 0.4 .1"/>
+								   </geometry>
+								   <material name="white"/>
+								   </visual>
+								   </link>
+								   
+								   <joint name="left_base_joint" type="fixed">
+								   <parent link="left_leg"/>
+								   <child link="left_base"/>
+								   <origin xyz="0 0 -0.6"/>
+								   </joint>
+								   
+								   <link name="left_front_wheel">
+								   <visual>
+								   <geometry>
+								   <cylinder length=".1" radius="0.035"/>
+								   </geometry>
+								   <material name="black"/>
+								   </visual>
+								   </link>
+								   
+								   <joint name="left_front_wheel_joint" type="continuous">
+								   <axis xyz="0 0 1"/>
+								   <parent link="left_base"/>
+								   <child link="left_front_wheel"/>
+								   <origin rpy="0 1.57075 0" xyz="0 0.133333333333 -0.085"/>
+								   <limit effort="100" velocity="100"/>
+								   <joint_properties damping="0.0" friction="0.0"/>
+								   </joint>
+								   
+								   <link name="left_back_wheel">
+								   <visual>
+								   <geometry>
+								   <cylinder length=".1" radius="0.035"/>
+								   </geometry>
+								   <material name="black"/>
+								   </visual>
+								   </link>
+								   
+								   <joint name="left_back_wheel_joint" type="continuous">
+								   <axis xyz="0 0 1"/>
+								   <parent link="left_base"/>
+								   <child link="left_back_wheel"/>
+								   <origin rpy="0 1.57075 0" xyz="0 -0.133333333333 -0.085"/>
+								   <limit effort="100" velocity="100"/>
+								   <joint_properties damping="0.0" friction="0.0"/>
+								   </joint>
+								   <joint name="gripper_extension" type="prismatic">
+								   <parent link="base_link"/>
+								   <child link="gripper_pole"/>
+								   <limit effort="1000.0" lower="-0.38" upper="0" velocity="0.5"/>
+								   <origin rpy="0 0 1.57075" xyz="0 0.19 .2"/>
+								   </joint>
+								   
+								   <link name="gripper_pole">
+								   <visual>
+								   <geometry>
+								   <cylinder length="0.2" radius=".01"/>
+								   </geometry>
+								   <origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
+								   <material name="Gray">
+								   <color rgba=".7 .7 .7 1"/>
+								   </material>
+								   </visual>
+								   </link>
+								   
+								   <joint name="left_gripper_joint" type="revolute">
+								   <axis xyz="0 0 1"/>
+								   <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
+								   <origin rpy="0 0 0" xyz="0.2 0.01 0"/>
+								   <parent link="gripper_pole"/>
+								   <child link="left_gripper"/>
+								   </joint>
+								   
+								   <link name="left_gripper">
+								   <visual>
+								   <origin rpy="0.0 0 0" xyz="0 0 0"/>
+								   <geometry>
+								   <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
+								   </geometry>
+								   </visual>
+								   </link>
+								   
+								   <joint name="left_tip_joint" type="fixed">
+								   <parent link="left_gripper"/>
+								   <child link="left_tip"/>
+								   </joint>
+								   
+								   <link name="left_tip">
+								   <visual>
+								   <origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
+								   <geometry>
+								   <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
+								   </geometry>
+								   </visual>
+								   </link>
+								   
+								   <joint name="right_gripper_joint" type="revolute">
+								   <axis xyz="0 0 -1"/>
+								   <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
+								   <origin rpy="0 0 0" xyz="0.2 -0.01 0"/>
+								   <parent link="gripper_pole"/>
+								   <child link="right_gripper"/>
+								   </joint>
+								   
+								   <link name="right_gripper">
+								   <visual>
+								   <origin rpy="-3.1415 0 0" xyz="0 0 0"/>
+								   <geometry>
+								   <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
+								   </geometry>
+								   </visual>
+								   </link>
+								   
+								   <joint name="right_tip_joint" type="fixed">
+								   <parent link="right_gripper"/>
+								   <child link="right_tip"/>
+								   </joint>
+								   
+								   <link name="right_tip">
+								   <visual>
+								   <origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
+								   <geometry>
+								   <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
+								   </geometry>
+								   </visual>
+								   </link>
+								   
+								   <link name="head">
+								   <visual>
+								   <geometry>
+								   <sphere radius="0.2"/>
+								   </geometry>
+								   <material name="white"/>
+								   </visual>
+								   </link>
+								   
+								   <joint name="head_swivel" type="continuous">
+								   <parent link="base_link"/>
+								   <child link="head"/>
+								   <axis xyz="0 0 1"/>
+								   <origin xyz="0 0 0.3"/>
+								   </joint>
+								   
+								   <link name="box">
+								   <visual>
+								   <geometry>
+								   <box size=".08 .08 .08"/>
+								   </geometry>
+								   <material name="blue"/>
+								   </visual>
+								   </link>
+								   
+								   <joint name="tobox" type="fixed">
+								   <parent link="head"/>
+								   <child link="box"/>
+								   <origin xyz="0 0.1414 0.1414"/>
+								   </joint>
+								   
+								   </robot>
+
+);
 
 #include "BulletCollision/CollisionShapes/btCylinderShape.h"
 
-void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhysicsBridge& gfxBridge)
+void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhysicsBridge& gfxBridge, const btTransform& parentTransformInWorldSpace, btDiscreteDynamicsWorld* world)
 {
     btCollisionShape* shape = 0;
-
+	
+	btTransform linkTransformInWorldSpace;
+	linkTransformInWorldSpace.setIdentity();
+	
+	btScalar mass = 0;
+	btTransform inertialFrame;
+	inertialFrame.setIdentity();
+	
+	if ((*link).inertial)
+	{
+		mass = (*link).inertial->mass;
+		inertialFrame.setOrigin(btVector3((*link).inertial->origin.position.x,(*link).inertial->origin.position.y,(*link).inertial->origin.position.z));
+		inertialFrame.setRotation(btQuaternion((*link).inertial->origin.rotation.x,(*link).inertial->origin.rotation.y,(*link).inertial->origin.rotation.z,(*link).inertial->origin.rotation.w));
+	}
+	
+	if ((*link).parent_joint)
+	{
+		btTransform p2j;
+		const urdf::Vector3 pos = (*link).parent_joint->parent_to_joint_origin_transform.position;
+		const urdf::Rotation orn = (*link).parent_joint->parent_to_joint_origin_transform.rotation;
+		btTransform parent2joint;
+		parent2joint.setOrigin(btVector3(pos.x,pos.y,pos.z));
+		parent2joint.setRotation(btQuaternion(orn.x,orn.y,orn.z,orn.w));
+		linkTransformInWorldSpace =parentTransformInWorldSpace*parent2joint;
+	} else
+	{
+		linkTransformInWorldSpace = parentTransformInWorldSpace;
+	}
+	
+	//btTransform linkCenterOfMass =inertialFrame*linkTransformInWorldSpace;
+	
     {
         printf("converting link %s",link->name.c_str());
-        
         for (int v=0;v<link->visual_array.size();v++)
         {
             const Visual* visual = link->visual_array[v].get();
@@ -172,12 +753,11 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
             
             switch (visual->geometry->type)
             {
-                    //            , BOX, CYLINDER, MESH:
                 case Geometry::CYLINDER:
                 {
                     printf("processing a cylinder\n");
                     urdf::Cylinder* cyl = (urdf::Cylinder*)visual->geometry.get();
-                    btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length);
+                    btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.);
                     btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
                     shape = cylZShape;
                     break;
@@ -186,13 +766,20 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
                 {
                     printf("processing a box\n");
                     urdf::Box* box = (urdf::Box*)visual->geometry.get();
-                    btVector3 halfExtents(box->dim.x,box->dim.y,box->dim.z);
+                    btVector3 extents(box->dim.x,box->dim.y,box->dim.z);
-                    btBoxShape* boxShape = new btBoxShape(halfExtents);
+                    btBoxShape* boxShape = new btBoxShape(extents*0.5f);
                     shape = boxShape;
                     break;
                 }
                 case Geometry::SPHERE:
                 {
+					printf("processing a sphere\n");
+                    urdf::Sphere* sphere = (urdf::Sphere*)visual->geometry.get();
+                    btScalar radius = sphere->radius;
+					btSphereShape* sphereShape = new btSphereShape(radius);
+                    shape = sphereShape;
+                    break;
+
                     break;
                 }
                 case Geometry::MESH:
@@ -205,6 +792,8 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
                 }
             }
             
+			
+			
             if (shape)
             {
                 gfxBridge.createCollisionShapeGraphicsObject(shape);
@@ -214,15 +803,27 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
                 {
                     color.setValue(visual->material->color.r,visual->material->color.g,visual->material->color.b);//,visual->material->color.a);
                 }
-                //            btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0)):
+               
-                btScalar mass = 0.f;
                 btVector3 localInertia(0,0,0);
                 if (mass)
                 {
                     shape->calculateLocalInertia(mass,localInertia);
                 }
                 btRigidBody::btRigidBodyConstructionInfo rbci(mass,0,shape,localInertia);
+				
+				
+				btVector3 visual_pos(visual->origin.position.x,visual->origin.position.y,visual->origin.position.z);
+				btQuaternion visual_orn(visual->origin.rotation.x,visual->origin.rotation.y,visual->origin.rotation.z,visual->origin.rotation.w);
+				btTransform visual_frame;
+				visual_frame.setOrigin(visual_pos);
+				visual_frame.setRotation(visual_orn);
+				
+				btTransform visualFrameInWorldSpace =linkTransformInWorldSpace*visual_frame;
+				rbci.m_startWorldTransform = visualFrameInWorldSpace;//linkCenterOfMass;
+				
+				
                 btRigidBody* body = new btRigidBody(rbci);
+				world->addRigidBody(body);
                 gfxBridge.createRigidBodyGraphicsObject(body,color);
             }
         }
@@ -232,7 +833,7 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
     {
         if (*child)
         {
-            URDFvisual2BulletCollisionShape(*child,gfxBridge);
+            URDFvisual2BulletCollisionShape(*child,gfxBridge, linkTransformInWorldSpace, world);
             
         }
         else
@@ -247,6 +848,27 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
 }
 void ImportUrdfDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge)
 {
+	
+	int upAxis = 2;
+	gfxBridge.setUpAxis(upAxis);
+	
+	this->createEmptyDynamicsWorld();
+/*	btVector3 groundHalfExtents(50,50,50);
+	groundHalfExtents[upAxis]=1.f;
+	btBoxShape* box = new btBoxShape(groundHalfExtents);
+	gfxBridge.createCollisionShapeGraphicsObject(box);
+	btTransform start; start.setIdentity();
+	btVector3 groundOrigin(0,0,0);
+	groundOrigin[upAxis]=-5;
+	start.setOrigin(groundOrigin);	
+	btRigidBody* body =  createRigidBody(0,start,box);
+	btVector3 color(0,1,0);
+	gfxBridge.createRigidBodyGraphicsObject(body,color);
+*/
+	btVector3 gravity(0,0,0);
+	gravity[upAxis]=-9.8;
+	
+	m_dynamicsWorld->setGravity(gravity);
     int argc=0;
     char* filename="somefile.urdf";
     
@@ -288,9 +910,11 @@ void ImportUrdfDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge)
     
     // print entire tree
     printTree(root_link);
-    
+    btTransform worldTrans;
+	worldTrans.setIdentity();
+	
     {
-        URDFvisual2BulletCollisionShape(root_link, gfxBridge);
+        URDFvisual2BulletCollisionShape(root_link, gfxBridge, worldTrans,m_dynamicsWorld);
     }
     
   

Demos3/bullet2/BasicDemo/Bullet2RigidBodyDemo.cpp

@@ -43,7 +43,11 @@ struct MyGraphicsPhysicsBridge : public GraphicsPhysicsBridge
 			box->setUserIndex(cubeShapeId);
 			break;
 		}
-
+		case TRIANGLE_MESH_SHAPE_PROXYTYPE:
+		{
+			
+			break;
+		}
 		default:
 		{
 			if (collisionShape->isConvex())
@@ -145,6 +149,11 @@ struct MyGraphicsPhysicsBridge : public GraphicsPhysicsBridge
 	{
 		return m_glApp->m_parameterInterface;
 	}
+	
+	virtual void setUpAxis(int axis)
+	{
+		m_glApp->setUpAxis(axis);
+	}
 };
 
 Bullet2RigidBodyDemo::Bullet2RigidBodyDemo(SimpleOpenGL3App* app, CommonPhysicsSetup* physicsSetup)
@@ -221,8 +230,10 @@ btVector3	Bullet2RigidBodyDemo::getRayTo(int x,int y)
 	rayForward*= farPlane;
 
 	btVector3 rightOffset;
-	btVector3 m_cameraUp=btVector3(0,1,0);
+	btVector3 cameraUp=btVector3(0,0,0);
-	btVector3 vertical = m_cameraUp;
+	cameraUp[m_glApp->getUpAxis()]=1;
+	
+	btVector3 vertical = cameraUp;
 
 	btVector3 hor;
 	hor = rayForward.cross(vertical);
@@ -266,6 +277,7 @@ bool	Bullet2RigidBodyDemo::mouseMoveCallback(float x,float y)
 
 	return false;
 }
+
 bool	Bullet2RigidBodyDemo::mouseButtonCallback(int button, int state, float x, float y)
 {
 
@@ -278,8 +290,7 @@ bool	Bullet2RigidBodyDemo::mouseButtonCallback(int button, int state, float x, f
 
 			btVector3 rayFrom = camPos;
 			btVector3 rayTo = getRayTo(x,y);
-
+			
-
 			m_physicsSetup->pickBody(rayFrom, rayTo);
 
 

btgui/OpenGLWindow/GLInstancingRenderer.cpp

@@ -379,7 +379,8 @@ GLInstancingRenderer::GLInstancingRenderer(int maxNumObjectCapacity, int maxShap
 	m_textureenabled(true),
 	m_textureinitialized(false),
 	m_screenWidth(0),
-	m_screenHeight(0)
+	m_screenHeight(0),
+	m_upAxis(1)
 {
 
 	m_data = new InternalDataRenderer;
@@ -1068,6 +1069,7 @@ void GLInstancingRenderer::updateCamera(int upAxis)
 
     b3Assert(glGetError() ==GL_NO_ERROR);
 
+	m_upAxis = upAxis;
 	int m_forwardAxis(-1);
 	switch (upAxis)
 	{

btgui/OpenGLWindow/GLInstancingRenderer.h

@@ -52,6 +52,8 @@ class GLInstancingRenderer
 
 	int m_screenWidth;
 	int m_screenHeight;
+	
+	int m_upAxis;
 
 	void renderSceneInternal(int renderMode=B3_DEFAULT_RENDERMODE);
 

btgui/OpenGLWindow/SimpleOpenGL3App.cpp

@@ -42,6 +42,7 @@ struct SimpleInternalData
 	FILE* m_ffmpegFile;
 	GLRenderToTexture*  m_renderTexture;
 	void* m_userPointer;
+	int m_upAxis;//y=1 or z=2 is supported
 
 };
 
@@ -85,6 +86,7 @@ SimpleOpenGL3App::SimpleOpenGL3App(	const char* title, int width,int height)
 	m_data->m_renderTexture = 0;
 	m_data->m_ffmpegFile = 0;
 	m_data->m_userPointer = 0;
+	m_data->m_upAxis = 1;
 
 	m_window = new b3gDefaultOpenGLWindow();
 	b3gWindowConstructionInfo ci;
@@ -123,7 +125,7 @@ SimpleOpenGL3App::SimpleOpenGL3App(	const char* title, int width,int height)
 
     b3Assert(glGetError() ==GL_NO_ERROR);
 
-	m_instancingRenderer = new GLInstancingRenderer(128*1024,4*1024*1024);
+	m_instancingRenderer = new GLInstancingRenderer(128*1024,32*1024*1024);
 	m_instancingRenderer->init();
 	m_instancingRenderer->resize(width,height);
 
@@ -562,3 +564,15 @@ void SimpleOpenGL3App::dumpNextFrameToPng(const char* filename)
     bool result = m_data->m_renderTexture->enable();
 */
 }
+
+void SimpleOpenGL3App::setUpAxis(int axis)
+{
+	b3Assert((axis == 1)||(axis==2));//only Y or Z is supported at the moment
+	m_data->m_upAxis = axis;
+}
+int SimpleOpenGL3App::getUpAxis() const
+{
+	return m_data->m_upAxis;
+}
+
+

btgui/OpenGLWindow/SimpleOpenGL3App.h

@@ -43,6 +43,9 @@ struct SimpleOpenGL3App
     void dumpFramesToVideo(const char* mp4Filename);
     
 	void drawGrid(DrawGridData data=DrawGridData());
+	void setUpAxis(int axis);
+	int getUpAxis() const;
+	
 	void swapBuffer();
 	void drawText( const char* txt, int posX, int posY);
 	struct sth_stash* getFontStash();

build3/premake4.lua

@@ -168,8 +168,6 @@ end
 		include "../test/enet/client"
 	end
 
-	include "../Demos"
-	include "../Extras"
 
 	if _OPTIONS["bullet2gpu"] then
 	end