Merge pull request #323 from erwincoumans/master

fix some btMultiBody URDF conversion issues in ImportURDFSetup, add RenderInstancingDemo, use GLInstanceVertex

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/BulletDemoEntries.h

@@ -29,10 +29,12 @@
 
 #include "../bullet2/CollisionDetection/SupportFuncDemo.h"
 #include "../bullet2/BasicConcepts/CoordinateSystemDemo.h"
+
 #include "../../Demos3/FiniteElementMethod/FiniteElementDemo.h"
 //#include "../../Demos3/bullet2/SoftDemo/SoftDemo.h"
 #include "../Geometry/SphereCreation.h"
 #include "../Geometry/DistributePoints.h"
+#include "../Geometry/RenderInstancingDemo.h"
 
 #define MYCREATEFUNC(func) \
 static BulletDemoInterface* func##CreateFunc(CommonGraphicsApp* app)\
@@ -105,6 +107,7 @@ static BulletDemoEntry allDemos[]=
 	{1,"SphereCreation", &SphereCreation::CreateFunc},
 	{1,"DistributePoints", &DistributePoints::CreateFunc},
 	{1,"Coordinate Frames", CoordinateFrameDemoPhysicsCreateFunc},
+    {1,"Instanced Rendering", &RenderInstancingDemo::CreateFunc},
 //    {0,"Soft Body", 0},
     
 //	{1,"Cloth1", SoftDemo::CreateFunc},

Demos3/AllBullet2Demos/main.cpp

@@ -1,12 +1,9 @@
-//#include "OpenGLWindow/OpenGLInclude.h"
+#include "OpenGLWindow/OpenGLInclude.h"
 //#include "OpenGL/gl.h"
 //#define USE_OPENGL2
-#ifdef  USE_OPENGL2
-#include "OpenGLWindow/SimpleOpenGL2App.h"
-#else
 
+#include "OpenGLWindow/SimpleOpenGL2App.h"
 #include "OpenGLWindow/SimpleOpenGL3App.h"
-#endif
 
 #include "OpenGLWindow/CommonRenderInterface.h"
 #ifdef __APPLE__
@@ -36,188 +33,10 @@
 #include "Bullet3AppSupport/GraphingTexture.h"
 
 #include "OpenGLWindow/SimpleCamera.h"
+#include "OpenGLWindow/SimpleOpenGL2Renderer.h"
 
 CommonGraphicsApp* app=0;
