First step in btMultiBody joint force/torque feedback. There is still some work to be done for 'mobilizer limit/motor'.

See examples/MultiBody/TestJointTorqueSetup and examples/Constraints/TestHingeTorque for joint feedback.

src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h

@@ -38,7 +38,7 @@ protected:
 	virtual void	calculateSimulationIslands();
 	virtual void	updateActivationState(btScalar timeStep);
 	virtual void	solveConstraints(btContactSolverInfo& solverInfo);
-
+	
 public:
 
 	btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
@@ -49,6 +49,16 @@ public:
 
 	virtual void	removeMultiBody(btMultiBody* body);
 
+	virtual int		getNumMultibodies() const
+	{
+		return m_multiBodies.size();
+	}
+
+	btMultiBody*	getMultiBody(int mbIndex)
+	{
+		return m_multiBodies[mbIndex];
+	}
+
 	virtual void	addMultiBodyConstraint( btMultiBodyConstraint* constraint);
 
 	virtual int     getNumMultiBodyConstraints() const
@@ -73,5 +83,7 @@ public:
 	virtual void	debugDrawWorld();
 
 	virtual void	debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint);
+	
+	void	forwardKinematics();
 };
 #endif //BT_MULTIBODY_DYNAMICS_WORLD_H