From f22ceecb82786a9e350ebdb269a89982156ea6f3 Mon Sep 17 00:00:00 2001
From: "erwin.coumans" <erwin.coumans@08e121b0-ae19-0410-a57b-3be3395fd4fd>
Date: Tue, 22 Oct 2013 21:06:47 +0000
Subject: [PATCH] Fix Featherstone btMultiBodyPoint2Point constraint and
 picking: don't assume body 0 is the fixed body!

---
 src/BulletDynamics/Featherstone/btMultiBodyConstraint.h  | 1 +
 .../Featherstone/btMultiBodyConstraintSolver.cpp         | 2 ++
 .../Featherstone/btMultiBodyPoint2Point.cpp              | 9 ++++++++-
 3 files changed, 11 insertions(+), 1 deletion(-)

diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h
index ba68fd305..8f744c4e4 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h
+++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h
@@ -34,6 +34,7 @@ struct btMultiBodyJacobianData
 	btAlignedObjectArray<btVector3>		scratch_v;
 	btAlignedObjectArray<btMatrix3x3>	scratch_m;
 	btAlignedObjectArray<btSolverBody>*	m_solverBodyPool;
+	int									m_fixedBodyId;
 
 };
 
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
index 5e0943bac..2a5dc815c 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
@@ -765,6 +765,8 @@ void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifol
 	{
 		btMultiBodyConstraint* c = m_tmpMultiBodyConstraints[i];
 		m_data.m_solverBodyPool = &m_tmpSolverBodyPool;
+		m_data.m_fixedBodyId = m_fixedBodyId;
+		
 		c->createConstraintRows(m_multiBodyNonContactConstraints,m_data,	infoGlobal);
 	}
 
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp
index a51ac346a..7b89bf2ce 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp
@@ -97,7 +97,10 @@ void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& co
 
 		btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
 
-		constraintRow.m_solverBodyIdB = 0;
+		constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
+		constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
+		
+
 		btVector3 contactNormalOnB(0,0,0);
 		contactNormalOnB[i] = -1;
 
@@ -107,6 +110,8 @@ void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& co
 		btVector3 pivotAworld = m_pivotInA;
 		if (m_rigidBodyA)
 		{
+			
+			constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
 			pivotAworld = m_rigidBodyA->getCenterOfMassTransform()*m_pivotInA;
 		} else
 		{
@@ -116,11 +121,13 @@ void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& co
 		btVector3 pivotBworld = m_pivotInB;
 		if (m_rigidBodyB)
 		{
+			constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
 			pivotBworld = m_rigidBodyB->getCenterOfMassTransform()*m_pivotInB;
 		} else
 		{
 			if (m_bodyB)
 				pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
+			
 		}
 		btScalar position = (pivotAworld-pivotBworld).dot(contactNormalOnB);
 		btScalar relaxation = 1.f;