fix some btMultiBody URDF conversion issues in ImportURDFSetup

remove various vertex format structures and use GLInstanceVertex from #include "OpenGLWindow/GLInstanceGraphicsShape.h"
btMultiBody::setupPrismatic takes an additional argument to allow a shift of inertia tensor, relative to the joint frame (link frame at q=0)

11 files changed, 472 insertions, 141 deletions

diff --git a/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.cpp b/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.cpp
index 987928c20..720054914 100644
--- a/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.cpp
+++ b/Demos/FeatherstoneMultiBodyDemo/FeatherstoneMultiBodyDemo.cpp
@@ -326,7 +326,7 @@ btMultiBody* FeatherstoneMultiBodyDemo::createFeatherstoneMultiBody(class btMult
 			if (settings.m_usePrismatic)// && i==(n_links-1))
 			{
 					bod->setupPrismatic(child_link_num, mass, inertia, this_link_num,
-						parent_to_child, joint_axis_child_prismatic, quatRotate(parent_to_child , pos),settings.m_disableParentCollision);
+						parent_to_child, joint_axis_child_prismatic, quatRotate(parent_to_child , pos),btVector3(0,0,0),settings.m_disableParentCollision);
 
 			} else
 			{
diff --git a/Demos3/AllBullet2Demos/main.cpp b/Demos3/AllBullet2Demos/main.cpp
index 7a641407b..e5b8c86a7 100644
--- a/Demos3/AllBullet2Demos/main.cpp
+++ b/Demos3/AllBullet2Demos/main.cpp
@@ -193,7 +193,9 @@ void openURDFDemo(const char* filename)
     ImportUrdfSetup* physicsSetup = new ImportUrdfSetup();
     physicsSetup->setFileName(filename);
     
+
     sCurrentDemo = new BasicDemo(app, physicsSetup);
+	app->setUpAxis(2);
     
     if (sCurrentDemo)
     {
diff --git a/Demos3/FiniteElementMethod/FiniteElementDemo.cpp b/Demos3/FiniteElementMethod/FiniteElementDemo.cpp
index 8612e1f08..05360ed86 100644
--- a/Demos3/FiniteElementMethod/FiniteElementDemo.cpp
+++ b/Demos3/FiniteElementMethod/FiniteElementDemo.cpp
@@ -28,6 +28,7 @@ subject to the following restrictions:
 #include <OpenTissue/core/containers/t4mesh/util/t4mesh_block_generator.h>
 #include "LinearMath/btAlignedObjectArray.h"
 #include "Bullet3AppSupport/CommonParameterInterface.h"
+#include "OpenGLWindow/GLInstanceGraphicsShape.h"
 
 //typedef OpenTissue::math::BasicMathTypes<float,size_t>    math_types;
 typedef OpenTissue::math::BasicMathTypes<double,size_t>    math_types;
@@ -100,12 +101,7 @@ FiniteElementDemo::~FiniteElementDemo()
     m_app->m_renderer->removeAllInstances();
 }
 
-struct MyTetVertex
-{
-    float x,y,z,w;
-    float nx,ny,nz;
-    float u,v;
-};
+
 
 void    FiniteElementDemo::initPhysics()
 {
@@ -151,19 +147,19 @@ void    FiniteElementDemo::initPhysics()
         int strideInBytes = 9*sizeof(float);
         int numVertices =m_data->m_mesh1.m_nodes.size();
         
-        btAlignedObjectArray<MyTetVertex> verts;
+		btAlignedObjectArray<GLInstanceVertex> verts;
         verts.resize(numVertices);
         for (int n=0;n<m_data->m_mesh1.m_nodes.size();n++)
         {
-            verts[n].x = m_data->m_mesh1.m_nodes[n].m_coord(0);
-             verts[n].y = m_data->m_mesh1.m_nodes[n].m_coord(1);
-             verts[n].z = m_data->m_mesh1.m_nodes[n].m_coord(2);
-            verts[n].w = 1;
-            verts[n].nx = 0;
-            verts[n].ny = 1;
-            verts[n].nz = 0;
-            verts[n].u = 0.5;
-            verts[n].v = 0.4;
+            verts[n].xyzw[0] = m_data->m_mesh1.m_nodes[n].m_coord(0);
+			verts[n].xyzw[1] = m_data->m_mesh1.m_nodes[n].m_coord(1);
+			verts[n].xyzw[2] = m_data->m_mesh1.m_nodes[n].m_coord(2);
+			verts[n].xyzw[3] = 1;
+            verts[n].normal[0] = 0;
+			verts[n].normal[1] = 1;
+			verts[n].normal[2] = 0;
+            verts[n].uv[0] = 0.5;
+			verts[n].uv[1] = 0.4;
             
         }
         btAlignedObjectArray<int> indices;
@@ -180,7 +176,7 @@ void    FiniteElementDemo::initPhysics()
             
         }
         
-        m_data->m_tetrahedralMeshRenderIndex = m_app->m_renderer->registerShape(&verts[0].x,verts.size(),&indices[0],indices.size());
+        m_data->m_tetrahedralMeshRenderIndex = m_app->m_renderer->registerShape(&verts[0].xyzw[0],verts.size(),&indices[0],indices.size());
         
         float pos[4] = {0,0,0,1};
         float orn[4] = {0,0,0,1};
@@ -282,19 +278,19 @@ void	FiniteElementDemo::renderScene()
         int strideInBytes = 9*sizeof(float);
         int numVertices =m_data->m_mesh1.m_nodes.size();
         
-        btAlignedObjectArray<MyTetVertex> verts;
+		btAlignedObjectArray<GLInstanceVertex> verts;
         verts.resize(numVertices);
         for (int n=0;n<m_data->m_mesh1.m_nodes.size();n++)
         {
-            verts[n].x = m_data->m_mesh1.m_nodes[n].m_coord(0);
-            verts[n].y = m_data->m_mesh1.m_nodes[n].m_coord(1);
-            verts[n].z = m_data->m_mesh1.m_nodes[n].m_coord(2);
-            verts[n].w = 1;
-            verts[n].nx = 0;
-            verts[n].ny = 1;
-            verts[n].nz = 0;
-            verts[n].u = 0.5;
-            verts[n].v = 0.4;
+            verts[n].xyzw[0] = m_data->m_mesh1.m_nodes[n].m_coord(0);
+			verts[n].xyzw[1] = m_data->m_mesh1.m_nodes[n].m_coord(1);
+			verts[n].xyzw[2] = m_data->m_mesh1.m_nodes[n].m_coord(2);
+			verts[n].xyzw[3] = 1;
+            verts[n].normal[0] = 0;
+			verts[n].normal[1] = 1;
+			verts[n].normal[2] = 0;
+            verts[n].uv[0] = 0.5;
+			verts[n].uv[1] = 0.4;
             
         }
         btAlignedObjectArray<int> indices;
@@ -311,7 +307,7 @@ void	FiniteElementDemo::renderScene()
             
         }
         
