X11OpenGLWindow: create stencil buffer for shadows in OpenGL2 mode (OpenGL3 uses shadow maps)

Add 'createCollisionObjectGraphicsObject' API for CommonPhysicsSetup
Add 'pthread' dependency as workaround for NVIDIA graphics driver issue
(see //See https://bugs.launchpad.net/ubuntu/+source/nvidia-graphics-drivers-319/+bug/1248642
)

Demos/CommonMultiBodySetup.h

@@ -0,0 +1,247 @@
+
+#ifndef COMMON_MULTI_BODY_SETUP_H
+#define COMMON_MULTI_BODY_SETUP_H
+
+//todo: replace this 'btBulletDynamicsCommon.h' header with specific used header files
+#include "btBulletDynamicsCommon.h"
+#include "CommonPhysicsSetup.h"
+#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+
+struct CommonMultiBodySetup : public CommonPhysicsSetup
+{
+		//keep the collision shapes, for deletion/cleanup
+	btAlignedObjectArray<btCollisionShape*>	m_collisionShapes;
+	btBroadphaseInterface*	m_broadphase;
+	btCollisionDispatcher*	m_dispatcher;
+	btMultiBodyConstraintSolver*	m_solver;
+	btDefaultCollisionConfiguration* m_collisionConfiguration;
+	btMultiBodyDynamicsWorld* m_dynamicsWorld;
+
+	//data for picking objects
+	class btRigidBody*	m_pickedBody;
+	class btTypedConstraint* m_pickedConstraint;
+	btVector3 m_oldPickingPos;
+	btVector3 m_hitPos;
+	btScalar m_oldPickingDist;
+
+	CommonMultiBodySetup()
+	:m_broadphase(0),
+		m_dispatcher(0),
+		m_solver(0),
+		m_collisionConfiguration(0),
+		m_dynamicsWorld(0),
+		m_pickedBody(0),
+		m_pickedConstraint(0)
+	{
+	}
+
+	virtual void createEmptyDynamicsWorld()
+	{
+		///collision configuration contains default setup for memory, collision setup
+		m_collisionConfiguration = new btDefaultCollisionConfiguration();
+		//m_collisionConfiguration->setConvexConvexMultipointIterations();
+
+		///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
+		m_dispatcher = new	btCollisionDispatcher(m_collisionConfiguration);
+
+		m_broadphase = new btDbvtBroadphase();
+
+		m_solver = new btMultiBodyConstraintSolver;
+
+		m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration);
+
+		m_dynamicsWorld->setGravity(btVector3(0, -10, 0));
+	}
+
+
+	virtual void stepSimulation(float deltaTime)
+	{
+		if (m_dynamicsWorld)
+		{
+			m_dynamicsWorld->stepSimulation(deltaTime);
+		}
+	}
+
+
+	virtual void exitPhysics()
+	{
+		removePickingConstraint();
+		//cleanup in the reverse order of creation/initialization
+
+		//remove the rigidbodies from the dynamics world and delete them
+
+		if (m_dynamicsWorld)
+		{
+
+            int i;
+            for (i = m_dynamicsWorld->getNumConstraints() - 1; i >= 0; i--)
+            {
+                m_dynamicsWorld->removeConstraint(m_dynamicsWorld->getConstraint(i));
+            }
+			for (i = m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
+			{
+				btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
+				btRigidBody* body = btRigidBody::upcast(obj);
+				if (body && body->getMotionState())
+				{
+					delete body->getMotionState();
+				}
+				m_dynamicsWorld->removeCollisionObject(obj);
+				delete obj;
+			}
+		}
+		//delete collision shapes
+		for (int j = 0; j<m_collisionShapes.size(); j++)
+		{
+			btCollisionShape* shape = m_collisionShapes[j];
+			delete shape;
+		}
+		m_collisionShapes.clear();
+
+		delete m_dynamicsWorld;
+
+		delete m_solver;
+
+		delete m_broadphase;
+
+		delete m_dispatcher;
+
+		delete m_collisionConfiguration;
+	}
+
+	virtual void syncPhysicsToGraphics(GraphicsPhysicsBridge& gfxBridge)
+	{
+		if (m_dynamicsWorld)
+		{
+			gfxBridge.syncPhysicsToGraphics(m_dynamicsWorld);
+		}
+	}
+
+    virtual void    debugDraw()
+    {
+     	if (m_dynamicsWorld)
+        {
+            m_dynamicsWorld->debugDrawWorld();
+        }
+
+    }
+
+	virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)
+	{
+		if (m_dynamicsWorld==0)
+			return false;
+
+		btCollisionWorld::ClosestRayResultCallback rayCallback(rayFromWorld, rayToWorld);
+
+		m_dynamicsWorld->rayTest(rayFromWorld, rayToWorld, rayCallback);
+		if (rayCallback.hasHit())
+		{
+
+			btVector3 pickPos = rayCallback.m_hitPointWorld;
+			btRigidBody* body = (btRigidBody*)btRigidBody::upcast(rayCallback.m_collisionObject);
+			if (body)
+			{
+				//other exclusions?
+				if (!(body->isStaticObject() || body->isKinematicObject()))
+				{
+					m_pickedBody = body;
+					m_pickedBody->setActivationState(DISABLE_DEACTIVATION);
+					//printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ());
+					btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos;
+					btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*body, localPivot);
+					m_dynamicsWorld->addConstraint(p2p, true);
+					m_pickedConstraint = p2p;
+					btScalar mousePickClamping = 30.f;
+					p2p->m_setting.m_impulseClamp = mousePickClamping;
+					//very weak constraint for picking
+					p2p->m_setting.m_tau = 0.001f;
+				}
+			}
+
+
+			//					pickObject(pickPos, rayCallback.m_collisionObject);
+			m_oldPickingPos = rayToWorld;
+			m_hitPos = pickPos;
+			m_oldPickingDist = (pickPos - rayFromWorld).length();
+			//					printf("hit !\n");
+			//add p2p
+		}
+		return false;
+	}
+	virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)
+	{
+		if (m_pickedBody  && m_pickedConstraint)
+		{
+			btPoint2PointConstraint* pickCon = static_cast<btPoint2PointConstraint*>(m_pickedConstraint);
+			if (pickCon)
+			{
+				//keep it at the same picking distance
+
+				btVector3 newPivotB;
+
+				btVector3 dir = rayToWorld - rayFromWorld;
+				dir.normalize();
+				dir *= m_oldPickingDist;
+
+				newPivotB = rayFromWorld + dir;
+				pickCon->setPivotB(newPivotB);
+				return true;
+			}
+		}
+		return false;
+	}
+	virtual void removePickingConstraint()
+	{
+		if (m_pickedConstraint)
+		{
+			m_dynamicsWorld->removeConstraint(m_pickedConstraint);
+			delete m_pickedConstraint;
+			m_pickedConstraint = 0;
+			m_pickedBody = 0;
+		}
+	}
+
+
+
+	btBoxShape* createBoxShape(const btVector3& halfExtents)
+	{
+		btBoxShape* box = new btBoxShape(halfExtents);
+		return box;
+	}
+
+	btRigidBody*	createRigidBody(float mass, const btTransform& startTransform, btCollisionShape* shape,  const btVector4& color = btVector4(1, 0, 0, 1))
+	{
+		btAssert((!shape || shape->getShapeType() != INVALID_SHAPE_PROXYTYPE));
+
+		//rigidbody is dynamic if and only if mass is non zero, otherwise static
+		bool isDynamic = (mass != 0.f);
+
+		btVector3 localInertia(0, 0, 0);
+		if (isDynamic)
+			shape->calculateLocalInertia(mass, localInertia);
+
+		//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
+
+#define USE_MOTIONSTATE 1
+#ifdef USE_MOTIONSTATE
+		btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
+
+		btRigidBody::btRigidBodyConstructionInfo cInfo(mass, myMotionState, shape, localInertia);
+
+		btRigidBody* body = new btRigidBody(cInfo);
+		//body->setContactProcessingThreshold(m_defaultContactProcessingThreshold);
+
+#else
+		btRigidBody* body = new btRigidBody(mass, 0, shape, localInertia);
+		body->setWorldTransform(startTransform);
+#endif//
+
+		body->setUserIndex(-1);
+		m_dynamicsWorld->addRigidBody(body);
+		return body;
+	}
+};
+
+#endif //COMMON_MULTI_BODY_SETUP_H
+

