1 files changed, 61 insertions, 3 deletions
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp
index 6e8b552dbbc..3fa98ee4c88 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp
@@ -19,13 +19,16 @@ subject to the following restrictions:
 
 static btRigidBody s_fixed(0, 0,0);
 
+#define DEFAULT_DEBUGDRAW_SIZE btScalar(0.3f)
+
 btTypedConstraint::btTypedConstraint(btTypedConstraintType type)
 :m_userConstraintType(-1),
 m_userConstraintId(-1),
 m_constraintType (type),
 m_rbA(s_fixed),
 m_rbB(s_fixed),
-m_appliedImpulse(btScalar(0.))
+m_appliedImpulse(btScalar(0.)),
+m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
 {
 	s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
 }
@@ -35,7 +38,8 @@ m_userConstraintId(-1),
 m_constraintType (type),
 m_rbA(rbA),
 m_rbB(s_fixed),
-m_appliedImpulse(btScalar(0.))
+m_appliedImpulse(btScalar(0.)),
+m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
 {
 		s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
 
@@ -48,9 +52,63 @@ m_userConstraintId(-1),
 m_constraintType (type),
 m_rbA(rbA),
 m_rbB(rbB),
-m_appliedImpulse(btScalar(0.))
+m_appliedImpulse(btScalar(0.)),
+m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
 {
 		s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
 
 }
 
+
+//-----------------------------------------------------------------------------
+
+btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScalar uppLim, btScalar vel, btScalar timeFact)
+{
+	if(lowLim > uppLim)
+	{
+		return btScalar(1.0f);
+	}
+	else if(lowLim == uppLim)
+	{
+		return btScalar(0.0f);
+	}
+	btScalar lim_fact = btScalar(1.0f);
+	btScalar delta_max = vel / timeFact;
+	if(delta_max < btScalar(0.0f))
+	{
+		if((pos >= lowLim) && (pos < (lowLim - delta_max)))
+		{
+			lim_fact = (lowLim - pos) / delta_max;
+		}
+		else if(pos  < lowLim)
+		{
+			lim_fact = btScalar(0.0f);
+		}
+		else
+		{
+			lim_fact = btScalar(1.0f);
+		}
+	}
+	else if(delta_max > btScalar(0.0f))
+	{
+		if((pos <= uppLim) && (pos > (uppLim - delta_max)))
+		{
+			lim_fact = (uppLim - pos) / delta_max;
+		}
+		else if(pos  > uppLim)
+		{
+			lim_fact = btScalar(0.0f);
+		}
+		else
+		{
+			lim_fact = btScalar(1.0f);
+		}
+	}
+	else
+	{
+			lim_fact = btScalar(0.0f);
+	}
+	return lim_fact;
+} // btTypedConstraint::getMotorFactor()
+
+