Merge pull request #309 from erwincoumans/master

minor fixes (GL2, preSwapFileNameOut), improved URDF/btMultiBody (work in progress), basic debug drawing for btMultiBody

Demos3/AllBullet2Demos/main.cpp

@@ -8,7 +8,7 @@
 #include "OpenGLWindow/SimpleOpenGL3App.h"
 #endif
 
-
+#include "OpenGLWindow/CommonRenderInterface.h"
 #ifdef __APPLE__
 #include "OpenGLWindow/MacOpenGLWindow.h"
 #else
@@ -34,7 +34,7 @@
 #include "Bullet3AppSupport/GwenProfileWindow.h"
 #include "Bullet3AppSupport/GwenTextureWindow.h"
 #include "Bullet3AppSupport/GraphingTexture.h"
-#include "OpenGLWindow/CommonRenderInterface.h"
+
 #include "OpenGLWindow/SimpleCamera.h"
 
 CommonGraphicsApp* app=0;
@@ -140,6 +140,10 @@ struct TestRenderer : public CommonRenderInterface
 	{
 		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;
@@ -192,6 +196,26 @@ struct TestRenderer : public CommonRenderInterface
 	{
 	}
 
+	
+	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;
@@ -210,9 +234,10 @@ extern bool useShadowMap;
 static bool visualWireframe=false;
 static bool renderVisualGeometry=true;
 static bool renderGrid = true;
-
+int gDebugDrawFlags = 0;
-static bool pauseSimulation=false;//true;
+static bool pauseSimulation=false;
 int midiBaseIndex = 176;
+extern bool gDisableDeactivation;
 
 //#include <float.h>
 //unsigned int fp_control_state = _controlfp(_EM_INEXACT, _MCW_EM);
@@ -244,9 +269,28 @@ void MyKeyboardCallback(int key, int state)
 	//if (handled)
 	//	return;
 
+	if (key=='a' && state)
+	{
+		gDebugDrawFlags ^= btIDebugDraw::DBG_DrawAabb;
+	}
+	if (key=='c' && state)
+	{
+		gDebugDrawFlags ^= btIDebugDraw::DBG_DrawConstraints;
+		gDebugDrawFlags ^= btIDebugDraw::DBG_DrawContactPoints;
+	}
+	if (key == 'd' && state)
+	{
+		gDebugDrawFlags ^= btIDebugDraw::DBG_NoDeactivation;
+		gDisableDeactivation = ((gDebugDrawFlags & btIDebugDraw::DBG_NoDeactivation) != 0);
+	}
+	if (key=='l' && state)
+	{
+		gDebugDrawFlags ^= btIDebugDraw::DBG_DrawConstraintLimits;
+	}
 	if (key=='w' && state)
 	{
 		visualWireframe=!visualWireframe;
+		gDebugDrawFlags ^= btIDebugDraw::DBG_DrawWireframe;
 	}
 	if (key=='v' && state)
 	{
@@ -768,7 +812,7 @@ int main(int argc, char* argv[])
             }
             {
 				glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
-                sCurrentDemo->physicsDebugDraw();
+                sCurrentDemo->physicsDebugDraw(gDebugDrawFlags);
             }
 		}
 

Demos3/FiniteElementMethod/FiniteElementDemo.cpp

@@ -317,7 +317,7 @@ void	FiniteElementDemo::renderScene()
 	m_app->m_renderer->renderScene();
 }
 
-void	FiniteElementDemo::physicsDebugDraw()
+void	FiniteElementDemo::physicsDebugDraw(int debugDrawFlags)
 {
     {
 		btAlignedObjectArray<btVector3FloatData> m_linePoints;

Demos3/FiniteElementMethod/FiniteElementDemo.h

@@ -50,7 +50,7 @@ public:
     virtual void	renderScene();
 
 	
-    virtual void	physicsDebugDraw();
+    virtual void	physicsDebugDraw(int debugDrawFlags);
     virtual bool	mouseMoveCallback(float x,float y);
     virtual bool	mouseButtonCallback(int button, int state, float x, float y);
     virtual bool	keyboardCallback(int key, int state);

Demos3/Geometry/DistributePoints.h

@@ -181,7 +181,7 @@ public:
 		
         
     }
-    virtual void	physicsDebugDraw()
+    virtual void	physicsDebugDraw(int debugDrawFlags)
     {
    	
 		int lineWidth = 1;

Demos3/Geometry/SphereCreation.h

@@ -168,7 +168,7 @@ public:
         m_app->m_renderer->renderScene();
         
     }
-    virtual void	physicsDebugDraw()
+    virtual void	physicsDebugDraw(int debugDrawFlags)
     {
    		int lineWidth = 1;
 		int pointSize = 2;

Demos3/ImportURDFDemo/ImportURDFSetup.cpp

@@ -12,9 +12,10 @@ static int bodyCollisionFilterMask=btBroadphaseProxy::AllFilter&(~btBroadphasePr
 static bool enableConstraints = true;//false;
 
 
+
 ImportUrdfSetup::ImportUrdfSetup()
 {
-    sprintf(m_fileName,"r2d2.urdf");//sphere2.urdf");//
+    sprintf(m_fileName,"r2d2.urdf");
 }
 
 ImportUrdfSetup::~ImportUrdfSetup()
@@ -86,12 +87,26 @@ struct URDF_LinkInformation
 {
     const Link* m_thisLink;
 	int m_linkIndex;
-	int m_parentIndex;
+	//int m_parentIndex;
 
     btTransform m_localInertialFrame;
-    btTransform m_localVisualFrame;
+    //btTransform m_localVisualFrame;
+	btTransform m_bodyWorldTransform;
+	btVector3 m_localInertiaDiagonal;
+	btScalar m_mass;
 
+	btCollisionShape* m_collisionShape;
     btRigidBody* m_bulletRigidBody;
+
+	URDF_LinkInformation()
+		:m_thisLink(0),
+		m_linkIndex(-2),
+		//m_parentIndex(-2),
+		m_collisionShape(0),
+		m_bulletRigidBody(0)
+	{
+
+	}
 	virtual ~URDF_LinkInformation()
 	{
         printf("~\n");
@@ -107,17 +122,30 @@ struct URDF_JointInformation
 struct URDF2BulletMappings
 {
     btHashMap<btHashPtr /*to Link*/, URDF_LinkInformation*> m_link2rigidbody;
-    btHashMap<btHashPtr /*to Joint*/, btTypedConstraint*> m_joint2Constraint;
+    //btHashMap<btHashPtr /*to Joint*/, btTypedConstraint*> m_joint2Constraint;
 
-	btAlignedObjectArray<btTransform>		m_linkLocalInertiaTransforms;//Body transform is in center of mass, aligned with Principal Moment Of Inertia;
+	//btAlignedObjectArray<btTransform>		m_linkLocalInertiaTransforms;//Body transform is in center of mass, aligned with Principal Moment Of Inertia;
 	btAlignedObjectArray<btScalar>			m_linkMasses;
-	btAlignedObjectArray<btVector3>			m_linkLocalDiagonalInertiaTensors;
+	//btAlignedObjectArray<btVector3>			m_linkLocalDiagonalInertiaTensors;
-	btAlignedObjectArray<btTransform>		m_jointTransforms;//for root, it is identity
+	
-	btAlignedObjectArray<int>				m_parentIndices;//for root, it is identity
+	//btAlignedObjectArray<int>				m_parentIndices;//for root, it is identity
-	btAlignedObjectArray<btVector3>			m_jointAxisArray;
+	//btAlignedObjectArray<btVector3>			m_jointAxisArray;
-	btAlignedObjectArray<btTransform>			m_jointOffsetInParent;
+	//btAlignedObjectArray<btTransform>			m_jointOffsetInParent;
-	btAlignedObjectArray<btTransform>			m_jointOffsetInChild;
+	//btAlignedObjectArray<btTransform>			m_jointOffsetInChild;
-	btAlignedObjectArray<int>				m_jointTypeArray;
+	//btAlignedObjectArray<int>				m_jointTypeArray;
+
+
+	
+	bool m_createMultiBody;
+	int m_totalNumJoints;
+	btMultiBody*	m_bulletMultiBody;
+
+	URDF2BulletMappings()
+		:m_createMultiBody(false),
+		m_totalNumJoints(0),
+		m_bulletMultiBody(0)
+	{
+	}
 
 };
 enum MyFileType
@@ -126,7 +154,8 @@ enum MyFileType
 	FILE_COLLADA=2
 };
 
-btCollisionShape* convertVisualToCollisionShape(const Collision* visual, const char* pathPrefix)
+template <typename T>
+btCollisionShape* convertURDFToCollisionShape(const T* visual, const char* pathPrefix)
 {
 	btCollisionShape* shape = 0;
 
@@ -151,6 +180,8 @@ btCollisionShape* convertVisualToCollisionShape(const Collision* visual, const c
             }
             btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
             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);
@@ -164,6 +195,7 @@ btCollisionShape* convertVisualToCollisionShape(const Collision* visual, const c
             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);
             shape = boxShape;
 			shape ->setMargin(0.001);
             break;
@@ -348,32 +380,46 @@ btCollisionShape* convertVisualToCollisionShape(const Collision* visual, const c
     }
 	return shape;
 }
-void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhysicsBridge& gfxBridge, const btTransform& parentTransformInWorldSpace, btDiscreteDynamicsWorld* 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)
 {
-    btCollisionShape* shape = 0;
+    //btCollisionShape* shape = 0;
 
 	btTransform linkTransformInWorldSpace;
 	linkTransformInWorldSpace.setIdentity();
 
-	btScalar mass = 1;
+	btScalar mass = 0;
 	btTransform inertialFrame;
 	inertialFrame.setIdentity();
     const Link* parentLink = (*link).getParent();
 	URDF_LinkInformation* pp = 0;
+	int linkIndex =  mappings.m_linkMasses.size();
+	btVector3 localInertiaDiagonal(0,0,0);
+
+	int parentIndex = -1;
+
+	
+	if (parentLink)
+	{
+		parentIndex = parentLink->m_link_index;
+		btAssert(parentIndex>=0);
+	}
 
     {
 		URDF_LinkInformation** ppRigidBody = mappings.m_link2rigidbody.find(parentLink);
 		if (ppRigidBody)
 		{
 			pp = (*ppRigidBody);
-			btRigidBody* parentRigidBody = pp->m_bulletRigidBody;
+			btTransform tr = pp->m_bodyWorldTransform;
-			btTransform tr = parentRigidBody->getWorldTransform();
 			printf("rigidbody origin (COM) of link(%s) parent(%s): %f,%f,%f\n",(*link).name.c_str(), parentLink->name.c_str(), tr.getOrigin().x(), tr.getOrigin().y(), tr.getOrigin().z());
 		}
 	}
+
+	(*link).m_link_index = linkIndex;
+
 	if ((*link).inertial)
 	{
 		mass = (*link).inertial->mass;
+		localInertiaDiagonal.setValue((*link).inertial->ixx,(*link).inertial->iyy,(*link).inertial->izz);
 		inertialFrame.setOrigin(btVector3((*link).inertial->origin.position.x,(*link).inertial->origin.position.y,(*link).inertial->origin.position.z));
 		inertialFrame.setRotation(btQuaternion((*link).inertial->origin.rotation.x,(*link).inertial->origin.rotation.y,(*link).inertial->origin.rotation.z,(*link).inertial->origin.rotation.w));
 	}
@@ -394,54 +440,123 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
 	} else
 	{
 		linkTransformInWorldSpace = parentTransformInWorldSpace;
+
+		
+		
 	}
 
 
     {
         printf("converting visuals of link %s",link->name.c_str());
+		
+
+       
+        {
+
+			
+			btCompoundShape* tmpGfxShape = new btCompoundShape();
+
+			for (int v=0;v<(int)link->visual_array.size();v++)
+			{
+				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)
+					{
+						tmpGfxShape->addChildShape(childTrans*inertialFrame.inverse(),childShape);
+					} else
+					{
+						tmpGfxShape->addChildShape(childTrans,childShape);
+					}
+				}
+			}
+
+			btCompoundShape* compoundShape = new btCompoundShape();
 			for (int v=0;v<(int)link->collision_array.size();v++)
 			{
-            const Collision* visual = link->collision_array[v].get();
+				const Collision* col = link->collision_array[v].get();
-
+				btCollisionShape* childShape = convertURDFToCollisionShape(col ,pathPrefix);
-			shape = convertVisualToCollisionShape(visual,pathPrefix);
+				if (childShape)
-			
-            if (shape)
 				{
-                gfxBridge.createCollisionShapeGraphicsObject(shape);
+					btVector3 childPos(col->origin.position.x, col->origin.position.y, col->origin.position.z);
+					btQuaternion childOrn(col->origin.rotation.x, col->origin.rotation.y, col->origin.rotation.z, col->origin.rotation.w);
+					btTransform childTrans;
+					childTrans.setOrigin(childPos);
+					childTrans.setRotation(childOrn);
+					if (!mappings.m_createMultiBody)
+					{
+						compoundShape->addChildShape(childTrans*inertialFrame.inverse(),childShape);
+					} else
+					{
+						compoundShape->addChildShape(childTrans,childShape);
+					}
+					
+				}
+			}
+
+			
+
+            
+			
+			if (compoundShape)
+			{
+				
 
 				btVector3 color = selectColor();
-/*                if (visual->material.get())
+				/*                if (visual->material.get())
 								{
 								color.setValue(visual->material->color.r,visual->material->color.g,visual->material->color.b);//,visual->material->color.a);
 								}
 								*/
-                btVector3 localInertia(0,0,0);
+				//btVector3 localInertiaDiagonal(0, 0, 0);
-                if (mass)
+				//if (mass)
-                {
+				//{
-                    shape->calculateLocalInertia(mass,localInertia);
+				//	shape->calculateLocalInertia(mass, localInertiaDiagonal);
-                }
+				//}
-                btRigidBody::btRigidBodyConstructionInfo rbci(mass,0,shape,localInertia);
 
 				
-				btVector3 visual_pos(visual->origin.position.x,visual->origin.position.y,visual->origin.position.z);
+				//btTransform visualFrameInWorldSpace = linkTransformInWorldSpace*visual_frame;
-				btQuaternion visual_orn(visual->origin.rotation.x,visual->origin.rotation.y,visual->origin.rotation.z,visual->origin.rotation.w);
+				btTransform inertialFrameInWorldSpace = linkTransformInWorldSpace*inertialFrame;
-				btTransform visual_frame;
-				visual_frame.setOrigin(visual_pos);
-				visual_frame.setRotation(visual_orn);
-
-				btTransform visualFrameInWorldSpace =linkTransformInWorldSpace*visual_frame;
-				rbci.m_startWorldTransform = visualFrameInWorldSpace;//linkCenterOfMass;
-
-
-                btRigidBody* body = new btRigidBody(rbci);
-
-				world1->addRigidBody(body,bodyCollisionFilterGroup,bodyCollisionFilterMask);
-    //            body->setFriction(0);
-
-                gfxBridge.createRigidBodyGraphicsObject(body,color);
 				URDF_LinkInformation* linkInfo = new URDF_LinkInformation;
+				
+				if (!mappings.m_createMultiBody)
+				{
+					btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0, compoundShape, localInertiaDiagonal);
+					rbci.m_startWorldTransform = inertialFrameInWorldSpace;
+					linkInfo->m_bodyWorldTransform = inertialFrameInWorldSpace;//visualFrameInWorldSpace
+					//rbci.m_startWorldTransform = inertialFrameInWorldSpace;//linkCenterOfMass;
+					btRigidBody* body = new btRigidBody(rbci);
+					world1->addRigidBody(body, bodyCollisionFilterGroup, bodyCollisionFilterMask);
+					gfxBridge.createCollisionShapeGraphicsObject(tmpGfxShape);
+					//hack-> transfer user inder from visual to collision shape
+					compoundShape->setUserIndex(tmpGfxShape->getUserIndex());
+
+					gfxBridge.createRigidBodyGraphicsObject(body, color);
 					linkInfo->m_bulletRigidBody = body;
-                linkInfo->m_localVisualFrame =visual_frame;
+				} else
+				{
+					if (mappings.m_bulletMultiBody==0)
+					{
+						bool multiDof = true;
+						bool canSleep = false;
+						bool isFixedBase = (mass==0);//todo: figure out when base is fixed
+						int totalNumJoints = mappings.m_totalNumJoints;
+						mappings.m_bulletMultiBody = new btMultiBody(totalNumJoints,mass, localInertiaDiagonal, isFixedBase, canSleep, multiDof);
+					}
+
+				}
+
+
+				linkInfo->m_collisionShape = compoundShape;
+				linkInfo->m_localInertiaDiagonal = localInertiaDiagonal;
+				linkInfo->m_mass = mass;
+                //linkInfo->m_localVisualFrame =visual_frame;
                 linkInfo->m_localInertialFrame =inertialFrame;
                 linkInfo->m_thisLink = link.get();
                 const Link* p = link.get();
@@ -452,24 +567,61 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
                 {
 					btAssert(pp);
 
-                    btRigidBody* parentBody =pp->m_bulletRigidBody;
+                    
 
                     const Joint* pj = (*link).parent_joint.get();
                     btTransform offsetInA,offsetInB;
+                    static bool once = true;
 
                     offsetInA.setIdentity();
+					static bool toggle=false;
+					
+					//offsetInA = pp->m_localVisualFrame.inverse()*parent2joint;
+					offsetInA = pp->m_localInertialFrame.inverse()*parent2joint;
 					
-                    offsetInA = pp->m_localVisualFrame.inverse()*parent2joint;
                     offsetInB.setIdentity();
-                    offsetInB = visual_frame.inverse();
+                    //offsetInB = visual_frame.inverse();
+					offsetInB = inertialFrame.inverse();
 					
+					
+					bool disableParentCollision = true;
+					btVector3 jointAxis(pj->axis.x,pj->axis.y,pj->axis.z);
                     switch (pj->type)
                     {
                         case Joint::FIXED:
+                        {
+							if (mappings.m_createMultiBody)
+							{
+								//todo: adjust the center of mass transform and pivot axis properly
+
+								printf("Fixed joint (btMultiBody)\n");
+								//btVector3 dVec = quatRotate(parentComToThisCom.getRotation(),offsetInB.inverse().getOrigin());
+								btQuaternion rot = parent2joint.inverse().getRotation();
+								//toggle=!toggle;
+								mappings.m_bulletMultiBody->setupFixed(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1, 
+									rot, parent2joint.getOrigin(), btVector3(0,0,0),disableParentCollision);
+								btMatrix3x3 rm(rot);
+								btScalar y,p,r;
+								rm.getEulerZYX(y,p,r);
+								//parent2joint.inverse().getRotation(), offsetInA.getOrigin(), -offsetInB.getOrigin(), disableParentCollision);
+								//linkInfo->m_localVisualFrame.setIdentity();
+								printf("y=%f,p=%f,r=%f\n", y,p,r);
+								
+
+
+							} else
 							{
 								printf("Fixed joint\n");
-                            btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*parentBody, *body,offsetInA,offsetInB);
+								
-//                            btVector3 bulletAxis(pj->axis.x,pj->axis.y,pj->axis.z);
+								btMatrix3x3 rm(offsetInA.getBasis());
+								btScalar y,p,r;
+								rm.getEulerZYX(y,p,r);
+								//parent2joint.inverse().getRotation(), offsetInA.getOrigin(), -offsetInB.getOrigin(), disableParentCollision);
+								//linkInfo->m_localVisualFrame.setIdentity();
+								printf("y=%f,p=%f,r=%f\n", y,p,r);
+
+								btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*pp->m_bulletRigidBody, *linkInfo->m_bulletRigidBody, offsetInA, offsetInB);
+								//                            btVector3 bulletAxis(pj->axis.x,pj->axis.y,pj->axis.z);
 								dof6->setLinearLowerLimit(btVector3(0,0,0));
 								dof6->setLinearUpperLimit(btVector3(0,0,0));
 
@@ -479,31 +631,98 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
 								if (enableConstraints)
 									world1->addConstraint(dof6,true);
 
-//                            btFixedConstraint* fixed = new btFixedConstraint(*parentBody, *body,offsetInA,offsetInB);
+								//                            btFixedConstraint* fixed = new btFixedConstraint(*parentBody, *body,offsetInA,offsetInB);
 								//                          world->addConstraint(fixed,true);
+								}
                             break;
                         }
                         case Joint::CONTINUOUS:
                         case Joint::REVOLUTE:
                         {
-                            btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*parentBody, *body,offsetInA,offsetInB);
+							if (mappings.m_createMultiBody)
-//                            btVector3 bulletAxis(pj->axis.x,pj->axis.y,pj->axis.z);
+							{
+								//todo: adjust the center of mass transform and pivot axis properly
+								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(),
+								-offsetInB.getOrigin(),
+								disableParentCollision);
+								linkInfo->m_localVisualFrame.setIdentity();
+								*/
+							} else
+							{
+								//only handle principle axis at the moment, 
+								//@todo(erwincoumans) orient the constraint for non-principal axis
+								btVector3 axis(pj->axis.x,pj->axis.y,pj->axis.z);
+								int principleAxis = axis.closestAxis();
+								switch (principleAxis)
+								{
+								case 0:
+									{
+										btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*pp->m_bulletRigidBody, *linkInfo->m_bulletRigidBody, offsetInA, offsetInB,RO_ZYX);
 										dof6->setLinearLowerLimit(btVector3(0,0,0));
 										dof6->setLinearUpperLimit(btVector3(0,0,0));
 								
-                            dof6->setAngularLowerLimit(btVector3(0,0,1000));
+										dof6->setAngularUpperLimit(btVector3(-1,0,0));
-                           dof6->setAngularUpperLimit(btVector3(0,0,-1000));
+										dof6->setAngularLowerLimit(btVector3(1,0,0));
 								
 										if (enableConstraints)
 											world1->addConstraint(dof6,true);
+										break;
+									}
+								case 1:
+									{
+										btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*pp->m_bulletRigidBody, *linkInfo->m_bulletRigidBody, offsetInA, offsetInB,RO_XZY);
+										dof6->setLinearLowerLimit(btVector3(0,0,0));
+										dof6->setLinearUpperLimit(btVector3(0,0,0));
 								
+										dof6->setAngularUpperLimit(btVector3(0,-1,0));
+										dof6->setAngularLowerLimit(btVector3(0,1,0));
+								
+										if (enableConstraints)
+											world1->addConstraint(dof6,true);
+										break;
+									}
+								case 2:
+								default:
+									{
+										btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*pp->m_bulletRigidBody, *linkInfo->m_bulletRigidBody, offsetInA, offsetInB,RO_XYZ);
+										dof6->setLinearLowerLimit(btVector3(0,0,0));
+										dof6->setLinearUpperLimit(btVector3(0,0,0));
+								
+										dof6->setAngularUpperLimit(btVector3(0,0,-1));
+										dof6->setAngularLowerLimit(btVector3(0,0,0));
+								
+										if (enableConstraints)
+											world1->addConstraint(dof6,true);
+									}
+								};
 								printf("Revolute/Continuous joint\n");
