From f0daaca58b57bb290ac52de927d626adef967a83 Mon Sep 17 00:00:00 2001 From: Recep Aslantas Date: Mon, 9 Apr 2018 00:46:00 +0300 Subject: [PATCH] improve matrix to quaternion --- include/cglm/mat3.h | 48 +++++++++++++++++++++++++++++ include/cglm/mat4.h | 58 +++++++++++++++++++---------------- include/cglm/simd/sse2/mat4.h | 51 ------------------------------ test/src/test_common.c | 6 +--- test/src/test_quat.c | 10 +++--- 5 files changed, 86 insertions(+), 87 deletions(-) diff --git a/include/cglm/mat3.h b/include/cglm/mat3.h index 61c4f3d..e7f8bcc 100644 --- a/include/cglm/mat3.h +++ b/include/cglm/mat3.h @@ -186,6 +186,54 @@ glm_mat3_mulv(mat3 m, vec3 v, vec3 dest) { dest[2] = m[0][2] * v[0] + m[1][2] * v[1] + m[2][2] * v[2]; } + +/*! + * @brief convert mat4's rotation part to quaternion + * + * @param[in] m left matrix + * @param[out] dest destination quaternion + */ +CGLM_INLINE +void +glm_mat3_quat(mat3 m, versor dest) { + float trace, r, rinv; + + trace = m[0][0] + m[1][1] + m[2][2]; + if (trace >= 0.0f) { + r = 2.0f * sqrtf(1 + trace); + rinv = 1.0f / r; + + dest[1] = rinv * (m[1][2] - m[2][1]); + dest[2] = rinv * (m[2][0] - m[0][2]); + dest[3] = rinv * (m[0][1] - m[1][0]); + dest[0] = r * 0.25f; + } else if (m[0][0] >= m[1][1] && m[0][0] >= m[2][2]) { + r = 2.0f * sqrtf(1 - m[1][1] - m[2][2] + m[0][0]); + rinv = 1.0f / r; + + dest[1] = r * 0.25f; + dest[2] = rinv * (m[0][1] + m[1][0]); + dest[3] = rinv * (m[0][2] + m[2][0]); + dest[0] = rinv * (m[1][2] - m[2][1]); + } else if (m[1][1] >= m[2][2]) { + r = 2.0f * sqrtf(1 - m[0][0] - m[2][2] + m[1][1]); + rinv = 1.0f / r; + + dest[1] = rinv * (m[0][1] + m[1][0]); + dest[2] = r * 0.25f; + dest[3] = rinv * (m[1][2] + m[2][1]); + dest[0] = rinv * (m[2][0] - m[0][2]); + } else { + r = 2.0f * sqrtf(1 - m[0][0] - m[1][1] + m[2][2]); + rinv = 1.0f / r; + + dest[1] = rinv * (m[0][2] + m[2][0]); + dest[2] = rinv * (m[1][2] + m[2][1]); + dest[3] = r * 0.25f; + dest[0] = rinv * (m[0][1] - m[1][0]); + } +} + /*! * @brief scale (multiply with scalar) matrix * diff --git a/include/cglm/mat4.h b/include/cglm/mat4.h index 782a24b..05aac60 100644 --- a/include/cglm/mat4.h +++ b/include/cglm/mat4.h @@ -343,38 +343,42 @@ glm_mat4_mulq(mat4 m, versor q, mat4 dest) { CGLM_INLINE void glm_mat4_quat(mat4 m, versor dest) { -#if defined( __SSE__ ) || defined( __SSE2__ ) - glm_mat4_quat_sse2(m, dest); -#else - vec4 vsgn, vzero = GLM_VEC4_ZERO_INIT; - versor q; - float m00, m10, m20, - m01, m11, m21, - m02, m12, m22; + float trace, r, rinv; - m00 = m[0][0]; m01 = m[0][1]; m02 = m[0][2]; - m10 = m[1][0]; m11 = m[1][1]; m12 = m[1][2]; - m20 = m[2][0]; m21 = m[2][1]; m22 = m[2][2]; + trace = m[0][0] + m[1][1] + m[2][2]; + if (trace >= 0.0f) { + r = 2.0f * sqrtf(1 + trace); + rinv = 1.0f / r; - q[0] = 1.0f + m00 + m11 + m22; /* w */ - q[1] = 1.0f + m00 - m11 - m22; /* x */ - q[2] = 1.0f - m00 + m11 - m22; /* y */ - q[3] = 1.0f - m00 - m11 + m22; /* z */ + dest[1] = rinv * (m[1][2] - m[2][1]); + dest[2] = rinv * (m[2][0] - m[0][2]); + dest[3] = rinv * (m[0][1] - m[1][0]); + dest[0] = r * 0.25f; + } else if (m[0][0] >= m[1][1] && m[0][0] >= m[2][2]) { + r = 2.0f * sqrtf(1 - m[1][1] - m[2][2] + m[0][0]); + rinv = 1.0f / r; - glm_vec4_maxv(q, vzero, q); - glm_vec4_sqrt(q, q); - glm_vec4_scale(q, 0.5f, q); + dest[1] = r * 0.25f; + dest[2] = rinv * (m[0][1] + m[1][0]); + dest[3] = rinv * (m[0][2] + m[2][0]); + dest[0] = rinv * (m[1][2] - m[2][1]); + } else if (m[1][1] >= m[2][2]) { + r = 2.0f * sqrtf(1 - m[0][0] - m[2][2] + m[1][1]); + rinv = 1.0f / r; - vsgn[0] = 1.0f; - vsgn[1] = m12 - m21; - vsgn[2] = m20 - m02; - vsgn[3] = m01 - m10; + dest[1] = rinv * (m[0][1] + m[1][0]); + dest[2] = r * 0.25f; + dest[3] = rinv * (m[1][2] + m[2][1]); + dest[0] = rinv * (m[2][0] - m[0][2]); + } else { + r = 2.0f * sqrtf(1 - m[0][0] - m[1][1] + m[2][2]); + rinv = 1.0f / r; - glm_vec4_sign(vsgn, vsgn); - glm_vec4_mulv(q, vsgn, q); - - glm_vec4_copy(q, dest); -#endif + dest[1] = rinv * (m[0][2] + m[2][0]); + dest[2] = rinv * (m[1][2] + m[2][1]); + dest[3] = r * 0.25f; + dest[0] = rinv * (m[0][1] - m[1][0]); + } } /*! diff --git a/include/cglm/simd/sse2/mat4.h b/include/cglm/simd/sse2/mat4.h index 0476176..77874a8 100644 --- a/include/cglm/simd/sse2/mat4.h +++ b/include/cglm/simd/sse2/mat4.h @@ -102,57 +102,6 @@ glm_mat4_mulv_sse2(mat4 m, vec4 v, vec4 dest) { _mm_store_ps(dest, _mm_add_ps(x1, x2)); } -CGLM_INLINE -void -glm_mat4_quat_sse2(mat4 m, versor dest) { - __m128 c0, c1, c2, x0, x1, x2, x3, m00, m11, m22, r; - __m128 zero, one, half, ngone; - - c0 = _mm_load_ps(m[0]); - c1 = _mm_load_ps(m[1]); - c2 = _mm_load_ps(m[2]); - - m00 = _mm_xor_ps(_mm_shuffle1_ps1(c0, 0), _mm_set_ps(-0.f, -0.f, 0.f, 0.f)); - m11 = _mm_xor_ps(_mm_shuffle1_ps1(c1, 1), _mm_set_ps(-0.f, 0.f, -0.f, 0.f)); - m22 = _mm_xor_ps(_mm_shuffle1_ps1(c2, 2), _mm_set_ps( 0.f, -0.f, -0.f, 0.f)); - - x0 = _mm_set_ps(-1.0f, 0.0f, 0.5, 1.0f); - one = _mm_shuffle1_ps1(x0, 0); - half = _mm_shuffle1_ps1(x0, 1); - zero = _mm_shuffle1_ps1(x0, 2); - ngone = _mm_shuffle1_ps1(x0, 3); - - /* - q[0] = sqrtf(glm_max(0.0f, 1.0f + m00 + m11 + m22)) * 0.5f; - q[1] = sqrtf(glm_max(0.0f, 1.0f + m00 - m11 - m22)) * 0.5f; - q[2] = sqrtf(glm_max(0.0f, 1.0f - m00 + m11 - m22)) * 0.5f; - q[3] = sqrtf(glm_max(0.0f, 1.0f - m00 - m11 + m22)) * 0.5f; - */ - x0 = _mm_add_ps(one, _mm_add_ps(_mm_add_ps(m00, m11), m22)); - r = _mm_mul_ps(_mm_sqrt_ps(_mm_max_ps(zero, x0)), half); - - /* - q[1] *= glm_signf(m12 - m21); - q[2] *= glm_signf(m20 - m02); - q[3] *= glm_signf(m01 - m10); - */ - x1 = _mm_shuffle_ps(c1, c2, _MM_SHUFFLE(0, 0, 2, 2)); /* m20 m20 m12 m12 */ - x1 = _mm_shuffle_ps(x1, c0, _MM_SHUFFLE(1, 1, 2, 0)); /* m01 m01 m20 m12 */ - - x2 = _mm_shuffle_ps(c2, c0, _MM_SHUFFLE(2, 2, 1, 1)); /* m02 m02 m21 m21 */ - x2 = _mm_shuffle_ps(x2, c1, _MM_SHUFFLE(0, 0, 2, 0)); /* m10 m10 m02 m21 */ - - x1 = _mm_sub_ps(x1, x2); - x2 = _mm_or_ps(_mm_and_ps(_mm_cmpgt_ps(x1, zero), one), - _mm_and_ps(_mm_cmplt_ps(x1, zero), ngone)); - x2 = _mm_shuffle1_ps(x2, 2, 1, 0, 0); - - x3 = _mm_shuffle_ps(one, x2, _MM_SHUFFLE(0, 0, 0, 0)); /* q1 q1 1 1 */ - x3 = _mm_shuffle_ps(x3, x2, _MM_SHUFFLE(3, 2, 2, 0)); /* q3 q2 q1 1 */ - - _mm_store_ps(dest, _mm_mul_ps(r, x3)); -} - CGLM_INLINE float glm_mat4_det_sse2(mat4 mat) { diff --git a/test/src/test_common.c b/test/src/test_common.c index c13ac0e..b824b37 100644 --- a/test/src/test_common.c +++ b/test/src/test_common.c @@ -55,12 +55,8 @@ test_rand_angle(void) { 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(q, drand48(), drand48(), drand48(), drand48()); glm_quat_normalize(q); } diff --git a/test/src/test_quat.c b/test/src/test_quat.c index 1a328de..36d2ef5 100644 --- a/test/src/test_quat.c +++ b/test/src/test_quat.c @@ -9,14 +9,16 @@ void test_quat(void **state) { - mat4 rot; + mat4 inRot, outRot; versor inQuat, outQuat; int i; - for (i = 0; i < 10000; i++) { + for (i = 0; i < 1000; i++) { test_rand_quat(inQuat); - glmc_quat_mat4(inQuat, rot); - glm_mat4_quat(rot, outQuat); + glmc_quat_mat4(inQuat, inRot); + glmc_mat4_quat(inRot, outQuat); + glmc_quat_mat4(outQuat, outRot); test_assert_quat_eq(inQuat, outQuat); + test_assert_mat4_eq2(inRot, outRot, 0.000009); /* almost equal */ } }