Files
cglm/include/cglm/quat.h
T

953 lines
22 KiB
C
Raw Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
/*
* Copyright (c), Recep Aslantas.
*
* MIT License (MIT), http://opensource.org/licenses/MIT
* Full license can be found in the LICENSE file
*/
/*
Macros:
GLM_QUAT_IDENTITY_INIT
GLM_QUAT_IDENTITY
Functions:
CGLM_INLINE void glm_quat_identity(versor q);
CGLM_INLINE void glm_quat_init(versor q, float x, float y, float z, float w);
CGLM_INLINE void glm_quat(versor q, float angle, float x, float y, float z);
CGLM_INLINE void glm_quatv(versor q, float angle, const vec3 axis);
CGLM_INLINE void glm_quat_copy(const versor q, versor dest);
CGLM_INLINE void glm_quat_from_vecs(const vec3 a, const vec3 b, versor dest);
CGLM_INLINE float glm_quat_norm(const versor q);
CGLM_INLINE void glm_quat_normalize(versor q);
CGLM_INLINE void glm_quat_normalize_to(const versor q, versor dest);
CGLM_INLINE float glm_quat_dot(const versor p, const versor q);
CGLM_INLINE void glm_quat_conjugate(const versor q, versor dest);
CGLM_INLINE void glm_quat_inv(const versor q, versor dest);
CGLM_INLINE void glm_quat_add(const versor p, const versor q, versor dest);
CGLM_INLINE void glm_quat_sub(const versor p, const versor q, versor dest);
CGLM_INLINE float glm_quat_real(const versor q);
CGLM_INLINE void glm_quat_imag(const versor q, vec3 dest);
CGLM_INLINE void glm_quat_imagn(const versor q, vec3 dest);
CGLM_INLINE float glm_quat_imaglen(const versor q);
CGLM_INLINE float glm_quat_angle(const versor q);
CGLM_INLINE void glm_quat_axis(const versor q, vec3 dest);
CGLM_INLINE void glm_quat_mul(const versor p, const versor q, versor dest);
CGLM_INLINE void glm_quat_mat4(const versor q, mat4 dest);
CGLM_INLINE void glm_quat_mat4t(const versor q, mat4 dest);
CGLM_INLINE void glm_quat_mat3(const versor q, mat3 dest);
CGLM_INLINE void glm_quat_mat3t(const versor q, mat3 dest);
CGLM_INLINE void glm_quat_lerp(const versor from, const versor to, float t, versor dest);
CGLM_INLINE void glm_quat_lerpc(const versor from, const versor to, float t, versor dest);
CGLM_INLINE void glm_quat_slerp(const versor from, const versor to, float t, versor dest);
CGLM_INLINE void glm_quat_slerp_longest(const versor from, const versor to, float t, versor dest);
CGLM_INLINE void glm_quat_nlerp(const versor from, const versor to, float t, versor dest);
CGLM_INLINE void glm_quat_look(const vec3 eye, const versor ori, mat4 dest);
CGLM_INLINE void glm_quat_for(const vec3 dir, const vec3 up, versor dest);
CGLM_INLINE void glm_quat_forp(const vec3 from,
const vec3 to,
const vec3 up,
versor dest);
CGLM_INLINE void glm_quat_rotatev(const versor q, const vec3 v, vec3 dest);
CGLM_INLINE void glm_quat_rotate(CGLM_CONST mat4 m, const versor q, mat4 dest);
CGLM_INLINE void glm_quat_make(const float * __restrict src, versor dest);
*/
#ifndef cglm_quat_h
#define cglm_quat_h
#include "common.h"
#include "vec3.h"
#include "vec4.h"
#include "mat4.h"
#include "mat3.h"
#include "affine-mat.h"
#include "affine.h"
#ifdef CGLM_SSE_FP
# include "simd/sse2/quat.h"
#endif
#ifdef CGLM_NEON_FP
# include "simd/neon/quat.h"
#endif
#ifdef CGLM_SIMD_WASM
# include "simd/wasm/quat.h"
#endif
CGLM_INLINE void glm_quat_normalize(versor q);
/*
* IMPORTANT:
* ----------------------------------------------------------------------------
* cglm stores quat as [x, y, z, w] since v0.3.6
*
* it was [w, x, y, z] before v0.3.6 it has been changed to [x, y, z, w]
* with v0.3.6 version.
* ----------------------------------------------------------------------------
*/
#define GLM_QUAT_IDENTITY_INIT {0.0f, 0.0f, 0.0f, 1.0f}
#define GLM_QUAT_IDENTITY ((versor)GLM_QUAT_IDENTITY_INIT)
/*!
* @brief makes given quat to identity
*
* @param[in, out] q quaternion
*/
CGLM_INLINE
void
glm_quat_identity(versor q) {
CGLM_ALIGN(16) versor v = GLM_QUAT_IDENTITY_INIT;
glm_vec4_copy(v, q);
}
/*!
* @brief make given quaternion array's each element identity quaternion
*
* @param[in, out] q quat array (must be aligned (16)
* if alignment is not disabled)
*
* @param[in] count count of quaternions
*/
CGLM_INLINE
void
glm_quat_identity_array(versor * __restrict q, size_t count) {
CGLM_ALIGN(16) versor v = GLM_QUAT_IDENTITY_INIT;
size_t i;
for (i = 0; i < count; i++) {
glm_vec4_copy(v, q[i]);
}
}
/*!
* @brief inits quaternion with raw values
*
* @param[out] q quaternion
* @param[in] x x
* @param[in] y y
* @param[in] z z
* @param[in] w w (real part)
*/
CGLM_INLINE
void
glm_quat_init(versor q, float x, float y, float z, float w) {
q[0] = x;
q[1] = y;
q[2] = z;
q[3] = w;
}
/*!
* @brief creates NEW quaternion with axis vector
*
* @param[out] q quaternion
* @param[in] angle angle (radians)
* @param[in] axis axis
*/
CGLM_INLINE
void
glm_quatv(versor q, float angle, const vec3 axis) {
CGLM_ALIGN(8) vec3 k;
float a, c, s;
a = angle * 0.5f;
c = cosf(a);
s = sinf(a);
glm_normalize_to(axis, k);
q[0] = s * k[0];
q[1] = s * k[1];
q[2] = s * k[2];
q[3] = c;
}
/*!
* @brief creates NEW quaternion with individual axis components
*
* @param[out] q quaternion
* @param[in] angle angle (radians)
* @param[in] x axis.x
* @param[in] y axis.y
* @param[in] z axis.z
*/
CGLM_INLINE
void
glm_quat(versor q, float angle, float x, float y, float z) {
CGLM_ALIGN(8) vec3 axis = {x, y, z};
glm_quatv(q, angle, axis);
}
/*!
* @brief copy quaternion to another one
*
* @param[in] q quaternion
* @param[out] dest destination
*/
CGLM_INLINE
void
glm_quat_copy(const 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(const vec3 a, const 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[in] q quaternion
*/
CGLM_INLINE
float
glm_quat_norm(const versor q) {
return glm_vec4_norm(q);
}
/*!
* @brief normalize quaternion and store result in dest
*
* @param[in] q quaternion to normalze
* @param[out] dest destination quaternion
*/
CGLM_INLINE
void
glm_quat_normalize_to(const versor q, versor dest) {
#if defined(__wasm__) && defined(__wasm_simd128__)
glmm_128 xdot, x0;
float dot;
x0 = glmm_load(q);
xdot = glmm_vdot(x0, x0);
/* dot = _mm_cvtss_f32(xdot); */
dot = wasm_f32x4_extract_lane(xdot, 0);
if (dot <= 0.0f) {
glm_quat_identity(dest);
return;
}
glmm_store(dest, wasm_f32x4_div(x0, wasm_f32x4_sqrt(xdot)));
#elif defined( __SSE__ ) || defined( __SSE2__ )
__m128 xdot, x0;
float dot;
x0 = glmm_load(q);
xdot = glmm_vdot(x0, x0);
dot = _mm_cvtss_f32(xdot);
if (dot <= 0.0f) {
glm_quat_identity(dest);
return;
}
glmm_store(dest, _mm_div_ps(x0, _mm_sqrt_ps(xdot)));
#else
float dot;
dot = glm_vec4_norm2(q);
if (dot <= 0.0f) {
glm_quat_identity(dest);
return;
}
glm_vec4_scale(q, 1.0f / sqrtf(dot), dest);
#endif
}
/*!
* @brief normalize quaternion
*
* @param[in, out] q quaternion
*/
CGLM_INLINE
void
glm_quat_normalize(versor q) {
glm_quat_normalize_to(q, q);
}
/*!
* @brief dot product of two quaternion
*
* @param[in] p quaternion 1
* @param[in] q quaternion 2
*/
CGLM_INLINE
float
glm_quat_dot(const versor p, const versor q) {
return glm_vec4_dot(p, q);
}
/*!
* @brief conjugate of quaternion
*
* @param[in] q quaternion
* @param[out] dest conjugate
*/
CGLM_INLINE
void
glm_quat_conjugate(const versor q, versor dest) {
glm_vec4_negate_to(q, dest);
dest[3] = -dest[3];
}
/*!
* @brief inverse of non-zero quaternion
*
* @param[in] q quaternion
* @param[out] dest inverse quaternion
*/
CGLM_INLINE
void
glm_quat_inv(const versor q, versor dest) {
CGLM_ALIGN(16) versor conj;
glm_quat_conjugate(q, conj);
glm_vec4_scale(conj, 1.0f / glm_vec4_norm2(q), dest);
}
/*!
* @brief add (componentwise) two quaternions and store result in dest
*
* @param[in] p quaternion 1
* @param[in] q quaternion 2
* @param[out] dest result quaternion
*/
CGLM_INLINE
void
glm_quat_add(const versor p, const versor q, versor dest) {
glm_vec4_add(p, q, dest);
}
/*!
* @brief subtract (componentwise) two quaternions and store result in dest
*
* @param[in] p quaternion 1
* @param[in] q quaternion 2
* @param[out] dest result quaternion
*/
CGLM_INLINE
void
glm_quat_sub(const versor p, const versor q, versor dest) {
glm_vec4_sub(p, q, dest);
}
/*!
* @brief returns real part of quaternion
*
* @param[in] q quaternion
*/
CGLM_INLINE
float
glm_quat_real(const versor q) {
return q[3];
}
/*!
* @brief returns imaginary part of quaternion
*
* @param[in] q quaternion
* @param[out] dest imag
*/
CGLM_INLINE
void
glm_quat_imag(const versor q, vec3 dest) {
dest[0] = q[0];
dest[1] = q[1];
dest[2] = q[2];
}
/*!
* @brief returns normalized imaginary part of quaternion
*
* @param[in] q quaternion
*/
CGLM_INLINE
void
glm_quat_imagn(const versor q, vec3 dest) {
glm_normalize_to(q, dest);
}
/*!
* @brief returns length of imaginary part of quaternion
*
* @param[in] q quaternion
*/
CGLM_INLINE
float
glm_quat_imaglen(const versor q) {
return glm_vec3_norm(q);
}
/*!
* @brief returns angle of quaternion
*
* @param[in] q quaternion
*/
CGLM_INLINE
float
glm_quat_angle(const versor q) {
/*
sin(theta / 2) = length(x*x + y*y + z*z)
cos(theta / 2) = w
theta = 2 * atan(sin(theta / 2) / cos(theta / 2))
*/
return 2.0f * atan2f(glm_quat_imaglen(q), glm_quat_real(q));
}
/*!
* @brief axis of quaternion
*
* @param[in] q quaternion
* @param[out] dest axis of quaternion
*/
CGLM_INLINE
void
glm_quat_axis(const versor q, vec3 dest) {
glm_quat_imagn(q, dest);
}
/*!
* @brief multiplies two quaternion and stores result in dest
* this is also called Hamilton Product
*
* According to WikiPedia:
* The product of two rotation quaternions [clarification needed] will be
* equivalent to the rotation q followed by the rotation p
*
* @param[in] p quaternion 1
* @param[in] q quaternion 2
* @param[out] dest result quaternion
*/
CGLM_INLINE
void
glm_quat_mul(const versor p, const versor q, versor dest) {
/*
+ (a1 b2 + b1 a2 + c1 d2 − d1 c2)i
+ (a1 c2 − b1 d2 + c1 a2 + d1 b2)j
+ (a1 d2 + b1 c2 − c1 b2 + d1 a2)k
a1 a2 − b1 b2 − c1 c2 − d1 d2
*/
#if defined(__wasm__) && defined(__wasm_simd128__)
glm_quat_mul_wasm(p, q, dest);
#elif defined( __SSE__ ) || defined( __SSE2__ )
glm_quat_mul_sse2(p, q, dest);
#elif defined(CGLM_NEON_FP)
glm_quat_mul_neon(p, q, dest);
#else
dest[0] = p[3] * q[0] + p[0] * q[3] + p[1] * q[2] - p[2] * q[1];
dest[1] = p[3] * q[1] - p[0] * q[2] + p[1] * q[3] + p[2] * q[0];
dest[2] = p[3] * q[2] + p[0] * q[1] - p[1] * q[0] + p[2] * q[3];
dest[3] = p[3] * q[3] - p[0] * q[0] - p[1] * q[1] - p[2] * q[2];
#endif
}
/*!
* @brief convert quaternion to mat4
*
* @param[in] q quaternion
* @param[out] dest result matrix
*/
CGLM_INLINE
void
glm_quat_mat4(const versor q, mat4 dest) {
CGLM_ALIGN(16) versor p;
float x, y, z,
xx, yy, zz,
xy, yz, xz,
wx, wy, wz;
glm_quat_normalize_to(q, p);
x = p[0];
y = p[1];
z = p[2];
glm_vec4_scale(p, 2.0f, p);
xx = p[0] * x; xy = p[0] * y; wx = p[3] * x;
yy = p[1] * y; yz = p[1] * z; wy = p[3] * y;
zz = p[2] * z; xz = p[0] * z; wz = p[3] * z;
dest[0][0] = 1.0f - yy - zz;
dest[1][1] = 1.0f - xx - zz;
dest[2][2] = 1.0f - xx - yy;
dest[0][1] = xy + wz;
dest[1][2] = yz + wx;
dest[2][0] = xz + wy;
dest[1][0] = xy - wz;
dest[2][1] = yz - wx;
dest[0][2] = xz - wy;
dest[0][3] = 0.0f;
dest[1][3] = 0.0f;
dest[2][3] = 0.0f;
dest[3][0] = 0.0f;
dest[3][1] = 0.0f;
dest[3][2] = 0.0f;
dest[3][3] = 1.0f;
}
/*!
* @brief convert quaternion to mat4 (transposed)
*
* @param[in] q quaternion
* @param[out] dest result matrix as transposed
*/
CGLM_INLINE
void
glm_quat_mat4t(const versor q, mat4 dest) {
CGLM_ALIGN(16) versor p;
float x, y, z,
xx, yy, zz,
xy, yz, xz,
wx, wy, wz;
glm_quat_normalize_to(q, p);
x = p[0];
y = p[1];
z = p[2];
glm_vec4_scale(p, 2.0f, p);
xx = p[0] * x; xy = p[0] * y; wx = p[3] * x;
yy = p[1] * y; yz = p[1] * z; wy = p[3] * y;
zz = p[2] * z; xz = p[0] * z; wz = p[3] * z;
dest[0][0] = 1.0f - yy - zz;
dest[1][1] = 1.0f - xx - zz;
dest[2][2] = 1.0f - xx - yy;
dest[1][0] = xy + wz;
dest[2][1] = yz + wx;
dest[0][2] = xz + wy;
dest[0][1] = xy - wz;
dest[1][2] = yz - wx;
dest[2][0] = xz - wy;
dest[0][3] = 0.0f;
dest[1][3] = 0.0f;
dest[2][3] = 0.0f;
dest[3][0] = 0.0f;
dest[3][1] = 0.0f;
dest[3][2] = 0.0f;
dest[3][3] = 1.0f;
}
/*!
* @brief convert quaternion to mat3
*
* @param[in] q quaternion
* @param[out] dest result matrix
*/
CGLM_INLINE
void
glm_quat_mat3(const versor q, mat3 dest) {
CGLM_ALIGN(16) versor p;
float x, y, z,
xx, yy, zz,
xy, yz, xz,
wx, wy, wz;
glm_quat_normalize_to(q, p);
x = p[0];
y = p[1];
z = p[2];
glm_vec4_scale(p, 2.0f, p);
xx = p[0] * x; xy = p[0] * y; wx = p[3] * x;
yy = p[1] * y; yz = p[1] * z; wy = p[3] * y;
zz = p[2] * z; xz = p[0] * z; wz = p[3] * z;
dest[0][0] = 1.0f - yy - zz;
dest[1][1] = 1.0f - xx - zz;
dest[2][2] = 1.0f - xx - yy;
dest[0][1] = xy + wz;
dest[1][2] = yz + wx;
dest[2][0] = xz + wy;
dest[1][0] = xy - wz;
dest[2][1] = yz - wx;
dest[0][2] = xz - wy;
}
/*!
* @brief convert quaternion to mat3 (transposed)
*
* @param[in] q quaternion
* @param[out] dest result matrix
*/
CGLM_INLINE
void
glm_quat_mat3t(const versor q, mat3 dest) {
CGLM_ALIGN(16) versor p;
float x, y, z,
xx, yy, zz,
xy, yz, xz,
wx, wy, wz;
glm_quat_normalize_to(q, p);
x = p[0];
y = p[1];
z = p[2];
glm_vec4_scale(p, 2.0f, p);
xx = p[0] * x; xy = p[0] * y; wx = p[3] * x;
yy = p[1] * y; yz = p[1] * z; wy = p[3] * y;
zz = p[2] * z; xz = p[0] * z; wz = p[3] * z;
dest[0][0] = 1.0f - yy - zz;
dest[1][1] = 1.0f - xx - zz;
dest[2][2] = 1.0f - xx - yy;
dest[1][0] = xy + wz;
dest[2][1] = yz + wx;
dest[0][2] = xz + wy;
dest[0][1] = xy - wz;
dest[1][2] = yz - wx;
dest[2][0] = xz - wy;
}
/*!
* @brief interpolates between two quaternions
* using linear interpolation (LERP)
*
* @param[in] from from
* @param[in] to to
* @param[in] t interpolant (amount)
* @param[out] dest result quaternion
*/
CGLM_INLINE
void
glm_quat_lerp(const versor from, const versor to, float t, versor dest) {
glm_vec4_lerp(from, to, t, dest);
}
/*!
* @brief interpolates between two quaternions
* using linear interpolation (LERP)
*
* @param[in] from from
* @param[in] to to
* @param[in] t interpolant (amount) clamped between 0 and 1
* @param[out] dest result quaternion
*/
CGLM_INLINE
void
glm_quat_lerpc(const versor from, const versor to, float t, versor dest) {
glm_vec4_lerpc(from, to, t, dest);
}
/*!
* @brief interpolates between two quaternions
* taking the shortest rotation path using
* normalized linear interpolation (NLERP)
*
* @param[in] from from
* @param[in] to to
* @param[in] t interpolant (amount)
* @param[out] dest result quaternion
*/
CGLM_INLINE
void
glm_quat_nlerp(const versor from, const versor to, float t, versor dest) {
versor target;
float dot;
dot = glm_vec4_dot(from, to);
glm_vec4_scale(to, (dot >= 0) ? 1.0f : -1.0f, target);
glm_quat_lerp(from, target, t, dest);
glm_quat_normalize(dest);
}
/*!
* @brief interpolates between two quaternions
* using spherical linear interpolation (SLERP)
*
* @param[in] from from
* @param[in] to to
* @param[in] t amount
* @param[out] dest result quaternion
*/
CGLM_INLINE
void
glm_quat_slerp(const versor from, const versor to, float t, versor dest) {
CGLM_ALIGN(16) vec4 q1, q2;
float cosTheta, sinTheta, angle;
cosTheta = glm_quat_dot(from, to);
glm_quat_copy(from, q1);
if (fabsf(cosTheta) >= 1.0f) {
glm_quat_copy(q1, dest);
return;
}
if (cosTheta < 0.0f) {
glm_vec4_negate(q1);
cosTheta = -cosTheta;
}
sinTheta = sqrtf(1.0f - cosTheta * cosTheta);
/* LERP to avoid zero division */
if (fabsf(sinTheta) < 0.001f) {
glm_quat_lerp(from, to, t, dest);
return;
}
/* SLERP */
angle = acosf(cosTheta);
glm_vec4_scale(q1, sinf((1.0f - t) * angle), q1);
glm_vec4_scale(to, sinf(t * angle), q2);
glm_vec4_add(q1, q2, q1);
glm_vec4_scale(q1, 1.0f / sinTheta, dest);
}
/*!
* @brief interpolates between two quaternions
* using spherical linear interpolation (SLERP) and always takes the long path
*
* @param[in] from from
* @param[in] to to
* @param[in] t amount
* @param[out] dest result quaternion
*/
CGLM_INLINE
void
glm_quat_slerp_longest(const versor from, const versor to, float t, versor dest) {
CGLM_ALIGN(16) vec4 q1, q2;
float cosTheta, sinTheta, angle;
cosTheta = glm_quat_dot(from, to);
glm_quat_copy(from, q1);
if (fabsf(cosTheta) >= 1.0f) {
glm_quat_copy(q1, dest);
return;
}
/* longest path */
if (!(cosTheta < 0.0f)) {
glm_vec4_negate(q1);
cosTheta = -cosTheta;
}
sinTheta = sqrtf(1.0f - cosTheta * cosTheta);
/* LERP to avoid zero division */
if (fabsf(sinTheta) < 0.001f) {
glm_quat_lerp(from, to, t, dest);
return;
}
/* SLERP */
angle = acosf(cosTheta);
glm_vec4_scale(q1, sinf((1.0f - t) * angle), q1);
glm_vec4_scale(to, sinf(t * angle), q2);
glm_vec4_add(q1, q2, q1);
glm_vec4_scale(q1, 1.0f / sinTheta, dest);
}
/*!
* @brief creates view matrix using quaternion as camera orientation
*
* @param[in] eye eye
* @param[in] ori orientation in world space as quaternion
* @param[out] dest view matrix
*/
CGLM_INLINE
void
glm_quat_look(const vec3 eye, const versor ori, mat4 dest) {
/* orientation */
glm_quat_mat4t(ori, dest);
/* translate */
glm_mat4_mulv3(dest, eye, 1.0f, dest[3]);
glm_vec3_negate(dest[3]);
}
/*!
* @brief creates look rotation quaternion
*
* @param[in] dir direction to look
* @param[in] up up vector
* @param[out] dest destination quaternion
*/
CGLM_INLINE
void
glm_quat_for(const vec3 dir, const vec3 up, versor dest) {
CGLM_ALIGN_MAT mat3 m;
glm_vec3_normalize_to(dir, m[2]);
/* No need to negate in LH, but we use RH here */
glm_vec3_negate(m[2]);
glm_vec3_crossn(up, m[2], m[0]);
glm_vec3_cross(m[2], m[0], m[1]);
glm_mat3_quat(m, dest);
}
/*!
* @brief creates look rotation quaternion using source and
* destination positions p suffix stands for position
*
* @param[in] from source point
* @param[in] to destination point
* @param[in] up up vector
* @param[out] dest destination quaternion
*/
CGLM_INLINE
void
glm_quat_forp(const vec3 from, const vec3 to, const vec3 up, versor dest) {
CGLM_ALIGN(8) vec3 dir;
glm_vec3_sub(to, from, dir);
glm_quat_for(dir, up, dest);
}
/*!
* @brief rotate vector using using quaternion
*
* @param[in] q quaternion
* @param[in] v vector to rotate
* @param[out] dest rotated vector
*/
CGLM_INLINE
void
glm_quat_rotatev(const versor q, const vec3 v, vec3 dest) {
CGLM_ALIGN(16) versor p;
CGLM_ALIGN(8) vec3 u, v1, v2;
float s;
glm_quat_normalize_to(q, p);
glm_quat_imag(p, u);
s = glm_quat_real(p);
glm_vec3_scale(u, 2.0f * glm_vec3_dot(u, v), v1);
glm_vec3_scale(v, s * s - glm_vec3_dot(u, u), v2);
glm_vec3_add(v1, v2, v1);
glm_vec3_cross(u, v, v2);
glm_vec3_scale(v2, 2.0f * s, v2);
glm_vec3_add(v1, v2, dest);
}
/*!
* @brief rotate existing transform matrix using quaternion
*
* @param[in] m existing transform matrix
* @param[in] q quaternion
* @param[out] dest rotated matrix/transform
*/
CGLM_INLINE
void
glm_quat_rotate(CGLM_CONST mat4 m, const versor q, mat4 dest) {
CGLM_ALIGN_MAT mat4 rot;
glm_quat_mat4(q, rot);
glm_mul_rot(m, rot, dest);
}
/*!
* @brief rotate existing transform matrix using quaternion at pivot point
*
* @param[in, out] m existing transform matrix
* @param[in] q quaternion
* @param[in] pivot pivot
*/
CGLM_INLINE
void
glm_quat_rotate_at(mat4 m, const versor q, const vec3 pivot) {
CGLM_ALIGN(8) vec3 pivotInv;
glm_vec3_negate_to(pivot, pivotInv);
glm_translate(m, pivot);
glm_quat_rotate(m, q, m);
glm_translate(m, pivotInv);
}
/*!
* @brief rotate NEW transform matrix using quaternion at pivot point
*
* this creates rotation matrix, it assumes you don't have a matrix
*
* this should work faster than glm_quat_rotate_at because it reduces
* one glm_translate.
*
* @param[out] m existing transform matrix
* @param[in] q quaternion
* @param[in] pivot pivot
*/
CGLM_INLINE
void
glm_quat_rotate_atm(mat4 m, const versor q, const vec3 pivot) {
CGLM_ALIGN(8) vec3 pivotInv;
glm_vec3_negate_to(pivot, pivotInv);
glm_translate_make(m, pivot);
glm_quat_rotate(m, q, m);
glm_translate(m, pivotInv);
}
/*!
* @brief Create quaternion from pointer
*
* @param[in] src pointer to an array of floats
* @param[out] dest quaternion
*/
CGLM_INLINE
void
glm_quat_make(const float * __restrict src, versor dest) {
dest[0] = src[0]; dest[1] = src[1];
dest[2] = src[2]; dest[3] = src[3];
}
#endif /* cglm_quat_h */