add btMultiBodyConstraint::finalizeMultiDof API: if you add multi-body constraints to a multi-dof btMultiBody, before it has been finalized using the btMultiBody::finalizeMultiDof call,

then you have to manually call the btMultiBodyConstraint::finalizeMultiDof for all multi-dof multi body constraints.

Demos/Raytracer/RaytracerSetup.cpp

@@ -0,0 +1,318 @@
+
+#include "RaytracerSetup.h"
+#include "Bullet3AppSupport/Common2dCanvasInterface.h"
+//#include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h"
+#include "BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.h"
+//#include "BulletCollision/NarrowPhaseCollision/btGjkConvexCast.h"
+//#include "BulletCollision/NarrowPhaseCollision/btContinuousConvexCollision.h"
+
+struct RaytracerInternalData
+{
+	int m_canvasIndex;
+	struct Common2dCanvasInterface* m_canvas;
+
+	int m_width;
+	int m_height;
+
+	btAlignedObjectArray<btConvexShape*> m_shapePtr;
+	btAlignedObjectArray<btTransform> m_transforms;
+	btVoronoiSimplexSolver	m_simplexSolver;
+	btScalar m_pitch;
+	btScalar m_roll;
+	btScalar m_yaw;
+	
+	RaytracerInternalData()
+		:m_canvasIndex(-1),
+		m_canvas(0),
+		m_roll(0),
+		m_pitch(0),
+		m_yaw(0),
+		m_width(128),
+		m_height(128)
+	{
+		btConeShape* cone = new btConeShape(1,1);
+		btSphereShape* sphere = new btSphereShape(1);
+		btBoxShape* box = new btBoxShape (btVector3(1,1,1));
+		m_shapePtr.push_back(cone);
+		m_shapePtr.push_back(sphere);
+		m_shapePtr.push_back(box);
+		
+		updateTransforms();
+	}
+	void updateTransforms()
+	{
+		int numObjects = m_shapePtr.size();
+		m_transforms.resize(numObjects);
+		for (int i=0;i<numObjects;i++)
+		{
+			m_transforms[i].setIdentity();
+			btVector3	pos(0.f,0.f,-(2.5* numObjects * 0.5)+i*2.5f);
+			m_transforms[i].setIdentity();
+			m_transforms[i].setOrigin( pos );
+			btQuaternion orn;
+			if (i < 2)
+			{
+				orn.setEuler(m_yaw,m_pitch,m_roll);
+				m_transforms[i].setRotation(orn);
+			}
+		}
+		m_pitch += 0.005f;
+		m_yaw += 0.01f;
+	}
+
+};
+
+RaytracerPhysicsSetup::RaytracerPhysicsSetup()
+{
+	m_internalData = new RaytracerInternalData;
+}
+
+RaytracerPhysicsSetup::~RaytracerPhysicsSetup()
+{
+	delete m_internalData;
+}
+
+void RaytracerPhysicsSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
+{
+	//request a visual bitma/texture we can render to
+	
+	
+
+	m_internalData->m_canvas = gfxBridge.get2dCanvasInterface();
+	
+
+	if (m_internalData->m_canvas)
+	{
+		
+		m_internalData->m_canvasIndex = m_internalData->m_canvas->createCanvas("raytracer",m_internalData->m_width,m_internalData->m_height);
+		for (int i=0;i<m_internalData->m_width;i++)
+		{
+			for (int j=0;j<m_internalData->m_height;j++)
+			{
+				unsigned char red=255;
+				unsigned char green=255;
+				unsigned char blue=255;
+				unsigned char alpha=255;
+				m_internalData->m_canvas->setPixel(m_internalData->m_canvasIndex,i,j,red,green,blue,alpha);
+			}
+		}
+		m_internalData->m_canvas->refreshImageData(m_internalData->m_canvasIndex);
+
+		//int bitmapId = gfxBridge.createRenderBitmap(width,height);
+	}
+	
+
+
+
+}
+
+
+///worldRaytest performs a ray versus all objects in a collision world, returning true is a hit is found (filling in worldNormal and worldHitPoint)
+bool	RaytracerPhysicsSetup::worldRaytest(const btVector3& rayFrom,const btVector3& rayTo,btVector3& worldNormal,btVector3& worldHitPoint)
+{
+	return false;
+}
+
+
+///singleObjectRaytest performs a ray versus one collision shape, returning true is a hit is found (filling in worldNormal and worldHitPoint)
+bool	RaytracerPhysicsSetup::singleObjectRaytest(const btVector3& rayFrom,const btVector3& rayTo,btVector3& worldNormal,btVector3& worldHitPoint)
+{
+	return false;
+}
+
+	
+///lowlevelRaytest performs a ray versus convex shape, returning true is a hit is found (filling in worldNormal and worldHitPoint)
+bool	RaytracerPhysicsSetup::lowlevelRaytest(const btVector3& rayFrom,const btVector3& rayTo,btVector3& worldNormal,btVector3& worldHitPoint)
+{
+	btScalar closestHitResults = 1.f;
+
+	bool hasHit = false;
+	btConvexCast::CastResult rayResult;
+	btSphereShape pointShape(0.0f);
+	btTransform rayFromTrans;
+	btTransform rayToTrans;
+
+	rayFromTrans.setIdentity();
+	rayFromTrans.setOrigin(rayFrom);
+	rayToTrans.setIdentity();
+	rayToTrans.setOrigin(rayTo);
+
+	int numObjects = m_internalData->m_shapePtr.size();
+
+	for (int s=0;s<numObjects;s++)
+	{
+		
+		//do some culling, ray versus aabb
+		btVector3 aabbMin,aabbMax;
+		m_internalData->m_shapePtr[s]->getAabb( m_internalData->m_transforms[s],aabbMin,aabbMax);
+		btScalar hitLambda = 1.f;
+		btVector3 hitNormal;
+		btCollisionObject	tmpObj;
+		tmpObj.setWorldTransform( m_internalData->m_transforms[s]);
+
+
+		if (btRayAabb(rayFrom,rayTo,aabbMin,aabbMax,hitLambda,hitNormal))
+		{
+			//reset previous result
+
+			//choose the continuous collision detection method
+			btSubsimplexConvexCast convexCaster(&pointShape, m_internalData->m_shapePtr[s],&m_internalData->m_simplexSolver);
+			//btGjkConvexCast convexCaster(&pointShape,shapePtr[s],&simplexSolver);
+			//btContinuousConvexCollision convexCaster(&pointShape,shapePtr[s],&simplexSolver,0);
+
+			if (convexCaster.calcTimeOfImpact(rayFromTrans,rayToTrans, m_internalData->m_transforms[s], m_internalData->m_transforms[s],rayResult))
+			{
+				if (rayResult.m_fraction < closestHitResults)
+				{
+					closestHitResults = rayResult.m_fraction;
+
+					worldNormal =  m_internalData->m_transforms[s].getBasis() *rayResult.m_normal;
+					worldNormal.normalize();
+					hasHit = true;
+				}
+			}
+		}
+	}
+	
+
+	return hasHit;
+
+}
+
+void RaytracerPhysicsSetup::exitPhysics()
+{
+	
+	if (m_internalData->m_canvas && m_internalData->m_canvasIndex>=0)
+	{
+		m_internalData->m_canvas->destroyCanvas(m_internalData->m_canvasIndex);
+	}
+}
+
+void RaytracerPhysicsSetup::stepSimulation(float deltaTime)
+{
+
+	m_internalData->updateTransforms();
+
+
+		float top = 1.f;
+	float bottom = -1.f;
+	float nearPlane = 1.f;
+
+	float tanFov = (top-bottom)*0.5f / nearPlane;
+
+	float fov = 2.0 * atanf (tanFov);
+
+	btVector3 cameraPosition(5,0,0);
+	btVector3 cameraTargetPosition(0,0,0);
+
+	btVector3	rayFrom = cameraPosition;
+	btVector3 rayForward = cameraTargetPosition-cameraPosition;
+	rayForward.normalize();
+	float farPlane = 600.f;
+	rayForward*= farPlane;
+
+	btVector3 rightOffset;
+	btVector3 vertical(0.f,1.f,0.f);
+	btVector3 hor;
+	hor = rayForward.cross(vertical);
+	hor.normalize();
+	vertical = hor.cross(rayForward);
+	vertical.normalize();
+
+	float tanfov = tanf(0.5f*fov);
+
+	hor *= 2.f * farPlane * tanfov;
+	vertical *= 2.f * farPlane * tanfov;
+
+	btVector3 rayToCenter = rayFrom + rayForward;
+
+	btVector3 dHor = hor * 1.f/float(m_internalData->m_width);
+	btVector3 dVert = vertical * 1.f/float(m_internalData->m_height);
+
+	
+
+	
+	int	mode = 0;
+	int x,y;
+
+	for (x=0;x<m_internalData->m_width;x++)
+	{
+		for (int y=0;y<m_internalData->m_height;y++)
+		{
+			btVector4 rgba(0,0,0,0);
+			btVector3 rayTo = rayToCenter - 0.5f * hor + 0.5f * vertical;
+			rayTo += x * dHor;
+			rayTo -= y * dVert;
+			btVector3	worldNormal(0,0,0);
+			btVector3	worldPoint(0,0,0);
+
+
+
+			bool hasHit = false;
+			int mode = 0;
+			switch (mode)
+			{
+			case 0:
+				hasHit = lowlevelRaytest(rayFrom,rayTo,worldNormal,worldPoint);
+				break;
+			case 1:
+				hasHit = singleObjectRaytest(rayFrom,rayTo,worldNormal,worldPoint);
+				break;
+			case 2:
+				hasHit = worldRaytest(rayFrom,rayTo,worldNormal,worldPoint);
+				break;
+			default:
+				{
+				}
+			}
+
+			if (hasHit)
+			{
+				float lightVec0 = worldNormal.dot(btVector3(0,-1,-1));//0.4f,-1.f,-0.4f));
+				float lightVec1= worldNormal.dot(btVector3(-1,0,-1));//-0.4f,-1.f,-0.4f));
+
+
+				rgba = btVector4(lightVec0,lightVec1,0,1.f);
+				rgba.setMin(btVector3(1,1,1));
+				rgba.setMax(btVector3(0.2,0.2,0.2));
+				rgba[3] = 1.f;
+				unsigned char red = rgba[0] * 255;
+				unsigned char green = rgba[1] * 255;
+				unsigned char blue = rgba[2] * 255;
+				unsigned char alpha=255;
+				m_internalData->m_canvas->setPixel(m_internalData->m_canvasIndex,x,y,red,green,blue,alpha);
+				
+			} else
+            {
+			//	btVector4 rgba = raytracePicture->getPixel(x,y);
+            }
+			if (!rgba.length2())
+			{
+				m_internalData->m_canvas->setPixel(m_internalData->m_canvasIndex,x,y,255,0,0,255);
+			}
+		}
+	}
+	m_internalData->m_canvas->refreshImageData(m_internalData->m_canvasIndex);
+}
+
+
+void    RaytracerPhysicsSetup::debugDraw(int debugDrawFlags)
+{
+}
+
+bool RaytracerPhysicsSetup::pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)
+{
+	return false;
+}
+bool RaytracerPhysicsSetup::movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)
+{
+	return false;
+}
+void RaytracerPhysicsSetup::removePickingConstraint()
+{
+}
+
+
+void RaytracerPhysicsSetup::syncPhysicsToGraphics(GraphicsPhysicsBridge& gfxBridge)
+{
+}

