quaternions

include/cglm-mat.h

@@ -84,6 +84,15 @@ glm_mat4_mulN(mat4 * __restrict matrices[], int len, mat4 dest) {
                  dest);
 }
 
+CGLM_INLINE
+void
+glm_mat4_mulv(mat4 m, vec4 v, vec4 dest) {
+  dest[0] = m[0][0] * v[0] + m[1][0] * v[1] + m[2][0] * v[2] + m[3][0] * v[3];
+  dest[0] = m[0][1] * v[0] + m[1][1] * v[1] + m[2][1] * v[2] + m[3][1] * v[3];
+  dest[0] = m[0][2] * v[0] + m[1][2] * v[1] + m[2][2] * v[2] + m[3][2] * v[3];
+  dest[0] = m[0][3] * v[0] + m[1][3] * v[1] + m[2][3] * v[2] + m[3][3] * v[3];
+}
+
 CGLM_INLINE
 void
 glm_mat4_transpose_to(mat4 m, mat4 dest) {

include/cglm-quat-simd.h

@@ -0,0 +1,66 @@
+/*
+ * Copyright (c), Recep Aslantas.
+ *
+ * MIT License (MIT), http://opensource.org/licenses/MIT
+ * Full license can be found in the LICENSE file
+ */
+
+#ifndef cglm_quat_simd_h
+#define cglm_quat_simd_h
+
+#include "cglm-intrin.h"
+
+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 = sqrt(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 = acos(cosTheta);
+  a = sin(c * angle);
+  b = sin(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 /* cglm_quat_simd_h */

include/cglm-quat.h

@@ -9,13 +9,93 @@
 #define cglm_quat_h
 
 #include "cglm.h"
+#include "cglm-vec.h"
 #include "cglm-intrin.h"
+#include "cglm-quat-simd.h"
 #include <math.h>
 
+CGLM_INLINE
+void
+glm_quat_init(versor q,
+              float angle,
+              float x,
+              float y,
+              float z) {
+  q[0] = cosf(angle / 2.0f);
+  q[1] = sinf(angle / 2.0f) * x;
+  q[2] = sinf(angle / 2.0f) * y;
+  q[3] = sinf(angle / 2.0f) * z;
+}
+
+
+CGLM_INLINE
+float
+glm_quat_norm(versor q) {
+  return glm_vec4_norm(q);
+}
+
+CGLM_INLINE
+void
+glm_quat_normalize(versor q) {
+  float sum, norm;
+
+  sum = q[0] * q[0] + q[1] * q[1]
+        + q[2] * q[2] + q[3] * q[3];
+
+  if (fabs(1.0f - sum) < 0.0001f)
+    return;
+
+  norm = sqrt(sum);
+
+  q[0] = q[0] / norm;
+  q[1] = q[1] / norm;
+  q[2] = q[2] / norm;
+  q[3] = q[3] / norm;
+}
+
 CGLM_INLINE
 float
 glm_quat_dot(versor q, versor r) {
-  return q[0] * r[0] + q[1] * r[1] + q[2] * r[2] + q[3] * r[3];
+  return glm_vec4_dot(q, r);
+}
+
+CGLM_INLINE
+void
+glm_quat_mulv(versor q1, versor q2, versor dest) {
+  dest[0] = q2[0] * q1[0] - q2[1] * q1[1] - q2[2] * q1[2] - q2[3] * q1[3];
+  dest[1] = q2[0] * q1[1] + q2[1] * q1[0] - q2[2] * q1[3] + q2[3] * q1[2];
+  dest[2] = q2[0] * q1[2] + q2[1] * q1[3] + q2[2] * q1[0] - q2[3] * q1[1];
+  dest[3] = q2[0] * q1[3] - q2[1] * q1[2] + q2[2] * q1[1] + q2[3] * q1[0];
+
+  glm_quat_normalize(dest);
+}
+
+CGLM_INLINE
+void
+glm_quat_mat4(versor q, mat4 dest) {
+  float w, x, y, z;
+
+  w = q[0];
+  x = q[1];
+  y = q[2];
+  z = q[3];
+
+  dest[0][0] = 1.0f - 2.0f * (y * y + z * z);
+  dest[0][1] = 2.0f * (x * y + w * z);
+  dest[0][2] = 2.0f * (x * z - w * y);
+  dest[0][3] = 0.0f;
+  dest[1][0] = 2.0f * (x * y - w * z);
+  dest[1][1] = 1.0f - 2.0f * (x * x + z * z);
+  dest[1][2] = 2.0f * (y * z + w * x);
+  dest[1][3] = 0.0f;
+  dest[2][0] = 2.0f * (x * z + w * y);
+  dest[2][1] = 2.0f * (y * z - w * x);
+  dest[2][2] = 1.0f - 2.0f * (x * x + y * y);
+  dest[2][3] = 0.0f;
+  dest[3][0] = 0.0f;
+  dest[3][1] = 0.0f;
+  dest[3][2] = 0.0f;
+  dest[3][3] = 1.0f;
 }
 
 CGLM_INLINE
@@ -24,52 +104,12 @@ glm_quat_slerp(versor q,
                versor r,
                float  t,
                versor dest) {
+  /* https://en.wikipedia.org/wiki/Slerp */
+#if defined( __SSE__ ) || defined( __SSE2__ )
+  glm_quat_slerp_sse2(q, r, t, dest);
+#else
   float cosTheta, sinTheta, angle, a, b, c;
 
-#if defined( __SSE__ ) || defined( __SSE2__ )
-  __m128 xmm_q;
-
-  xmm_q = _mm_load_ps(q);
-
-  cosTheta = glm_quat_dot(q, r);
-  if (cosTheta < 0.0f) {
-    _mm_store_ps(q,
-                 _mm_xor_ps(xmm_q,
-                            _mm_set1_ps(-0.f))) ;
-
-    cosTheta = glm_quat_dot(q, r);
-  }
-
-  if (fabs(cosTheta) >= 1.0f) {
-    _mm_store_ps(dest, xmm_q);
-    return;
-  }
-
-  sinTheta = sqrt(1.0f - cosTheta * cosTheta);
-
-  c = 1.0f - t;
-
-  /* LERP */
-  if (fabs(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 = acos(cosTheta);
-  a = sin(c * angle);
-  b = sin(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)));
-#else
   cosTheta = glm_quat_dot(q, r);
   if (cosTheta < 0.0f) {
     q[0] *= -1.0f;
@@ -77,7 +117,7 @@ glm_quat_slerp(versor q,
     q[2] *= -1.0f;
     q[3] *= -1.0f;
 
-    cosTheta = glm_quat_dot(q, r);
+    cosTheta = -cosTheta;
   }
 
   if (fabs(cosTheta) >= 1.0f) {
@@ -85,7 +125,6 @@ glm_quat_slerp(versor q,
     dest[1] = q[1];
     dest[2] = q[2];
     dest[3] = q[3];
-
     return;
   }
 
@@ -94,12 +133,12 @@ glm_quat_slerp(versor q,
   c = 1.0f - t;
 
   /* LERP */
-  if (fabs(sinTheta) < 0.001f) {
+  /* 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;
   }
 

include/cglm-vec.h

@@ -41,7 +41,7 @@ glm_vec_dot(vec3 a, vec3 b) {
 
 CGLM_INLINE
 float
-glm_vec_dot4(vec4 a, vec4 b) {
+glm_vec4_dot(vec4 a, vec4 b) {
   return a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + a[3] * b[3];
 }
 
@@ -60,6 +60,13 @@ glm_vec_norm(vec3 vec) {
   return sqrtf(vec[0] * vec[0] + vec[1] * vec[1] + vec[2] * vec[2]);
 }
 
+CGLM_INLINE
+float
+glm_vec4_norm(vec3 vec) {
+  return sqrtf(vec[0] * vec[0] + vec[1] * vec[1]
+               + vec[2] * vec[2] + vec[3] * vec[3]);
+}
+
 CGLM_INLINE
 void
 glm_vec_normalize_to(vec3 vec, vec3 dest) {
@@ -94,6 +101,24 @@ glm_vec_normalize(vec3 v) {
   v[2] = v[2] / norm;
 }
 
+CGLM_INLINE
+void
+glm_vec4_normalize(vec4 v) {
+  float norm;
+
+  norm = glm_vec_norm(v);
+
+  if (norm == 0.0f) {
+    v[0] = v[1] = v[2] = v[3] = 0.0f;
+    return;
+  }
+
+  v[0] = v[0] / norm;
+  v[1] = v[1] / norm;
+  v[2] = v[2] / norm;
+  v[3] = v[3] / norm;
+}
+
 CGLM_INLINE
 void
 glm_vec_add(vec3 v1, vec3 v2, vec3 dest) {