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()); return btSqrt(length2());
} }
btQuaternion& safeNormalize()
{
btScalar l2 = length2();
if (l2>SIMD_EPSILON)
{
normalize();
}
return *this;
}
/**@brief Normalize the quaternion /**@brief Normalize the quaternion
* Such that x^2 + y^2 + z^2 +w^2 = 1 */ * Such that x^2 + y^2 + z^2 +w^2 = 1 */
btQuaternion& normalize() btQuaternion& normalize()

21
src/LinearMath/btTransformUtil.h
View File

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