-#ifdef USE_OPENGL2
-struct TestRenderer : public CommonRenderInterface
-{
-	int m_width;
-	int m_height;
-	SimpleCamera	m_camera;
 
-	TestRenderer(int width, int height)
-		:m_width(width),
-		m_height(height)
-	{
-
-	}
-	virtual void init()
-	{
-		
-	}
-	virtual void updateCamera(int upAxis)
-	{
-		float projection[16];
-		float view[16];
-		m_camera.setAspectRatio((float)m_width/(float)m_height);
-		m_camera.update();
-		m_camera.getCameraProjectionMatrix(projection);
-		m_camera.getCameraViewMatrix(view);
-		GLfloat projMat[16];
-		GLfloat viewMat[16];
-		for (int i=0;i<16;i++)
-		{
-			viewMat[i] = view[i];
-			projMat[i] = projection[i];
-		}
-		glMatrixMode(GL_PROJECTION);
-		glLoadIdentity();
-		glMultMatrixf(projMat);
-
-		glMatrixMode(GL_MODELVIEW);
-		glLoadIdentity();
-		glMultMatrixf(viewMat);
-	}
-	virtual void removeAllInstances()
-	{
-	}
-	virtual void setCameraDistance(float dist)
-	{
-		m_camera.setCameraDistance(dist);
-	}
-	virtual void setCameraPitch(float pitch)
-	{
-		m_camera.setCameraPitch(pitch);
-	}
-	virtual void setCameraTargetPosition(float x, float y, float z)
-	{
-		m_camera.setCameraTargetPosition(x,y,z);
-	}
-
-	virtual void	getCameraPosition(float cameraPos[4])
-	{
-		float pos[3];
-		m_camera.getCameraPosition(pos);
-		cameraPos[0] = pos[0];
-		cameraPos[1] = pos[1];
-		cameraPos[2] = pos[2];
-		
-	}
-	virtual void	getCameraPosition(double cameraPos[4])
-	{
-		float pos[3];
-		m_camera.getCameraPosition(pos);
-		cameraPos[0] = pos[0];
-		cameraPos[1] = pos[1];
-		cameraPos[2] = pos[2];
-	}
-
-	virtual void	setCameraTargetPosition(float cameraPos[4])
-	{
-		m_camera.setCameraTargetPosition(cameraPos[0],cameraPos[1],cameraPos[2]);
-	}
-	virtual void	getCameraTargetPosition(float cameraPos[4]) const
-	{
-		m_camera.getCameraTargetPosition(cameraPos);
-	}
-    virtual void	getCameraTargetPosition(double cameraPos[4]) const
-	{
-			cameraPos[0] = 1;
-		cameraPos[1] = 1;
-		cameraPos[2] = 1;
-	}
-
-	virtual void renderScene()
-	{
-	}
-
-
-	virtual int getScreenWidth()
-	{
-		return m_width;
-	}
-	virtual int getScreenHeight()
-	{
-		return m_height;
-	}
-	virtual int registerGraphicsInstance(int shapeIndex, const double* position, const double* quaternion, const double* color, const double* scaling)
-	{
-		return 0;
-	}
-	virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling)
-	{
-		return 0;
-	}
-	virtual void drawLines(const float* positions, const float color[4], int numPoints, int pointStrideInBytes, const unsigned int* indices, int numIndices, float pointDrawSize)
-	{
-		int pointStrideInFloats = pointStrideInBytes/4;
-		glLineWidth(pointDrawSize);
-		for (int i=0;i<numIndices;i+=2)
-		{
-			int index0 = indices[i];
-			int index1 = indices[i+1];
-
-			btVector3 fromColor(color[0],color[1],color[2]);
-			btVector3 toColor(color[0],color[1],color[2]);
-			
-			btVector3 from(positions[index0*pointStrideInFloats],positions[index0*pointStrideInFloats+1],positions[index0*pointStrideInFloats+2]);
-			btVector3 to(positions[index1*pointStrideInFloats],positions[index1*pointStrideInFloats+1],positions[index1*pointStrideInFloats+2]);
-
-			glBegin(GL_LINES);
-				glColor3f(fromColor.getX(), fromColor.getY(), fromColor.getZ());
-				glVertex3d(from.getX(), from.getY(), from.getZ());
-				glColor3f(toColor.getX(), toColor.getY(), toColor.getZ());
-				glVertex3d(to.getX(), to.getY(), to.getZ());
-			glEnd();
-			
-		}
-	}
-	virtual void drawLine(const float from[4], const float to[4], const float color[4], float lineWidth)
-	{
-		glLineWidth(lineWidth);
-		glBegin(GL_LINES);
-			glColor3f(color[0],color[1],color[2]);
-			glVertex3d(from[0],from[1],from[2]);
-			glVertex3d(to[0],to[1],to[2]);
-		glEnd();
-	}
-	virtual int registerShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType=B3_GL_TRIANGLES, int textureIndex=-1)
-	{
-		return 0;
-	}
-
-	virtual void writeSingleInstanceTransformToCPU(const float* position, const float* orientation, int srcIndex)
-	{
-	}
-	virtual void writeSingleInstanceTransformToCPU(const double* position, const double* orientation, int srcIndex)
-	{
-	}
-	virtual void writeTransforms()
-	{
-	}
-
-	
-	virtual void drawLine(const double from[4], const double to[4], const double color[4], double lineWidth)
-	{
-
-	}
-	virtual void drawPoint(const float* position, const float color[4], float pointDrawSize)
-	{
-	}
-	virtual void drawPoint(const double* position, const double color[4], double pointDrawSize)
-	{
-	}
-	
-    virtual void updateShape(int shapeIndex, const float* vertices)
-	{
-	}
-    
-    virtual void enableBlend(bool blend)
-	{
-	}
-
-};
-#endif //USE_OPENGL2
 b3gWindowInterface* s_window = 0;
 CommonParameterInterface*	s_parameterInterface=0;
 CommonRenderInterface*	s_instancingRenderer=0;