Demos/CommonPhysicsSetup.h

@@ -3,6 +3,7 @@
 #define COMMON_PHYSICS_SETUP_H
 
 class btRigidBody;
+class btCollisionObject;
 class btBoxShape;
 class btTransform;
 class btCollisionShape;
@@ -18,6 +19,10 @@ struct GraphicsPhysicsBridge
 	virtual void createRigidBodyGraphicsObject(btRigidBody* body,const btVector3& color)
 	{
 	}
+
+	virtual void createCollisionObjectGraphicsObject(btCollisionObject* obj,const btVector3& color)
+	{
+	}
 	virtual void createCollisionShapeGraphicsObject(btCollisionShape* collisionShape)
 	{
 	}
@@ -32,11 +37,11 @@ struct GraphicsPhysicsBridge
 	{
 		return 0;
 	}
-	
+
 	virtual void setUpAxis(int axis)
 	{
 	}
-	
+
 };
 
 struct CommonPhysicsSetup

Demos/OpenGL/GL_ShapeDrawer.cpp

@@ -462,7 +462,7 @@ void GL_ShapeDrawer::drawOpenGL(btScalar* m, const btCollisionShape* shape, cons
 				{
 					const int		s=x>>4;
 					const GLubyte	b=180;					
-					GLubyte			c=b+((s+t&1)&1)*(255-b);
+					GLubyte			c=b+((s+(t&1))&1)*(255-b);
 					pi[0]=pi[1]=pi[2]=pi[3]=c;pi+=3;
 				}
 			}
