mirror of
https://github.com/recp/cglm.git
synced 2025-10-03 16:51:35 +00:00
fix euler: use ExEyEz instead of RxRyRz
* implement other sequences * always use vec3 for store/get angles
This commit is contained in:
Recep Aslantas
@@ -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;
|
||||
|
||||
Reference in New Issue
Block a user