-        m_app->m_renderer->updateShape(m_data->m_tetrahedralMeshRenderIndex,&verts[0].x);
+        m_app->m_renderer->updateShape(m_data->m_tetrahedralMeshRenderIndex,&verts[0].xyzw[0]);
         
     }
 	m_app->m_renderer->renderScene();
diff --git a/Demos3/ImportURDFDemo/ImportURDFSetup.cpp b/Demos3/ImportURDFDemo/ImportURDFSetup.cpp
index 964729a7f..ecd4d9125 100644
--- a/Demos3/ImportURDFDemo/ImportURDFSetup.cpp
+++ b/Demos3/ImportURDFDemo/ImportURDFSetup.cpp
@@ -6,6 +6,7 @@
 #include "../ImportColladaDemo/LoadMeshFromCollada.h"

 #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"

 #include "Bullet3Common/b3FileUtils.h"

+#include "BulletCollision/CollisionShapes/btShapeHull.h"//to create a tesselation of a generic btConvexShape

 

 static int bodyCollisionFilterGroup=btBroadphaseProxy::CharacterFilter;

 static int bodyCollisionFilterMask=btBroadphaseProxy::AllFilter&(~btBroadphaseProxy::CharacterFilter);

@@ -154,8 +155,295 @@ enum MyFileType
 	FILE_COLLADA=2

 };

 

-template <typename T>

-btCollisionShape* convertURDFToCollisionShape(const T* visual, const char* pathPrefix)

+

+

+void convertURDFToVisualShape(const Visual* visual, const char* pathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut)

+{

+

+	

+	GLInstanceGraphicsShape* glmesh = 0;

+

+	btConvexShape* convexColShape = 0;

+

+	switch (visual->geometry->type)

+	{

+		case Geometry::CYLINDER:

+		{

+			printf("processing a cylinder\n");

+			urdf::Cylinder* cyl = (urdf::Cylinder*)visual->geometry.get();

+			btAlignedObjectArray<btVector3> vertices;

+		

+			//int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));

+			int numSteps = 32;

+			for (int i = 0; i<numSteps; i++)

+			{

+

+				btVector3 vert(cyl->radius*btSin(SIMD_2_PI*(float(i) / numSteps)), cyl->radius*btCos(SIMD_2_PI*(float(i) / numSteps)), cyl->length / 2.);

+				vertices.push_back(vert);

+				vert[2] = -cyl->length / 2.;

+				vertices.push_back(vert);

+			}

+

+			btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));

+			cylZShape->setMargin(0.001);

+			convexColShape = cylZShape;

+			break;

+		}

+		case Geometry::BOX:

+		{

+			printf("processing a box\n");

+			urdf::Box* box = (urdf::Box*)visual->geometry.get();

+			btVector3 extents(box->dim.x, box->dim.y, box->dim.z);

+			btBoxShape* boxShape = new btBoxShape(extents*0.5f);

+			//btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5);

+			convexColShape = boxShape;

+			convexColShape->setMargin(0.001);

+			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);

+			convexColShape = sphereShape;

+			convexColShape->setMargin(0.001);

+			break;

+

+			break;

+		}

+		case Geometry::MESH:

