Bullet: Fix velocity calculation of new spring constraint

This is a backport of bullet commit: 86ca7dc

1 files changed, 18 insertions, 1 deletions

diff --git a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp
index 49ff78c2621..31371944864 100644
--- a/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp
+++ b/extern/bullet2/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp
@@ -782,6 +782,12 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
 		btScalar cfm = BT_ZERO;
 		btScalar mA = BT_ONE / m_rbA.getInvMass();
 		btScalar mB = BT_ONE / m_rbB.getInvMass();
+		if (rotational) {
+			btScalar rrA = (m_calculatedTransformA.getOrigin() - transA.getOrigin()).length2();
+			btScalar rrB = (m_calculatedTransformB.getOrigin() - transB.getOrigin()).length2();
+			if (m_rbA.getInvMass()) mA = mA * rrA + 1 / (m_rbA.getInvInertiaTensorWorld() * ax1).length();
+			if (m_rbB.getInvMass()) mB = mB * rrB + 1 / (m_rbB.getInvInertiaTensorWorld() * ax1).length();
+		}
 		btScalar m = mA > mB ? mB : mA;
 		btScalar angularfreq = sqrt(ks / m);
 
@@ -800,7 +806,18 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
 		btScalar fd = -kd * (vel) * (rotational ? -1 : 1) * dt;
 		btScalar f = (fs+fd);
 
-		info->m_constraintError[srow] = (vel + f * (rotational ? -1 : 1)) ;
+	// after the spring force affecting the body(es) the new velocity will be
+	// vel + f / m * (rotational ? -1 : 1)
+	// so in theory this should be set here for m_constraintError
+	// (with m_constraintError we set a desired velocity for the affected body(es))
+	// however in practice any value is fine as long as it is greater then the "proper" velocity,
+	// because the m_lowerLimit and the m_upperLimit will determinate the strength of the final pulling force
+	// so it is much simpler (and more robust) just to simply use inf (with the proper sign)
+	// you may also wonder what if the current velocity (vel) so high that the pulling force will not change its direction (in this iteration)
+	// will we not request a velocity with the wrong direction ?
+	// and the answare is not, because in practice during the solving the current velocity is subtracted from the m_constraintError
+	// so the sign of the force that is really matters
+	info->m_constraintError[srow] = (rotational ? -1 : 1) * (f < 0 ? -SIMD_INFINITY : SIMD_INFINITY);
 
 		btScalar minf = f < fd ? f : fd;
 		btScalar maxf = f < fd ? fd : f;