From 4db3b4da463538e1be2a36331653524d7f988188 Mon Sep 17 00:00:00 2001 From: Sergej Reich Date: Sat, 7 Apr 2018 17:37:23 +0200 Subject: Bullet: Fix velocity calculation of new spring constraint This is a backport of bullet commit: 86ca7dc --- .../btGeneric6DofSpring2Constraint.cpp | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) (limited to 'extern') 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; -- cgit v1.2.3