+		{

+			if (visual->name.length())

+			{

+				printf("visual->name=%s\n", visual->name.c_str());

+			}

+			if (visual->geometry)

+			{

+				const urdf::Mesh* mesh = (const urdf::Mesh*) visual->geometry.get();

+				if (mesh->filename.length())

+				{

+					const char* filename = mesh->filename.c_str();

+					printf("mesh->filename=%s\n", filename);

+					char fullPath[1024];

+					int fileType = 0;

+					sprintf(fullPath, "%s%s", pathPrefix, filename);

+					b3FileUtils::toLower(fullPath);

+					if (strstr(fullPath, ".dae"))

+					{

+						fileType = FILE_COLLADA;

+					}

+					if (strstr(fullPath, ".stl"))

+					{

+						fileType = FILE_STL;

+					}

+

+

+					sprintf(fullPath, "%s%s", pathPrefix, filename);

+					FILE* f = fopen(fullPath, "rb");

+					if (f)

+					{

+						fclose(f);

+						

+

+

+						switch (fileType)

+						{

+						case FILE_STL:

+						{

+							glmesh = LoadMeshFromSTL(fullPath);

+							break;

+						}

+						case FILE_COLLADA:

+						{

+

+							btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes;

+							btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances;

+							btTransform upAxisTrans; upAxisTrans.setIdentity();

+							float unitMeterScaling = 1;

+

+							LoadMeshFromCollada(fullPath,

+								visualShapes,

+								visualShapeInstances,

+								upAxisTrans,

+								unitMeterScaling);

+

+							glmesh = new GLInstanceGraphicsShape;

+							int index = 0;

+							glmesh->m_indices = new b3AlignedObjectArray<int>();

+							glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();

+

+							for (int i = 0; i<visualShapeInstances.size(); i++)

+							{

+								ColladaGraphicsInstance* instance = &visualShapeInstances[i];

+								GLInstanceGraphicsShape* gfxShape = &visualShapes[instance->m_shapeIndex];

+

+								b3AlignedObjectArray<GLInstanceVertex> verts;

+								verts.resize(gfxShape->m_vertices->size());

+

+								int baseIndex = glmesh->m_vertices->size();

+

+								for (int i = 0; i<gfxShape->m_vertices->size(); i++)

+								{

+									verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0];

+									verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1];

+									verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2];

+									verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0];

+									verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1];

+									verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0];

+									verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1];

+									verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2];

+									verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3];

+

+								}

+

+								int curNumIndices = glmesh->m_indices->size();

+								int additionalIndices = gfxShape->m_indices->size();

+								glmesh->m_indices->resize(curNumIndices + additionalIndices);

+								for (int k = 0; k<additionalIndices; k++)

+								{

+									glmesh->m_indices->at(curNumIndices + k) = gfxShape->m_indices->at(k) + baseIndex;

+								}

+

+								//compensate upAxisTrans and unitMeterScaling here

+								btMatrix4x4 upAxisMat;

+								upAxisMat.setPureRotation(upAxisTrans.getRotation());

+								btMatrix4x4 unitMeterScalingMat;

+								unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling, unitMeterScaling, unitMeterScaling));

+								btMatrix4x4 worldMat = unitMeterScalingMat*upAxisMat*instance->m_worldTransform;

+								//btMatrix4x4 worldMat = instance->m_worldTransform;

+								int curNumVertices = glmesh->m_vertices->size();

+								int additionalVertices = verts.size();

+								glmesh->m_vertices->reserve(curNumVertices + additionalVertices);

+

+								for (int v = 0; v<verts.size(); v++)

+								{

+									btVector3 pos(verts[v].xyzw[0], verts[v].xyzw[1], verts[v].xyzw[2]);

+									pos = worldMat*pos;

+									verts[v].xyzw[0] = float(pos[0]);

+									verts[v].xyzw[1] = float(pos[1]);

+									verts[v].xyzw[2] = float(pos[2]);

+									glmesh->m_vertices->push_back(verts[v]);

+								}

+							}

+							glmesh->m_numIndices = glmesh->m_indices->size();

+							glmesh->m_numvertices = glmesh->m_vertices->size();

+							//glmesh = LoadMeshFromCollada(fullPath);

+

+							break;

+						}

+						default:

+						{

+						}

+						}

+

+

+						if (glmesh && (glmesh->m_numvertices>0))

+						{

+						}

+						else

+						{

+							printf("issue extracting mesh from COLLADA/STL file %s\n", fullPath);

+						}

+

+					}

+					else

+					{

+						printf("mesh geometry not found %s\n", fullPath);

+					}

+

+

+				}

+			}

+

+

+			break;

+		}

+		default:

+		{

+			printf("Error: unknown visual geometry type\n");

+		}

+	}

+

+	//if we have a convex, tesselate into localVertices/localIndices

+	if (convexColShape)

+	{

+		btShapeHull* hull = new btShapeHull(convexColShape);

+		hull->buildHull(0.0);

+		{

+			//	int strideInBytes = 9*sizeof(float);

+			int numVertices = hull->numVertices();

+			int numIndices = hull->numIndices();

+

+			

+			glmesh = new GLInstanceGraphicsShape;

+			int index = 0;

+			glmesh->m_indices = new b3AlignedObjectArray<int>();

+			glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();

+

+

+			for (int i = 0; i < numVertices; i++)

+			{

+				GLInstanceVertex vtx;

+				btVector3 pos = hull->getVertexPointer()[i];

+				vtx.xyzw[0] = pos.x();

+				vtx.xyzw[1] = pos.y();

+				vtx.xyzw[2] = pos.z();

+				vtx.xyzw[3] = 1.f;

+				pos.normalize();

+				vtx.normal[0] = pos.x();

+				vtx.normal[1] = pos.y();

+				vtx.normal[2] = pos.z();

+				vtx.uv[0] = 0.5f;

+				vtx.uv[1] = 0.5f;

+				glmesh->m_vertices->push_back(vtx);

+			}

+

+			btAlignedObjectArray<int> indices;

+			for (int i = 0; i < numIndices; i++)

+			{

+				glmesh->m_indices->push_back(hull->getIndexPointer()[i]);

+			}

+			

+			glmesh->m_numvertices = glmesh->m_vertices->size();

+			glmesh->m_numIndices = glmesh->m_indices->size();

+		}

+		delete convexColShape;

+		convexColShape = 0;

+

+	}