@@ -374,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)
     {
@@ -585,14 +406,18 @@ int main(int argc, char* argv[])
 
 	//	wci.m_resizeCallback = MyResizeCallback;
 
-	
+    SimpleOpenGL3App* simpleApp=0;
-#ifdef USE_OPENGL2
+    bool useOpenGL2=false;
+    if (useOpenGL2)
+    {
         app = new SimpleOpenGL2App("AllBullet2Demos",width,height);
-	app->m_renderer = new TestRenderer(width,height);
+        app->m_renderer = new SimpleOpenGL2Renderer(width,height);
-#else
+    } else
-	SimpleOpenGL3App* simpleApp = new SimpleOpenGL3App("AllBullet2Demos",width,height);
+    {
+        simpleApp = new SimpleOpenGL3App("AllBullet2Demos",width,height);
         app = simpleApp;
-#endif
+    }
+    
     s_instancingRenderer = app->m_renderer;
 	s_window  = app->m_window;
 	prevMouseMoveCallback  = s_window->getMouseMoveCallback();
@@ -627,12 +452,16 @@ int main(int argc, char* argv[])
 
 	gui = new GwenUserInterface;
 	GL3TexLoader* myTexLoader = new GL3TexLoader;
-#ifdef USE_OPENGL2
+    
-	Gwen::Renderer::Base* gwenRenderer = new Gwen::Renderer::OpenGL_DebugFont();
+    Gwen::Renderer::Base* gwenRenderer = 0;
-#else
+    if (useOpenGL2)
+    {
+        gwenRenderer = new Gwen::Renderer::OpenGL_DebugFont();
+    } else
+    {
         sth_stash* fontstash=simpleApp->getFontStash();
-	Gwen::Renderer::Base* gwenRenderer = new GwenOpenGL3CoreRenderer(simpleApp->m_primRenderer,fontstash,width,height,s_window->getRetinaScale(),myTexLoader);
+        gwenRenderer = new GwenOpenGL3CoreRenderer(simpleApp->m_primRenderer,fontstash,width,height,s_window->getRetinaScale(),myTexLoader);
-#endif
+    }
 	//
 
 	gui->init(width,height,gwenRenderer,s_window->getRetinaScale());
@@ -822,16 +651,16 @@ int main(int argc, char* argv[])
             if (!pauseSimulation)
                 processProfileData(profWindow,false);
             {
-#ifdef  USE_OPENGL2
+                if (useOpenGL2)
 				{
 					saveOpenGLState(width,height);
 				}
-#endif
                 BT_PROFILE("Draw Gwen GUI");
                 gui->draw(s_instancingRenderer->getScreenWidth(),s_instancingRenderer->getScreenHeight());
-#ifdef  USE_OPENGL2
+                if (useOpenGL2)
+                {
                     restoreOpenGLState();
-#endif
+                }
             }
 		}
 		toggle=1-toggle;

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/Geometry/RenderInstancingDemo.h

