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) {