+	

+	if (glmesh && glmesh->m_numIndices>0 && glmesh->m_numvertices >0)

+	{

+

+		int baseIndex = verticesOut.size();

+

+

+

+		for (int i = 0; i < glmesh->m_indices->size(); i++)

+		{

+			indicesOut.push_back(glmesh->m_indices->at(i) + baseIndex);

+		}

+

+		for (int i = 0; i < glmesh->m_vertices->size(); i++)

+		{

+			GLInstanceVertex& v = glmesh->m_vertices->at(i);

+			btVector3 vert(v.xyzw[0],v.xyzw[1],v.xyzw[2]);

+			btVector3 vt = visualTransform*vert;

+			v.xyzw[0] = vt[0];

+			v.xyzw[1] = vt[1];

+			v.xyzw[2] = vt[2];

+			btVector3 triNormal(v.normal[0],v.normal[1],v.normal[2]);

+			triNormal = visualTransform.getBasis()*triNormal;

+			v.normal[0] = triNormal[0];

+			v.normal[1] = triNormal[1];

+			v.normal[2] = triNormal[2];

+			verticesOut.push_back(v);

+		}

+	}

+}

+

+btCollisionShape* convertURDFToCollisionShape(const Collision* visual, const char* pathPrefix)

 {

 	btCollisionShape* shape = 0;

 

@@ -179,12 +467,13 @@ btCollisionShape* convertURDFToCollisionShape(const T* visual, const char* pathP
 

             }

             btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));

-            cylZShape->initializePolyhedralFeatures();

+            cylZShape->setMargin(0.001);

+			cylZShape->initializePolyhedralFeatures();

 			//btConvexShape* cylZShape = new btConeShapeZ(cyl->radius,cyl->length);//(vexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));

             

             //btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.);

             //btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);

-            cylZShape->setMargin(0.001);

+            

 

             shape = cylZShape;

             break;

