diff --git a/CREDITS b/CREDITS index 66604eb..44e55a8 100644 --- a/CREDITS +++ b/CREDITS @@ -46,3 +46,7 @@ http://old.cescg.org/CESCG-2002/DSykoraJJelinek/ 7. Quaternions Initial mat4_quat is borrowed from Apple's simd library + + +8. Vector Rotation using Quaternion +https://gamedev.stackexchange.com/questions/28395/rotating-vector3-by-a-quaternion diff --git a/include/cglm/quat.h b/include/cglm/quat.h index effebae..664a069 100644 --- a/include/cglm/quat.h +++ b/include/cglm/quat.h @@ -168,7 +168,7 @@ glm_quat_normalize_to(versor q, versor dest) { x0 = _mm_load_ps(q); xdot = glm_simd_dot(x0, x0); - dot = _mm_cvtss_f32(xdot); + dot = _mm_cvtss_f32(xdot); if (dot <= 0.0f) { glm_quat_identity(dest); @@ -676,4 +676,32 @@ glm_quat_forp(vec3 from, vec3 to, vec3 fwd, vec3 up, versor dest) { glm_quat_for(dir, fwd, up, dest); } +/*! + * @brief rotate existing transform matrix using quaternion + * + * @param[in] q quaternion + * @param[in] v vector to rotate + * @param[out] dest destination vector + */ +CGLM_INLINE +void +glm_quat_rotatev(versor q, vec3 v, vec3 dest) { + versor p; + vec3 u, v1, v2; + float s; + + glm_quat_normalize_to(q, p); + glm_quat_imag(p, u); + s = glm_quat_real(p); + + glm_vec_scale(u, 2.0f * glm_vec_dot(u, v), v1); + glm_vec_scale(v, s * s - glm_vec_dot(u, u), v2); + glm_vec_add(v1, v2, v1); + + glm_vec_cross(u, v, v2); + glm_vec_scale(v2, 2.0f * s, v2); + + glm_vec_add(v1, v2, dest); +} + #endif /* cglm_quat_h */ diff --git a/test/src/test_common.c b/test/src/test_common.c index bd6deaf..514c006 100644 --- a/test/src/test_common.c +++ b/test/src/test_common.c @@ -86,9 +86,9 @@ 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); /* rounding errors */ - assert_true(fabsf(v1[1] - v2[1]) <= 0.0000009); - assert_true(fabsf(v1[2] - v2[2]) <= 0.0000009); + assert_true(fabsf(v1[0] - v2[0]) <= 0.000009); /* rounding errors */ + assert_true(fabsf(v1[1] - v2[1]) <= 0.000009); + assert_true(fabsf(v1[2] - v2[2]) <= 0.000009); } void @@ -101,9 +101,9 @@ test_assert_quat_eq_abs(versor v1, versor v2) { void test_assert_quat_eq(versor v1, versor v2) { - 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); - assert_true(fabsf(v1[3] - v2[3]) <= 0.0000009); + assert_true(fabsf(v1[0] - v2[0]) <= 0.000009); /* rounding errors */ + assert_true(fabsf(v1[1] - v2[1]) <= 0.000009); + assert_true(fabsf(v1[2] - v2[2]) <= 0.000009); + assert_true(fabsf(v1[3] - v2[3]) <= 0.000009); } diff --git a/test/src/test_quat.c b/test/src/test_quat.c index 1c1502d..43dbdb2 100644 --- a/test/src/test_quat.c +++ b/test/src/test_quat.c @@ -20,7 +20,7 @@ void test_quat(void **state) { mat4 inRot, outRot, view1, view2, rot1, rot2; versor inQuat, outQuat, q3, q4, q5; - vec3 eye, axis, imag; + vec3 eye, axis, imag, v1, v2; int i; /* 0. test identiy quat */ @@ -134,7 +134,25 @@ test_quat(void **state) { imag[1] = -1.0f; imag[2] = 0.0f; - assert_true(glm_vec_eqv_eps(imag, axis)); + test_assert_vec3_eq(imag, axis); + + /* 10. test rotate vector using quat */ + /* (0,0,-1) around (1,0,0) must give (0,1,0) */ + v1[0] = 0.0f; v1[1] = 0.0f; v1[2] = -1.0f; + v2[0] = 0.0f; v2[1] = 0.0f; v2[2] = -1.0f; + + glm_vec_rotate(v1, glm_rad(90.0f), (vec3){1.0f, 0.0f, 0.0f}); + glm_quatv(q3, glm_rad(90.0f), (vec3){1.0f, 0.0f, 0.0f}); + + glm_vec4_scale(q3, 1.5, q3); + glm_quat_rotatev(q3, v2, v2); + + /* result must be : (0,1,0) */ + assert_true(fabsf(v1[0]) <= 0.00009f + && fabsf(v1[1] - 1.0f) <= 0.00009f + && fabsf(v1[2]) <= 0.00009f); + + test_assert_vec3_eq(v1, v2); /* TODO: add tests for slerp, lerp */ }