Added btMultiBodyPoint2Point, it can be used between btMultiBody vs btMultiBody or btMultiBody vs btRigidBody

Allow picking of btMultiBody, using a btMultiBodyPoint2Point constraint, with limited strength to avoid adding too much energy to the system (= blowup)
Add btMultiBodyJointMotor, it can be used in combination with joint limit (just add the joint limit after the motor, to avoid jitter)

src/BulletDynamics/Featherstone/btMultiBody.h

@@ -126,14 +126,15 @@ public:
     //
 
     const btVector3 & getBasePos() const { return base_pos; }    // in world frame
-    btVector3 getBaseVel() const { 
-		return btVector3(real_buf[3],real_buf[4],real_buf[5]); 
+    const btVector3 getBaseVel() const 
+	{ 
+		return btVector3(m_real_buf[3],m_real_buf[4],m_real_buf[5]); 
 	}     // in world frame
     const btQuaternion & getWorldToBaseRot() const 
 	{ 
 		return base_quat; 
 	}     // rotates world vectors into base frame
-    btVector3 getBaseOmega() const { return btVector3(real_buf[0],real_buf[1],real_buf[2]); }   // in world frame
+    btVector3 getBaseOmega() const { return btVector3(m_real_buf[0],m_real_buf[1],m_real_buf[2]); }   // in world frame
 
     void setBasePos(const btVector3 &pos) 
 	{ 
@@ -141,13 +142,19 @@ public:
 	}
     void setBaseVel(const btVector3 &vel) 
 	{ 
-		real_buf[3]=vel[0]; real_buf[4]=vel[1]; real_buf[5]=vel[2]; 
+
+		m_real_buf[3]=vel[0]; m_real_buf[4]=vel[1]; m_real_buf[5]=vel[2]; 
 	}
     void setWorldToBaseRot(const btQuaternion &rot) 
 	{ 
 		base_quat = rot; 
 	}
-    void setBaseOmega(const btVector3 &omega) { real_buf[0]=omega[0]; real_buf[1]=omega[1]; real_buf[2]=omega[2]; }
+    void setBaseOmega(const btVector3 &omega) 
+	{ 
+		m_real_buf[0]=omega[0]; 
+		m_real_buf[1]=omega[1]; 
+		m_real_buf[2]=omega[2]; 
+	}
 
 
     //
@@ -166,13 +173,13 @@ public:
     //
     const btScalar * getVelocityVector() const 
 	{ 
-		return &real_buf[0]; 
+		return &m_real_buf[0]; 
 	}
-    btScalar * getVelocityVector() 
+/*    btScalar * getVelocityVector() 
 	{ 
 		return &real_buf[0]; 
 	}
-    
+  */  
 
     //
     // get the frames of reference (positions and orientations) of the child links
@@ -258,13 +265,38 @@ public:
     // apply a delta-vee directly. used in sequential impulses code.
     void applyDeltaVee(const btScalar * delta_vee) 
 	{
+
         for (int i = 0; i < 6 + getNumLinks(); ++i) 
-			real_buf[i] += delta_vee[i];
+		{
+			m_real_buf[i] += delta_vee[i];
+		}
+		
     }
     void applyDeltaVee(const btScalar * delta_vee, btScalar multiplier) 
 	{
-        for (int i = 0; i < 6 + getNumLinks(); ++i) 
-			real_buf[i] += delta_vee[i] * multiplier;
+		btScalar sum = 0;
+        for (int i = 0; i < 6 + getNumLinks(); ++i)
+		{
+			sum += delta_vee[i]*multiplier*delta_vee[i]*multiplier;
+		}
+		btScalar l = btSqrt(sum);
+		static btScalar maxl = -1e30f;
+		if (l>maxl)
+		{
+			maxl=l;
+			printf("maxl=%f\n",maxl);
+		}
+		if (l>100)
+		{
+			printf("exceeds 100: l=%f\n",maxl);
+			multiplier *= 100.f/l;
+		}
+
+        for (int i = 0; i < 6 + getNumLinks(); ++i)
+		{
+			sum += delta_vee[i]*multiplier*delta_vee[i]*multiplier;
+			m_real_buf[i] += delta_vee[i] * multiplier;
+		}
     }
 
     // timestep the positions (given current velocities).
@@ -290,6 +322,10 @@ public:
     //
     // sleeping
     //
+	void	setCanSleep(bool canSleep)
+	{
+		can_sleep = canSleep;
+	}
 
     bool isAwake() const { return awake; }
     void wakeUp();
@@ -315,6 +351,29 @@ public:
 	{
 		links.resize(numLinks);
 	}
+
+	btScalar getLinearDamping() const
+	{
+			return m_linearDamping;
+	}
+	void setLinearDamping( btScalar damp)
+	{
+		m_linearDamping = damp;
+	}
+	btScalar getAngularDamping() const
+	{
+		return m_angularDamping;
+	}
+		
+	bool getUseGyroTerm() const
+	{
+		return m_useGyroTerm;
+	}
+	void setUseGyroTerm(bool useGyro)
+	{
+		m_useGyroTerm = useGyro;
+	}
+
 private:
     btMultiBody(const btMultiBody &);  // not implemented
     void operator=(const btMultiBody &);  // not implemented
@@ -356,7 +415,7 @@ private:
     //   0              num_links+1  rot_from_parent
     //
     
-    btAlignedObjectArray<btScalar> real_buf;
+    btAlignedObjectArray<btScalar> m_real_buf;
     btAlignedObjectArray<btVector3> vector_buf;
     btAlignedObjectArray<btMatrix3x3> matrix_buf;
 
@@ -375,6 +434,10 @@ private:
     btScalar sleep_timer;
 
 	int	m_companionId;
+	btScalar	m_linearDamping;
+	btScalar	m_angularDamping;
+	bool	m_useGyroTerm;
+
 };
 
 #endif