@@ -447,37 +736,45 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
 

 

     {

-        printf("converting visuals of link %s",link->name.c_str());

-		

+		printf("converting visuals of link %s", link->name.c_str());

+

+

+

+		{

 

-       

-        {

 

 			

-			btCompoundShape* tmpGfxShape = new btCompoundShape();

 

-			for (int v=0;v<(int)link->visual_array.size();v++)

+			btAlignedObjectArray<GLInstanceVertex> vertices;

+			btAlignedObjectArray<int> indices;

+			btTransform startTrans; startTrans.setIdentity();

+			int graphicsIndex = -1;

+

+			for (int v = 0; v < (int)link->visual_array.size(); v++)

 			{

 				const Visual* vis = link->visual_array[v].get();

-				btCollisionShape* childShape = convertURDFToCollisionShape(vis,pathPrefix);

-				if (childShape)

+				btVector3 childPos(vis->origin.position.x, vis->origin.position.y, vis->origin.position.z);

+				btQuaternion childOrn(vis->origin.rotation.x, vis->origin.rotation.y, vis->origin.rotation.z, vis->origin.rotation.w);

+				btTransform childTrans;

+				childTrans.setOrigin(childPos);

+				childTrans.setRotation(childOrn);

+				if (1)//!mappings.m_createMultiBody)

 				{

-					btVector3 childPos(vis->origin.position.x, vis->origin.position.y, vis->origin.position.z);

-					btQuaternion childOrn(vis->origin.rotation.x, vis->origin.rotation.y, vis->origin.rotation.z, vis->origin.rotation.w);

-					btTransform childTrans;

-					childTrans.setOrigin(childPos);

-					childTrans.setRotation(childOrn);

-					if (!mappings.m_createMultiBody)

-					{

-						tmpGfxShape->addChildShape(childTrans*inertialFrame.inverse(),childShape);

-					} else

-					{

-						tmpGfxShape->addChildShape(childTrans,childShape);

-					}

+					convertURDFToVisualShape(vis, pathPrefix, childTrans*inertialFrame.inverse(), vertices, indices);

+				}

+				else

+				{

+					convertURDFToVisualShape(vis, pathPrefix, childTrans, vertices, indices);

 				}

 			}

 

+			if (vertices.size() && indices.size())

+			{

+				graphicsIndex  = gfxBridge.registerGraphicsShape(&vertices[0].xyzw[0], vertices.size(), &indices[0], indices.size());

+			}

+

 			btCompoundShape* compoundShape = new btCompoundShape();

+			compoundShape->setMargin(0.001);

 			for (int v=0;v<(int)link->collision_array.size();v++)

 			{

 				const Collision* col = link->collision_array[v].get();

@@ -489,7 +786,7 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
 					btTransform childTrans;

 					childTrans.setOrigin(childPos);

 					childTrans.setRotation(childOrn);

-					if (!mappings.m_createMultiBody)

+					if (1)//!mappings.m_createMultiBody)

 					{

 						compoundShape->addChildShape(childTrans*inertialFrame.inverse(),childShape);

 					} else

@@ -533,9 +830,8 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
 					//rbci.m_startWorldTransform = inertialFrameInWorldSpace;//linkCenterOfMass;

 					btRigidBody* body = new btRigidBody(rbci);

 					world1->addRigidBody(body, bodyCollisionFilterGroup, bodyCollisionFilterMask);

-					gfxBridge.createCollisionShapeGraphicsObject(tmpGfxShape);

-					//hack-> transfer user inder from visual to collision shape

-					compoundShape->setUserIndex(tmpGfxShape->getUserIndex());

+					

+					compoundShape->setUserIndex(graphicsIndex);

 

 					gfxBridge.createRigidBodyGraphicsObject(body, color);

 					linkInfo->m_bulletRigidBody = body;

@@ -596,10 +892,20 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
 

 								printf("Fixed joint (btMultiBody)\n");

 								//btVector3 dVec = quatRotate(parentComToThisCom.getRotation(),offsetInB.inverse().getOrigin());

-								btQuaternion rot = parent2joint.inverse().getRotation();

+								btQuaternion rot = offsetInA.inverse().getRotation();//parent2joint.inverse().getRotation();

 								//toggle=!toggle;

+								//mappings.m_bulletMultiBody->setupFixed(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1, 

+								//	rot, parent2joint.getOrigin(), btVector3(0,0,0),disableParentCollision);

 								mappings.m_bulletMultiBody->setupFixed(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1, 

-									rot, parent2joint.getOrigin(), btVector3(0,0,0),disableParentCollision);

+									rot*offsetInB.getRotation(), offsetInA.getOrigin(),-offsetInB.getOrigin(),disableParentCollision);

+

+								/*

+								mappings.m_bulletMultiBody->setupRevolute(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1,

+								parent2joint.inverse().getRotation(), jointAxis, offsetInA.getOrigin(),//parent2joint.getOrigin(),

+								-offsetInB.getOrigin(),

+								disableParentCollision);

+								*/

+

 								btMatrix3x3 rm(rot);

 								btScalar y,p,r;

 								rm.getEulerZYX(y,p,r);

@@ -642,19 +948,23 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
 							if (mappings.m_createMultiBody)

 							{

 								//todo: adjust the center of mass transform and pivot axis properly

-								mappings.m_bulletMultiBody->setupRevolute(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1, 

+								/*mappings.m_bulletMultiBody->setupRevolute(

+									linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1, 

+

 									parent2joint.inverse().getRotation(), jointAxis, parent2joint.getOrigin(), 

 									btVector3(0,0,0),//offsetInB.getOrigin(), 

 									disableParentCollision);

-

-								/*

+									*/

+								

 								

 								mappings.m_bulletMultiBody->setupRevolute(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1,

-								parent2joint.inverse().getRotation(), jointAxis, offsetInA.getOrigin(),//parent2joint.getOrigin(),

+								//parent2joint.inverse().getRotation(), jointAxis, offsetInA.getOrigin(),//parent2joint.getOrigin(),

+									offsetInA.inverse().getRotation()*offsetInB.getRotation(), quatRotate(offsetInB.inverse().getRotation(),jointAxis), offsetInA.getOrigin(),//parent2joint.getOrigin(),

+							

 								-offsetInB.getOrigin(),

 								disableParentCollision);

-								linkInfo->m_localVisualFrame.setIdentity();

-								*/

+								//linkInfo->m_localVisualFrame.setIdentity();

+								

 							} else

 							{

 								//only handle principle axis at the moment, 

@@ -711,24 +1021,50 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
                         {

 							if (mappings.m_createMultiBody)

 							{

+								//mappings.m_bulletMultiBody->setupPrismatic(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1,

+								//	parent2joint.inverse().getRotation(),jointAxis,parent2joint.getOrigin(),disableParentCollision);

+

+								//mappings.m_bulletMultiBody->setupPrismatic(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1,

+								//	parent2joint.inverse().getRotation(),jointAxis,parent2joint.getOrigin(),disableParentCollision);

+

 								mappings.m_bulletMultiBody->setupPrismatic(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1,

-									parent2joint.inverse().getRotation(),jointAxis,parent2joint.getOrigin(),disableParentCollision);

+								offsetInA.inverse().getRotation()*offsetInB.getRotation(), quatRotate(offsetInB.inverse().getRotation(),jointAxis), offsetInA.getOrigin(),//parent2joint.getOrigin(),

+								-offsetInB.getOrigin(),

+								disableParentCollision);

 

-								//mappings.m_bulletMultiBody->setupRevolute(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1, 

-								//	parent2joint.getRotation(), jointAxis, parent2joint.getOrigin(), 

-								//	offsetInB.getOrigin(), 

-								//	disableParentCollision);

+								

 

 							} else

 							{

 								

 								btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*pp->m_bulletRigidBody, *linkInfo->m_bulletRigidBody, offsetInA, offsetInB);

-								dof6->setLinearLowerLimit(btVector3(pj->limits->lower,0,0));

-								dof6->setLinearUpperLimit(btVector3(pj->limits->upper,0,0));

-

+								//todo(erwincoumans) for now, we only support principle axis along X, Y or Z

+								btVector3 axis(pj->axis.x,pj->axis.y,pj->axis.z);

+								int principleAxis = axis.closestAxis();

+								switch (principleAxis)

+								{

+								case 0:

+									{

+										dof6->setLinearLowerLimit(btVector3(pj->limits->lower,0,0));

+										dof6->setLinearUpperLimit(btVector3(pj->limits->upper,0,0));

+										break;

+									}

+								case 1:

+									{

+										dof6->setLinearLowerLimit(btVector3(0,pj->limits->lower,0));

+										dof6->setLinearUpperLimit(btVector3(0,pj->limits->upper,0));

+										break;

+									}

+								case 2:

+								default:

+									{

+										dof6->setLinearLowerLimit(btVector3(0,0,pj->limits->lower));

+										dof6->setLinearUpperLimit(btVector3(0,0,pj->limits->upper));

+									}

+								};

+								

 								dof6->setAngularLowerLimit(btVector3(0,0,0));

 								dof6->setAngularUpperLimit(btVector3(0,0,0));

-

 								if (enableConstraints)

 									world1->addConstraint(dof6,true);

 

@@ -752,9 +1088,9 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
 						

 						//btCompoundShape* comp = new btCompoundShape();

 						//comp->addChildShape(linkInfo->m_localVisualFrame,shape);

-						

-						gfxBridge.createCollisionShapeGraphicsObject(tmpGfxShape);

-						compoundShape->setUserIndex(tmpGfxShape->getUserIndex());

+

+						compoundShape->setUserIndex(graphicsIndex);

+

 						col->setCollisionShape(compoundShape);

 

 						btTransform tr;

@@ -826,6 +1162,7 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
 	gfxBridge.setUpAxis(2);

 

 	this->createEmptyDynamicsWorld();

+	//m_dynamicsWorld->getSolverInfo().m_numIterations = 100;

     gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld);

     m_dynamicsWorld->getDebugDrawer()->setDebugMode(

     btIDebugDraw::DBG_DrawConstraints

@@ -833,8 +1170,7 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
     +btIDebugDraw::DBG_DrawAabb

         );//+btIDebugDraw::DBG_DrawConstraintLimits);

 

-

-

+	

 	btVector3 gravity(0,0,0);

 	gravity[upAxis]=-9.8;

 

@@ -915,23 +1251,9 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
 	useFeatherstone = !useFeatherstone;

 	printf("numJoints/DOFS = %d\n", numJoints);

 

-	if (0)

+	bool createGround=true;

+	if (createGround)

 	{

-        btVector3 halfExtents(1,1,1);

-        btBoxShape* box = new btBoxShape(halfExtents);

-        box->initializePolyhedralFeatures();

-

-        gfxBridge.createCollisionShapeGraphicsObject(box);

-        btTransform start; start.setIdentity();

-        btVector3 origin(0,0,0);

-        origin[upAxis]=5;

-        start.setOrigin(origin);

-        btRigidBody* body =  createRigidBody(1,start,box);

-        btVector3 color(0.5,0.5,0.5);

-        gfxBridge.createRigidBodyGraphicsObject(body,color);

-    }

-

-    {

         btVector3 groundHalfExtents(20,20,20);

         groundHalfExtents[upAxis]=1.f;

         btBoxShape* box = new btBoxShape(groundHalfExtents);

@@ -949,6 +1271,7 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
         gfxBridge.createRigidBodyGraphicsObject(body,color);

     }

 

+	///this extra stepSimulation call makes sure that all the btMultibody transforms are properly propagates.

 	m_dynamicsWorld->stepSimulation(1. / 240., 0);// 1., 10, 1. / 240.);

 }

 

diff --git a/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp b/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp
index 61f6a97c1..f2b9b0625 100644
--- a/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp
+++ b/Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp
@@ -27,16 +27,11 @@ static float friction = 1.;
 
 #include "OpenGLWindow/GLInstancingRenderer.h"
 #include "BulletCollision/CollisionShapes/btShapeHull.h"
+#include "OpenGLWindow/GLInstanceGraphicsShape.h"
 
 #define CONSTRAINT_DEBUG_SIZE 0.2f
 static bool prevCanSleep = false;
 
-struct GraphicsVertex
-{
-	float pos[4];
-	float normal[3];
-	float texcoord[2];
-};
 
 static btVector4 colors[4] =
 {
@@ -378,7 +373,7 @@ btMultiBody* FeatherstoneDemo1::createFeatherstoneMultiBody(class btMultiBodyDyn
 			if (settings.m_usePrismatic)// && i==(n_links-1))
 			{
 					bod->setupPrismatic(child_link_num, mass, inertia, this_link_num,
-						parent_to_child, joint_axis_child_prismatic, quatRotate(parent_to_child , pos),settings.m_disableParentCollision);
+						parent_to_child, joint_axis_child_prismatic, quatRotate(parent_to_child , pos),btVector3(0,0,0),settings.m_disableParentCollision);
 
 			} else
 			{
@@ -735,22 +730,22 @@ class RagDoll2
 			int numVertices = hull->numVertices();
 			int numIndices =hull->numIndices();
 			
-			btAlignedObjectArray<GraphicsVertex> gvertices;
+			btAlignedObjectArray<GLInstanceVertex> gvertices;
 			
 			for (int i=0;i<numVertices;i++)
 			{
-				GraphicsVertex vtx;
+				GLInstanceVertex vtx;
 				btVector3 pos =hull->getVertexPointer()[i];
-				vtx.pos[0] = pos.x();
-				vtx.pos[1] = pos.y();
-				vtx.pos[2] = pos.z();
-				vtx.pos[3] = 1.f;
+				vtx.xyzw[0] = pos.x();
+				vtx.xyzw[1] = pos.y();
+				vtx.xyzw[2] = pos.z();
+				vtx.xyzw[3] = 1.f;
 				pos.normalize();
 				vtx.normal[0] =pos.x();
 				vtx.normal[1] =pos.y();
 				vtx.normal[2] =pos.z();
-				vtx.texcoord[0] = 0.5f;
-				vtx.texcoord[1] = 0.5f;
+				vtx.uv[0] = 0.5f;
+				vtx.uv[1] = 0.5f;
 				gvertices.push_back(vtx);
 			}
 			
@@ -758,7 +753,7 @@ class RagDoll2
 			for (int i=0;i<numIndices;i++)
 				indices.push_back(hull->getIndexPointer()[i]);
 			
-			int shapeId = m_app->m_instancingRenderer->registerShape(&gvertices[0].pos[0],numVertices,&indices[0],numIndices);
+			int shapeId = m_app->m_instancingRenderer->registerShape(&gvertices[0].xyzw[0],numVertices,&indices[0],numIndices);
 			
 			int index = m_app->m_instancingRenderer->registerGraphicsInstance(shapeId,body->getWorldTransform().getOrigin(),body->getWorldTransform().getRotation(),color,scaling);
 			body ->setUserIndex(index);
diff --git a/btgui/Bullet3AppSupport/Bullet2RigidBodyDemo.cpp b/btgui/Bullet3AppSupport/Bullet2RigidBodyDemo.cpp
index db5c98eef..a991023fe 100644
--- a/btgui/Bullet3AppSupport/Bullet2RigidBodyDemo.cpp
+++ b/btgui/Bullet3AppSupport/Bullet2RigidBodyDemo.cpp
@@ -7,12 +7,7 @@
 
 #include "BulletCollision/CollisionShapes/btShapeHull.h"//to create a tesselation of a generic btConvexShape
 #include "MyDebugDrawer.h"
-struct GraphicsVertex
-{
-	float pos[4];
-	float normal[3];
-	float texcoord[2];
-};
+#include "OpenGLWindow/GLInstanceGraphicsShape.h"
 
 
 struct MyGraphicsPhysicsBridge : public GraphicsPhysicsBridge
@@ -42,7 +37,18 @@ struct MyGraphicsPhysicsBridge : public GraphicsPhysicsBridge
 		}
 	}
 
-	virtual void createCollisionShapeGraphicsObject(btCollisionShape* collisionShape, const btTransform& parentTransform, btAlignedObjectArray<GraphicsVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut)
+	virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices)
+	{
+		int shapeId = m_glApp->m_renderer->registerShape(vertices, numvertices,indices,numIndices);
+		return shapeId;
+	}
+
+	virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling)
+	{
+		return m_glApp->m_renderer->registerGraphicsInstance(shapeIndex,position,quaternion,color,scaling);
+	}
+
+	virtual void createCollisionShapeGraphicsObject(btCollisionShape* collisionShape, const btTransform& parentTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut)
 	{
 	//todo: support all collision shape types
 		switch (collisionShape->getShapeType())
@@ -82,19 +88,19 @@ struct MyGraphicsPhysicsBridge : public GraphicsPhysicsBridge
 								for (int v=0;v<3;v++)
 								{
 									int index = hull->getIndexPointer()[t*3+v];
-									GraphicsVertex vtx;
+									GLInstanceVertex vtx;
 									btVector3 pos =parentTransform*hull->getVertexPointer()[index];
-									vtx.pos[0] = pos.x();
-									vtx.pos[1] = pos.y();
-									vtx.pos[2] = pos.z();
-									vtx.pos[3] = 0.f;
+									vtx.xyzw[0] = pos.x();
+									vtx.xyzw[1] = pos.y();
+									vtx.xyzw[2] = pos.z();
+									vtx.xyzw[3] = 0.f;
 
 									vtx.normal[0] =triNormal.x();
 									vtx.normal[1] =triNormal.y();
 									vtx.normal[2] =triNormal.z();
 
-									vtx.texcoord[0] = 0.5f;
-									vtx.texcoord[1] = 0.5f;
+									vtx.uv[0] = 0.5f;
+									vtx.uv[1] = 0.5f;
 
 									indicesOut.push_back(verticesOut.size());
 									verticesOut.push_back(vtx);
@@ -129,7 +135,7 @@ struct MyGraphicsPhysicsBridge : public GraphicsPhysicsBridge
 		if (collisionShape->getUserIndex()>=0)
 			return;
 
-		btAlignedObjectArray<GraphicsVertex> vertices;
+		btAlignedObjectArray<GLInstanceVertex> vertices;
 		btAlignedObjectArray<int> indices;
 		btTransform startTrans;startTrans.setIdentity();
 
@@ -137,7 +143,7 @@ struct MyGraphicsPhysicsBridge : public GraphicsPhysicsBridge
 
 		if (vertices.size() && indices.size())
 		{
-			int shapeId = m_glApp->m_renderer->registerShape(&vertices[0].pos[0],vertices.size(),&indices[0],indices.size());
+			int shapeId = m_glApp->m_renderer->registerShape(&vertices[0].xyzw[0],vertices.size(),&indices[0],indices.size());
 			collisionShape->setUserIndex(shapeId);
 		}
 		
diff --git a/btgui/Bullet3AppSupport/CommonPhysicsSetup.h b/btgui/Bullet3AppSupport/CommonPhysicsSetup.h
index 0039eb1b2..76b803d24 100644
--- a/btgui/Bullet3AppSupport/CommonPhysicsSetup.h
+++ b/btgui/Bullet3AppSupport/CommonPhysicsSetup.h
@@ -12,7 +12,7 @@ class btCollisionShape;
 
 class btDiscreteDynamicsWorld;
 
-///The GraphicsPhysicsBridge let's the graphics engine create graphics representation and synchronize
+///The Bullet 2 GraphicsPhysicsBridge let's the graphics engine create graphics representation and synchronize
 struct GraphicsPhysicsBridge
 {
 	virtual ~GraphicsPhysicsBridge() {}
@@ -34,6 +34,11 @@ struct GraphicsPhysicsBridge
 	{
 	}
 
+	virtual int registerGraphicsShape(const float* vertices, int numvertices, const int* indices, int numIndices) { return -1; }//, int primitiveType = B3_GL_TRIANGLES, int textureIndex = -1);
+
+	virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling) { return -1;}
+
+	
 	virtual CommonParameterInterface* getParameterInterface()
 	{
 		return 0;
@@ -45,6 +50,7 @@ struct GraphicsPhysicsBridge
 
 };
 
+///Bullet 2 specific physics setup, that allows to share code between old and new demo frameworks
 struct CommonPhysicsSetup
 {
 public:
diff --git a/btgui/OpenGLWindow/SimpleOpenGL3App.cpp b/btgui/OpenGLWindow/SimpleOpenGL3App.cpp
index e2ee9ab20..8e9e94f42 100644
--- a/btgui/OpenGLWindow/SimpleOpenGL3App.cpp
+++ b/btgui/OpenGLWindow/SimpleOpenGL3App.cpp
@@ -143,7 +143,7 @@ SimpleOpenGL3App::SimpleOpenGL3App(	const char* title, int width,int height)
 
     b3Assert(glGetError() ==GL_NO_ERROR);
 
-	m_instancingRenderer = new GLInstancingRenderer(128*1024,32*1024*1024);
+	m_instancingRenderer = new GLInstancingRenderer(128*1024,64*1024*1024);
 	m_renderer = m_instancingRenderer ;
 	m_instancingRenderer->init();
 	m_instancingRenderer->resize(width,height);
diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp
index dc51b0adf..3786ead2a 100644
--- a/src/BulletDynamics/Featherstone/btMultiBody.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp
@@ -167,13 +167,15 @@ void btMultiBody::setupFixed(int i,
 

 }

 

+

 void btMultiBody::setupPrismatic(int i,

                                btScalar mass,

                                const btVector3 &inertia,

                                int parent,

                                const btQuaternion &rotParentToThis,

                                const btVector3 &jointAxis,

-                               const btVector3 &parentComToThisComOffset,							   

+                               const btVector3 &parentComToThisComOffset,

+							   const btVector3 &thisPivotToThisComOffset,

 							   bool disableParentCollision)

 {

 	if(m_isMultiDof)

@@ -189,6 +191,7 @@ void btMultiBody::setupPrismatic(int i,
     m_links[i].setAxisTop(0, 0., 0., 0.);

     m_links[i].setAxisBottom(0, jointAxis);

     m_links[i].m_eVector = parentComToThisComOffset;

+	m_links[i].m_dVector = thisPivotToThisComOffset;

     m_links[i].m_cachedRotParentToThis = rotParentToThis;

 

 	m_links[i].m_jointType = btMultibodyLink::ePrismatic;

diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h
index c837f00c4..2e2f645ff 100644
--- a/src/BulletDynamics/Featherstone/btMultiBody.h
+++ b/src/BulletDynamics/Featherstone/btMultiBody.h
@@ -65,16 +65,16 @@ public:
                            const btVector3 &thisPivotToThisComOffset,

 						   bool disableParentCollision);

 

-

-    void setupPrismatic(int linkIndex,             // 0 to num_links-1

-                        btScalar mass,

-                        const btVector3 &inertia,       // in my frame; assumed diagonal

-                        int parent,

-                        const btQuaternion &rotParentToThis,  // rotate points in parent frame to my frame.

-                        const btVector3 &jointAxis,             // in my frame

-                        const btVector3 &parentComToThisComOffset,  // vector from parent COM to my COM, in my frame, when q = 0.

-						bool disableParentCollision=false

-						);

+						

+	void setupPrismatic(int i,

+                               btScalar mass,

+                               const btVector3 &inertia,

+                               int parent,

+                               const btQuaternion &rotParentToThis,

+                               const btVector3 &jointAxis,

+                               const btVector3 &parentComToThisComOffset,

+							   const btVector3 &thisPivotToThisComOffset,

+							   bool disableParentCollision);

 

     void setupRevolute(int linkIndex,            // 0 to num_links-1

                        btScalar mass,

diff --git a/src/BulletDynamics/Featherstone/btMultiBodyLink.h b/src/BulletDynamics/Featherstone/btMultiBodyLink.h
index c3cbaf9fd..90acef7d3 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyLink.h
+++ b/src/BulletDynamics/Featherstone/btMultiBodyLink.h
@@ -493,7 +493,7 @@ struct btMultibodyLink
 			case ePrismatic:

 			{

 				// m_cachedRotParentToThis never changes, so no need to update

-				m_cachedRVector = quatRotate(m_cachedRotParentToThis,m_eVector) + pJointPos[0] * getAxisBottom(0);

+				m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector) + pJointPos[0] * getAxisBottom(0);

 

 				break;

 			}