Demos/Raytracer/RaytracerSetup.h

@@ -0,0 +1,42 @@
+#ifndef RAYTRACER_SETUP_H
+#define RAYTRACER_SETUP_H
+
+
+#include "Bullet3AppSupport/CommonRigidBodySetup.h"
+
+struct RaytracerPhysicsSetup : public CommonPhysicsSetup
+{
+	
+	struct RaytracerInternalData* m_internalData;
+
+	RaytracerPhysicsSetup();
+
+	virtual ~RaytracerPhysicsSetup();
+
+	virtual void initPhysics(GraphicsPhysicsBridge& gfxBridge);
+
+	virtual void exitPhysics();
+
+	virtual void stepSimulation(float deltaTime);
+
+    virtual void    debugDraw(int debugDrawFlags);
+
+	virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld);
+	virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld);
+	virtual void removePickingConstraint();
+
+	virtual void syncPhysicsToGraphics(GraphicsPhysicsBridge& gfxBridge);
+
+		///worldRaytest performs a ray versus all objects in a collision world, returning true is a hit is found (filling in worldNormal and worldHitPoint)
+	bool	worldRaytest(const btVector3& rayFrom,const btVector3& rayTo,btVector3& worldNormal,btVector3& worldHitPoint);
+
+	///singleObjectRaytest performs a ray versus one collision shape, returning true is a hit is found (filling in worldNormal and worldHitPoint)
+	bool	singleObjectRaytest(const btVector3& rayFrom,const btVector3& rayTo,btVector3& worldNormal,btVector3& worldHitPoint);
+	
+	///lowlevelRaytest performs a ray versus convex shape, returning true is a hit is found (filling in worldNormal and worldHitPoint)
+	bool	lowlevelRaytest(const btVector3& rayFrom,const btVector3& rayTo,btVector3& worldNormal,btVector3& worldHitPoint);
+
+
+};
+
+#endif //RAYTRACER_SETUP_H

