1 files changed, 119 insertions, 23 deletions

diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp
index 3fa98ee4c88..8212c36ff4e 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btTypedConstraint.cpp
@@ -16,51 +16,38 @@ subject to the following restrictions:
 
 #include "btTypedConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
+#include "LinearMath/btSerializer.h"
 
-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_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
-{
-	s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
-}
 btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA)
-:m_userConstraintType(-1),
+:btTypedObject(type),
+m_userConstraintType(-1),
 m_userConstraintId(-1),
-m_constraintType (type),
+m_needsFeedback(false),
 m_rbA(rbA),
-m_rbB(s_fixed),
+m_rbB(getFixedBody()),
 m_appliedImpulse(btScalar(0.)),
 m_dbgDrawSize(DEFAULT_DEBUGDRAW_SIZE)
 {
-		s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
-
 }
 
 
 btTypedConstraint::btTypedConstraint(btTypedConstraintType type, btRigidBody& rbA,btRigidBody& rbB)
-:m_userConstraintType(-1),
+:btTypedObject(type),
+m_userConstraintType(-1),
 m_userConstraintId(-1),
-m_constraintType (type),
+m_needsFeedback(false),
 m_rbA(rbA),
 m_rbB(rbB),
 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)
 {
@@ -109,6 +96,115 @@ btScalar btTypedConstraint::getMotorFactor(btScalar pos, btScalar lowLim, btScal
 			lim_fact = btScalar(0.0f);
 	}
 	return lim_fact;
-} // btTypedConstraint::getMotorFactor()
+}
+
+///fills the dataBuffer and returns the struct name (and 0 on failure)
+const char*	btTypedConstraint::serialize(void* dataBuffer, btSerializer* serializer) const
+{
+	btTypedConstraintData* tcd = (btTypedConstraintData*) dataBuffer;
+
+	tcd->m_rbA = (btRigidBodyData*)serializer->getUniquePointer(&m_rbA);
+	tcd->m_rbB = (btRigidBodyData*)serializer->getUniquePointer(&m_rbB);
+	char* name = (char*) serializer->findNameForPointer(this);
+	tcd->m_name = (char*)serializer->getUniquePointer(name);
+	if (tcd->m_name)
+	{
+		serializer->serializeName(name);
+	}
+
+	tcd->m_objectType = m_objectType;
+	tcd->m_needsFeedback = m_needsFeedback;
+	tcd->m_userConstraintId =m_userConstraintId;
+	tcd->m_userConstraintType =m_userConstraintType;
+
+	tcd->m_appliedImpulse = float(m_appliedImpulse);
+	tcd->m_dbgDrawSize = float(m_dbgDrawSize );
+
+	tcd->m_disableCollisionsBetweenLinkedBodies = false;
+
+	int i;
+	for (i=0;i<m_rbA.getNumConstraintRefs();i++)
+		if (m_rbA.getConstraintRef(i) == this)
+			tcd->m_disableCollisionsBetweenLinkedBodies = true;
+	for (i=0;i<m_rbB.getNumConstraintRefs();i++)
+		if (m_rbB.getConstraintRef(i) == this)
+			tcd->m_disableCollisionsBetweenLinkedBodies = true;
+
+	return "btTypedConstraintData";
+}
+
+btRigidBody& btTypedConstraint::getFixedBody()
+{
+	static btRigidBody s_fixed(0, 0,0);
+	s_fixed.setMassProps(btScalar(0.),btVector3(btScalar(0.),btScalar(0.),btScalar(0.)));
+	return s_fixed;
+}
+
+
+void btAngularLimit::set(btScalar low, btScalar high, btScalar _softness, btScalar _biasFactor, btScalar _relaxationFactor)
+{
+	m_halfRange = (high - low) / 2.0f;
+	m_center = btNormalizeAngle(low + m_halfRange);
+	m_softness =  _softness;
+	m_biasFactor = _biasFactor;
+	m_relaxationFactor = _relaxationFactor;
+}
+
+void btAngularLimit::test(const btScalar angle)
+{
+	m_correction = 0.0f;
+	m_sign = 0.0f;
+	m_solveLimit = false;
+
+	if (m_halfRange >= 0.0f)
+	{
+		btScalar deviation = btNormalizeAngle(angle - m_center);
+		if (deviation < -m_halfRange)
+		{
+			m_solveLimit = true;
+			m_correction = - (deviation + m_halfRange);
+			m_sign = +1.0f;
+		}
+		else if (deviation > m_halfRange)
+		{
+			m_solveLimit = true;
+			m_correction = m_halfRange - deviation;
+			m_sign = -1.0f;
+		}
+	}
+}
 
 
+btScalar btAngularLimit::getError() const
+{
+	return m_correction * m_sign;
+}
+
+void btAngularLimit::fit(btScalar& angle) const
+{
+	if (m_halfRange > 0.0f)
+	{
+		btScalar relativeAngle = btNormalizeAngle(angle - m_center);
+		if (!btEqual(relativeAngle, m_halfRange))
+		{
+			if (relativeAngle > 0.0f)
+			{
+				angle = getHigh();
+			}
+			else
+			{
+				angle = getLow();
+			}
+		}
+	}
+}
+
+btScalar btAngularLimit::getLow() const
+{
+	return btNormalizeAngle(m_center - m_halfRange);
+}
+
+btScalar btAngularLimit::getHigh() const
+{
+	return btNormalizeAngle(m_center + m_halfRange);
+}