quat: add lerp and improve slerp

include/cglm/quat.h

@@ -432,60 +432,64 @@ glm_quat_mat3(versor q, mat3 dest) {
   dest[0][2] = xz - wy;
 }
 
+/*!
+ * @brief interpolates between two quaternions
+ *        using linear interpolation (LERP)
+ *
+ * @param[in]   from  from
+ * @param[in]   to    to
+ * @param[in]   t     interpolant (amount) clamped between 0 and 1
+ * @param[out]  dest  result quaternion
+ */
+CGLM_INLINE
+void
+glm_quat_lerp(versor from, versor to, float t, versor dest) {
+  glm_vec4_lerp(from, to, t, dest);
+}
+
 /*!
  * @brief interpolates between two quaternions
  *        using spherical linear interpolation (SLERP)
  *
- * @param[in]   q     from
+ * @param[in]   from  from
- * @param[in]   r     to
+ * @param[in]   to    to
  * @param[in]   t     amout
  * @param[out]  dest  result quaternion
  */
 CGLM_INLINE
 void
-glm_quat_slerp(versor q, versor r, float t, versor dest) {
+glm_quat_slerp(versor from, versor to, float t, versor dest) {
+  vec4  q1, q2;
+  float cosTheta, sinTheta, angle;
 
-  /* https://en.wikipedia.org/wiki/Slerp */
+  cosTheta = glm_quat_dot(from, to);
-#if defined( __SSE__ ) || defined( __SSE2__ )
+  glm_quat_copy(from, q1);
-  glm_quat_slerp_sse2(q, r, t, dest);
-#else
-  float cosTheta, sinTheta, angle, a, b, c;
 
-  cosTheta = glm_quat_dot(q, r);
+  if (fabsf(cosTheta) >= 1.0f) {
-  if (cosTheta < 0.0f) {
+    glm_quat_copy(q1, dest);
-    glm_vec4_flipsign(q);
+    return;
-    cosTheta = -cosTheta;
   }
 
-  if (fabs(cosTheta) >= 1.0f) {
+  if (cosTheta < 0.0f) {
-    glm_quat_copy(q, dest);
+    glm_vec4_flipsign(q1);
-    return;
+    cosTheta = -cosTheta;
   }
 
   sinTheta = sqrtf(1.0f - cosTheta * cosTheta);
 
-  c = 1.0f - t;
+  /* LERP to avoid zero division */
-
+  if (fabsf(sinTheta) < 0.001f) {
-  /* LERP */
+    glm_quat_lerp(from, to, t, dest);
-  /* TODO: FLT_EPSILON vs 0.001? */
-  if (sinTheta < 0.001f) {
-    dest[0] = c * q[0] + t * r[0];
-    dest[1] = c * q[1] + t * r[1];
-    dest[2] = c * q[2] + t * r[2];
-    dest[3] = c * q[3] + t * r[3];
     return;
   }
 
   /* SLERP */
   angle = acosf(cosTheta);
-  a = sinf(c * angle);
+  glm_vec4_scale(q1, sinf((1.0f - t) * angle), q1);
-  b = sinf(t * angle);
+  glm_vec4_scale(to, sinf(t * angle), q2);
 
-  dest[0] = (q[0] * a + r[0] * b) / sinTheta;
+  glm_vec4_add(q1, q2, q1);
-  dest[1] = (q[1] * a + r[1] * b) / sinTheta;
+  glm_vec4_scale(q1, 1.0f / sinTheta, dest);
-  dest[2] = (q[2] * a + r[2] * b) / sinTheta;
-  dest[3] = (q[3] * a + r[3] * b) / sinTheta;
-#endif
 }
 
 #endif /* cglm_quat_h */

include/cglm/simd/sse2/quat.h

@@ -41,58 +41,6 @@ glm_quat_mul_sse2(versor p, versor q, versor dest) {
   _mm_store_ps(dest, r);
 }
 
-CGLM_INLINE
-void
-glm_quat_slerp_sse2(versor q,
-                    versor r,
-                    float  t,
-                    versor dest) {
-  /* https://en.wikipedia.org/wiki/Slerp */
-  float cosTheta, sinTheta, angle, a, b, c;
-
-  __m128 xmm_q;
-
-  xmm_q = _mm_load_ps(q);
-
-  cosTheta = glm_vec4_dot(q, r);
-  if (cosTheta < 0.0f) {
-    _mm_store_ps(q,
-                 _mm_xor_ps(xmm_q,
-                            _mm_set1_ps(-0.f))) ;
-
-    cosTheta = -cosTheta;
-  }
-
-  if (cosTheta >= 1.0f) {
-    _mm_store_ps(dest, xmm_q);
-    return;
-  }
-
-  sinTheta = sqrtf(1.0f - cosTheta * cosTheta);
-
-  c = 1.0f - t;
-
-  /* LERP */
-  if (sinTheta < 0.001f) {
-    _mm_store_ps(dest, _mm_add_ps(_mm_mul_ps(_mm_set1_ps(c),
-                                             xmm_q),
-                                  _mm_mul_ps(_mm_set1_ps(t),
-                                             _mm_load_ps(r))));
-    return;
-  }
-
-  /* SLERP */
-  angle = acosf(cosTheta);
-  a = sinf(c * angle);
-  b = sinf(t * angle);
-
-  _mm_store_ps(dest,
-               _mm_div_ps(_mm_add_ps(_mm_mul_ps(_mm_set1_ps(a),
-                                                xmm_q),
-                                     _mm_mul_ps(_mm_set1_ps(b),
-                                                _mm_load_ps(r))),
-                          _mm_set1_ps(sinTheta)));
-}
 
 #endif
 #endif /* cglm_quat_simd_h */