@@ -518,7 +518,7 @@ void GL_ShapeDrawer::drawOpenGL(btScalar* m, const btCollisionShape* shape, cons
 
 		glColor3f(color.x(),color.y(), color.z());		
 
-		bool useWireframeFallback = true;
+		//bool useWireframeFallback = true;
 
 		if (!(debugMode & btIDebugDraw::DBG_DrawWireframe))
 		{
@@ -535,7 +535,7 @@ void GL_ShapeDrawer::drawOpenGL(btScalar* m, const btCollisionShape* shape, cons
 					const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape);
 					float radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin
 					drawSphere(radius,10,10);
-					useWireframeFallback = false;
+					//useWireframeFallback = false;
 					break;
 				}
 
@@ -586,7 +586,7 @@ void GL_ShapeDrawer::drawOpenGL(btScalar* m, const btCollisionShape* shape, cons
 					glEnd();
 #endif
 
-					useWireframeFallback = false;
+					//useWireframeFallback = false;
 					break;
 				}
 
@@ -617,7 +617,7 @@ void GL_ShapeDrawer::drawOpenGL(btScalar* m, const btCollisionShape* shape, cons
 
 					glTranslatef(0.0, 0.0, -0.5*height);
 					glutSolidCone(radius,height,10,10);
-					useWireframeFallback = false;
+					//useWireframeFallback = false;
 					break;
 
 				}

Demos/OpenGL/GlutStuff.cpp

@@ -4,8 +4,8 @@ Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
 
 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, 
+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.
@@ -17,6 +17,11 @@ subject to the following restrictions:
 
 #include "DemoApplication.h"
 
+#if !defined(_WIN32) && !defined(__APPLE__)
+//assume linux workaround
+#include <pthread.h>
+#endif
+
 //glut is C code, this global gDemoApplication links glut to the C++ demo
 static DemoApplication* gDemoApplication = 0;
 
@@ -74,9 +79,17 @@ static void glutDisplayCallback(void)
 //#include <GL/glut.h>
 
 int glutmain(int argc, char **argv,int width,int height,const char* title,DemoApplication* demoApp) {
-    
+
 	gDemoApplication = demoApp;
 
+#if !defined(_WIN32) && !defined(__APPLE__)
+//Access pthreads as a workaround for a bug in Linux/Ubuntu
+//See https://bugs.launchpad.net/ubuntu/+source/nvidia-graphics-drivers-319/+bug/1248642
+
+	int i=pthread_getconcurrency();
+        printf("pthread_getconcurrency()=%d\n",i);
+#endif
+
 	glutInit(&argc, argv);
     glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA | GLUT_DEPTH | GLUT_STENCIL);
     glutInitWindowPosition(width/2, height/2);
@@ -112,7 +125,7 @@ CGLSetParameter(cgl_context, kCGLCPSwapInterval, &swap_interval);
 #endif
 
 
-	
+
     glutMainLoop();
     return 0;
 }

Demos/SerializeDemo/SerializeSetup.cpp

@@ -3,19 +3,59 @@
 
 SerializeSetup::SerializeSetup()
 {
-    
+
 }
 SerializeSetup::~SerializeSetup()
 {
-    
+
 }
-    
+
 void SerializeSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
 {
     this->createEmptyDynamicsWorld();
     gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld);
     m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe);
     btBulletWorldImporter* importer = new btBulletWorldImporter(m_dynamicsWorld);
-    const char* filename = "testFile.bullet";
-    importer->loadFile(filename);
-}
+	const char* someFileName="spider.bullet";
+
+    const char* prefix[]={"./","./data/","../data/","../../data/","../../../data/","../../../../data/"};
+        int numPrefixes = sizeof(prefix)/sizeof(const char*);
+        char relativeFileName[1024];
+        FILE* f=0;
+        bool fileFound = false;
+        int result = 0;
+
+        for (int i=0;!f && i<numPrefixes;i++)
+        {
+                sprintf(relativeFileName,"%s%s",prefix[i],someFileName);
+                f = fopen(relativeFileName,"rb");
+                if (f)
+                {
+                    fileFound = true;
+                    break;
+                }
+        }
+        if (f)
+        {
+                fclose(f);
+        }
+
+
+    importer->loadFile(relativeFileName);
+
+	//for now, guess the up axis from gravity
+	if (m_dynamicsWorld->getGravity()[1] == 0.f)
+	{
+		gfxBridge.setUpAxis(2);
+	} else
+	{
+		gfxBridge.setUpAxis(1);
+	}
+
+
+}
+
+void SerializeSetup::stepSimulation(float deltaTime)
+{
+    CommonRigidBodySetup::stepSimulation(deltaTime);
+}

Demos/SerializeDemo/SerializeSetup.h

