mat4 to quaternion

.gitignore

@@ -59,4 +59,5 @@ cglm_test_ios/*
 cglm_test_iosTests/*
 docs/build/*
 win/cglm_test_*
-* copy.*
+* copy.*
+*.o

include/cglm/call/mat4.h

@@ -53,6 +53,14 @@ CGLM_EXPORT
 void
 glmc_mat4_mulv(mat4 m, vec4 v, vec4 dest);
 
+CGLM_EXPORT
+void
+glmc_mat4_mulq(mat4 m, versor q, mat4 dest);
+
+CGLM_EXPORT
+void
+glmc_mat4_quat(mat4 m, versor dest);
+
 CGLM_EXPORT
 void
 glmc_mat4_transpose_to(mat4 m, mat4 dest);

include/cglm/mat4.h

@@ -334,6 +334,36 @@ glm_mat4_mulq(mat4 m, versor q, mat4 dest) {
   glm_mat4_mul(m, rot, dest);
 }
 
+/*!
+ * @brief convert mat4's rotation part to quaternion
+ *
+ * @param[in]  m    left matrix
+ * @param[out] dest destination quaternion
+ */
+CGLM_INLINE
+void
+glm_mat4_quat(mat4 m, versor dest) {
+  versor q;
+  float  m00, m10, m20,
+         m01, m11, m21,
+         m02, m12, m22;
+
+  m00  = m[0][0];  m10 = m[1][0];  m20 = m[2][0];
+  m01  = m[0][1];  m11 = m[1][1];  m21 = m[2][1];
+  m02  = m[0][2];  m12 = m[1][2];  m22 = m[2][2];
+
+  q[0] = sqrtf(glm_max(0.0f, 1.0f + m00 + m11 + m22)) * 0.5f; /* w */
+  q[1] = sqrtf(glm_max(0.0f, 1.0f + m00 - m11 - m22)) * 0.5f; /* x */
+  q[2] = sqrtf(glm_max(0.0f, 1.0f - m00 + m11 - m22)) * 0.5f; /* y */
+  q[3] = sqrtf(glm_max(0.0f, 1.0f - m00 - m11 + m22)) * 0.5f; /* z */
+
+  q[1] *= glm_signf(m12 - m21);
+  q[2] *= glm_signf(m20 - m02);
+  q[3] *= glm_signf(m01 - m10);
+
+  glm_vec4_copy(q, dest);
+}
+
 /*!
  * @brief multiply vector with mat4's mat3 part(rotation)
  *

makefile.am

@@ -108,7 +108,7 @@ test_tests_SOURCES=\
     test/src/test_cam.c \
     test/src/test_project.c \
     test/src/test_clamp.c \
-    test/src/test_euler.c
-
+    test/src/test_euler.c \
+    test/src/test_quat.c
 all-local:
 	sh ./post-build.sh

src/mat4.c

@@ -62,6 +62,18 @@ glmc_mat4_mulv(mat4 m, vec4 v, vec4 dest) {
   glm_mat4_mulv(m, v, dest);
 }
 
+CGLM_EXPORT
+void
+glmc_mat4_mulq(mat4 m, versor q, mat4 dest) {
+  glm_mat4_mulq(m, q, dest);
+}
+
+CGLM_EXPORT
+void
+glmc_mat4_quat(mat4 m, versor dest) {
+  glm_mat4_quat(m, dest);
+}
+
 CGLM_EXPORT
 void
 glmc_mat4_transpose_to(mat4 m, mat4 dest) {

test/src/test_common.c

@@ -27,6 +27,33 @@ test_rand_mat4(mat4 dest) {
   /* glm_scale(dest, (vec3){drand48(), drand48(), drand48()}); */
 }
 
+void
+test_rand_vec3(vec3 dest) {
+  srand((unsigned int)time(NULL));
+
+  dest[0] = drand48();
+  dest[1] = drand48();
+  dest[2] = drand48();
+}
+
+float
+test_rand_angle(void) {
+  srand((unsigned int)time(NULL));
+  return drand48();
+}
+
+void
+test_rand_quat(versor q) {
+  srand((unsigned int)time(NULL));
+  
+  q[0] = drand48();
+  q[1] = drand48();
+  q[2] = drand48();
+  q[3] = drand48();
+
+  glm_quat_normalize(q);
+}
+
 void
 test_assert_mat4_eq(mat4 m1, mat4 m2) {
   int i, j, k;
@@ -53,7 +80,16 @@ test_assert_mat4_eq2(mat4 m1, mat4 m2, float eps) {
 
 void
 test_assert_vec3_eq(vec3 v1, vec3 v2) {
-  assert_true(fabsf(v1[0] - v2[0]) <= 0.0000009);
+  assert_true(fabsf(v1[0] - v2[0]) <= 0.0000009); /* rounding errors */
   assert_true(fabsf(v1[1] - v2[1]) <= 0.0000009);
   assert_true(fabsf(v1[2] - v2[2]) <= 0.0000009);
 }
+
+void
+test_assert_quat_eq(versor v1, versor v2) {
+  assert_true(fabsf(v1[0] - v2[0]) <= 0.0009); /* rounding errors */
+  assert_true(fabsf(v1[1] - v2[1]) <= 0.0009);
+  assert_true(fabsf(v1[2] - v2[2]) <= 0.0009);
+  assert_true(fabsf(v1[3] - v2[3]) <= 0.0009);
+}
+

test/src/test_common.h

@@ -34,4 +34,16 @@ test_assert_mat4_eq2(mat4 m1, mat4 m2, float eps);
 void
 test_assert_vec3_eq(vec3 v1, vec3 v2);
 
+void
+test_assert_quat_eq(versor v1, versor v2);
+
+void
+test_rand_vec3(vec3 dest);
+
+float
+test_rand_angle(void);
+
+void
+test_rand_quat(versor q);
+
 #endif /* test_common_h */

test/src/test_main.c

@@ -23,7 +23,10 @@ main(int argc, const char * argv[]) {
     cmocka_unit_test(test_clamp),
 
     /* euler */
-    cmocka_unit_test(test_euler)
+    cmocka_unit_test(test_euler),
+
+    /* quaternion */
+    cmocka_unit_test(test_quat)
   };
 
   return cmocka_run_group_tests(tests, NULL, NULL);

test/src/test_quat.c

@@ -0,0 +1,22 @@
+/*
+ * Copyright (c), Recep Aslantas.
+ *
+ * MIT License (MIT), http://opensource.org/licenses/MIT
+ * Full license can be found in the LICENSE file
+ */
+
+#include "test_common.h"
+
+void
+test_quat(void **state) {
+  mat4   rot;
+  versor inQuat, outQuat;
+  int    i;
+
+  for (i = 0; i < 10000; i++) {
+    test_rand_quat(inQuat);
+    glmc_quat_mat4(inQuat, rot);
+    glm_mat4_quat(rot, outQuat);
+    test_assert_quat_eq(inQuat, outQuat);
+  }
+}

test/src/test_tests.h

@@ -25,4 +25,7 @@ test_clamp(void **state);
 void
 test_euler(void **state);
 
+void
+test_quat(void **state);
+
 #endif /* test_tests_h */