+							}
                             break;
                         }
                         case Joint::PRISMATIC:
                         {
-                            btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*parentBody, *body,offsetInA,offsetInB);
+							if (mappings.m_createMultiBody)
+							{
+								mappings.m_bulletMultiBody->setupPrismatic(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1,
+									parent2joint.inverse().getRotation(),jointAxis,parent2joint.getOrigin(),disableParentCollision);
 
+								//mappings.m_bulletMultiBody->setupRevolute(linkIndex - 1, mass, localInertiaDiagonal, parentIndex - 1, 
+								//	parent2joint.getRotation(), jointAxis, parent2joint.getOrigin(), 
+								//	offsetInB.getOrigin(), 
+								//	disableParentCollision);
+
+							} else
+							{
+								
+								btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*pp->m_bulletRigidBody, *linkInfo->m_bulletRigidBody, offsetInA, offsetInB);
 								dof6->setLinearLowerLimit(btVector3(pj->limits->lower,0,0));
 								dof6->setLinearUpperLimit(btVector3(pj->limits->upper,0,0));
 
@@ -514,6 +733,7 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
 									world1->addConstraint(dof6,true);
 
 								printf("Prismatic\n");
+							}
                             break;
                         }
                         default:
@@ -523,10 +743,63 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
                     }
 
                 }
+
+				if (mappings.m_createMultiBody)
+				{
+					if (compoundShape->getNumChildShapes()>0)
+					{
+						btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(mappings.m_bulletMultiBody, linkIndex-1);
+						
+						//btCompoundShape* comp = new btCompoundShape();
+						//comp->addChildShape(linkInfo->m_localVisualFrame,shape);
+						
+						gfxBridge.createCollisionShapeGraphicsObject(tmpGfxShape);
+						compoundShape->setUserIndex(tmpGfxShape->getUserIndex());
+						col->setCollisionShape(compoundShape);
+
+						btTransform tr;
+						tr.setIdentity();
+						tr = linkTransformInWorldSpace;
+						//if we don't set the initial pose of the btCollisionObject, the simulator will do this 
+						//when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider
+                
+						//tr.setOrigin(local_origin[0]);
+						//tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
+						col->setWorldTransform(tr);
+
+						bool isDynamic = true;
+						short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
+						short collisionFilterMask = isDynamic? 	short(btBroadphaseProxy::AllFilter) : 	short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
+		
+						world1->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);
+
+						btVector3 color = selectColor();//(0.0,0.0,0.5);
+						
+						gfxBridge.createCollisionObjectGraphicsObject(col,color);
+						btScalar friction = 0.5f;
+
+						col->setFriction(friction);
+        
+						if (parentIndex>=0)
+						{
+							mappings.m_bulletMultiBody->getLink(linkIndex-1).m_collider=col;
+						} else
+						{
+							mappings.m_bulletMultiBody->setBaseCollider(col);
 						}
 					}
 				}
 				
+				//mappings.m_linkLocalDiagonalInertiaTensors.push_back(localInertiaDiagonal);
+				//mappings.m_linkLocalInertiaTransforms.push_back(localInertialTransform);
+            }
+
+			
+        }
+    }
+
+	mappings.m_linkMasses.push_back(mass);
+
     for (std::vector<my_shared_ptr<Link> >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
     {
         if (*child)
@@ -546,257 +819,6 @@ void URDFvisual2BulletCollisionShape(my_shared_ptr<const Link> link, GraphicsPhy
 }
 
 
-
-btMultiBody* URDF2BulletMultiBody(my_shared_ptr<const Link> link, GraphicsPhysicsBridge& gfxBridge, const btTransform& parentTransformInWorldSpace, btMultiBodyDynamicsWorld* world, URDF2BulletMappings& mappings, const char* pathPrefix, btMultiBody* mb, int totalNumJoints)
-{
-
-	btScalar mass = 0.f;
-	btTransform localInertialTransform; localInertialTransform.setIdentity();
-	btVector3 localInertiaDiagonal(0,0,0);
-
-	{
-		
-		if ((*link).inertial)
-		{
-			mass = (*link).inertial->mass;
-			btMatrix3x3 inertiaMat;
-			inertiaMat.setIdentity();
-			inertiaMat.setValue(
-				(*link).inertial->ixx,(*link).inertial->ixy,(*link).inertial->ixz,
-				(*link).inertial->ixy,(*link).inertial->iyy,(*link).inertial->iyz,
-				(*link).inertial->ixz,(*link).inertial->iyz,(*link).inertial->izz);
-			
-			btScalar threshold = 0.00001f;
-			int maxSteps=20;
-			btMatrix3x3 inertia2PrincipalAxis;
-			inertiaMat.diagonalize(inertia2PrincipalAxis,threshold,maxSteps);
-			localInertiaDiagonal.setValue(inertiaMat[0][0],inertiaMat[1][1],inertiaMat[2][2]);
-            
-            btVector3 inertiaLocalCOM((*link).inertial->origin.position.x,(*link).inertial->origin.position.y,(*link).inertial->origin.position.z);
-            localInertialTransform.setOrigin(inertiaLocalCOM);
-            btQuaternion inertiaOrn((*link).inertial->origin.rotation.x,(*link).inertial->origin.rotation.y,(*link).inertial->origin.rotation.z,(*link).inertial->origin.rotation.w);
-            btMatrix3x3 inertiaOrnMat(inertiaOrn);
-            
-            if (mass > 0 && (localInertiaDiagonal[0]==0.f || localInertiaDiagonal[1] == 0.f
-				|| localInertiaDiagonal[2] == 0.f))
-            {
-				b3Warning("Error: inertia should not be zero if mass is positive\n");
-                localInertiaDiagonal.setMax(btVector3(0.1,0.1,0.1));
-                localInertialTransform.setIdentity();//.setBasis(inertiaOrnMat);
-            }
-            else
-            {
-                localInertialTransform.setBasis(inertiaOrnMat*inertia2PrincipalAxis);
-            }
-		}
-	}
-	btTransform linkTransformInWorldSpace;
-	int parentIndex = -1;
-
-	const Link* parentLink = (*link).getParent();
-	if (parentLink)
-	{
-		parentIndex = parentLink->m_link_index;
-		btAssert(parentIndex>=0);
-	}
-	int linkIndex =  mappings.m_linkMasses.size();
-
-	btTransform parent2joint;
-
-	if ((*link).parent_joint)
-	{
-		const urdf::Vector3 pos = (*link).parent_joint->parent_to_joint_origin_transform.position;
-		const urdf::Rotation orn = (*link).parent_joint->parent_to_joint_origin_transform.rotation;
-		parent2joint.setOrigin(btVector3(pos.x,pos.y,pos.z));
-		parent2joint.setRotation(btQuaternion(orn.x,orn.y,orn.z,orn.w));
-		linkTransformInWorldSpace =parentTransformInWorldSpace*parent2joint;
-	} else
-	{
-		linkTransformInWorldSpace = parentTransformInWorldSpace;
-
-		btAssert(mb==0);
-
-		bool multiDof = true;
-		bool canSleep = false;
-		bool isFixedBase = (mass==0);//todo: figure out when base is fixed
-
-		mb = new btMultiBody(totalNumJoints,mass, localInertiaDiagonal, isFixedBase, canSleep, multiDof);
-
-
-	}
-
-	btAssert(mb);
-	
-	(*link).m_link_index = linkIndex;
-
-	//compute this links center of mass transform, aligned with the principal axis of inertia
-
-
-	{
-		//btTransform rigidBodyFrameInWorldSpace =linkTransformInWorldSpace*inertialFrame;
-			
-		mappings.m_linkMasses.push_back(mass);
-		mappings.m_linkLocalDiagonalInertiaTensors.push_back(localInertiaDiagonal);
-		mappings.m_linkLocalInertiaTransforms.push_back(localInertialTransform);
-		
-			
-		
-		if ((*link).parent_joint)
-		{
-			btTransform offsetInA,offsetInB;
-        offsetInA.setIdentity();
-		//offsetInA = mappings.m_linkLocalInertiaTransforms[parentIndex].inverse()*parent2joint;
-		offsetInA = parent2joint;
-        offsetInB.setIdentity();
-        //offsetInB = localInertialTransform.inverse();
-
-			const Joint* pj = (*link).parent_joint.get();
-			//btVector3 jointAxis(0,0,1);//pj->axis.x,pj->axis.y,pj->axis.z);
-			btVector3 jointAxis(pj->axis.x,pj->axis.y,pj->axis.z);
-			mappings.m_jointAxisArray.push_back(jointAxis);
-			mappings.m_jointOffsetInParent.push_back(offsetInA);
-			mappings.m_jointOffsetInChild.push_back(offsetInB);
-			mappings.m_jointTypeArray.push_back(pj->type);
-
-			switch (pj->type)
-            {
-                case Joint::FIXED:
-                {
-                    printf("Fixed joint\n");
-					mb->setupFixed(linkIndex-1,mass,localInertiaDiagonal,parentIndex-1,offsetInA.getRotation(),offsetInA.getOrigin(),offsetInB.getOrigin());
-					
-					break;
-				}
-				case Joint::CONTINUOUS:
-                case Joint::REVOLUTE:
-                {
-                    printf("Revolute joint\n");
-					mb->setupRevolute(linkIndex-1,mass,localInertiaDiagonal,parentIndex-1,offsetInA.getRotation(),jointAxis,offsetInA.getOrigin(),offsetInB.getOrigin(),true);
-					mb->finalizeMultiDof();
-					//mb->setJointVel(linkIndex-1,1);
-
-					break;
-				}
-				case Joint::PRISMATIC:
-				{
-					mb->setupPrismatic(linkIndex-1,mass,localInertiaDiagonal,parentIndex-1,offsetInA.getRotation(),jointAxis,offsetInB.getOrigin(),true);
-					printf("Prismatic joint\n");
-					break;
-				}
-				default:
-				{
-					printf("Unknown joint\n");
-					btAssert(0);
-				}
-			};
-			
-			
-
-		
-		} else
-		{
-			mappings.m_jointAxisArray.push_back(btVector3(0,0,0));
-			btTransform ident;
-			ident.setIdentity();
-			mappings.m_jointOffsetInParent.push_back(ident);
-			mappings.m_jointOffsetInChild.push_back(ident);
-			mappings.m_jointTypeArray.push_back(-1);
-			
-
-		}
-	}
-
-	//btCompoundShape* compoundShape = new btCompoundShape();
-	btCollisionShape* shape = 0;
-
-	for (int v=0;v<(int)link->collision_array.size();v++)
-	{
-		const Collision* visual = link->collision_array[v].get();
-
-		shape = convertVisualToCollisionShape(visual,pathPrefix);
-			
-		if (shape)//childShape)
-		{
-			gfxBridge.createCollisionShapeGraphicsObject(shape);//childShape);
-
-			//btVector3 color = selectColor();
-			/*
-			if (visual->material.get())
-			{
-				color.setValue(visual->material->color.r,visual->material->color.g,visual->material->color.b);//,visual->material->color.a);
-			}
-			*/
-			btVector3 localInertia(0,0,0);
-			if (mass)
-			{
-				shape->calculateLocalInertia(mass,localInertia);
-			}
-			//btRigidBody::btRigidBodyConstructionInfo rbci(mass,0,shape,localInertia);
-
-
-			btVector3 visual_pos(visual->origin.position.x,visual->origin.position.y,visual->origin.position.z);
-			btQuaternion visual_orn(visual->origin.rotation.x,visual->origin.rotation.y,visual->origin.rotation.z,visual->origin.rotation.w);
-			btTransform visual_frame;
-			visual_frame.setOrigin(visual_pos);
-			visual_frame.setRotation(visual_orn);
-			btTransform childTransform;
-			childTransform.setIdentity();//TODO(erwincoumans): compute relative visual/inertial transform
-		//	compoundShape->addChildShape(childTransform,childShape);
-		}
-	}
-	
-	if (shape)//compoundShape->getNumChildShapes()>0)
-	{
-		btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(mb, linkIndex-1);
-        col->setCollisionShape(shape);
-
-        btTransform tr;
-        tr.setIdentity();
-		tr = linkTransformInWorldSpace;
-		//if we don't set the initial pose of the btCollisionObject, the simulator will do this 
-		//when syncing the btMultiBody link transforms to the btMultiBodyLinkCollider
-                
-		//tr.setOrigin(local_origin[0]);
-        //tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
-        col->setWorldTransform(tr);
-
-		bool isDynamic = true;
-		short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
-		short collisionFilterMask = isDynamic? 	short(btBroadphaseProxy::AllFilter) : 	short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
-		
-        world->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);
-
-        btVector3 color = selectColor();//(0.0,0.0,0.5);
-        gfxBridge.createCollisionObjectGraphicsObject(col,color);
-		btScalar friction = 0.5f;
-
-        col->setFriction(friction);
-        
-		if (parentIndex>=0)
-		{
-			mb->getLink(linkIndex-1).m_collider=col;
-		} else
-		{
-			mb->setBaseCollider(col);
-		}
-	}
-
-	for (std::vector<my_shared_ptr<Link> >::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++)
-    {
-        if (*child)
-        {
-            URDF2BulletMultiBody(*child,gfxBridge, linkTransformInWorldSpace, world,mappings,pathPrefix,mb,totalNumJoints);
-
-        }
-        else
-        {
-            std::cout << "root link: " << link->name << " has a null child!" << *child << std::endl;
-        }
-    }
-	return mb;
-}
-
-
 void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
 {
 
@@ -874,27 +896,22 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
 	int numJoints = (*robot).m_numJoints;
 
 	static bool useFeatherstone = false;
-    if (!useFeatherstone)
     {
         URDF2BulletMappings mappings;
+		mappings.m_createMultiBody = useFeatherstone;
+		mappings.m_totalNumJoints = numJoints;
         URDFvisual2BulletCollisionShape(root_link, gfxBridge, identityTrans,m_dynamicsWorld,mappings,pathPrefix);
-    }
+		if (useFeatherstone)
-
-	//the btMultiBody support is work-in-progress :-)
-#if 1
-	else
 		{
-        URDF2BulletMappings mappings;
+			btMultiBody* mb = mappings.m_bulletMultiBody;
-		
-		btMultiBody* mb = URDF2BulletMultiBody(root_link, gfxBridge, identityTrans,m_dynamicsWorld,mappings,pathPrefix, 0,numJoints);
-		
 			mb->setHasSelfCollision(false);
 			mb->finalizeMultiDof();
 			m_dynamicsWorld->addMultiBody(mb);
-		//m_dynamicsWorld->integrateTransforms(0.f);
-
 		}
-#endif//
+    }
+
+	//the btMultiBody support is work-in-progress :-)
+
 	useFeatherstone = !useFeatherstone;
 	printf("numJoints/DOFS = %d\n", numJoints);
 
@@ -923,7 +940,7 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
         gfxBridge.createCollisionShapeGraphicsObject(box);
         btTransform start; start.setIdentity();
         btVector3 groundOrigin(0,0,0);
-        groundOrigin[upAxis]=-2.5;
+        groundOrigin[upAxis]=-1.5;
         start.setOrigin(groundOrigin);
         btRigidBody* body =  createRigidBody(0,start,box);
         //m_dynamicsWorld->removeRigidBody(body);
@@ -932,7 +949,7 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
         gfxBridge.createRigidBodyGraphicsObject(body,color);
     }
 