@@ -7,8 +7,9 @@ class SerializeSetup : public CommonRigidBodySetup
 public:
     SerializeSetup();
     virtual ~SerializeSetup();
-    
+
 	virtual void initPhysics(GraphicsPhysicsBridge& gfxBridge);
+	virtual void stepSimulation(float deltaTime);
 };
 
 #endif //SERIALIZE_SETUP_H

Demos/premake4.lua

@@ -34,7 +34,7 @@ function createDemos( demos, incdirs, linknames)
 	 		linkoptions { "-framework Carbon -framework OpenGL -framework AGL -framework Glut" } 
 		
 		configuration {"not Windows", "not MacOSX"}
-			links {"GL","GLU","glut"}
+			links {"GL","GLU","glut","pthread"}
 		configuration{}
 	
 		

Demos3/AllBullet2Demos/BulletDemoEntries.h

@@ -18,6 +18,14 @@
 #include "../ImportURDFDemo/ImportURDFSetup.h"
 #include "../ImportObjDemo/ImportObjSetup.h"
 #include "../ImportSTLDemo/ImportSTLSetup.h"
+#include "../../Demos/SerializeDemo/SerializeSetup.h"
+#include "../bullet2/MultiBodyDemo/TestJointTorqueSetup.h"
+
+static BulletDemoInterface* TestJointTorqueCreateFunc(SimpleOpenGL3App* app)
+{
+       CommonPhysicsSetup* physicsSetup = new TestJointTorqueSetup();
+       return new BasicDemo(app, physicsSetup);
+}
 
 static BulletDemoInterface* LuaDemoCreateFunc(SimpleOpenGL3App* app)
 {
@@ -36,7 +44,11 @@ static BulletDemoInterface* MyKinematicObjectCreateFunc(SimpleOpenGL3App* app)
 	CommonPhysicsSetup* physicsSetup = new KinematicObjectSetup();
 	return new BasicDemo(app, physicsSetup);
 }
-
+static BulletDemoInterface* MySerializeCreateFunc(SimpleOpenGL3App* app)
+{
+    CommonPhysicsSetup* physicsSetup = new SerializeSetup();
+	return new BasicDemo(app, physicsSetup);
+}
 static BulletDemoInterface* MyConstraintCreateFunc(SimpleOpenGL3App* app)
 {
 	CommonPhysicsSetup* physicsSetup = new ConstraintPhysicsSetup();
@@ -70,7 +82,7 @@ struct BulletDemoEntry
 
 static BulletDemoEntry allDemos[]=
 {
-	
+
 	//{"emptydemo",EmptyBulletDemo::MyCreateFunc},
 	{0,"API Demos", 0},
 
@@ -82,10 +94,11 @@ static BulletDemoEntry allDemos[]=
 
 	{0,"File Formats", 0},
 //@todo(erwincoumans)	{ 1, "bullet", MyImportSTLCreateFunc},
+    { 1, ".bullet",MySerializeCreateFunc},
 	{ 1, "Wavefront Obj", MyImportObjCreateFunc},
     { 1, "URDF", MyImportUrdfCreateFunc },
 	{ 1, "STL", MyImportSTLCreateFunc},
-    
+
 /*	{1,"ChainDemo",ChainDemo::MyCreateFunc},
 //	{0, "Stress tests", 0 },
 
@@ -95,13 +108,15 @@ static BulletDemoEntry allDemos[]=
 	{1,"LemkeHingeDemo",HingeDemo::LemkeCreateFunc},
 	{1,"InertiaHingeDemo",HingeDemo::InertiaCreateFunc},
 	{1,"ABMHingeDemo",HingeDemo::FeatherstoneCreateFunc},
-	
+
 	{1,"Ragdoll",RagDollDemo::MyCreateFunc},
 	*/
 	{ 0, "Multibody" ,0},
 	{1,"MultiBody1",FeatherstoneDemo1::MyCreateFunc},
 //	{"MultiBody2",FeatherstoneDemo2::MyCreateFunc},
 	{1,"MultiDofDemo",MultiDofDemo::MyCreateFunc},
+	{1,"TestJointTorque",TestJointTorqueCreateFunc},
+
 
 };
 

Demos3/AllBullet2Demos/premake4.lua

@@ -44,12 +44,21 @@
 		"../bullet2/BasicDemo/Bullet2RigidBodyDemo.h",
 		"../bullet2/LuaDemo/LuaPhysicsSetup.cpp",
 		"../bullet2/LuaDemo/LuaPhysicsSetup.h",
+		"../bullet2/MultiBodyDemo/TestJointTorqueSetup.cpp",
+		"../bullet2/MultiBodyDemo/TestJointTorqueSetup.h",
 	--	"../DifferentialGearDemo/DifferentialGearSetup.cpp",
 --		"../DifferentialGearDemo/DifferentialGearSetup.h",
 		"../../Demos/BasicDemo/BasicDemoPhysicsSetup.cpp",
 		"../../Demos/BasicDemo/BasicDemoPhysicsSetup.h",
 		"../../Demos/CcdPhysicsDemo/CcdPhysicsSetup.cpp",
 		"../../Demos/CcdPhysicsDemo/CcdPhysicsSetup.h",
+		"../../Demos/SerializeDemo/SerializeSetup.cpp",
+		"../../Extras/Serialize/BulletFileLoader/bChunk.cpp",
+		"../../Extras/Serialize/BulletFileLoader/bDNA.cpp",
+		"../../Extras/Serialize/BulletFileLoader/bFile.cpp",
+		"../../Extras/Serialize/BulletFileLoader/btBulletFile.cpp",	
+		"../../Extras/Serialize/BulletWorldImporter/btBulletWorldImporter.cpp",
+		"../../Extras/Serialize/BulletWorldImporter/btWorldImporter.cpp",	
 		"../bullet2/ConstraintDemo/ConstraintPhysicsSetup.cpp",
   		"../bullet2/ConstraintDemo/ConstraintPhysicsSetup.h",
 		"../ImportURDFDemo/ImportURDFSetup.cpp",

Demos3/BasicDemoCustomOpenGL2/main.cpp

@@ -144,7 +144,7 @@ int main(int argc, char* argv[])
     }
     if (majorGlVersion>=3 && wci.m_openglVersion>=3)
     {
-        float retinaScale = 1.f;
+       // float retinaScale = 1.f;
 
 #ifndef __APPLE__
 #ifndef _WIN32
@@ -154,14 +154,15 @@ int main(int argc, char* argv[])
         glewInit();
 #endif
 
-    //we ned to call glGetError twice, because of some Ubuntu/Intel/OpenGL issue
+    //we need to call glGetError twice, because of some Ubuntu/Intel/OpenGL issue
 
-    GLuint err = glGetError();
-    err = glGetError();
-    btAssert(err==GL_NO_ERROR);
+   glGetError();
+   glGetError();
+
+    btAssert(glGetError()==GL_NO_ERROR);
 
 
-        retinaScale = window->getRetinaScale();
+        //retinaScale = window->getRetinaScale();
 
         //primRenderer = new GLPrimitiveRenderer(sWidth,sHeight);
         //sth_stash* font = initFont(primRenderer );

Demos3/ImportURDFDemo/ImportURDFSetup.cpp

@@ -364,21 +364,40 @@ void ImportUrdfDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge)
 	gravity[upAxis]=-9.8;
 
 	m_dynamicsWorld->setGravity(gravity);
