+ renamed pad3 to m_contactProcessingThreshold (thanks Campbell Barton/ideasman for confirming it is ok to rename it)

+ fixed Python method, PyArg_ParseTuple already checks for errors, no returning of NULL, thanks Campbell too)
+ added linear/angular spring for each of the 6DOFs of a generic 6dof constraint.  This makes the generic 6dof constraint very versatile.

2 files changed, 135 insertions, 1 deletions

diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
index 9644f2b6d57..2edf28c6bac 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
@@ -22,9 +22,11 @@ http://gimpact.sf.net
 #include "btGeneric6DofConstraint.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include "LinearMath/btTransformUtil.h"
+#include "LinearMath/btTransformUtil.h"
 #include <new>
 
 
+
 #define D6_USE_OBSOLETE_METHOD false
 
 
@@ -738,7 +740,9 @@ int btGeneric6DofConstraint::get_limit_motor_info2(
         }
         // if we're limited low and high simultaneously, the joint motor is
         // ineffective
-        if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = 0;
+        if (limit && (limot->m_loLimit == limot->m_hiLimit)) 
+			powered = 0;
+
         info->m_constraintError[srow] = btScalar(0.f);
         if (powered)
         {
@@ -827,3 +831,117 @@ int btGeneric6DofConstraint::get_limit_motor_info2(
 
 
 
+btGeneric6DofSpringConstraint::btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA)
+	: btGeneric6DofConstraint(rbA, rbB, frameInA, frameInB, useLinearReferenceFrameA)
+{
+	for(int i = 0; i < 6; i++)
+	{
+		m_springEnabled[i] = false;
+		m_equilibriumPoint[i] = btScalar(0.f);
+		m_springStiffness[i] = btScalar(0.f);
+	}
+}
+
+
+void btGeneric6DofSpringConstraint::enableSpring(int index, bool onOff)
+{
+	btAssert((index >= 0) && (index < 6));
+	m_springEnabled[index] = onOff;
+	if(index < 3)
+	{
+		m_linearLimits.m_enableMotor[index] = onOff;
+	}
+	else
+	{
+		m_angularLimits[index - 3].m_enableMotor = onOff;
+	}
+}
+
+
+
+void btGeneric6DofSpringConstraint::setStiffness(int index, btScalar stiffness)
+{
+	btAssert((index >= 0) && (index < 6));
+	m_springStiffness[index] = stiffness;
+}
+
+
+void btGeneric6DofSpringConstraint::setEquilibriumPoint()
+{
+	calculateTransforms();
+	for(int i = 0; i < 3; i++)
+	{
+		m_equilibriumPoint[i] = m_calculatedLinearDiff[i];
+	}
+	for(int i = 0; i < 3; i++)
+	{
+		m_equilibriumPoint[i + 3] = m_calculatedAxisAngleDiff[i];
+	}
+}
+
+
+
+void btGeneric6DofSpringConstraint::setEquilibriumPoint(int index)
+{
+	btAssert((index >= 0) && (index < 6));
+	calculateTransforms();
+	if(index < 3)
+	{
+		m_equilibriumPoint[index] = m_calculatedLinearDiff[index];
+	}
+	else
+	{
+		m_equilibriumPoint[index + 3] = m_calculatedAxisAngleDiff[index];
+	}
+}
+
+
+
+void btGeneric6DofSpringConstraint::internalUpdateSprings(btConstraintInfo2* info)
+{
+	// it is assumed that calculateTransforms() have been called before this call
+	int i;
+	btVector3 relVel = m_rbB.getLinearVelocity() - m_rbA.getLinearVelocity();
+	for(i = 0; i < 3; i++)
+	{
+		if(m_springEnabled[i])
+		{
+			// get current position of constraint
+			btScalar currPos = m_calculatedLinearDiff[i];
+			// calculate difference
+			btScalar delta = currPos - m_equilibriumPoint[i];
+			// spring force is (delta * m_stiffness) according to Hooke's Law
+			btScalar force = delta * m_springStiffness[i];
+			m_linearLimits.m_targetVelocity[i] = force  * info->fps;
+			m_linearLimits.m_maxMotorForce[i] = btFabs(force) / info->fps;
+		}
+	}
+	for(i = 0; i < 3; i++)
+	{
+		if(m_springEnabled[i + 3])
+		{
+			// get current position of constraint
+			btScalar currPos = m_calculatedAxisAngleDiff[i];
+			// calculate difference
+			btScalar delta = currPos - m_equilibriumPoint[i+3];
+			// spring force is (-delta * m_stiffness) according to Hooke's Law
+			btScalar force = -delta * m_springStiffness[i+3];
+			m_angularLimits[i].m_targetVelocity = force  * info->fps;
+			m_angularLimits[i].m_maxMotorForce = btFabs(force) / info->fps;
+		}
+	}
+}
+
+
+void btGeneric6DofSpringConstraint::getInfo2(btConstraintInfo2* info)
+{
+	// this will be called by constraint solver at the constraint setup stage
+	// set current motor parameters
+	internalUpdateSprings(info);
+	// do the rest of job for constraint setup
+	btGeneric6DofConstraint::getInfo2(info);
+}
+
+
+
+
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
index 5af866390ee..33e4749ffd0 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
@@ -478,4 +478,20 @@ public:
 };
 
 
+class btGeneric6DofSpringConstraint : public btGeneric6DofConstraint
+{
+protected:
+	bool		m_springEnabled[6];
+	btScalar	m_equilibriumPoint[6];
+	btScalar	m_springStiffness[6];
+	void internalUpdateSprings(btConstraintInfo2* info);
+public: 
+    btGeneric6DofSpringConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB ,bool useLinearReferenceFrameA);
+	void enableSpring(int index, bool onOff);
+	void setStiffness(int index, btScalar stiffness);
+	void setEquilibriumPoint(); // set the current constraint position/orientation as an equilibrium point for all DOF
+	void setEquilibriumPoint(int index);  // set the current constraint position/orientation as an equilibrium point for given DOF
+	virtual void getInfo2 (btConstraintInfo2* info);
+};
+
 #endif //GENERIC_6DOF_CONSTRAINT_H