From 257c57d41ff56905b25b0fcc5449f1319843cb85 Mon Sep 17 00:00:00 2001 From: Recep Aslantas Date: Sat, 7 Apr 2018 19:46:46 +0300 Subject: [PATCH] mat4 to quaternion --- .gitignore | 3 ++- include/cglm/call/mat4.h | 8 ++++++++ include/cglm/mat4.h | 30 ++++++++++++++++++++++++++++++ makefile.am | 4 ++-- src/mat4.c | 12 ++++++++++++ test/src/test_common.c | 38 +++++++++++++++++++++++++++++++++++++- test/src/test_common.h | 12 ++++++++++++ test/src/test_main.c | 5 ++++- test/src/test_quat.c | 22 ++++++++++++++++++++++ test/src/test_tests.h | 3 +++ 10 files changed, 132 insertions(+), 5 deletions(-) create mode 100644 test/src/test_quat.c diff --git a/.gitignore b/.gitignore index 180c5d9..37f68c8 100644 --- a/.gitignore +++ b/.gitignore @@ -59,4 +59,5 @@ cglm_test_ios/* cglm_test_iosTests/* docs/build/* win/cglm_test_* -* copy.* +* copy.* +*.o diff --git a/include/cglm/call/mat4.h b/include/cglm/call/mat4.h index 5c8ff38..35b9f66 100644 --- a/include/cglm/call/mat4.h +++ b/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); diff --git a/include/cglm/mat4.h b/include/cglm/mat4.h index eddfe6b..ec7ac39 100644 --- a/include/cglm/mat4.h +++ b/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) * diff --git a/makefile.am b/makefile.am index 217fff3..436973c 100644 --- a/makefile.am +++ b/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 diff --git a/src/mat4.c b/src/mat4.c index 0b1ccee..8b2a2d0 100644 --- a/src/mat4.c +++ b/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) { diff --git a/test/src/test_common.c b/test/src/test_common.c index 23f2b50..c38c474 100644 --- a/test/src/test_common.c +++ b/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); +} + diff --git a/test/src/test_common.h b/test/src/test_common.h index aeea4d6..f692483 100644 --- a/test/src/test_common.h +++ b/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 */ diff --git a/test/src/test_main.c b/test/src/test_main.c index 5c1a647..384250f 100644 --- a/test/src/test_main.c +++ b/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); diff --git a/test/src/test_quat.c b/test/src/test_quat.c new file mode 100644 index 0000000..1a328de --- /dev/null +++ b/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); + } +} diff --git a/test/src/test_tests.h b/test/src/test_tests.h index 7234782..398caa3 100644 --- a/test/src/test_tests.h +++ b/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 */