From 086c1cf326a057eb1b9273c8fa74035c4312d452 Mon Sep 17 00:00:00 2001 From: Recep Aslantas Date: Wed, 26 Oct 2016 11:06:48 +0300 Subject: [PATCH] fix euler: use ExEyEz instead of RxRyRz * implement other sequences * always use vec3 for store/get angles --- include/cglm-euler.h | 220 +++++++++++++++++++++---------------------- 1 file changed, 108 insertions(+), 112 deletions(-) diff --git a/include/cglm-euler.h b/include/cglm-euler.h index eaffc61..9b8dba4 100644 --- a/include/cglm-euler.h +++ b/include/cglm-euler.h @@ -10,56 +10,6 @@ #include "cglm-common.h" -/*! - * @brief euler angles (in radian) using xyz sequence - * - * @param[in] m affine transform - * @param[out] pitch x - * @param[out] yaw y - * @param[out] roll z - */ -CGLM_INLINE -void -glm_euler_angles(mat4 m, - float * __restrict pitch, - float * __restrict yaw, - float * __restrict roll) { - if (m[2][0] < 1.0f) { - if (m[2][0] > -1.0f) { - vec3 a[2]; - float cy1, cy2; - int path; - - a[0][1] = asinf(m[2][0]); - a[1][1] = M_PI - a[0][1]; - - cy1 = cosf(a[0][1]); - cy2 = cosf(a[1][1]); - - a[0][0] = atan2f(-m[2][1] / cy1, m[2][2] / cy1); - a[1][0] = atan2f(-m[2][1] / cy2, m[2][2] / cy2); - - a[0][2] = atan2f(-m[1][0] / cy1, m[0][0] / cy1); - a[1][2] = atan2f(-m[1][0] / cy2, m[0][0] / cy2); - - path = (fabsf(a[0][0]) + fabsf(a[0][1]) + fabsf(a[0][2])) > - (fabsf(a[1][0]) + fabsf(a[1][1]) + fabsf(a[1][2])); - - *pitch = a[path][0]; - *yaw = a[path][1]; - *roll = a[path][2]; - } else { - *pitch = -atan2(m[0][1], m[2][1]); - *yaw = -M_PI_2; - *roll = 0.0f; - } - } else { - *pitch = atan2f(m[0][1], m[1][1]); - *yaw = M_PI_2; - *roll = 0.0f; - } -} - /*! * @brief euler angles (in radian) using xyz sequence * @@ -68,65 +18,65 @@ glm_euler_angles(mat4 m, */ CGLM_INLINE void -glm_euler_anglesv(mat4 m, vec3 v) { +glm_euler_angles(mat4 m, vec3 dest) { if (m[2][0] < 1.0f) { if (m[2][0] > -1.0f) { vec3 a[2]; float cy1, cy2; int path; - - a[0][1] = asinf(m[2][0]); + + a[0][1] = asinf(-m[0][2]); a[1][1] = M_PI - a[0][1]; cy1 = cosf(a[0][1]); cy2 = cosf(a[1][1]); - a[0][0] = atan2f(-m[2][1] / cy1, m[2][2] / cy1); - a[1][0] = atan2f(-m[2][1] / cy2, m[2][2] / cy2); + a[0][0] = atan2f(m[1][2] / cy1, m[2][2] / cy1); + a[1][0] = atan2f(m[1][2] / cy2, m[2][2] / cy2); - a[0][2] = atan2f(-m[1][0] / cy1, m[0][0] / cy1); - a[1][2] = atan2f(-m[1][0] / cy2, m[0][0] / cy2); + a[0][2] = atan2f(m[0][1] / cy1, m[0][0] / cy1); + a[1][2] = atan2f(m[0][1] / cy2, m[0][0] / cy2); - path = (fabsf(a[0][0]) + fabsf(a[0][1]) + fabsf(a[0][2])) > + path = (fabsf(a[0][0]) + fabsf(a[0][1]) + fabsf(a[0][2])) >= (fabsf(a[1][0]) + fabsf(a[1][1]) + fabsf(a[1][2])); - glm_vec_dup(a[path], v); + glm_vec_dup(a[path], dest); } else { - v[0] = -atan2(m[0][1], m[2][1]); - v[1] = -M_PI_2; - v[3] = 0.0f; + dest[0] = -atan2(m[0][1], m[2][1]); + dest[1] = -M_PI_2; + dest[3] = 0.0f; } } else { - v[0] = atan2f(m[0][1], m[1][1]); - v[1] = M_PI_2; - v[2] = 0; + dest[0] = atan2f(m[0][1], m[1][1]); + dest[1] = M_PI_2; + dest[2] = 0.0f; } } /*! - * @brief build rotation matrix from euler angles(xyz) + * @brief build rotation matrix from euler angles(ExEyEz/RzRyRx) + * + * @param[in] angles angles as vector [Ex, Ey, Ez] + * @param[out] dest rotation matrix */ CGLM_INLINE void -glm_euler(float pitch, - float yaw, - float roll, - mat4 dest) { +glm_euler(vec3 angles, mat4 dest) { float cx, cy, cz, sx, sy, sz; - sx = sinf(pitch); cx = cosf(pitch); - sy = sinf(yaw); cy = cosf(yaw); - sz = sinf(roll); cz = cosf(roll); + sx = sinf(angles[0]); cx = cosf(angles[0]); + sy = sinf(angles[1]); cy = cosf(angles[1]); + sz = sinf(angles[2]); cz = cosf(angles[2]); dest[0][0] = cy * cz; - dest[0][1] = cz * sx * sy + cx * sz; - dest[0][2] =-cx * cz * sy + sx * sz; - dest[1][0] =-cy * sz; - dest[1][1] = cx * cz - sx * sy * sz; - dest[1][2] = cz * sx + cx * sy * sz; - dest[2][0] = sy; - dest[2][1] =-cy * sx; + dest[0][1] = cy * sz; + dest[0][2] =-sy; + dest[1][0] = cz * sx * sy - cx * sz; + dest[1][1] = cx * cz + sx * sy * sz; + dest[1][2] = cy * sx; + dest[2][0] = cx * cz * sy + sx * sz; + dest[2][1] =-cz * sx + cx * sy * sz; dest[2][2] = cx * cy; dest[0][3] = 0.0f; dest[1][3] = 0.0f; @@ -138,14 +88,12 @@ glm_euler(float pitch, } /*! - * @brief build rotation matrix from euler angles(xyz) - * - * @param[in] angles angles as vector [x, y, z] - * @param[out] dest rotation matrix + * @brief build rotation matrix from euler angles (EzEyEx/RxRyRz) */ CGLM_INLINE void -glm_eulerv(vec3 angles, mat4 dest) { +glm_euler_zyx(vec3 angles, + mat4 dest) { float cx, cy, cz, sx, sy, sz; @@ -171,30 +119,25 @@ glm_eulerv(vec3 angles, mat4 dest) { dest[3][3] = 1.0f; } -/*! - * @brief build rotation matrix from euler angles (zyx) - */ CGLM_INLINE void -glm_euler_zyx(float yaw, - float pitch, - float roll, +glm_euler_zxy(vec3 angles, mat4 dest) { float cx, cy, cz, sx, sy, sz; - sx = sinf(pitch); cx = cosf(pitch); - sy = sinf(yaw); cy = cosf(yaw); - sz = sinf(roll); cz = cosf(roll); + sx = sinf(angles[0]); cx = cosf(angles[0]); + sy = sinf(angles[1]); cy = cosf(angles[1]); + sz = sinf(angles[2]); cz = cosf(angles[2]); - dest[0][0] = cy * cz; - dest[0][1] = cy * sz; - dest[0][2] =-sy; - dest[1][0] = cz * sx * sy - cx * sz; - dest[1][1] = cx * cz + sx * sy * sz; - dest[1][2] = cy * sx; - dest[2][0] = cx * cz * sy + sx * sz; - dest[2][1] =-cz * sx + cx * sy * sz; + dest[0][0] = cy * cz + sx * sy * sz; + dest[0][1] = cx * sz; + dest[0][2] =-cz * sy + cy * sx * sz; + dest[1][0] = cz * sx * sy - cy * sz; + dest[1][1] = cx * cz; + dest[1][2] = cy * cz * sx + sy * sz; + dest[2][0] = cx * sy; + dest[2][1] =-sx; dest[2][2] = cx * cy; dest[0][3] = 0.0f; dest[1][3] = 0.0f; @@ -205,24 +148,77 @@ glm_euler_zyx(float yaw, dest[3][3] = 1.0f; } -/*! - * @brief build rotation matrix from euler angles (zxy) - */ CGLM_INLINE void -glm_euler_zxy(float yaw, - float pitch, - float roll, +glm_euler_xzy(vec3 angles, mat4 dest) { float cx, cy, cz, sx, sy, sz; - sx = sinf(pitch); cx = cosf(pitch); - sy = sinf(yaw); cy = cosf(yaw); - sz = sinf(roll); cz = cosf(roll); + sx = sinf(angles[0]); cx = cosf(angles[0]); + sy = sinf(angles[1]); cy = cosf(angles[1]); + sz = sinf(angles[2]); cz = cosf(angles[2]); + + dest[0][0] = cy * cz; + dest[0][1] = sz; + dest[0][2] =-cz * sy; + dest[1][0] = sx * sy - cx * cy * sz; + dest[1][1] = cx * cz; + dest[1][2] = cy * sx + cx * sy * sz; + dest[2][0] = cx * sy + cy * sx * sz; + dest[2][1] =-cz * sx; + dest[2][2] = cx * cy - sx * sy * sz; + 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; +} + +CGLM_INLINE +void +glm_euler_yzx(vec3 angles, + mat4 dest) { + float cx, cy, cz, + sx, sy, sz; + + sx = sinf(angles[0]); cx = cosf(angles[0]); + sy = sinf(angles[1]); cy = cosf(angles[1]); + sz = sinf(angles[2]); cz = cosf(angles[2]); + + dest[0][0] = cy * cz; + dest[0][1] = sx * sy + cx * cy * sz; + dest[0][2] =-cx * sy + cy * sx * sz; + dest[1][0] =-sz; + dest[1][1] = cx * cz; + dest[1][2] = cz * sx; + dest[2][0] = cz * sy; + dest[2][1] =-cy * sx + cx * sy * sz; + dest[2][2] = cx * cy + sx * sy * sz; + 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; +} + +CGLM_INLINE +void +glm_euler_yxz(vec3 angles, + mat4 dest) { + float cx, cy, cz, + sx, sy, sz; + + sx = sinf(angles[0]); cx = cosf(angles[0]); + sy = sinf(angles[1]); cy = cosf(angles[1]); + sz = sinf(angles[2]); cz = cosf(angles[2]); dest[0][0] = cy * cz - sx * sy * sz; - dest[0][1] = cz * sx * sy + cy + sz; + dest[0][1] = cz * sx * sy + cy * sz; dest[0][2] =-cx * sy; dest[1][0] =-cx * sz; dest[1][1] = cx * cz;