Demos3/AllBullet2Demos/BulletDemoEntries.h

@@ -22,7 +22,7 @@
 #include "../ImportObjDemo/ImportObjSetup.h"
 #include "../ImportSTLDemo/ImportSTLSetup.h"
 #include "../ImportColladaDemo/ImportColladaSetup.h"
-
+#include "../Demos/Raytracer/RaytracerSetup.h"
 #include "../../Demos/SerializeDemo/SerializeSetup.h"
 #include "../bullet2/MultiBodyDemo/TestJointTorqueSetup.h"
 #include "../bullet2/MultiBodyDemo/MultiBodyVehicle.h"
@@ -62,6 +62,9 @@ MYCREATEFUNC2(ImportObjCreateFunc,ImportObjSetup);
 MYCREATEFUNC2(ImportSTLCreateFunc,ImportSTLSetup);
 MYCREATEFUNC(CoordinateFrameDemoPhysics);
 
+MYCREATEFUNC(RaytracerPhysics);
+//Bullet2RigidBodyDemo
+
 
 static BulletDemoInterface* MyImportColladaCreateFunc(CommonGraphicsApp* app)
 {
@@ -88,7 +91,7 @@ static BulletDemoEntry allDemos[]=
 	{1,"SupportFunc", &MySupportFuncDemo::CreateFunc},
 	
 	{0,"API Demos", 0},
-
+	{1,"Raytracer",RaytracerPhysicsCreateFunc},
 	{1,"BasicDemo",BasicDemo::MyCreateFunc},
 	{ 1, "CcdDemo", CcdPhysicsCreateFunc },
 	{ 1, "Kinematic", KinematicObjectCreateFunc },

Demos3/AllBullet2Demos/main.cpp

@@ -31,6 +31,7 @@
 #include "Bullet3AppSupport/GwenProfileWindow.h"
 #include "Bullet3AppSupport/GwenTextureWindow.h"
 #include "Bullet3AppSupport/GraphingTexture.h"
+#include "Bullet3AppSupport/Common2dCanvasInterface.h"
 
 #include "OpenGLWindow/SimpleCamera.h"
 #include "OpenGLWindow/SimpleOpenGL2Renderer.h"
@@ -393,6 +394,71 @@ void fileOpenCallback()
  }
 }
 