-    int argc=0;
-    const char* filename="somefile.urdf";
+    //int argc=0;
+    const char* someFileName="r2d2.urdf";
 
     std::string xml_string;
+
+    const char* prefix[]={"./","./data/","../data/","../../data/","../../../data/","../../../../data/"};
+	int numPrefixes = sizeof(prefix)/sizeof(const char*);
+	char relativeFileName[1024];
+	FILE* f=0;
+	bool fileFound = false;
+	int result = 0;
+
+	for (int i=0;!f && i<numPrefixes;i++)
+	{
+		sprintf(relativeFileName,"%s%s",prefix[i],someFileName);
+		f = fopen(relativeFileName,"rb");
+		if (f)
+		{
+		    fileFound = true;
+		    break;
+		}
+	}
+	if (f)
+	{
+		fclose(f);
+	}
 
-    if (argc < 2){
-        std::cerr << "No URDF file name provided, using a dummy test URDF" << std::endl;
-
+    if (!fileFound){
+        std::cerr << "URDF file not found, using a dummy test URDF" << std::endl;
         xml_string = std::string(urdf_char);
 
     } else
     {
-
-
-        std::fstream xml_file(filename, std::fstream::in);
+        std::fstream xml_file(relativeFileName, std::fstream::in);
         while ( xml_file.good() )
         {
             std::string line;
@@ -407,7 +426,8 @@ void ImportUrdfDemo::initPhysics(GraphicsPhysicsBridge& gfxBridge)
     printTree(root_link);
     btTransform worldTrans;
 	worldTrans.setIdentity();
-
+
+    if (1)
     {
         URDF2BulletMappings mappings;
         URDFvisual2BulletCollisionShape(root_link, gfxBridge, worldTrans,m_dynamicsWorld,mappings);

Demos3/ImportURDFDemo/ImportURDFSetup.h

@@ -2,14 +2,14 @@
 #define IMPORT_URDF_SETUP_H
 
 
-#include "../../Demos/CommonRigidBodySetup.h"
+#include "../../Demos/CommonMultiBodySetup.h"
 
-class ImportUrdfDemo : public CommonRigidBodySetup
+class ImportUrdfDemo : public CommonMultiBodySetup
 {
 public:
     ImportUrdfDemo();
     virtual ~ImportUrdfDemo();
-    
+
 	virtual void initPhysics(GraphicsPhysicsBridge& gfxBridge);
 };
 

Demos3/SimpleOpenGL3/main.cpp

@@ -11,15 +11,13 @@ int main(int argc, char* argv[])
 {
     b3CommandLineArgs myArgs(argc,argv);
 
-	float dt = 1./120.f;
 
 	SimpleOpenGL3App* app = new SimpleOpenGL3App("SimpleOpenGL3App",1024,768);
 	app->m_instancingRenderer->setCameraDistance(13);
 	app->m_instancingRenderer->setCameraPitch(0);
 	app->m_instancingRenderer->setCameraTargetPosition(b3MakeVector3(0,0,0));
 
-	GLint err = glGetError();
-    assert(err==GL_NO_ERROR);
+    assert(glGetError()==GL_NO_ERROR);
 
     myArgs.GetCmdLineArgument("mp4_file",gVideoFileName);
     if (gVideoFileName)
@@ -40,8 +38,7 @@ int main(int argc, char* argv[])
             app->dumpNextFrameToPng(fileName);
         }
 
-		GLint err = glGetError();
-		assert(err==GL_NO_ERROR);
+		assert(glGetError()==GL_NO_ERROR);
 		app->m_instancingRenderer->init();
 		app->m_instancingRenderer->updateCamera();
 

Demos3/bullet2/BasicDemo/Bullet2RigidBodyDemo.cpp

@@ -21,6 +21,10 @@ struct MyGraphicsPhysicsBridge : public GraphicsPhysicsBridge
 	{
 	}
 	virtual void createRigidBodyGraphicsObject(btRigidBody* body, const btVector3& color)
+	{
+	    createCollisionObjectGraphicsObject(body,color);
+	}
+	virtual void createCollisionObjectGraphicsObject(btCollisionObject* body, const btVector3& color)
 	{
 		btCollisionShape* shape = body->getCollisionShape();
 		btTransform startTransform = body->getWorldTransform();
@@ -45,7 +49,7 @@ struct MyGraphicsPhysicsBridge : public GraphicsPhysicsBridge
 		}
 		case TRIANGLE_MESH_SHAPE_PROXYTYPE:
 		{
-			
+
 			break;
 		}
 		default:
@@ -149,7 +153,7 @@ struct MyGraphicsPhysicsBridge : public GraphicsPhysicsBridge
 	{
 		return m_glApp->m_parameterInterface;
 	}
-	
+
 	virtual void setUpAxis(int axis)
 	{
 		m_glApp->setUpAxis(axis);
@@ -233,7 +237,7 @@ btVector3	Bullet2RigidBodyDemo::getRayTo(int x,int y)
 	btVector3 rightOffset;
 	btVector3 cameraUp=btVector3(0,0,0);
 	cameraUp[m_glApp->getUpAxis()]=1;
-	
+
 	btVector3 vertical = cameraUp;
 
 	btVector3 hor;
@@ -291,7 +295,7 @@ bool	Bullet2RigidBodyDemo::mouseButtonCallback(int button, int state, float x, f
 
 			btVector3 rayFrom = camPos;
 			btVector3 rayTo = getRayTo(x,y);
-			
+
 			m_physicsSetup->pickBody(rayFrom, rayTo);
 
 

Demos3/bullet2/MultiBodyDemo/TestJointTorqueSetup.cpp

@@ -0,0 +1,248 @@
+//test addJointTorque
+#include "TestJointTorqueSetup.h"
+
+#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+TestJointTorqueSetup::TestJointTorqueSetup()
+{
+}
+
+TestJointTorqueSetup::~TestJointTorqueSetup()
+{
+
+}
+
+void TestJointTorqueSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
+{
+    int upAxis = 2;
+
+    btVector4 colors[4] =
+    {
+        btVector4(1,0,0,1),
+        btVector4(0,1,0,1),
+        btVector4(0,1,1,1),
+        btVector4(1,1,0,1),
+    };
+    int curColor = 0;
+
+
+
+    gfxBridge.setUpAxis(upAxis);
+
+	this->createEmptyDynamicsWorld();
+    gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld);
+    m_dynamicsWorld->getDebugDrawer()->setDebugMode(
+        //btIDebugDraw::DBG_DrawConstraints
+        +btIDebugDraw::DBG_DrawWireframe
+        +btIDebugDraw::DBG_DrawContactPoints
+        +btIDebugDraw::DBG_DrawAabb
+        );//+btIDebugDraw::DBG_DrawConstraintLimits);
+
+    //create a static ground object
+    if (0)
+        {
+            btVector3 groundHalfExtents(20,20,20);
+            groundHalfExtents[upAxis]=1.f;
+            btBoxShape* box = new btBoxShape(groundHalfExtents);
+            box->initializePolyhedralFeatures();
+
+            gfxBridge.createCollisionShapeGraphicsObject(box);
+            btTransform start; start.setIdentity();
+            btVector3 groundOrigin(0,0,0);
+            groundOrigin[upAxis]=-1.5;
+            start.setOrigin(groundOrigin);
+            btRigidBody* body =  createRigidBody(0,start,box);
+            btVector4 color = colors[curColor];
+			curColor++;
+			curColor&=3;
+            gfxBridge.createRigidBodyGraphicsObject(body,color);
+        }
+
+    {
+        bool floating = false;
+        bool damping = true;
+        bool gyro = true;
+        int numLinks = 5;
+        bool spherical = false;					//set it ot false -to use 1DoF hinges instead of 3DoF sphericals
+        bool canSleep = false;
+        bool selfCollide = false;
+        btVector3 linkHalfExtents(0.05, 0.37, 0.1);
+        btVector3 baseHalfExtents(0.05, 0.37, 0.1);
+
+        btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f);
+        //mbC->forceMultiDof();							//if !spherical, you can comment this line to check the 1DoF algorithm
+        //init the base
+        btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
+        float baseMass = 1.f;
+
+        if(baseMass)
+        {
+            btCollisionShape *pTempBox = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
+            pTempBox->calculateLocalInertia(baseMass, baseInertiaDiag);
+            delete pTempBox;
+        }
+
+        bool isMultiDof = false;
+        btMultiBody *pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep, isMultiDof);
+        m_multiBody = pMultiBody;
+        btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f);
+        pMultiBody->setBasePos(basePosition);
+        pMultiBody->setWorldToBaseRot(baseOriQuat);
+        btVector3 vel(0, 0, 0);
+    //	pMultiBody->setBaseVel(vel);
+
+        //init the links
+        btVector3 hingeJointAxis(1, 0, 0);
+        float linkMass = 1.f;
+        btVector3 linkInertiaDiag(0.f, 0.f, 0.f);
+
+        btCollisionShape *pTempBox = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));
+        pTempBox->calculateLocalInertia(linkMass, linkInertiaDiag);
+        delete pTempBox;
+
+        //y-axis assumed up
+        btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0);						//par body's COM to cur body's COM offset
+        btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0);							//cur body's COM to cur body's PIV offset
+        btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom;	//par body's COM to cur body's PIV offset
+
+        //////
+        btScalar q0 = 0.f * SIMD_PI/ 180.f;
+        btQuaternion quat0(btVector3(0, 1, 0).normalized(), q0);
+        quat0.normalize();
+        /////
+
+        for(int i = 0; i < numLinks; ++i)
+        {
+            if(!spherical)
+                pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false);
+            else
+                //pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false);
+                pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, false);
+        }
+
+        //pMultiBody->finalizeMultiDof();
+
+        btMultiBodyDynamicsWorld* world = m_dynamicsWorld;
+
+        ///
+        world->addMultiBody(pMultiBody);
+        btMultiBody* mbC = pMultiBody;
+        mbC->setCanSleep(canSleep);
+        mbC->setHasSelfCollision(selfCollide);
+        mbC->setUseGyroTerm(gyro);
+        //
+        if(!damping)
+        {
+            mbC->setLinearDamping(0.f);
+            mbC->setAngularDamping(0.f);
+        }else
+        {	mbC->setLinearDamping(0.1f);
+            mbC->setAngularDamping(0.9f);
+        }
+        //
+        btVector3 gravity(0,0,0);
+        //gravity[upAxis] = -9.81;
+        m_dynamicsWorld->setGravity(gravity);
+        //////////////////////////////////////////////
+        if(numLinks > 0)
+        {
+            btScalar q0 = 45.f * SIMD_PI/ 180.f;
+            if(!spherical)
+                if(mbC->isMultiDof())
+                    mbC->setJointPosMultiDof(0, &q0);
+                else
+                    mbC->setJointPos(0, q0);
+            else
+            {
+                btQuaternion quat0(btVector3(1, 1, 0).normalized(), q0);
+                quat0.normalize();
+                mbC->setJointPosMultiDof(0, quat0);
+            }
+        }
+        ///
+
+        btAlignedObjectArray<btQuaternion> world_to_local;
+        world_to_local.resize(pMultiBody->getNumLinks() + 1);
+
+        btAlignedObjectArray<btVector3> local_origin;
+        local_origin.resize(pMultiBody->getNumLinks() + 1);
+        world_to_local[0] = pMultiBody->getWorldToBaseRot();
+        local_origin[0] = pMultiBody->getBasePos();
+        double friction = 1;
+        {
+
+        //	float pos[4]={local_origin[0].x(),local_origin[0].y(),local_origin[0].z(),1};
+            float quat[4]={-world_to_local[0].x(),-world_to_local[0].y(),-world_to_local[0].z(),world_to_local[0].w()};
+
+
+            if (1)
+            {
+                btCollisionShape* box = new btBoxShape(baseHalfExtents);
+                gfxBridge.createCollisionShapeGraphicsObject(box);
+
+                btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(pMultiBody, -1);
+                col->setCollisionShape(box);
+
+                btTransform tr;
+                tr.setIdentity();
+                tr.setOrigin(local_origin[0]);
+                tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
+                col->setWorldTransform(tr);
+
+                world->addCollisionObject(col, 2,1+2);
+
+                btVector3 color(0.0,0.0,0.5);
+                gfxBridge.createCollisionObjectGraphicsObject(col,color);
+
+                col->setFriction(friction);
+                pMultiBody->setBaseCollider(col);
+
+            }
+        }
+
+
+        for (int i=0; i < pMultiBody->getNumLinks(); ++i)
+        {
+            const int parent = pMultiBody->getParent(i);
+            world_to_local[i+1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent+1];
+            local_origin[i+1] = local_origin[parent+1] + (quatRotate(world_to_local[i+1].inverse() , pMultiBody->getRVector(i)));
+        }
+
+
+        for (int i=0; i < pMultiBody->getNumLinks(); ++i)
+        {
+
+            btVector3 posr = local_origin[i+1];
+        //	float pos[4]={posr.x(),posr.y(),posr.z(),1};
+
+            float quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()};
+
+            btCollisionShape* box = new btBoxShape(linkHalfExtents);
+            gfxBridge.createCollisionShapeGraphicsObject(box);
+            btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i);
+
+            col->setCollisionShape(box);
+            btTransform tr;
+            tr.setIdentity();
+            tr.setOrigin(posr);
+            tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
+            col->setWorldTransform(tr);
+            col->setFriction(friction);
+            world->addCollisionObject(col,2,1+2);
+            btVector4 color = colors[curColor];
+			curColor++;
+			curColor&=3;
+            gfxBridge.createCollisionObjectGraphicsObject(col,color);
+
+
+            pMultiBody->getLink(i).m_collider=col;
+        }
+    }
+
+}
+
+void TestJointTorqueSetup::stepSimulation(float deltaTime)
+{
+    m_multiBody->addJointTorque(0, 10.0);
+    m_dynamicsWorld->stepSimulation(deltaTime);
+}
+