@@ -0,0 +1,131 @@
+#ifndef RENDER_INSTANCING_DEMO_H
+#define RENDER_INSTANCING_DEMO_H
+
+#include "Bullet3AppSupport/BulletDemoInterface.h"
+#include "OpenGLWindow/CommonGraphicsApp.h"
+#include "Bullet3Common/b3Quaternion.h"
+
+
+
+///quick demo showing the right-handed coordinate system and positive rotations around each axis
+class RenderInstancingDemo : public BulletDemoInterface
+{
+    CommonGraphicsApp* m_app;
+    float m_x;
+    float m_y;
+    float m_z;
+	b3AlignedObjectArray<int> m_movingInstances;
+	enum
+	{
+		numCubesX = 20,
+		numCubesY = 20
+	};
+public:
+    
+    RenderInstancingDemo(CommonGraphicsApp* app)
+    :m_app(app),
+    m_x(0),
+    m_y(0),
+	m_z(0)
+    {
+		m_app->setUpAxis(2);
+        
+		 {
+             b3Vector3 extents=b3MakeVector3(100,100,100);
+             extents[m_app->getUpAxis()]=1;
+             
+			 int xres = 20;
+			 int yres = 20;
+			 
+			 b3Vector4 color0=b3MakeVector4(0.1, 0.1, 0.1,1);
+			 b3Vector4 color1=b3MakeVector4(0.6, 0.6, 0.6,1);
+            m_app->registerGrid(xres, yres, color0, color1);
+        }
+		 
+        {
+            int boxId = m_app->registerCubeShape(0.1,0.1,0.1);
+           
+            
+            
+            for (int i=-numCubesX/2;i<numCubesX/2;i++)
+            {
+                for (int j = -numCubesY/2;j<numCubesY/2;j++)
+                {
+                    b3Vector3 pos=b3MakeVector3(i,j,j);
+                    pos[app->getUpAxis()] = 1;
+                    b3Quaternion orn(0,0,0,1);
+                    b3Vector4 color=b3MakeVector4(0.3,0.3,0.3,1);
+                    b3Vector3 scaling=b3MakeVector3(1,1,1);
+                   int instanceId = m_app->m_renderer->registerGraphicsInstance(boxId,pos,orn,color,scaling);
+				   m_movingInstances.push_back(instanceId);
+                }
+            }
+        }
+
+		 m_app->m_renderer->writeTransforms();
+    }
+    virtual ~RenderInstancingDemo()
+    {
+        m_app->m_renderer->enableBlend(false);
+    }
+
+    static BulletDemoInterface*    CreateFunc(CommonGraphicsApp* app)
+    {
+        return new RenderInstancingDemo(app);
+    }
+    virtual void physicsDebugDraw(int debugDrawMode)
+    {
+        
+    }
+    virtual void    initPhysics()
+    {
+    }
+    virtual void    exitPhysics()
+    {
+        
+    }
+    virtual void	stepSimulation(float deltaTime)
+    {
+        m_x+=0.01f;
+        m_y+=0.01f;
+		m_z+=0.01f;
+		int index=0;
+        for (int i=-numCubesX/2;i<numCubesX/2;i++)
+        {
+            for (int j = -numCubesY/2;j<numCubesY/2;j++)
+            {
+                b3Vector3 pos=b3MakeVector3(i,j,j);
+                pos[m_app->getUpAxis()] = 1+1*b3Sin(m_x+i-j);
+                float orn[4]={0,0,0,1};
+                m_app->m_renderer->writeSingleInstanceTransformToCPU(pos,orn,m_movingInstances[index++]);
+            }
+        }
+        m_app->m_renderer->writeTransforms();
+        
+    }
+    virtual void	renderScene()
+    {
+		m_app->m_renderer->renderScene();
+    }
+
+	
+    virtual void	physicsDebugDraw()
+    {
+      		
+    }
+    virtual bool	mouseMoveCallback(float x,float y)
+    {
+		return false;   
+    }
+    virtual bool	mouseButtonCallback(int button, int state, float x, float y)
+    {
+        return false;   
+    }
+    virtual bool	keyboardCallback(int key, int state)
+    {
+        return false;   
+    }
+    
+};
+#endif //RENDER_INSTANCING_DEMO_H
+

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->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;
@@ -454,30 +743,38 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
 		{
 
 
-			btCompoundShape* tmpGfxShape = new btCompoundShape();
+			
+
+			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 (!mappings.m_createMultiBody)
+				if (1)//!mappings.m_createMultiBody)
 				{
-						tmpGfxShape->addChildShape(childTrans*inertialFrame.inverse(),childShape);
+					convertURDFToVisualShape(vis, pathPrefix, childTrans*inertialFrame.inverse(), vertices, indices);
-					} else
-					{
-						tmpGfxShape->addChildShape(childTrans,childShape);
 				}
+				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(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);
+								//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);
 
@@ -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,22 +1251,8 @@ 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;
@@ -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/CommonRenderInterface.h

@@ -54,6 +54,7 @@ struct CommonRenderInterface
 	virtual void writeSingleInstanceTransformToCPU(const double* position, const double* orientation, int srcIndex)=0;
 	virtual void writeSingleInstanceColorToCPU(float* color, int srcIndex)=0;
 	virtual void writeSingleInstanceColorToCPU(double* color, int srcIndex)=0;
+    
 	virtual void writeTransforms()=0;
     virtual void enableBlend(bool blend)=0;
 };