-
+	m_dynamicsWorld->stepSimulation(1. / 240., 0);// 1., 10, 1. / 240.);
 }
 
 void ImportUrdfSetup::stepSimulation(float deltaTime)

Demos3/ImportURDFDemo/urdf_samples.h

@@ -402,7 +402,7 @@ const char* urdf_char_r2d2 = MSTRINGIFY(
 const char* urdf_char = MSTRINGIFY(
 
 								<?xml version="1.0"?>
-								   <robot name="flexible">
+<robot name="physics">
   <link name="base_link">
     <visual>
       <geometry>
@@ -412,6 +412,15 @@ const char* urdf_char = MSTRINGIFY(
         <color rgba="0 0 .8 1"/>
       </material>
     </visual>
+    <collision>
+      <geometry>
+        <cylinder length="0.6" radius="0.2"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="10"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
   </link>
 
   <link name="right_leg">
@@ -424,6 +433,17 @@ const char* urdf_char = MSTRINGIFY(
         <color rgba="1 1 1 1"/>
       </material>
     </visual>
+    <collision>
+      <geometry>
+        <box size="0.6 .2 .1"/>
+      </geometry>
+      <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
+    </collision>
+    <inertial>
+      <mass value="10"/>
+      <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
   </link>
 
   <joint name="base_to_right_leg" type="fixed">
@@ -439,6 +459,15 @@ const char* urdf_char = MSTRINGIFY(
       </geometry>
       <material name="white"/>
     </visual>
+    <collision>
+      <geometry>
+        <box size=".1 0.4 .1"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="10"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
   </link>
 
   <joint name="right_base_joint" type="fixed">
@@ -456,6 +485,15 @@ const char* urdf_char = MSTRINGIFY(
         <color rgba="0 0 0 1"/>
       </material>
     </visual>
+    <collision>
+      <geometry>
+        <cylinder length=".1" radius="0.035"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="1"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
   </link>
 
   <joint name="right_front_wheel_joint" type="continuous">
@@ -474,6 +512,15 @@ const char* urdf_char = MSTRINGIFY(
       </geometry>
       <material name="black"/>
     </visual>
+    <collision>
+      <geometry>
+        <cylinder length=".1" radius="0.035"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="1"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
   </link>
 
   <joint name="right_back_wheel_joint" type="continuous">
@@ -493,6 +540,17 @@ const char* urdf_char = MSTRINGIFY(
       <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
       <material name="white"/>
     </visual>
+    <collision>
+      <geometry>
+        <box size="0.6 .2 .1"/>
+      </geometry>
+      <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
+    </collision>
+    <inertial>
+      <mass value="10"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+       <origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
+    </inertial>
   </link>
 
   <joint name="base_to_left_leg" type="fixed">
@@ -508,6 +566,15 @@ const char* urdf_char = MSTRINGIFY(
       </geometry>
       <material name="white"/>
     </visual>
+    <collision>
+      <geometry>
+        <box size=".1 0.4 .1"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="10"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
   </link>
 
   <joint name="left_base_joint" type="fixed">
@@ -523,6 +590,15 @@ const char* urdf_char = MSTRINGIFY(
       </geometry>
       <material name="black"/>
     </visual>
+    <collision>
+      <geometry>
+        <cylinder length=".1" radius="0.035"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="1"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
   </link>
 
   <joint name="left_front_wheel_joint" type="continuous">
@@ -541,6 +617,15 @@ const char* urdf_char = MSTRINGIFY(
       </geometry>
       <material name="black"/>
     </visual>
+    <collision>
+      <geometry>
+        <cylinder length=".1" radius="0.035"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="1"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
   </link>
 
   <joint name="left_back_wheel_joint" type="continuous">
@@ -568,6 +653,16 @@ const char* urdf_char = MSTRINGIFY(
         <color rgba=".7 .7 .7 1"/>
       </material>
     </visual>
+    <collision>
+      <geometry>
+        <cylinder length="0.2" radius=".01"/>
+      </geometry>
+      <origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
+    </collision>
+    <inertial>
+      <mass value="0.05"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
   </link>
 
   <joint name="left_gripper_joint" type="revolute">
@@ -582,9 +677,19 @@ const char* urdf_char = MSTRINGIFY(
     <visual>
       <origin rpy="0.0 0 0" xyz="0 0 0"/>
       <geometry>
-								   <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
+        <mesh filename="l_finger.stl"/>
       </geometry>
     </visual>
+    <collision>
+      <geometry>
+        <mesh filename="l_finger.stl"/>
+      </geometry>
+      <origin rpy="0.0 0 0" xyz="0 0 0"/>
+    </collision>
+    <inertial>
+      <mass value="0.05"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
   </link>
 
   <joint name="left_tip_joint" type="fixed">
@@ -596,9 +701,19 @@ const char* urdf_char = MSTRINGIFY(
     <visual>
       <origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
       <geometry>
-								   <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
+        <mesh filename="l_finger_tip.stl"/>
       </geometry>
     </visual>
+    <collision>
+      <geometry>
+        <mesh filename="l_finger_tip.stl"/>
+      </geometry>
+      <origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
+    </collision>
+    <inertial>
+      <mass value="0.05"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
   </link>
 
   <joint name="right_gripper_joint" type="revolute">
@@ -613,9 +728,19 @@ const char* urdf_char = MSTRINGIFY(
     <visual>
       <origin rpy="-3.1415 0 0" xyz="0 0 0"/>
       <geometry>
-								   <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
+        <mesh filename="l_finger.stl"/>
       </geometry>
     </visual>
+    <collision>
+      <geometry>
+        <mesh filename="l_finger.stl"/>
+      </geometry>
+      <origin rpy="-3.1415 0 0" xyz="0 0 0"/>
+    </collision>
+    <inertial>
+      <mass value="0.05"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
   </link>
 
   <joint name="right_tip_joint" type="fixed">
@@ -627,9 +752,19 @@ const char* urdf_char = MSTRINGIFY(
     <visual>
       <origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
       <geometry>
-								   <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
+        <mesh filename="l_finger_tip.stl"/>
       </geometry>
     </visual>
+    <collision>
+      <geometry>
+        <mesh filename="l_finger_tip.stl"/>
+      </geometry>
+      <origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
+    </collision>
+    <inertial>
+      <mass value="0.05"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
   </link>
 
   <link name="head">
@@ -639,6 +774,15 @@ const char* urdf_char = MSTRINGIFY(
       </geometry>
       <material name="white"/>
     </visual>
+    <collision>
+      <geometry>
+        <sphere radius="0.2"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="10"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
   </link>
 
   <joint name="head_swivel" type="continuous">
@@ -655,6 +799,15 @@ const char* urdf_char = MSTRINGIFY(
       </geometry>
       <material name="blue"/>
     </visual>
+    <collision>
+      <geometry>
+        <box size=".08 .08 .08"/>
+      </geometry>
+    </collision>
+    <inertial>
+      <mass value="1"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
   </link>
 
   <joint name="tobox" type="fixed">
@@ -663,7 +816,8 @@ const char* urdf_char = MSTRINGIFY(
     <origin xyz="0 0.1414 0.1414"/>
   </joint>
 
-								   </robot>
+</robot>
+
 
 );
 

Demos3/bullet2/BasicConcepts/CoordinateSystemDemo.h

@@ -60,6 +60,9 @@ public:
     virtual void	renderScene()
     {
 		m_app->m_renderer->renderScene();
+		m_app->drawText3D("X",1,0,0,1);
+		m_app->drawText3D("Y",0,1,0,1);
+		m_app->drawText3D("Z",0,0,1,1);
     }
 
 	virtual void drawArc(const btVector3& center, const btVector3& normal, const btVector3& axis, btScalar radiusA, btScalar radiusB, btScalar minAngle, btScalar maxAngle, 
@@ -89,7 +92,7 @@ public:
 		}
 	}
 
-    virtual void	physicsDebugDraw()
+    virtual void	physicsDebugDraw(int debugDrawFlags)
     {
       
 		btVector3 xUnit(1,0,0);

Demos3/bullet2/CollisionDetection/SupportFuncDemo.h

@@ -74,7 +74,7 @@ public:
         m_app->m_renderer->renderScene();
         
     }
-    virtual void	physicsDebugDraw()
+    virtual void	physicsDebugDraw(int debugDrawFlags)
     {
         int width=3;
         btVector3 from(0,0,0);

Demos3/bullet2/CoordinateFrameDemo/CoordinateFrameDemoPhysicsSetup.cpp

@@ -21,6 +21,7 @@ void    CoordinateFrameDemoPhysicsSetup::debugDraw()
 		}
 	}
 	*/
+
 	m_dynamicsWorld->debugDrawWorld();
 }
 

Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.cpp

@@ -612,10 +612,12 @@ void	FeatherstoneDemo1::renderScene()
 	m_glApp->m_renderer->renderScene();
 }
 
-void	FeatherstoneDemo1::physicsDebugDraw()
+void	FeatherstoneDemo1::physicsDebugDraw(int debugDrawFlags)
 {
 	if (m_dynamicsWorld)
 	{
+		if (m_dynamicsWorld->getDebugDrawer())
+			m_dynamicsWorld->getDebugDrawer()->setDebugMode(debugDrawFlags);
 		m_dynamicsWorld->debugDrawWorld();	
 	}
 }

Demos3/bullet2/FeatherstoneMultiBodyDemo/BulletMultiBodyDemos.h

@@ -89,7 +89,7 @@ public:
 	virtual void	initPhysics();
 	virtual void	exitPhysics();
 	virtual void	renderScene();
-        virtual void    physicsDebugDraw();
+    virtual void    physicsDebugDraw(int debugDrawFlags);
 
 	virtual void	stepSimulation(float deltaTime);
 };

Demos3/bullet2/LuaDemo/LuaPhysicsSetup.cpp

@@ -413,10 +413,16 @@ void LuaPhysicsSetup::stepSimulation(float deltaTime)
 
 }
 
-void    LuaPhysicsSetup::debugDraw()
+void    LuaPhysicsSetup::debugDraw(int debugDrawFlags)
 {
     if (m_dynamicsWorld)
+	{
+		if (m_dynamicsWorld->getDebugDrawer())
+		{
+			m_dynamicsWorld->getDebugDrawer()->setDebugMode(debugDrawFlags);
+		}
         m_dynamicsWorld->debugDrawWorld();
+	}
 
 }
 

Demos3/bullet2/LuaDemo/LuaPhysicsSetup.h

@@ -25,7 +25,7 @@ struct LuaPhysicsSetup : public CommonPhysicsSetup
 
 	virtual void stepSimulation(float deltaTime);
 
-    virtual void    debugDraw();
+    virtual void    debugDraw(int debugDrawFlags);
 
 	virtual bool pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld);
 	virtual bool movePickedBody(const btVector3& rayFromWorld, const btVector3& rayToWorld);

Extras/Serialize/BulletFileLoader/bFile.cpp

@@ -388,7 +388,9 @@ void bFile::swapDNA(char* ptr)
 {
 	bool swap = ((mFlags & FD_ENDIAN_SWAP)!=0);
 
-	char* data = &ptr[20];
+	int offset = (mFlags & FD_FILE_64)? 24 : 20;
+	char* data = &ptr[offset];
+
 //	void bDNA::init(char *data, int len, bool swap)
 	int *intPtr=0;short *shtPtr=0;
 	char *cp = 0;int dataLen =0;long nr=0;

btgui/Bullet3AppSupport/Bullet2RigidBodyDemo.cpp

@@ -226,9 +226,9 @@ void Bullet2RigidBodyDemo::renderScene()
 
 }
 
-void	Bullet2RigidBodyDemo::physicsDebugDraw()
+void	Bullet2RigidBodyDemo::physicsDebugDraw(int debugDrawFlags)
 {
-    m_physicsSetup->debugDraw();
+    m_physicsSetup->debugDraw(debugDrawFlags);
 }
 
 Bullet2RigidBodyDemo::~Bullet2RigidBodyDemo()

btgui/Bullet3AppSupport/Bullet2RigidBodyDemo.h

@@ -25,7 +25,7 @@ public:
 	virtual void initPhysics();
 	virtual void exitPhysics();
 	virtual void	renderScene();
-    virtual void	physicsDebugDraw();
+    virtual void	physicsDebugDraw(int debugDrawFlags);
 	virtual void	stepSimulation(float dt);
 	virtual CommonPhysicsSetup* getPhysicsSetup()
 	{

btgui/Bullet3AppSupport/BulletDemoInterface.h

@@ -3,6 +3,7 @@
 
 struct CommonGraphicsApp;
 
+
 class BulletDemoInterface
 {
 public:
@@ -17,7 +18,7 @@ public:
 	virtual void    exitPhysics()=0;
 	virtual void	stepSimulation(float deltaTime)=0;
 	virtual void	renderScene()=0;
-	virtual void	physicsDebugDraw()=0;
+	virtual void	physicsDebugDraw(int debugFlags)=0;//for now we reuse the flags in Bullet/src/LinearMath/btIDebugDraw.h
 	virtual bool	mouseMoveCallback(float x,float y)=0;
 	virtual bool	mouseButtonCallback(int button, int state, float x, float y)=0;
 	virtual bool	keyboardCallback(int key, int state)=0;
@@ -44,7 +45,7 @@ public:
 	virtual void	renderScene()
 	{
 	}
-	virtual void	physicsDebugDraw()
+	virtual void	physicsDebugDraw(int debugFlags)
 	{
 	}
 	virtual bool	mouseMoveCallback(float x,float y)

btgui/Bullet3AppSupport/CommonMultiBodySetup.h

@@ -125,10 +125,14 @@ struct CommonMultiBodySetup : public CommonPhysicsSetup
 		}
 	}
 
-    virtual void    debugDraw()
+    virtual void    debugDraw(int debugDrawFlags)
     {
      	if (m_dynamicsWorld)
         {
+			if (m_dynamicsWorld->getDebugDrawer())
+			{
+				m_dynamicsWorld->getDebugDrawer()->setDebugMode(debugDrawFlags);
+			}
             m_dynamicsWorld->debugDrawWorld();
         }
 

btgui/Bullet3AppSupport/CommonPhysicsSetup.h

@@ -57,7 +57,7 @@ public:
 
 	virtual void stepSimulation(float deltaTime)=0;
 
-    virtual void    debugDraw()=0;
+    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;

btgui/Bullet3AppSupport/CommonRigidBodySetup.h

@@ -118,10 +118,14 @@ struct CommonRigidBodySetup : public CommonPhysicsSetup
 		}
 	}
 
-    virtual void    debugDraw()
+    virtual void    debugDraw(int debugDrawFlags)
     {
      	if (m_dynamicsWorld)
         {
+			if (m_dynamicsWorld->getDebugDrawer())
+			{
+				m_dynamicsWorld->getDebugDrawer()->setDebugMode(debugDrawFlags);
+			}
             m_dynamicsWorld->debugDrawWorld();
         }
 

btgui/OpenGLWindow/CMakeLists.txt

@@ -1,7 +1,7 @@
 
 INCLUDE_DIRECTORIES(
-	${BULLET_PHYSICS_SOURCE_DIR}/src
+	..
-	${BULLET_PHYSICS_SOURCE_DIR}/btgui
+	../../src
 )
 
 FILE(GLOB OpenGLWindow_HDRS "*.h" )
@@ -19,9 +19,9 @@ LIST(REMOVE_ITEM OpenGLWindowCommon_CPP X11OpenGLWindow.cpp )
 #MESSAGE (${OpenGLWindowCommon_CPP})
 
 IF (WIN32)
-	SET(OpenGLWindow_SRCS ${BULLET_PHYSICS_SOURCE_DIR}/btgui/OpenGLWindow/GlewWindows/glew.c ${OpenGLWindowWin32_CPP} ${OpenGLWindowCommon_CPP})	
+	SET(OpenGLWindow_SRCS GlewWindows/glew.c ${OpenGLWindowWin32_CPP} ${OpenGLWindowCommon_CPP})	
 	INCLUDE_DIRECTORIES(
-				${BULLET_PHYSICS_SOURCE_DIR}/btgui/OpenGLWindow/GlewWindows
+				GlewWindows
 	)
 	ADD_DEFINITIONS(-DGLEW_STATIC)
 ENDIF(WIN32)
@@ -33,13 +33,13 @@ ENDIF(APPLE)
 #no Linux detection?
 IF(NOT WIN32 AND NOT APPLE)
 	INCLUDE_DIRECTORIES(
-                                ${BULLET_PHYSICS_SOURCE_DIR}/btgui/OpenGLWindow/GlewWindows
+                                GlewWindows
         )
         ADD_DEFINITIONS(-DGLEW_STATIC)
 	ADD_DEFINITIONS("-DGLEW_INIT_OPENGL11_FUNCTIONS=1")
         ADD_DEFINITIONS("-DGLEW_DYNAMIC_LOAD_ALL_GLX_FUNCTIONS=1")
 
-	SET(OpenGLWindow_SRCS ${OpenGLWindowLinux_CPP} ${BULLET_PHYSICS_SOURCE_DIR}/btgui/OpenGLWindow/GlewWindows/glew.c ${OpenGLWindowCommon_CPP} )
+	SET(OpenGLWindow_SRCS ${OpenGLWindowLinux_CPP} GlewWindows/glew.c ${OpenGLWindowCommon_CPP} )
 ENDIF()
 
 

btgui/OpenGLWindow/CommonGraphicsApp.h

@@ -42,9 +42,10 @@ struct CommonGraphicsApp
 	
 	virtual void swapBuffer() = 0;
 	virtual void drawText( const char* txt, int posX, int posY) = 0;
-	
+	virtual void drawText3D( const char* txt, float posX, float posZY, float posZ, float size)=0;
 	virtual int	registerCubeShape(float halfExtentsX,float halfExtentsY, float halfExtentsZ)=0;
 	virtual int	registerGraphicsSphereShape(float radius, bool usePointSprites=true, int largeSphereThreshold=100, int mediumSphereThreshold=10)=0;
+	virtual void registerGrid(int xres, int yres, float color0[4], float color1[4])=0;
 };
 
 

btgui/OpenGLWindow/CommonRenderInterface.h

@@ -32,6 +32,9 @@ struct CommonRenderInterface
 	virtual void	getCameraTargetPosition(float cameraPos[4]) const=0;
     virtual void	getCameraTargetPosition(double cameraPos[4]) const=0;
 
+	virtual void	getCameraViewMatrix(float viewMat[16]) const=0;
+	virtual void	getCameraProjectionMatrix(float projMat[16]) const=0;
+
 	virtual void renderScene()=0;
 
 	virtual int getScreenWidth() = 0;
@@ -49,8 +52,59 @@ struct CommonRenderInterface
     
 	virtual void writeSingleInstanceTransformToCPU(const float* position, const float* orientation, int srcIndex)=0;
 	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;
 };
+
+template <typename T>
+inline int projectWorldCoordToScreen(T objx, T objy, T objz,
+		const T modelMatrix[16],
+		const T projMatrix[16],
+		const int viewport[4],
+		T *winx, T *winy, T *winz)
+{
+	int i;
+	T in2[4];
+	T tmp[4];
+
+	in2[0]=objx;
+	in2[1]=objy;
+	in2[2]=objz;
+	in2[3]=T(1.0);
+
+	for (i=0; i<4; i++) 
+	{
+		tmp[i] =   in2[0] * modelMatrix[0*4+i] +  in2[1] * modelMatrix[1*4+i] +  
+			in2[2] * modelMatrix[2*4+i] +  in2[3] * modelMatrix[3*4+i];
+	}
+
+	T out[4];
+	for (i=0; i<4; i++) 
+	{
+		out[i] =   tmp[0] * projMatrix[0*4+i] +  tmp[1] * projMatrix[1*4+i] +  tmp[2] * projMatrix[2*4+i] +  tmp[3] * projMatrix[3*4+i];
+	}
+
+	if (out[3] == T(0.0)) 
+		return 0;
+	out[0] /= out[3];
+	out[1] /= out[3];
+	out[2] /= out[3];
+	/* Map x, y and z to range 0-1 */
+	out[0] = out[0] * T(0.5) + T(0.5);
+	out[1] = out[1] * T(0.5) + T(0.5);
+	out[2] = out[2] * T(0.5) + T(0.5);
+
+	/* Map x,y to viewport */
+	out[0] = out[0] * viewport[2] + viewport[0];
+	out[1] = out[1] * viewport[3] + viewport[1];
+
+	*winx=out[0];
+	*winy=out[1];
+	*winz=out[2];
+	return 1;
+}
+
 #endif//COMMON_RENDER_INTERFACE_H
 

btgui/OpenGLWindow/GLInstancingRenderer.cpp

@@ -27,7 +27,7 @@ float MOUSE_MOVE_MULTIPLIER = 0.4f;
 
 #include "OpenGLInclude.h"
 #include "b3gWindowInterface.h"
-#include "Bullet3Common/b3MinMax.h"
+//#include "Bullet3Common/b3MinMax.h"
 
 #ifndef __APPLE__
 #ifndef glVertexAttribDivisor
@@ -112,8 +112,7 @@ struct b3GraphicsInstance
 
 bool m_ortho = false;
 
-static GLfloat projectionMatrix[16];
+
-static GLfloat modelviewMatrix[16];
 
 //static GLfloat depthLightModelviewMatrix[16];
 
@@ -157,7 +156,8 @@ struct InternalDataRenderer : public GLInstanceRendererInternalData
 	int m_middleMouseButton;
 	int m_rightMouseButton;
 
-
+	GLfloat m_projectionMatrix[16];
+	GLfloat m_viewMatrix[16];
 
 
 	GLuint				m_defaultTexturehandle;
@@ -185,7 +185,12 @@ struct InternalDataRenderer : public GLInstanceRendererInternalData
 		m_altPressed(0),
 		m_controlPressed(0)
 	{
-
+		//clear to zero to make it obvious if the matrix is used uninitialized
+		for (int i=0;i<16;i++)
+		{
+			m_projectionMatrix[i]=0;
+			m_viewMatrix[i]=0;
+		}
 
 	}
 
@@ -224,6 +229,7 @@ struct InternalDataRenderer : public GLInstanceRendererInternalData
 
 	void mouseMoveCallback(float x, float y)
 	{
+//        printf("moved to %f,%f\n",x,y);
 		if (m_altPressed || m_controlPressed)
 		{
 			float xDelta = x-m_mouseXpos;
@@ -460,6 +466,13 @@ void GLInstancingRenderer::writeSingleInstanceTransformToCPU(const float* positi
 	*/
 }
 
+void GLInstancingRenderer::writeSingleInstanceColorToCPU(double* color, int srcIndex)
+{
+	m_data->m_instance_colors_ptr[srcIndex*4+0]=float(color[0]);
+	m_data->m_instance_colors_ptr[srcIndex*4+1]=float(color[1]);
+	m_data->m_instance_colors_ptr[srcIndex*4+2]=float(color[2]);
+	m_data->m_instance_colors_ptr[srcIndex*4+3]=float(color[3]);
+}
 
 void GLInstancingRenderer::writeSingleInstanceColorToCPU(float* color, int srcIndex)
 {
@@ -643,6 +656,7 @@ int GLInstancingRenderer::registerGraphicsInstance(int shapeIndex, const float*
 		m_data->m_instance_scale_ptr[index*3+2] = scaling[2];
 
 		gfxObj->m_numGraphicsInstances++;
+		m_data->m_totalNumInstances++;
 	} else
 	{
 		b3Error("registerGraphicsInstance out of range, %d\n", maxElements);
@@ -690,8 +704,6 @@ void GLInstancingRenderer::updateShape(int shapeIndex, const float* vertices)
 	glUnmapBuffer( GL_ARRAY_BUFFER);
 }
 
-
-
 int GLInstancingRenderer::registerShape(const float* vertices, int numvertices, const int* indices, int numIndices,int primitiveType, int textureId)
 {
 	b3GraphicsInstance* gfxObj = new b3GraphicsInstance;
@@ -897,9 +909,9 @@ void GLInstancingRenderer::init()
 					{
 						if (x<2||y<2||x>253||y>253)
 						{
-							pi[0]=0;
+							pi[0]=255;//0;
-							pi[1]=0;
+							pi[1]=255;//0;
-							pi[2]=0;
+							pi[2]=255;//0;
 						} else
 						{
 							pi[0]=255;
@@ -1091,8 +1103,8 @@ void GLInstancingRenderer::updateCamera(int upAxis)
 	};
 
 
-    float m_frustumZNear=1;
+    float m_frustumZNear=0.01;
-    float m_frustumZFar=10000.f;
+    float m_frustumZFar=1000.f;
 
 
 //    m_azi=m_azi+0.01;
@@ -1132,13 +1144,13 @@ void GLInstancingRenderer::updateCamera(int upAxis)
 
 	if (m_screenWidth > m_screenHeight)
 	{
-		b3CreateFrustum(-aspect * m_frustumZNear, aspect * m_frustumZNear, -m_frustumZNear, m_frustumZNear, m_frustumZNear, m_frustumZFar,projectionMatrix);
+		b3CreateFrustum(-aspect * m_frustumZNear, aspect * m_frustumZNear, -m_frustumZNear, m_frustumZNear, m_frustumZNear, m_frustumZFar,m_data->m_projectionMatrix);
 	} else
 	{
-		b3CreateFrustum(-aspect * m_frustumZNear, aspect * m_frustumZNear, -m_frustumZNear, m_frustumZNear, m_frustumZNear, m_frustumZFar,projectionMatrix);
+		b3CreateFrustum(-aspect * m_frustumZNear, aspect * m_frustumZNear, -m_frustumZNear, m_frustumZNear, m_frustumZNear, m_frustumZFar,m_data->m_projectionMatrix);
 	}
 
-	b3CreateLookAt(m_data->m_cameraPosition,m_data->m_cameraTargetPosition,m_data->m_cameraUp,modelviewMatrix);
+	b3CreateLookAt(m_data->m_cameraPosition,m_data->m_cameraTargetPosition,m_data->m_cameraUp,m_data->m_viewMatrix);
 
 
 }
@@ -1276,7 +1288,7 @@ void writeTextureToPng(int textureWidth, int textureHeight, const char* fileName
 			pixels[(j*textureWidth+i)*numComponents]=char(orgPixels[(j*textureWidth+i)]*255.f);
 			pixels[(j*textureWidth+i)*numComponents+1]=0;//255.f;
 			pixels[(j*textureWidth+i)*numComponents+2]=0;//255.f;
-			pixels[(j*textureWidth+i)*numComponents+3]=255;
+			pixels[(j*textureWidth+i)*numComponents+3]=127;
 
 
 			//pixels[(j*textureWidth+i)*+1]=val;
@@ -1342,8 +1354,8 @@ void GLInstancingRenderer::renderScene()
 
 void GLInstancingRenderer::drawPoint(const double* position, const double color[4], double pointDrawSize)
 {
-	float pos[4]={position[0],position[1],position[2],0};
+	float pos[4]={(float)position[0],(float)position[1],(float)position[2],0};
-	float clr[4] = {color[0],color[1],color[2],color[3]};
+	float clr[4] = {(float)color[0],(float)color[1],(float)color[2],(float)color[3]};
 	drawPoints(pos,clr,1,3*sizeof(float),float(pointDrawSize));
 }
 
@@ -1351,16 +1363,16 @@ void GLInstancingRenderer::drawPoint(const float* positions, const float color[4
 {
 	drawPoints(positions,color,1,3*sizeof(float),pointDrawSize);
 }
-
 void GLInstancingRenderer::drawPoints(const float* positions, const float color[4], int numPoints, int pointStrideInBytes, float pointDrawSize)
 {
 
 	glActiveTexture(GL_TEXTURE0);
 	glBindTexture(GL_TEXTURE_2D,0);
 
-	b3Assert(glGetError() ==GL_NO_ERROR);glUseProgram(linesShader);
+	b3Assert(glGetError() ==GL_NO_ERROR);
-	glUniformMatrix4fv(lines_ProjectionMatrix, 1, false, &projectionMatrix[0]);
+	glUseProgram(linesShader);
-	glUniformMatrix4fv(lines_ModelViewMatrix, 1, false, &modelviewMatrix[0]);
+	glUniformMatrix4fv(lines_ProjectionMatrix, 1, false, &m_data->m_projectionMatrix[0]);
+	glUniformMatrix4fv(lines_ModelViewMatrix, 1, false, &m_data->m_viewMatrix[0]);
 	glUniform4f(lines_colour,color[0],color[1],color[2],color[3]);
 
 	glPointSize(pointDrawSize);
@@ -1393,22 +1405,26 @@ void GLInstancingRenderer::drawPoints(const float* positions, const float color[
 
 	glBindVertexArray(0);
 	glPointSize(1);
+	glUseProgram(0);
 }
 
-void GLInstancingRenderer::drawLines(const float* positions, const float color[4], int numPoints, int pointStrideInBytes, const unsigned int* indices, int numIndices, float pointDrawSize)
+void GLInstancingRenderer::drawLines(const float* positions, const float color[4], int numPoints, int pointStrideInBytes, const unsigned int* indices, int numIndices, float lineWidthIn)
 {
+	float lineWidth = lineWidthIn;
+	b3Clamp(lineWidth,(float)lineWidthRange[0],(float)lineWidthRange[1]);
+	glLineWidth(lineWidth);
 
 	glBindBuffer(GL_ARRAY_BUFFER, m_data->m_vbo);
-
 	b3Assert(glGetError() ==GL_NO_ERROR);
 	glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, 0);
 
 	glActiveTexture(GL_TEXTURE0);
 	glBindTexture(GL_TEXTURE_2D,0);
 
-	b3Assert(glGetError() ==GL_NO_ERROR);glUseProgram(linesShader);
+	b3Assert(glGetError() ==GL_NO_ERROR);
-	glUniformMatrix4fv(lines_ProjectionMatrix, 1, false, &projectionMatrix[0]);
+	glUseProgram(linesShader);
-	glUniformMatrix4fv(lines_ModelViewMatrix, 1, false, &modelviewMatrix[0]);
+	glUniformMatrix4fv(lines_ProjectionMatrix, 1, false, &m_data->m_projectionMatrix[0]);
+	glUniformMatrix4fv(lines_ModelViewMatrix, 1, false, &m_data->m_viewMatrix[0]);
 	glUniform4f(lines_colour,color[0],color[1],color[2],color[3]);
 
 //	glPointSize(pointDrawSize);
@@ -1450,7 +1466,7 @@ void GLInstancingRenderer::drawLines(const float* positions, const float color[4
 	b3Assert(glGetError() ==GL_NO_ERROR);
 	glPointSize(1);
     b3Assert(glGetError() ==GL_NO_ERROR);
-
+	glUseProgram(0);
 }
 
 void GLInstancingRenderer::drawLine(const double fromIn[4], const double toIn[4], const double colorIn[4], double lineWidthIn)
@@ -1463,6 +1479,7 @@ void GLInstancingRenderer::drawLine(const double fromIn[4], const double toIn[4]
 }
 void GLInstancingRenderer::drawLine(const float from[4], const float to[4], const float color[4], float lineWidth)
 {
+		
 	b3Assert(glGetError() ==GL_NO_ERROR);
 
 	glActiveTexture(GL_TEXTURE0);
@@ -1475,8 +1492,8 @@ void GLInstancingRenderer::drawLine(const float from[4], const float to[4], cons
 
 	b3Assert(glGetError() ==GL_NO_ERROR);
 
-	glUniformMatrix4fv(lines_ProjectionMatrix, 1, false, &projectionMatrix[0]);
+	glUniformMatrix4fv(lines_ProjectionMatrix, 1, false, &m_data->m_projectionMatrix[0]);
-	glUniformMatrix4fv(lines_ModelViewMatrix, 1, false, &modelviewMatrix[0]);
+	glUniformMatrix4fv(lines_ModelViewMatrix, 1, false, &m_data->m_viewMatrix[0]);
 	glUniform4f(lines_colour,color[0],color[1],color[2],color[3]);
 
 
@@ -1526,7 +1543,7 @@ void GLInstancingRenderer::drawLine(const float from[4], const float to[4], cons
 	glLineWidth(1);
 
 	b3Assert(glGetError() ==GL_NO_ERROR);
-
+	glUseProgram(0);
 }
 
 struct PointerCaster
@@ -1769,8 +1786,8 @@ b3Assert(glGetError() ==GL_NO_ERROR);
 				if (gfxObj->m_primitiveType==B3_GL_POINTS)
 				{
 					glUseProgram(instancingShaderPointSprite);
-					glUniformMatrix4fv(ProjectionMatrixPointSprite, 1, false, &projectionMatrix[0]);
+					glUniformMatrix4fv(ProjectionMatrixPointSprite, 1, false, &m_data->m_projectionMatrix[0]);
-					glUniformMatrix4fv(ModelViewMatrixPointSprite, 1, false, &modelviewMatrix[0]);
+					glUniformMatrix4fv(ModelViewMatrixPointSprite, 1, false, &m_data->m_viewMatrix[0]);
 					glUniform1f(screenWidthPointSprite,float(m_screenWidth));
 
 					//glUniform1i(uniform_texture_diffusePointSprite, 0);
@@ -1792,8 +1809,8 @@ b3Assert(glGetError() ==GL_NO_ERROR);
 					case B3_DEFAULT_RENDERMODE:
 						{
 							glUseProgram(instancingShader);
-							glUniformMatrix4fv(ProjectionMatrix, 1, false, &projectionMatrix[0]);
+							glUniformMatrix4fv(ProjectionMatrix, 1, false, &m_data->m_projectionMatrix[0]);
-							glUniformMatrix4fv(ModelViewMatrix, 1, false, &modelviewMatrix[0]);
+							glUniformMatrix4fv(ModelViewMatrix, 1, false, &m_data->m_viewMatrix[0]);
 							glUniform1i(uniform_texture_diffuse, 0);
 							glDrawElementsInstanced(GL_TRIANGLES, indexCount, GL_UNSIGNED_INT, indexOffset, gfxObj->m_numGraphicsInstances);
 							break;
@@ -1821,10 +1838,10 @@ b3Assert(glGetError() ==GL_NO_ERROR);
                             }
                             
 							glUseProgram(useShadowMapInstancingShader);
-							glUniformMatrix4fv(useShadow_ProjectionMatrix, 1, false, &projectionMatrix[0]);
+							glUniformMatrix4fv(useShadow_ProjectionMatrix, 1, false, &m_data->m_projectionMatrix[0]);
-							glUniformMatrix4fv(useShadow_ModelViewMatrix, 1, false, &modelviewMatrix[0]);
+							glUniformMatrix4fv(useShadow_ModelViewMatrix, 1, false, &m_data->m_viewMatrix[0]);
 							float MVP[16];
-							b3Matrix4x4Mul16(projectionMatrix,modelviewMatrix,MVP);
+							b3Matrix4x4Mul16(m_data->m_projectionMatrix,m_data->m_viewMatrix,MVP);
 							glUniformMatrix4fv(useShadow_MVP, 1, false, &MVP[0]);
 							b3Vector3 gLightDir = gLightPos;
 							gLightDir.normalize();
@@ -1877,7 +1894,7 @@ b3Assert(glGetError() ==GL_NO_ERROR);
 
 
 
-
+	glDisable(GL_CULL_FACE);
 	b3Assert(glGetError() ==GL_NO_ERROR);
 }
 
@@ -1894,3 +1911,19 @@ void GLInstancingRenderer::enableShadowMap()
 	//glBindTexture(GL_TEXTURE_2D, m_data->m_defaultTexturehandle);
 
 }
+
+void	GLInstancingRenderer::getCameraViewMatrix(float viewMat[16]) const
+{
+	for (int i=0;i<16;i++)
+	{
+		viewMat[i] = m_data->m_viewMatrix[i];
+	}
+	
+}
+void	GLInstancingRenderer::getCameraProjectionMatrix(float projMat[16]) const
+{
+	for (int i=0;i<16;i++)
+	{
+		projMat[i] = m_data->m_projectionMatrix[i];
+	}
+}

btgui/OpenGLWindow/GLInstancingRenderer.h

@@ -65,7 +65,6 @@ public:
 	///vertices must be in the format x,y,z, nx,ny,nz, u,v
 	virtual int registerShape(const float* vertices, int numvertices, const int* indices, int numIndices, int primitiveType=B3_GL_TRIANGLES, int textureIndex=-1);
 	
-
 	virtual int	registerTexture(const unsigned char* texels, int width, int height);
 
 	///position x,y,z, quaternion x,y,z,w, color r,g,b,a, scaling x,y,z
@@ -94,6 +93,7 @@ public:
 	virtual void writeSingleInstanceTransformToGPU(float* position, float* orientation, int srcIndex);
 
 	virtual void writeSingleInstanceColorToCPU(float* color, int srcIndex);
+	virtual void writeSingleInstanceColorToCPU(double* color, int srcIndex);
 
 	virtual void getMouseDirection(float* dir, int mouseX, int mouseY);
 
@@ -141,6 +141,10 @@ public:
 	virtual float	getCameraYaw() const;
 	virtual float	getCameraPitch() const;
 
+	virtual void	getCameraViewMatrix(float viewMat[16]) const;
+	virtual void	getCameraProjectionMatrix(float projMat[16]) const;
+
+
 	virtual void	resize(int width, int height);
 	virtual int	getScreenWidth()
 	{

btgui/OpenGLWindow/GLPrimInternalData.h

@@ -6,6 +6,8 @@
 struct PrimInternalData
 {
 	GLuint m_shaderProg;
+	GLint m_viewmatUniform;
+	GLint m_projMatUniform;
     GLint m_positionUniform;
     GLint m_colourAttribute;
     GLint m_positionAttribute;

btgui/OpenGLWindow/GLPrimitiveRenderer.cpp

@@ -5,11 +5,10 @@
 
 #include <assert.h>
 
-
+static const char* vertexShader3D= \
-static const char* vertexShader= \
 "#version 150   \n"
 "\n"
-"\n"
+"uniform mat4 viewMatrix, projMatrix;\n"
 "in vec4 position;\n"
 "in vec4 colour;\n"
 "out vec4 colourV;\n"
@@ -21,11 +20,11 @@ static const char* vertexShader= \
 "void main (void)\n"
 "{\n"
 "    colourV = colour;\n"
-"	gl_Position = vec4(position.x, position.y,0.f,1.f);\n"
+"   gl_Position = projMatrix * viewMatrix * position ;\n"
 "	texuvV=texuv;\n"
 "}\n";
 
-static const char* fragmentShader= \
+static const char* fragmentShader3D= \
 "#version 150\n"
 "\n"
 "uniform vec2 p;\n"
@@ -42,42 +41,12 @@ static const char* fragmentShader= \
 "  {\n"
 "		texcolor = vec4(1,1,1,texcolor.x);\n"
 "  }\n"
-"\n"
 "   fragColour = colourV*texcolor;\n"
 "}\n";
 
 
+
 static unsigned int s_indexData[6] = {0,1,2,0,2,3};
-struct vec2
-{
-	vec2(float x, float y)
-	{
-		p[0] = x;
-		p[1] = y;
-	}
-	float p[2];
-};
-
-struct vec4
-{
-	vec4(float x,float y, float z, float w)
-	{
-		p[0] = x;
-		p[1] = y;
-		p[2] = z;
-		p[3] = w;
-        
-	}
-    
-	float p[4];
-};
-
-typedef struct
-{
-    vec4 position;
-    vec4 colour;
-	vec2 uv;
-} Vertex;
 
 
    
@@ -90,9 +59,16 @@ m_screenHeight(screenHeight)
     
 	m_data = new PrimInternalData;
 
-    m_data->m_shaderProg = gltLoadShaderPair(vertexShader,fragmentShader);
+    m_data->m_shaderProg = gltLoadShaderPair(vertexShader3D,fragmentShader3D);
-	
 	
+	m_data->m_viewmatUniform = glGetUniformLocation(m_data->m_shaderProg,"viewMatrix");
+	if (m_data->m_viewmatUniform < 0) {
+		assert(0);
+	}
+	m_data->m_projMatUniform = glGetUniformLocation(m_data->m_shaderProg,"projMatrix");
+	if (m_data->m_projMatUniform < 0) {
+		assert(0);
+	}
     m_data->m_positionUniform = glGetUniformLocation(m_data->m_shaderProg, "p");
     if (m_data->m_positionUniform < 0) {
 		assert(0);
@@ -117,11 +93,11 @@ m_screenHeight(screenHeight)
 void GLPrimitiveRenderer::loadBufferData()
 {
     
-    Vertex vertexData[4] = {
+    PrimVertex vertexData[4] = {
-        { vec4(-1, -1, 0.0, 1.0 ), vec4( 1.0, 0.0, 0.0, 1.0 ) ,vec2(0,0)},
+        { PrimVec4(-1, -1, 0.0, 1.0 ), PrimVec4( 1.0, 0.0, 0.0, 1.0 ) ,PrimVec2(0,0)},
-        { vec4(-1,  1, 0.0, 1.0 ), vec4( 0.0, 1.0, 0.0, 1.0 ) ,vec2(0,1)},
+        { PrimVec4(-1,  1, 0.0, 1.0 ), PrimVec4( 0.0, 1.0, 0.0, 1.0 ) ,PrimVec2(0,1)},
-        { vec4( 1,  1, 0.0, 1.0 ), vec4( 0.0, 0.0, 1.0, 1.0 ) ,vec2(1,1)},
+        { PrimVec4( 1,  1, 0.0, 1.0 ), PrimVec4( 0.0, 0.0, 1.0, 1.0 ) ,PrimVec2(1,1)},
-        { vec4( 1, -1, 0.0, 1.0 ), vec4( 1.0, 1.0, 1.0, 1.0 ) ,vec2(1,0)}
+        { PrimVec4( 1, -1, 0.0, 1.0 ), PrimVec4( 1.0, 1.0, 1.0, 1.0 ) ,PrimVec2(1,0)}
     };
     
         
@@ -130,7 +106,7 @@ void GLPrimitiveRenderer::loadBufferData()
     
     glGenBuffers(1, &m_data->m_vertexBuffer);
     glBindBuffer(GL_ARRAY_BUFFER, m_data->m_vertexBuffer);
-    glBufferData(GL_ARRAY_BUFFER, 4 * sizeof(Vertex), vertexData, GL_DYNAMIC_DRAW);
+    glBufferData(GL_ARRAY_BUFFER, 4 * sizeof(PrimVertex), vertexData, GL_DYNAMIC_DRAW);
     
     assert(glGetError()==GL_NO_ERROR);
     
@@ -146,9 +122,9 @@ void GLPrimitiveRenderer::loadBufferData()
     
 	glEnableVertexAttribArray(m_data->m_textureAttribute);
     
-    glVertexAttribPointer(m_data->m_positionAttribute, 4, GL_FLOAT, GL_FALSE, sizeof(Vertex), (const GLvoid *)0);
+    glVertexAttribPointer(m_data->m_positionAttribute, 4, GL_FLOAT, GL_FALSE, sizeof(PrimVertex), (const GLvoid *)0);
-    glVertexAttribPointer(m_data->m_colourAttribute  , 4, GL_FLOAT, GL_FALSE, sizeof(Vertex), (const GLvoid *)sizeof(vec4));
+    glVertexAttribPointer(m_data->m_colourAttribute  , 4, GL_FLOAT, GL_FALSE, sizeof(PrimVertex), (const GLvoid *)sizeof(PrimVec4));
-    glVertexAttribPointer(m_data->m_textureAttribute , 2, GL_FLOAT, GL_FALSE, sizeof(Vertex), (const GLvoid *)(sizeof(vec4)+sizeof(vec4)));
+    glVertexAttribPointer(m_data->m_textureAttribute , 2, GL_FLOAT, GL_FALSE, sizeof(PrimVertex), (const GLvoid *)(sizeof(PrimVec4)+sizeof(PrimVec4)));
 	assert(glGetError()==GL_NO_ERROR);
     
     
@@ -225,13 +201,18 @@ void GLPrimitiveRenderer::drawRect(float x0, float y0, float x1, float y1, float
 
 }
 
-void GLPrimitiveRenderer::drawTexturedRect(float x0, float y0, float x1, float y1, float color[4], float u0,float v0, float u1, float v1, int useRGBA)
+
+void GLPrimitiveRenderer::drawTexturedRect3D(const PrimVertex& v0,const PrimVertex& v1,const PrimVertex& v2,const PrimVertex& v3,float viewMat[16],float projMat[16], bool useRGBA)
 {
+	
     assert(glGetError()==GL_NO_ERROR);
    
     
     glUseProgram(m_data->m_shaderProg);
  
+	glUniformMatrix4fv(m_data->m_viewmatUniform, 1, false, viewMat);
+	glUniformMatrix4fv(m_data->m_projMatUniform, 1, false, projMat);
+
     assert(glGetError()==GL_NO_ERROR);
     
     glBindBuffer(GL_ARRAY_BUFFER, m_data->m_vertexBuffer);
@@ -248,14 +229,11 @@ void GLPrimitiveRenderer::drawTexturedRect(float x0, float y0, float x1, float y
 		glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
 	}
 
-	   Vertex vertexData[4] = {
+	   PrimVertex vertexData[4] = {
-        { vec4(-1.f+2.f*x0/float(m_screenWidth), 1.f-2.f*y0/float(m_screenHeight), 0.f, 0.f ), vec4( color[0], color[1], color[2], color[3] ) ,vec2(u0,v0)},
+		   v0,v1,v2,v3
-        { vec4(-1.f+2.f*x0/float(m_screenWidth),  1.f-2.f*y1/float(m_screenHeight), 0.f, 1.f ), vec4( color[0], color[1], color[2], color[3] ) ,vec2(u0,v1)},
-        { vec4( -1.f+2.f*x1/float(m_screenWidth),  1.f-2.f*y1/float(m_screenHeight), 1.f, 1.f ), vec4(color[0], color[1], color[2], color[3]) ,vec2(u1,v1)},
-        { vec4( -1.f+2.f*x1/float(m_screenWidth), 1.f-2.f*y0/float(m_screenHeight), 1.f, 0.f ), vec4( color[0], color[1], color[2], color[3] ) ,vec2(u1,v0)}
 		};
     
-	   glBufferSubData(GL_ARRAY_BUFFER, 0,4 * sizeof(Vertex), vertexData);
+	   glBufferSubData(GL_ARRAY_BUFFER, 0,4 * sizeof(PrimVertex), vertexData);
 
 
 
@@ -264,7 +242,7 @@ void GLPrimitiveRenderer::drawTexturedRect(float x0, float y0, float x1, float y
     
     assert(glGetError()==GL_NO_ERROR);
     
-    vec2 p( 0.f,0.f);//?b?0.5f * sinf(timeValue), 0.5f * cosf(timeValue) );
+    PrimVec2 p( 0.f,0.f);//?b?0.5f * sinf(timeValue), 0.5f * cosf(timeValue) );
 	if (useRGBA)
 	{
 		p.p[0] = 1.f;
@@ -285,9 +263,9 @@ void GLPrimitiveRenderer::drawTexturedRect(float x0, float y0, float x1, float y
     
 	glEnableVertexAttribArray(m_data->m_textureAttribute);
     
-    glVertexAttribPointer(m_data->m_positionAttribute, 4, GL_FLOAT, GL_FALSE, sizeof(Vertex), (const GLvoid *)0);
+    glVertexAttribPointer(m_data->m_positionAttribute, 4, GL_FLOAT, GL_FALSE, sizeof(PrimVertex), (const GLvoid *)0);
-    glVertexAttribPointer(m_data->m_colourAttribute  , 4, GL_FLOAT, GL_FALSE, sizeof(Vertex), (const GLvoid *)sizeof(vec4));
+    glVertexAttribPointer(m_data->m_colourAttribute  , 4, GL_FLOAT, GL_FALSE, sizeof(PrimVertex), (const GLvoid *)sizeof(PrimVec4));
-    glVertexAttribPointer(m_data->m_textureAttribute , 2, GL_FLOAT, GL_FALSE, sizeof(Vertex), (const GLvoid *)(sizeof(vec4)+sizeof(vec4)));
+    glVertexAttribPointer(m_data->m_textureAttribute , 2, GL_FLOAT, GL_FALSE, sizeof(PrimVertex), (const GLvoid *)(sizeof(PrimVec4)+sizeof(PrimVec4)));
 	assert(glGetError()==GL_NO_ERROR);
     
     glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, m_data->m_indexBuffer);
@@ -319,6 +297,23 @@ void GLPrimitiveRenderer::drawTexturedRect(float x0, float y0, float x1, float y
 
 }
 
+
+void GLPrimitiveRenderer::drawTexturedRect(float x0, float y0, float x1, float y1, float color[4], float u0,float v0, float u1, float v1, int useRGBA)
+{
+    float identity[16]={1,0,0,0,
+						0,1,0,0,
+						0,0,1,0,
+						0,0,0,1};
+	   PrimVertex vertexData[4] = {
+        { PrimVec4(-1.f+2.f*x0/float(m_screenWidth), 1.f-2.f*y0/float(m_screenHeight), 0.f, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v0)},
+        { PrimVec4(-1.f+2.f*x0/float(m_screenWidth),  1.f-2.f*y1/float(m_screenHeight), 0.f, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v1)},
+        { PrimVec4( -1.f+2.f*x1/float(m_screenWidth),  1.f-2.f*y1/float(m_screenHeight), 0.f, 1.f ), PrimVec4(color[0], color[1], color[2], color[3]) ,PrimVec2(u1,v1)},
+        { PrimVec4( -1.f+2.f*x1/float(m_screenWidth), 1.f-2.f*y0/float(m_screenHeight), 0.f, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u1,v0)}
+    };
+    
+	drawTexturedRect3D(vertexData[0],vertexData[1],vertexData[2],vertexData[3],identity,identity,useRGBA);
+}
+
 void GLPrimitiveRenderer::setScreenSize(int width, int height)
 {
 	m_screenWidth = width;

btgui/OpenGLWindow/GLPrimitiveRenderer.h

@@ -3,6 +3,39 @@
 
 //#include "OpenGLInclude.h"
 
+struct PrimVec2
+{
+	PrimVec2(float x, float y)
+	{
+		p[0] = x;
+		p[1] = y;
+	}
+	float p[2];
+};
+
+struct PrimVec4
+{
+	PrimVec4() {}
+	PrimVec4(float x,float y, float z, float w)
+	{
+		p[0] = x;
+		p[1] = y;
+		p[2] = z;
+		p[3] = w;
+        
+	}
+    
+	float p[4];
+};
+
+typedef struct
+{
+    PrimVec4 position;
+    PrimVec4 colour;
+	PrimVec2 uv;
+} PrimVertex;
+
+
 class GLPrimitiveRenderer
 {
     int m_screenWidth;
@@ -19,6 +52,7 @@ public:
 	
 	void drawRect(float x0, float y0, float x1, float y1, float color[4]);
     void drawTexturedRect(float x0, float y0, float x1, float y1, float color[4], float u0,float v0, float u1, float v1, int useRGBA=0);
+	void drawTexturedRect3D(const PrimVertex& v0,const PrimVertex& v1,const PrimVertex& v2,const PrimVertex& v3,float viewMat[16],float projMat[16], bool useRGBA = true);
     void drawLine();//float from[4], float to[4], float color[4]);
     void setScreenSize(int width, int height);
     

btgui/OpenGLWindow/MacOpenGLWindow.mm

@@ -398,10 +398,11 @@ void MacOpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci)
    // }];
 
     //see http://stackoverflow.com/questions/8238473/cant-get-nsmousemoved-events-from-nexteventmatchingmask-with-an-nsopenglview
-       ProcessSerialNumber psn;
+/*       ProcessSerialNumber psn;
 	GetCurrentProcess(&psn);
      TransformProcessType(&psn, kProcessTransformToForegroundApplication);
      SetFrontProcess(&psn);
+    */
  
     m_retinaScaleFactor = [m_internalData->m_myview convertSizeToBacking:sz].width;
 
@@ -808,45 +809,49 @@ void MacOpenGLWindow::startRendering()
 			}
 		}
 
-        if ([event type]== NSLeftMouseUp)
+
+        if (([event type]== NSRightMouseDown) || ([ event type]==NSLeftMouseDown)||([event type]==NSOtherMouseDown))
         {
            // printf("right mouse!");
+          //  float mouseX,mouseY;
             
             NSPoint eventLocation = [event locationInWindow];
             NSPoint center = [m_internalData->m_myview convertPoint:eventLocation fromView:nil];
             m_mouseX = center.x;
             m_mouseY = [m_internalData->m_myview GetWindowHeight] - center.y;
-           
+            int button=0;
-            
+	        switch ([event type])
-            // printf("mouse coord = %f, %f\n",mouseX,mouseY);
-            if (m_mouseButtonCallback)
             {
-                (*m_mouseButtonCallback)(0,0,m_mouseX,m_mouseY);
+                case NSLeftMouseDown:
-				//handledEvent = true;
+                {
+                    button=0;
+                    break;
                 }
+                case NSOtherMouseDown:
+                {
+                    button=1;
+                    break;
+                }
+                case NSRightMouseDown:
+                {
+                    button=2;
+                    break;
+                }
+                default:
+                {
                 
                 }
-
+            };
-        
-        if ([event type]== NSLeftMouseDown)
-        {
-            // printf("right mouse!");
-            
-            NSPoint eventLocation = [event locationInWindow];
-            NSPoint center = [m_internalData->m_myview convertPoint:eventLocation fromView:nil];
-            m_mouseX = center.x;
-            m_mouseY = [m_internalData->m_myview GetWindowHeight] - center.y;
-            
            // printf("mouse coord = %f, %f\n",mouseX,mouseY);
             if (m_mouseButtonCallback)
 			{
 				//handledEvent = true;
-                (*m_mouseButtonCallback)(0,1,m_mouseX,m_mouseY);
+                (*m_mouseButtonCallback)(button,1,m_mouseX,m_mouseY);
             }
         }
 		
 		
-        if ([event type]== NSRightMouseDown)
+        if (([event type]== NSRightMouseUp) || ([ event type]==NSLeftMouseUp)||([event type]==NSOtherMouseUp))
         {
 			// printf("right mouse!");
 			//  float mouseX,mouseY;
@@ -856,28 +861,33 @@ void MacOpenGLWindow::startRendering()
             m_mouseX = center.x;
             m_mouseY = [m_internalData->m_myview GetWindowHeight] - center.y;
 	        
-           // printf("mouse coord = %f, %f\n",mouseX,mouseY);
+            int button=0;
-            if (m_mouseButtonCallback)
+            switch ([event type])
             {
-				//handledEvent = true;
+                case NSLeftMouseUp:
-                (*m_mouseButtonCallback)(2,1,m_mouseX,m_mouseY);
+                {
-            }
+                    button=0;
-        }
+                    break;
-		
+                }
-		
+                case NSOtherMouseUp:
-        if ([event type]== NSRightMouseUp)
+                {
+                    button=1;
+                    break;
+                }
+                case NSRightMouseUp:
+                {
+                    button=2;
+                    break;
+                }
+                default:
                 {
-			// printf("right mouse!");
-			//  float mouseX,mouseY;
                     
-            NSPoint eventLocation = [event locationInWindow];
+                }
-            NSPoint center = [m_internalData->m_myview convertPoint:eventLocation fromView:nil];
+            };
-            m_mouseX = center.x;
-            m_mouseY = [m_internalData->m_myview GetWindowHeight] - center.y;
             
 			// printf("mouse coord = %f, %f\n",mouseX,mouseY);
             if (m_mouseButtonCallback)
-                (*m_mouseButtonCallback)(2,0,m_mouseX,m_mouseY);
+                (*m_mouseButtonCallback)(button,0,m_mouseX,m_mouseY);
             
         }
 		
@@ -899,7 +909,7 @@ void MacOpenGLWindow::startRendering()
 			}
         }
         
-        if ([event type] == NSLeftMouseDragged)
+        if (([event type] == NSLeftMouseDragged) || ([event type] == NSRightMouseDragged) || ([event type] == NSOtherMouseDragged))
         {
             int dx1, dy1;
             CGGetLastMouseDelta (&dx1, &dy1);

btgui/OpenGLWindow/SimpleOpenGL3App.cpp

@@ -26,6 +26,7 @@
 #include "OpenGLWindow/opengl_fontstashcallbacks.h"
 #include <assert.h>
 #include "OpenGLWindow/GLRenderToTexture.h"
+#include "Bullet3Common/b3Quaternion.h"
 
 #ifdef _WIN32
     #define popen _popen
@@ -35,6 +36,7 @@
 struct SimpleInternalData
 {
 	GLuint m_fontTextureId;
+	GLuint m_largeFontTextureId;
 	struct sth_stash* m_fontStash;
 	OpenGL2RenderCallbacks*		m_renderCallbacks;
 	int m_droidRegular;
@@ -48,8 +50,10 @@ struct SimpleInternalData
 
 static SimpleOpenGL3App* gApp=0;
 
-static void SimpleResizeCallback( float width, float height)
+static void SimpleResizeCallback( float widthf, float heightf)
 {
+	int width = (int)widthf;
+	int height = (int)heightf;
 	gApp->m_instancingRenderer->resize(width,height);
 	gApp->m_primRenderer->setScreenSize(width,height);
 
@@ -156,6 +160,8 @@ SimpleOpenGL3App::SimpleOpenGL3App(	const char* title, int width,int height)
 
 	TwGenerateDefaultFonts();
 	m_data->m_fontTextureId = BindFont(g_DefaultNormalFont);
+	m_data->m_largeFontTextureId = BindFont(g_DefaultLargeFont);
+	
 
 
 	{
@@ -189,13 +195,194 @@ struct sth_stash* SimpleOpenGL3App::getFontStash()
 	return m_data->m_fontStash;
 }
 
-
+void SimpleOpenGL3App::drawText3D( const char* txt, float worldPosX, float worldPosY, float worldPosZ, float size1)
-
-void SimpleOpenGL3App::drawText( const char* txt, int posX, int posY)
 {
 
+	float viewMat[16];
+	float projMat[16];
+	m_instancingRenderer->getCameraViewMatrix(viewMat);
+	m_instancingRenderer->getCameraProjectionMatrix(projMat);
 
 	
+	float camPos[4];
+	this->m_instancingRenderer->getCameraPosition(camPos);
+	b3Vector3 cp= b3MakeVector3(camPos[0],camPos[2],camPos[1]);
+	b3Vector3 p = b3MakeVector3(worldPosX,worldPosY,worldPosZ);
+	float dist = (cp-p).length();
+	float dv = 0;//dist/1000.f;
+    //
+    //printf("str = %s\n",unicodeText);
+
+    float dx=0;
+
+    //int measureOnly=0;
+
+	glEnable(GL_BLEND);
+	glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+	int viewport[4]={0,0,m_instancingRenderer->getScreenWidth(),m_instancingRenderer->getScreenHeight()};
+
+	float posX = 450.f;
+	float posY = 100.f;
+	float winx,winy, winz;
+
+	if (!projectWorldCoordToScreen(worldPosX, worldPosY, worldPosZ,viewMat,projMat,viewport,&winx, &winy, &winz))
+	{
+		return;
+	}
+	posX = winx;
+	posY = m_instancingRenderer->getScreenHeight()/2+(m_instancingRenderer->getScreenHeight()/2)-winy;
+
+	
+	if (0)//m_useTrueTypeFont)
+	{
+		bool measureOnly = false;
+
+		float fontSize= 32;//64;//512;//128;
+		sth_draw_text(m_data->m_fontStash,
+                    m_data->m_droidRegular,fontSize,posX,posY,
+					txt,&dx, this->m_instancingRenderer->getScreenWidth(),this->m_instancingRenderer->getScreenHeight(),measureOnly,m_window->getRetinaScale());
+		sth_end_draw(m_data->m_fontStash);
+		sth_flush_draw(m_data->m_fontStash);
+	} else
+	{
+		//float width = 0.f;
+		int pos=0;
+		//float color[]={0.2f,0.2,0.2f,1.f};
+		glActiveTexture(GL_TEXTURE0);
+		glBindTexture(GL_TEXTURE_2D,m_data->m_largeFontTextureId);
+
+		//float width = r.x;
+		//float extraSpacing = 0.;
+
+		float startX = posX;
+		float startY = posY-g_DefaultLargeFont->m_CharHeight;
+		
+
+		while (txt[pos])
+		{
+			int c = txt[pos];
+			//r.h = g_DefaultNormalFont->m_CharHeight;
+			//r.w = g_DefaultNormalFont->m_CharWidth[c]+extraSpacing;
+			float endX = startX+g_DefaultLargeFont->m_CharWidth[c];
+			float endY = posY;
+
+
+			float currentColor[]={1.f,0.2,0.2f,1.f};
+
+		//	m_primRenderer->drawTexturedRect(startX, startY, endX, endY, currentColor,g_DefaultLargeFont->m_CharU0[c],g_DefaultLargeFont->m_CharV0[c],g_DefaultLargeFont->m_CharU1[c],g_DefaultLargeFont->m_CharV1[c]);
+			float u0 = g_DefaultLargeFont->m_CharU0[c];
+			float u1 = g_DefaultLargeFont->m_CharU1[c];
+			float v0 = g_DefaultLargeFont->m_CharV0[c];
+			float v1 = g_DefaultLargeFont->m_CharV1[c];
+			float color[4] = {currentColor[0],currentColor[1],currentColor[2],currentColor[3]};
+			float x0 = startX;
+			float x1 = endX;
+			float y0 = startY;
+			float y1 = endY;
+			int screenWidth = m_instancingRenderer->getScreenWidth();
+			int screenHeight = m_instancingRenderer->getScreenHeight();
+
+			float z = 2.f*winz-1.f;//*(far
+			 float identity[16]={1,0,0,0,
+						0,1,0,0,
+						0,0,1,0,
+						0,0,0,1};
+				   PrimVertex vertexData[4] = {
+					{ PrimVec4(-1.f+2.f*x0/float(screenWidth), 1.f-2.f*y0/float(screenHeight), z, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v0)},
+					{ PrimVec4(-1.f+2.f*x0/float(screenWidth),  1.f-2.f*y1/float(screenHeight), z, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v1)},
+					{ PrimVec4( -1.f+2.f*x1/float(screenWidth),  1.f-2.f*y1/float(screenHeight), z, 1.f ), PrimVec4(color[0], color[1], color[2], color[3]) ,PrimVec2(u1,v1)},
+					{ PrimVec4( -1.f+2.f*x1/float(screenWidth), 1.f-2.f*y0/float(screenHeight), z, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u1,v0)}
+				};
+    
+				m_primRenderer->drawTexturedRect3D(vertexData[0],vertexData[1],vertexData[2],vertexData[3],identity,identity,false);
+
+			//DrawTexturedRect(0,r,g_DefaultNormalFont->m_CharU0[c],g_DefaultNormalFont->m_CharV0[c],g_DefaultNormalFont->m_CharU1[c],g_DefaultNormalFont->m_CharV1[c]);
+		//	DrawFilledRect(r);
+
+			startX = endX;
+			//startY = endY;
+
+			pos++;
+
+		}
+		glBindTexture(GL_TEXTURE_2D,0);
+	}
+
+	glDisable(GL_BLEND);
+
+
+#if 0
+	glEnable(GL_BLEND);
+	glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+
+
+	int pos=0;
+	//float color[]={0.2f,0.2,0.2f,1.f};
+	glActiveTexture(GL_TEXTURE0);
+	glBindTexture(GL_TEXTURE_2D,m_data->m_largeFontTextureId);
+
+	//float width = r.x;
+	//float extraSpacing = 0.;
+
+	float startX = posX;
+	float startY = posY;
+
+
+	while (txt[pos])
+	{
+			float scaling = 0.02;
+
+		int c = txt[pos];
+		//r.h = g_DefaultNormalFont->m_CharHeight;
+		//r.w = g_DefaultNormalFont->m_CharWidth[c]+extraSpacing;
+		float endX = startX-float(g_DefaultLargeFont->m_CharWidth[c])*scaling;
+		float endY = startY-float(g_DefaultLargeFont->m_CharHeight)*scaling;
+	
+
+		float currentColor[]={0.2f,0.2,0.2f,1.f};
+
+		float u0  = g_DefaultLargeFont->m_CharU0[c];
+		float v0 = g_DefaultLargeFont->m_CharV0[c];
+		float u1 = g_DefaultLargeFont->m_CharU1[c];
+		float v1 = g_DefaultLargeFont->m_CharV1[c];
+		float color[4] = {0,0,0,1};
+
+		PrimVertex vertexData[4] = {
+				{ PrimVec4(startX, startY, 0, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v0)},
+				{ PrimVec4(startX, endY, 0 , 1.f), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u0,v1)},
+				{ PrimVec4(endX, endY,0.f, 1.f ), PrimVec4(color[0], color[1], color[2], color[3]) ,PrimVec2(u1,v1)},
+				{ PrimVec4(endX,startY, 0.f, 1.f ), PrimVec4( color[0], color[1], color[2], color[3] ) ,PrimVec2(u1,v0)}
+		};
+		float viewMat[16];
+		float projMat[16];
+		m_instancingRenderer->getCameraViewMatrix(viewMat);
+		m_instancingRenderer->getCameraProjectionMatrix(projMat);
+
+		m_primRenderer->drawTexturedRect3D(vertexData[0],vertexData[1],vertexData[2],vertexData[3],viewMat,projMat,false);
+
+		//DrawTexturedRect(0,r,g_DefaultNormalFont->m_CharU0[c],g_DefaultNormalFont->m_CharV0[c],g_DefaultNormalFont->m_CharU1[c],g_DefaultNormalFont->m_CharV1[c]);
+	//	DrawFilledRect(r);
+
+		startX = endX;
+		//startY = endY;
+
+		pos++;
+
+	}
+	glBindTexture(GL_TEXTURE_2D,0);
+
+	glDisable(GL_BLEND);
+#endif
+
+}
+
+
+void SimpleOpenGL3App::drawText( const char* txt, int posXi, int posYi)
+{
+
+	float posX = (float)posXi;
+	float posY = (float) posYi;
+
 
     //
     //printf("str = %s\n",unicodeText);
@@ -213,9 +400,15 @@ void SimpleOpenGL3App::drawText( const char* txt, int posX, int posY)
 		bool measureOnly = false;
 
 		float fontSize= 64;//512;//128;
+		int bla=0;
+		float bla2=1;
 		sth_draw_text(m_data->m_fontStash,
                     m_data->m_droidRegular,fontSize,posX,posY,
-					txt,&dx, this->m_instancingRenderer->getScreenWidth(),this->m_instancingRenderer->getScreenHeight(),measureOnly,m_window->getRetinaScale());
+					txt,&dx, this->m_instancingRenderer->getScreenWidth(),
+					this->m_instancingRenderer->getScreenHeight(),
+					measureOnly,
+					m_window->getRetinaScale());
+					
 		sth_end_draw(m_data->m_fontStash);
 		sth_flush_draw(m_data->m_fontStash);
 	} else
@@ -224,13 +417,13 @@ void SimpleOpenGL3App::drawText( const char* txt, int posX, int posY)
 		int pos=0;
 		//float color[]={0.2f,0.2,0.2f,1.f};
 		glActiveTexture(GL_TEXTURE0);
-		glBindTexture(GL_TEXTURE_2D,m_data->m_fontTextureId);
+		glBindTexture(GL_TEXTURE_2D,m_data->m_largeFontTextureId);
 
 		//float width = r.x;
 		//float extraSpacing = 0.;
 
-		int startX = posX;
+		float startX = posX;
-		int startY = posY;
+		float startY = posY;
 
 
 		while (txt[pos])
@@ -238,13 +431,13 @@ void SimpleOpenGL3App::drawText( const char* txt, int posX, int posY)
 			int c = txt[pos];
 			//r.h = g_DefaultNormalFont->m_CharHeight;
 			//r.w = g_DefaultNormalFont->m_CharWidth[c]+extraSpacing;
-			int endX = startX+g_DefaultNormalFont->m_CharWidth[c];
+			float endX = startX+g_DefaultLargeFont->m_CharWidth[c];
-			int endY = startY+g_DefaultNormalFont->m_CharHeight;
+			float endY = startY+g_DefaultLargeFont->m_CharHeight;
 
 
 			float currentColor[]={0.2f,0.2,0.2f,1.f};
 
-			m_primRenderer->drawTexturedRect(startX, startY, endX, endY, currentColor,g_DefaultNormalFont->m_CharU0[c],g_DefaultNormalFont->m_CharV0[c],g_DefaultNormalFont->m_CharU1[c],g_DefaultNormalFont->m_CharV1[c]);
+			m_primRenderer->drawTexturedRect(startX, startY, endX, endY, currentColor,g_DefaultLargeFont->m_CharU0[c],g_DefaultLargeFont->m_CharV0[c],g_DefaultLargeFont->m_CharU1[c],g_DefaultLargeFont->m_CharV1[c]);
 
 			//DrawTexturedRect(0,r,g_DefaultNormalFont->m_CharU0[c],g_DefaultNormalFont->m_CharV0[c],g_DefaultNormalFont->m_CharU1[c],g_DefaultNormalFont->m_CharV1[c]);
 		//	DrawFilledRect(r);
@@ -295,6 +488,40 @@ int	SimpleOpenGL3App::registerCubeShape(float halfExtentsX,float halfExtentsY, f
 	return shapeId;
 }
 
+void SimpleOpenGL3App::registerGrid(int cells_x, int cells_z, float color0[4], float color1[4])
+{
+	b3Vector3 cubeExtents=b3MakeVector3(0.5,0.5,0.5);
+	cubeExtents[m_data->m_upAxis] = 0;
+	int cubeId = registerCubeShape(cubeExtents[0],cubeExtents[1],cubeExtents[2]);
+
+	b3Quaternion orn(0,0,0,1);
+	b3Vector3 center=b3MakeVector3(0,0,0,1);
+	b3Vector3 scaling=b3MakeVector3(1,1,1,1);
+
+	for ( int i = 0; i < cells_x; i++) 
+	{
+		for (int j = 0; j < cells_z; j++) 
+		{
+			float* color =0;
+			if ((i + j) % 2 == 0) 
+			{
+				color = (float*)color0;
+			} else {
+				color = (float*)color1;
+			}
+			if (this->m_data->m_upAxis==1)
+			{
+				center =b3MakeVector3((i + 0.5f) - cells_x * 0.5f, 0.f, (j + 0.5f) - cells_z * 0.5f);
+			} else
+			{
+				center =b3MakeVector3((i + 0.5f) - cells_x * 0.5f, (j + 0.5f) - cells_z * 0.5f,0.f );
+			}
+			m_instancingRenderer->registerGraphicsInstance(cubeId,center,orn,color,scaling);
+		}
+	}
+	
+}
+
 
 int	SimpleOpenGL3App::registerGraphicsSphereShape(float radius, bool usePointSprites, int largeSphereThreshold, int mediumSphereThreshold)
 {
@@ -459,10 +686,10 @@ static void writeTextureToFile(int textureWidth, int textureHeight, const char*
 	{
 		for (int i=0;i<textureWidth;i++)
 		{
-			pixels[(j*textureWidth+i)*numComponents] = orgPixels[(j*textureWidth+i)*numComponents]*255.f;
+			pixels[(j*textureWidth+i)*numComponents] = char(orgPixels[(j*textureWidth+i)*numComponents]*255.f);
-			pixels[(j*textureWidth+i)*numComponents+1]=orgPixels[(j*textureWidth+i)*numComponents+1]*255.f;
+			pixels[(j*textureWidth+i)*numComponents+1]=char(orgPixels[(j*textureWidth+i)*numComponents+1]*255.f);
-			pixels[(j*textureWidth+i)*numComponents+2]=orgPixels[(j*textureWidth+i)*numComponents+2]*255.f;
+			pixels[(j*textureWidth+i)*numComponents+2]=char(orgPixels[(j*textureWidth+i)*numComponents+2]*255.f);
-			pixels[(j*textureWidth+i)*numComponents+3]=orgPixels[(j*textureWidth+i)*numComponents+3]*255.f;
+			pixels[(j*textureWidth+i)*numComponents+3]=char(orgPixels[(j*textureWidth+i)*numComponents+3]*255.f);
 		}
 	}
 
@@ -508,8 +735,8 @@ void SimpleOpenGL3App::swapBuffer()
 	m_window->endRendering();
 	if (m_data->m_frameDumpPngFileName)
     {
-        writeTextureToFile(m_window->getRetinaScale()*m_instancingRenderer->getScreenWidth(),
+        writeTextureToFile((int)m_window->getRetinaScale()*m_instancingRenderer->getScreenWidth(),
-                           m_window->getRetinaScale()*this->m_instancingRenderer->getScreenHeight(),m_data->m_frameDumpPngFileName,
+                          (int) m_window->getRetinaScale()*this->m_instancingRenderer->getScreenHeight(),m_data->m_frameDumpPngFileName,
                           m_data->m_ffmpegFile);
         //m_data->m_renderTexture->disable();
         //if (m_data->m_ffmpegFile==0)
@@ -522,8 +749,8 @@ void SimpleOpenGL3App::swapBuffer()
 // see also http://blog.mmacklin.com/2013/06/11/real-time-video-capture-with-ffmpeg/
 void SimpleOpenGL3App::dumpFramesToVideo(const char* mp4FileName)
 {
-    int width = m_window->getRetinaScale()*m_instancingRenderer->getScreenWidth();
+    int width = (int)m_window->getRetinaScale()*m_instancingRenderer->getScreenWidth();
-    int height = m_window->getRetinaScale()*m_instancingRenderer->getScreenHeight();
+    int height = (int)m_window->getRetinaScale()*m_instancingRenderer->getScreenHeight();
     char cmd[8192];
 
     sprintf(cmd,"ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - "

btgui/OpenGLWindow/SimpleOpenGL3App.h

@@ -19,9 +19,9 @@ struct SimpleOpenGL3App : public CommonGraphicsApp
 	SimpleOpenGL3App(const char* title, int width,int height);
 	virtual ~SimpleOpenGL3App();
 
-	int	registerCubeShape(float halfExtentsX=1.f,float halfExtentsY=1.f, float halfExtentsZ = 1.f);
+	virtual int	registerCubeShape(float halfExtentsX=1.f,float halfExtentsY=1.f, float halfExtentsZ = 1.f);
-	int	registerGraphicsSphereShape(float radius, bool usePointSprites=true, int largeSphereThreshold=100, int mediumSphereThreshold=10);
+	virtual int	registerGraphicsSphereShape(float radius, bool usePointSprites=true, int largeSphereThreshold=100, int mediumSphereThreshold=10);
-
+	virtual void registerGrid(int xres, int yres, float color0[4], float color1[4]);
     void dumpNextFrameToPng(const char* pngFilename);
     void dumpFramesToVideo(const char* mp4Filename);
     
@@ -31,6 +31,7 @@ struct SimpleOpenGL3App : public CommonGraphicsApp
 	
 	virtual void swapBuffer();
 	virtual void drawText( const char* txt, int posX, int posY);
+	virtual void drawText3D( const char* txt, float posX, float posZY, float posZ, float size);
 	struct sth_stash* getFontStash();
 
 

src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp

@@ -34,6 +34,7 @@ subject to the following restrictions:
 #include "BulletDynamics/ConstraintSolver/btHingeConstraint.h"
 #include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h"
 #include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h"
+#include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
 #include "BulletDynamics/ConstraintSolver/btSliderConstraint.h"
 #include "BulletDynamics/ConstraintSolver/btContactConstraint.h"
 
@@ -1306,6 +1307,57 @@ void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
 				}
 			}
 			break;
+		///note: the code for D6_SPRING_2_CONSTRAINT_TYPE is identical to D6_CONSTRAINT_TYPE, the D6_CONSTRAINT_TYPE+D6_SPRING_CONSTRAINT_TYPE will likely become obsolete/deprecated at some stage
+		case D6_SPRING_2_CONSTRAINT_TYPE:
+		{
+			{
+				btGeneric6DofSpring2Constraint* p6DOF = (btGeneric6DofSpring2Constraint*)constraint;
+				btTransform tr = p6DOF->getCalculatedTransformA();
+				if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+				tr = p6DOF->getCalculatedTransformB();
+				if (drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
+				if (drawLimits)
+				{
+					tr = p6DOF->getCalculatedTransformA();
+					const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin();
+					btVector3 up = tr.getBasis().getColumn(2);
+					btVector3 axis = tr.getBasis().getColumn(0);
+					btScalar minTh = p6DOF->getRotationalLimitMotor(1)->m_loLimit;
+					btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit;
+					btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit;
+					btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit;
+					getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0, 0, 0));
+					axis = tr.getBasis().getColumn(1);
+					btScalar ay = p6DOF->getAngle(1);
+					btScalar az = p6DOF->getAngle(2);
+					btScalar cy = btCos(ay);
+					btScalar sy = btSin(ay);
+					btScalar cz = btCos(az);
+					btScalar sz = btSin(az);
+					btVector3 ref;
+					ref[0] = cy*cz*axis[0] + cy*sz*axis[1] - sy*axis[2];
+					ref[1] = -sz*axis[0] + cz*axis[1];
+					ref[2] = cz*sy*axis[0] + sz*sy*axis[1] + cy*axis[2];
+					tr = p6DOF->getCalculatedTransformB();
+					btVector3 normal = -tr.getBasis().getColumn(0);
+					btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit;
+					btScalar maxFi = p6DOF->getRotationalLimitMotor(0)->m_hiLimit;
+					if (minFi > maxFi)
+					{
+						getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, -SIMD_PI, SIMD_PI, btVector3(0, 0, 0), false);
+					}
+					else if (minFi < maxFi)
+					{
+						getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, minFi, maxFi, btVector3(0, 0, 0), true);
+					}
+					tr = p6DOF->getCalculatedTransformA();
+					btVector3 bbMin = p6DOF->getTranslationalLimitMotor()->m_lowerLimit;
+					btVector3 bbMax = p6DOF->getTranslationalLimitMotor()->m_upperLimit;
+					getDebugDrawer()->drawBox(bbMin, bbMax, tr, btVector3(0, 0, 0));
+				}
+			}
+			break;
+		}
 		case SLIDER_CONSTRAINT_TYPE:
 			{
 				btSliderConstraint* pSlider = (btSliderConstraint*)constraint;

src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h

@@ -151,7 +151,7 @@ public:
 	virtual void	removeCollisionObject(btCollisionObject* collisionObject);
 
 
-	void	debugDrawConstraint(btTypedConstraint* constraint);
+	virtual void	debugDrawConstraint(btTypedConstraint* constraint);
 
 	virtual void	debugDrawWorld();
 

src/BulletDynamics/Featherstone/btMultiBody.cpp

@@ -137,16 +137,18 @@ void btMultiBody::setupFixed(int i,
 						   const btVector3 &inertia,
 						   int parent,
 						   const btQuaternion &rotParentToThis,
-						   const btVector3 &parentComToThisComOffset,
+						   const btVector3 &parentComToThisPivotOffset,
+                           const btVector3 &thisPivotToThisComOffset,
 						   bool disableParentCollision)
 {
 	btAssert(m_isMultiDof);
 
 	m_links[i].m_mass = mass;
-    m_links[i].m_inertia = inertia;
+    m_links[i].m_inertiaLocal = inertia;
     m_links[i].m_parent = parent;
     m_links[i].m_zeroRotParentToThis = rotParentToThis;
-    m_links[i].m_eVector = parentComToThisComOffset;    
+	m_links[i].m_dVector = thisPivotToThisComOffset;
+    m_links[i].m_eVector = parentComToThisPivotOffset;    
 
 	m_links[i].m_jointType = btMultibodyLink::eFixed;
 	m_links[i].m_dofCount = 0;
@@ -156,6 +158,7 @@ void btMultiBody::setupFixed(int i,
 		m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;    
 	//
 	m_links[i].updateCacheMultiDof();
+
 	//
 	//if(m_isMultiDof)
 	//	resizeInternalMultiDofBuffers();
@@ -180,7 +183,7 @@ void btMultiBody::setupPrismatic(int i,
 	}
 
     m_links[i].m_mass = mass;
-    m_links[i].m_inertia = inertia;
+    m_links[i].m_inertiaLocal = inertia;
     m_links[i].m_parent = parent;
     m_links[i].m_zeroRotParentToThis = rotParentToThis;
     m_links[i].setAxisTop(0, 0., 0., 0.);
@@ -223,7 +226,7 @@ void btMultiBody::setupRevolute(int i,
 	}
 
     m_links[i].m_mass = mass;
-    m_links[i].m_inertia = inertia;
+    m_links[i].m_inertiaLocal = inertia;
     m_links[i].m_parent = parent;
     m_links[i].m_zeroRotParentToThis = rotParentToThis;
     m_links[i].setAxisTop(0, jointAxis);
@@ -265,7 +268,7 @@ void btMultiBody::setupSpherical(int i,
 	m_posVarCnt += 4;
 
 	m_links[i].m_mass = mass;
-    m_links[i].m_inertia = inertia;
+    m_links[i].m_inertiaLocal = inertia;
     m_links[i].m_parent = parent;
     m_links[i].m_zeroRotParentToThis = rotParentToThis;    
     m_links[i].m_dVector = thisPivotToThisComOffset;
@@ -307,7 +310,7 @@ void btMultiBody::setupPlanar(int i,
 	m_posVarCnt += 3;
 
 	m_links[i].m_mass = mass;
-    m_links[i].m_inertia = inertia;
+    m_links[i].m_inertiaLocal = inertia;
     m_links[i].m_parent = parent;
     m_links[i].m_zeroRotParentToThis = rotParentToThis;    
 	m_links[i].m_dVector.setZero();
@@ -365,7 +368,7 @@ btScalar btMultiBody::getLinkMass(int i) const
 
 const btVector3 & btMultiBody::getLinkInertia(int i) const
 {
-    return m_links[i].m_inertia;
+    return m_links[i].m_inertiaLocal;
 }
 
 btScalar btMultiBody::getJointPos(int i) const
@@ -506,7 +509,7 @@ btScalar btMultiBody::getKineticEnergy() const
     
     for (int i = 0; i < num_links; ++i) {
         result += m_links[i].m_mass * vel[i+1].dot(vel[i+1]);
-        result += omega[i+1].dot(m_links[i].m_inertia * omega[i+1]);
+        result += omega[i+1].dot(m_links[i].m_inertiaLocal * omega[i+1]);
     }
 
     return 0.5f * result;
@@ -526,7 +529,7 @@ btVector3 btMultiBody::getAngularMomentum() const
 
     for (int i = 0; i < num_links; ++i) {
         rot_from_world[i+1] = m_links[i].m_cachedRotParentToThis * rot_from_world[m_links[i].m_parent+1];
-        result += (quatRotate(rot_from_world[i+1].inverse() , (m_links[i].m_inertia * omega[i+1])));
+        result += (quatRotate(rot_from_world[i+1].inverse() , (m_links[i].m_inertiaLocal * omega[i+1])));
     }
 
     return result;
@@ -1241,7 +1244,7 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
 		//
 		//adding damping terms (only)
 		btScalar linDampMult = 1., angDampMult = 1.;
-		zeroAccSpatFrc[i+1].addVector(angDampMult * m_links[i].m_inertia * spatVel[i+1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i+1].getAngular().norm()),
+		zeroAccSpatFrc[i+1].addVector(angDampMult * m_links[i].m_inertiaLocal * spatVel[i+1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i+1].getAngular().norm()),
 									 linDampMult * m_links[i].m_mass * spatVel[i+1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i+1].getLinear().norm()));
 		
         // calculate Ihat_i^A
@@ -1252,9 +1255,9 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
 												0, m_links[i].m_mass, 0,
 												0, 0, m_links[i].m_mass),
 									//
-									btMatrix3x3(m_links[i].m_inertia[0], 0, 0,
+									btMatrix3x3(m_links[i].m_inertiaLocal[0], 0, 0,
-												0, m_links[i].m_inertia[1], 0,
+												0, m_links[i].m_inertiaLocal[1], 0,
-												0, 0, m_links[i].m_inertia[2])
+												0, 0, m_links[i].m_inertiaLocal[2])
 								);
 		//
 		//p += vhat x Ihat vhat - done in a simpler way
@@ -1694,19 +1697,19 @@ void btMultiBody::stepVelocities(btScalar dt,
             - (rot_from_world[i+1] * m_links[i].m_appliedTorque);
 		if (m_useGyroTerm)
 		{
-			zero_acc_bottom_linear[i+1] += vel_top_angular[i+1].cross( m_links[i].m_inertia * vel_top_angular[i+1] );
+			zero_acc_bottom_linear[i+1] += vel_top_angular[i+1].cross( m_links[i].m_inertiaLocal * vel_top_angular[i+1] );
 		}
 
-        zero_acc_bottom_linear[i+1] += m_links[i].m_inertia * vel_top_angular[i+1] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[i+1].norm());
+        zero_acc_bottom_linear[i+1] += m_links[i].m_inertiaLocal * vel_top_angular[i+1] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[i+1].norm());
 
         // calculate Ihat_i^A
         inertia_top_left[i+1] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero();
         inertia_top_right[i+1].setValue(m_links[i].m_mass, 0, 0,
                                   0, m_links[i].m_mass, 0,
                                   0, 0, m_links[i].m_mass);
-        inertia_bottom_left[i+1].setValue(m_links[i].m_inertia[0], 0, 0,
+        inertia_bottom_left[i+1].setValue(m_links[i].m_inertiaLocal[0], 0, 0,
-                                    0, m_links[i].m_inertia[1], 0,
+                                    0, m_links[i].m_inertiaLocal[1], 0,
-                                    0, 0, m_links[i].m_inertia[2]);
+                                    0, 0, m_links[i].m_inertiaLocal[2]);
     }
 
 

src/BulletDynamics/Featherstone/btMultiBody.h

@@ -61,7 +61,8 @@ public:
 						   const btVector3 &inertia,
 						   int parent,
 						   const btQuaternion &rotParentToThis,
-						   const btVector3 &parentComToThisComOffset,
+						   const btVector3 &parentComToThisPivotOffset,
+                           const btVector3 &thisPivotToThisComOffset,
 						   bool disableParentCollision);
 
 

src/BulletDynamics/Featherstone/btMultiBodyConstraint.h

@@ -153,6 +153,7 @@ public:
 		m_maxAppliedImpulse = maxImp;
 	}
 	
+	virtual void debugDraw(class btIDebugDraw* drawer)=0;
 
 };
 

src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp

@@ -20,7 +20,7 @@ subject to the following restrictions:
 #include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
 #include "LinearMath/btQuickprof.h"
 #include "btMultiBodyConstraint.h"
-
+#include "LinearMath/btIDebugDraw.h"
 	
 
 
@@ -755,3 +755,92 @@ void	btMultiBodyDynamicsWorld::removeMultiBodyConstraint( btMultiBodyConstraint*
 {
 	m_multiBodyConstraints.remove(constraint);
 }
+
+void btMultiBodyDynamicsWorld::debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint)
+{
+	constraint->debugDraw(getDebugDrawer());
+}
+
+
+void	btMultiBodyDynamicsWorld::debugDrawWorld()
+{
+	BT_PROFILE("btMultiBodyDynamicsWorld debugDrawWorld");
+
+	bool drawConstraints = false;
+	if (getDebugDrawer())
+	{
+		int mode = getDebugDrawer()->getDebugMode();
+		if (mode  & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits))
+		{
+			drawConstraints = true;
+		}
+
+		if (drawConstraints)
+		{
+			BT_PROFILE("btMultiBody debugDrawWorld");
+			
+			btAlignedObjectArray<btQuaternion> world_to_local;
+			btAlignedObjectArray<btVector3> local_origin;
+
+			for (int c=0;c<m_multiBodyConstraints.size();c++)
+			{
+				btMultiBodyConstraint* constraint = m_multiBodyConstraints[c];
+				debugDrawMultiBodyConstraint(constraint);
+			}
+
+			for (int b = 0; b<m_multiBodies.size(); b++)
+			{
+				btMultiBody* bod = m_multiBodies[b];
+				int nLinks = bod->getNumLinks();
+
+				///base + num m_links
+				world_to_local.resize(nLinks + 1);
+				local_origin.resize(nLinks + 1);
+
+					
+				world_to_local[0] = bod->getWorldToBaseRot();
+				local_origin[0] = bod->getBasePos();
+
+				
+				{
+					btVector3 posr = local_origin[0];
+					//	float pos[4]={posr.x(),posr.y(),posr.z(),1};
+					btScalar quat[4] = { -world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w() };
+					btTransform tr;
+					tr.setIdentity();
+					tr.setOrigin(posr);
+					tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
+
+					getDebugDrawer()->drawTransform(tr, 0.1);
+
+				}
+
+				for (int k = 0; k<bod->getNumLinks(); k++)
+				{
+					const int parent = bod->getParent(k);
+					world_to_local[k + 1] = bod->getParentToLocalRot(k) * world_to_local[parent + 1];
+					local_origin[k + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[k + 1].inverse(), bod->getRVector(k)));
+				}
+
+
+				for (int m = 0; m<bod->getNumLinks(); m++)
+				{
+					int link = m;
+					int index = link + 1;
+
+					btVector3 posr = local_origin[index];
+					//			float pos[4]={posr.x(),posr.y(),posr.z(),1};
+					btScalar quat[4] = { -world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w() };
+					btTransform tr;
+					tr.setIdentity();
+					tr.setOrigin(posr);
+					tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
+
+					getDebugDrawer()->drawTransform(tr, 0.1);
+				}
+			}
+		}
+	}
+
+	btDiscreteDynamicsWorld::debugDrawWorld();
+}

src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h

@@ -54,5 +54,9 @@ public:
 	virtual void	removeMultiBodyConstraint( btMultiBodyConstraint* constraint);
 
 	virtual void	integrateTransforms(btScalar timeStep);
+
+	virtual void	debugDrawWorld();
+	
+	virtual void	debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint);
 };
 #endif //BT_MULTIBODY_DYNAMICS_WORLD_H

src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h

@@ -37,6 +37,10 @@ public:
 		btMultiBodyJacobianData& data,
 		const btContactSolverInfo& infoGlobal);
 
+	virtual void debugDraw(class btIDebugDraw* drawer)
+	{
+		//todo(erwincoumans)
+	}
 	
 };
 

src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h

@@ -46,6 +46,10 @@ public:
         m_desiredVelocity = velTarget;
     }
 
+	virtual void debugDraw(class btIDebugDraw* drawer)
+	{
+		//todo(erwincoumans)
+	}
 };
 
 #endif //BT_MULTIBODY_JOINT_MOTOR_H

src/BulletDynamics/Featherstone/btMultiBodyLink.h

@@ -355,7 +355,7 @@ struct btMultibodyLink
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
     btScalar m_mass;         // mass of link
-    btVector3 m_inertia;   // inertia of link (local frame; diagonal)
+    btVector3 m_inertiaLocal;   // inertia of link (local frame; diagonal)
 
     int m_parent;         // index of the parent link (assumed to be < index of this link), or -1 if parent is the base link.
 
@@ -440,15 +440,15 @@ struct btMultibodyLink
 	btMultibodyLink()
 		: 	m_mass(1),
 			m_parent(-1),
-			m_zeroRotParentToThis(1, 0, 0, 0),			
+			m_zeroRotParentToThis(0, 0, 0, 1),
-			m_cachedRotParentToThis(1, 0, 0, 0),			
+			m_cachedRotParentToThis(0, 0, 0, 1),
 			m_collider(0),
 			m_flags(0),
 			m_dofCount(0),
 			m_posVarCount(0),
 			m_jointType(btMultibodyLink::eInvalid)
 	{
-		m_inertia.setValue(1, 1, 1);
+		m_inertiaLocal.setValue(1, 1, 1);
 		setAxisTop(0, 0., 0., 0.);
 		setAxisBottom(0, 1., 0., 0.);
 		m_dVector.setValue(0, 0, 0);
@@ -493,7 +493,7 @@ struct btMultibodyLink
 			case ePrismatic:
 			{
 				// m_cachedRotParentToThis never changes, so no need to update
-				m_cachedRVector = m_eVector + pJointPos[0] * getAxisBottom(0);
+				m_cachedRVector = quatRotate(m_cachedRotParentToThis,m_eVector) + pJointPos[0] * getAxisBottom(0);
 
 				break;
 			}
@@ -516,7 +516,7 @@ struct btMultibodyLink
 			case eFixed:
 			{
 				m_cachedRotParentToThis = m_zeroRotParentToThis;
-				m_cachedRVector = quatRotate(m_cachedRotParentToThis,m_eVector);
+				m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector);
 
 				break;
 			}

src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp

@@ -18,6 +18,7 @@ subject to the following restrictions:
 #include "btMultiBodyPoint2Point.h"
 #include "btMultiBodyLinkCollider.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btIDebugDraw.h"
 
 #ifndef BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
 	#define BTMBP2PCONSTRAINT_DIM 3
@@ -177,3 +178,35 @@ int numDim = BTMBP2PCONSTRAINT_DIM;
 #endif
 	}
 }
+
+void btMultiBodyPoint2Point::debugDraw(class btIDebugDraw* drawer)
+{
+	btTransform tr;
+	tr.setIdentity();
+
+	if (m_rigidBodyA)
+	{
+		btVector3 pivot = m_rigidBodyA->getCenterOfMassTransform() * m_pivotInA;
+		tr.setOrigin(pivot);
+		drawer->drawTransform(tr, 0.1);
+	}
+	if (m_bodyA)
+	{
+		btVector3 pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
+		tr.setOrigin(pivotAworld);
+		drawer->drawTransform(tr, 0.1);
+	}
+	if (m_rigidBodyB)
+	{
+		// that ideally should draw the same frame
+		btVector3 pivot = m_rigidBodyB->getCenterOfMassTransform() * m_pivotInB;
+		tr.setOrigin(pivot);
+		drawer->drawTransform(tr, 0.1);
+	}
+	if (m_bodyB)
+	{
+		btVector3 pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
+		tr.setOrigin(pivotBworld);
+		drawer->drawTransform(tr, 0.1);
+	}
+}

src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h

@@ -56,6 +56,7 @@ public:
 		m_pivotInB = pivotInB;
 	}
 
+	virtual void debugDraw(class btIDebugDraw* drawer);
 	
 };
 

src/LinearMath/btQuickprof.cpp

@@ -43,6 +43,11 @@ static btClock gProfileClock;
 	#include <Xtl.h>
 #else //_XBOX
 	#include <windows.h>
+
+#if WINVER < 0x0600
+ULONGLONG GetTickCount64() { return GetTickCount(); }
+#endif
+
 #endif //_XBOX
 
 #include <time.h>