expose btMultiBodyConstraint applied impulse (force) on its degree of freedom(s),

only tested for btMultiBodyJointMotor for now.
See also MultiBody/MultiBodyConstraintFeedback example

examples/ExampleBrowser/CMakeLists.txt

@@ -98,6 +98,7 @@ SET(App_ExampleBrowser_SRCS
   ../Vehicles/Hinge2Vehicle.h
   ../MultiBody/TestJointTorqueSetup.cpp
   ../MultiBody/TestJointTorqueSetup.h
+ ../MultiBody/MultiBodyConstraintFeedback.cpp
   ../MultiBody/MultiDofDemo.cpp
   ../MultiBody/MultiDofDemo.h
   ../Constraints/TestHingeTorque.cpp

examples/ExampleBrowser/ExampleEntries.cpp

@@ -20,6 +20,7 @@
 #include "../Constraints/Dof6Spring2Setup.h"
 #include "../Constraints/ConstraintPhysicsSetup.h"
 #include "../MultiBody/TestJointTorqueSetup.h"
+#include "../MultiBody/MultiBodyConstraintFeedback.h"
 #include "../MultiBody/MultiDofDemo.h"
 #include "../VoronoiFracture/VoronoiFractureDemo.h"
 #include "../SoftDemo/SoftDemo.h"
@@ -83,7 +84,6 @@ static ExampleEntry gDefaultExamples[]=
 	ExampleEntry(1,"Motorized Hinge","Use of a btHingeConstraint. You can adjust the first slider to change the target velocity, and the second slider to adjust the maximum impulse applied to reach the target velocity. Note that the hinge angle can reach beyond -360 and 360 degrees.", ConstraintCreateFunc),
     ExampleEntry(1,"TestHingeTorque", "Apply a torque in the hinge axis. This example uses a btHingeConstraint and btRigidBody. The setup is similar to the multi body example TestJointTorque.",
                  TestHingeTorqueCreateFunc),
-    
 //	ExampleEntry(0,"What's new in 2.83"),
 	
 	ExampleEntry(1,"6DofSpring2","Show the use of the btGeneric6DofSpring2Constraint. This is a replacement of the btGeneric6DofSpringConstraint, it has various improvements. This includes improved spring implementation and better control over the restitution (bounce) when the constraint hits its limits.", 
@@ -99,6 +99,7 @@ static ExampleEntry gDefaultExamples[]=
 	ExampleEntry(0,"MultiBody"),
 	ExampleEntry(1,"MultiDofCreateFunc","Create a basic btMultiBody with 3-DOF spherical joints (mobilizers). The demo uses a fixed base or a floating base at restart.", MultiDofCreateFunc),
 	ExampleEntry(1,"TestJointTorque","Apply a torque to a btMultiBody with 1-DOF joints (mobilizers). This setup is similar to API/TestHingeTorque.", TestJointTorqueCreateFunc),
+ ExampleEntry(1,"Constraint Feedback", "The example shows how to receive joint reaction forces in a btMultiBody. Also the applied impulse is available for a btMultiBodyJointMotor", MultiBodyConstraintFeedbackCreateFunc),
 	
 	
 #ifdef INCLUDE_CLOTH_DEMOS

examples/ExampleBrowser/premake4.lua

@@ -73,6 +73,7 @@
 		"../Raycast/*",
 		"../MultiBody/MultiDofDemo.cpp",
 		"../MultiBody/TestJointTorqueSetup.cpp",
+		"../MultiBody/MultiBodyConstraintFeedback.cpp",
 		"../ThirdPartyLibs/stb_image/*",
 		"../ThirdPartyLibs/Wavefront/tiny_obj_loader.*",
 		"../ThirdPartyLibs/tinyxml/*",

examples/MultiBody/TestJointTorqueSetup.cpp

@@ -6,7 +6,7 @@
 							
 #include "../CommonInterfaces/CommonMultiBodyBase.h"
 
-btScalar radius(0.2);
+static btScalar radius(0.2);
 
 struct TestJointTorqueSetup : public CommonMultiBodyBase
 {
@@ -48,10 +48,16 @@ TestJointTorqueSetup::~TestJointTorqueSetup()
 
 ///this is a temporary global, until we determine if we need the option or not
 extern  bool gJointFeedbackInWorldSpace;
+extern bool gJointFeedbackInJointFrame;
+
+
+
 void TestJointTorqueSetup::initPhysics()
 {
     int upAxis = 1;
 	gJointFeedbackInWorldSpace = true;
+	gJointFeedbackInJointFrame = true;
+
 	m_guiHelper->setUpAxis(upAxis);
 
     btVector4 colors[4] =

src/BulletDynamics/Featherstone/btMultiBodyConstraint.h

@@ -58,6 +58,7 @@ protected:
 	btScalar		m_maxAppliedImpulse;
 
 
+    // warning: the data block lay out is not consistent for all constraints
     // data block laid out as follows:
     // cached impulses. (one per row.)
     // jacobians. (interleaved, row1 body1 then row1 body2 then row2 body 1 etc)
@@ -108,6 +109,19 @@ public:
 		return m_bodyB;
 	}
 
+	void	internalSetAppliedImpulse(int dof, btScalar appliedImpulse)
+	{
+		btAssert(dof>=0);
+		btAssert(dof < getNumRows());
+		m_data[dof] = appliedImpulse;
+	}
+	
+	btScalar	getAppliedImpulse(int dof)
+	{
+		btAssert(dof>=0);
+		btAssert(dof < getNumRows());
+		return m_data[dof];
+	}
 	// current constraint position
     // constraint is pos >= 0 for unilateral, or pos = 0 for bilateral
     // NOTE: ignored position for friction rows.

src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp

@@ -612,7 +612,9 @@ btMultiBodySolverConstraint&	btMultiBodyConstraintSolver::addMultiBodyFrictionCo
 {
 	BT_PROFILE("addMultiBodyFrictionConstraint");
 	btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing();
-	solverConstraint.m_useJointForce = false;
+    solverConstraint.m_orgConstraint = 0;
+    solverConstraint.m_orgDofIndex = -1;
+    
 	solverConstraint.m_frictionIndex = frictionIndex;
 	bool isFriction = true;
 
@@ -680,10 +682,11 @@ void	btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
 			int frictionIndex = m_multiBodyNormalContactConstraints.size();
 
 			btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints.expandNonInitializing();
-			solverConstraint.m_useJointForce = false;
 
 	//		btRigidBody* rb0 = btRigidBody::upcast(colObj0);
 	//		btRigidBody* rb1 = btRigidBody::upcast(colObj1);
+            solverConstraint.m_orgConstraint = 0;
+            solverConstraint.m_orgDofIndex = -1;
 			solverConstraint.m_solverBodyIdA = solverBodyIdA;
 			solverConstraint.m_solverBodyIdB = solverBodyIdB;
 			solverConstraint.m_multiBodyA = mbA;
@@ -892,6 +895,11 @@ void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolv
 	//bod->addBaseForce(m_gravity * bod->getBaseMass());
 	//bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
 
+	if (c.m_orgConstraint)
+	{
+		c.m_orgConstraint->internalSetAppliedImpulse(c.m_orgDofIndex,c.m_appliedImpulse);
+	}
+	
 
 	if (c.m_multiBodyA)
 	{

src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp

@@ -114,6 +114,9 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint
 		btScalar direction = row? -1 : 1;
 
 		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
+        constraintRow.m_orgConstraint = this;
+        constraintRow.m_orgDofIndex = row;
+        
 		constraintRow.m_multiBodyA = m_bodyA;
 		constraintRow.m_multiBodyB = m_bodyB;
 		const btScalar posError = 0;						//why assume it's zero?
@@ -123,7 +126,6 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint
 
 		if (m_bodyA->isMultiDof())
 		{
-			constraintRow.m_useJointForce = false;
 			//expect either prismatic or revolute joint type for now
 			btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
 			switch (m_bodyA->getLink(m_linkA).m_jointType)

src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp

@@ -116,10 +116,10 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con
 
 
 		fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,1,false,m_desiredVelocity);
-
+		constraintRow.m_orgConstraint = this;
+		constraintRow.m_orgDofIndex = row;
 		if (m_bodyA->isMultiDof())
 		{
-			constraintRow.m_useJointForce = false;
 			//expect either prismatic or revolute joint type for now
 			btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
 			switch (m_bodyA->getLink(m_linkA).m_jointType)

src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp

@@ -33,6 +33,7 @@ btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* body, int link, btRi
 	m_pivotInA(pivotInA),
 	m_pivotInB(pivotInB)
 {
+    m_data.resize(BTMBP2PCONSTRAINT_DIM);//at least store the applied impulses
 }
 
 btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB)
@@ -42,6 +43,7 @@ btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, bt
 	m_pivotInA(pivotInA),
 	m_pivotInB(pivotInB)
 {
+    m_data.resize(BTMBP2PCONSTRAINT_DIM);//at least store the applied impulses
 }
 
 void btMultiBodyPoint2Point::finalizeMultiDof()
@@ -108,7 +110,8 @@ int numDim = BTMBP2PCONSTRAINT_DIM;
 
 		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
         //memset(&constraintRow,0xffffffff,sizeof(btMultiBodySolverConstraint));
-		constraintRow.m_useJointForce = false;
+	constraintRow.m_orgConstraint = this;
+	constraintRow.m_orgDofIndex = i;
         constraintRow.m_relpos1CrossNormal.setValue(0,0,0);
         constraintRow.m_contactNormal1.setValue(0,0,0);
         constraintRow.m_relpos2CrossNormal.setValue(0,0,0);

src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h

@@ -20,6 +20,7 @@ subject to the following restrictions:
 #include "LinearMath/btAlignedObjectArray.h"
 
 class btMultiBody;
+class btMultiBodyConstraint;
 #include "BulletDynamics/ConstraintSolver/btSolverBody.h"
 #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
 
@@ -28,7 +29,7 @@ ATTRIBUTE_ALIGNED16 (struct)	btMultiBodySolverConstraint
 {
 	BT_DECLARE_ALIGNED_ALLOCATOR();
 
-	btMultiBodySolverConstraint() : m_solverBodyIdA(-1), m_multiBodyA(0), m_linkA(-1), m_solverBodyIdB(-1), m_multiBodyB(0), m_linkB(-1),m_useJointForce(false)
+	btMultiBodySolverConstraint() : m_solverBodyIdA(-1), m_multiBodyA(0), m_linkA(-1), m_solverBodyIdB(-1), m_multiBodyB(0), m_linkB(-1),m_orgConstraint(0), m_orgDofIndex(-1)
 	{}
 
 	int				m_deltaVelAindex;//more generic version of m_relpos1CrossNormal/m_contactNormal1
@@ -73,7 +74,9 @@ ATTRIBUTE_ALIGNED16 (struct)	btMultiBodySolverConstraint
 	btMultiBody* m_multiBodyB;
 	int			m_linkB;
 
-	bool		m_useJointForce;//needed for write-back of joint versus link/base force/torque
+	//for writing back applied impulses
+	btMultiBodyConstraint*	m_orgConstraint;
+	int m_orgDofIndex;
 
 	enum		btSolverConstraintType
 	{