From b5802b99b2b36320fa5fc1a979c26ea219eb58ad Mon Sep 17 00:00:00 2001 From: Sundaram Ramaswamy Date: Thu, 10 Jun 2021 22:53:28 +0530 Subject: [PATCH 1/4] Compute quaternion rotating a vector into another Both `vec3` inputs should be of unit length; returns a unit quaternion. --- include/cglm/call/quat.h | 4 ++++ include/cglm/quat.h | 35 ++++++++++++++++++++++++++++++++++- include/cglm/struct/quat.h | 18 +++++++++++++++++- src/quat.c | 6 ++++++ 4 files changed, 61 insertions(+), 2 deletions(-) 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..a57e136 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,40 @@ 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) { + float cos_theta = glm_vec3_dot(a, b); + if (cos_theta >= 1.f - GLM_FLT_EPSILON) { // a ∥ b + glm_quat_identity(dest); + return; + } + CGLM_ALIGN(8) vec3 axis; + float cos_half_theta; + if (cos_theta < -1.f + GLM_FLT_EPSILON) { // angle(a, b) = 180° + glm_vec3_ortho(a, axis); + cos_half_theta = 0.f; + } + else { + glm_vec3_cross(a, b, axis); + const float cos_zero = 1.0f; + cos_half_theta = cos_zero + cos_theta; + } + 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) { From ee5050f43c90e864207dfe06ba5d00052d141782 Mon Sep 17 00:00:00 2001 From: Sundaram Ramaswamy Date: Thu, 10 Jun 2021 23:11:03 +0530 Subject: [PATCH 2/4] Tests for quaternion from two vec3 --- test/src/test_quat.h | 53 ++++++++++++++++++++++++++++++++++++++++++++ test/tests.h | 4 ++++ 2 files changed, 57 insertions(+) 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) From 01e2b74a29debe3c4cf203f3da28673f3d587aa0 Mon Sep 17 00:00:00 2001 From: Sundaram Ramaswamy Date: Thu, 10 Jun 2021 23:51:25 +0530 Subject: [PATCH 3/4] Update documentation for qaut_from_vecs --- docs/source/quat.rst | 15 +++++++++++++++ 1 file changed, 15 insertions(+) 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 From 03b4594d9e6718c8341faad85201db0c09ba3438 Mon Sep 17 00:00:00 2001 From: Sundaram Ramaswamy Date: Tue, 15 Jun 2021 19:04:51 +0530 Subject: [PATCH 4/4] quat_from_vecs: incorporate PR comments MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * C89-style comments * Move all variable declarations to function start * Remove constant variables * Remove newline for ‘else’ --- include/cglm/quat.h | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/include/cglm/quat.h b/include/cglm/quat.h index a57e136..c76fa03 100644 --- a/include/cglm/quat.h +++ b/include/cglm/quat.h @@ -197,22 +197,23 @@ glm_quat_copy(versor q, versor dest) { CGLM_INLINE void glm_quat_from_vecs(vec3 a, vec3 b, versor dest) { - float cos_theta = glm_vec3_dot(a, b); - if (cos_theta >= 1.f - GLM_FLT_EPSILON) { // a ∥ b + 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; } - CGLM_ALIGN(8) vec3 axis; - float cos_half_theta; - if (cos_theta < -1.f + GLM_FLT_EPSILON) { // angle(a, b) = 180° + if (cos_theta < -1.f + GLM_FLT_EPSILON) { /* angle(a, b) = π */ glm_vec3_ortho(a, axis); - cos_half_theta = 0.f; - } - else { + cos_half_theta = 0.f; /* cos π/2 */ + } else { glm_vec3_cross(a, b, axis); - const float cos_zero = 1.0f; - cos_half_theta = cos_zero + cos_theta; + 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); }