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

add btVector3::safeNorm to avoid/workaround certain clang/g++ issue or returning -INF

Browse Source 
when taking sqrtf(0.0000000000000000000000000000000000000108333558)
This commit is contained in:
Erwin Coumans
2016-09-25 23:13:23 -07:00
parent c05784ea88
commit 6563c9c821
3 changed files with 16 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
src/BulletDynamics/Featherstone/btMultiBody.cpp
View File

@@ -769,8 +769,8 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
//adding damping terms (only) //adding damping terms (only)
btScalar linDampMult = 1., angDampMult = 1.; btScalar linDampMult = 1., angDampMult = 1.;
zeroAccSpatFrc[0].addVector(angDampMult * m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().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().norm())); 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 //p += vhat x Ihat vhat - done in a simpler way
@@ -856,8 +856,8 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
// //
//adding damping terms (only) //adding damping terms (only)
btScalar linDampMult = 1., angDampMult = 1.; 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()), 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().norm())); 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 // calculate Ihat_i^A
//init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs) //init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs)

2
src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.cpp
View File

@@ -133,7 +133,7 @@ void btMultiBodySliderConstraint::createConstraintRows(btMultiBodyConstraintArra
for (int i = 0; i < 3; ++i) for (int i = 0; i < 3; ++i)
{ {
constraintAxis[0] = frameAworld.getColumn(i).cross(jointAxis); constraintAxis[0] = frameAworld.getColumn(i).cross(jointAxis);
if (constraintAxis[0].norm() > EPSILON) if (constraintAxis[0].safeNorm() > EPSILON)
{ {
constraintAxis[0] = constraintAxis[0].normalized(); constraintAxis[0] = constraintAxis[0].normalized();
constraintAxis[1] = jointAxis.cross(constraintAxis[0]); constraintAxis[1] = jointAxis.cross(constraintAxis[0]);

10
src/LinearMath/btVector3.h
View File

@@ -271,6 +271,16 @@ public:
return length(); 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 /**@brief Return the distance squared between the ends of this and another vector
* This is symantically treating the vector like a point */ * This is symantically treating the vector like a point */
SIMD_FORCE_INLINE btScalar distance2(const btVector3& v) const; SIMD_FORCE_INLINE btScalar distance2(const btVector3& v) const;