From 6be2689f01a5feb5cc56be2755aea4d37c6fce01 Mon Sep 17 00:00:00 2001
From: "erwin.coumans" <erwin.coumans@08e121b0-ae19-0410-a57b-3be3395fd4fd>
Date: Fri, 5 Oct 2012 05:37:12 +0000
Subject: [PATCH] add joint feedback (applied force and applied torque for a
 constraint)

---
 Demos/ConstraintDemo/CMakeLists.txt           |  2 ++
 .../btSequentialImpulseConstraintSolver.cpp   | 22 ++++++++++++++++
 .../ConstraintSolver/btTypedConstraint.cpp    |  6 +++--
 .../ConstraintSolver/btTypedConstraint.h      | 26 +++++++++++++++++++
 4 files changed, 54 insertions(+), 2 deletions(-)

diff --git a/Demos/ConstraintDemo/CMakeLists.txt b/Demos/ConstraintDemo/CMakeLists.txt
index ad920d8c5..835769499 100644
--- a/Demos/ConstraintDemo/CMakeLists.txt
+++ b/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)
diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
index a8534d194..89cf46cfe 100644
--- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
+++ b/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())
 		{
diff --git a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp
index 06bde5e7e..465c0746c 100644
--- a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp
+++ b/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)
 {
 }
 
diff --git a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h b/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h
index 594fbcca6..441fa3750 100644
--- a/src/BulletDynamics/ConstraintSolver/btTypedConstraint.h
+++ b/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;