diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index 29b851611..8a8a5da77 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -769,8 +769,8 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar //adding damping terms (only) btScalar linDampMult = 1., angDampMult = 1.; - zeroAccSpatFrc[0].addVector(angDampMult * m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().norm()), - linDampMult * m_baseMass * spatVel[0].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[0].getLinear().norm())); + zeroAccSpatFrc[0].addVector(angDampMult * m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().safeNorm()), + linDampMult * m_baseMass * spatVel[0].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[0].getLinear().safeNorm())); // //p += vhat x Ihat vhat - done in a simpler way @@ -856,8 +856,8 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar // //adding damping terms (only) btScalar linDampMult = 1., angDampMult = 1.; - zeroAccSpatFrc[i+1].addVector(angDampMult * m_links[i].m_inertiaLocal * spatVel[i+1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i+1].getAngular().norm()), - linDampMult * m_links[i].m_mass * spatVel[i+1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i+1].getLinear().norm())); + zeroAccSpatFrc[i+1].addVector(angDampMult * m_links[i].m_inertiaLocal * spatVel[i+1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i+1].getAngular().safeNorm()), + linDampMult * m_links[i].m_mass * spatVel[i+1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i+1].getLinear().safeNorm())); // calculate Ihat_i^A //init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs) diff --git a/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp index e29ae0691..67b106f28 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp @@ -133,7 +133,7 @@ void btMultiBodySliderConstraint::createConstraintRows(btMultiBodyConstraintArra for (int i = 0; i < 3; ++i) { constraintAxis[0] = frameAworld.getColumn(i).cross(jointAxis); - if (constraintAxis[0].norm() > EPSILON) + if (constraintAxis[0].safeNorm() > EPSILON) { constraintAxis[0] = constraintAxis[0].normalized(); constraintAxis[1] = jointAxis.cross(constraintAxis[0]); diff --git a/src/LinearMath/btVector3.h b/src/LinearMath/btVector3.h index 839b19c14..487670009 100644 --- a/src/LinearMath/btVector3.h +++ b/src/LinearMath/btVector3.h @@ -267,10 +267,20 @@ public: /**@brief Return the norm (length) of the vector */ SIMD_FORCE_INLINE btScalar norm() const - { + { return length(); } + /**@brief Return the norm (length) of the vector */ + SIMD_FORCE_INLINE btScalar safeNorm() const + { + btScalar d = length2(); + //workaround for some clang/gcc issue of sqrtf(tiny number) = -INF + if (d>SIMD_EPSILON) + return btSqrt(d); + return btScalar(0); + } + /**@brief Return the distance squared between the ends of this and another vector * This is symantically treating the vector like a point */ SIMD_FORCE_INLINE btScalar distance2(const btVector3& v) const;