diff --git a/core/math/linalg/specific_euler_angles.odin b/core/math/linalg/specific_euler_angles.odin index 759fd6201..53b33cfe0 100644 --- a/core/math/linalg/specific_euler_angles.odin +++ b/core/math/linalg/specific_euler_angles.odin @@ -18,18 +18,14 @@ Euler_Angle_Order :: enum { ZYZ, } -euler_angles_from_matrix4 :: proc{euler_angles_from_matrix4_f32, euler_angles_from_matrix4_f64}; -euler_angles_from_quaternion :: proc{euler_angles_from_quaternion_f32, euler_angles_from_quaternion_f64}; -matrix4_from_euler_angles :: proc{matrix4_from_euler_angles_f32, matrix4_from_euler_angles_f64}; + quaternion_from_euler_angles :: proc{quaternion_from_euler_angles_f32, quaternion_from_euler_angles_f64}; quaternion_from_euler_angle_x :: proc{quaternion_from_euler_angle_x_f32, quaternion_from_euler_angle_x_f64}; quaternion_from_euler_angle_y :: proc{quaternion_from_euler_angle_y_f32, quaternion_from_euler_angle_y_f64}; quaternion_from_euler_angle_z :: proc{quaternion_from_euler_angle_z_f32, quaternion_from_euler_angle_z_f64}; quaternion_from_pitch_yaw_roll :: proc{quaternion_from_pitch_yaw_roll_f32, quaternion_from_pitch_yaw_roll_f64}; -roll_from_quaternion :: proc{roll_from_quaternion_f32, roll_from_quaternion_f64}; -pitch_from_quaternion :: proc{pitch_from_quaternion_f32, pitch_from_quaternion_f64}; -yaw_from_quaternion :: proc{yaw_from_quaternion_f32, yaw_from_quaternion_f64}; -pitch_yaw_roll_from_quaternion :: proc{pitch_yaw_roll_from_quaternion_f32, pitch_yaw_roll_from_quaternion_f64}; + +euler_angles_from_quaternion :: proc{euler_angles_from_quaternion_f32, euler_angles_from_quaternion_f64}; euler_angles_xyz_from_quaternion :: proc{euler_angles_xyz_from_quaternion_f32, euler_angles_xyz_from_quaternion_f64}; euler_angles_yxz_from_quaternion :: proc{euler_angles_yxz_from_quaternion_f32, euler_angles_yxz_from_quaternion_f64}; euler_angles_xzx_from_quaternion :: proc{euler_angles_xzx_from_quaternion_f32, euler_angles_xzx_from_quaternion_f64}; @@ -42,6 +38,54 @@ euler_angles_xzy_from_quaternion :: proc{euler_angles_xzy_from_quaternion_f32, euler_angles_yzx_from_quaternion :: proc{euler_angles_yzx_from_quaternion_f32, euler_angles_yzx_from_quaternion_f64}; euler_angles_zyx_from_quaternion :: proc{euler_angles_zyx_from_quaternion_f32, euler_angles_zyx_from_quaternion_f64}; euler_angles_zxy_from_quaternion :: proc{euler_angles_zxy_from_quaternion_f32, euler_angles_zxy_from_quaternion_f64}; + +roll_from_quaternion :: proc{roll_from_quaternion_f32, roll_from_quaternion_f64}; +pitch_from_quaternion :: proc{pitch_from_quaternion_f32, pitch_from_quaternion_f64}; +yaw_from_quaternion :: proc{yaw_from_quaternion_f32, yaw_from_quaternion_f64}; +pitch_yaw_roll_from_quaternion :: proc{pitch_yaw_roll_from_quaternion_f32, pitch_yaw_roll_from_quaternion_f64}; + +matrix3_from_euler_angles :: proc{matrix3_from_euler_angles_f32, matrix3_from_euler_angles_f64}; +matrix3_from_euler_angle_x :: proc{matrix3_from_euler_angle_x_f32, matrix3_from_euler_angle_x_f64}; +matrix3_from_euler_angle_y :: proc{matrix3_from_euler_angle_y_f32, matrix3_from_euler_angle_y_f64}; +matrix3_from_euler_angle_z :: proc{matrix3_from_euler_angle_z_f32, matrix3_from_euler_angle_z_f64}; +matrix3_from_derived_euler_angle_x :: proc{matrix3_from_derived_euler_angle_x_f32, matrix3_from_derived_euler_angle_x_f64}; +matrix3_from_derived_euler_angle_y :: proc{matrix3_from_derived_euler_angle_y_f32, matrix3_from_derived_euler_angle_y_f64}; +matrix3_from_derived_euler_angle_z :: proc{matrix3_from_derived_euler_angle_z_f32, matrix3_from_derived_euler_angle_z_f64}; +matrix3_from_euler_angles_xy :: proc{matrix3_from_euler_angles_xy_f32, matrix3_from_euler_angles_xy_f64}; +matrix3_from_euler_angles_yx :: proc{matrix3_from_euler_angles_yx_f32, matrix3_from_euler_angles_yx_f64}; +matrix3_from_euler_angles_xz :: proc{matrix3_from_euler_angles_xz_f32, matrix3_from_euler_angles_xz_f64}; +matrix3_from_euler_angles_zx :: proc{matrix3_from_euler_angles_zx_f32, matrix3_from_euler_angles_zx_f64}; +matrix3_from_euler_angles_yz :: proc{matrix3_from_euler_angles_yz_f32, matrix3_from_euler_angles_yz_f64}; +matrix3_from_euler_angles_zy :: proc{matrix3_from_euler_angles_zy_f32, matrix3_from_euler_angles_zy_f64}; +matrix3_from_euler_angles_xyz :: proc{matrix3_from_euler_angles_xyz_f32, matrix3_from_euler_angles_xyz_f64}; +matrix3_from_euler_angles_yxz :: proc{matrix3_from_euler_angles_yxz_f32, matrix3_from_euler_angles_yxz_f64}; +matrix3_from_euler_angles_xzx :: proc{matrix3_from_euler_angles_xzx_f32, matrix3_from_euler_angles_xzx_f64}; +matrix3_from_euler_angles_xyx :: proc{matrix3_from_euler_angles_xyx_f32, matrix3_from_euler_angles_xyx_f64}; +matrix3_from_euler_angles_yxy :: proc{matrix3_from_euler_angles_yxy_f32, matrix3_from_euler_angles_yxy_f64}; +matrix3_from_euler_angles_yzy :: proc{matrix3_from_euler_angles_yzy_f32, matrix3_from_euler_angles_yzy_f64}; +matrix3_from_euler_angles_zyz :: proc{matrix3_from_euler_angles_zyz_f32, matrix3_from_euler_angles_zyz_f64}; +matrix3_from_euler_angles_zxz :: proc{matrix3_from_euler_angles_zxz_f32, matrix3_from_euler_angles_zxz_f64}; +matrix3_from_euler_angles_xzy :: proc{matrix3_from_euler_angles_xzy_f32, matrix3_from_euler_angles_xzy_f64}; +matrix3_from_euler_angles_yzx :: proc{matrix3_from_euler_angles_yzx_f32, matrix3_from_euler_angles_yzx_f64}; +matrix3_from_euler_angles_zyx :: proc{matrix3_from_euler_angles_zyx_f32, matrix3_from_euler_angles_zyx_f64}; +matrix3_from_euler_angles_zxy :: proc{matrix3_from_euler_angles_zxy_f32, matrix3_from_euler_angles_zxy_f64}; +matrix3_from_yaw_pitch_roll :: proc{matrix3_from_yaw_pitch_roll_f32, matrix3_from_yaw_pitch_roll_f64}; + +euler_angles_from_matrix3 :: proc{euler_angles_from_matrix3_f32, euler_angles_from_matrix3_f64}; +euler_angles_xyz_from_matrix3 :: proc{euler_angles_xyz_from_matrix3_f32, euler_angles_xyz_from_matrix3_f64}; +euler_angles_yxz_from_matrix3 :: proc{euler_angles_yxz_from_matrix3_f32, euler_angles_yxz_from_matrix3_f64}; +euler_angles_xzx_from_matrix3 :: proc{euler_angles_xzx_from_matrix3_f32, euler_angles_xzx_from_matrix3_f64}; +euler_angles_xyx_from_matrix3 :: proc{euler_angles_xyx_from_matrix3_f32, euler_angles_xyx_from_matrix3_f64}; +euler_angles_yxy_from_matrix3 :: proc{euler_angles_yxy_from_matrix3_f32, euler_angles_yxy_from_matrix3_f64}; +euler_angles_yzy_from_matrix3 :: proc{euler_angles_yzy_from_matrix3_f32, euler_angles_yzy_from_matrix3_f64}; +euler_angles_zyz_from_matrix3 :: proc{euler_angles_zyz_from_matrix3_f32, euler_angles_zyz_from_matrix3_f64}; +euler_angles_zxz_from_matrix3 :: proc{euler_angles_zxz_from_matrix3_f32, euler_angles_zxz_from_matrix3_f64}; +euler_angles_xzy_from_matrix3 :: proc{euler_angles_xzy_from_matrix3_f32, euler_angles_xzy_from_matrix3_f64}; +euler_angles_yzx_from_matrix3 :: proc{euler_angles_yzx_from_matrix3_f32, euler_angles_yzx_from_matrix3_f64}; +euler_angles_zyx_from_matrix3 :: proc{euler_angles_zyx_from_matrix3_f32, euler_angles_zyx_from_matrix3_f64}; +euler_angles_zxy_from_matrix3 :: proc{euler_angles_zxy_from_matrix3_f32, euler_angles_zxy_from_matrix3_f64}; + +matrix4_from_euler_angles :: proc{matrix4_from_euler_angles_f32, matrix4_from_euler_angles_f64}; matrix4_from_euler_angle_x :: proc{matrix4_from_euler_angle_x_f32, matrix4_from_euler_angle_x_f64}; matrix4_from_euler_angle_y :: proc{matrix4_from_euler_angle_y_f32, matrix4_from_euler_angle_y_f64}; matrix4_from_euler_angle_z :: proc{matrix4_from_euler_angle_z_f32, matrix4_from_euler_angle_z_f64}; @@ -67,6 +111,8 @@ matrix4_from_euler_angles_yzx :: proc{matrix4_from_euler_angles_yzx_f32, ma matrix4_from_euler_angles_zyx :: proc{matrix4_from_euler_angles_zyx_f32, matrix4_from_euler_angles_zyx_f64}; matrix4_from_euler_angles_zxy :: proc{matrix4_from_euler_angles_zxy_f32, matrix4_from_euler_angles_zxy_f64}; matrix4_from_yaw_pitch_roll :: proc{matrix4_from_yaw_pitch_roll_f32, matrix4_from_yaw_pitch_roll_f64}; + +euler_angles_from_matrix4 :: proc{euler_angles_from_matrix4_f32, euler_angles_from_matrix4_f64}; euler_angles_xyz_from_matrix4 :: proc{euler_angles_xyz_from_matrix4_f32, euler_angles_xyz_from_matrix4_f64}; euler_angles_yxz_from_matrix4 :: proc{euler_angles_yxz_from_matrix4_f32, euler_angles_yxz_from_matrix4_f64}; euler_angles_xzx_from_matrix4 :: proc{euler_angles_xzx_from_matrix4_f32, euler_angles_xzx_from_matrix4_f64}; diff --git a/core/math/linalg/specific_euler_angles_f32.odin b/core/math/linalg/specific_euler_angles_f32.odin index 35f497746..e7069bd81 100644 --- a/core/math/linalg/specific_euler_angles_f32.odin +++ b/core/math/linalg/specific_euler_angles_f32.odin @@ -2,55 +2,89 @@ package linalg import "core:math" +euler_angles_from_matrix3_f32 :: proc(m: Matrix3f32, order: Euler_Angle_Order) -> (t1, t2, t3: f32) { + switch order { + case .XYZ: t1, t2, t3 = euler_angles_xyz_from_matrix3(m); + case .XZY: t1, t2, t3 = euler_angles_xzy_from_matrix3(m); + case .YXZ: t1, t2, t3 = euler_angles_yxz_from_matrix3(m); + case .YZX: t1, t2, t3 = euler_angles_yzx_from_matrix3(m); + case .ZXY: t1, t2, t3 = euler_angles_zxy_from_matrix3(m); + case .ZYX: t1, t2, t3 = euler_angles_zyx_from_matrix3(m); + case .XYX: t1, t2, t3 = euler_angles_xyx_from_matrix3(m); + case .XZX: t1, t2, t3 = euler_angles_xzx_from_matrix3(m); + case .YXY: t1, t2, t3 = euler_angles_yxy_from_matrix3(m); + case .YZY: t1, t2, t3 = euler_angles_yzy_from_matrix3(m); + case .ZXZ: t1, t2, t3 = euler_angles_zxz_from_matrix3(m); + case .ZYZ: t1, t2, t3 = euler_angles_zyz_from_matrix3(m); + } + return; +} euler_angles_from_matrix4_f32 :: proc(m: Matrix4f32, order: Euler_Angle_Order) -> (t1, t2, t3: f32) { switch order { - case .XYZ: t1, t2, t3 = euler_angles_xyz_from_matrix4_f32(m); - case .XZY: t1, t2, t3 = euler_angles_xzy_from_matrix4_f32(m); - case .YXZ: t1, t2, t3 = euler_angles_yxz_from_matrix4_f32(m); - case .YZX: t1, t2, t3 = euler_angles_yzx_from_matrix4_f32(m); - case .ZXY: t1, t2, t3 = euler_angles_zxy_from_matrix4_f32(m); - case .ZYX: t1, t2, t3 = euler_angles_zyx_from_matrix4_f32(m); - case .XYX: t1, t2, t3 = euler_angles_xyx_from_matrix4_f32(m); - case .XZX: t1, t2, t3 = euler_angles_xzx_from_matrix4_f32(m); - case .YXY: t1, t2, t3 = euler_angles_yxy_from_matrix4_f32(m); - case .YZY: t1, t2, t3 = euler_angles_yzy_from_matrix4_f32(m); - case .ZXZ: t1, t2, t3 = euler_angles_zxz_from_matrix4_f32(m); - case .ZYZ: t1, t2, t3 = euler_angles_zyz_from_matrix4_f32(m); + case .XYZ: t1, t2, t3 = euler_angles_xyz_from_matrix4(m); + case .XZY: t1, t2, t3 = euler_angles_xzy_from_matrix4(m); + case .YXZ: t1, t2, t3 = euler_angles_yxz_from_matrix4(m); + case .YZX: t1, t2, t3 = euler_angles_yzx_from_matrix4(m); + case .ZXY: t1, t2, t3 = euler_angles_zxy_from_matrix4(m); + case .ZYX: t1, t2, t3 = euler_angles_zyx_from_matrix4(m); + case .XYX: t1, t2, t3 = euler_angles_xyx_from_matrix4(m); + case .XZX: t1, t2, t3 = euler_angles_xzx_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 .ZXZ: t1, t2, t3 = euler_angles_zxz_from_matrix4(m); + case .ZYZ: t1, t2, t3 = euler_angles_zyz_from_matrix4(m); } return; } euler_angles_from_quaternion_f32 :: proc(m: Quaternionf32, order: Euler_Angle_Order) -> (t1, t2, t3: f32) { switch order { - case .XYZ: t1, t2, t3 = euler_angles_xyz_from_quaternion_f32(m); - case .XZY: t1, t2, t3 = euler_angles_xzy_from_quaternion_f32(m); - case .YXZ: t1, t2, t3 = euler_angles_yxz_from_quaternion_f32(m); - case .YZX: t1, t2, t3 = euler_angles_yzx_from_quaternion_f32(m); - case .ZXY: t1, t2, t3 = euler_angles_zxy_from_quaternion_f32(m); - case .ZYX: t1, t2, t3 = euler_angles_zyx_from_quaternion_f32(m); - case .XYX: t1, t2, t3 = euler_angles_xyx_from_quaternion_f32(m); - case .XZX: t1, t2, t3 = euler_angles_xzx_from_quaternion_f32(m); - case .YXY: t1, t2, t3 = euler_angles_yxy_from_quaternion_f32(m); - case .YZY: t1, t2, t3 = euler_angles_yzy_from_quaternion_f32(m); - case .ZXZ: t1, t2, t3 = euler_angles_zxz_from_quaternion_f32(m); - case .ZYZ: t1, t2, t3 = euler_angles_zyz_from_quaternion_f32(m); + case .XYZ: t1, t2, t3 = euler_angles_xyz_from_quaternion(m); + case .XZY: t1, t2, t3 = euler_angles_xzy_from_quaternion(m); + case .YXZ: t1, t2, t3 = euler_angles_yxz_from_quaternion(m); + case .YZX: t1, t2, t3 = euler_angles_yzx_from_quaternion(m); + case .ZXY: t1, t2, t3 = euler_angles_zxy_from_quaternion(m); + case .ZYX: t1, t2, t3 = euler_angles_zyx_from_quaternion(m); + case .XYX: t1, t2, t3 = euler_angles_xyx_from_quaternion(m); + case .XZX: t1, t2, t3 = euler_angles_xzx_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 .ZXZ: t1, t2, t3 = euler_angles_zxz_from_quaternion(m); + case .ZYZ: t1, t2, t3 = euler_angles_zyz_from_quaternion(m); } return; } +matrix3_from_euler_angles_f32 :: proc(t1, t2, t3: f32, order: Euler_Angle_Order) -> (m: Matrix3f32) { + switch order { + case .XYZ: return matrix3_from_euler_angles_xyz(t1, t2, t3); // m1, m2, m3 = X(t1), Y(t2), Z(t3); + case .XZY: return matrix3_from_euler_angles_xzy(t1, t2, t3); // m1, m2, m3 = X(t1), Z(t2), Y(t3); + case .YXZ: return matrix3_from_euler_angles_yxz(t1, t2, t3); // m1, m2, m3 = Y(t1), X(t2), Z(t3); + case .YZX: return matrix3_from_euler_angles_yzx(t1, t2, t3); // m1, m2, m3 = Y(t1), Z(t2), X(t3); + case .ZXY: return matrix3_from_euler_angles_zxy(t1, t2, t3); // m1, m2, m3 = Z(t1), X(t2), Y(t3); + case .ZYX: return matrix3_from_euler_angles_zyx(t1, t2, t3); // m1, m2, m3 = Z(t1), Y(t2), X(t3); + case .XYX: return matrix3_from_euler_angles_xyx(t1, t2, t3); // m1, m2, m3 = X(t1), Y(t2), X(t3); + case .XZX: return matrix3_from_euler_angles_xzx(t1, t2, t3); // m1, m2, m3 = X(t1), Z(t2), X(t3); + case .YXY: return matrix3_from_euler_angles_yxy(t1, t2, t3); // m1, m2, m3 = Y(t1), X(t2), Y(t3); + case .YZY: return matrix3_from_euler_angles_yzy(t1, t2, t3); // m1, m2, m3 = Y(t1), Z(t2), Y(t3); + case .ZXZ: return matrix3_from_euler_angles_zxz(t1, t2, t3); // m1, m2, m3 = Z(t1), X(t2), Z(t3); + case .ZYZ: return matrix3_from_euler_angles_zyz(t1, t2, t3); // m1, m2, m3 = Z(t1), Y(t2), Z(t3); + } + return; +} matrix4_from_euler_angles_f32 :: proc(t1, t2, t3: f32, order: Euler_Angle_Order) -> (m: Matrix4f32) { switch order { - case .XYZ: return matrix4_from_euler_angles_xyz_f32(t1, t2, t3); // m1, m2, m3 = X(t1), Y(t2), Z(t3); - case .XZY: return matrix4_from_euler_angles_xzy_f32(t1, t2, t3); // m1, m2, m3 = X(t1), Z(t2), Y(t3); - case .YXZ: return matrix4_from_euler_angles_yxz_f32(t1, t2, t3); // m1, m2, m3 = Y(t1), X(t2), Z(t3); - case .YZX: return matrix4_from_euler_angles_yzx_f32(t1, t2, t3); // m1, m2, m3 = Y(t1), Z(t2), X(t3); - case .ZXY: return matrix4_from_euler_angles_zxy_f32(t1, t2, t3); // m1, m2, m3 = Z(t1), X(t2), Y(t3); - case .ZYX: return matrix4_from_euler_angles_zyx_f32(t1, t2, t3); // m1, m2, m3 = Z(t1), Y(t2), X(t3); - case .XYX: return matrix4_from_euler_angles_xyx_f32(t1, t2, t3); // m1, m2, m3 = X(t1), Y(t2), X(t3); - case .XZX: return matrix4_from_euler_angles_xzx_f32(t1, t2, t3); // m1, m2, m3 = X(t1), Z(t2), X(t3); - case .YXY: return matrix4_from_euler_angles_yxy_f32(t1, t2, t3); // m1, m2, m3 = Y(t1), X(t2), Y(t3); - case .YZY: return matrix4_from_euler_angles_yzy_f32(t1, t2, t3); // m1, m2, m3 = Y(t1), Z(t2), Y(t3); - case .ZXZ: return matrix4_from_euler_angles_zxz_f32(t1, t2, t3); // m1, m2, m3 = Z(t1), X(t2), Z(t3); - case .ZYZ: return matrix4_from_euler_angles_zyz_f32(t1, t2, t3); // m1, m2, m3 = Z(t1), Y(t2), Z(t3); + case .XYZ: return matrix4_from_euler_angles_xyz(t1, t2, t3); // m1, m2, m3 = X(t1), Y(t2), Z(t3); + case .XZY: return matrix4_from_euler_angles_xzy(t1, t2, t3); // m1, m2, m3 = X(t1), Z(t2), Y(t3); + case .YXZ: return matrix4_from_euler_angles_yxz(t1, t2, t3); // m1, m2, m3 = Y(t1), X(t2), Z(t3); + case .YZX: return matrix4_from_euler_angles_yzx(t1, t2, t3); // m1, m2, m3 = Y(t1), Z(t2), X(t3); + case .ZXY: return matrix4_from_euler_angles_zxy(t1, t2, t3); // m1, m2, m3 = Z(t1), X(t2), Y(t3); + case .ZYX: return matrix4_from_euler_angles_zyx(t1, t2, t3); // m1, m2, m3 = Z(t1), Y(t2), X(t3); + case .XYX: return matrix4_from_euler_angles_xyx(t1, t2, t3); // m1, m2, m3 = X(t1), Y(t2), X(t3); + case .XZX: return matrix4_from_euler_angles_xzx(t1, t2, t3); // m1, m2, m3 = X(t1), Z(t2), X(t3); + case .YXY: return matrix4_from_euler_angles_yxy(t1, t2, t3); // m1, m2, m3 = Y(t1), X(t2), Y(t3); + case .YZY: return matrix4_from_euler_angles_yzy(t1, t2, t3); // m1, m2, m3 = Y(t1), Z(t2), Y(t3); + case .ZXZ: return matrix4_from_euler_angles_zxz(t1, t2, t3); // m1, m2, m3 = Z(t1), X(t2), Z(t3); + case .ZYZ: return matrix4_from_euler_angles_zyz(t1, t2, t3); // m1, m2, m3 = Z(t1), Y(t2), Z(t3); } return; } @@ -173,7 +207,532 @@ euler_angles_zxy_from_quaternion_f32 :: proc(q: Quaternionf32) -> (t1, t2, t3: f } -// Matrices +// Matrix3 + + +matrix3_from_euler_angle_x_f32 :: proc(angle_x: f32) -> (m: Matrix3f32) { + cos_x, sin_x := math.cos(angle_x), math.sin(angle_x); + m[0][0] = 1; + m[1][1] = +cos_x; + m[2][1] = +sin_x; + m[1][2] = -sin_x; + m[2][2] = +cos_x; + return; +} +matrix3_from_euler_angle_y_f32 :: proc(angle_y: f32) -> (m: Matrix3f32) { + cos_y, sin_y := math.cos(angle_y), math.sin(angle_y); + m[0][0] = +cos_y; + m[2][0] = -sin_y; + m[1][1] = 1; + m[0][2] = +sin_y; + m[2][2] = +cos_y; + return; +} +matrix3_from_euler_angle_z_f32 :: proc(angle_z: f32) -> (m: Matrix3f32) { + cos_z, sin_z := math.cos(angle_z), math.sin(angle_z); + m[0][0] = +cos_z; + m[1][0] = +sin_z; + m[1][1] = +cos_z; + m[0][1] = -sin_z; + m[2][2] = 1; + return; +} + + +matrix3_from_derived_euler_angle_x_f32 :: proc(angle_x: f32, angular_velocity_x: f32) -> (m: Matrix3f32) { + cos_x := math.cos(angle_x) * angular_velocity_x; + sin_x := math.sin(angle_x) * angular_velocity_x; + m[0][0] = 1; + m[1][1] = +cos_x; + m[2][1] = +sin_x; + m[1][2] = -sin_x; + m[2][2] = +cos_x; + return; +} +matrix3_from_derived_euler_angle_y_f32 :: proc(angle_y: f32, angular_velocity_y: f32) -> (m: Matrix3f32) { + cos_y := math.cos(angle_y) * angular_velocity_y; + sin_y := math.sin(angle_y) * angular_velocity_y; + m[0][0] = +cos_y; + m[2][0] = -sin_y; + m[1][1] = 1; + m[0][2] = +sin_y; + m[2][2] = +cos_y; + return; +} +matrix3_from_derived_euler_angle_z_f32 :: proc(angle_z: f32, angular_velocity_z: f32) -> (m: Matrix3f32) { + cos_z := math.cos(angle_z) * angular_velocity_z; + sin_z := math.sin(angle_z) * angular_velocity_z; + m[0][0] = +cos_z; + m[1][0] = +sin_z; + m[1][1] = +cos_z; + m[0][1] = -sin_z; + m[2][2] = 1; + return; +} + + +matrix3_from_euler_angles_xy_f32 :: proc(angle_x, angle_y: f32) -> (m: Matrix3f32) { + 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; + m[1][0] = -sin_x * - sin_y; + m[2][0] = -cos_x * - sin_y; + m[1][1] = cos_x; + m[2][1] = sin_x; + m[0][2] = sin_y; + m[1][2] = -sin_x * cos_y; + m[2][2] = cos_x * cos_y; + return; +} + + +matrix3_from_euler_angles_yx_f32 :: proc(angle_y, angle_x: f32) -> (m: Matrix3f32) { + 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; + m[2][0] = -sin_y; + m[0][1] = sin_y*sin_x; + m[1][1] = cos_x; + m[2][1] = cos_y*sin_x; + m[0][2] = sin_y*cos_x; + m[1][2] = -sin_x; + m[2][2] = cos_y*cos_x; + return; +} + +matrix3_from_euler_angles_xz_f32 :: proc(angle_x, angle_z: f32) -> (m: Matrix3f32) { + return mul(matrix3_from_euler_angle_x(angle_x), matrix3_from_euler_angle_z(angle_z)); +} +matrix3_from_euler_angles_zx_f32 :: proc(angle_z, angle_x: f32) -> (m: Matrix3f32) { + return mul(matrix3_from_euler_angle_z(angle_z), matrix3_from_euler_angle_x(angle_x)); +} +matrix3_from_euler_angles_yz_f32 :: proc(angle_y, angle_z: f32) -> (m: Matrix3f32) { + return mul(matrix3_from_euler_angle_y(angle_y), matrix3_from_euler_angle_z(angle_z)); +} +matrix3_from_euler_angles_zy_f32 :: proc(angle_z, angle_y: f32) -> (m: Matrix3f32) { + return mul(matrix3_from_euler_angle_z(angle_z), matrix3_from_euler_angle_y(angle_y)); +} + + +matrix3_from_euler_angles_xyz_f32 :: proc(t1, t2, t3: f32) -> (m: Matrix3f32) { + c1 := math.cos(-t1); + c2 := math.cos(-t2); + c3 := math.cos(-t3); + s1 := math.sin(-t1); + s2 := math.sin(-t2); + s3 := math.sin(-t3); + + m[0][0] = c2 * c3; + m[0][1] =-c1 * s3 + s1 * s2 * c3; + m[0][2] = s1 * s3 + c1 * s2 * c3; + m[1][0] = c2 * s3; + m[1][1] = c1 * c3 + s1 * s2 * s3; + m[1][2] =-s1 * c3 + c1 * s2 * s3; + m[2][0] =-s2; + m[2][1] = s1 * c2; + m[2][2] = c1 * c2; + return; +} + +matrix3_from_euler_angles_yxz_f32 :: proc(yaw, pitch, roll: f32) -> (m: Matrix3f32) { + ch := math.cos(yaw); + sh := math.sin(yaw); + cp := math.cos(pitch); + sp := math.sin(pitch); + cb := math.cos(roll); + sb := math.sin(roll); + + m[0][0] = ch * cb + sh * sp * sb; + m[0][1] = sb * cp; + m[0][2] = -sh * cb + ch * sp * sb; + m[1][0] = -ch * sb + sh * sp * cb; + m[1][1] = cb * cp; + m[1][2] = sb * sh + ch * sp * cb; + m[2][0] = sh * cp; + m[2][1] = -sp; + m[2][2] = ch * cp; + return; +} + +matrix3_from_euler_angles_xzx_f32 :: proc(t1, t2, t3: f32) -> (m: Matrix3f32) { + c1 := math.cos(t1); + s1 := math.sin(t1); + c2 := math.cos(t2); + s2 := math.sin(t2); + c3 := math.cos(t3); + s3 := math.sin(t3); + + m[0][0] = c2; + m[0][1] = c1 * s2; + m[0][2] = s1 * s2; + m[1][0] =-c3 * s2; + m[1][1] = c1 * c2 * c3 - s1 * s3; + m[1][2] = c1 * s3 + c2 * c3 * s1; + m[2][0] = s2 * s3; + m[2][1] =-c3 * s1 - c1 * c2 * s3; + m[2][2] = c1 * c3 - c2 * s1 * s3; + return; +} + +matrix3_from_euler_angles_xyx_f32 :: proc(t1, t2, t3: f32) -> (m: Matrix3f32) { + c1 := math.cos(t1); + s1 := math.sin(t1); + c2 := math.cos(t2); + s2 := math.sin(t2); + c3 := math.cos(t3); + s3 := math.sin(t3); + + m[0][0] = c2; + m[0][1] = s1 * s2; + m[0][2] =-c1 * s2; + m[1][0] = s2 * s3; + m[1][1] = c1 * c3 - c2 * s1 * s3; + m[1][2] = c3 * s1 + c1 * c2 * s3; + m[2][0] = c3 * s2; + m[2][1] =-c1 * s3 - c2 * c3 * s1; + m[2][2] = c1 * c2 * c3 - s1 * s3; + return; +} + +matrix3_from_euler_angles_yxy_f32 :: proc(t1, t2, t3: f32) -> (m: Matrix3f32) { + c1 := math.cos(t1); + s1 := math.sin(t1); + c2 := math.cos(t2); + s2 := math.sin(t2); + c3 := math.cos(t3); + s3 := math.sin(t3); + + m[0][0] = c1 * c3 - c2 * s1 * s3; + m[0][1] = s2* s3; + m[0][2] =-c3 * s1 - c1 * c2 * s3; + m[1][0] = s1 * s2; + m[1][1] = c2; + m[1][2] = c1 * s2; + m[2][0] = c1 * s3 + c2 * c3 * s1; + m[2][1] =-c3 * s2; + m[2][2] = c1 * c2 * c3 - s1 * s3; + return; +} + +matrix3_from_euler_angles_yzy_f32 :: proc(t1, t2, t3: f32) -> (m: Matrix3f32) { + c1 := math.cos(t1); + s1 := math.sin(t1); + c2 := math.cos(t2); + s2 := math.sin(t2); + c3 := math.cos(t3); + s3 := math.sin(t3); + + m[0][0] = c1 * c2 * c3 - s1 * s3; + m[0][1] = c3 * s2; + m[0][2] =-c1 * s3 - c2 * c3 * s1; + m[1][0] =-c1 * s2; + m[1][1] = c2; + m[1][2] = s1 * s2; + m[2][0] = c3 * s1 + c1 * c2 * s3; + m[2][1] = s2 * s3; + m[2][2] = c1 * c3 - c2 * s1 * s3; + return; +} + +matrix3_from_euler_angles_zyz_f32 :: proc(t1, t2, t3: f32) -> (m: Matrix3f32) { + c1 := math.cos(t1); + s1 := math.sin(t1); + c2 := math.cos(t2); + s2 := math.sin(t2); + c3 := math.cos(t3); + s3 := math.sin(t3); + + m[0][0] = c1 * c2 * c3 - s1 * s3; + m[0][1] = c1 * s3 + c2 * c3 * s1; + m[0][2] =-c3 * s2; + m[1][0] =-c3 * s1 - c1 * c2 * s3; + m[1][1] = c1 * c3 - c2 * s1 * s3; + m[1][2] = s2 * s3; + m[2][0] = c1 * s2; + m[2][1] = s1 * s2; + m[2][2] = c2; + return; +} + +matrix3_from_euler_angles_zxz_f32 :: proc(t1, t2, t3: f32) -> (m: Matrix3f32) { + c1 := math.cos(t1); + s1 := math.sin(t1); + c2 := math.cos(t2); + s2 := math.sin(t2); + c3 := math.cos(t3); + s3 := math.sin(t3); + + m[0][0] = c1 * c3 - c2 * s1 * s3; + m[0][1] = c3 * s1 + c1 * c2 * s3; + m[0][2] = s2 *s3; + m[1][0] =-c1 * s3 - c2 * c3 * s1; + m[1][1] = c1 * c2 * c3 - s1 * s3; + m[1][2] = c3 * s2; + m[2][0] = s1 * s2; + m[2][1] =-c1 * s2; + m[2][2] = c2; + return; +} + + +matrix3_from_euler_angles_xzy_f32 :: proc(t1, t2, t3: f32) -> (m: Matrix3f32) { + c1 := math.cos(t1); + s1 := math.sin(t1); + c2 := math.cos(t2); + s2 := math.sin(t2); + c3 := math.cos(t3); + s3 := math.sin(t3); + + m[0][0] = c2 * c3; + m[0][1] = s1 * s3 + c1 * c3 * s2; + m[0][2] = c3 * s1 * s2 - c1 * s3; + m[1][0] =-s2; + m[1][1] = c1 * c2; + m[1][2] = c2 * s1; + m[2][0] = c2 * s3; + m[2][1] = c1 * s2 * s3 - c3 * s1; + m[2][2] = c1 * c3 + s1 * s2 *s3; + return; +} + +matrix3_from_euler_angles_yzx_f32 :: proc(t1, t2, t3: f32) -> (m: Matrix3f32) { + c1 := math.cos(t1); + s1 := math.sin(t1); + c2 := math.cos(t2); + s2 := math.sin(t2); + c3 := math.cos(t3); + s3 := math.sin(t3); + + m[0][0] = c1 * c2; + m[0][1] = s2; + m[0][2] =-c2 * s1; + m[1][0] = s1 * s3 - c1 * c3 * s2; + m[1][1] = c2 * c3; + m[1][2] = c1 * s3 + c3 * s1 * s2; + m[2][0] = c3 * s1 + c1 * s2 * s3; + m[2][1] =-c2 * s3; + m[2][2] = c1 * c3 - s1 * s2 * s3; + return; +} + +matrix3_from_euler_angles_zyx_f32 :: proc(t1, t2, t3: f32) -> (m: Matrix3f32) { + c1 := math.cos(t1); + s1 := math.sin(t1); + c2 := math.cos(t2); + s2 := math.sin(t2); + c3 := math.cos(t3); + s3 := math.sin(t3); + + m[0][0] = c1 * c2; + m[0][1] = c2 * s1; + m[0][2] =-s2; + m[1][0] = c1 * s2 * s3 - c3 * s1; + m[1][1] = c1 * c3 + s1 * s2 * s3; + m[1][2] = c2 * s3; + m[2][0] = s1 * s3 + c1 * c3 * s2; + m[2][1] = c3 * s1 * s2 - c1 * s3; + m[2][2] = c2 * c3; + return; +} + +matrix3_from_euler_angles_zxy_f32 :: proc(t1, t2, t3: f32) -> (m: Matrix3f32) { + c1 := math.cos(t1); + s1 := math.sin(t1); + c2 := math.cos(t2); + s2 := math.sin(t2); + c3 := math.cos(t3); + s3 := math.sin(t3); + + m[0][0] = c1 * c3 - s1 * s2 * s3; + m[0][1] = c3 * s1 + c1 * s2 * s3; + m[0][2] =-c2 * s3; + m[1][0] =-c2 * s1; + m[1][1] = c1 * c2; + m[1][2] = s2; + m[2][0] = c1 * s3 + c3 * s1 * s2; + m[2][1] = s1 * s3 - c1 * c3 * s2; + m[2][2] = c2 * c3; + return; +} + + +matrix3_from_yaw_pitch_roll_f32 :: proc(yaw, pitch, roll: f32) -> (m: Matrix3f32) { + ch := math.cos(yaw); + sh := math.sin(yaw); + cp := math.cos(pitch); + sp := math.sin(pitch); + cb := math.cos(roll); + sb := math.sin(roll); + + m[0][0] = ch * cb + sh * sp * sb; + m[0][1] = sb * cp; + m[0][2] = -sh * cb + ch * sp * sb; + m[1][0] = -ch * sb + sh * sp * cb; + m[1][1] = cb * cp; + m[1][2] = sb * sh + ch * sp * cb; + m[2][0] = sh * cp; + m[2][1] = -sp; + m[2][2] = ch * cp; + return m; +} + +euler_angles_xyz_from_matrix3_f32 :: proc(m: Matrix3f32) -> (t1, t2, t3: f32) { + 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); + S1 := math.sin(T1); + C1 := math.cos(T1); + T3 := math.atan2(S1*m[0][2] - C1*m[0][1], C1*m[1][1] - S1*m[1][2]); + t1 = -T1; + t2 = -T2; + t3 = -T3; + return; +} + +euler_angles_yxz_from_matrix3_f32 :: proc(m: Matrix3f32) -> (t1, t2, t3: f32) { + 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); + S1 := math.sin(T1); + C1 := math.cos(T1); + T3 := math.atan2(S1*m[1][2] - C1*m[1][0], C1*m[0][0] - S1*m[0][2]); + t1 = T1; + t2 = T2; + t3 = T3; + return; +} + +euler_angles_xzx_from_matrix3_f32 :: proc(m: Matrix3f32) -> (t1, t2, t3: f32) { + 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]); + S1 := math.sin(T1); + C1 := math.cos(T1); + T3 := math.atan2(C1*m[1][2] - S1*m[1][1], C1*m[2][2] - S1*m[2][1]); + t1 = T1; + t2 = T2; + t3 = T3; + return; +} + +euler_angles_xyx_from_matrix3_f32 :: proc(m: Matrix3f32) -> (t1, t2, t3: f32) { + 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]); + S1 := math.sin(T1); + C1 := math.cos(T1); + T3 := math.atan2(-C1*m[2][1] - S1*m[2][2], C1*m[1][1] + S1*m[1][2]); + t1 = T1; + t2 = T2; + t3 = T3; + return; +} + +euler_angles_yxy_from_matrix3_f32 :: proc(m: Matrix3f32) -> (t1, t2, t3: f32) { + 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]); + S1 := math.sin(T1); + C1 := math.cos(T1); + T3 := math.atan2(C1*m[2][0] - S1*m[2][2], C1*m[0][0] - S1*m[0][2]); + t1 = T1; + t2 = T2; + t3 = T3; + return; +} + +euler_angles_yzy_from_matrix3_f32 :: proc(m: Matrix3f32) -> (t1, t2, t3: f32) { + 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]); + S1 := math.sin(T1); + C1 := math.cos(T1); + T3 := math.atan2(-S1*m[0][0] - C1*m[0][2], S1*m[2][0] + C1*m[2][2]); + t1 = T1; + t2 = T2; + t3 = T3; + return; +} +euler_angles_zyz_from_matrix3_f32 :: proc(m: Matrix3f32) -> (t1, t2, t3: f32) { + 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]); + S1 := math.sin(T1); + C1 := math.cos(T1); + T3 := math.atan2(C1*m[0][1] - S1*m[0][0], C1*m[1][1] - S1*m[1][0]); + t1 = T1; + t2 = T2; + t3 = T3; + return; +} + +euler_angles_zxz_from_matrix3_f32 :: proc(m: Matrix3f32) -> (t1, t2, t3: f32) { + 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]); + S1 := math.sin(T1); + C1 := math.cos(T1); + T3 := math.atan2(-C1*m[1][0] - S1*m[1][1], C1*m[0][0] + S1*m[0][1]); + t1 = T1; + t2 = T2; + t3 = T3; + return; +} + +euler_angles_xzy_from_matrix3_f32 :: proc(m: Matrix3f32) -> (t1, t2, t3: f32) { + 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); + S1 := math.sin(T1); + C1 := math.cos(T1); + T3 := math.atan2(S1*m[0][1] - C1*m[0][2], C1*m[2][2] - S1*m[2][1]); + t1 = T1; + t2 = T2; + t3 = T3; + return; +} + +euler_angles_yzx_from_matrix3_f32 :: proc(m: Matrix3f32) -> (t1, t2, t3: f32) { + 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); + S1 := math.sin(T1); + C1 := math.cos(T1); + T3 := math.atan2(S1*m[1][0] + C1*m[1][2], S1*m[2][0] + C1*m[2][2]); + t1 = T1; + t2 = T2; + t3 = T3; + return; +} + +euler_angles_zyx_from_matrix3_f32 :: proc(m: Matrix3f32) -> (t1, t2, t3: f32) { + 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); + S1 := math.sin(T1); + C1 := math.cos(T1); + T3 := math.atan2(S1*m[2][0] - C1*m[2][1], C1*m[1][1] - S1*m[1][0]); + t1 = T1; + t2 = T2; + t3 = T3; + return; +} + +euler_angles_zxy_from_matrix3_f32 :: proc(m: Matrix3f32) -> (t1, t2, t3: f32) { + 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); + S1 := math.sin(T1); + C1 := math.cos(T1); + T3 := math.atan2(C1*m[2][0] + S1*m[2][1], C1*m[0][0] + S1*m[0][1]); + t1 = T1; + t2 = T2; + t3 = T3; + return; +} + + +// Matrix4 matrix4_from_euler_angle_x_f32 :: proc(angle_x: f32) -> (m: Matrix4f32) { diff --git a/core/math/linalg/specific_euler_angles_f64.odin b/core/math/linalg/specific_euler_angles_f64.odin index a3633ec1f..a62c2d5e8 100644 --- a/core/math/linalg/specific_euler_angles_f64.odin +++ b/core/math/linalg/specific_euler_angles_f64.odin @@ -2,6 +2,23 @@ package linalg import "core:math" +euler_angles_from_matrix3_f64 :: proc(m: Matrix3f64, order: Euler_Angle_Order) -> (t1, t2, t3: f64) { + switch order { + case .XYZ: t1, t2, t3 = euler_angles_xyz_from_matrix3(m); + case .XZY: t1, t2, t3 = euler_angles_xzy_from_matrix3(m); + case .YXZ: t1, t2, t3 = euler_angles_yxz_from_matrix3(m); + case .YZX: t1, t2, t3 = euler_angles_yzx_from_matrix3(m); + case .ZXY: t1, t2, t3 = euler_angles_zxy_from_matrix3(m); + case .ZYX: t1, t2, t3 = euler_angles_zyx_from_matrix3(m); + case .XYX: t1, t2, t3 = euler_angles_xyx_from_matrix3(m); + case .XZX: t1, t2, t3 = euler_angles_xzx_from_matrix3(m); + case .YXY: t1, t2, t3 = euler_angles_yxy_from_matrix3(m); + case .YZY: t1, t2, t3 = euler_angles_yzy_from_matrix3(m); + case .ZXZ: t1, t2, t3 = euler_angles_zxz_from_matrix3(m); + case .ZYZ: t1, t2, t3 = euler_angles_zyz_from_matrix3(m); + } + return; +} euler_angles_from_matrix4_f64 :: proc(m: Matrix4f64, order: Euler_Angle_Order) -> (t1, t2, t3: f64) { switch order { case .XYZ: t1, t2, t3 = euler_angles_xyz_from_matrix4(m); @@ -37,6 +54,23 @@ euler_angles_from_quaternion_f64 :: proc(m: Quaternionf64, order: Euler_Angle_Or return; } +matrix3_from_euler_angles_f64 :: proc(t1, t2, t3: f64, order: Euler_Angle_Order) -> (m: Matrix3f64) { + switch order { + case .XYZ: return matrix3_from_euler_angles_xyz(t1, t2, t3); // m1, m2, m3 = X(t1), Y(t2), Z(t3); + case .XZY: return matrix3_from_euler_angles_xzy(t1, t2, t3); // m1, m2, m3 = X(t1), Z(t2), Y(t3); + case .YXZ: return matrix3_from_euler_angles_yxz(t1, t2, t3); // m1, m2, m3 = Y(t1), X(t2), Z(t3); + case .YZX: return matrix3_from_euler_angles_yzx(t1, t2, t3); // m1, m2, m3 = Y(t1), Z(t2), X(t3); + case .ZXY: return matrix3_from_euler_angles_zxy(t1, t2, t3); // m1, m2, m3 = Z(t1), X(t2), Y(t3); + case .ZYX: return matrix3_from_euler_angles_zyx(t1, t2, t3); // m1, m2, m3 = Z(t1), Y(t2), X(t3); + case .XYX: return matrix3_from_euler_angles_xyx(t1, t2, t3); // m1, m2, m3 = X(t1), Y(t2), X(t3); + case .XZX: return matrix3_from_euler_angles_xzx(t1, t2, t3); // m1, m2, m3 = X(t1), Z(t2), X(t3); + case .YXY: return matrix3_from_euler_angles_yxy(t1, t2, t3); // m1, m2, m3 = Y(t1), X(t2), Y(t3); + case .YZY: return matrix3_from_euler_angles_yzy(t1, t2, t3); // m1, m2, m3 = Y(t1), Z(t2), Y(t3); + case .ZXZ: return matrix3_from_euler_angles_zxz(t1, t2, t3); // m1, m2, m3 = Z(t1), X(t2), Z(t3); + case .ZYZ: return matrix3_from_euler_angles_zyz(t1, t2, t3); // m1, m2, m3 = Z(t1), Y(t2), Z(t3); + } + return; +} matrix4_from_euler_angles_f64 :: proc(t1, t2, t3: f64, order: Euler_Angle_Order) -> (m: Matrix4f64) { switch order { case .XYZ: return matrix4_from_euler_angles_xyz(t1, t2, t3); // m1, m2, m3 = X(t1), Y(t2), Z(t3); @@ -173,7 +207,532 @@ euler_angles_zxy_from_quaternion_f64 :: proc(q: Quaternionf64) -> (t1, t2, t3: f } -// Matrices +// Matrix3 + + +matrix3_from_euler_angle_x_f64 :: proc(angle_x: f64) -> (m: Matrix3f64) { + cos_x, sin_x := math.cos(angle_x), math.sin(angle_x); + m[0][0] = 1; + m[1][1] = +cos_x; + m[2][1] = +sin_x; + m[1][2] = -sin_x; + m[2][2] = +cos_x; + return; +} +matrix3_from_euler_angle_y_f64 :: proc(angle_y: f64) -> (m: Matrix3f64) { + cos_y, sin_y := math.cos(angle_y), math.sin(angle_y); + m[0][0] = +cos_y; + m[2][0] = -sin_y; + m[1][1] = 1; + m[0][2] = +sin_y; + m[2][2] = +cos_y; + return; +} +matrix3_from_euler_angle_z_f64 :: proc(angle_z: f64) -> (m: Matrix3f64) { + cos_z, sin_z := math.cos(angle_z), math.sin(angle_z); + m[0][0] = +cos_z; + m[1][0] = +sin_z; + m[1][1] = +cos_z; + m[0][1] = -sin_z; + m[2][2] = 1; + return; +} + + +matrix3_from_derived_euler_angle_x_f64 :: proc(angle_x: f64, angular_velocity_x: f64) -> (m: Matrix3f64) { + cos_x := math.cos(angle_x) * angular_velocity_x; + sin_x := math.sin(angle_x) * angular_velocity_x; + m[0][0] = 1; + m[1][1] = +cos_x; + m[2][1] = +sin_x; + m[1][2] = -sin_x; + m[2][2] = +cos_x; + return; +} +matrix3_from_derived_euler_angle_y_f64 :: proc(angle_y: f64, angular_velocity_y: f64) -> (m: Matrix3f64) { + cos_y := math.cos(angle_y) * angular_velocity_y; + sin_y := math.sin(angle_y) * angular_velocity_y; + m[0][0] = +cos_y; + m[2][0] = -sin_y; + m[1][1] = 1; + m[0][2] = +sin_y; + m[2][2] = +cos_y; + return; +} +matrix3_from_derived_euler_angle_z_f64 :: proc(angle_z: f64, angular_velocity_z: f64) -> (m: Matrix3f64) { + cos_z := math.cos(angle_z) * angular_velocity_z; + sin_z := math.sin(angle_z) * angular_velocity_z; + m[0][0] = +cos_z; + m[1][0] = +sin_z; + m[1][1] = +cos_z; + m[0][1] = -sin_z; + m[2][2] = 1; + return; +} + + +matrix3_from_euler_angles_xy_f64 :: proc(angle_x, angle_y: f64) -> (m: Matrix3f64) { + 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; + m[1][0] = -sin_x * - sin_y; + m[2][0] = -cos_x * - sin_y; + m[1][1] = cos_x; + m[2][1] = sin_x; + m[0][2] = sin_y; + m[1][2] = -sin_x * cos_y; + m[2][2] = cos_x * cos_y; + return; +} + + +matrix3_from_euler_angles_yx_f64 :: proc(angle_y, angle_x: f64) -> (m: Matrix3f64) { + 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; + m[2][0] = -sin_y; + m[0][1] = sin_y*sin_x; + m[1][1] = cos_x; + m[2][1] = cos_y*sin_x; + m[0][2] = sin_y*cos_x; + m[1][2] = -sin_x; + m[2][2] = cos_y*cos_x; + return; +} + +matrix3_from_euler_angles_xz_f64 :: proc(angle_x, angle_z: f64) -> (m: Matrix3f64) { + return mul(matrix3_from_euler_angle_x(angle_x), matrix3_from_euler_angle_z(angle_z)); +} +matrix3_from_euler_angles_zx_f64 :: proc(angle_z, angle_x: f64) -> (m: Matrix3f64) { + return mul(matrix3_from_euler_angle_z(angle_z), matrix3_from_euler_angle_x(angle_x)); +} +matrix3_from_euler_angles_yz_f64 :: proc(angle_y, angle_z: f64) -> (m: Matrix3f64) { + return mul(matrix3_from_euler_angle_y(angle_y), matrix3_from_euler_angle_z(angle_z)); +} +matrix3_from_euler_angles_zy_f64 :: proc(angle_z, angle_y: f64) -> (m: Matrix3f64) { + return mul(matrix3_from_euler_angle_z(angle_z), matrix3_from_euler_angle_y(angle_y)); +} + + +matrix3_from_euler_angles_xyz_f64 :: proc(t1, t2, t3: f64) -> (m: Matrix3f64) { + c1 := math.cos(-t1); + c2 := math.cos(-t2); + c3 := math.cos(-t3); + s1 := math.sin(-t1); + s2 := math.sin(-t2); + s3 := math.sin(-t3); + + m[0][0] = c2 * c3; + m[0][1] =-c1 * s3 + s1 * s2 * c3; + m[0][2] = s1 * s3 + c1 * s2 * c3; + m[1][0] = c2 * s3; + m[1][1] = c1 * c3 + s1 * s2 * s3; + m[1][2] =-s1 * c3 + c1 * s2 * s3; + m[2][0] =-s2; + m[2][1] = s1 * c2; + m[2][2] = c1 * c2; + return; +} + +matrix3_from_euler_angles_yxz_f64 :: proc(yaw, pitch, roll: f64) -> (m: Matrix3f64) { + ch := math.cos(yaw); + sh := math.sin(yaw); + cp := math.cos(pitch); + sp := math.sin(pitch); + cb := math.cos(roll); + sb := math.sin(roll); + + m[0][0] = ch * cb + sh * sp * sb; + m[0][1] = sb * cp; + m[0][2] = -sh * cb + ch * sp * sb; + m[1][0] = -ch * sb + sh * sp * cb; + m[1][1] = cb * cp; + m[1][2] = sb * sh + ch * sp * cb; + m[2][0] = sh * cp; + m[2][1] = -sp; + m[2][2] = ch * cp; + return; +} + +matrix3_from_euler_angles_xzx_f64 :: proc(t1, t2, t3: f64) -> (m: Matrix3f64) { + c1 := math.cos(t1); + s1 := math.sin(t1); + c2 := math.cos(t2); + s2 := math.sin(t2); + c3 := math.cos(t3); + s3 := math.sin(t3); + + m[0][0] = c2; + m[0][1] = c1 * s2; + m[0][2] = s1 * s2; + m[1][0] =-c3 * s2; + m[1][1] = c1 * c2 * c3 - s1 * s3; + m[1][2] = c1 * s3 + c2 * c3 * s1; + m[2][0] = s2 * s3; + m[2][1] =-c3 * s1 - c1 * c2 * s3; + m[2][2] = c1 * c3 - c2 * s1 * s3; + return; +} + +matrix3_from_euler_angles_xyx_f64 :: proc(t1, t2, t3: f64) -> (m: Matrix3f64) { + c1 := math.cos(t1); + s1 := math.sin(t1); + c2 := math.cos(t2); + s2 := math.sin(t2); + c3 := math.cos(t3); + s3 := math.sin(t3); + + m[0][0] = c2; + m[0][1] = s1 * s2; + m[0][2] =-c1 * s2; + m[1][0] = s2 * s3; + m[1][1] = c1 * c3 - c2 * s1 * s3; + m[1][2] = c3 * s1 + c1 * c2 * s3; + m[2][0] = c3 * s2; + m[2][1] =-c1 * s3 - c2 * c3 * s1; + m[2][2] = c1 * c2 * c3 - s1 * s3; + return; +} + +matrix3_from_euler_angles_yxy_f64 :: proc(t1, t2, t3: f64) -> (m: Matrix3f64) { + c1 := math.cos(t1); + s1 := math.sin(t1); + c2 := math.cos(t2); + s2 := math.sin(t2); + c3 := math.cos(t3); + s3 := math.sin(t3); + + m[0][0] = c1 * c3 - c2 * s1 * s3; + m[0][1] = s2* s3; + m[0][2] =-c3 * s1 - c1 * c2 * s3; + m[1][0] = s1 * s2; + m[1][1] = c2; + m[1][2] = c1 * s2; + m[2][0] = c1 * s3 + c2 * c3 * s1; + m[2][1] =-c3 * s2; + m[2][2] = c1 * c2 * c3 - s1 * s3; + return; +} + +matrix3_from_euler_angles_yzy_f64 :: proc(t1, t2, t3: f64) -> (m: Matrix3f64) { + c1 := math.cos(t1); + s1 := math.sin(t1); + c2 := math.cos(t2); + s2 := math.sin(t2); + c3 := math.cos(t3); + s3 := math.sin(t3); + + m[0][0] = c1 * c2 * c3 - s1 * s3; + m[0][1] = c3 * s2; + m[0][2] =-c1 * s3 - c2 * c3 * s1; + m[1][0] =-c1 * s2; + m[1][1] = c2; + m[1][2] = s1 * s2; + m[2][0] = c3 * s1 + c1 * c2 * s3; + m[2][1] = s2 * s3; + m[2][2] = c1 * c3 - c2 * s1 * s3; + return; +} + +matrix3_from_euler_angles_zyz_f64 :: proc(t1, t2, t3: f64) -> (m: Matrix3f64) { + c1 := math.cos(t1); + s1 := math.sin(t1); + c2 := math.cos(t2); + s2 := math.sin(t2); + c3 := math.cos(t3); + s3 := math.sin(t3); + + m[0][0] = c1 * c2 * c3 - s1 * s3; + m[0][1] = c1 * s3 + c2 * c3 * s1; + m[0][2] =-c3 * s2; + m[1][0] =-c3 * s1 - c1 * c2 * s3; + m[1][1] = c1 * c3 - c2 * s1 * s3; + m[1][2] = s2 * s3; + m[2][0] = c1 * s2; + m[2][1] = s1 * s2; + m[2][2] = c2; + return; +} + +matrix3_from_euler_angles_zxz_f64 :: proc(t1, t2, t3: f64) -> (m: Matrix3f64) { + c1 := math.cos(t1); + s1 := math.sin(t1); + c2 := math.cos(t2); + s2 := math.sin(t2); + c3 := math.cos(t3); + s3 := math.sin(t3); + + m[0][0] = c1 * c3 - c2 * s1 * s3; + m[0][1] = c3 * s1 + c1 * c2 * s3; + m[0][2] = s2 *s3; + m[1][0] =-c1 * s3 - c2 * c3 * s1; + m[1][1] = c1 * c2 * c3 - s1 * s3; + m[1][2] = c3 * s2; + m[2][0] = s1 * s2; + m[2][1] =-c1 * s2; + m[2][2] = c2; + return; +} + + +matrix3_from_euler_angles_xzy_f64 :: proc(t1, t2, t3: f64) -> (m: Matrix3f64) { + c1 := math.cos(t1); + s1 := math.sin(t1); + c2 := math.cos(t2); + s2 := math.sin(t2); + c3 := math.cos(t3); + s3 := math.sin(t3); + + m[0][0] = c2 * c3; + m[0][1] = s1 * s3 + c1 * c3 * s2; + m[0][2] = c3 * s1 * s2 - c1 * s3; + m[1][0] =-s2; + m[1][1] = c1 * c2; + m[1][2] = c2 * s1; + m[2][0] = c2 * s3; + m[2][1] = c1 * s2 * s3 - c3 * s1; + m[2][2] = c1 * c3 + s1 * s2 *s3; + return; +} + +matrix3_from_euler_angles_yzx_f64 :: proc(t1, t2, t3: f64) -> (m: Matrix3f64) { + c1 := math.cos(t1); + s1 := math.sin(t1); + c2 := math.cos(t2); + s2 := math.sin(t2); + c3 := math.cos(t3); + s3 := math.sin(t3); + + m[0][0] = c1 * c2; + m[0][1] = s2; + m[0][2] =-c2 * s1; + m[1][0] = s1 * s3 - c1 * c3 * s2; + m[1][1] = c2 * c3; + m[1][2] = c1 * s3 + c3 * s1 * s2; + m[2][0] = c3 * s1 + c1 * s2 * s3; + m[2][1] =-c2 * s3; + m[2][2] = c1 * c3 - s1 * s2 * s3; + return; +} + +matrix3_from_euler_angles_zyx_f64 :: proc(t1, t2, t3: f64) -> (m: Matrix3f64) { + c1 := math.cos(t1); + s1 := math.sin(t1); + c2 := math.cos(t2); + s2 := math.sin(t2); + c3 := math.cos(t3); + s3 := math.sin(t3); + + m[0][0] = c1 * c2; + m[0][1] = c2 * s1; + m[0][2] =-s2; + m[1][0] = c1 * s2 * s3 - c3 * s1; + m[1][1] = c1 * c3 + s1 * s2 * s3; + m[1][2] = c2 * s3; + m[2][0] = s1 * s3 + c1 * c3 * s2; + m[2][1] = c3 * s1 * s2 - c1 * s3; + m[2][2] = c2 * c3; + return; +} + +matrix3_from_euler_angles_zxy_f64 :: proc(t1, t2, t3: f64) -> (m: Matrix3f64) { + c1 := math.cos(t1); + s1 := math.sin(t1); + c2 := math.cos(t2); + s2 := math.sin(t2); + c3 := math.cos(t3); + s3 := math.sin(t3); + + m[0][0] = c1 * c3 - s1 * s2 * s3; + m[0][1] = c3 * s1 + c1 * s2 * s3; + m[0][2] =-c2 * s3; + m[1][0] =-c2 * s1; + m[1][1] = c1 * c2; + m[1][2] = s2; + m[2][0] = c1 * s3 + c3 * s1 * s2; + m[2][1] = s1 * s3 - c1 * c3 * s2; + m[2][2] = c2 * c3; + return; +} + + +matrix3_from_yaw_pitch_roll_f64 :: proc(yaw, pitch, roll: f64) -> (m: Matrix3f64) { + ch := math.cos(yaw); + sh := math.sin(yaw); + cp := math.cos(pitch); + sp := math.sin(pitch); + cb := math.cos(roll); + sb := math.sin(roll); + + m[0][0] = ch * cb + sh * sp * sb; + m[0][1] = sb * cp; + m[0][2] = -sh * cb + ch * sp * sb; + m[1][0] = -ch * sb + sh * sp * cb; + m[1][1] = cb * cp; + m[1][2] = sb * sh + ch * sp * cb; + m[2][0] = sh * cp; + m[2][1] = -sp; + m[2][2] = ch * cp; + return m; +} + +euler_angles_xyz_from_matrix3_f64 :: proc(m: Matrix3f64) -> (t1, t2, t3: f64) { + 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); + S1 := math.sin(T1); + C1 := math.cos(T1); + T3 := math.atan2(S1*m[0][2] - C1*m[0][1], C1*m[1][1] - S1*m[1][2]); + t1 = -T1; + t2 = -T2; + t3 = -T3; + return; +} + +euler_angles_yxz_from_matrix3_f64 :: proc(m: Matrix3f64) -> (t1, t2, t3: f64) { + 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); + S1 := math.sin(T1); + C1 := math.cos(T1); + T3 := math.atan2(S1*m[1][2] - C1*m[1][0], C1*m[0][0] - S1*m[0][2]); + t1 = T1; + t2 = T2; + t3 = T3; + return; +} + +euler_angles_xzx_from_matrix3_f64 :: proc(m: Matrix3f64) -> (t1, t2, t3: f64) { + 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]); + S1 := math.sin(T1); + C1 := math.cos(T1); + T3 := math.atan2(C1*m[1][2] - S1*m[1][1], C1*m[2][2] - S1*m[2][1]); + t1 = T1; + t2 = T2; + t3 = T3; + return; +} + +euler_angles_xyx_from_matrix3_f64 :: proc(m: Matrix3f64) -> (t1, t2, t3: f64) { + 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]); + S1 := math.sin(T1); + C1 := math.cos(T1); + T3 := math.atan2(-C1*m[2][1] - S1*m[2][2], C1*m[1][1] + S1*m[1][2]); + t1 = T1; + t2 = T2; + t3 = T3; + return; +} + +euler_angles_yxy_from_matrix3_f64 :: proc(m: Matrix3f64) -> (t1, t2, t3: f64) { + 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]); + S1 := math.sin(T1); + C1 := math.cos(T1); + T3 := math.atan2(C1*m[2][0] - S1*m[2][2], C1*m[0][0] - S1*m[0][2]); + t1 = T1; + t2 = T2; + t3 = T3; + return; +} + +euler_angles_yzy_from_matrix3_f64 :: proc(m: Matrix3f64) -> (t1, t2, t3: f64) { + 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]); + S1 := math.sin(T1); + C1 := math.cos(T1); + T3 := math.atan2(-S1*m[0][0] - C1*m[0][2], S1*m[2][0] + C1*m[2][2]); + t1 = T1; + t2 = T2; + t3 = T3; + return; +} +euler_angles_zyz_from_matrix3_f64 :: proc(m: Matrix3f64) -> (t1, t2, t3: f64) { + 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]); + S1 := math.sin(T1); + C1 := math.cos(T1); + T3 := math.atan2(C1*m[0][1] - S1*m[0][0], C1*m[1][1] - S1*m[1][0]); + t1 = T1; + t2 = T2; + t3 = T3; + return; +} + +euler_angles_zxz_from_matrix3_f64 :: proc(m: Matrix3f64) -> (t1, t2, t3: f64) { + 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]); + S1 := math.sin(T1); + C1 := math.cos(T1); + T3 := math.atan2(-C1*m[1][0] - S1*m[1][1], C1*m[0][0] + S1*m[0][1]); + t1 = T1; + t2 = T2; + t3 = T3; + return; +} + +euler_angles_xzy_from_matrix3_f64 :: proc(m: Matrix3f64) -> (t1, t2, t3: f64) { + 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); + S1 := math.sin(T1); + C1 := math.cos(T1); + T3 := math.atan2(S1*m[0][1] - C1*m[0][2], C1*m[2][2] - S1*m[2][1]); + t1 = T1; + t2 = T2; + t3 = T3; + return; +} + +euler_angles_yzx_from_matrix3_f64 :: proc(m: Matrix3f64) -> (t1, t2, t3: f64) { + 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); + S1 := math.sin(T1); + C1 := math.cos(T1); + T3 := math.atan2(S1*m[1][0] + C1*m[1][2], S1*m[2][0] + C1*m[2][2]); + t1 = T1; + t2 = T2; + t3 = T3; + return; +} + +euler_angles_zyx_from_matrix3_f64 :: proc(m: Matrix3f64) -> (t1, t2, t3: f64) { + 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); + S1 := math.sin(T1); + C1 := math.cos(T1); + T3 := math.atan2(S1*m[2][0] - C1*m[2][1], C1*m[1][1] - S1*m[1][0]); + t1 = T1; + t2 = T2; + t3 = T3; + return; +} + +euler_angles_zxy_from_matrix3_f64 :: proc(m: Matrix3f64) -> (t1, t2, t3: f64) { + 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); + S1 := math.sin(T1); + C1 := math.cos(T1); + T3 := math.atan2(C1*m[2][0] + S1*m[2][1], C1*m[0][0] + S1*m[0][1]); + t1 = T1; + t2 = T2; + t3 = T3; + return; +} + + +// Matrix4 matrix4_from_euler_angle_x_f64 :: proc(angle_x: f64) -> (m: Matrix4f64) {