diff --git a/docs/source/quat.rst b/docs/source/quat.rst index a8ca028..a44c918 100644 --- a/docs/source/quat.rst +++ b/docs/source/quat.rst @@ -32,6 +32,7 @@ Functions: #. :c:func:`glm_quat` #. :c:func:`glm_quatv` #. :c:func:`glm_quat_copy` +#. :c:func:`glm_quat_from_vecs` #. :c:func:`glm_quat_norm` #. :c:func:`glm_quat_normalize` #. :c:func:`glm_quat_normalize_to` @@ -123,6 +124,20 @@ Functions documentation | *[in]* **q** source quaternion | *[out]* **dest** destination quaternion +.. c:function:: void glm_quat_from_vecs(vec3 a, vec3 b, versor dest) + + | compute unit quaternion needed to rotate a into b + + References: + * `Finding quaternion representing the rotation from one vector to another `_ + * `Quaternion from two vectors `_ + * `Angle between vectors `_ + + Parameters: + | *[in]* **a** unit vector + | *[in]* **b** unit vector + | *[in]* **dest** unit quaternion + .. c:function:: float glm_quat_norm(versor q) | returns norm (magnitude) of quaternion diff --git a/include/cglm/call/quat.h b/include/cglm/call/quat.h index dfd380d..1a2766d 100644 --- a/include/cglm/call/quat.h +++ b/include/cglm/call/quat.h @@ -37,6 +37,10 @@ CGLM_EXPORT void glmc_quat_copy(versor q, versor dest); +CGLM_EXPORT +void +glmc_quat_from_vecs(vec3 a, vec3 b, versor dest); + CGLM_EXPORT float glmc_quat_norm(versor q); diff --git a/include/cglm/quat.h b/include/cglm/quat.h index f531344..c76fa03 100644 --- a/include/cglm/quat.h +++ b/include/cglm/quat.h @@ -16,6 +16,7 @@ CGLM_INLINE void glm_quat(versor q, float angle, float x, float y, float z); CGLM_INLINE void glm_quatv(versor q, float angle, vec3 axis); CGLM_INLINE void glm_quat_copy(versor q, versor dest); + CGLM_INLINE void glm_quat_from_vecs(vec3 a, vec3 b, versor dest); CGLM_INLINE float glm_quat_norm(versor q); CGLM_INLINE void glm_quat_normalize(versor q); CGLM_INLINE void glm_quat_normalize_to(versor q, versor dest); @@ -69,6 +70,8 @@ # include "simd/neon/quat.h" #endif +CGLM_INLINE void glm_quat_normalize(versor q); + /* * IMPORTANT: * ---------------------------------------------------------------------------- @@ -184,10 +187,41 @@ glm_quat_copy(versor q, versor dest) { glm_vec4_copy(q, dest); } +/*! + * @brief compute quaternion rotating vector A to vector B + * + * @param[in] a vec3 (must have unit length) + * @param[in] b vec3 (must have unit length) + * @param[out] dest quaternion (of unit length) + */ +CGLM_INLINE +void +glm_quat_from_vecs(vec3 a, vec3 b, versor dest) { + CGLM_ALIGN(8) vec3 axis; + float cos_theta; + float cos_half_theta; + + cos_theta = glm_vec3_dot(a, b); + if (cos_theta >= 1.f - GLM_FLT_EPSILON) { /* a ∥ b */ + glm_quat_identity(dest); + return; + } + if (cos_theta < -1.f + GLM_FLT_EPSILON) { /* angle(a, b) = π */ + glm_vec3_ortho(a, axis); + cos_half_theta = 0.f; /* cos π/2 */ + } else { + glm_vec3_cross(a, b, axis); + cos_half_theta = 1.0f + cos_theta; /* cos 0 + cos θ */ + } + + glm_quat_init(dest, axis[0], axis[1], axis[2], cos_half_theta); + glm_quat_normalize(dest); +} + /*! * @brief returns norm (magnitude) of quaternion * - * @param[out] q quaternion + * @param[in] q quaternion */ CGLM_INLINE float diff --git a/include/cglm/struct/quat.h b/include/cglm/struct/quat.h index aefddd6..68a0929 100644 --- a/include/cglm/struct/quat.h +++ b/include/cglm/struct/quat.h @@ -16,6 +16,7 @@ CGLM_INLINE versors glms_quat_init(float x, float y, float z, float w) CGLM_INLINE versors glms_quatv(float angle, vec3s axis) CGLM_INLINE versors glms_quat(float angle, float x, float y, float z) + CGLM_INLINE versors glms_quat_from_vecs(vec3s a, vec3s b) CGLM_INLINE float glms_quat_norm(versors q) CGLM_INLINE versors glms_quat_normalize(versors q) CGLM_INLINE float glms_quat_dot(versors p, versors q) @@ -147,10 +148,25 @@ glms_quat(float angle, float x, float y, float z) { return dest; } +/*! + * @brief compute quaternion rotating vector A to vector B + * + * @param[in] a vec3 (must have unit length) + * @param[in] b vec3 (must have unit length) + * @returns quaternion (of unit length) + */ +CGLM_INLINE +versors +glms_quat_from_vecs(vec3s a, vec3s b) { + versors dest; + glm_quat_from_vecs(a.raw, b.raw, dest.raw); + return dest; +} + /*! * @brief returns norm (magnitude) of quaternion * - * @param[out] q quaternion + * @param[in] q quaternion */ CGLM_INLINE float diff --git a/src/quat.c b/src/quat.c index 716bb88..415269a 100644 --- a/src/quat.c +++ b/src/quat.c @@ -44,6 +44,12 @@ glmc_quat_copy(versor q, versor dest) { glm_quat_copy(q, dest); } +CGLM_EXPORT +void +glmc_quat_from_vecs(vec3 a, vec3 b, versor dest) { + glm_quat_from_vecs(a, b, dest); +} + CGLM_EXPORT float glmc_quat_norm(versor q) { diff --git a/test/src/test_quat.h b/test/src/test_quat.h index ee9e7dc..60ca5a1 100644 --- a/test/src/test_quat.h +++ b/test/src/test_quat.h @@ -151,6 +151,59 @@ TEST_IMPL(GLM_PREFIX, quat_copy) { TEST_SUCCESS } +TEST_IMPL(GLM_PREFIX, quat_from_vecs) { + versor q1, q2, q3, q4, q5, q6, q7; + vec3 v1 = {1.f, 0.f, 0.f}, v2 = {1.f, 0.f, 0.f}; // parallel + vec3 v3 = {0.f, 1.f, 0.f}, v4 = {1.f, 0.f, 0.f}; // perpendicular + vec3 v5 = {0.f, 0.f, 1.f}, v6 = {0.f, 0.f, -1.f}; // straight + vec3 v7, v8; // random + vec3 v9 = {0.57735026f, 0.57735026f, 0.57735026f}, // acute + v10 = {0.70710678f, 0.70710678f, 0.f}; + vec3 v11 = {0.87287156f, 0.21821789f, 0.43643578f}, // obtuse + v12 = {-0.87287156f, 0.21821789f, 0.43643578f}; + vec3 v13 = {}; // zero + + GLM(quat_from_vecs)(v1, v2, q1); + ASSERTIFY(test_assert_quat_eq_identity(q1)) + + GLM(quat_from_vecs)(v3, v4, q2); + GLM(quat_rotatev)(q2, v3, v3); + ASSERT(test_eq(GLM(vec3_dot)(v3, v4), 1.f)) + ASSERT(test_eq(q2[0], 0.f)) + ASSERT(test_eq(q2[1], 0.f)) + ASSERT(test_eq(q2[2], -0.707106781187f)) + ASSERT(test_eq(q2[3], 0.707106781187f)) + + GLM(quat_from_vecs)(v5, v6, q3); + GLM(quat_rotatev)(q3, v5, v5); + ASSERT(test_eq(GLM(vec3_dot)(v5, v6), 1.f)) + ASSERT(test_eq(q3[0], 0.f)) + ASSERT(test_eq(q3[1], -1.f)) + ASSERT(test_eq(q3[2], 0.f)) + ASSERT(test_eq(q3[3], 0.f)) + + test_rand_vec3(v7); + test_rand_vec3(v8); + GLM(vec3_normalize(v7)); + GLM(vec3_normalize(v8)); + GLM(quat_from_vecs)(v7, v8, q4); + GLM(quat_rotatev)(q4, v7, v7); + ASSERT(test_eq(GLM(vec3_dot)(v7, v8), 1.f)) + + GLM(quat_from_vecs)(v9, v10, q5); + GLM(quat_rotatev)(q5, v9, v9); + ASSERT(test_eq(GLM(vec3_dot)(v9, v10), 1.f)) + + GLM(quat_from_vecs)(v11, v12, q6); + GLM(quat_rotatev)(q6, v11, v11); + ASSERT(test_eq(GLM(vec3_dot)(v11, v12), 1.f)) + + GLM(quat_from_vecs)(v13, v1, q7); + ASSERTIFY(test_assert_quat_eq_identity(q7)) + + TEST_SUCCESS +} + TEST_IMPL(GLM_PREFIX, quat_norm) { versor a = {10.0f, 9.0f, 8.0f, 78.0f}; float n1, n2; diff --git a/test/tests.h b/test/tests.h index d5688e2..bd1eaa1 100644 --- a/test/tests.h +++ b/test/tests.h @@ -297,6 +297,7 @@ TEST_DECLARE(glm_quat_rotatev) TEST_DECLARE(glm_quat_rotate) TEST_DECLARE(glm_quat_rotate_at) TEST_DECLARE(glm_quat_rotate_atm) +TEST_DECLARE(glm_quat_from_vecs) TEST_DECLARE(glmc_quat_identity) TEST_DECLARE(glmc_quat_identity_array) @@ -334,6 +335,7 @@ TEST_DECLARE(glmc_quat_rotatev) TEST_DECLARE(glmc_quat_rotate) TEST_DECLARE(glmc_quat_rotate_at) TEST_DECLARE(glmc_quat_rotate_atm) +TEST_DECLARE(glmc_quat_from_vecs) /* bezier */ TEST_DECLARE(bezier) @@ -1025,6 +1027,7 @@ TEST_LIST { TEST_ENTRY(glm_quat_rotate) TEST_ENTRY(glm_quat_rotate_at) TEST_ENTRY(glm_quat_rotate_atm) + TEST_ENTRY(glm_quat_from_vecs) TEST_ENTRY(glmc_quat_identity) TEST_ENTRY(glmc_quat_identity_array) @@ -1062,6 +1065,7 @@ TEST_LIST { TEST_ENTRY(glmc_quat_rotate) TEST_ENTRY(glmc_quat_rotate_at) TEST_ENTRY(glmc_quat_rotate_atm) + TEST_ENTRY(glmc_quat_from_vecs) /* bezier */ TEST_ENTRY(bezier)