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)
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)

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

@@ -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]);

12
src/LinearMath/btVector3.h
View File

@@ -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;