add joint feedback (applied force and applied torque for a constraint)

Demos/ConstraintDemo/CMakeLists.txt

@@ -36,12 +36,14 @@ IF (USE_GLUT)
 					TARGET AppConstraintDemo
 					POST_BUILD
 					COMMAND ${CMAKE_COMMAND} ARGS -E copy_if_different ${BULLET_PHYSICS_SOURCE_DIR}/glut64.dll ${CMAKE_CURRENT_BINARY_DIR}
+					COMMAND ${CMAKE_COMMAND} ARGS -E copy_if_different ${BULLET_PHYSICS_SOURCE_DIR}/glut64.dll ${CMAKE_CURRENT_BINARY_DIR}/Debug
 				)
 			ELSE(CMAKE_CL_64)
 				ADD_CUSTOM_COMMAND(
 					TARGET AppConstraintDemo
 					POST_BUILD
 					COMMAND ${CMAKE_COMMAND} ARGS -E copy_if_different ${BULLET_PHYSICS_SOURCE_DIR}/GLUT32.DLL ${CMAKE_CURRENT_BINARY_DIR}
+					COMMAND ${CMAKE_COMMAND} ARGS -E copy_if_different ${BULLET_PHYSICS_SOURCE_DIR}/GLUT32.DLL ${CMAKE_CURRENT_BINARY_DIR}/Debug
 				)
 			ENDIF(CMAKE_CL_64)
 		ENDIF (NOT INTERNAL_CREATE_DISTRIBUTABLE_MSVC_PROJECTFILES)

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

@@ -1038,6 +1038,18 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 			for (i=0;i<numConstraints;i++)
 			{
 				btTypedConstraint::btConstraintInfo1& info1 = m_tmpConstraintSizesPool[i];
+				btJointFeedback* fb = constraints[i]->getJointFeedback();
+				if (fb)
+				{
+					fb->m_appliedForceBodyA.setZero();
+					fb->m_appliedTorqueBodyA.setZero();
+					fb->m_appliedForceBodyB.setZero();
+					fb->m_appliedTorqueBodyB.setZero();
+				}
+
+				if (constraints[i]->isEnabled())
+				{
+				}
 				if (constraints[i]->isEnabled())
 				{
 					constraints[i]->getInfo1(&info1);
@@ -1565,6 +1577,16 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(btCo
 	{
 		const btSolverConstraint& solverConstr = m_tmpSolverNonContactConstraintPool[j];
 		btTypedConstraint* constr = (btTypedConstraint*)solverConstr.m_originalContactPoint;
+		btJointFeedback* fb = constr->getJointFeedback();
+		if (fb)
+		{
+			fb->m_appliedForceBodyA += solverConstr.m_contactNormal*solverConstr.m_appliedImpulse*constr->getRigidBodyA().getLinearFactor()/infoGlobal.m_timeStep;
+			fb->m_appliedForceBodyB += -solverConstr.m_contactNormal*solverConstr.m_appliedImpulse*constr->getRigidBodyB().getLinearFactor()/infoGlobal.m_timeStep;
+			fb->m_appliedTorqueBodyA += solverConstr.m_relpos1CrossNormal* constr->getRigidBodyA().getAngularFactor()*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep;
+			fb->m_appliedTorqueBodyB += -solverConstr.m_relpos1CrossNormal* constr->getRigidBodyB().getAngularFactor()*solverConstr.m_appliedImpulse/infoGlobal.m_timeStep;
+			
+		}
+
 		constr->internalSetAppliedImpulse(solverConstr.m_appliedImpulse);
 		if (btFabs(solverConstr.m_appliedImpulse)>=constr->getBreakingImpulseThreshold())
 		{

src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp

@@ -32,7 +32,8 @@ m_overrideNumSolverIterations(-1),
 m_rbA(rbA),
 m_rbB(getFixedBody()),
 m_appliedImpulse(btScalar(0.)),
-m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
+m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE),
+m_jointFeedback(0)
 {
 }
 
@@ -48,7 +49,8 @@ m_overrideNumSolverIterations(-1),
 m_rbA(rbA),
 m_rbB(rbB),
 m_appliedImpulse(btScalar(0.)),
-m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
+m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE),
+m_jointFeedback(0)
 {
 }
 

src/BulletDynamics/ConstraintSolver/btTypedConstraint.h

@@ -53,6 +53,15 @@ enum btConstraintParams
 #endif
 
 
+ATTRIBUTE_ALIGNED16(struct)	btJointFeedback
+{
+	btVector3	m_appliedForceBodyA;
+	btVector3	m_appliedTorqueBodyA;
+	btVector3	m_appliedForceBodyB;
+	btVector3	m_appliedTorqueBodyB;
+};
+
+
 ///TypedConstraint is the baseclass for Bullet constraints and vehicles
 ATTRIBUTE_ALIGNED16(class) btTypedConstraint : public btTypedObject
 {
@@ -82,6 +91,7 @@ protected:
 	btRigidBody&	m_rbB;
 	btScalar	m_appliedImpulse;
 	btScalar	m_dbgDrawSize;
+	btJointFeedback*	m_jointFeedback;
 
 	///internal method used by the constraint solver, don't use them directly
 	btScalar getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact);
@@ -250,6 +260,22 @@ public:
 		return m_userConstraintPtr;
 	}
 
+	void	setJointFeedback(btJointFeedback* jointFeedback)
+	{
+		m_jointFeedback = jointFeedback;
+	}
+
+	const btJointFeedback* getJointFeedback() const
+	{
+		return m_jointFeedback;
+	}
+
+	btJointFeedback* getJointFeedback()
+	{
+		return m_jointFeedback;
+	}
+
+
 	int getUid() const
 	{
 		return m_userConstraintId;