Merge pull request #251 from T4Larson/slerpfix

replaced bullet's unstable quaternion slerp

Author: Anatoly Grishin

src/entity.cpp

@@ -770,7 +770,7 @@ void Entity::updateCurrentBoneFrame(SSBoneFrame *bf, const btTransform* etr)
         if(k == 0)
         {
             btag->transform.getOrigin() += bf->pos;
-            btag->qrotate = src_btag->qrotate.slerp(next_btag->qrotate, bf->animations.lerp);
+            btag->qrotate = Quat_Slerp(src_btag->qrotate, next_btag->qrotate, bf->animations.lerp);
         }
         else
         {
@@ -789,7 +789,7 @@ void Entity::updateCurrentBoneFrame(SSBoneFrame *bf, const btTransform* etr)
                     break;
                 }
             }
-            btag->qrotate = ov_src_btag->qrotate.slerp(ov_next_btag->qrotate, ov_lerp);
+            btag->qrotate = Quat_Slerp(ov_src_btag->qrotate, ov_next_btag->qrotate, ov_lerp);
         }
         btag->transform.setRotation(btag->qrotate);
     }

src/mesh.cpp

@@ -284,7 +284,7 @@ void SkeletalModel::interpolateFrames()
                     for(uint16_t k = 0; k < mesh_count; k++)
                     {
                         bf->bone_tags[k].offset = anim->frames[j - 1].bone_tags[k].offset.lerp(anim->frames[j].bone_tags[k].offset, lerp);
-                        bf->bone_tags[k].qrotate = anim->frames[j - 1].bone_tags[k].qrotate.slerp(anim->frames[j].bone_tags[k].qrotate, lerp);
+                        bf->bone_tags[k].qrotate = Quat_Slerp(anim->frames[j - 1].bone_tags[k].qrotate, anim->frames[j].bone_tags[k].qrotate, lerp);
                     }
                     bf++;
                 }

src/vmath.cpp

@@ -68,4 +68,34 @@ void Mat4_RotateZ(btTransform& mat, btScalar ang)
     m[1] = -mat.getBasis().getColumn(0) * sina + mat.getBasis().getColumn(1) * cosa;
 
     mat.getBasis() = m.transpose();
-}
+}
+
+btQuaternion Quat_Slerp(const btQuaternion& q1, const btQuaternion& q2, const btScalar& t)
+{
+    const btScalar magnitude = btSqrt(q1.length2() * q2.length2());
+    btAssert(magnitude > btScalar(0));
+
+    const btScalar product = q1.dot(q2) / magnitude;
+    const btScalar absproduct = btFabs(product);
+
+    if(absproduct < btScalar(1.0 - SIMD_EPSILON))
+    {
+        const btScalar theta = btAcos(absproduct);
+        const btScalar d = btSin(theta);
+        btAssert(d > btScalar(0))
+
+        const btScalar sign = (product < 0) ? btScalar(-1) : btScalar(1);
+        const btScalar s0 = btSin((btScalar(1.0) - t) * theta) / d;
+        const btScalar s1 = btSin(sign * t * theta) / d;
+
+        return btQuaternion(
+            (q1.x() * s0 + q2.x() * s1),
+            (q1.y() * s0 + q2.y() * s1),
+            (q1.z() * s0 + q2.z() * s1),
+            (q1.w() * s0 + q2.w() * s1));
+    }
+    else
+    {
+        return btQuaternion(q1);
+    }
+}

src/vmath.h

@@ -92,5 +92,6 @@ void Mat4_Scale(btTransform &mat, btScalar x, btScalar y, btScalar z);
 void Mat4_RotateX(btTransform &mat, btScalar ang);
 void Mat4_RotateY(btTransform &mat, btScalar ang);
 void Mat4_RotateZ(btTransform &mat, btScalar ang);
+btQuaternion Quat_Slerp(const btQuaternion& q1, const btQuaternion& q2, const btScalar& t);
 
 #endif  // VMATH_H