diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
index d3045fd3e..782a1d28d 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
@@ -242,7 +242,7 @@ void	btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScala
 }
 
 
-void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstraint& solverConstraint, 
+void btMultiBodyConstraint::fillMultiBodyConstraintMixed_old(btMultiBodySolverConstraint& solverConstraint, 
 																	btMultiBodyJacobianData& data,
 																 const btVector3& contactNormalOnB,
 																 const btVector3& posAworld, const btVector3& posBworld, 
@@ -251,8 +251,7 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr
 																 btScalar& relaxation,
 																 bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
 {
-			
-	
+
 	btVector3 rel_pos1 = posAworld;
 	btVector3 rel_pos2 = posBworld;
 
@@ -483,6 +482,359 @@ void btMultiBodyConstraint::fillMultiBodyConstraintMixed(btMultiBodySolverConstr
 	}
 
 
+	///warm starting (or zero if disabled)
+	/*
+	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
+	{
+		solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
+
+		if (solverConstraint.m_appliedImpulse)
+		{
+			if (multiBodyA)
+			{
+				btScalar impulse = solverConstraint.m_appliedImpulse;
+				btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
+				multiBodyA->applyDeltaVee(deltaV,impulse);
+				applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
+			} else
+			{
+				if (rb0)
+					bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
+			}
+			if (multiBodyB)
+			{
+				btScalar impulse = solverConstraint.m_appliedImpulse;
+				btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
+				multiBodyB->applyDeltaVee(deltaV,impulse);
+				applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
+			} else
+			{
+				if (rb1)
+					bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
+			}
+		}
+	} else
+	*/
+	{
+		solverConstraint.m_appliedImpulse = 0.f;
+	}
+
+	solverConstraint.m_appliedPushImpulse = 0.f;
+
+	{
+		
+
+		btScalar positionalError = 0.f;
+		btScalar	velocityError = restitution - rel_vel;// * damping;
+					
+
+		btScalar erp = infoGlobal.m_erp2;
+		if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
+		{
+			erp = infoGlobal.m_erp;
+		}
+
+		//commented out on purpose, see below
+		//if (penetration>0)
+		//{
+		//	positionalError = 0;
+		//	velocityError = -penetration / infoGlobal.m_timeStep;
+
+		//} else
+		//{
+		//	positionalError = -penetration * erp/infoGlobal.m_timeStep;
+		//}
+
+		//we cannot assume negative penetration to be the actual penetration and positive - speculative constraint (like for normal contact constraints)
+		//both are valid in general and definitely so in the case of a point2Point constraint
+		positionalError = -penetration * erp/infoGlobal.m_timeStep;
+
+		btScalar  penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
+		btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
+
+		if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
+		{
+			//combine position and velocity into rhs
+			solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
+			solverConstraint.m_rhsPenetration = 0.f;
+
+		} else
+		{
+			//split position and velocity into rhs and m_rhsPenetration
+			solverConstraint.m_rhs = velocityImpulse;
+			solverConstraint.m_rhsPenetration = penetrationImpulse;
+		}
+
+		solverConstraint.m_cfm = 0.f;
+		solverConstraint.m_lowerLimit = -m_maxAppliedImpulse;
+		solverConstraint.m_upperLimit = m_maxAppliedImpulse;
+	}
+
+}
+
+void btMultiBodyConstraint::fillMultiBodyConstraint(	btMultiBodySolverConstraint& solverConstraint, 
+															btMultiBodyJacobianData& data,
+															btScalar* jacOrgA, btScalar* jacOrgB,
+															const btVector3& contactNormalOnB,
+															const btVector3& posAworld, const btVector3& posBworld, 
+															btScalar position,
+															const btContactSolverInfo& infoGlobal,
+															btScalar& relaxation,
+															bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
+{
+			
+	
+	btVector3 rel_pos1 = posAworld;
+	btVector3 rel_pos2 = posBworld;
+
+	solverConstraint.m_multiBodyA = m_bodyA;
+	solverConstraint.m_multiBodyB = m_bodyB;
+	solverConstraint.m_linkA = m_linkA;
+	solverConstraint.m_linkB = m_linkB;
+	
+
+	btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
+	btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
+
+	const btVector3& pos1 = posAworld;
+	const btVector3& pos2 = posBworld;
+
+	btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA);
+	btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB);
+
+	btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
+	btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
+
+	if (bodyA)
+		rel_pos1 = pos1 - bodyA->getWorldTransform().getOrigin(); 
+	if (bodyB)
+		rel_pos2 = pos2 - bodyB->getWorldTransform().getOrigin();
+
+	relaxation = 1.f;
+
+	if (multiBodyA)
+	{
+		const int ndofA  = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
+
+		solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
+
+		if (solverConstraint.m_deltaVelAindex <0)
+		{
+			solverConstraint.m_deltaVelAindex = data.m_deltaVelocities.size();
+			multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
+			data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofA);
+		} else
+		{
+			btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
+		}
+
+		//determine jacobian of this 1D constraint
+		//resize..
+		solverConstraint.m_jacAindex = data.m_jacobians.size();
+		data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
+		//copy/determine
+		if(jacOrgA)
+		{
+			for (int i=0;i<ndofA;i++)
+				data.m_jacobians[solverConstraint.m_jacAindex+i] = jacOrgA[i];
+		}
+		else
+		{
+			btScalar* jac1=&data.m_jacobians[solverConstraint.m_jacAindex];
+			if(multiBodyA->isMultiDof())
+				multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
+			else
+				multiBodyA->fillContactJacobian(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
+		}
+
+		//determine the response of the multibody the constraint impulses of this constraint (i.e. multibody's inverse inertia with respect to this 1D constraint)
+		data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA);		//=> each constraint row has the constrained tree dofs allocated in m_deltaVelocitiesUnitImpulse
+		btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
+		btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
+
+		if(multiBodyA->isMultiDof())
+			multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
+		else
+			multiBodyA->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
+	}
+	else if(rb0)
+	{
+		btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB);
+		solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
+		solverConstraint.m_relpos1CrossNormal = torqueAxis0;
+		solverConstraint.m_contactNormal1 = contactNormalOnB;
+	}
+
+	if (multiBodyB)
+	{
+		const int ndofB  = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
+
+		solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
+		if (solverConstraint.m_deltaVelBindex <0)
+		{
+			solverConstraint.m_deltaVelBindex = data.m_deltaVelocities.size();
+			multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
+			data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB);
+		}
+
+		//determine jacobian of this 1D constraint
+		//resize..
+		solverConstraint.m_jacBindex = data.m_jacobians.size();
+		data.m_jacobians.resize(data.m_jacobians.size()+ndofB);
+		//copy/determine..
+		if(jacOrgB)
+		{
+			for (int i=0;i<ndofB;i++)
+				data.m_jacobians[solverConstraint.m_jacBindex+i] = jacOrgB[i];
+		}
+		else
+		{
+			if(multiBodyB->isMultiDof())
+				multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
+			else
+				multiBodyB->fillContactJacobian(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
+		}
+
+		//determine the response of the multibody the constraint impulses of this constraint (i.e. multibody's inverse inertia with respect to this 1D constraint)
+		//resize..
+		data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
+		btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
+		btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
+		//determine..
+		if(multiBodyB->isMultiDof())
+			multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v);
+		else
+			multiBodyB->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v);
+
+	}
+	else if(rb1)
+	{
+		btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);		
+		solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
+		solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
+		solverConstraint.m_contactNormal2 = -contactNormalOnB;
+	}
+
+	{
+						
+		btVector3 vec;
+		btScalar denom0 = 0.f;
+		btScalar denom1 = 0.f;
+		btScalar* jacB = 0;
+		btScalar* jacA = 0;
+		btScalar* lambdaA =0;
+		btScalar* lambdaB =0;
+		int ndofA  = 0;
+		if (multiBodyA)
+		{
+			ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
+			jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
+			lambdaA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
+			for (int i = 0; i < ndofA; ++i)
+			{
+				btScalar j = jacA[i] ;
+				btScalar l =lambdaA[i];
+				denom0 += j*l;
+			}
+		} else
+		{
+			if (rb0)
+			{
+				vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
+				denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec);
+			}
+		}
+		if (multiBodyB)
+		{
+			const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
+			jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
+			lambdaB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
+			for (int i = 0; i < ndofB; ++i)
+			{
+				btScalar j = jacB[i] ;
+				btScalar l =lambdaB[i];
+				denom1 += j*l;
+			}
+
+		} else
+		{
+			if (rb1)
+			{
+				vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
+				denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec);
+			}
+		}
+
+		 if (multiBodyA && (multiBodyA==multiBodyB))
+		 {
+            // ndof1 == ndof2 in this case
+            for (int i = 0; i < ndofA; ++i) 
+			{
+                denom1 += jacB[i] * lambdaA[i];
+                denom1 += jacA[i] * lambdaB[i];
+            }
+        }
+
+		 btScalar d = denom0+denom1;
+		 if (btFabs(d)>SIMD_EPSILON)
+		 {
+			 
+			 solverConstraint.m_jacDiagABInv = relaxation/(d);
+		 } else
+		 {
+			solverConstraint.m_jacDiagABInv  = 1.f;
+		 }
+		
+	}
+
+	
+	//compute rhs and remaining solverConstraint fields
+
+	
+
+	btScalar restitution = 0.f;
+	btScalar penetration = isFriction? 0 : position+infoGlobal.m_linearSlop;
+
+	btScalar rel_vel = 0.f;
+	int ndofA  = 0;
+	int ndofB  = 0;
+	{
+		btVector3 vel1,vel2;
+		if (multiBodyA)
+		{
+			ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
+			btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
+			for (int i = 0; i < ndofA ; ++i) 
+				rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
+		}
+		else if(rb0)
+		{
+			rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
+		}
+		if (multiBodyB)
+		{
+			ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
+			btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
+			for (int i = 0; i < ndofB ; ++i) 
+				rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
+
+		}
+		else if(rb1)
+		{
+			rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
+		}
+
+		solverConstraint.m_friction = 0.f;//cp.m_combinedFriction;
+
+				
+		restitution =  restitution * -rel_vel;//restitutionCurve(rel_vel, cp.m_combinedRestitution);
+		if (restitution <= btScalar(0.))
+		{
+			restitution = 0.f;
+		};
+	}
+
+
 	///warm starting (or zero if disabled)
 	/*
 	if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h
index 12b5a33f2..7b27df1c5 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h
+++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h
@@ -66,7 +66,7 @@ protected:
 
 	void	applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof);
 
-	void fillMultiBodyConstraintMixed(btMultiBodySolverConstraint& solverConstraint, 
+	void fillMultiBodyConstraintMixed_old(btMultiBodySolverConstraint& solverConstraint, 
 																	btMultiBodyJacobianData& data,
 																 const btVector3& contactNormalOnB,
 																 const btVector3& posAworld, const btVector3& posBworld, 
@@ -83,6 +83,16 @@ protected:
 														btScalar lowerLimit,
 														btScalar upperLimit);
 
+	void fillMultiBodyConstraint(btMultiBodySolverConstraint& solverConstraint, 
+																btMultiBodyJacobianData& data,
+																btScalar* jacOrgA, btScalar* jacOrgB,
+																const btVector3& contactNormalOnB,
+																const btVector3& posAworld, const btVector3& posBworld, 
+																btScalar position,
+																const btContactSolverInfo& infoGlobal,
+																btScalar& relaxation,
+																bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0);
+
 public:
 
 	btMultiBodyConstraint(btMultiBody* bodyA,btMultiBody* bodyB,int linkA, int linkB, int numRows, bool isUnilateral);
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp
index d4fa1d43b..13336ae56 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp
@@ -131,7 +131,7 @@ void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& co
 		}
 		btScalar position = (pivotAworld-pivotBworld).dot(contactNormalOnB);
 		btScalar relaxation = 1.f;
-		fillMultiBodyConstraintMixed(constraintRow, data,
+		fillMultiBodyConstraintMixed_old(constraintRow, data,
 																 contactNormalOnB,
 																 pivotAworld, pivotBworld, 
 																 position,
@@ -278,7 +278,7 @@ void btMultiBodyPoint2Point::createConstraintRows(btMultiBodyConstraintArray& co
 		m_bodyA->fillContactJacobianMultiDof_test(m_linkA, pivotAworld, normalAng, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
 		
 		
-		fillMultiBodyConstraintMixed(constraintRow, data,
+		fillMultiBodyConstraintMixed_old(constraintRow, data,
 																 contactNormalOnB,
 																 pivotAworld, pivotBworld, 
 																 position,
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h
index cae8fa917..c9bfbc176 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h
+++ b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h
@@ -20,7 +20,7 @@ subject to the following restrictions:
 
 #include "btMultiBodyConstraint.h"
 
-#define BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
+//#define BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST
 
 class btMultiBodyPoint2Point : public btMultiBodyConstraint
 {