From 824491f410d180d9a48e1185cbf5f192001dd837 Mon Sep 17 00:00:00 2001 From: gingerBill Date: Thu, 10 Sep 2020 15:23:08 +0100 Subject: [PATCH] Clean up Euler Angle code for math/linalg --- core/math/linalg/specific.odin | 49 +--- core/math/linalg/specific_euler_angles.odin | 277 +++++++++++++++++--- 2 files changed, 239 insertions(+), 87 deletions(-) diff --git a/core/math/linalg/specific.odin b/core/math/linalg/specific.odin index 13b349db5..08ec234c6 100644 --- a/core/math/linalg/specific.odin +++ b/core/math/linalg/specific.odin @@ -204,49 +204,6 @@ angle_axis_from_quaternion :: proc(q: Quaternion) -> (angle: Float, axis: Vector return; } - -quaternion_from_euler_angles :: proc(pitch, yaw, roll: Float) -> Quaternion { - a, b, c := pitch, yaw, roll; - - ca, sa := math.cos(a*0.5), math.sin(a*0.5); - cb, sb := math.cos(b*0.5), math.sin(b*0.5); - cc, sc := math.cos(c*0.5), math.sin(c*0.5); - - q: Quaternion; - q.x = sa*cb*cc - ca*sb*sc; - q.y = ca*sb*cc + sa*cb*sc; - q.z = ca*cb*sc - sa*sb*cc; - q.w = ca*cb*cc + sa*sb*sc; - return q; -} - -roll_from_quaternion :: proc(q: Quaternion) -> Float { - return math.atan2(2 * q.x*q.y + q.w*q.z, q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z); -} - -pitch_from_quaternion :: proc(q: Quaternion) -> Float { - y := 2 * (q.y*q.z + q.w*q.w); - x := q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z; - - if abs(x) <= FLOAT_EPSILON && abs(y) <= FLOAT_EPSILON { - return 2 * math.atan2(q.x, q.w); - } - - return math.atan2(y, x); -} - -yaw_from_quaternion :: proc(q: Quaternion) -> Float { - return math.asin(clamp(-2 * (q.x*q.z - q.w*q.y), -1, 1)); -} - - -euler_angles_from_quaternion :: proc(q: Quaternion) -> (pitch, yaw, roll: Float) { - pitch = pitch_from_quaternion(q); - yaw = yaw_from_quaternion(q); - roll = roll_from_quaternion(q); - return; -} - quaternion_from_forward_and_up :: proc(forward, up: Vector3) -> Quaternion { f := normalize(forward); s := normalize(cross(f, up)); @@ -620,9 +577,7 @@ matrix3_look_at :: proc(eye, centre, up: Vector3) -> Matrix3 { }; } -matrix4_from_quaternion :: proc(q: Quaternion) -> Matrix4 { - m := MATRIX4_IDENTITY; - +matrix4_from_quaternion :: proc(q: Quaternion) -> (m: Matrix4) { xx := q.x * q.x; xy := q.x * q.y; xz := q.x * q.z; @@ -633,6 +588,8 @@ matrix4_from_quaternion :: proc(q: Quaternion) -> Matrix4 { zz := q.z * q.z; zw := q.z * q.w; + m = MATRIX4_IDENTITY; + m[0][0] = 1 - 2 * (yy + zz); m[1][0] = 2 * (xy - zw); m[2][0] = 2 * (xz + yw); diff --git a/core/math/linalg/specific_euler_angles.odin b/core/math/linalg/specific_euler_angles.odin index 5d50cd788..2f7d5dfa8 100644 --- a/core/math/linalg/specific_euler_angles.odin +++ b/core/math/linalg/specific_euler_angles.odin @@ -2,8 +2,203 @@ package linalg import "core:math" +Euler_Angle_Order :: enum { + XYZ, + YXZ, + XZX, + XYX, + YXY, + YZY, + ZYZ, + ZXZ, + XZY, + YZX, + ZYX, + ZXY, +} -euler_angle_x :: proc(angle_x: Float) -> (m: Matrix4) { +euler_angles_from_matrix4 :: proc(m: Matrix4, order: Euler_Angle_Order) -> (t1, t2, t3: Float) { + switch order { + case .XYZ: t1, t2, t3 = euler_angles_xyz_from_matrix4(m); + case .YXZ: t1, t2, t3 = euler_angles_yxz_from_matrix4(m); + case .XZX: t1, t2, t3 = euler_angles_xzx_from_matrix4(m); + case .XYX: t1, t2, t3 = euler_angles_xyx_from_matrix4(m); + case .YXY: t1, t2, t3 = euler_angles_yxy_from_matrix4(m); + case .YZY: t1, t2, t3 = euler_angles_yzy_from_matrix4(m); + case .ZYZ: t1, t2, t3 = euler_angles_zyz_from_matrix4(m); + case .ZXZ: t1, t2, t3 = euler_angles_zxz_from_matrix4(m); + case .XZY: t1, t2, t3 = euler_angles_xzy_from_matrix4(m); + case .YZX: t1, t2, t3 = euler_angles_yzx_from_matrix4(m); + case .ZYX: t1, t2, t3 = euler_angles_zyx_from_matrix4(m); + case .ZXY: t1, t2, t3 = euler_angles_zxy_from_matrix4(m); + } + return; +} +euler_angles_from_quaternion :: proc(m: Quaternion, order: Euler_Angle_Order) -> (t1, t2, t3: Float) { + switch order { + case .XYZ: t1, t2, t3 = euler_angles_xyz_from_quaternion(m); + case .YXZ: t1, t2, t3 = euler_angles_yxz_from_quaternion(m); + case .XZX: t1, t2, t3 = euler_angles_xzx_from_quaternion(m); + case .XYX: t1, t2, t3 = euler_angles_xyx_from_quaternion(m); + case .YXY: t1, t2, t3 = euler_angles_yxy_from_quaternion(m); + case .YZY: t1, t2, t3 = euler_angles_yzy_from_quaternion(m); + case .ZYZ: t1, t2, t3 = euler_angles_zyz_from_quaternion(m); + case .ZXZ: t1, t2, t3 = euler_angles_zxz_from_quaternion(m); + case .XZY: t1, t2, t3 = euler_angles_xzy_from_quaternion(m); + case .YZX: t1, t2, t3 = euler_angles_yzx_from_quaternion(m); + case .ZYX: t1, t2, t3 = euler_angles_zyx_from_quaternion(m); + case .ZXY: t1, t2, t3 = euler_angles_zxy_from_quaternion(m); + } + return; +} + +matrix4_from_euler_angles :: proc(t1, t2, t3: Float, order: Euler_Angle_Order) -> Matrix4 { + X :: matrix4_from_euler_angle_x; + Y :: matrix4_from_euler_angle_y; + Z :: matrix4_from_euler_angle_z; + + m1, m2, m3: Matrix4; + + switch order { + case .XYZ: m1, m2, m3 = X(t1), Y(t2), Z(t3); + case .YXZ: m1, m2, m3 = Y(t1), X(t2), Z(t3); + case .XZX: m1, m2, m3 = X(t1), Z(t2), X(t3); + case .XYX: m1, m2, m3 = X(t1), Y(t2), X(t3); + case .YXY: m1, m2, m3 = Y(t1), X(t2), Y(t3); + case .YZY: m1, m2, m3 = Y(t1), Z(t2), Y(t3); + case .ZYZ: m1, m2, m3 = Z(t1), Y(t2), Z(t3); + case .ZXZ: m1, m2, m3 = Z(t1), X(t2), Z(t3); + case .XZY: m1, m2, m3 = X(t1), Z(t2), Y(t3); + case .YZX: m1, m2, m3 = Y(t1), Z(t2), X(t3); + case .ZYX: m1, m2, m3 = Z(t1), Y(t2), X(t3); + case .ZXY: m1, m2, m3 = Z(t1), X(t2), Y(t3); + } + + return mul(m1, mul(m2, m3)); +} + +quaternion_from_euler_angles :: proc(t1, t2, t3: Float, order: Euler_Angle_Order) -> Quaternion { + X :: quaternion_from_euler_angle_x; + Y :: quaternion_from_euler_angle_y; + Z :: quaternion_from_euler_angle_z; + + m1, m2, m3: Quaternion; + + switch order { + case .XYZ: m1, m2, m3 = X(t1), Y(t2), Z(t3); + case .YXZ: m1, m2, m3 = Y(t1), X(t2), Z(t3); + case .XZX: m1, m2, m3 = X(t1), Z(t2), X(t3); + case .XYX: m1, m2, m3 = X(t1), Y(t2), X(t3); + case .YXY: m1, m2, m3 = Y(t1), X(t2), Y(t3); + case .YZY: m1, m2, m3 = Y(t1), Z(t2), Y(t3); + case .ZYZ: m1, m2, m3 = Z(t1), Y(t2), Z(t3); + case .ZXZ: m1, m2, m3 = Z(t1), X(t2), Z(t3); + case .XZY: m1, m2, m3 = X(t1), Z(t2), Y(t3); + case .YZX: m1, m2, m3 = Y(t1), Z(t2), X(t3); + case .ZYX: m1, m2, m3 = Z(t1), Y(t2), X(t3); + case .ZXY: m1, m2, m3 = Z(t1), X(t2), Y(t3); + } + + return m1 * (m2 * m3); +} + + +// Quaternions + +quaternion_from_euler_angle_x :: proc(angle_x: Float) -> (q: Quaternion) { + return quaternion_angle_axis(angle_x, Vector3{1, 0, 0}); +} +quaternion_from_euler_angle_y :: proc(angle_y: Float) -> (q: Quaternion) { + return quaternion_angle_axis(angle_y, Vector3{0, 1, 0}); +} +quaternion_from_euler_angle_z :: proc(angle_z: Float) -> (q: Quaternion) { + return quaternion_angle_axis(angle_z, Vector3{0, 0, 1}); +} + +quaternion_from_pitch_yaw_roll :: proc(pitch, yaw, roll: Float) -> Quaternion { + a, b, c := pitch, yaw, roll; + + ca, sa := math.cos(a*0.5), math.sin(a*0.5); + cb, sb := math.cos(b*0.5), math.sin(b*0.5); + cc, sc := math.cos(c*0.5), math.sin(c*0.5); + + q: Quaternion; + q.x = sa*cb*cc - ca*sb*sc; + q.y = ca*sb*cc + sa*cb*sc; + q.z = ca*cb*sc - sa*sb*cc; + q.w = ca*cb*cc + sa*sb*sc; + return q; +} + +roll_from_quaternion :: proc(q: Quaternion) -> Float { + return math.atan2(2 * q.x*q.y + q.w*q.z, q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z); +} + +pitch_from_quaternion :: proc(q: Quaternion) -> Float { + y := 2 * (q.y*q.z + q.w*q.w); + x := q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z; + + if abs(x) <= FLOAT_EPSILON && abs(y) <= FLOAT_EPSILON { + return 2 * math.atan2(q.x, q.w); + } + + return math.atan2(y, x); +} + +yaw_from_quaternion :: proc(q: Quaternion) -> Float { + return math.asin(clamp(-2 * (q.x*q.z - q.w*q.y), -1, 1)); +} + + +pitch_yaw_roll_from_quaternion :: proc(q: Quaternion) -> (pitch, yaw, roll: Float) { + pitch = pitch_from_quaternion(q); + yaw = yaw_from_quaternion(q); + roll = roll_from_quaternion(q); + return; +} + +euler_angles_xyz_from_quaternion :: proc(q: Quaternion) -> (t1, t2, t3: Float) { + return euler_angles_xyz_from_matrix4(matrix4_from_quaternion(q)); +} +euler_angles_yxz_from_quaternion :: proc(q: Quaternion) -> (t1, t2, t3: Float) { + return euler_angles_yxz_from_matrix4(matrix4_from_quaternion(q)); +} +euler_angles_xzx_from_quaternion :: proc(q: Quaternion) -> (t1, t2, t3: Float) { + return euler_angles_xzx_from_matrix4(matrix4_from_quaternion(q)); +} +euler_angles_xyx_from_quaternion :: proc(q: Quaternion) -> (t1, t2, t3: Float) { + return euler_angles_xyx_from_matrix4(matrix4_from_quaternion(q)); +} +euler_angles_yxy_from_quaternion :: proc(q: Quaternion) -> (t1, t2, t3: Float) { + return euler_angles_yxy_from_matrix4(matrix4_from_quaternion(q)); +} +euler_angles_yzy_from_quaternion :: proc(q: Quaternion) -> (t1, t2, t3: Float) { + return euler_angles_yzy_from_matrix4(matrix4_from_quaternion(q)); +} +euler_angles_zyz_from_quaternion :: proc(q: Quaternion) -> (t1, t2, t3: Float) { + return euler_angles_zyz_from_matrix4(matrix4_from_quaternion(q)); +} +euler_angles_zxz_from_quaternion :: proc(q: Quaternion) -> (t1, t2, t3: Float) { + return euler_angles_zxz_from_matrix4(matrix4_from_quaternion(q)); +} +euler_angles_xzy_from_quaternion :: proc(q: Quaternion) -> (t1, t2, t3: Float) { + return euler_angles_xzy_from_matrix4(matrix4_from_quaternion(q)); +} +euler_angles_yzx_from_quaternion :: proc(q: Quaternion) -> (t1, t2, t3: Float) { + return euler_angles_yzx_from_matrix4(matrix4_from_quaternion(q)); +} +euler_angles_zyx_from_quaternion :: proc(q: Quaternion) -> (t1, t2, t3: Float) { + return euler_angles_zyx_from_matrix4(matrix4_from_quaternion(q)); +} +euler_angles_zxy_from_quaternion :: proc(q: Quaternion) -> (t1, t2, t3: Float) { + return euler_angles_zxy_from_matrix4(matrix4_from_quaternion(q)); +} + + +// Matrices + + +matrix4_from_euler_angle_x :: proc(angle_x: Float) -> (m: Matrix4) { cos_x, sin_x := math.cos(angle_x), math.sin(angle_x); m[0][0] = 1; m[1][1] = +cos_x; @@ -13,7 +208,7 @@ euler_angle_x :: proc(angle_x: Float) -> (m: Matrix4) { m[3][3] = 1; return; } -euler_angle_y :: proc(angle_y: Float) -> (m: Matrix4) { +matrix4_from_euler_angle_y :: proc(angle_y: Float) -> (m: Matrix4) { cos_y, sin_y := math.cos(angle_y), math.sin(angle_y); m[0][0] = +cos_y; m[2][0] = -sin_y; @@ -23,7 +218,7 @@ euler_angle_y :: proc(angle_y: Float) -> (m: Matrix4) { m[3][3] = 1; return; } -euler_angle_z :: proc(angle_z: Float) -> (m: Matrix4) { +matrix4_from_euler_angle_z :: proc(angle_z: Float) -> (m: Matrix4) { cos_z, sin_z := math.cos(angle_z), math.sin(angle_z); m[0][0] = +cos_z; m[1][0] = +sin_z; @@ -35,7 +230,7 @@ euler_angle_z :: proc(angle_z: Float) -> (m: Matrix4) { } -derived_euler_angle_x :: proc(angle_x: Float, angular_velocity_x: Float) -> (m: Matrix4) { +matrix4_from_derived_euler_angle_x :: proc(angle_x: Float, angular_velocity_x: Float) -> (m: Matrix4) { cos_x := math.cos(angle_x) * angular_velocity_x; sin_x := math.sin(angle_x) * angular_velocity_x; m[0][0] = 1; @@ -46,7 +241,7 @@ derived_euler_angle_x :: proc(angle_x: Float, angular_velocity_x: Float) -> (m: m[3][3] = 1; return; } -derived_euler_angle_y :: proc(angle_y: Float, angular_velocity_y: Float) -> (m: Matrix4) { +matrix4_from_derived_euler_angle_y :: proc(angle_y: Float, angular_velocity_y: Float) -> (m: Matrix4) { cos_y := math.cos(angle_y) * angular_velocity_y; sin_y := math.sin(angle_y) * angular_velocity_y; m[0][0] = +cos_y; @@ -57,7 +252,7 @@ derived_euler_angle_y :: proc(angle_y: Float, angular_velocity_y: Float) -> (m: m[3][3] = 1; return; } -derived_euler_angle_z :: proc(angle_z: Float, angular_velocity_z: Float) -> (m: Matrix4) { +matrix4_from_derived_euler_angle_z :: proc(angle_z: Float, angular_velocity_z: Float) -> (m: Matrix4) { cos_z := math.cos(angle_z) * angular_velocity_z; sin_z := math.sin(angle_z) * angular_velocity_z; m[0][0] = +cos_z; @@ -70,7 +265,7 @@ derived_euler_angle_z :: proc(angle_z: Float, angular_velocity_z: Float) -> (m: } -euler_angle_xy :: proc(angle_x, angle_y: Float) -> (m: Matrix4) { +matrix4_from_euler_angle_xy :: proc(angle_x, angle_y: Float) -> (m: Matrix4) { cos_x, sin_x := math.cos(angle_x), math.sin(angle_x); cos_y, sin_y := math.cos(angle_y), math.sin(angle_y); m[0][0] = cos_y; @@ -86,7 +281,7 @@ euler_angle_xy :: proc(angle_x, angle_y: Float) -> (m: Matrix4) { } -euler_angle_yx :: proc(angle_y, angle_x: Float) -> (m: Matrix4) { +matrix4_from_euler_angle_yx :: proc(angle_y, angle_x: Float) -> (m: Matrix4) { cos_x, sin_x := math.cos(angle_x), math.sin(angle_x); cos_y, sin_y := math.cos(angle_y), math.sin(angle_y); m[0][0] = cos_y; @@ -101,21 +296,21 @@ euler_angle_yx :: proc(angle_y, angle_x: Float) -> (m: Matrix4) { return; } -euler_angle_xz :: proc(angle_x, angle_z: Float) -> (m: Matrix4) { - return mul(euler_angle_x(angle_x), euler_angle_z(angle_z)); +matrix4_from_euler_angle_xz :: proc(angle_x, angle_z: Float) -> (m: Matrix4) { + return mul(matrix4_from_euler_angle_x(angle_x), matrix4_from_euler_angle_z(angle_z)); } -euler_angle_zx :: proc(angle_z, angle_x: Float) -> (m: Matrix4) { - return mul(euler_angle_z(angle_z), euler_angle_x(angle_x)); +matrix4_from_euler_angle_zx :: proc(angle_z, angle_x: Float) -> (m: Matrix4) { + return mul(matrix4_from_euler_angle_z(angle_z), matrix4_from_euler_angle_x(angle_x)); } -euler_angle_yz :: proc(angle_y, angle_z: Float) -> (m: Matrix4) { - return mul(euler_angle_y(angle_y), euler_angle_z(angle_z)); +matrix4_from_euler_angle_yz :: proc(angle_y, angle_z: Float) -> (m: Matrix4) { + return mul(matrix4_from_euler_angle_y(angle_y), matrix4_from_euler_angle_z(angle_z)); } -euler_angle_zy :: proc(angle_z, angle_y: Float) -> (m: Matrix4) { - return mul(euler_angle_z(angle_z), euler_angle_y(angle_y)); +matrix4_from_euler_angle_zy :: proc(angle_z, angle_y: Float) -> (m: Matrix4) { + return mul(matrix4_from_euler_angle_z(angle_z), matrix4_from_euler_angle_y(angle_y)); } -euler_angle_xyz :: proc(t1, t2, t3: Float) -> (m: Matrix4) { +matrix4_from_euler_angle_xyz :: proc(t1, t2, t3: Float) -> (m: Matrix4) { c1 := math.cos(-t1); c2 := math.cos(-t2); c3 := math.cos(-t3); @@ -142,7 +337,7 @@ euler_angle_xyz :: proc(t1, t2, t3: Float) -> (m: Matrix4) { return; } -euler_angle_yxz :: proc(yaw, pitch, roll: Float) -> (m: Matrix4) { +matrix4_from_euler_angle_yxz :: proc(yaw, pitch, roll: Float) -> (m: Matrix4) { ch := math.cos(yaw); sh := math.sin(yaw); cp := math.cos(pitch); @@ -169,7 +364,7 @@ euler_angle_yxz :: proc(yaw, pitch, roll: Float) -> (m: Matrix4) { return; } -euler_angle_xzx :: proc(t1, t2, t3: Float) -> (m: Matrix4) { +matrix4_from_euler_angle_xzx :: proc(t1, t2, t3: Float) -> (m: Matrix4) { c1 := math.cos(t1); s1 := math.sin(t1); c2 := math.cos(t2); @@ -196,7 +391,7 @@ euler_angle_xzx :: proc(t1, t2, t3: Float) -> (m: Matrix4) { return; } -euler_angle_xyx :: proc(t1, t2, t3: Float) -> (m: Matrix4) { +matrix4_from_euler_angle_xyx :: proc(t1, t2, t3: Float) -> (m: Matrix4) { c1 := math.cos(t1); s1 := math.sin(t1); c2 := math.cos(t2); @@ -223,7 +418,7 @@ euler_angle_xyx :: proc(t1, t2, t3: Float) -> (m: Matrix4) { return; } -euler_angle_yxy :: proc(t1, t2, t3: Float) -> (m: Matrix4) { +matrix4_from_euler_angle_yxy :: proc(t1, t2, t3: Float) -> (m: Matrix4) { c1 := math.cos(t1); s1 := math.sin(t1); c2 := math.cos(t2); @@ -250,7 +445,7 @@ euler_angle_yxy :: proc(t1, t2, t3: Float) -> (m: Matrix4) { return; } -euler_angle_yzy :: proc(t1, t2, t3: Float) -> (m: Matrix4) { +matrix4_from_euler_angle_yzy :: proc(t1, t2, t3: Float) -> (m: Matrix4) { c1 := math.cos(t1); s1 := math.sin(t1); c2 := math.cos(t2); @@ -277,7 +472,7 @@ euler_angle_yzy :: proc(t1, t2, t3: Float) -> (m: Matrix4) { return; } -euler_angle_zyz :: proc(t1, t2, t3: Float) -> (m: Matrix4) { +matrix4_from_euler_angle_zyz :: proc(t1, t2, t3: Float) -> (m: Matrix4) { c1 := math.cos(t1); s1 := math.sin(t1); c2 := math.cos(t2); @@ -304,7 +499,7 @@ euler_angle_zyz :: proc(t1, t2, t3: Float) -> (m: Matrix4) { return; } -euler_angle_zxz :: proc(t1, t2, t3: Float) -> (m: Matrix4) { +matrix4_from_euler_angle_zxz :: proc(t1, t2, t3: Float) -> (m: Matrix4) { c1 := math.cos(t1); s1 := math.sin(t1); c2 := math.cos(t2); @@ -332,7 +527,7 @@ euler_angle_zxz :: proc(t1, t2, t3: Float) -> (m: Matrix4) { } -euler_angle_xzy :: proc(t1, t2, t3: Float) -> (m: Matrix4) { +matrix4_from_euler_angle_xzy :: proc(t1, t2, t3: Float) -> (m: Matrix4) { c1 := math.cos(t1); s1 := math.sin(t1); c2 := math.cos(t2); @@ -359,7 +554,7 @@ euler_angle_xzy :: proc(t1, t2, t3: Float) -> (m: Matrix4) { return; } -euler_angle_yzx :: proc(t1, t2, t3: Float) -> (m: Matrix4) { +matrix4_from_euler_angle_yzx :: proc(t1, t2, t3: Float) -> (m: Matrix4) { c1 := math.cos(t1); s1 := math.sin(t1); c2 := math.cos(t2); @@ -386,7 +581,7 @@ euler_angle_yzx :: proc(t1, t2, t3: Float) -> (m: Matrix4) { return; } -euler_angle_zyx :: proc(t1, t2, t3: Float) -> (m: Matrix4) { +matrix4_from_euler_angles_zyx :: proc(t1, t2, t3: Float) -> (m: Matrix4) { c1 := math.cos(t1); s1 := math.sin(t1); c2 := math.cos(t2); @@ -413,7 +608,7 @@ euler_angle_zyx :: proc(t1, t2, t3: Float) -> (m: Matrix4) { return; } -euler_angle_zxy :: proc(t1, t2, t3: Float) -> (m: Matrix4) { +matrix4_from_euler_angles_zxy :: proc(t1, t2, t3: Float) -> (m: Matrix4) { c1 := math.cos(t1); s1 := math.sin(t1); c2 := math.cos(t2); @@ -441,7 +636,7 @@ euler_angle_zxy :: proc(t1, t2, t3: Float) -> (m: Matrix4) { } -yaw_pitch_roll :: proc(yaw, pitch, roll: Float) -> (m: Matrix4) { +matrix4_from_yaw_pitch_roll :: proc(yaw, pitch, roll: Float) -> (m: Matrix4) { ch := math.cos(yaw); sh := math.sin(yaw); cp := math.cos(pitch); @@ -468,7 +663,7 @@ yaw_pitch_roll :: proc(yaw, pitch, roll: Float) -> (m: Matrix4) { return m; } -extract_euler_angle_xyz :: proc(m: Matrix4) -> (t1, t2, t3: Float) { +euler_angles_xyz_from_matrix4 :: proc(m: Matrix4) -> (t1, t2, t3: Float) { T1 := math.atan2(m[2][1], m[2][2]); C2 := math.sqrt(m[0][0]*m[0][0] + m[1][0]*m[1][0]); T2 := math.atan2(-m[2][0], C2); @@ -481,7 +676,7 @@ extract_euler_angle_xyz :: proc(m: Matrix4) -> (t1, t2, t3: Float) { return; } -extract_euler_angle_yxz :: proc(m: Matrix4) -> (t1, t2, t3: Float) { +euler_angles_yxz_from_matrix4 :: proc(m: Matrix4) -> (t1, t2, t3: Float) { T1 := math.atan2(m[2][0], m[2][2]); C2 := math.sqrt(m[0][1]*m[0][1] + m[1][1]*m[1][1]); T2 := math.atan2(-m[2][1], C2); @@ -494,7 +689,7 @@ extract_euler_angle_yxz :: proc(m: Matrix4) -> (t1, t2, t3: Float) { return; } -extract_euler_angle_xzx :: proc(m: Matrix4) -> (t1, t2, t3: Float) { +euler_angles_xzx_from_matrix4 :: proc(m: Matrix4) -> (t1, t2, t3: Float) { T1 := math.atan2(m[0][2], m[0][1]); S2 := math.sqrt(m[1][0]*m[1][0] + m[2][0]*m[2][0]); T2 := math.atan2(S2, m[0][0]); @@ -507,7 +702,7 @@ extract_euler_angle_xzx :: proc(m: Matrix4) -> (t1, t2, t3: Float) { return; } -extract_euler_angle_xyx :: proc(m: Matrix4) -> (t1, t2, t3: Float) { +euler_angles_xyx_from_matrix4 :: proc(m: Matrix4) -> (t1, t2, t3: Float) { T1 := math.atan2(m[0][1], -m[0][2]); S2 := math.sqrt(m[1][0]*m[1][0] + m[2][0]*m[2][0]); T2 := math.atan2(S2, m[0][0]); @@ -520,7 +715,7 @@ extract_euler_angle_xyx :: proc(m: Matrix4) -> (t1, t2, t3: Float) { return; } -extract_euler_angle_yxy :: proc(m: Matrix4) -> (t1, t2, t3: Float) { +euler_angles_yxy_from_matrix4 :: proc(m: Matrix4) -> (t1, t2, t3: Float) { T1 := math.atan2(m[1][0], m[1][2]); S2 := math.sqrt(m[0][1]*m[0][1] + m[2][1]*m[2][1]); T2 := math.atan2(S2, m[1][1]); @@ -533,7 +728,7 @@ extract_euler_angle_yxy :: proc(m: Matrix4) -> (t1, t2, t3: Float) { return; } -extract_euler_angle_yzy :: proc(m: Matrix4) -> (t1, t2, t3: Float) { +euler_angles_yzy_from_matrix4 :: proc(m: Matrix4) -> (t1, t2, t3: Float) { T1 := math.atan2(m[1][2], -m[1][0]); S2 := math.sqrt(m[0][1]*m[0][1] + m[2][1]*m[2][1]); T2 := math.atan2(S2, m[1][1]); @@ -545,7 +740,7 @@ extract_euler_angle_yzy :: proc(m: Matrix4) -> (t1, t2, t3: Float) { t3 = T3; return; } -extract_euler_angle_zyz :: proc(m: Matrix4) -> (t1, t2, t3: Float) { +euler_angles_zyz_from_matrix4 :: proc(m: Matrix4) -> (t1, t2, t3: Float) { T1 := math.atan2(m[2][1], m[2][0]); S2 := math.sqrt(m[0][2]*m[0][2] + m[1][2]*m[1][2]); T2 := math.atan2(S2, m[2][2]); @@ -558,7 +753,7 @@ extract_euler_angle_zyz :: proc(m: Matrix4) -> (t1, t2, t3: Float) { return; } -extract_euler_angle_zxz :: proc(m: Matrix4) -> (t1, t2, t3: Float) { +euler_angles_zxz_from_matrix4 :: proc(m: Matrix4) -> (t1, t2, t3: Float) { T1 := math.atan2(m[2][0], -m[2][1]); S2 := math.sqrt(m[0][2]*m[0][2] + m[1][2]*m[1][2]); T2 := math.atan2(S2, m[2][2]); @@ -571,7 +766,7 @@ extract_euler_angle_zxz :: proc(m: Matrix4) -> (t1, t2, t3: Float) { return; } -extract_euler_angle_xzy :: proc(m: Matrix4) -> (t1, t2, t3: Float) { +euler_angles_xzy_from_matrix4 :: proc(m: Matrix4) -> (t1, t2, t3: Float) { T1 := math.atan2(m[1][2], m[1][1]); C2 := math.sqrt(m[0][0]*m[0][0] + m[2][0]*m[2][0]); T2 := math.atan2(-m[1][0], C2); @@ -584,7 +779,7 @@ extract_euler_angle_xzy :: proc(m: Matrix4) -> (t1, t2, t3: Float) { return; } -extract_euler_angle_yzx :: proc(m: Matrix4) -> (t1, t2, t3: Float) { +euler_angles_yzx_from_matrix4 :: proc(m: Matrix4) -> (t1, t2, t3: Float) { T1 := math.atan2(-m[0][2], m[0][0]); C2 := math.sqrt(m[1][1]*m[1][1] + m[2][1]*m[2][1]); T2 := math.atan2(m[0][1], C2); @@ -597,7 +792,7 @@ extract_euler_angle_yzx :: proc(m: Matrix4) -> (t1, t2, t3: Float) { return; } -extract_euler_angle_zyx :: proc(m: Matrix4) -> (t1, t2, t3: Float) { +euler_angles_zyx_from_matrix4 :: proc(m: Matrix4) -> (t1, t2, t3: Float) { T1 := math.atan2(m[0][1], m[0][0]); C2 := math.sqrt(m[1][2]*m[1][2] + m[2][2]*m[2][2]); T2 := math.atan2(-m[0][2], C2); @@ -610,7 +805,7 @@ extract_euler_angle_zyx :: proc(m: Matrix4) -> (t1, t2, t3: Float) { return; } -extract_euler_angle_zxy :: proc(m: Matrix4) -> (t1, t2, t3: Float) { +euler_angles_zxy_from_matrix4 :: proc(m: Matrix4) -> (t1, t2, t3: Float) { T1 := math.atan2(-m[1][0], m[1][1]); C2 := math.sqrt(m[0][2]*m[0][2] + m[2][2]*m[2][2]); T2 := math.atan2(m[1][2], C2);