add basic debug drawing interface for btMultiBodyPoint2Point constraint

add basic debug drawing drawText3D in SimpleOpenGL3App
remove a few warnings
add drawTexturedRect3D to GLPrimitiveRenderer to support debug drawing

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

@@ -756,6 +756,11 @@ void	btMultiBodyDynamicsWorld::removeMultiBodyConstraint( btMultiBodyConstraint*
 	m_multiBodyConstraints.remove(constraint);
 }
 
+void btMultiBodyDynamicsWorld::debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint)
+{
+	constraint->debugDraw(getDebugDrawer());
+}
+
 
 void	btMultiBodyDynamicsWorld::debugDrawWorld()
 {
@@ -772,11 +777,17 @@ void	btMultiBodyDynamicsWorld::debugDrawWorld()
 
 		if (drawConstraints)
 		{
-			BT_PROFILE("btMultiBody stepPositions");
-			//integrate and update the Featherstone hierarchies
+			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];

src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h

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

src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h

@@ -36,7 +36,11 @@ public:
 	virtual void createConstraintRows(btMultiBodyConstraintArray& constraintRows,
 		btMultiBodyJacobianData& data,
 		const btContactSolverInfo& infoGlobal);
-	
+
+	virtual void debugDraw(class btIDebugDraw* drawer)
+	{
+		//todo(erwincoumans)
+	}
 	
 };
 

src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h

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

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
@@ -95,18 +96,18 @@ void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& co
 		const btContactSolverInfo& infoGlobal)
 {
 
-//	int i=1;
+//	int i=1;
 int numDim = BTMBP2PCONSTRAINT_DIM;
 	for (int i=0;i<numDim;i++)
 	{
 
 		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
-        //memset(&constraintRow,0xffffffff,sizeof(btMultiBodySolverConstraint));
-        constraintRow.m_relpos1CrossNormal.setValue(0,0,0);
-        constraintRow.m_contactNormal1.setValue(0,0,0);
-        constraintRow.m_relpos2CrossNormal.setValue(0,0,0);
-        constraintRow.m_contactNormal2.setValue(0,0,0);
-        constraintRow.m_angularComponentA.setValue(0,0,0);
+        //memset(&constraintRow,0xffffffff,sizeof(btMultiBodySolverConstraint));
+        constraintRow.m_relpos1CrossNormal.setValue(0,0,0);
+        constraintRow.m_contactNormal1.setValue(0,0,0);
+        constraintRow.m_relpos2CrossNormal.setValue(0,0,0);
+        constraintRow.m_contactNormal2.setValue(0,0,0);
+        constraintRow.m_angularComponentA.setValue(0,0,0);
         constraintRow.m_angularComponentB.setValue(0,0,0);
 
 		constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
@@ -154,8 +155,8 @@ int numDim = BTMBP2PCONSTRAINT_DIM;
 															posError,
 															infoGlobal,
 															-m_maxAppliedImpulse, m_maxAppliedImpulse
-															);
-    //@todo: support the case of btMultiBody versus btRigidBody,
+															);
+    //@todo: support the case of btMultiBody versus btRigidBody,
     //see btPoint2PointConstraint::getInfo2NonVirtual
 #else
 		const btVector3 dummy(0, 0, 0);
@@ -176,4 +177,36 @@ 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);
 	
 };