Demos3/bullet2/MultiBodyDemo/TestJointTorqueSetup.h

@@ -0,0 +1,21 @@
+#ifndef TEST_JOINT_TORQUE_SETUP_H
+#define TEST_JOINT_TORQUE_SETUP_H
+
+#include "../../../Demos/CommonMultiBodySetup.h"
+
+struct TestJointTorqueSetup : public CommonMultiBodySetup
+{
+    btMultiBody* m_multiBody;
+
+public:
+
+    TestJointTorqueSetup();
+    virtual ~TestJointTorqueSetup();
+
+    virtual void initPhysics(GraphicsPhysicsBridge& gfxBridge);
+
+    virtual void stepSimulation(float deltaTime);
+
+};
+#endif //TEST_JOINT_TORQUE_SETUP_H
+

btgui/OpenGLWindow/X11OpenGLWindow.cpp

@@ -29,7 +29,34 @@
 
 #include <pthread.h>
 
-GLint                   att[] = { GLX_RGBA, GLX_DEPTH_SIZE, 24, GLX_DOUBLEBUFFER, None };
+GLint                   att[] = { GLX_RGBA,
+GLX_DEPTH_SIZE, 24,
+GLX_RED_SIZE        , 8,
+GLX_GREEN_SIZE      , 8,
+GLX_BLUE_SIZE       , 8,
+GLX_ALPHA_SIZE      , 8,
+GLX_STENCIL_SIZE    , 8,
+GLX_DOUBLEBUFFER,
+None };
+/*
+ static int att[] =
+            {
+                GLX_RGBA, GLX_DEPTH_SIZE, 24, GLX_DOUBLEBUFFER, None
+
+              GLX_X_RENDERABLE    , True,
+              GLX_DRAWABLE_TYPE   , GLX_WINDOW_BIT,
+              GLX_RENDER_TYPE     , GLX_RGBA_BIT,
+              GLX_X_VISUAL_TYPE   , GLX_TRUE_COLOR,
+              GLX_RED_SIZE        , 8,
+              GLX_GREEN_SIZE      , 8,
+              GLX_BLUE_SIZE       , 8,
+              GLX_ALPHA_SIZE      , 8,
+              GLX_DEPTH_SIZE      , 24,
+              GLX_STENCIL_SIZE    , 8,
+              GLX_DOUBLEBUFFER    , True,
+              None
+            };
+*/
 static bool forceOpenGL3 = true;
 
 

src/BulletCollision/CollisionDispatch/btCollisionObject.cpp

@@ -4,8 +4,8 @@ Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
 
 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, 
+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.
@@ -35,6 +35,7 @@ btCollisionObject::btCollisionObject()
 		m_rollingFriction(0.0f),
 		m_internalType(CO_COLLISION_OBJECT),
 		m_userObjectPointer(0),
+		m_userIndex(-1),
 		m_hitFraction(btScalar(1.)),
 		m_ccdSweptSphereRadius(btScalar(0.)),
 		m_ccdMotionThreshold(btScalar(0.)),