Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge pull request #1589 from GaborPuhr/Fix-for-#1567

Browse Source 
Fix for #1567
This commit is contained in:
erwincoumans
2018-03-05 16:10:46 -08:00
committed by GitHub
parent 6d326a79c8 86ca7dc2d1
commit 8bde0d35cc
1 changed files with 18 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp
View File

@@ -797,6 +797,12 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
btScalar cfm = BT_ZERO; btScalar cfm = BT_ZERO;
btScalar mA = BT_ONE / m_rbA.getInvMass(); btScalar mA = BT_ONE / m_rbA.getInvMass();
btScalar mB = BT_ONE / m_rbB.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 m = mA > mB ? mB : mA;
btScalar angularfreq = sqrt(ks / m); btScalar angularfreq = sqrt(ks / m);
@@ -815,7 +821,18 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2(
btScalar fd = -kd * (vel) * (rotational ? -1 : 1) * dt; btScalar fd = -kd * (vel) * (rotational ? -1 : 1) * dt;
btScalar f = (fs+fd); 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 minf = f < fd ? f : fd;
btScalar maxf = f < fd ? fd : f; btScalar maxf = f < fd ? fd : f;