Damping for spring motors added

see btGeneric6DofSpringConstraint::setDamping()

Demos/ConstraintDemo/ConstraintDemo.cpp

@@ -383,8 +383,10 @@ void	ConstraintDemo::initPhysics()
 		
 		pGen6DOFSpring->enableSpring(0, true);
 		pGen6DOFSpring->setStiffness(0, 39.478f);
+		pGen6DOFSpring->setDamping(0, 0.5f);
 		pGen6DOFSpring->enableSpring(5, true);
 		pGen6DOFSpring->setStiffness(5, 39.478f);
+		pGen6DOFSpring->setDamping(0, 0.3f);
 		pGen6DOFSpring->setEquilibriumPoint();
 	}
 #endif

src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.cpp

@@ -26,6 +26,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);
 	}
 }
 
@@ -53,6 +54,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();
@@ -99,8 +107,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;
+			btScalar velFactor = info->fps * m_springDamping[i] / btScalar(info->m_numIterations);
-			m_linearLimits.m_maxMotorForce[i] = btFabs(force) / info->fps;
+			m_linearLimits.m_targetVelocity[i] =  velFactor * force;
+			m_linearLimits.m_maxMotorForce[i] =  btFabs(force) / info->fps;
 		}
 	}
 	for(i = 0; i < 3; i++)
@@ -113,7 +122,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] / btScalar(info->m_numIterations);
+			m_angularLimits[i].m_targetVelocity = velFactor * force;
 			m_angularLimits[i].m_maxMotorForce = btFabs(force) / info->fps;
 		}
 	}

src/BulletDynamics/ConstraintSolver/btGeneric6DofSpringConstraint.h

@@ -38,11 +38,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);

src/BulletDynamics/ConstraintSolver/btHinge2Constraint.cpp

@@ -60,6 +60,7 @@ btHinge2Constraint::btHinge2Constraint(btRigidBody& rbA, btRigidBody& rbB, btVec
 	// enable suspension
 	enableSpring(2, true);
 	setStiffness(2, SIMD_PI * SIMD_PI * 4.f); // period 1 sec for 1 kilogramm weel :-)
+	setDamping(2, 0.01f);
 	setEquilibriumPoint();
 }
 

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

@@ -730,6 +730,7 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
 					info2.cfm = &currentConstraintRow->m_cfm;
 					info2.m_lowerLimit = &currentConstraintRow->m_lowerLimit;
 					info2.m_upperLimit = &currentConstraintRow->m_upperLimit;
+					info2.m_numIterations = infoGlobal.m_numIterations;
 					constraints[i]->getInfo2(&info2);
 
 					///finalize the constraint setup

src/BulletDynamics/ConstraintSolver/btTypedConstraint.h

@@ -97,6 +97,8 @@ public:
 		// note that the returned indexes are relative to the first index of
 		// the constraint.
 		int *findex;
+		// number of solver iterations
+		int m_numIterations;
 	};
 
 	///internal method used by the constraint solver, don't use them directly