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

fix for division by zero, in case angular velocity goes close to zero

Browse Source 
(but not zero)
This commit is contained in:
Erwin Coumans
2017-05-11 17:45:01 -07:00
parent f74adffb84
commit 424a29474f
2 changed files with 26 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
src/LinearMath/btQuaternion.h
View File

@@ -355,7 +355,15 @@ public:
{
return btSqrt(length2());
}
btQuaternion& safeNormalize()
{
btScalar l2 = length2();
if (l2>SIMD_EPSILON)
{
normalize();
}
return *this;
}
/**@brief Normalize the quaternion
* Such that x^2 + y^2 + z^2 +w^2 = 1 */
btQuaternion& normalize()

21
src/LinearMath/btTransformUtil.h
View File

@@ -47,13 +47,19 @@ public:
#ifdef QUATERNION_DERIVATIVE
btQuaternion predictedOrn = curTrans.getRotation();
predictedOrn += (angvel * predictedOrn) * (timeStep * btScalar(0.5));
predictedOrn.normalize();
predictedOrn.safeNormalize();
#else
//Exponential map
//google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia
btVector3 axis;
btScalar fAngle = angvel.length();
btScalar fAngle2 = angvel.length2();
btScalar fAngle = 0;
if (fAngle2>SIMD_EPSILON)
{
fAngle = btSqrt(fAngle2);
}
//limit the angular motion
if (fAngle*timeStep > ANGULAR_MOTION_THRESHOLD)
{
@@ -74,9 +80,16 @@ public:
btQuaternion orn0 = curTrans.getRotation();
btQuaternion predictedOrn = dorn * orn0;
predictedOrn.normalize();
predictedOrn.safeNormalize();
#endif
predictedTransform.setRotation(predictedOrn);
if (predictedOrn.length2()>SIMD_EPSILON)
{
predictedTransform.setRotation(predictedOrn);
}
else
{
predictedTransform.setBasis(curTrans.getBasis());
}
}
static void calculateVelocityQuaternion(const btVector3& pos0,const btVector3& pos1,const btQuaternion& orn0,const btQuaternion& orn1,btScalar timeStep,btVector3& linVel,btVector3& angVel)