#18872 bugfix for torque on dynamic objects

#18893, fix to getParam for generic 6dof constraints

2 files changed, 32 insertions, 8 deletions

diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
index f60eabe2d2b..4d69fe75f83 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.cpp
@@ -663,10 +663,15 @@ btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const
 }
 
 
+btScalar	btGeneric6DofConstraint::getRelativePivotPosition(int axisIndex) const
+{
+	return m_calculatedLinearDiff[axisIndex];
+}
+
 
-btScalar btGeneric6DofConstraint::getAngle(int axis_index) const
+btScalar btGeneric6DofConstraint::getAngle(int axisIndex) const
 {
-	return m_calculatedAxisAngleDiff[axis_index];
+	return m_calculatedAxisAngleDiff[axisIndex];
 }
 
 
@@ -839,7 +844,6 @@ 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)
 {
@@ -848,6 +852,7 @@ btGeneric6DofSpringConstraint::btGeneric6DofSpringConstraint(btRigidBody& rbA, b
 		m_springEnabled[i] = false;
 		m_equilibriumPoint[i] = btScalar(0.f);
 		m_springStiffness[i] = btScalar(0.f);
+		m_springDamping[i] = btScalar(1.f);
 	}
 }
 
@@ -875,6 +880,13 @@ void btGeneric6DofSpringConstraint::setStiffness(int index, btScalar stiffness)
 }
 
 
+void btGeneric6DofSpringConstraint::setDamping(int index, btScalar damping)
+{
+	btAssert((index >= 0) && (index < 6));
+	m_springDamping[index] = damping;
+}
+
+
 void btGeneric6DofSpringConstraint::setEquilibriumPoint()
 {
 	calculateTransforms();
@@ -908,6 +920,7 @@ void btGeneric6DofSpringConstraint::setEquilibriumPoint(int index)
 
 void btGeneric6DofSpringConstraint::internalUpdateSprings(btConstraintInfo2* info)
 {
+	calculateTransforms();
 	// it is assumed that calculateTransforms() have been called before this call
 	int i;
 	btVector3 relVel = m_rbB.getLinearVelocity() - m_rbA.getLinearVelocity();
@@ -921,8 +934,9 @@ void btGeneric6DofSpringConstraint::internalUpdateSprings(btConstraintInfo2* inf
 			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;
+			btScalar velFactor = info->fps * m_springDamping[i];
+			m_linearLimits.m_targetVelocity[i] =  velFactor * force;
+			m_linearLimits.m_maxMotorForce[i] =  btFabs(force) / info->fps;
 		}
 	}
 	for(i = 0; i < 3; i++)
@@ -935,7 +949,8 @@ void btGeneric6DofSpringConstraint::internalUpdateSprings(btConstraintInfo2* inf
 			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;
+			btScalar velFactor = info->fps * m_springDamping[i+3];
+			m_angularLimits[i].m_targetVelocity = velFactor * force;
 			m_angularLimits[i].m_maxMotorForce = btFabs(force) / info->fps;
 		}
 	}
diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
index d01c9cf63ab..8082eb1f132 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h
@@ -382,14 +382,21 @@ public:
 
     //! Get the relative Euler angle
     /*!
-	\pre btGeneric6DofConstraint.buildJacobian must be called previously.
+	\pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
 	*/
     btScalar getAngle(int axis_index) const;
 
+	//! Get the relative position of the constraint pivot
+    /*!
+	\pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
+	*/
+	btScalar getRelativePivotPosition(int axis_index) const;
+
+
 	//! Test angular limit.
 	/*!
 	Calculates angular correction and returns true if limit needs to be corrected.
-	\pre btGeneric6DofConstraint.buildJacobian must be called previously.
+	\pre btGeneric6DofConstraint::calculateTransforms() must be called previously.
 	*/
     bool testAngularLimitMotor(int axis_index);
 
@@ -496,11 +503,13 @@ protected:
 	bool		m_springEnabled[6];
 	btScalar	m_equilibriumPoint[6];
 	btScalar	m_springStiffness[6];
+	btScalar	m_springDamping[6]; // between 0 and 1 (1 == no damping)
 	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 setDamping(int index, btScalar damping);
 	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);