btgui/OpenGLWindow/OpenGL2Include.h

@@ -0,0 +1,55 @@
+/*
+Copyright (c) 2012 Advanced Micro Devices, Inc.
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it freely,
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+//Originally written by Erwin Coumans
+
+
+#ifndef __OPENGL_INCLUDE_H
+#define __OPENGL_INCLUDE_H
+
+
+//think different
+#if defined(__APPLE__) && !defined (VMDMESA)
+#include <OpenGL/OpenGL.h>
+#include <OpenGL/gl.h>
+#else
+
+#include "GlewWindows/GL/glew.h"
+
+#ifdef _WINDOWS
+#include <windows.h>
+//#include <GL/gl.h>
+//#include <GL/glu.h>
+#else
+//#include <GL/gl.h>
+//#include <GL/glu.h>
+#endif //_WINDOWS
+#endif //APPLE
+
+//disable glGetError
+//#undef glGetError
+//#define glGetError MyGetError
+//
+//GLenum inline MyGetError()
+//{
+//	return 0;
+//}
+
+///on Linux only glDrawElementsInstancedARB is defined?!?
+//#ifdef __linux
+//#define glDrawElementsInstanced glDrawElementsInstancedARB
+//
+//#endif //__linux
+
+#endif //__OPENGL_INCLUDE_H
+

btgui/OpenGLWindow/SimpleOpenGL2App.cpp

@@ -194,3 +194,14 @@ void SimpleOpenGL2App::drawText( const char* txt, int posX, int posY)
 {
 
 }
+
+void SimpleOpenGL2App::drawText3D( const char* txt, float posX, float posZY, float posZ, float size)
+{
+
+}
+
+void SimpleOpenGL2App::registerGrid(int xres, int yres, float color0[4], float color1[4])
+{
+    
+}
+

btgui/OpenGLWindow/SimpleOpenGL2App.h

@@ -27,5 +27,9 @@ public:
 	{
 		return 0;
 	}
+    virtual void drawText3D( const char* txt, float posX, float posZY, float posZ, float size);
+    virtual void registerGrid(int xres, int yres, float color0[4], float color1[4]);
+
+    
 };
 #endif //SIMPLE_OPENGL2_APP_H

btgui/OpenGLWindow/SimpleOpenGL2Renderer.cpp

@@ -0,0 +1,199 @@
+
+#include "SimpleOpenGL2Renderer.h"
+#include "OpenGL2Include.h"
+#include "Bullet3Common/b3Vector3.h"
+
+
+SimpleOpenGL2Renderer::SimpleOpenGL2Renderer(int width, int height)
+ :m_width(width),
+ m_height(height)
+{
+    
+}
+
+void SimpleOpenGL2Renderer::init()
+{
+}
+
+void SimpleOpenGL2Renderer::updateCamera(int upAxis)
+{
+    float projection[16];
+    float view[16];
+    m_camera.setAspectRatio((float)m_width/(float)m_height);
+    m_camera.update();
+    m_camera.getCameraProjectionMatrix(projection);
+    m_camera.getCameraViewMatrix(view);
+    GLfloat projMat[16];
+    GLfloat viewMat[16];
+    for (int i=0;i<16;i++)
+    {
+        viewMat[i] = view[i];
+        projMat[i] = projection[i];
+    }
+    glMatrixMode(GL_PROJECTION);
+    glLoadIdentity();
+    glMultMatrixf(projMat);
+    
+    glMatrixMode(GL_MODELVIEW);
+    glLoadIdentity();
+    glMultMatrixf(viewMat);
+}
+
+void SimpleOpenGL2Renderer::removeAllInstances()
+{
+}
+
+void SimpleOpenGL2Renderer::setCameraDistance(float dist)
+{
+    m_camera.setCameraDistance(dist);
+}
+
+void SimpleOpenGL2Renderer::setCameraPitch(float pitch)
+{
+    m_camera.setCameraPitch(pitch);
+}
+
+void SimpleOpenGL2Renderer::setCameraTargetPosition(float x, float y, float z)
+{
+    m_camera.setCameraTargetPosition(x,y,z);
+}
+    
+void	SimpleOpenGL2Renderer::getCameraPosition(float cameraPos[4])
+{
+    float pos[3];
+    m_camera.getCameraPosition(pos);
+    cameraPos[0] = pos[0];
+    cameraPos[1] = pos[1];
+    cameraPos[2] = pos[2];
+    
+}
+
+void	SimpleOpenGL2Renderer::getCameraPosition(double cameraPos[4])
+{
+    float pos[3];
+    m_camera.getCameraPosition(pos);
+    cameraPos[0] = pos[0];
+    cameraPos[1] = pos[1];
+    cameraPos[2] = pos[2];
+}
+    
+void	SimpleOpenGL2Renderer::setCameraTargetPosition(float cameraPos[4])
+{
+    m_camera.setCameraTargetPosition(cameraPos[0],cameraPos[1],cameraPos[2]);
+}
+
+void	SimpleOpenGL2Renderer::getCameraTargetPosition(float cameraPos[4]) const
+{
+    m_camera.getCameraTargetPosition(cameraPos);
+}
+
+void	SimpleOpenGL2Renderer::getCameraTargetPosition(double cameraPos[4]) const
+{
+    cameraPos[0] = 1;
+    cameraPos[1] = 1;
+    cameraPos[2] = 1;
+}
+
+void SimpleOpenGL2Renderer::writeSingleInstanceColorToCPU(float* color, int srcIndex)
+{
+}
+void SimpleOpenGL2Renderer::writeSingleInstanceColorToCPU(double* color, int srcIndex)
+{
+    
+}
+void	SimpleOpenGL2Renderer::getCameraViewMatrix(float viewMat[16]) const
+{
+    b3Assert(0);
+}
+void	SimpleOpenGL2Renderer::getCameraProjectionMatrix(float projMat[16]) const
+{
+    b3Assert(0);
+    
+}
+
+
+void SimpleOpenGL2Renderer::renderScene()
+{
+}
+    
+    
+
+
+int SimpleOpenGL2Renderer::registerGraphicsInstance(int shapeIndex, const double* position, const double* quaternion, const double* color, const double* scaling)
+{
+    return 0;
+}
+
+int SimpleOpenGL2Renderer::registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling)
+{
+    return 0;
+}
+
+void SimpleOpenGL2Renderer::drawLines(const float* positions, const float color[4], int numPoints, int pointStrideInBytes, const unsigned int* indices, int numIndices, float pointDrawSize)
+{
+    int pointStrideInFloats = pointStrideInBytes/4;
+    glLineWidth(pointDrawSize);
+    for (int i=0;i<numIndices;i+=2)
+    {
+        int index0 = indices[i];
+        int index1 = indices[i+1];
+        
+        b3Vector3 fromColor = b3MakeVector3(color[0],color[1],color[2]);
+        b3Vector3 toColor = b3MakeVector3(color[0],color[1],color[2]);
+        
+        b3Vector3 from= b3MakeVector3(positions[index0*pointStrideInFloats],positions[index0*pointStrideInFloats+1],positions[index0*pointStrideInFloats+2]);
+        b3Vector3 to= b3MakeVector3(positions[index1*pointStrideInFloats],positions[index1*pointStrideInFloats+1],positions[index1*pointStrideInFloats+2]);
+        
+        glBegin(GL_LINES);
+        glColor3f(fromColor.getX(), fromColor.getY(), fromColor.getZ());
+        glVertex3d(from.getX(), from.getY(), from.getZ());
+        glColor3f(toColor.getX(), toColor.getY(), toColor.getZ());
+        glVertex3d(to.getX(), to.getY(), to.getZ());
+        glEnd();
+        
+    }
+}
+
+void SimpleOpenGL2Renderer::drawLine(const float from[4], const float to[4], const float color[4], float lineWidth)
+{
+        glLineWidth(lineWidth);
+        glBegin(GL_LINES);
+        glColor3f(color[0],color[1],color[2]);
+        glVertex3d(from[0],from[1],from[2]);
+        glVertex3d(to[0],to[1],to[2]);
+        glEnd();
+}
+int SimpleOpenGL2Renderer::registerShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureIndex)
+{
+    return 0;
+}
+
+void SimpleOpenGL2Renderer::writeSingleInstanceTransformToCPU(const float* position, const float* orientation, int srcIndex)
+{
+}
+void SimpleOpenGL2Renderer::writeSingleInstanceTransformToCPU(const double* position, const double* orientation, int srcIndex)
+{
+}
+void SimpleOpenGL2Renderer::writeTransforms()
+{
+}
+
+
+void SimpleOpenGL2Renderer::drawLine(const double from[4], const double to[4], const double color[4], double lineWidth)
+{
+    
+}
+void SimpleOpenGL2Renderer::drawPoint(const float* position, const float color[4], float pointDrawSize)
+{
+}
+void SimpleOpenGL2Renderer::drawPoint(const double* position, const double color[4], double pointDrawSize)
+{
+}
+
+void SimpleOpenGL2Renderer::updateShape(int shapeIndex, const float* vertices)
+{
+}
+
+void SimpleOpenGL2Renderer::enableBlend(bool blend)
+{
+}

btgui/OpenGLWindow/SimpleOpenGL2Renderer.h

@@ -0,0 +1,81 @@
+
+#ifndef SIMPLE_OPENGL2_RENDERER_H
+#define SIMPLE_OPENGL2_RENDERER_H
+
+#include "CommonRenderInterface.h"
+#include "SimpleCamera.h"
+
+struct SimpleOpenGL2Renderer : public CommonRenderInterface
+{
+    int m_width;
+    int m_height;
+    SimpleCamera	m_camera;
+    
+    SimpleOpenGL2Renderer(int width, int height);
+    
+    virtual void init();
+    
+    virtual void updateCamera(int upAxis);
+    
+    virtual void removeAllInstances();
+    
+    virtual void setCameraDistance(float dist);
+    
+    virtual void setCameraPitch(float pitch);
+    
+    virtual void setCameraTargetPosition(float x, float y, float z);
+    
+    virtual void	getCameraPosition(float cameraPos[4]);
+    
+    virtual void	getCameraPosition(double cameraPos[4]);
+    
+    virtual void	setCameraTargetPosition(float cameraPos[4]);
+    
+    virtual void	getCameraTargetPosition(float cameraPos[4]) const;
+    
+    virtual void	getCameraTargetPosition(double cameraPos[4]) const;
+    
+    virtual void writeSingleInstanceColorToCPU(float* color, int srcIndex);
+    virtual void writeSingleInstanceColorToCPU(double* color, int srcIndex);
+    virtual void	getCameraViewMatrix(float viewMat[16]) const;
+    virtual void	getCameraProjectionMatrix(float projMat[16]) const;
+
+    
+    virtual void renderScene();
+    
+    virtual int getScreenWidth()
+    {
+        return m_width;
+    }
+    virtual int getScreenHeight()
+    {
+        return m_height;
+    }
+    virtual int registerGraphicsInstance(int shapeIndex, const double* position, const double* quaternion, const double* color, const double* scaling);
+    
+    virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling);
+    
+    virtual void drawLines(const float* positions, const float color[4], int numPoints, int pointStrideInBytes, const unsigned int* indices, int numIndices, float pointDrawSize);
+    
+    virtual void drawLine(const float from[4], const float to[4], const float color[4], float lineWidth);
+    
+    virtual int registerShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType=B3_GL_TRIANGLES, int textureIndex=-1);
+    
+    virtual void writeSingleInstanceTransformToCPU(const float* position, const float* orientation, int srcIndex);
+    
+    virtual void writeSingleInstanceTransformToCPU(const double* position, const double* orientation, int srcIndex);
+    
+    virtual void writeTransforms();
+    
+    virtual void drawLine(const double from[4], const double to[4], const double color[4], double lineWidth);
+    
+    virtual void drawPoint(const float* position, const float color[4], float pointDrawSize);
+    
+    virtual void drawPoint(const double* position, const double color[4], double pointDrawSize);
+    
+    virtual void updateShape(int shapeIndex, const float* vertices);
+    
+    virtual void enableBlend(bool blend);
+    
+};
+#endif //SIMPLE_OPENGL2_RENDERER_H

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,
-                        const btVector3 &inertia,       // in my frame; assumed diagonal
+                               const btVector3 &inertia,
                                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;
 			}