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)

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
 			{

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)
     {

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].xyzw[0] = 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].xyzw[1] = 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].xyzw[2] = m_data->m_mesh1.m_nodes[n].m_coord(2);
-            verts[n].w = 1;
+			verts[n].xyzw[3] = 1;
-            verts[n].nx = 0;
+            verts[n].normal[0] = 0;
-            verts[n].ny = 1;
+			verts[n].normal[1] = 1;
-            verts[n].nz = 0;
+			verts[n].normal[2] = 0;
-            verts[n].u = 0.5;
+            verts[n].uv[0] = 0.5;
-            verts[n].v = 0.4;
+			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].xyzw[0] = 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].xyzw[1] = 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].xyzw[2] = m_data->m_mesh1.m_nodes[n].m_coord(2);
-            verts[n].w = 1;
+			verts[n].xyzw[3] = 1;
-            verts[n].nx = 0;
+            verts[n].normal[0] = 0;
-            verts[n].ny = 1;
+			verts[n].normal[1] = 1;
-            verts[n].nz = 0;
+			verts[n].normal[2] = 0;
-            verts[n].u = 0.5;
+            verts[n].uv[0] = 0.5;
-            verts[n].v = 0.4;
+			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();

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);
+				btVector3 childPos(vis->origin.position.x, vis->origin.position.y, vis->origin.position.z);
-				if (childShape)
+				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);
+					convertURDFToVisualShape(vis, pathPrefix, childTrans*inertialFrame.inverse(), vertices, indices);
-					btQuaternion childOrn(vis->origin.rotation.x, vis->origin.rotation.y, vis->origin.rotation.z, vis->origin.rotation.w);
+				}
-					btTransform childTrans;
+				else
-					childTrans.setOrigin(childPos);
+				{
-					childTrans.setRotation(childOrn);
+					convertURDFToVisualShape(vis, pathPrefix, childTrans, vertices, indices);
-					if (!mappings.m_createMultiBody)
-					{
-						tmpGfxShape->addChildShape(childTrans*inertialFrame.inverse(),childShape);
-					} else
-					{
-						tmpGfxShape->addChildShape(childTrans,childShape);
-					}
 				}
 			}
 
+			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(graphicsIndex);
-					compoundShape->setUserIndex(tmpGfxShape->getUserIndex());
 
 					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,
+								//mappings.m_bulletMultiBody->setupPrismatic(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1,
-									parent2joint.inverse().getRotation(),jointAxis,parent2joint.getOrigin(),disableParentCollision);
+								//	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,
+								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));
+								//todo(erwincoumans) for now, we only support principle axis along X, Y or Z
-								dof6->setLinearUpperLimit(btVector3(pj->limits->upper,0,0));
+								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);
 
@@ -753,8 +1089,8 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
 						//btCompoundShape* comp = new btCompoundShape();
 						//comp->addChildShape(linkInfo->m_localVisualFrame,shape);
 
-						gfxBridge.createCollisionShapeGraphicsObject(tmpGfxShape);
+						compoundShape->setUserIndex(graphicsIndex);
-						compoundShape->setUserIndex(tmpGfxShape->getUserIndex());
+
 						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
@@ -834,7 +1171,6 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
         );//+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.);
 }
 

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.xyzw[0] = pos.x();
-				vtx.pos[1] = pos.y();
+				vtx.xyzw[1] = pos.y();
-				vtx.pos[2] = pos.z();
+				vtx.xyzw[2] = pos.z();
-				vtx.pos[3] = 1.f;
+				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.uv[0] = 0.5f;
-				vtx.texcoord[1] = 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);

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
+#include "OpenGLWindow/GLInstanceGraphicsShape.h"
-{
-	float pos[4];
-	float normal[3];
-	float texcoord[2];
-};
 
 
 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.xyzw[0] = pos.x();
-									vtx.pos[1] = pos.y();
+									vtx.xyzw[1] = pos.y();
-									vtx.pos[2] = pos.z();
+									vtx.xyzw[2] = pos.z();
-									vtx.pos[3] = 0.f;
+									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.uv[0] = 0.5f;
-									vtx.texcoord[1] = 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);
 		}
 		

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:

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

src/BulletDynamics/Featherstone/btMultiBody.cpp

@@ -167,6 +167,7 @@ void btMultiBody::setupFixed(int i,
 
 }
 
+
 void btMultiBody::setupPrismatic(int i,
                                btScalar mass,
                                const btVector3 &inertia,
@@ -174,6 +175,7 @@ void btMultiBody::setupPrismatic(int i,
                                const btQuaternion &rotParentToThis,
                                const btVector3 &jointAxis,
                                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;

src/BulletDynamics/Featherstone/btMultiBody.h

@@ -66,15 +66,15 @@ public:
 						   bool disableParentCollision);
 
 						
-    void setupPrismatic(int linkIndex,             // 0 to num_links-1
+	void setupPrismatic(int i,
-                        btScalar mass,
+                               btScalar mass,
-                        const btVector3 &inertia,       // in my frame; assumed diagonal
+                               const btVector3 &inertia,
-                        int parent,
+                               int parent,
-                        const btQuaternion &rotParentToThis,  // rotate points in parent frame to my frame.
+                               const btQuaternion &rotParentToThis,
-                        const btVector3 &jointAxis,             // in my frame
+                               const btVector3 &jointAxis,
-                        const btVector3 &parentComToThisComOffset,  // vector from parent COM to my COM, in my frame, when q = 0.
+                               const btVector3 &parentComToThisComOffset,
-						bool disableParentCollision=false
+							   const btVector3 &thisPivotToThisComOffset,
-						);
+							   bool disableParentCollision);
 
     void setupRevolute(int linkIndex,            // 0 to num_links-1
                        btScalar mass,

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;
 			}