+#define MAX_GRAPH_WINDOWS 5
+
+struct QuickCanvas : public Common2dCanvasInterface
+{
+	GL3TexLoader* m_myTexLoader;
+
+	MyGraphWindow* m_gw[MAX_GRAPH_WINDOWS];
+	GraphingTexture* m_gt[MAX_GRAPH_WINDOWS];
+	int m_curNumGraphWindows;
+
+	QuickCanvas(GL3TexLoader* myTexLoader)
+		:m_myTexLoader(myTexLoader),
+		m_curNumGraphWindows(0)
+	{
+		for (int i=0;i<MAX_GRAPH_WINDOWS;i++)
+		{
+			m_gw[i] = 0;
+			m_gt[i] = 0;
+		}
+	}
+	virtual ~QuickCanvas() {}
+	virtual int createCanvas(const char* canvasName, int width, int height)
+	{
+		if (m_curNumGraphWindows<MAX_GRAPH_WINDOWS)
+		{
+			//find a slot
+			int slot = 0;//hardcoded for now
+			m_curNumGraphWindows++;
+
+			MyGraphInput input(gui->getInternalData());
+			input.m_width=width;
+			input.m_height=height;
+			input.m_xPos = 300;
+			input.m_yPos = height-input.m_height;
+			input.m_name=canvasName;
+			input.m_texName = canvasName;
+			m_gt[slot] = new GraphingTexture;
+			m_gt[slot]->create(width,height);
+			int texId = m_gt[slot]->getTextureId();
+			m_myTexLoader->m_hashMap.insert(canvasName, texId);
+			m_gw[slot] = setupTextureWindow(input);
+			
+			return slot;
+		}
+		return -1;
+	}
+	virtual void destroyCanvas(int canvasId)
+	{
+		btAssert(canvasId==0);//hardcoded to zero for now, only a single canvas
+		btAssert(m_curNumGraphWindows==1);
+		destroyTextureWindow(m_gw[canvasId]);
+		m_curNumGraphWindows--;
+	}
+	virtual void setPixel(int canvasId, int x, int y, unsigned char red, unsigned char green,unsigned char blue, unsigned char alpha)
+	{
+		btAssert(canvasId==0);//hardcoded
+		btAssert(m_curNumGraphWindows==1);
+		m_gt[canvasId]->setPixel(x,y,red,green,blue,alpha);
+	}
+	virtual void refreshImageData(int canvasId)
+	{
+		m_gt[canvasId]->uploadImageData();
+	}
+};
+
 extern float shadowMapWorldSize;
 int main(int argc, char* argv[])
 {
@@ -522,7 +588,8 @@ int main(int argc, char* argv[])
 	//destroyTextureWindow(gw);
 #endif 
 	s_parameterInterface  = app->m_parameterInterface = new GwenParameterInterface(gui->getInternalData());
-	
+	app->m_2dCanvasInterface = new QuickCanvas(myTexLoader);
+
 	//gui->getInternalData()->m_demoPage;
 
 	int numDemos = sizeof(allDemos)/sizeof(BulletDemoEntry);

Demos3/ImportURDFDemo/ImportURDFSetup.cpp

@@ -5,6 +5,7 @@
 #include "../ImportSTLDemo/LoadMeshFromSTL.h"
 #include "../ImportColladaDemo/LoadMeshFromCollada.h"
 #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
 #include "Bullet3Common/b3FileUtils.h"
 #include "BulletCollision/CollisionShapes/btShapeHull.h"//to create a tesselation of a generic btConvexShape
 
@@ -16,10 +17,31 @@ static bool enableConstraints = true;//false;
 const char* fileNames[] = 
 {
 	"r2d2.urdf",
+	"r2d2.urdf",
+	
 };
 
+#define MAX_NUM_MOTORS 1024
+
+struct ImportUrdfInternalData
+{
+	ImportUrdfInternalData()
+		:m_numMotors(0)
+	{
+	}
+	
+	float m_motorTargetVelocities[MAX_NUM_MOTORS];
+	btMultiBodyJointMotor* m_jointMotors [MAX_NUM_MOTORS];
+	int m_numMotors;
+};
+
+
+
+
 ImportUrdfSetup::ImportUrdfSetup()
 {
+	m_data = new ImportUrdfInternalData;
+
 	static int count = 0;
 
 	sprintf(m_fileName,fileNames[count++]);
@@ -32,7 +54,7 @@ ImportUrdfSetup::ImportUrdfSetup()
 
 ImportUrdfSetup::~ImportUrdfSetup()
 {
-
+	delete m_data;
 }
 
 static btVector4 colors[4] =
@@ -684,7 +706,7 @@ btCollisionShape* convertURDFToCollisionShape(const Collision* visual, const cha
     }
 	return shape;
 }
-void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhysicsBridge& gfxBridge, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1, URDF2BulletMappings& mappings, const char* pathPrefix)
+void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhysicsBridge& gfxBridge, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world1, URDF2BulletMappings& mappings, const char* pathPrefix, ImportUrdfInternalData* data)
 {
     //btCollisionShape* shape = 0;
 
@@ -952,6 +974,7 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
                         {
 							if (mappings.m_createMultiBody)
 							{
+								printf("Revolute joint (btMultiBody)\n");
 								//todo: adjust the center of mass transform and pivot axis properly
 								/*mappings.m_bulletMultiBody->setupRevolute(
 									linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1, 
@@ -969,11 +992,35 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
 								-offsetInB.getOrigin(),
 								disableParentCollision);
 								//linkInfo->m_localVisualFrame.setIdentity();
+
+								{
+									if (data->m_numMotors<MAX_NUM_MOTORS)
+									{
+										const char* jointName = pj->name.c_str();
+										char motorName[1024];
+										sprintf(motorName,"%s q'", jointName);
+
+										float* motorVel = &data->m_motorTargetVelocities[data->m_numMotors];
+										*motorVel = 0.f;
+
+										SliderParams slider(motorName,motorVel);
+										slider.m_minVal=-4;
+										slider.m_maxVal=4;
+										gfxBridge.getParameterInterface()->registerSliderFloatParameter(slider);
+										float maxMotorImpulse = 0.1f;
+
+										btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mappings.m_bulletMultiBody,linkIndex-1,0,0,maxMotorImpulse);
+										data->m_jointMotors[data->m_numMotors]=motor;
+										world1->addMultiBodyConstraint(motor);
+										data->m_numMotors++;
+									}
+								}
 								
 							} else
 							{
 								//only handle principle axis at the moment, 
 								//@todo(erwincoumans) orient the constraint for non-principal axis
+								printf("Hinge joint (btGeneric6DofSpring2Constraint)\n");
 								btVector3 axis(pj->axis.x,pj->axis.y,pj->axis.z);
 								int principleAxis = axis.closestAxis();
 								switch (principleAxis)
@@ -1026,11 +1073,7 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
                         {
 							if (mappings.m_createMultiBody)
 							{
-								//mappings.m_bulletMultiBody->setupPrismatic(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1,
+								printf("Prismatic joint (btMultiBody)\n");
-								//	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(),
@@ -1038,10 +1081,12 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
 								disableParentCollision);
 
 								
+								
 
 							} else
 							{
 								
+								printf("Slider joint (btGeneric6DofSpring2Constraint)\n");
 								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);
@@ -1145,7 +1190,7 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
     {
         if (*child)
         {
-            URDFvisual2BulletCollisionShape(*child,gfxBridge, linkTransformInWorldSpace, world1,mappings,pathPrefix);
+            URDFvisual2BulletCollisionShape(*child,gfxBridge, linkTransformInWorldSpace, world1,mappings,pathPrefix, data);
 
         }
         else
@@ -1236,18 +1281,26 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
 	
 	int numJoints = (*robot).m_numJoints;
 
-	static bool useFeatherstone = false;
+	static bool useFeatherstone = true;
     {
         URDF2BulletMappings mappings;
 		mappings.m_createMultiBody = useFeatherstone;
 		mappings.m_totalNumJoints = numJoints;
-        URDFvisual2BulletCollisionShape(root_link, gfxBridge, identityTrans,m_dynamicsWorld,mappings,pathPrefix);
+        URDFvisual2BulletCollisionShape(root_link, gfxBridge, identityTrans,m_dynamicsWorld,mappings,pathPrefix,m_data);
 		if (useFeatherstone)
 		{
 			btMultiBody* mb = mappings.m_bulletMultiBody;
 			mb->setHasSelfCollision(false);
-			mb->finalizeMultiDof();
+			if (mb->isMultiDof())
+			{
+				mb->finalizeMultiDof();
+			}
 			m_dynamicsWorld->addMultiBody(mb);
+
+			for (int i=0;i<m_data->m_numMotors;i++)
+			{
+				m_data->m_jointMotors[i]->finalizeMultiDof();
+			}
 		}
     }
 
@@ -1284,7 +1337,14 @@ void ImportUrdfSetup::stepSimulation(float deltaTime)
 {
 	if (m_dynamicsWorld)
 	{
+		//set the new target velocities
+		for (int i=0;i<m_data->m_numMotors;i++)
+		{
+			m_data->m_jointMotors[i]->setVelocityTarget(m_data->m_motorTargetVelocities[i]);
+		}
 		//the maximal coordinates/iterative MLCP solver requires a smallish timestep to converge
-		m_dynamicsWorld->stepSimulation(deltaTime,10,1./240.);
+		int actualSteps = m_dynamicsWorld->stepSimulation(1./240.,0);//deltaTime,10,1./240.);
+		//int actualSteps = m_dynamicsWorld->stepSimulation(deltaTime,1000,1./3000.f);//240.);
+		printf("deltaTime %f took actualSteps = %d\n",deltaTime,actualSteps);
 	}
 }

Demos3/ImportURDFDemo/ImportURDFSetup.h

@@ -7,6 +7,7 @@
 class ImportUrdfSetup : public CommonMultiBodySetup
 {
     char m_fileName[1024];
+	struct ImportUrdfInternalData* m_data;
     
 public:
     ImportUrdfSetup();

btgui/Bullet3AppSupport/Bullet2RigidBodyDemo.cpp

@@ -180,6 +180,10 @@ struct MyGraphicsPhysicsBridge : public GraphicsPhysicsBridge
 
 	}
 
+	virtual struct Common2dCanvasInterface*	get2dCanvasInterface()
+	{
+		return m_glApp->m_2dCanvasInterface;
+	}
 	virtual CommonParameterInterface* getParameterInterface()
 	{
 		return m_glApp->m_parameterInterface;

btgui/Bullet3AppSupport/Common2dCanvasInterface.h

@@ -0,0 +1,15 @@
+#ifndef COMMON_2D_CANVAS_INTERFACE_H
+#define COMMON_2D_CANVAS_INTERFACE_H
+
+struct Common2dCanvasInterface
+{
+	virtual ~Common2dCanvasInterface() {}
+	virtual int createCanvas(const char* canvasName, int width, int height)=0;
+	virtual void destroyCanvas(int canvasId)=0;
+	virtual void setPixel(int canvasId, int x, int y, unsigned char red, unsigned char green,unsigned char blue, unsigned char alpha)=0;
+	virtual void refreshImageData(int canvasId)=0;
+
+};
+
+#endif //COMMON_2D_CANVAS_INTERFACE_H
+

btgui/Bullet3AppSupport/CommonPhysicsSetup.h

@@ -38,6 +38,10 @@ struct GraphicsPhysicsBridge
 
 	virtual int registerGraphicsInstance(int shapeIndex, const float* position, const float* quaternion, const float* color, const float* scaling) { return -1;}
 
+	virtual struct Common2dCanvasInterface* get2dCanvasInterface()
+	{
+		return 0;
+	}
 	
 	virtual CommonParameterInterface* getParameterInterface()
 	{
@@ -65,6 +69,7 @@ public:
 
     virtual void    debugDraw(int debugDrawFlags)=0;
 
+	
 	virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) = 0;
 	virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)=0;
 	virtual void removePickingConstraint() = 0;

btgui/OpenGLWindow/CommonGraphicsApp.h

@@ -25,7 +25,8 @@ struct CommonGraphicsApp
 	CommonGraphicsApp()
 		:m_window(0),
 		m_renderer(0),
-		m_parameterInterface(0)
+		m_parameterInterface(0),
+		m_2dCanvasInterface(0)
 	{
 	}
 	virtual ~CommonGraphicsApp()
@@ -35,6 +36,7 @@ struct CommonGraphicsApp
 	class b3gWindowInterface*	m_window;
 	struct CommonRenderInterface*	m_renderer;
 	struct CommonParameterInterface*	m_parameterInterface;
+	struct Common2dCanvasInterface*	m_2dCanvasInterface;
 
 	virtual void drawGrid(DrawGridData data=DrawGridData()) = 0;
 	virtual void setUpAxis(int axis) = 0;

src/BulletDynamics/Featherstone/btMultiBody.cpp

@@ -2171,7 +2171,7 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar
 
     // Y_i (scratch), invD_i (cached)
     const btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0;
-	btScalar * Y = r_ptr;
+	btScalar * Y = r_ptr; 
 	////////////////
 	//aux variables
 	static btScalar invD_times_Y[6];							//D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies

src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp

@@ -15,21 +15,25 @@ btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bod
 	m_isUnilateral(isUnilateral),
 	m_maxAppliedImpulse(100)
 {
-	
+	finalizeMultiDof();
-	if(bodyA)
+}
+
+void btMultiBodyConstraint::finalizeMultiDof()
+{
+	if(m_bodyA)
 	{
-		if(bodyA->isMultiDof())
+		if(m_bodyA->isMultiDof())
-			m_jacSizeA = (6 + bodyA->getNumDofs());
+			m_jacSizeA = (6 + m_bodyA->getNumDofs());
 		else
-			m_jacSizeA = (6 + bodyA->getNumLinks());
+			m_jacSizeA = (6 + m_bodyA->getNumLinks());
 	}
 
-	if(bodyB)
+	if(m_bodyB)
 	{
-		if(bodyB->isMultiDof())
+		if(m_bodyB->isMultiDof())
-			m_jacSizeBoth = m_jacSizeA + 6 + bodyB->getNumDofs();
+			m_jacSizeBoth = m_jacSizeA + 6 + m_bodyB->getNumDofs();
 		else
-			m_jacSizeBoth = m_jacSizeA + 6 + bodyB->getNumLinks();
+			m_jacSizeBoth = m_jacSizeA + 6 + m_bodyB->getNumLinks();
 	}
 	else
 		m_jacSizeBoth = m_jacSizeA;

src/BulletDynamics/Featherstone/btMultiBodyConstraint.h

@@ -82,7 +82,7 @@ public:
 	btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral);
 	virtual ~btMultiBodyConstraint();
 
-
+	void finalizeMultiDof();
 
 	virtual int getIslandIdA() const =0;
 	virtual int getIslandIdB() const =0;