Add preliminary PhysX 4.0 backend for PyBullet

Add inverse dynamics / mass matrix code from DeepMimic, thanks to Xue Bin (Jason) Peng
Add example how to use stable PD control for humanoid with spherical joints (see humanoidMotionCapture.py)
Fix related to TinyRenderer object transforms not updating when using collision filtering

src/BulletCollision/CollisionDispatch/btCollisionObject.cpp

@@ -43,6 +43,7 @@ btCollisionObject::btCollisionObject()
 	  m_userObjectPointer(0),
 	  m_userIndex2(-1),
 	  m_userIndex(-1),
+	  m_userIndex3(-1),
 	  m_hitFraction(btScalar(1.)),
 	  m_ccdSweptSphereRadius(btScalar(0.)),
 	  m_ccdMotionThreshold(btScalar(0.)),

src/BulletCollision/CollisionDispatch/btCollisionObject.h

@@ -101,6 +101,8 @@ protected:
 
 	int m_userIndex;
 
+	int m_userIndex3;
+
 	///time of impact calculation
 	btScalar m_hitFraction;
 
@@ -526,6 +528,11 @@ public:
 		return m_userIndex2;
 	}
 
+	int getUserIndex3() const
+	{
+		return m_userIndex3;
+	}
+
 	///users can point to their objects, userPointer is not used by Bullet
 	void setUserPointer(void* userPointer)
 	{
@@ -543,6 +550,11 @@ public:
 		m_userIndex2 = index;
 	}
 
+	void setUserIndex3(int index)
+	{
+		m_userIndex3 = index;
+	}
+
 	int getUpdateRevisionInternal() const
 	{
 		return m_updateRevision;

src/BulletDynamics/Featherstone/btMultiBody.h

@@ -182,7 +182,10 @@ public:
 	// get/set pos/vel/rot/omega for the base link
 	//
 
-	const btVector3 &getBasePos() const { return m_basePos; }  // in world frame
+	const btVector3 &getBasePos() const 
+	{ 
+		return m_basePos; 
+	}  // in world frame
 	const btVector3 getBaseVel() const
 	{
 		return btVector3(m_realBuf[3], m_realBuf[4], m_realBuf[5]);

src/BulletInverseDynamics/MultiBodyTree.cpp

@@ -349,7 +349,7 @@ int MultiBodyTree::finalize()
 	const int &num_bodies = m_init_cache->numBodies();
 	const int &num_dofs = m_init_cache->numDoFs();
 
-	if (num_dofs <= 0)
+	if (num_dofs < 0)
 	{
 		bt_id_error_message("Need num_dofs>=1, but num_dofs= %d\n", num_dofs);
 		//return -1;

src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp

@@ -479,9 +479,14 @@ int MultiBodyTree::MultiBodyImpl::calculateKinematics(const vecx &q, const vecx
 		//todo: review
 		RigidBody &body = m_body_list[m_body_spherical_list[i]];
 
-		body.m_body_T_parent = transformZ(q(body.m_q_index + 2)) *
-							   transformY(q(body.m_q_index + 1)) *
-							   transformX(q(body.m_q_index));
+		mat33 T;
+
+		T = transformX(q(body.m_q_index)) *
+				transformY(q(body.m_q_index + 1)) *
+				transformZ(q(body.m_q_index + 2));
+		body.m_body_T_parent = T * body.m_body_T_parent_ref;
+			
+		body.m_parent_pos_parent_body.setValue(0,0,0);
 		body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body;
 
 		if (type >= POSITION_VELOCITY)
@@ -832,6 +837,22 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool
 
 			body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body;
 		}
+
+		for (idArrayIdx i = 0; i < m_body_spherical_list.size(); i++)
+		{
+			//todo: review
+			RigidBody &body = m_body_list[m_body_spherical_list[i]];
+
+			mat33 T;
+
+			T = transformX(q(body.m_q_index)) *
+				transformY(q(body.m_q_index + 1)) *
+				transformZ(q(body.m_q_index + 2));
+			body.m_body_T_parent = T * body.m_body_T_parent_ref;
+
+			body.m_parent_pos_parent_body.setValue(0, 0, 0);
+			body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body;
+		}
 	}
 	for (int i = m_body_list.size() - 1; i >= 0; i--)
 	{

src/LinearMath/btMatrixX.h

@@ -263,7 +263,10 @@ struct btMatrixX
 	{
 		{
 			BT_PROFILE("storage=0");
-			btSetZero(&m_storage[0], m_storage.size());
+			if (m_storage.size())
+			{
+				btSetZero(&m_storage[0], m_storage.size());
+			}
 			//memset(&m_storage[0],0,sizeof(T)*m_storage.size());
 			//for (int i=0;i<m_storage.size();i++)
 			//			m_storage[i]=0;