diff --git a/core/math/linalg/general.odin b/core/math/linalg/general.odin index fb9754051..4d3e02ae8 100644 --- a/core/math/linalg/general.odin +++ b/core/math/linalg/general.odin @@ -313,3 +313,31 @@ hermite :: proc(v1, t1, v2, t2: $T/[$N]$E, s: E) -> T { cubic :: proc(v1, v2, v3, v4: $T/[$N]$E, s: E) -> T { return ((v1 * s + v2) * s + v3) * s + v3; } + + + +array_cast :: proc(v: $A/[$N]$T, $U: typeid) -> [N]U { + w: [N]U; + for _, i in v do w[i] = U(v[i]); + return w; +} + +to_f32 :: #force_inline proc(v: $A/[$N]$T) -> [N]f32 { return array_cast(v, f32); } +to_f64 :: #force_inline proc(v: $A/[$N]$T) -> [N]f64 { return array_cast(v, f64); } + +to_i8 :: #force_inline proc(v: $A/[$N]$T) -> [N]i8 { return array_cast(v, i8); } +to_i16 :: #force_inline proc(v: $A/[$N]$T) -> [N]i16 { return array_cast(v, i16); } +to_i32 :: #force_inline proc(v: $A/[$N]$T) -> [N]i32 { return array_cast(v, i32); } +to_i64 :: #force_inline proc(v: $A/[$N]$T) -> [N]i64 { return array_cast(v, i64); } +to_int :: #force_inline proc(v: $A/[$N]$T) -> [N]int { return array_cast(v, int); } + +to_u8 :: #force_inline proc(v: $A/[$N]$T) -> [N]u8 { return array_cast(v, u8); } +to_u16 :: #force_inline proc(v: $A/[$N]$T) -> [N]u16 { return array_cast(v, u16); } +to_u32 :: #force_inline proc(v: $A/[$N]$T) -> [N]u32 { return array_cast(v, u32); } +to_u64 :: #force_inline proc(v: $A/[$N]$T) -> [N]u64 { return array_cast(v, u64); } +to_uint :: #force_inline proc(v: $A/[$N]$T) -> [N]uint { return array_cast(v, uint); } + +to_complex64 :: #force_inline proc(v: $A/[$N]$T) -> [N]complex64 { return array_cast(v, complex64); } +to_complex128 :: #force_inline proc(v: $A/[$N]$T) -> [N]complex128 { return array_cast(v, complex128); } +to_quaternion128 :: #force_inline proc(v: $A/[$N]$T) -> [N]quaternion128 { return array_cast(v, quaternion128); } +to_quaternion256 :: #force_inline proc(v: $A/[$N]$T) -> [N]quaternion256 { return array_cast(v, quaternion256); } diff --git a/core/math/linalg/specific.odin b/core/math/linalg/specific.odin index ab3180a73..1b3bc1f2d 100644 --- a/core/math/linalg/specific.odin +++ b/core/math/linalg/specific.odin @@ -7,49 +7,90 @@ import "core:math" Float :: f64 when #config(ODIN_MATH_LINALG_USE_F64, false) else f32; -FLOAT_EPSILON :: 1e-7 when size_of(Float) == 4 else 1e-15; +F32_EPSILON :: 1e-7; +F64_EPSILON :: 1e-15; -Vector2 :: distinct [2]Float; -Vector3 :: distinct [3]Float; -Vector4 :: distinct [4]Float; +Vector2f32 :: distinct [2]f32; +Vector3f32 :: distinct [3]f32; +Vector4f32 :: distinct [4]f32; -Matrix1x1 :: distinct [1][1]Float; -Matrix1x2 :: distinct [1][2]Float; -Matrix1x3 :: distinct [1][3]Float; -Matrix1x4 :: distinct [1][4]Float; +Matrix1x1f32 :: distinct [1][1]f32; +Matrix1x2f32 :: distinct [1][2]f32; +Matrix1x3f32 :: distinct [1][3]f32; +Matrix1x4f32 :: distinct [1][4]f32; -Matrix2x1 :: distinct [2][1]Float; -Matrix2x2 :: distinct [2][2]Float; -Matrix2x3 :: distinct [2][3]Float; -Matrix2x4 :: distinct [2][4]Float; +Matrix2x1f32 :: distinct [2][1]f32; +Matrix2x2f32 :: distinct [2][2]f32; +Matrix2x3f32 :: distinct [2][3]f32; +Matrix2x4f32 :: distinct [2][4]f32; -Matrix3x1 :: distinct [3][1]Float; -Matrix3x2 :: distinct [3][2]Float; -Matrix3x3 :: distinct [3][3]Float; -Matrix3x4 :: distinct [3][4]Float; +Matrix3x1f32 :: distinct [3][1]f32; +Matrix3x2f32 :: distinct [3][2]f32; +Matrix3x3f32 :: distinct [3][3]f32; +Matrix3x4f32 :: distinct [3][4]f32; -Matrix4x1 :: distinct [4][1]Float; -Matrix4x2 :: distinct [4][2]Float; -Matrix4x3 :: distinct [4][3]Float; -Matrix4x4 :: distinct [4][4]Float; +Matrix4x1f32 :: distinct [4][1]f32; +Matrix4x2f32 :: distinct [4][2]f32; +Matrix4x3f32 :: distinct [4][3]f32; +Matrix4x4f32 :: distinct [4][4]f32; -Matrix1 :: Matrix1x1; -Matrix2 :: Matrix2x2; -Matrix3 :: Matrix3x3; -Matrix4 :: Matrix4x4; +Matrix1f32 :: Matrix1x1f32; +Matrix2f32 :: Matrix2x2f32; +Matrix3f32 :: Matrix3x3f32; +Matrix4f32 :: Matrix4x4f32; -Quaternion :: distinct (quaternion128 when size_of(Float) == size_of(f32) else quaternion256); +Vector2f64 :: distinct [2]f64; +Vector3f64 :: distinct [3]f64; +Vector4f64 :: distinct [4]f64; -MATRIX1_IDENTITY :: Matrix1{{1}}; -MATRIX2_IDENTITY :: Matrix2{{1, 0}, {0, 1}}; -MATRIX3_IDENTITY :: Matrix3{{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; -MATRIX4_IDENTITY :: Matrix4{{1, 0, 0, 0}, {0, 1, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}}; +Matrix1x1f64 :: distinct [1][1]f64; +Matrix1x2f64 :: distinct [1][2]f64; +Matrix1x3f64 :: distinct [1][3]f64; +Matrix1x4f64 :: distinct [1][4]f64; -QUATERNION_IDENTITY :: Quaternion(1); +Matrix2x1f64 :: distinct [2][1]f64; +Matrix2x2f64 :: distinct [2][2]f64; +Matrix2x3f64 :: distinct [2][3]f64; +Matrix2x4f64 :: distinct [2][4]f64; -VECTOR3_X_AXIS :: Vector3{1, 0, 0}; -VECTOR3_Y_AXIS :: Vector3{0, 1, 0}; -VECTOR3_Z_AXIS :: Vector3{0, 0, 1}; +Matrix3x1f64 :: distinct [3][1]f64; +Matrix3x2f64 :: distinct [3][2]f64; +Matrix3x3f64 :: distinct [3][3]f64; +Matrix3x4f64 :: distinct [3][4]f64; + +Matrix4x1f64 :: distinct [4][1]f64; +Matrix4x2f64 :: distinct [4][2]f64; +Matrix4x3f64 :: distinct [4][3]f64; +Matrix4x4f64 :: distinct [4][4]f64; + +Matrix1f64 :: Matrix1x1f64; +Matrix2f64 :: Matrix2x2f64; +Matrix3f64 :: Matrix3x3f64; +Matrix4f64 :: Matrix4x4f64; + +Quaternionf32 :: distinct quaternion128; +Quaternionf64 :: distinct quaternion256; + +MATRIX1F32_IDENTITY :: Matrix1f32{{1}}; +MATRIX2F32_IDENTITY :: Matrix2f32{{1, 0}, {0, 1}}; +MATRIX3F32_IDENTITY :: Matrix3f32{{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; +MATRIX4F32_IDENTITY :: Matrix4f32{{1, 0, 0, 0}, {0, 1, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}}; + +MATRIX1F64_IDENTITY :: Matrix1f64{{1}}; +MATRIX2F64_IDENTITY :: Matrix2f64{{1, 0}, {0, 1}}; +MATRIX3F64_IDENTITY :: Matrix3f64{{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; +MATRIX4F64_IDENTITY :: Matrix4f64{{1, 0, 0, 0}, {0, 1, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}}; + +QUATERNIONF32_IDENTITY :: Quaternionf32(1); +QUATERNIONF64_IDENTITY :: Quaternionf64(1); + +VECTOR3F32_X_AXIS :: Vector3f32{1, 0, 0}; +VECTOR3F32_Y_AXIS :: Vector3f32{0, 1, 0}; +VECTOR3F32_Z_AXIS :: Vector3f32{0, 0, 1}; + +VECTOR3F64_X_AXIS :: Vector3f64{1, 0, 0}; +VECTOR3F64_Y_AXIS :: Vector3f64{0, 1, 0}; +VECTOR3F64_Z_AXIS :: Vector3f64{0, 0, 1}; vector2_orthogonal :: proc(v: $V/[2]$E) -> V where !IS_ARRAY(E), IS_FLOAT(E) { @@ -82,15 +123,26 @@ orthogonal :: proc{vector2_orthogonal, vector3_orthogonal}; -vector4_srgb_to_linear :: proc(col: Vector4) -> Vector4 { +vector4_srgb_to_linear_f32 :: proc(col: Vector4f32) -> Vector4f32 { r := math.pow(col.x, 2.2); g := math.pow(col.y, 2.2); b := math.pow(col.z, 2.2); a := col.w; return {r, g, b, a}; } +vector4_srgb_to_linear_f64 :: proc(col: Vector4f64) -> Vector4f64 { + r := math.pow(col.x, 2.2); + g := math.pow(col.y, 2.2); + b := math.pow(col.z, 2.2); + a := col.w; + return {r, g, b, a}; +} +vector4_srgb_to_linear :: proc{ + vector4_srgb_to_linear_f32, + vector4_srgb_to_linear_f64, +}; -vector4_linear_to_srgb :: proc(col: Vector4) -> Vector4 { +vector4_linear_to_srgb_f32 :: proc(col: Vector4f32) -> Vector4f32 { a :: 2.51; b :: 0.03; c :: 2.43; @@ -111,9 +163,35 @@ vector4_linear_to_srgb :: proc(col: Vector4) -> Vector4 { return {x, y, z, col.w}; } +vector4_linear_to_srgb_f64 :: proc(col: Vector4f64) -> Vector4f64 { + a :: 2.51; + b :: 0.03; + c :: 2.43; + d :: 0.59; + e :: 0.14; -vector4_hsl_to_rgb :: proc(h, s, l: Float, a: Float = 1) -> Vector4 { - hue_to_rgb :: proc(p, q, t: Float) -> Float { + x := col.x; + y := col.y; + z := col.z; + + x = (x * (a * x + b)) / (x * (c * x + d) + e); + y = (y * (a * y + b)) / (y * (c * y + d) + e); + z = (z * (a * z + b)) / (z * (c * z + d) + e); + + x = math.pow(clamp(x, 0, 1), 1.0 / 2.2); + y = math.pow(clamp(y, 0, 1), 1.0 / 2.2); + z = math.pow(clamp(z, 0, 1), 1.0 / 2.2); + + return {x, y, z, col.w}; +} +vector4_linear_to_srgb :: proc{ + vector4_linear_to_srgb_f32, + vector4_linear_to_srgb_f64, +}; + + +vector4_hsl_to_rgb_f32 :: proc(h, s, l: f32, a: f32 = 1) -> Vector4f32 { + hue_to_rgb :: proc(p, q, t: f32) -> f32 { t := t; if t < 0 { t += 1; } if t > 1 { t -= 1; } @@ -125,7 +203,7 @@ vector4_hsl_to_rgb :: proc(h, s, l: Float, a: Float = 1) -> Vector4 { return p; } - r, g, b: Float; + r, g, b: f32; if s == 0 { r = l; g = l; @@ -139,15 +217,46 @@ vector4_hsl_to_rgb :: proc(h, s, l: Float, a: Float = 1) -> Vector4 { } return {r, g, b, a}; } +vector4_hsl_to_rgb_f64 :: proc(h, s, l: f64, a: f64 = 1) -> Vector4f64 { + hue_to_rgb :: proc(p, q, t: f64) -> f64 { + t := t; + if t < 0 { t += 1; } + if t > 1 { t -= 1; } + switch { + case t < 1.0/6.0: return p + (q - p) * 6.0 * t; + case t < 1.0/2.0: return q; + case t < 2.0/3.0: return p + (q - p) * 6.0 * (2.0/3.0 - t); + } + return p; + } -vector4_rgb_to_hsl :: proc(col: Vector4) -> Vector4 { + r, g, b: f64; + if s == 0 { + r = l; + g = l; + b = l; + } else { + q := l * (1+s) if l < 0.5 else l+s - l*s; + p := 2*l - q; + r = hue_to_rgb(p, q, h + 1.0/3.0); + g = hue_to_rgb(p, q, h); + b = hue_to_rgb(p, q, h - 1.0/3.0); + } + return {r, g, b, a}; +} +vector4_hsl_to_rgb :: proc{ + vector4_hsl_to_rgb_f32, + vector4_hsl_to_rgb_f64, +}; + +vector4_rgb_to_hsl_f32 :: proc(col: Vector4f32) -> Vector4f32 { r := col.x; g := col.y; b := col.z; a := col.w; v_min := min(r, g, b); v_max := max(r, g, b); - h, s, l: Float; + h, s, l: f32; h = 0.0; s = 0.0; l = (v_min + v_max) * 0.5; @@ -170,9 +279,42 @@ vector4_rgb_to_hsl :: proc(col: Vector4) -> Vector4 { return {h, s, l, a}; } +vector4_rgb_to_hsl_f64 :: proc(col: Vector4f64) -> Vector4f64 { + r := col.x; + g := col.y; + b := col.z; + a := col.w; + v_min := min(r, g, b); + v_max := max(r, g, b); + h, s, l: f64; + h = 0.0; + s = 0.0; + l = (v_min + v_max) * 0.5; + + if v_max != v_min { + d: = v_max - v_min; + s = d / (2.0 - v_max - v_min) if l > 0.5 else d / (v_max + v_min); + switch { + case v_max == r: + h = (g - b) / d + (6.0 if g < b else 0.0); + case v_max == g: + h = (b - r) / d + 2.0; + case v_max == b: + h = (r - g) / d + 4.0; + } + + h *= 1.0/6.0; + } + + return {h, s, l, a}; +} +vector4_rgb_to_hsl :: proc{ + vector4_rgb_to_hsl_f32, + vector4_rgb_to_hsl_f64, +}; -quaternion_angle_axis :: proc(angle_radians: Float, axis: Vector3) -> (q: Quaternion) { +quaternion_angle_axis_f32 :: proc(angle_radians: f32, axis: Vector3f32) -> (q: Quaternionf32) { t := angle_radians*0.5; v := normalize(axis) * math.sin(t); q.x = v.x; @@ -181,34 +323,81 @@ quaternion_angle_axis :: proc(angle_radians: Float, axis: Vector3) -> (q: Quater q.w = math.cos(t); return; } +quaternion_angle_axis_f64 :: proc(angle_radians: f64, axis: Vector3f64) -> (q: Quaternionf64) { + t := angle_radians*0.5; + v := normalize(axis) * math.sin(t); + q.x = v.x; + q.y = v.y; + q.z = v.z; + q.w = math.cos(t); + return; +} +quaternion_angle_axis :: proc{ + quaternion_angle_axis_f32, + quaternion_angle_axis_f64, +}; -angle_from_quaternion :: proc(q: Quaternion) -> Float { +angle_from_quaternion_f32 :: proc(q: Quaternionf32) -> f32 { if abs(q.w) > math.SQRT_THREE*0.5 { return math.asin(q.x*q.x + q.y*q.y + q.z*q.z) * 2; } return math.cos(q.x) * 2; } +angle_from_quaternion_f64 :: proc(q: Quaternionf64) -> f64 { + if abs(q.w) > math.SQRT_THREE*0.5 { + return math.asin(q.x*q.x + q.y*q.y + q.z*q.z) * 2; + } -axis_from_quaternion :: proc(q: Quaternion) -> Vector3 { + return math.cos(q.x) * 2; +} +angle_from_quaternion :: proc{ + angle_from_quaternion_f32, + angle_from_quaternion_f64, +}; + +axis_from_quaternion_f32 :: proc(q: Quaternionf32) -> Vector3f32 { t1 := 1 - q.w*q.w; if t1 < 0 { - return Vector3{0, 0, 1}; + return {0, 0, 1}; } t2 := 1.0 / math.sqrt(t1); - return Vector3{q.x*t2, q.y*t2, q.z*t2}; + return {q.x*t2, q.y*t2, q.z*t2}; } -angle_axis_from_quaternion :: proc(q: Quaternion) -> (angle: Float, axis: Vector3) { +axis_from_quaternion_f64 :: proc(q: Quaternionf64) -> Vector3f64 { + t1 := 1 - q.w*q.w; + if t1 < 0 { + return {0, 0, 1}; + } + t2 := 1.0 / math.sqrt(t1); + return {q.x*t2, q.y*t2, q.z*t2}; +} +axis_from_quaternion :: proc{ + axis_from_quaternion_f32, + axis_from_quaternion_f64, +}; + +angle_axis_from_quaternion_f32 :: proc(q: Quaternionf32) -> (angle: f32, axis: Vector3f32) { angle = angle_from_quaternion(q); axis = axis_from_quaternion(q); return; } +angle_axis_from_quaternion_f64 :: proc(q: Quaternionf64) -> (angle: f64, axis: Vector3f64) { + angle = angle_from_quaternion(q); + axis = axis_from_quaternion(q); + return; +} +angle_axis_from_quaternion :: proc { + angle_axis_from_quaternion_f32, + angle_axis_from_quaternion_f64, +}; -quaternion_from_forward_and_up :: proc(forward, up: Vector3) -> Quaternion { + +quaternion_from_forward_and_up_f32 :: proc(forward, up: Vector3f32) -> Quaternionf32 { f := normalize(forward); s := normalize(cross(f, up)); u := cross(s, f); - m := Matrix3{ + m := Matrix3f32{ {+s.x, +u.x, -f.x}, {+s.y, +u.y, -f.y}, {+s.z, +u.z, -f.z}, @@ -216,7 +405,7 @@ quaternion_from_forward_and_up :: proc(forward, up: Vector3) -> Quaternion { tr := trace(m); - q: Quaternion; + q: Quaternionf32; switch { case tr > 0: @@ -247,29 +436,93 @@ quaternion_from_forward_and_up :: proc(forward, up: Vector3) -> Quaternion { return normalize(q); } +quaternion_from_forward_and_up_f64 :: proc(forward, up: Vector3f64) -> Quaternionf64 { + f := normalize(forward); + s := normalize(cross(f, up)); + u := cross(s, f); + m := Matrix3f64{ + {+s.x, +u.x, -f.x}, + {+s.y, +u.y, -f.y}, + {+s.z, +u.z, -f.z}, + }; -quaternion_look_at :: proc(eye, centre: Vector3, up: Vector3) -> Quaternion { + tr := trace(m); + + q: Quaternionf64; + + switch { + case tr > 0: + S := 2 * math.sqrt(1 + tr); + q.w = 0.25 * S; + q.x = (m[2][1] - m[1][2]) / S; + q.y = (m[0][2] - m[2][0]) / S; + q.z = (m[1][0] - m[0][1]) / S; + case (m[0][0] > m[1][1]) && (m[0][0] > m[2][2]): + S := 2 * math.sqrt(1 + m[0][0] - m[1][1] - m[2][2]); + q.w = (m[2][1] - m[1][2]) / S; + q.x = 0.25 * S; + q.y = (m[0][1] + m[1][0]) / S; + q.z = (m[0][2] + m[2][0]) / S; + case m[1][1] > m[2][2]: + S := 2 * math.sqrt(1 + m[1][1] - m[0][0] - m[2][2]); + q.w = (m[0][2] - m[2][0]) / S; + q.x = (m[0][1] + m[1][0]) / S; + q.y = 0.25 * S; + q.z = (m[1][2] + m[2][1]) / S; + case: + S := 2 * math.sqrt(1 + m[2][2] - m[0][0] - m[1][1]); + q.w = (m[1][0] - m[0][1]) / S; + q.x = (m[0][2] - m[2][0]) / S; + q.y = (m[1][2] + m[2][1]) / S; + q.z = 0.25 * S; + } + + return normalize(q); +} +quaternion_from_forward_and_up :: proc{ + quaternion_from_forward_and_up_f32, + quaternion_from_forward_and_up_f64, +}; + +quaternion_look_at_f32 :: proc(eye, centre: Vector3f32, up: Vector3f32) -> Quaternionf32 { return quaternion_from_matrix3(matrix3_look_at(eye, centre, up)); } +quaternion_look_at_f64 :: proc(eye, centre: Vector3f64, up: Vector3f64) -> Quaternionf64 { + return quaternion_from_matrix3(matrix3_look_at(eye, centre, up)); +} +quaternion_look_at :: proc{ + quaternion_look_at_f32, + quaternion_look_at_f64, +}; -quaternion_nlerp :: proc(a, b: Quaternion, t: Float) -> (c: Quaternion) { +quaternion_nlerp_f32 :: proc(a, b: Quaternionf32, t: f32) -> (c: Quaternionf32) { c.x = a.x + (b.x-a.x)*t; c.y = a.y + (b.y-a.y)*t; c.z = a.z + (b.z-a.z)*t; c.w = a.w + (b.w-a.w)*t; return normalize(c); } +quaternion_nlerp_f64 :: proc(a, b: Quaternionf64, t: f64) -> (c: Quaternionf64) { + c.x = a.x + (b.x-a.x)*t; + c.y = a.y + (b.y-a.y)*t; + c.z = a.z + (b.z-a.z)*t; + c.w = a.w + (b.w-a.w)*t; + return normalize(c); +} +quaternion_nlerp :: proc{ + quaternion_nlerp_f32, + quaternion_nlerp_f64, +}; - -quaternion_slerp :: proc(x, y: Quaternion, t: Float) -> (q: Quaternion) { +quaternion_slerp_f32 :: proc(x, y: Quaternionf32, t: f32) -> (q: Quaternionf32) { a, b := x, y; cos_angle := dot(a, b); if cos_angle < 0 { b = -b; cos_angle = -cos_angle; } - if cos_angle > 1 - FLOAT_EPSILON { + if cos_angle > 1 - F32_EPSILON { q.x = a.x + (b.x-a.x)*t; q.y = a.y + (b.y-a.y)*t; q.z = a.z + (b.z-a.z)*t; @@ -289,23 +542,71 @@ quaternion_slerp :: proc(x, y: Quaternion, t: Float) -> (q: Quaternion) { q.w = factor_a * a.w + factor_b * b.w; return; } +quaternion_slerp_f64 :: proc(x, y: Quaternionf64, t: f64) -> (q: Quaternionf64) { + a, b := x, y; + cos_angle := dot(a, b); + if cos_angle < 0 { + b = -b; + cos_angle = -cos_angle; + } + if cos_angle > 1 - F64_EPSILON { + q.x = a.x + (b.x-a.x)*t; + q.y = a.y + (b.y-a.y)*t; + q.z = a.z + (b.z-a.z)*t; + q.w = a.w + (b.w-a.w)*t; + return; + } -quaternion_squad :: proc(q1, q2, s1, s2: Quaternion, h: Float) -> Quaternion { + angle := math.acos(cos_angle); + sin_angle := math.sin(angle); + factor_a := math.sin((1-t) * angle) / sin_angle; + factor_b := math.sin(t * angle) / sin_angle; + + + q.x = factor_a * a.x + factor_b * b.x; + q.y = factor_a * a.y + factor_b * b.y; + q.z = factor_a * a.z + factor_b * b.z; + q.w = factor_a * a.w + factor_b * b.w; + return; +} +quaternion_slerp :: proc{ + quaternion_slerp_f32, + quaternion_slerp_f64, +}; + +quaternion_squad_f32 :: proc(q1, q2, s1, s2: Quaternionf32, h: f32) -> Quaternionf32 { slerp :: quaternion_slerp; return slerp(slerp(q1, q2, h), slerp(s1, s2, h), 2 * (1 - h) * h); } +quaternion_squad_f64 :: proc(q1, q2, s1, s2: Quaternionf64, h: f64) -> Quaternionf64 { + slerp :: quaternion_slerp; + return slerp(slerp(q1, q2, h), slerp(s1, s2, h), 2 * (1 - h) * h); +} +quaternion_squad :: proc{ + quaternion_squad_f32, + quaternion_squad_f64, +}; - -quaternion_from_matrix4 :: proc(m: Matrix4) -> (q: Quaternion) { - m3: Matrix3 = ---; +quaternion_from_matrix4_f32 :: proc(m: Matrix4f32) -> (q: Quaternionf32) { + m3: Matrix3f32 = ---; m3[0][0], m3[0][1], m3[0][2] = m[0][0], m[0][1], m[0][2]; m3[1][0], m3[1][1], m3[1][2] = m[1][0], m[1][1], m[1][2]; m3[2][0], m3[2][1], m3[2][2] = m[2][0], m[2][1], m[2][2]; return quaternion_from_matrix3(m3); } +quaternion_from_matrix4_f64 :: proc(m: Matrix4f64) -> (q: Quaternionf64) { + m3: Matrix3f64 = ---; + m3[0][0], m3[0][1], m3[0][2] = m[0][0], m[0][1], m[0][2]; + m3[1][0], m3[1][1], m3[1][2] = m[1][0], m[1][1], m[1][2]; + m3[2][0], m3[2][1], m3[2][2] = m[2][0], m[2][1], m[2][2]; + return quaternion_from_matrix3(m3); +} +quaternion_from_matrix4 :: proc{ + quaternion_from_matrix4_f32, + quaternion_from_matrix4_f64, +}; - -quaternion_from_matrix3 :: proc(m: Matrix3) -> (q: Quaternion) { +quaternion_from_matrix3_f32 :: proc(m: Matrix3f32) -> (q: Quaternionf32) { four_x_squared_minus_1 := m[0][0] - m[1][1] - m[2][2]; four_y_squared_minus_1 := m[1][1] - m[0][0] - m[2][2]; four_z_squared_minus_1 := m[2][2] - m[0][0] - m[1][1]; @@ -354,13 +655,66 @@ quaternion_from_matrix3 :: proc(m: Matrix3) -> (q: Quaternion) { } return; } +quaternion_from_matrix3_f64 :: proc(m: Matrix3f64) -> (q: Quaternionf64) { + four_x_squared_minus_1 := m[0][0] - m[1][1] - m[2][2]; + four_y_squared_minus_1 := m[1][1] - m[0][0] - m[2][2]; + four_z_squared_minus_1 := m[2][2] - m[0][0] - m[1][1]; + four_w_squared_minus_1 := m[0][0] + m[1][1] + m[2][2]; -quaternion_between_two_vector3 :: proc(from, to: Vector3) -> (q: Quaternion) { + biggest_index := 0; + four_biggest_squared_minus_1 := four_w_squared_minus_1; + if four_x_squared_minus_1 > four_biggest_squared_minus_1 { + four_biggest_squared_minus_1 = four_x_squared_minus_1; + biggest_index = 1; + } + if four_y_squared_minus_1 > four_biggest_squared_minus_1 { + four_biggest_squared_minus_1 = four_y_squared_minus_1; + biggest_index = 2; + } + if four_z_squared_minus_1 > four_biggest_squared_minus_1 { + four_biggest_squared_minus_1 = four_z_squared_minus_1; + biggest_index = 3; + } + + biggest_val := math.sqrt(four_biggest_squared_minus_1 + 1) * 0.5; + mult := 0.25 / biggest_val; + + q = 1; + switch biggest_index { + case 0: + q.w = biggest_val; + q.x = (m[1][2] - m[2][1]) * mult; + q.y = (m[2][0] - m[0][2]) * mult; + q.z = (m[0][1] - m[1][0]) * mult; + case 1: + q.w = (m[1][2] - m[2][1]) * mult; + q.x = biggest_val; + q.y = (m[0][1] + m[1][0]) * mult; + q.z = (m[2][0] + m[0][2]) * mult; + case 2: + q.w = (m[2][0] - m[0][2]) * mult; + q.x = (m[0][1] + m[1][0]) * mult; + q.y = biggest_val; + q.z = (m[1][2] + m[2][1]) * mult; + case 3: + q.w = (m[0][1] - m[1][0]) * mult; + q.x = (m[2][0] + m[0][2]) * mult; + q.y = (m[1][2] + m[2][1]) * mult; + q.z = biggest_val; + } + return; +} +quaternion_from_matrix3 :: proc{ + quaternion_from_matrix3_f32, + quaternion_from_matrix3_f64, +}; + +quaternion_between_two_vector3_f32 :: proc(from, to: Vector3f32) -> (q: Quaternionf32) { x := normalize(from); y := normalize(to); cos_theta := dot(x, y); - if abs(cos_theta + 1) < 2*FLOAT_EPSILON { + if abs(cos_theta + 1) < 2*F32_EPSILON { v := vector3_orthogonal(x); q.x = v.x; q.y = v.y; @@ -376,9 +730,33 @@ quaternion_between_two_vector3 :: proc(from, to: Vector3) -> (q: Quaternion) { q.z = v.z; return normalize(q); } +quaternion_between_two_vector3_f64 :: proc(from, to: Vector3f64) -> (q: Quaternionf64) { + x := normalize(from); + y := normalize(to); + cos_theta := dot(x, y); + if abs(cos_theta + 1) < 2*F64_EPSILON { + v := vector3_orthogonal(x); + q.x = v.x; + q.y = v.y; + q.z = v.z; + q.w = 0; + return; + } + v := cross(x, y); + w := cos_theta + 1; + q.w = w; + q.x = v.x; + q.y = v.y; + q.z = v.z; + return normalize(q); +} +quaternion_between_two_vector3 :: proc{ + quaternion_between_two_vector3_f32, + quaternion_between_two_vector3_f64, +}; -matrix2_inverse_transpose :: proc(m: Matrix2) -> (c: Matrix2) { +matrix2_inverse_transpose_f32 :: proc(m: Matrix2f32) -> (c: Matrix2f32) { d := m[0][0]*m[1][1] - m[1][0]*m[0][1]; id := 1.0/d; c[0][0] = +m[1][1] * id; @@ -387,10 +765,32 @@ matrix2_inverse_transpose :: proc(m: Matrix2) -> (c: Matrix2) { c[1][1] = +m[0][0] * id; return c; } -matrix2_determinant :: proc(m: Matrix2) -> Float { +matrix2_inverse_transpose_f64 :: proc(m: Matrix2f64) -> (c: Matrix2f64) { + d := m[0][0]*m[1][1] - m[1][0]*m[0][1]; + id := 1.0/d; + c[0][0] = +m[1][1] * id; + c[0][1] = -m[0][1] * id; + c[1][0] = -m[1][0] * id; + c[1][1] = +m[0][0] * id; + return c; +} +matrix2_inverse_transpose :: proc{ + matrix2_inverse_transpose_f32, + matrix2_inverse_transpose_f64, +}; + +matrix2_determinant_f32 :: proc(m: Matrix2f32) -> f32 { return m[0][0]*m[1][1] - m[1][0]*m[0][1]; } -matrix2_inverse :: proc(m: Matrix2) -> (c: Matrix2) { +matrix2_determinant_f64 :: proc(m: Matrix2f64) -> f64 { + return m[0][0]*m[1][1] - m[1][0]*m[0][1]; +} +matrix2_determinant :: proc{ + matrix2_determinant_f32, + matrix2_determinant_f64, +}; + +matrix2_inverse_f32 :: proc(m: Matrix2f32) -> (c: Matrix2f32) { d := m[0][0]*m[1][1] - m[1][0]*m[0][1]; id := 1.0/d; c[0][0] = +m[1][1] * id; @@ -399,17 +799,40 @@ matrix2_inverse :: proc(m: Matrix2) -> (c: Matrix2) { c[1][1] = +m[0][0] * id; return c; } +matrix2_inverse_f64 :: proc(m: Matrix2f64) -> (c: Matrix2f64) { + d := m[0][0]*m[1][1] - m[1][0]*m[0][1]; + id := 1.0/d; + c[0][0] = +m[1][1] * id; + c[1][0] = -m[0][1] * id; + c[0][1] = -m[1][0] * id; + c[1][1] = +m[0][0] * id; + return c; +} +matrix2_inverse :: proc{ + matrix2_inverse_f32, + matrix2_inverse_f64, +}; -matrix2_adjoint :: proc(m: Matrix2) -> (c: Matrix2) { +matrix2_adjoint_f32 :: proc(m: Matrix2f32) -> (c: Matrix2f32) { c[0][0] = +m[1][1]; c[0][1] = -m[1][0]; c[1][0] = -m[0][1]; c[1][1] = +m[0][0]; return c; } +matrix2_adjoint_f64 :: proc(m: Matrix2f64) -> (c: Matrix2f64) { + c[0][0] = +m[1][1]; + c[0][1] = -m[1][0]; + c[1][0] = -m[0][1]; + c[1][1] = +m[0][0]; + return c; +} +matrix2_adjoint :: proc{ + matrix2_adjoint_f32, + matrix2_adjoint_f64, +}; - -matrix3_from_quaternion :: proc(q: Quaternion) -> (m: Matrix3) { +matrix3_from_quaternion_f32 :: proc(q: Quaternionf32) -> (m: Matrix3f32) { qxx := q.x * q.x; qyy := q.y * q.y; qzz := q.z * q.z; @@ -433,20 +856,64 @@ matrix3_from_quaternion :: proc(q: Quaternion) -> (m: Matrix3) { m[2][2] = 1 - 2 * (qxx + qyy); return m; } +matrix3_from_quaternion_f64 :: proc(q: Quaternionf64) -> (m: Matrix3f64) { + qxx := q.x * q.x; + qyy := q.y * q.y; + qzz := q.z * q.z; + qxz := q.x * q.z; + qxy := q.x * q.y; + qyz := q.y * q.z; + qwx := q.w * q.x; + qwy := q.w * q.y; + qwz := q.w * q.z; -matrix3_inverse :: proc(m: Matrix3) -> Matrix3 { + m[0][0] = 1 - 2 * (qyy + qzz); + m[0][1] = 2 * (qxy + qwz); + m[0][2] = 2 * (qxz - qwy); + + m[1][0] = 2 * (qxy - qwz); + m[1][1] = 1 - 2 * (qxx + qzz); + m[1][2] = 2 * (qyz + qwx); + + m[2][0] = 2 * (qxz + qwy); + m[2][1] = 2 * (qyz - qwx); + m[2][2] = 1 - 2 * (qxx + qyy); + return m; +} +matrix3_from_quaternion :: proc{ + matrix3_from_quaternion_f32, + matrix3_from_quaternion_f64, +}; + +matrix3_inverse_f32 :: proc(m: Matrix3f32) -> Matrix3f32 { return transpose(matrix3_inverse_transpose(m)); } +matrix3_inverse_f64 :: proc(m: Matrix3f64) -> Matrix3f64 { + return transpose(matrix3_inverse_transpose(m)); +} +matrix3_inverse :: proc{ + matrix3_inverse_f32, + matrix3_inverse_f64, +}; - -matrix3_determinant :: proc(m: Matrix3) -> Float { +matrix3_determinant_f32 :: proc(m: Matrix3f32) -> f32 { a := +m[0][0] * (m[1][1] * m[2][2] - m[2][1] * m[1][2]); b := -m[1][0] * (m[0][1] * m[2][2] - m[2][1] * m[0][2]); c := +m[2][0] * (m[0][1] * m[1][2] - m[1][1] * m[0][2]); return a + b + c; } +matrix3_determinant_f64 :: proc(m: Matrix3f64) -> f64 { + a := +m[0][0] * (m[1][1] * m[2][2] - m[2][1] * m[1][2]); + b := -m[1][0] * (m[0][1] * m[2][2] - m[2][1] * m[0][2]); + c := +m[2][0] * (m[0][1] * m[1][2] - m[1][1] * m[0][2]); + return a + b + c; +} +matrix3_determinant :: proc{ + matrix3_determinant_f32, + matrix3_determinant_f64, +}; -matrix3_adjoint :: proc(m: Matrix3) -> (adjoint: Matrix3) { +matrix3_adjoint_f32 :: proc(m: Matrix3f32) -> (adjoint: Matrix3f32) { adjoint[0][0] = +(m[1][1] * m[2][2] - m[1][2] * m[2][1]); adjoint[1][0] = -(m[0][1] * m[2][2] - m[0][2] * m[2][1]); adjoint[2][0] = +(m[0][1] * m[1][2] - m[0][2] * m[1][1]); @@ -458,10 +925,24 @@ matrix3_adjoint :: proc(m: Matrix3) -> (adjoint: Matrix3) { adjoint[2][2] = +(m[0][0] * m[1][1] - m[0][1] * m[1][0]); return adjoint; } +matrix3_adjoint_f64 :: proc(m: Matrix3f64) -> (adjoint: Matrix3f64) { + adjoint[0][0] = +(m[1][1] * m[2][2] - m[1][2] * m[2][1]); + adjoint[1][0] = -(m[0][1] * m[2][2] - m[0][2] * m[2][1]); + adjoint[2][0] = +(m[0][1] * m[1][2] - m[0][2] * m[1][1]); + adjoint[0][1] = -(m[1][0] * m[2][2] - m[1][2] * m[2][0]); + adjoint[1][1] = +(m[0][0] * m[2][2] - m[0][2] * m[2][0]); + adjoint[2][1] = -(m[0][0] * m[1][2] - m[0][2] * m[1][0]); + adjoint[0][2] = +(m[1][0] * m[2][1] - m[1][1] * m[2][0]); + adjoint[1][2] = -(m[0][0] * m[2][1] - m[0][1] * m[2][0]); + adjoint[2][2] = +(m[0][0] * m[1][1] - m[0][1] * m[1][0]); + return adjoint; +} +matrix3_adjoint :: proc{ + matrix3_adjoint_f32, + matrix3_adjoint_f64, +}; -matrix3_inverse_transpose :: proc(m: Matrix3) -> Matrix3 { - inverse_transpose: Matrix3; - +matrix3_inverse_transpose_f32 :: proc(m: Matrix3f32) -> (inverse_transpose: Matrix3f32) { adjoint := matrix3_adjoint(m); determinant := matrix3_determinant(m); inv_determinant := 1.0 / determinant; @@ -470,26 +951,48 @@ matrix3_inverse_transpose :: proc(m: Matrix3) -> Matrix3 { inverse_transpose[i][j] = adjoint[i][j] * inv_determinant; } } - return inverse_transpose; + return; } +matrix3_inverse_transpose_f64 :: proc(m: Matrix3f64) -> (inverse_transpose: Matrix3f64) { + adjoint := matrix3_adjoint(m); + determinant := matrix3_determinant(m); + inv_determinant := 1.0 / determinant; + for i in 0..<3 { + for j in 0..<3 { + inverse_transpose[i][j] = adjoint[i][j] * inv_determinant; + } + } + return; +} +matrix3_inverse_transpose :: proc{ + matrix3_inverse_transpose_f32, + matrix3_inverse_transpose_f64, +}; - -matrix3_scale :: proc(s: Vector3) -> (m: Matrix3) { +matrix3_scale_f32 :: proc(s: Vector3f32) -> (m: Matrix3f32) { m[0][0] = s[0]; m[1][1] = s[1]; m[2][2] = s[2]; return m; } +matrix3_scale_f64 :: proc(s: Vector3f64) -> (m: Matrix3f64) { + m[0][0] = s[0]; + m[1][1] = s[1]; + m[2][2] = s[2]; + return m; +} +matrix3_scale :: proc{ + matrix3_scale_f32, + matrix3_scale_f64, +}; -matrix3_rotate :: proc(angle_radians: Float, v: Vector3) -> Matrix3 { +matrix3_rotate_f32 :: proc(angle_radians: f32, v: Vector3f32) -> (rot: Matrix3f32) { c := math.cos(angle_radians); s := math.sin(angle_radians); a := normalize(v); t := a * (1-c); - rot: Matrix3 = ---; - rot[0][0] = c + t[0]*a[0]; rot[0][1] = 0 + t[0]*a[1] + s*a[2]; rot[0][2] = 0 + t[0]*a[2] - s*a[1]; @@ -504,19 +1007,58 @@ matrix3_rotate :: proc(angle_radians: Float, v: Vector3) -> Matrix3 { return rot; } +matrix3_rotate_f64 :: proc(angle_radians: f64, v: Vector3f64) -> (rot: Matrix3f64) { + c := math.cos(angle_radians); + s := math.sin(angle_radians); -matrix3_look_at :: proc(eye, centre, up: Vector3) -> Matrix3 { + a := normalize(v); + t := a * (1-c); + + rot[0][0] = c + t[0]*a[0]; + rot[0][1] = 0 + t[0]*a[1] + s*a[2]; + rot[0][2] = 0 + t[0]*a[2] - s*a[1]; + + rot[1][0] = 0 + t[1]*a[0] - s*a[2]; + rot[1][1] = c + t[1]*a[1]; + rot[1][2] = 0 + t[1]*a[2] + s*a[0]; + + rot[2][0] = 0 + t[2]*a[0] + s*a[1]; + rot[2][1] = 0 + t[2]*a[1] - s*a[0]; + rot[2][2] = c + t[2]*a[2]; + + return rot; +} +matrix3_rotate :: proc{ + matrix3_rotate_f32, + matrix3_rotate_f64, +}; + +matrix3_look_at_f32 :: proc(eye, centre, up: Vector3f32) -> Matrix3f32 { f := normalize(centre - eye); s := normalize(cross(f, up)); u := cross(s, f); - return Matrix3{ + return Matrix3f32{ {+s.x, +u.x, -f.x}, {+s.y, +u.y, -f.y}, {+s.z, +u.z, -f.z}, }; } +matrix3_look_at_f64 :: proc(eye, centre, up: Vector3f64) -> Matrix3f64 { + f := normalize(centre - eye); + s := normalize(cross(f, up)); + u := cross(s, f); + return Matrix3f64{ + {+s.x, +u.x, -f.x}, + {+s.y, +u.y, -f.y}, + {+s.z, +u.z, -f.z}, + }; +} +matrix3_look_at :: proc{ + matrix3_look_at_f32, + matrix3_look_at_f64, +}; -matrix4_from_quaternion :: proc(q: Quaternion) -> (m: Matrix4) { +matrix4_from_quaternion_f32 :: proc(q: Quaternionf32) -> (m: Matrix4f32) { qxx := q.x * q.x; qyy := q.y * q.y; qzz := q.z * q.z; @@ -543,22 +1085,69 @@ matrix4_from_quaternion :: proc(q: Quaternion) -> (m: Matrix4) { return m; } +matrix4_from_quaternion_f64 :: proc(q: Quaternionf64) -> (m: Matrix4f64) { + qxx := q.x * q.x; + qyy := q.y * q.y; + qzz := q.z * q.z; + qxz := q.x * q.z; + qxy := q.x * q.y; + qyz := q.y * q.z; + qwx := q.w * q.x; + qwy := q.w * q.y; + qwz := q.w * q.z; -matrix4_from_trs :: proc(t: Vector3, r: Quaternion, s: Vector3) -> Matrix4 { + m[0][0] = 1 - 2 * (qyy + qzz); + m[0][1] = 2 * (qxy + qwz); + m[0][2] = 2 * (qxz - qwy); + + m[1][0] = 2 * (qxy - qwz); + m[1][1] = 1 - 2 * (qxx + qzz); + m[1][2] = 2 * (qyz + qwx); + + m[2][0] = 2 * (qxz + qwy); + m[2][1] = 2 * (qyz - qwx); + m[2][2] = 1 - 2 * (qxx + qyy); + + m[3][3] = 1; + + return m; +} +matrix4_from_quaternion :: proc{ + matrix4_from_quaternion_f32, + matrix4_from_quaternion_f64, +}; + +matrix4_from_trs_f32 :: proc(t: Vector3f32, r: Quaternionf32, s: Vector3f32) -> Matrix4f32 { translation := matrix4_translate(t); rotation := matrix4_from_quaternion(r); scale := matrix4_scale(s); return mul(translation, mul(rotation, scale)); } +matrix4_from_trs_f64 :: proc(t: Vector3f64, r: Quaternionf64, s: Vector3f64) -> Matrix4f64 { + translation := matrix4_translate(t); + rotation := matrix4_from_quaternion(r); + scale := matrix4_scale(s); + return mul(translation, mul(rotation, scale)); +} +matrix4_from_trs :: proc{ + matrix4_from_trs_f32, + matrix4_from_trs_f64, +}; -matrix4_inverse :: proc(m: Matrix4) -> Matrix4 { +matrix4_inverse_f32 :: proc(m: Matrix4f32) -> Matrix4f32 { return transpose(matrix4_inverse_transpose(m)); } +matrix4_inverse_f64 :: proc(m: Matrix4f64) -> Matrix4f64 { + return transpose(matrix4_inverse_transpose(m)); +} +matrix4_inverse :: proc{ + matrix4_inverse_f32, + matrix4_inverse_f64, +}; - -matrix4_minor :: proc(m: Matrix4, c, r: int) -> Float { - cut_down: Matrix3; +matrix4_minor_f32 :: proc(m: Matrix4f32, c, r: int) -> f32 { + cut_down: Matrix3f32; for i in 0..<3 { col := i if i < c else i+1; for j in 0..<3 { @@ -568,67 +1157,139 @@ matrix4_minor :: proc(m: Matrix4, c, r: int) -> Float { } return matrix3_determinant(cut_down); } +matrix4_minor_f64 :: proc(m: Matrix4f64, c, r: int) -> f64 { + cut_down: Matrix3f64; + for i in 0..<3 { + col := i if i < c else i+1; + for j in 0..<3 { + row := j if j < r else j+1; + cut_down[i][j] = m[col][row]; + } + } + return matrix3_determinant(cut_down); +} +matrix4_minor :: proc{ + matrix4_minor_f32, + matrix4_minor_f64, +}; -matrix4_cofactor :: proc(m: Matrix4, c, r: int) -> Float { - sign, minor: Float; +matrix4_cofactor_f32 :: proc(m: Matrix4f32, c, r: int) -> f32 { + sign, minor: f32; sign = 1 if (c + r) % 2 == 0 else -1; minor = matrix4_minor(m, c, r); return sign * minor; } +matrix4_cofactor_f64 :: proc(m: Matrix4f64, c, r: int) -> f64 { + sign, minor: f64; + sign = 1 if (c + r) % 2 == 0 else -1; + minor = matrix4_minor(m, c, r); + return sign * minor; +} +matrix4_cofactor :: proc{ + matrix4_cofactor_f32, + matrix4_cofactor_f64, +}; -matrix4_adjoint :: proc(m: Matrix4) -> Matrix4 { - adjoint: Matrix4; +matrix4_adjoint_f32 :: proc(m: Matrix4f32) -> (adjoint: Matrix4f32) { for i in 0..<4 { for j in 0..<4 { adjoint[i][j] = matrix4_cofactor(m, i, j); } } - return adjoint; + return; } +matrix4_adjoint_f64 :: proc(m: Matrix4f64) -> (adjoint: Matrix4f64) { + for i in 0..<4 { + for j in 0..<4 { + adjoint[i][j] = matrix4_cofactor(m, i, j); + } + } + return; +} +matrix4_adjoint :: proc{ + matrix4_adjoint_f32, + matrix4_adjoint_f64, +}; -matrix4_determinant :: proc(m: Matrix4) -> Float { +matrix4_determinant_f32 :: proc(m: Matrix4f32) -> (determinant: f32) { adjoint := matrix4_adjoint(m); - determinant: Float = 0; for i in 0..<4 { determinant += m[i][0] * adjoint[i][0]; } - return determinant; - + return; } - -matrix4_inverse_transpose :: proc(m: Matrix4) -> Matrix4 { +matrix4_determinant_f64 :: proc(m: Matrix4f64) -> (determinant: f64) { adjoint := matrix4_adjoint(m); - determinant: Float = 0; + for i in 0..<4 { + determinant += m[i][0] * adjoint[i][0]; + } + return; +} +matrix4_determinant :: proc{ + matrix4_determinant_f32, + matrix4_determinant_f64, +}; + +matrix4_inverse_transpose_f32 :: proc(m: Matrix4f32) -> (inverse_transpose: Matrix4f32) { + adjoint := matrix4_adjoint(m); + determinant: f32 = 0; for i in 0..<4 { determinant += m[i][0] * adjoint[i][0]; } inv_determinant := 1.0 / determinant; - inverse_transpose: Matrix4; for i in 0..<4 { for j in 0..<4 { inverse_transpose[i][j] = adjoint[i][j] * inv_determinant; } } - return inverse_transpose; + return; } +matrix4_inverse_transpose_f64 :: proc(m: Matrix4f64) -> (inverse_transpose: Matrix4f64) { + adjoint := matrix4_adjoint(m); + determinant: f64 = 0; + for i in 0..<4 { + determinant += m[i][0] * adjoint[i][0]; + } + inv_determinant := 1.0 / determinant; + for i in 0..<4 { + for j in 0..<4 { + inverse_transpose[i][j] = adjoint[i][j] * inv_determinant; + } + } + return; +} +matrix4_inverse_transpose :: proc{ + matrix4_inverse_transpose_f32, + matrix4_inverse_transpose_f64, +}; -matrix4_translate :: proc(v: Vector3) -> Matrix4 { - m := MATRIX4_IDENTITY; +matrix4_translate_f32 :: proc(v: Vector3f32) -> Matrix4f32 { + m := MATRIX4F32_IDENTITY; m[3][0] = v[0]; m[3][1] = v[1]; m[3][2] = v[2]; return m; } +matrix4_translate_f64 :: proc(v: Vector3f64) -> Matrix4f64 { + m := MATRIX4F64_IDENTITY; + m[3][0] = v[0]; + m[3][1] = v[1]; + m[3][2] = v[2]; + return m; +} +matrix4_translate :: proc{ + matrix4_translate_f32, + matrix4_translate_f64, +}; - -matrix4_rotate :: proc(angle_radians: Float, v: Vector3) -> Matrix4 { +matrix4_rotate_f32 :: proc(angle_radians: f32, v: Vector3f32) -> Matrix4f32 { c := math.cos(angle_radians); s := math.sin(angle_radians); a := normalize(v); t := a * (1-c); - rot := MATRIX4_IDENTITY; + rot := MATRIX4F32_IDENTITY; rot[0][0] = c + t[0]*a[0]; rot[0][1] = 0 + t[0]*a[1] + s*a[2]; @@ -647,34 +1308,91 @@ matrix4_rotate :: proc(angle_radians: Float, v: Vector3) -> Matrix4 { return rot; } +matrix4_rotate_f64 :: proc(angle_radians: f64, v: Vector3f64) -> Matrix4f64 { + c := math.cos(angle_radians); + s := math.sin(angle_radians); -matrix4_scale :: proc(v: Vector3) -> Matrix4 { - m: Matrix4; + a := normalize(v); + t := a * (1-c); + + rot := MATRIX4F64_IDENTITY; + + rot[0][0] = c + t[0]*a[0]; + rot[0][1] = 0 + t[0]*a[1] + s*a[2]; + rot[0][2] = 0 + t[0]*a[2] - s*a[1]; + rot[0][3] = 0; + + rot[1][0] = 0 + t[1]*a[0] - s*a[2]; + rot[1][1] = c + t[1]*a[1]; + rot[1][2] = 0 + t[1]*a[2] + s*a[0]; + rot[1][3] = 0; + + rot[2][0] = 0 + t[2]*a[0] + s*a[1]; + rot[2][1] = 0 + t[2]*a[1] - s*a[0]; + rot[2][2] = c + t[2]*a[2]; + rot[2][3] = 0; + + return rot; +} +matrix4_rotate :: proc{ + matrix4_rotate_f32, + matrix4_rotate_f64, +}; + +matrix4_scale_f32 :: proc(v: Vector3f32) -> (m: Matrix4f32) { m[0][0] = v[0]; m[1][1] = v[1]; m[2][2] = v[2]; m[3][3] = 1; - return m; + return; } +matrix4_scale_f64 :: proc(v: Vector3f64) -> (m: Matrix4f64) { + m[0][0] = v[0]; + m[1][1] = v[1]; + m[2][2] = v[2]; + m[3][3] = 1; + return; +} +matrix4_scale :: proc{ + matrix4_scale_f32, + matrix4_scale_f64, +}; -matrix4_look_at :: proc(eye, centre, up: Vector3, flip_z_axis := true) -> Matrix4 { +matrix4_look_at_f32 :: proc(eye, centre, up: Vector3f32, flip_z_axis := true) -> (m: Matrix4f32) { f := normalize(centre - eye); s := normalize(cross(f, up)); u := cross(s, f); fe := dot(f, eye); - m := Matrix4{ + return { {+s.x, +u.x, -f.x, 0}, {+s.y, +u.y, -f.y, 0}, {+s.z, +u.z, -f.z, 0}, {-dot(s, eye), -dot(u, eye), +fe if flip_z_axis else -fe, 1}, }; - return m; } +matrix4_look_at_f64 :: proc(eye, centre, up: Vector3f64, flip_z_axis := true) -> (m: Matrix4f64) { + f := normalize(centre - eye); + s := normalize(cross(f, up)); + u := cross(s, f); + + fe := dot(f, eye); + + return { + {+s.x, +u.x, -f.x, 0}, + {+s.y, +u.y, -f.y, 0}, + {+s.z, +u.z, -f.z, 0}, + {-dot(s, eye), -dot(u, eye), +fe if flip_z_axis else -fe, 1}, + }; +} +matrix4_look_at :: proc{ + matrix4_look_at_f32, + matrix4_look_at_f64, +}; -matrix4_perspective :: proc(fovy, aspect, near, far: Float, flip_z_axis := true) -> (m: Matrix4) { +matrix4_perspective_f32 :: proc(fovy, aspect, near, far: f32, flip_z_axis := true) -> (m: Matrix4f32) { tan_half_fovy := math.tan(0.5 * fovy); m[0][0] = 1 / (aspect*tan_half_fovy); m[1][1] = 1 / (tan_half_fovy); @@ -688,9 +1406,26 @@ matrix4_perspective :: proc(fovy, aspect, near, far: Float, flip_z_axis := true) return; } +matrix4_perspective_f64 :: proc(fovy, aspect, near, far: f64, flip_z_axis := true) -> (m: Matrix4f64) { + tan_half_fovy := math.tan(0.5 * fovy); + m[0][0] = 1 / (aspect*tan_half_fovy); + m[1][1] = 1 / (tan_half_fovy); + m[2][2] = +(far + near) / (far - near); + m[2][3] = +1; + m[3][2] = -2*far*near / (far - near); + if flip_z_axis { + m[2] = -m[2]; + } -matrix_ortho3d :: proc(left, right, bottom, top, near, far: Float, flip_z_axis := true) -> (m: Matrix4) { + return; +} +matrix4_perspective :: proc{ + matrix4_perspective_f32, + matrix4_perspective_f64, +}; + +matrix_ortho3d_f32 :: proc(left, right, bottom, top, near, far: f32, flip_z_axis := true) -> (m: Matrix4f32) { m[0][0] = +2 / (right - left); m[1][1] = +2 / (top - bottom); m[2][2] = +2 / (far - near); @@ -705,9 +1440,27 @@ matrix_ortho3d :: proc(left, right, bottom, top, near, far: Float, flip_z_axis : return; } +matrix_ortho3d_f64 :: proc(left, right, bottom, top, near, far: f64, flip_z_axis := true) -> (m: Matrix4f64) { + m[0][0] = +2 / (right - left); + m[1][1] = +2 / (top - bottom); + m[2][2] = +2 / (far - near); + m[3][0] = -(right + left) / (right - left); + m[3][1] = -(top + bottom) / (top - bottom); + m[3][2] = -(far + near) / (far- near); + m[3][3] = 1; + if flip_z_axis { + m[2] = -m[2]; + } -matrix4_infinite_perspective :: proc(fovy, aspect, near: Float, flip_z_axis := true) -> (m: Matrix4) { + return; +} +matrix_ortho3d :: proc{ + matrix_ortho3d_f32, + matrix_ortho3d_f64, +}; + +matrix4_infinite_perspective_f32 :: proc(fovy, aspect, near: f32, flip_z_axis := true) -> (m: Matrix4f32) { tan_half_fovy := math.tan(0.5 * fovy); m[0][0] = 1 / (aspect*tan_half_fovy); m[1][1] = 1 / (tan_half_fovy); @@ -721,83 +1474,222 @@ matrix4_infinite_perspective :: proc(fovy, aspect, near: Float, flip_z_axis := t return; } +matrix4_infinite_perspective_f64 :: proc(fovy, aspect, near: f64, flip_z_axis := true) -> (m: Matrix4f64) { + tan_half_fovy := math.tan(0.5 * fovy); + m[0][0] = 1 / (aspect*tan_half_fovy); + m[1][1] = 1 / (tan_half_fovy); + m[2][2] = +1; + m[2][3] = +1; + m[3][2] = -2*near; + + if flip_z_axis { + m[2] = -m[2]; + } + + return; +} +matrix4_infinite_perspective :: proc{ + matrix4_infinite_perspective_f32, + matrix4_infinite_perspective_f64, +}; -matrix2_from_scalar :: proc(f: Float) -> (m: Matrix2) { +matrix2_from_scalar_f32 :: proc(f: f32) -> (m: Matrix2f32) { m[0][0], m[0][1] = f, 0; m[1][0], m[1][1] = 0, f; return; } +matrix2_from_scalar_f64 :: proc(f: f64) -> (m: Matrix2f64) { + m[0][0], m[0][1] = f, 0; + m[1][0], m[1][1] = 0, f; + return; +} +matrix2_from_scalar :: proc{ + matrix2_from_scalar_f32, + matrix2_from_scalar_f64, +}; -matrix3_from_scalar :: proc(f: Float) -> (m: Matrix3) { +matrix3_from_scalar_f32 :: proc(f: f32) -> (m: Matrix3f32) { m[0][0], m[0][1], m[0][2] = f, 0, 0; m[1][0], m[1][1], m[1][2] = 0, f, 0; m[2][0], m[2][1], m[2][2] = 0, 0, f; return; } +matrix3_from_scalar_f64 :: proc(f: f64) -> (m: Matrix3f64) { + m[0][0], m[0][1], m[0][2] = f, 0, 0; + m[1][0], m[1][1], m[1][2] = 0, f, 0; + m[2][0], m[2][1], m[2][2] = 0, 0, f; + return; +} +matrix3_from_scalar :: proc{ + matrix3_from_scalar_f32, + matrix3_from_scalar_f64, +}; -matrix4_from_scalar :: proc(f: Float) -> (m: Matrix4) { +matrix4_from_scalar_f32 :: proc(f: f32) -> (m: Matrix4f32) { m[0][0], m[0][1], m[0][2], m[0][3] = f, 0, 0, 0; m[1][0], m[1][1], m[1][2], m[1][3] = 0, f, 0, 0; m[2][0], m[2][1], m[2][2], m[2][3] = 0, 0, f, 0; m[3][0], m[3][1], m[3][2], m[3][3] = 0, 0, 0, f; return; } +matrix4_from_scalar_f64 :: proc(f: f64) -> (m: Matrix4f64) { + m[0][0], m[0][1], m[0][2], m[0][3] = f, 0, 0, 0; + m[1][0], m[1][1], m[1][2], m[1][3] = 0, f, 0, 0; + m[2][0], m[2][1], m[2][2], m[2][3] = 0, 0, f, 0; + m[3][0], m[3][1], m[3][2], m[3][3] = 0, 0, 0, f; + return; +} +matrix4_from_scalar :: proc{ + matrix4_from_scalar_f32, + matrix4_from_scalar_f64, +}; -matrix2_from_matrix3 :: proc(m: Matrix3) -> (r: Matrix2) { +matrix2_from_matrix3_f32 :: proc(m: Matrix3f32) -> (r: Matrix2f32) { r[0][0], r[0][1] = m[0][0], m[0][1]; r[1][0], r[1][1] = m[1][0], m[1][1]; return; } - -matrix2_from_matrix4 :: proc(m: Matrix4) -> (r: Matrix2) { +matrix2_from_matrix3_f64 :: proc(m: Matrix3f64) -> (r: Matrix2f64) { r[0][0], r[0][1] = m[0][0], m[0][1]; r[1][0], r[1][1] = m[1][0], m[1][1]; return; } +matrix2_from_matrix3 :: proc{ + matrix2_from_matrix3_f32, + matrix2_from_matrix3_f64, +}; -matrix3_from_matrix2 :: proc(m: Matrix2) -> (r: Matrix3) { +matrix2_from_matrix4_f32 :: proc(m: Matrix4f32) -> (r: Matrix2f32) { + r[0][0], r[0][1] = m[0][0], m[0][1]; + r[1][0], r[1][1] = m[1][0], m[1][1]; + return; +} +matrix2_from_matrix4_f64 :: proc(m: Matrix4f64) -> (r: Matrix2f64) { + r[0][0], r[0][1] = m[0][0], m[0][1]; + r[1][0], r[1][1] = m[1][0], m[1][1]; + return; +} +matrix2_from_matrix4 :: proc{ + matrix2_from_matrix4_f32, + matrix2_from_matrix4_f64, +}; + +matrix3_from_matrix2_f32 :: proc(m: Matrix2f32) -> (r: Matrix3f32) { r[0][0], r[0][1], r[0][2] = m[0][0], m[0][1], 0; r[1][0], r[1][1], r[1][2] = m[1][0], m[1][1], 0; r[2][0], r[2][1], r[2][2] = 0, 0, 1; return; } +matrix3_from_matrix2_f64 :: proc(m: Matrix2f64) -> (r: Matrix3f64) { + r[0][0], r[0][1], r[0][2] = m[0][0], m[0][1], 0; + r[1][0], r[1][1], r[1][2] = m[1][0], m[1][1], 0; + r[2][0], r[2][1], r[2][2] = 0, 0, 1; + return; +} +matrix3_from_matrix2 :: proc{ + matrix3_from_matrix2_f32, + matrix3_from_matrix2_f64, +}; -matrix3_from_matrix4 :: proc(m: Matrix4) -> (r: Matrix3) { +matrix3_from_matrix4_f32 :: proc(m: Matrix4f32) -> (r: Matrix3f32) { r[0][0], r[0][1], r[0][2] = m[0][0], m[0][1], m[0][2]; r[1][0], r[1][1], r[1][2] = m[1][0], m[1][1], m[1][2]; r[2][0], r[2][1], r[2][2] = m[2][0], m[2][1], m[2][2]; return; } +matrix3_from_matrix4_f64 :: proc(m: Matrix4f64) -> (r: Matrix3f64) { + r[0][0], r[0][1], r[0][2] = m[0][0], m[0][1], m[0][2]; + r[1][0], r[1][1], r[1][2] = m[1][0], m[1][1], m[1][2]; + r[2][0], r[2][1], r[2][2] = m[2][0], m[2][1], m[2][2]; + return; +} +matrix3_from_matrix4 :: proc{ + matrix3_from_matrix4_f32, + matrix3_from_matrix4_f64, +}; -matrix4_from_matrix2 :: proc(m: Matrix2) -> (r: Matrix4) { +matrix4_from_matrix2_f32 :: proc(m: Matrix2f32) -> (r: Matrix4f32) { r[0][0], r[0][1], r[0][2], r[0][3] = m[0][0], m[0][1], 0, 0; r[1][0], r[1][1], r[1][2], r[1][3] = m[1][0], m[1][1], 0, 0; r[2][0], r[2][1], r[2][2], r[2][3] = 0, 0, 1, 0; r[3][0], r[3][1], r[3][2], r[3][3] = 0, 0, 0, 1; return; } -matrix4_from_matrix3 :: proc(m: Matrix3) -> (r: Matrix4) { +matrix4_from_matrix2_f64 :: proc(m: Matrix2f64) -> (r: Matrix4f64) { + r[0][0], r[0][1], r[0][2], r[0][3] = m[0][0], m[0][1], 0, 0; + r[1][0], r[1][1], r[1][2], r[1][3] = m[1][0], m[1][1], 0, 0; + r[2][0], r[2][1], r[2][2], r[2][3] = 0, 0, 1, 0; + r[3][0], r[3][1], r[3][2], r[3][3] = 0, 0, 0, 1; + return; +} +matrix4_from_matrix2 :: proc{ + matrix4_from_matrix2_f32, + matrix4_from_matrix2_f64, +}; + +matrix4_from_matrix3_f32 :: proc(m: Matrix3f32) -> (r: Matrix4f32) { r[0][0], r[0][1], r[0][2], r[0][3] = m[0][0], m[0][1], m[0][2], 0; r[1][0], r[1][1], r[1][2], r[1][3] = m[1][0], m[1][1], m[1][2], 0; r[2][0], r[2][1], r[2][2], r[2][3] = m[2][0], m[2][1], m[2][2], 0; r[3][0], r[3][1], r[3][2], r[3][3] = 0, 0, 0, 1; return; } +matrix4_from_matrix3_f64 :: proc(m: Matrix3f64) -> (r: Matrix4f64) { + r[0][0], r[0][1], r[0][2], r[0][3] = m[0][0], m[0][1], m[0][2], 0; + r[1][0], r[1][1], r[1][2], r[1][3] = m[1][0], m[1][1], m[1][2], 0; + r[2][0], r[2][1], r[2][2], r[2][3] = m[2][0], m[2][1], m[2][2], 0; + r[3][0], r[3][1], r[3][2], r[3][3] = 0, 0, 0, 1; + return; +} +matrix4_from_matrix3 :: proc{ + matrix4_from_matrix3_f32, + matrix4_from_matrix3_f64, +}; -quaternion_from_scalar :: proc(f: Float) -> (q: Quaternion) { +quaternion_from_scalar_f32 :: proc(f: f32) -> (q: Quaternionf32) { q.w = f; return; } +quaternion_from_scalar_f64 :: proc(f: f64) -> (q: Quaternionf64) { + q.w = f; + return; +} +quaternion_from_scalar :: proc{ + quaternion_from_scalar_f32, + quaternion_from_scalar_f64, +}; -to_matrix2 :: proc{matrix2_from_scalar, matrix2_from_matrix3, matrix2_from_matrix4}; -to_matrix3 :: proc{matrix3_from_scalar, matrix3_from_matrix2, matrix3_from_matrix4, matrix3_from_quaternion}; -to_matrix4 :: proc{matrix4_from_scalar, matrix4_from_matrix2, matrix4_from_matrix3, matrix4_from_quaternion}; -to_quaternion :: proc{quaternion_from_scalar, quaternion_from_matrix3, quaternion_from_matrix4}; +to_matrix2f32 :: proc{matrix2_from_scalar_f32, matrix2_from_matrix3_f32, matrix2_from_matrix4_f32}; +to_matrix3f32 :: proc{matrix3_from_scalar_f32, matrix3_from_matrix2_f32, matrix3_from_matrix4_f32, matrix3_from_quaternion_f32}; +to_matrix4f32 :: proc{matrix4_from_scalar_f32, matrix4_from_matrix2_f32, matrix4_from_matrix3_f32, matrix4_from_quaternion_f32}; +to_quaternionf32 :: proc{quaternion_from_scalar_f32, quaternion_from_matrix3_f32, quaternion_from_matrix4_f32}; + +to_matrix2f64 :: proc{matrix2_from_scalar_f64, matrix2_from_matrix3_f64, matrix2_from_matrix4_f64}; +to_matrix3f64 :: proc{matrix3_from_scalar_f64, matrix3_from_matrix2_f64, matrix3_from_matrix4_f64, matrix3_from_quaternion_f64}; +to_matrix4f64 :: proc{matrix4_from_scalar_f64, matrix4_from_matrix2_f64, matrix4_from_matrix3_f64, matrix4_from_quaternion_f64}; +to_quaternionf64 :: proc{quaternion_from_scalar_f64, quaternion_from_matrix3_f64, quaternion_from_matrix4_f64}; +to_matrix2f :: proc{ + matrix2_from_scalar_f32, matrix2_from_matrix3_f32, matrix2_from_matrix4_f32, + matrix2_from_scalar_f64, matrix2_from_matrix3_f64, matrix2_from_matrix4_f64, +}; +to_matrix3 :: proc{ + matrix3_from_scalar_f32, matrix3_from_matrix2_f32, matrix3_from_matrix4_f32, matrix3_from_quaternion_f32, + matrix3_from_scalar_f64, matrix3_from_matrix2_f64, matrix3_from_matrix4_f64, matrix3_from_quaternion_f64, +}; +to_matrix4 :: proc{ + matrix4_from_scalar_f32, matrix4_from_matrix2_f32, matrix4_from_matrix3_f32, matrix4_from_quaternion_f32, + matrix4_from_scalar_f64, matrix4_from_matrix2_f64, matrix4_from_matrix3_f64, matrix4_from_quaternion_f64, +}; +to_quaternion :: proc{ + quaternion_from_scalar_f32, quaternion_from_matrix3_f32, quaternion_from_matrix4_f32, + quaternion_from_scalar_f64, quaternion_from_matrix3_f64, quaternion_from_matrix4_f64, +}; -matrix2_orthonormalize :: proc(m: Matrix2) -> (r: Matrix2) { + +matrix2_orthonormalize_f32 :: proc(m: Matrix2f32) -> (r: Matrix2f32) { r[0] = normalize(m[0]); d0 := dot(r[0], r[1]); @@ -806,8 +1698,21 @@ matrix2_orthonormalize :: proc(m: Matrix2) -> (r: Matrix2) { return; } +matrix2_orthonormalize_f64 :: proc(m: Matrix2f64) -> (r: Matrix2f64) { + r[0] = normalize(m[0]); -matrix3_orthonormalize :: proc(m: Matrix3) -> (r: Matrix3) { + d0 := dot(r[0], r[1]); + r[1] -= r[0] * d0; + r[1] = normalize(r[1]); + + return; +} +matrix2_orthonormalize :: proc{ + matrix2_orthonormalize_f32, + matrix2_orthonormalize_f64, +}; + +matrix3_orthonormalize_f32 :: proc(m: Matrix3f32) -> (r: Matrix3f32) { r[0] = normalize(m[0]); d0 := dot(r[0], r[1]); @@ -821,22 +1726,49 @@ matrix3_orthonormalize :: proc(m: Matrix3) -> (r: Matrix3) { return; } +matrix3_orthonormalize_f64 :: proc(m: Matrix3f64) -> (r: Matrix3f64) { + r[0] = normalize(m[0]); -vector3_orthonormalize :: proc(x, y: Vector3) -> (z: Vector3) { + d0 := dot(r[0], r[1]); + r[1] -= r[0] * d0; + r[1] = normalize(r[1]); + + d1 := dot(r[1], r[2]); + d0 = dot(r[0], r[2]); + r[2] -= r[0]*d0 + r[1]*d1; + r[2] = normalize(r[2]); + + return; +} +matrix3_orthonormalize :: proc{ + matrix3_orthonormalize_f32, + matrix3_orthonormalize_f64, +}; + +vector3_orthonormalize_f32 :: proc(x, y: Vector3f32) -> (z: Vector3f32) { return normalize(x - y * dot(y, x)); } - +vector3_orthonormalize_f64 :: proc(x, y: Vector3f64) -> (z: Vector3f64) { + return normalize(x - y * dot(y, x)); +} +vector3_orthonormalize :: proc{ + vector3_orthonormalize_f32, + vector3_orthonormalize_f64, +}; orthonormalize :: proc{ - matrix2_orthonormalize, - matrix3_orthonormalize, - vector3_orthonormalize, + matrix2_orthonormalize_f32, + matrix2_orthonormalize_f64, + matrix3_orthonormalize_f32, + matrix3_orthonormalize_f64, + vector3_orthonormalize_f32, + vector3_orthonormalize_f64, }; -matrix4_orientation :: proc(normal, up: Vector3) -> Matrix4 { +matrix4_orientation_f32 :: proc(normal, up: Vector3f32) -> Matrix4f32 { if all(equal(normal, up)) { - return MATRIX4_IDENTITY; + return MATRIX4F32_IDENTITY; } rotation_axis := cross(up, normal); @@ -844,29 +1776,73 @@ matrix4_orientation :: proc(normal, up: Vector3) -> Matrix4 { return matrix4_rotate(angle, rotation_axis); } +matrix4_orientation_f64 :: proc(normal, up: Vector3f64) -> Matrix4f64 { + if all(equal(normal, up)) { + return MATRIX4F64_IDENTITY; + } + rotation_axis := cross(up, normal); + angle := math.acos(dot(normal, up)); + return matrix4_rotate(angle, rotation_axis); +} +matrix4_orientation :: proc{ + matrix4_orientation_f32, + matrix4_orientation_f64, +}; -euclidean_from_polar :: proc(polar: Vector2) -> Vector3 { +euclidean_from_polar_f32 :: proc(polar: Vector2f32) -> Vector3f32 { latitude, longitude := polar.x, polar.y; cx, sx := math.cos(latitude), math.sin(latitude); cy, sy := math.cos(longitude), math.sin(longitude); - return Vector3{ + return { cx*sy, sx, cx*cy, }; } -polar_from_euclidean :: proc(euclidean: Vector3) -> Vector3 { +euclidean_from_polar_f64 :: proc(polar: Vector2f64) -> Vector3f64 { + latitude, longitude := polar.x, polar.y; + cx, sx := math.cos(latitude), math.sin(latitude); + cy, sy := math.cos(longitude), math.sin(longitude); + + return { + cx*sy, + sx, + cx*cy, + }; +} +euclidean_from_polar :: proc{ + euclidean_from_polar_f32, + euclidean_from_polar_f64, +}; + +polar_from_euclidean_f32 :: proc(euclidean: Vector3f32) -> Vector3f32 { n := length(euclidean); tmp := euclidean / n; xz_dist := math.sqrt(tmp.x*tmp.x + tmp.z*tmp.z); - return Vector3{ + return { math.asin(tmp.y), math.atan2(tmp.x, tmp.z), xz_dist, }; } +polar_from_euclidean_f64 :: proc(euclidean: Vector3f64) -> Vector3f64 { + n := length(euclidean); + tmp := euclidean / n; + + xz_dist := math.sqrt(tmp.x*tmp.x + tmp.z*tmp.z); + + return { + math.asin(tmp.y), + math.atan2(tmp.x, tmp.z), + xz_dist, + }; +} +polar_from_euclidean :: proc{ + polar_from_euclidean_f32, + polar_from_euclidean_f64, +}; diff --git a/core/math/linalg/specific_euler_angles.odin b/core/math/linalg/specific_euler_angles.odin index 580ba147d..759fd6201 100644 --- a/core/math/linalg/specific_euler_angles.odin +++ b/core/math/linalg/specific_euler_angles.odin @@ -1,7 +1,5 @@ package linalg -import "core:math" - Euler_Angle_Order :: enum { // Tait-Bryan XYZ, @@ -20,796 +18,64 @@ Euler_Angle_Order :: enum { ZYZ, } -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 .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 :: 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 .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; -} - -matrix4_from_euler_angles :: proc(t1, t2, t3: Float, order: Euler_Angle_Order) -> (m: Matrix4) { - switch order { - 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; -} - -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; - - q1, q2, q3: Quaternion; - - switch order { - case .XYZ: q1, q2, q3 = X(t1), Y(t2), Z(t3); - case .XZY: q1, q2, q3 = X(t1), Z(t2), Y(t3); - case .YXZ: q1, q2, q3 = Y(t1), X(t2), Z(t3); - case .YZX: q1, q2, q3 = Y(t1), Z(t2), X(t3); - case .ZXY: q1, q2, q3 = Z(t1), X(t2), Y(t3); - case .ZYX: q1, q2, q3 = Z(t1), Y(t2), X(t3); - case .XYX: q1, q2, q3 = X(t1), Y(t2), X(t3); - case .XZX: q1, q2, q3 = X(t1), Z(t2), X(t3); - case .YXY: q1, q2, q3 = Y(t1), X(t2), Y(t3); - case .YZY: q1, q2, q3 = Y(t1), Z(t2), Y(t3); - case .ZXZ: q1, q2, q3 = Z(t1), X(t2), Z(t3); - case .ZYZ: q1, q2, q3 = Z(t1), Y(t2), Z(t3); - } - - return q1 * (q2 * q3); -} - - -// 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; - m[2][1] = +sin_x; - m[1][2] = -sin_x; - m[2][2] = +cos_x; - m[3][3] = 1; - return; -} -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; - m[1][1] = 1; - m[0][2] = +sin_y; - m[2][2] = +cos_y; - m[3][3] = 1; - return; -} -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; - m[1][1] = +cos_z; - m[0][1] = -sin_z; - m[2][2] = 1; - m[3][3] = 1; - return; -} - - -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; - m[1][1] = +cos_x; - m[2][1] = +sin_x; - m[1][2] = -sin_x; - m[2][2] = +cos_x; - m[3][3] = 1; - return; -} -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; - m[2][0] = -sin_y; - m[1][1] = 1; - m[0][2] = +sin_y; - m[2][2] = +cos_y; - m[3][3] = 1; - return; -} -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; - m[1][0] = +sin_z; - m[1][1] = +cos_z; - m[0][1] = -sin_z; - m[2][2] = 1; - m[3][3] = 1; - return; -} - - -matrix4_from_euler_angles_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; - 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; - m[3][3] = 1; - return; -} - - -matrix4_from_euler_angles_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; - 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; - m[3][3] = 1; - return; -} - -matrix4_from_euler_angles_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)); -} -matrix4_from_euler_angles_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)); -} -matrix4_from_euler_angles_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)); -} -matrix4_from_euler_angles_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)); -} - - -matrix4_from_euler_angles_xyz :: proc(t1, t2, t3: Float) -> (m: Matrix4) { - 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[0][3] = 0; - m[1][0] = c2 * s3; - m[1][1] = c1 * c3 + s1 * s2 * s3; - m[1][2] =-s1 * c3 + c1 * s2 * s3; - m[1][3] = 0; - m[2][0] =-s2; - m[2][1] = s1 * c2; - m[2][2] = c1 * c2; - m[2][3] = 0; - m[3][0] = 0; - m[3][1] = 0; - m[3][2] = 0; - m[3][3] = 1; - return; -} - -matrix4_from_euler_angles_yxz :: proc(yaw, pitch, roll: Float) -> (m: Matrix4) { - 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[0][3] = 0; - m[1][0] = -ch * sb + sh * sp * cb; - m[1][1] = cb * cp; - m[1][2] = sb * sh + ch * sp * cb; - m[1][3] = 0; - m[2][0] = sh * cp; - m[2][1] = -sp; - m[2][2] = ch * cp; - m[2][3] = 0; - m[3][0] = 0; - m[3][1] = 0; - m[3][2] = 0; - m[3][3] = 1; - return; -} - -matrix4_from_euler_angles_xzx :: proc(t1, t2, t3: Float) -> (m: Matrix4) { - 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[0][3] = 0; - m[1][0] =-c3 * s2; - m[1][1] = c1 * c2 * c3 - s1 * s3; - m[1][2] = c1 * s3 + c2 * c3 * s1; - m[1][3] = 0; - m[2][0] = s2 * s3; - m[2][1] =-c3 * s1 - c1 * c2 * s3; - m[2][2] = c1 * c3 - c2 * s1 * s3; - m[2][3] = 0; - m[3][0] = 0; - m[3][1] = 0; - m[3][2] = 0; - m[3][3] = 1; - return; -} - -matrix4_from_euler_angles_xyx :: proc(t1, t2, t3: Float) -> (m: Matrix4) { - 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[0][3] = 0; - m[1][0] = s2 * s3; - m[1][1] = c1 * c3 - c2 * s1 * s3; - m[1][2] = c3 * s1 + c1 * c2 * s3; - m[1][3] = 0; - m[2][0] = c3 * s2; - m[2][1] =-c1 * s3 - c2 * c3 * s1; - m[2][2] = c1 * c2 * c3 - s1 * s3; - m[2][3] = 0; - m[3][0] = 0; - m[3][1] = 0; - m[3][2] = 0; - m[3][3] = 1; - return; -} - -matrix4_from_euler_angles_yxy :: proc(t1, t2, t3: Float) -> (m: Matrix4) { - 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[0][3] = 0; - m[1][0] = s1 * s2; - m[1][1] = c2; - m[1][2] = c1 * s2; - m[1][3] = 0; - m[2][0] = c1 * s3 + c2 * c3 * s1; - m[2][1] =-c3 * s2; - m[2][2] = c1 * c2 * c3 - s1 * s3; - m[2][3] = 0; - m[3][0] = 0; - m[3][1] = 0; - m[3][2] = 0; - m[3][3] = 1; - return; -} - -matrix4_from_euler_angles_yzy :: proc(t1, t2, t3: Float) -> (m: Matrix4) { - 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[0][3] = 0; - m[1][0] =-c1 * s2; - m[1][1] = c2; - m[1][2] = s1 * s2; - m[1][3] = 0; - m[2][0] = c3 * s1 + c1 * c2 * s3; - m[2][1] = s2 * s3; - m[2][2] = c1 * c3 - c2 * s1 * s3; - m[2][3] = 0; - m[3][0] = 0; - m[3][1] = 0; - m[3][2] = 0; - m[3][3] = 1; - return; -} - -matrix4_from_euler_angles_zyz :: proc(t1, t2, t3: Float) -> (m: Matrix4) { - 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[0][3] = 0; - m[1][0] =-c3 * s1 - c1 * c2 * s3; - m[1][1] = c1 * c3 - c2 * s1 * s3; - m[1][2] = s2 * s3; - m[1][3] = 0; - m[2][0] = c1 * s2; - m[2][1] = s1 * s2; - m[2][2] = c2; - m[2][3] = 0; - m[3][0] = 0; - m[3][1] = 0; - m[3][2] = 0; - m[3][3] = 1; - return; -} - -matrix4_from_euler_angles_zxz :: proc(t1, t2, t3: Float) -> (m: Matrix4) { - 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[0][3] = 0; - m[1][0] =-c1 * s3 - c2 * c3 * s1; - m[1][1] = c1 * c2 * c3 - s1 * s3; - m[1][2] = c3 * s2; - m[1][3] = 0; - m[2][0] = s1 * s2; - m[2][1] =-c1 * s2; - m[2][2] = c2; - m[2][3] = 0; - m[3][0] = 0; - m[3][1] = 0; - m[3][2] = 0; - m[3][3] = 1; - return; -} - - -matrix4_from_euler_angles_xzy :: proc(t1, t2, t3: Float) -> (m: Matrix4) { - 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[0][3] = 0; - m[1][0] =-s2; - m[1][1] = c1 * c2; - m[1][2] = c2 * s1; - m[1][3] = 0; - m[2][0] = c2 * s3; - m[2][1] = c1 * s2 * s3 - c3 * s1; - m[2][2] = c1 * c3 + s1 * s2 *s3; - m[2][3] = 0; - m[3][0] = 0; - m[3][1] = 0; - m[3][2] = 0; - m[3][3] = 1; - return; -} - -matrix4_from_euler_angles_yzx :: proc(t1, t2, t3: Float) -> (m: Matrix4) { - 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[0][3] = 0; - m[1][0] = s1 * s3 - c1 * c3 * s2; - m[1][1] = c2 * c3; - m[1][2] = c1 * s3 + c3 * s1 * s2; - m[1][3] = 0; - m[2][0] = c3 * s1 + c1 * s2 * s3; - m[2][1] =-c2 * s3; - m[2][2] = c1 * c3 - s1 * s2 * s3; - m[2][3] = 0; - m[3][0] = 0; - m[3][1] = 0; - m[3][2] = 0; - m[3][3] = 1; - return; -} - -matrix4_from_euler_angles_zyx :: proc(t1, t2, t3: Float) -> (m: Matrix4) { - 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[0][3] = 0; - m[1][0] = c1 * s2 * s3 - c3 * s1; - m[1][1] = c1 * c3 + s1 * s2 * s3; - m[1][2] = c2 * s3; - m[1][3] = 0; - m[2][0] = s1 * s3 + c1 * c3 * s2; - m[2][1] = c3 * s1 * s2 - c1 * s3; - m[2][2] = c2 * c3; - m[2][3] = 0; - m[3][0] = 0; - m[3][1] = 0; - m[3][2] = 0; - m[3][3] = 1; - return; -} - -matrix4_from_euler_angles_zxy :: proc(t1, t2, t3: Float) -> (m: Matrix4) { - 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[0][3] = 0; - m[1][0] =-c2 * s1; - m[1][1] = c1 * c2; - m[1][2] = s2; - m[1][3] = 0; - m[2][0] = c1 * s3 + c3 * s1 * s2; - m[2][1] = s1 * s3 - c1 * c3 * s2; - m[2][2] = c2 * c3; - m[2][3] = 0; - m[3][0] = 0; - m[3][1] = 0; - m[3][2] = 0; - m[3][3] = 1; - return; -} - - -matrix4_from_yaw_pitch_roll :: proc(yaw, pitch, roll: Float) -> (m: Matrix4) { - 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[0][3] = 0; - m[1][0] = -ch * sb + sh * sp * cb; - m[1][1] = cb * cp; - m[1][2] = sb * sh + ch * sp * cb; - m[1][3] = 0; - m[2][0] = sh * cp; - m[2][1] = -sp; - m[2][2] = ch * cp; - m[2][3] = 0; - m[3][0] = 0; - m[3][1] = 0; - m[3][2] = 0; - m[3][3] = 1; - return m; -} - -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); - 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_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); - 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_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]); - 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_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]); - 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_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]); - 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_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]); - 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_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]); - 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_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]); - 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_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); - 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_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); - 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_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); - 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_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); - 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; -} +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_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}; +euler_angles_xyx_from_quaternion :: proc{euler_angles_xyx_from_quaternion_f32, euler_angles_xyx_from_quaternion_f64}; +euler_angles_yxy_from_quaternion :: proc{euler_angles_yxy_from_quaternion_f32, euler_angles_yxy_from_quaternion_f64}; +euler_angles_yzy_from_quaternion :: proc{euler_angles_yzy_from_quaternion_f32, euler_angles_yzy_from_quaternion_f64}; +euler_angles_zyz_from_quaternion :: proc{euler_angles_zyz_from_quaternion_f32, euler_angles_zyz_from_quaternion_f64}; +euler_angles_zxz_from_quaternion :: proc{euler_angles_zxz_from_quaternion_f32, euler_angles_zxz_from_quaternion_f64}; +euler_angles_xzy_from_quaternion :: proc{euler_angles_xzy_from_quaternion_f32, euler_angles_xzy_from_quaternion_f64}; +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}; +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}; +matrix4_from_derived_euler_angle_x :: proc{matrix4_from_derived_euler_angle_x_f32, matrix4_from_derived_euler_angle_x_f64}; +matrix4_from_derived_euler_angle_y :: proc{matrix4_from_derived_euler_angle_y_f32, matrix4_from_derived_euler_angle_y_f64}; +matrix4_from_derived_euler_angle_z :: proc{matrix4_from_derived_euler_angle_z_f32, matrix4_from_derived_euler_angle_z_f64}; +matrix4_from_euler_angles_xy :: proc{matrix4_from_euler_angles_xy_f32, matrix4_from_euler_angles_xy_f64}; +matrix4_from_euler_angles_yx :: proc{matrix4_from_euler_angles_yx_f32, matrix4_from_euler_angles_yx_f64}; +matrix4_from_euler_angles_xz :: proc{matrix4_from_euler_angles_xz_f32, matrix4_from_euler_angles_xz_f64}; +matrix4_from_euler_angles_zx :: proc{matrix4_from_euler_angles_zx_f32, matrix4_from_euler_angles_zx_f64}; +matrix4_from_euler_angles_yz :: proc{matrix4_from_euler_angles_yz_f32, matrix4_from_euler_angles_yz_f64}; +matrix4_from_euler_angles_zy :: proc{matrix4_from_euler_angles_zy_f32, matrix4_from_euler_angles_zy_f64}; +matrix4_from_euler_angles_xyz :: proc{matrix4_from_euler_angles_xyz_f32, matrix4_from_euler_angles_xyz_f64}; +matrix4_from_euler_angles_yxz :: proc{matrix4_from_euler_angles_yxz_f32, matrix4_from_euler_angles_yxz_f64}; +matrix4_from_euler_angles_xzx :: proc{matrix4_from_euler_angles_xzx_f32, matrix4_from_euler_angles_xzx_f64}; +matrix4_from_euler_angles_xyx :: proc{matrix4_from_euler_angles_xyx_f32, matrix4_from_euler_angles_xyx_f64}; +matrix4_from_euler_angles_yxy :: proc{matrix4_from_euler_angles_yxy_f32, matrix4_from_euler_angles_yxy_f64}; +matrix4_from_euler_angles_yzy :: proc{matrix4_from_euler_angles_yzy_f32, matrix4_from_euler_angles_yzy_f64}; +matrix4_from_euler_angles_zyz :: proc{matrix4_from_euler_angles_zyz_f32, matrix4_from_euler_angles_zyz_f64}; +matrix4_from_euler_angles_zxz :: proc{matrix4_from_euler_angles_zxz_f32, matrix4_from_euler_angles_zxz_f64}; +matrix4_from_euler_angles_xzy :: proc{matrix4_from_euler_angles_xzy_f32, matrix4_from_euler_angles_xzy_f64}; +matrix4_from_euler_angles_yzx :: proc{matrix4_from_euler_angles_yzx_f32, matrix4_from_euler_angles_yzx_f64}; +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_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}; +euler_angles_xyx_from_matrix4 :: proc{euler_angles_xyx_from_matrix4_f32, euler_angles_xyx_from_matrix4_f64}; +euler_angles_yxy_from_matrix4 :: proc{euler_angles_yxy_from_matrix4_f32, euler_angles_yxy_from_matrix4_f64}; +euler_angles_yzy_from_matrix4 :: proc{euler_angles_yzy_from_matrix4_f32, euler_angles_yzy_from_matrix4_f64}; +euler_angles_zyz_from_matrix4 :: proc{euler_angles_zyz_from_matrix4_f32, euler_angles_zyz_from_matrix4_f64}; +euler_angles_zxz_from_matrix4 :: proc{euler_angles_zxz_from_matrix4_f32, euler_angles_zxz_from_matrix4_f64}; +euler_angles_xzy_from_matrix4 :: proc{euler_angles_xzy_from_matrix4_f32, euler_angles_xzy_from_matrix4_f64}; +euler_angles_yzx_from_matrix4 :: proc{euler_angles_yzx_from_matrix4_f32, euler_angles_yzx_from_matrix4_f64}; +euler_angles_zyx_from_matrix4 :: proc{euler_angles_zyx_from_matrix4_f32, euler_angles_zyx_from_matrix4_f64}; +euler_angles_zxy_from_matrix4 :: proc{euler_angles_zxy_from_matrix4_f32, euler_angles_zxy_from_matrix4_f64}; diff --git a/core/math/linalg/specific_euler_angles_f32.odin b/core/math/linalg/specific_euler_angles_f32.odin new file mode 100644 index 000000000..35f497746 --- /dev/null +++ b/core/math/linalg/specific_euler_angles_f32.odin @@ -0,0 +1,797 @@ +package linalg + +import "core:math" + +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); + } + 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); + } + 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); + } + return; +} + +quaternion_from_euler_angles_f32 :: proc(t1, t2, t3: f32, order: Euler_Angle_Order) -> Quaternionf32 { + X :: quaternion_from_euler_angle_x; + Y :: quaternion_from_euler_angle_y; + Z :: quaternion_from_euler_angle_z; + + q1, q2, q3: Quaternionf32; + + switch order { + case .XYZ: q1, q2, q3 = X(t1), Y(t2), Z(t3); + case .XZY: q1, q2, q3 = X(t1), Z(t2), Y(t3); + case .YXZ: q1, q2, q3 = Y(t1), X(t2), Z(t3); + case .YZX: q1, q2, q3 = Y(t1), Z(t2), X(t3); + case .ZXY: q1, q2, q3 = Z(t1), X(t2), Y(t3); + case .ZYX: q1, q2, q3 = Z(t1), Y(t2), X(t3); + case .XYX: q1, q2, q3 = X(t1), Y(t2), X(t3); + case .XZX: q1, q2, q3 = X(t1), Z(t2), X(t3); + case .YXY: q1, q2, q3 = Y(t1), X(t2), Y(t3); + case .YZY: q1, q2, q3 = Y(t1), Z(t2), Y(t3); + case .ZXZ: q1, q2, q3 = Z(t1), X(t2), Z(t3); + case .ZYZ: q1, q2, q3 = Z(t1), Y(t2), Z(t3); + } + + return q1 * (q2 * q3); +} + + +// Quaternionf32s + +quaternion_from_euler_angle_x_f32 :: proc(angle_x: f32) -> (q: Quaternionf32) { + return quaternion_angle_axis_f32(angle_x, {1, 0, 0}); +} +quaternion_from_euler_angle_y_f32 :: proc(angle_y: f32) -> (q: Quaternionf32) { + return quaternion_angle_axis_f32(angle_y, {0, 1, 0}); +} +quaternion_from_euler_angle_z_f32 :: proc(angle_z: f32) -> (q: Quaternionf32) { + return quaternion_angle_axis_f32(angle_z, {0, 0, 1}); +} + +quaternion_from_pitch_yaw_roll_f32 :: proc(pitch, yaw, roll: f32) -> Quaternionf32 { + 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: Quaternionf32; + 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_f32 :: proc(q: Quaternionf32) -> f32 { + 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_f32 :: proc(q: Quaternionf32) -> f32 { + 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) <= F32_EPSILON && abs(y) <= F32_EPSILON { + return 2 * math.atan2(q.x, q.w); + } + + return math.atan2(y, x); +} + +yaw_from_quaternion_f32 :: proc(q: Quaternionf32) -> f32 { + return math.asin(clamp(-2 * (q.x*q.z - q.w*q.y), -1, 1)); +} + + +pitch_yaw_roll_from_quaternion_f32 :: proc(q: Quaternionf32) -> (pitch, yaw, roll: f32) { + pitch = pitch_from_quaternion(q); + yaw = yaw_from_quaternion(q); + roll = roll_from_quaternion(q); + return; +} + +euler_angles_xyz_from_quaternion_f32 :: proc(q: Quaternionf32) -> (t1, t2, t3: f32) { + return euler_angles_xyz_from_matrix4(matrix4_from_quaternion(q)); +} +euler_angles_yxz_from_quaternion_f32 :: proc(q: Quaternionf32) -> (t1, t2, t3: f32) { + return euler_angles_yxz_from_matrix4(matrix4_from_quaternion(q)); +} +euler_angles_xzx_from_quaternion_f32 :: proc(q: Quaternionf32) -> (t1, t2, t3: f32) { + return euler_angles_xzx_from_matrix4(matrix4_from_quaternion(q)); +} +euler_angles_xyx_from_quaternion_f32 :: proc(q: Quaternionf32) -> (t1, t2, t3: f32) { + return euler_angles_xyx_from_matrix4(matrix4_from_quaternion(q)); +} +euler_angles_yxy_from_quaternion_f32 :: proc(q: Quaternionf32) -> (t1, t2, t3: f32) { + return euler_angles_yxy_from_matrix4(matrix4_from_quaternion(q)); +} +euler_angles_yzy_from_quaternion_f32 :: proc(q: Quaternionf32) -> (t1, t2, t3: f32) { + return euler_angles_yzy_from_matrix4(matrix4_from_quaternion(q)); +} +euler_angles_zyz_from_quaternion_f32 :: proc(q: Quaternionf32) -> (t1, t2, t3: f32) { + return euler_angles_zyz_from_matrix4(matrix4_from_quaternion(q)); +} +euler_angles_zxz_from_quaternion_f32 :: proc(q: Quaternionf32) -> (t1, t2, t3: f32) { + return euler_angles_zxz_from_matrix4(matrix4_from_quaternion(q)); +} +euler_angles_xzy_from_quaternion_f32 :: proc(q: Quaternionf32) -> (t1, t2, t3: f32) { + return euler_angles_xzy_from_matrix4(matrix4_from_quaternion(q)); +} +euler_angles_yzx_from_quaternion_f32 :: proc(q: Quaternionf32) -> (t1, t2, t3: f32) { + return euler_angles_yzx_from_matrix4(matrix4_from_quaternion(q)); +} +euler_angles_zyx_from_quaternion_f32 :: proc(q: Quaternionf32) -> (t1, t2, t3: f32) { + return euler_angles_zyx_from_matrix4(matrix4_from_quaternion(q)); +} +euler_angles_zxy_from_quaternion_f32 :: proc(q: Quaternionf32) -> (t1, t2, t3: f32) { + return euler_angles_zxy_from_matrix4(matrix4_from_quaternion(q)); +} + + +// Matrices + + +matrix4_from_euler_angle_x_f32 :: proc(angle_x: f32) -> (m: Matrix4f32) { + 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; + m[3][3] = 1; + return; +} +matrix4_from_euler_angle_y_f32 :: proc(angle_y: f32) -> (m: Matrix4f32) { + 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; + m[3][3] = 1; + return; +} +matrix4_from_euler_angle_z_f32 :: proc(angle_z: f32) -> (m: Matrix4f32) { + 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; + m[3][3] = 1; + return; +} + + +matrix4_from_derived_euler_angle_x_f32 :: proc(angle_x: f32, angular_velocity_x: f32) -> (m: Matrix4f32) { + 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; + m[3][3] = 1; + return; +} +matrix4_from_derived_euler_angle_y_f32 :: proc(angle_y: f32, angular_velocity_y: f32) -> (m: Matrix4f32) { + 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; + m[3][3] = 1; + return; +} +matrix4_from_derived_euler_angle_z_f32 :: proc(angle_z: f32, angular_velocity_z: f32) -> (m: Matrix4f32) { + 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; + m[3][3] = 1; + return; +} + + +matrix4_from_euler_angles_xy_f32 :: proc(angle_x, angle_y: f32) -> (m: Matrix4f32) { + 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; + m[3][3] = 1; + return; +} + + +matrix4_from_euler_angles_yx_f32 :: proc(angle_y, angle_x: f32) -> (m: Matrix4f32) { + 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; + m[3][3] = 1; + return; +} + +matrix4_from_euler_angles_xz_f32 :: proc(angle_x, angle_z: f32) -> (m: Matrix4f32) { + return mul(matrix4_from_euler_angle_x(angle_x), matrix4_from_euler_angle_z(angle_z)); +} +matrix4_from_euler_angles_zx_f32 :: proc(angle_z, angle_x: f32) -> (m: Matrix4f32) { + return mul(matrix4_from_euler_angle_z(angle_z), matrix4_from_euler_angle_x(angle_x)); +} +matrix4_from_euler_angles_yz_f32 :: proc(angle_y, angle_z: f32) -> (m: Matrix4f32) { + return mul(matrix4_from_euler_angle_y(angle_y), matrix4_from_euler_angle_z(angle_z)); +} +matrix4_from_euler_angles_zy_f32 :: proc(angle_z, angle_y: f32) -> (m: Matrix4f32) { + return mul(matrix4_from_euler_angle_z(angle_z), matrix4_from_euler_angle_y(angle_y)); +} + + +matrix4_from_euler_angles_xyz_f32 :: proc(t1, t2, t3: f32) -> (m: Matrix4f32) { + 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[0][3] = 0; + m[1][0] = c2 * s3; + m[1][1] = c1 * c3 + s1 * s2 * s3; + m[1][2] =-s1 * c3 + c1 * s2 * s3; + m[1][3] = 0; + m[2][0] =-s2; + m[2][1] = s1 * c2; + m[2][2] = c1 * c2; + m[2][3] = 0; + m[3][0] = 0; + m[3][1] = 0; + m[3][2] = 0; + m[3][3] = 1; + return; +} + +matrix4_from_euler_angles_yxz_f32 :: proc(yaw, pitch, roll: f32) -> (m: Matrix4f32) { + 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[0][3] = 0; + m[1][0] = -ch * sb + sh * sp * cb; + m[1][1] = cb * cp; + m[1][2] = sb * sh + ch * sp * cb; + m[1][3] = 0; + m[2][0] = sh * cp; + m[2][1] = -sp; + m[2][2] = ch * cp; + m[2][3] = 0; + m[3][0] = 0; + m[3][1] = 0; + m[3][2] = 0; + m[3][3] = 1; + return; +} + +matrix4_from_euler_angles_xzx_f32 :: proc(t1, t2, t3: f32) -> (m: Matrix4f32) { + 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[0][3] = 0; + m[1][0] =-c3 * s2; + m[1][1] = c1 * c2 * c3 - s1 * s3; + m[1][2] = c1 * s3 + c2 * c3 * s1; + m[1][3] = 0; + m[2][0] = s2 * s3; + m[2][1] =-c3 * s1 - c1 * c2 * s3; + m[2][2] = c1 * c3 - c2 * s1 * s3; + m[2][3] = 0; + m[3][0] = 0; + m[3][1] = 0; + m[3][2] = 0; + m[3][3] = 1; + return; +} + +matrix4_from_euler_angles_xyx_f32 :: proc(t1, t2, t3: f32) -> (m: Matrix4f32) { + 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[0][3] = 0; + m[1][0] = s2 * s3; + m[1][1] = c1 * c3 - c2 * s1 * s3; + m[1][2] = c3 * s1 + c1 * c2 * s3; + m[1][3] = 0; + m[2][0] = c3 * s2; + m[2][1] =-c1 * s3 - c2 * c3 * s1; + m[2][2] = c1 * c2 * c3 - s1 * s3; + m[2][3] = 0; + m[3][0] = 0; + m[3][1] = 0; + m[3][2] = 0; + m[3][3] = 1; + return; +} + +matrix4_from_euler_angles_yxy_f32 :: proc(t1, t2, t3: f32) -> (m: Matrix4f32) { + 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[0][3] = 0; + m[1][0] = s1 * s2; + m[1][1] = c2; + m[1][2] = c1 * s2; + m[1][3] = 0; + m[2][0] = c1 * s3 + c2 * c3 * s1; + m[2][1] =-c3 * s2; + m[2][2] = c1 * c2 * c3 - s1 * s3; + m[2][3] = 0; + m[3][0] = 0; + m[3][1] = 0; + m[3][2] = 0; + m[3][3] = 1; + return; +} + +matrix4_from_euler_angles_yzy_f32 :: proc(t1, t2, t3: f32) -> (m: Matrix4f32) { + 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[0][3] = 0; + m[1][0] =-c1 * s2; + m[1][1] = c2; + m[1][2] = s1 * s2; + m[1][3] = 0; + m[2][0] = c3 * s1 + c1 * c2 * s3; + m[2][1] = s2 * s3; + m[2][2] = c1 * c3 - c2 * s1 * s3; + m[2][3] = 0; + m[3][0] = 0; + m[3][1] = 0; + m[3][2] = 0; + m[3][3] = 1; + return; +} + +matrix4_from_euler_angles_zyz_f32 :: proc(t1, t2, t3: f32) -> (m: Matrix4f32) { + 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[0][3] = 0; + m[1][0] =-c3 * s1 - c1 * c2 * s3; + m[1][1] = c1 * c3 - c2 * s1 * s3; + m[1][2] = s2 * s3; + m[1][3] = 0; + m[2][0] = c1 * s2; + m[2][1] = s1 * s2; + m[2][2] = c2; + m[2][3] = 0; + m[3][0] = 0; + m[3][1] = 0; + m[3][2] = 0; + m[3][3] = 1; + return; +} + +matrix4_from_euler_angles_zxz_f32 :: proc(t1, t2, t3: f32) -> (m: Matrix4f32) { + 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[0][3] = 0; + m[1][0] =-c1 * s3 - c2 * c3 * s1; + m[1][1] = c1 * c2 * c3 - s1 * s3; + m[1][2] = c3 * s2; + m[1][3] = 0; + m[2][0] = s1 * s2; + m[2][1] =-c1 * s2; + m[2][2] = c2; + m[2][3] = 0; + m[3][0] = 0; + m[3][1] = 0; + m[3][2] = 0; + m[3][3] = 1; + return; +} + + +matrix4_from_euler_angles_xzy_f32 :: proc(t1, t2, t3: f32) -> (m: Matrix4f32) { + 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[0][3] = 0; + m[1][0] =-s2; + m[1][1] = c1 * c2; + m[1][2] = c2 * s1; + m[1][3] = 0; + m[2][0] = c2 * s3; + m[2][1] = c1 * s2 * s3 - c3 * s1; + m[2][2] = c1 * c3 + s1 * s2 *s3; + m[2][3] = 0; + m[3][0] = 0; + m[3][1] = 0; + m[3][2] = 0; + m[3][3] = 1; + return; +} + +matrix4_from_euler_angles_yzx_f32 :: proc(t1, t2, t3: f32) -> (m: Matrix4f32) { + 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[0][3] = 0; + m[1][0] = s1 * s3 - c1 * c3 * s2; + m[1][1] = c2 * c3; + m[1][2] = c1 * s3 + c3 * s1 * s2; + m[1][3] = 0; + m[2][0] = c3 * s1 + c1 * s2 * s3; + m[2][1] =-c2 * s3; + m[2][2] = c1 * c3 - s1 * s2 * s3; + m[2][3] = 0; + m[3][0] = 0; + m[3][1] = 0; + m[3][2] = 0; + m[3][3] = 1; + return; +} + +matrix4_from_euler_angles_zyx_f32 :: proc(t1, t2, t3: f32) -> (m: Matrix4f32) { + 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[0][3] = 0; + m[1][0] = c1 * s2 * s3 - c3 * s1; + m[1][1] = c1 * c3 + s1 * s2 * s3; + m[1][2] = c2 * s3; + m[1][3] = 0; + m[2][0] = s1 * s3 + c1 * c3 * s2; + m[2][1] = c3 * s1 * s2 - c1 * s3; + m[2][2] = c2 * c3; + m[2][3] = 0; + m[3][0] = 0; + m[3][1] = 0; + m[3][2] = 0; + m[3][3] = 1; + return; +} + +matrix4_from_euler_angles_zxy_f32 :: proc(t1, t2, t3: f32) -> (m: Matrix4f32) { + 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[0][3] = 0; + m[1][0] =-c2 * s1; + m[1][1] = c1 * c2; + m[1][2] = s2; + m[1][3] = 0; + m[2][0] = c1 * s3 + c3 * s1 * s2; + m[2][1] = s1 * s3 - c1 * c3 * s2; + m[2][2] = c2 * c3; + m[2][3] = 0; + m[3][0] = 0; + m[3][1] = 0; + m[3][2] = 0; + m[3][3] = 1; + return; +} + + +matrix4_from_yaw_pitch_roll_f32 :: proc(yaw, pitch, roll: f32) -> (m: Matrix4f32) { + 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[0][3] = 0; + m[1][0] = -ch * sb + sh * sp * cb; + m[1][1] = cb * cp; + m[1][2] = sb * sh + ch * sp * cb; + m[1][3] = 0; + m[2][0] = sh * cp; + m[2][1] = -sp; + m[2][2] = ch * cp; + m[2][3] = 0; + m[3][0] = 0; + m[3][1] = 0; + m[3][2] = 0; + m[3][3] = 1; + return m; +} + +euler_angles_xyz_from_matrix4_f32 :: proc(m: Matrix4f32) -> (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_matrix4_f32 :: proc(m: Matrix4f32) -> (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_matrix4_f32 :: proc(m: Matrix4f32) -> (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_matrix4_f32 :: proc(m: Matrix4f32) -> (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_matrix4_f32 :: proc(m: Matrix4f32) -> (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_matrix4_f32 :: proc(m: Matrix4f32) -> (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_matrix4_f32 :: proc(m: Matrix4f32) -> (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_matrix4_f32 :: proc(m: Matrix4f32) -> (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_matrix4_f32 :: proc(m: Matrix4f32) -> (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_matrix4_f32 :: proc(m: Matrix4f32) -> (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_matrix4_f32 :: proc(m: Matrix4f32) -> (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_matrix4_f32 :: proc(m: Matrix4f32) -> (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; +} diff --git a/core/math/linalg/specific_euler_angles_f64.odin b/core/math/linalg/specific_euler_angles_f64.odin new file mode 100644 index 000000000..a3633ec1f --- /dev/null +++ b/core/math/linalg/specific_euler_angles_f64.odin @@ -0,0 +1,797 @@ +package linalg + +import "core:math" + +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); + 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_f64 :: proc(m: Quaternionf64, order: Euler_Angle_Order) -> (t1, t2, t3: f64) { + switch order { + 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; +} + +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); + 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; +} + +quaternion_from_euler_angles_f64 :: proc(t1, t2, t3: f64, order: Euler_Angle_Order) -> Quaternionf64 { + X :: quaternion_from_euler_angle_x; + Y :: quaternion_from_euler_angle_y; + Z :: quaternion_from_euler_angle_z; + + q1, q2, q3: Quaternionf64; + + switch order { + case .XYZ: q1, q2, q3 = X(t1), Y(t2), Z(t3); + case .XZY: q1, q2, q3 = X(t1), Z(t2), Y(t3); + case .YXZ: q1, q2, q3 = Y(t1), X(t2), Z(t3); + case .YZX: q1, q2, q3 = Y(t1), Z(t2), X(t3); + case .ZXY: q1, q2, q3 = Z(t1), X(t2), Y(t3); + case .ZYX: q1, q2, q3 = Z(t1), Y(t2), X(t3); + case .XYX: q1, q2, q3 = X(t1), Y(t2), X(t3); + case .XZX: q1, q2, q3 = X(t1), Z(t2), X(t3); + case .YXY: q1, q2, q3 = Y(t1), X(t2), Y(t3); + case .YZY: q1, q2, q3 = Y(t1), Z(t2), Y(t3); + case .ZXZ: q1, q2, q3 = Z(t1), X(t2), Z(t3); + case .ZYZ: q1, q2, q3 = Z(t1), Y(t2), Z(t3); + } + + return q1 * (q2 * q3); +} + + +// Quaternionf64s + +quaternion_from_euler_angle_x_f64 :: proc(angle_x: f64) -> (q: Quaternionf64) { + return quaternion_angle_axis_f64(angle_x, {1, 0, 0}); +} +quaternion_from_euler_angle_y_f64 :: proc(angle_y: f64) -> (q: Quaternionf64) { + return quaternion_angle_axis_f64(angle_y, {0, 1, 0}); +} +quaternion_from_euler_angle_z_f64 :: proc(angle_z: f64) -> (q: Quaternionf64) { + return quaternion_angle_axis_f64(angle_z, {0, 0, 1}); +} + +quaternion_from_pitch_yaw_roll_f64 :: proc(pitch, yaw, roll: f64) -> Quaternionf64 { + 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: Quaternionf64; + 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_f64 :: proc(q: Quaternionf64) -> f64 { + 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_f64 :: proc(q: Quaternionf64) -> f64 { + 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) <= F64_EPSILON && abs(y) <= F64_EPSILON { + return 2 * math.atan2(q.x, q.w); + } + + return math.atan2(y, x); +} + +yaw_from_quaternion_f64 :: proc(q: Quaternionf64) -> f64 { + return math.asin(clamp(-2 * (q.x*q.z - q.w*q.y), -1, 1)); +} + + +pitch_yaw_roll_from_quaternion_f64 :: proc(q: Quaternionf64) -> (pitch, yaw, roll: f64) { + pitch = pitch_from_quaternion(q); + yaw = yaw_from_quaternion(q); + roll = roll_from_quaternion(q); + return; +} + +euler_angles_xyz_from_quaternion_f64 :: proc(q: Quaternionf64) -> (t1, t2, t3: f64) { + return euler_angles_xyz_from_matrix4(matrix4_from_quaternion(q)); +} +euler_angles_yxz_from_quaternion_f64 :: proc(q: Quaternionf64) -> (t1, t2, t3: f64) { + return euler_angles_yxz_from_matrix4(matrix4_from_quaternion(q)); +} +euler_angles_xzx_from_quaternion_f64 :: proc(q: Quaternionf64) -> (t1, t2, t3: f64) { + return euler_angles_xzx_from_matrix4(matrix4_from_quaternion(q)); +} +euler_angles_xyx_from_quaternion_f64 :: proc(q: Quaternionf64) -> (t1, t2, t3: f64) { + return euler_angles_xyx_from_matrix4(matrix4_from_quaternion(q)); +} +euler_angles_yxy_from_quaternion_f64 :: proc(q: Quaternionf64) -> (t1, t2, t3: f64) { + return euler_angles_yxy_from_matrix4(matrix4_from_quaternion(q)); +} +euler_angles_yzy_from_quaternion_f64 :: proc(q: Quaternionf64) -> (t1, t2, t3: f64) { + return euler_angles_yzy_from_matrix4(matrix4_from_quaternion(q)); +} +euler_angles_zyz_from_quaternion_f64 :: proc(q: Quaternionf64) -> (t1, t2, t3: f64) { + return euler_angles_zyz_from_matrix4(matrix4_from_quaternion(q)); +} +euler_angles_zxz_from_quaternion_f64 :: proc(q: Quaternionf64) -> (t1, t2, t3: f64) { + return euler_angles_zxz_from_matrix4(matrix4_from_quaternion(q)); +} +euler_angles_xzy_from_quaternion_f64 :: proc(q: Quaternionf64) -> (t1, t2, t3: f64) { + return euler_angles_xzy_from_matrix4(matrix4_from_quaternion(q)); +} +euler_angles_yzx_from_quaternion_f64 :: proc(q: Quaternionf64) -> (t1, t2, t3: f64) { + return euler_angles_yzx_from_matrix4(matrix4_from_quaternion(q)); +} +euler_angles_zyx_from_quaternion_f64 :: proc(q: Quaternionf64) -> (t1, t2, t3: f64) { + return euler_angles_zyx_from_matrix4(matrix4_from_quaternion(q)); +} +euler_angles_zxy_from_quaternion_f64 :: proc(q: Quaternionf64) -> (t1, t2, t3: f64) { + return euler_angles_zxy_from_matrix4(matrix4_from_quaternion(q)); +} + + +// Matrices + + +matrix4_from_euler_angle_x_f64 :: proc(angle_x: f64) -> (m: Matrix4f64) { + 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; + m[3][3] = 1; + return; +} +matrix4_from_euler_angle_y_f64 :: proc(angle_y: f64) -> (m: Matrix4f64) { + 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; + m[3][3] = 1; + return; +} +matrix4_from_euler_angle_z_f64 :: proc(angle_z: f64) -> (m: Matrix4f64) { + 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; + m[3][3] = 1; + return; +} + + +matrix4_from_derived_euler_angle_x_f64 :: proc(angle_x: f64, angular_velocity_x: f64) -> (m: Matrix4f64) { + 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; + m[3][3] = 1; + return; +} +matrix4_from_derived_euler_angle_y_f64 :: proc(angle_y: f64, angular_velocity_y: f64) -> (m: Matrix4f64) { + 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; + m[3][3] = 1; + return; +} +matrix4_from_derived_euler_angle_z_f64 :: proc(angle_z: f64, angular_velocity_z: f64) -> (m: Matrix4f64) { + 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; + m[3][3] = 1; + return; +} + + +matrix4_from_euler_angles_xy_f64 :: proc(angle_x, angle_y: f64) -> (m: Matrix4f64) { + 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; + m[3][3] = 1; + return; +} + + +matrix4_from_euler_angles_yx_f64 :: proc(angle_y, angle_x: f64) -> (m: Matrix4f64) { + 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; + m[3][3] = 1; + return; +} + +matrix4_from_euler_angles_xz_f64 :: proc(angle_x, angle_z: f64) -> (m: Matrix4f64) { + return mul(matrix4_from_euler_angle_x(angle_x), matrix4_from_euler_angle_z(angle_z)); +} +matrix4_from_euler_angles_zx_f64 :: proc(angle_z, angle_x: f64) -> (m: Matrix4f64) { + return mul(matrix4_from_euler_angle_z(angle_z), matrix4_from_euler_angle_x(angle_x)); +} +matrix4_from_euler_angles_yz_f64 :: proc(angle_y, angle_z: f64) -> (m: Matrix4f64) { + return mul(matrix4_from_euler_angle_y(angle_y), matrix4_from_euler_angle_z(angle_z)); +} +matrix4_from_euler_angles_zy_f64 :: proc(angle_z, angle_y: f64) -> (m: Matrix4f64) { + return mul(matrix4_from_euler_angle_z(angle_z), matrix4_from_euler_angle_y(angle_y)); +} + + +matrix4_from_euler_angles_xyz_f64 :: proc(t1, t2, t3: f64) -> (m: Matrix4f64) { + 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[0][3] = 0; + m[1][0] = c2 * s3; + m[1][1] = c1 * c3 + s1 * s2 * s3; + m[1][2] =-s1 * c3 + c1 * s2 * s3; + m[1][3] = 0; + m[2][0] =-s2; + m[2][1] = s1 * c2; + m[2][2] = c1 * c2; + m[2][3] = 0; + m[3][0] = 0; + m[3][1] = 0; + m[3][2] = 0; + m[3][3] = 1; + return; +} + +matrix4_from_euler_angles_yxz_f64 :: proc(yaw, pitch, roll: f64) -> (m: Matrix4f64) { + 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[0][3] = 0; + m[1][0] = -ch * sb + sh * sp * cb; + m[1][1] = cb * cp; + m[1][2] = sb * sh + ch * sp * cb; + m[1][3] = 0; + m[2][0] = sh * cp; + m[2][1] = -sp; + m[2][2] = ch * cp; + m[2][3] = 0; + m[3][0] = 0; + m[3][1] = 0; + m[3][2] = 0; + m[3][3] = 1; + return; +} + +matrix4_from_euler_angles_xzx_f64 :: proc(t1, t2, t3: f64) -> (m: Matrix4f64) { + 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[0][3] = 0; + m[1][0] =-c3 * s2; + m[1][1] = c1 * c2 * c3 - s1 * s3; + m[1][2] = c1 * s3 + c2 * c3 * s1; + m[1][3] = 0; + m[2][0] = s2 * s3; + m[2][1] =-c3 * s1 - c1 * c2 * s3; + m[2][2] = c1 * c3 - c2 * s1 * s3; + m[2][3] = 0; + m[3][0] = 0; + m[3][1] = 0; + m[3][2] = 0; + m[3][3] = 1; + return; +} + +matrix4_from_euler_angles_xyx_f64 :: proc(t1, t2, t3: f64) -> (m: Matrix4f64) { + 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[0][3] = 0; + m[1][0] = s2 * s3; + m[1][1] = c1 * c3 - c2 * s1 * s3; + m[1][2] = c3 * s1 + c1 * c2 * s3; + m[1][3] = 0; + m[2][0] = c3 * s2; + m[2][1] =-c1 * s3 - c2 * c3 * s1; + m[2][2] = c1 * c2 * c3 - s1 * s3; + m[2][3] = 0; + m[3][0] = 0; + m[3][1] = 0; + m[3][2] = 0; + m[3][3] = 1; + return; +} + +matrix4_from_euler_angles_yxy_f64 :: proc(t1, t2, t3: f64) -> (m: Matrix4f64) { + 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[0][3] = 0; + m[1][0] = s1 * s2; + m[1][1] = c2; + m[1][2] = c1 * s2; + m[1][3] = 0; + m[2][0] = c1 * s3 + c2 * c3 * s1; + m[2][1] =-c3 * s2; + m[2][2] = c1 * c2 * c3 - s1 * s3; + m[2][3] = 0; + m[3][0] = 0; + m[3][1] = 0; + m[3][2] = 0; + m[3][3] = 1; + return; +} + +matrix4_from_euler_angles_yzy_f64 :: proc(t1, t2, t3: f64) -> (m: Matrix4f64) { + 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[0][3] = 0; + m[1][0] =-c1 * s2; + m[1][1] = c2; + m[1][2] = s1 * s2; + m[1][3] = 0; + m[2][0] = c3 * s1 + c1 * c2 * s3; + m[2][1] = s2 * s3; + m[2][2] = c1 * c3 - c2 * s1 * s3; + m[2][3] = 0; + m[3][0] = 0; + m[3][1] = 0; + m[3][2] = 0; + m[3][3] = 1; + return; +} + +matrix4_from_euler_angles_zyz_f64 :: proc(t1, t2, t3: f64) -> (m: Matrix4f64) { + 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[0][3] = 0; + m[1][0] =-c3 * s1 - c1 * c2 * s3; + m[1][1] = c1 * c3 - c2 * s1 * s3; + m[1][2] = s2 * s3; + m[1][3] = 0; + m[2][0] = c1 * s2; + m[2][1] = s1 * s2; + m[2][2] = c2; + m[2][3] = 0; + m[3][0] = 0; + m[3][1] = 0; + m[3][2] = 0; + m[3][3] = 1; + return; +} + +matrix4_from_euler_angles_zxz_f64 :: proc(t1, t2, t3: f64) -> (m: Matrix4f64) { + 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[0][3] = 0; + m[1][0] =-c1 * s3 - c2 * c3 * s1; + m[1][1] = c1 * c2 * c3 - s1 * s3; + m[1][2] = c3 * s2; + m[1][3] = 0; + m[2][0] = s1 * s2; + m[2][1] =-c1 * s2; + m[2][2] = c2; + m[2][3] = 0; + m[3][0] = 0; + m[3][1] = 0; + m[3][2] = 0; + m[3][3] = 1; + return; +} + + +matrix4_from_euler_angles_xzy_f64 :: proc(t1, t2, t3: f64) -> (m: Matrix4f64) { + 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[0][3] = 0; + m[1][0] =-s2; + m[1][1] = c1 * c2; + m[1][2] = c2 * s1; + m[1][3] = 0; + m[2][0] = c2 * s3; + m[2][1] = c1 * s2 * s3 - c3 * s1; + m[2][2] = c1 * c3 + s1 * s2 *s3; + m[2][3] = 0; + m[3][0] = 0; + m[3][1] = 0; + m[3][2] = 0; + m[3][3] = 1; + return; +} + +matrix4_from_euler_angles_yzx_f64 :: proc(t1, t2, t3: f64) -> (m: Matrix4f64) { + 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[0][3] = 0; + m[1][0] = s1 * s3 - c1 * c3 * s2; + m[1][1] = c2 * c3; + m[1][2] = c1 * s3 + c3 * s1 * s2; + m[1][3] = 0; + m[2][0] = c3 * s1 + c1 * s2 * s3; + m[2][1] =-c2 * s3; + m[2][2] = c1 * c3 - s1 * s2 * s3; + m[2][3] = 0; + m[3][0] = 0; + m[3][1] = 0; + m[3][2] = 0; + m[3][3] = 1; + return; +} + +matrix4_from_euler_angles_zyx_f64 :: proc(t1, t2, t3: f64) -> (m: Matrix4f64) { + 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[0][3] = 0; + m[1][0] = c1 * s2 * s3 - c3 * s1; + m[1][1] = c1 * c3 + s1 * s2 * s3; + m[1][2] = c2 * s3; + m[1][3] = 0; + m[2][0] = s1 * s3 + c1 * c3 * s2; + m[2][1] = c3 * s1 * s2 - c1 * s3; + m[2][2] = c2 * c3; + m[2][3] = 0; + m[3][0] = 0; + m[3][1] = 0; + m[3][2] = 0; + m[3][3] = 1; + return; +} + +matrix4_from_euler_angles_zxy_f64 :: proc(t1, t2, t3: f64) -> (m: Matrix4f64) { + 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[0][3] = 0; + m[1][0] =-c2 * s1; + m[1][1] = c1 * c2; + m[1][2] = s2; + m[1][3] = 0; + m[2][0] = c1 * s3 + c3 * s1 * s2; + m[2][1] = s1 * s3 - c1 * c3 * s2; + m[2][2] = c2 * c3; + m[2][3] = 0; + m[3][0] = 0; + m[3][1] = 0; + m[3][2] = 0; + m[3][3] = 1; + return; +} + + +matrix4_from_yaw_pitch_roll_f64 :: proc(yaw, pitch, roll: f64) -> (m: Matrix4f64) { + 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[0][3] = 0; + m[1][0] = -ch * sb + sh * sp * cb; + m[1][1] = cb * cp; + m[1][2] = sb * sh + ch * sp * cb; + m[1][3] = 0; + m[2][0] = sh * cp; + m[2][1] = -sp; + m[2][2] = ch * cp; + m[2][3] = 0; + m[3][0] = 0; + m[3][1] = 0; + m[3][2] = 0; + m[3][3] = 1; + return m; +} + +euler_angles_xyz_from_matrix4_f64 :: proc(m: Matrix4f64) -> (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_matrix4_f64 :: proc(m: Matrix4f64) -> (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_matrix4_f64 :: proc(m: Matrix4f64) -> (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_matrix4_f64 :: proc(m: Matrix4f64) -> (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_matrix4_f64 :: proc(m: Matrix4f64) -> (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_matrix4_f64 :: proc(m: Matrix4f64) -> (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_matrix4_f64 :: proc(m: Matrix4f64) -> (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_matrix4_f64 :: proc(m: Matrix4f64) -> (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_matrix4_f64 :: proc(m: Matrix4f64) -> (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_matrix4_f64 :: proc(m: Matrix4f64) -> (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_matrix4_f64 :: proc(m: Matrix4f64) -> (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_matrix4_f64 :: proc(m: Matrix4f64) -> (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; +} diff --git a/core/math/linalg/swizzle.odin b/core/math/linalg/swizzle.odin new file mode 100644 index 000000000..335d22b9b --- /dev/null +++ b/core/math/linalg/swizzle.odin @@ -0,0 +1,222 @@ +package linalg + +Scalar_Components :: enum u8 { + x = 0, + r = 0, +} + +Vector2_Components :: enum u8 { + x = 0, + y = 1, + r = 0, + g = 1, +} + +Vector3_Components :: enum u8 { + x = 0, + y = 1, + z = 2, + r = 0, + g = 1, + b = 2, +} + +Vector4_Components :: enum u8 { + x = 0, + y = 1, + z = 2, + w = 3, + r = 0, + g = 1, + b = 2, + a = 3, +} + +scalar_f32_swizzle1 :: proc(f: f32, c0: Scalar_Components) -> f32 { + return f; +} +scalar_f32_swizzle2 :: proc(f: f32, c0, c1: Scalar_Components) -> Vector2f32 { + return {f, f}; +} +scalar_f32_swizzle3 :: proc(f: f32, c0, c1, c2: Scalar_Components) -> Vector3f32 { + return {f, f, f}; +} +scalar_f32_swizzle4 :: proc(f: f32, c0, c1, c2, c3: Scalar_Components) -> Vector4f32 { + return {f, f, f, f}; +} + +vector2f32_swizzle1 :: proc(v: Vector2f32, c0: Vector2_Components) -> f32 { + return v[c0]; +} +vector2f32_swizzle2 :: proc(v: Vector2f32, c0, c1: Vector2_Components) -> Vector2f32 { + return {v[c0], v[c1]}; +} +vector2f32_swizzle3 :: proc(v: Vector2f32, c0, c1, c2: Vector2_Components) -> Vector3f32 { + return {v[c0], v[c1], v[c2]}; +} +vector2f32_swizzle4 :: proc(v: Vector2f32, c0, c1, c2, c3: Vector2_Components) -> Vector4f32 { + return {v[c0], v[c1], v[c2], v[c3]}; +} + + +vector3f32_swizzle1 :: proc(v: Vector3f32, c0: Vector3_Components) -> f32 { + return v[c0]; +} +vector3f32_swizzle2 :: proc(v: Vector3f32, c0, c1: Vector3_Components) -> Vector2f32 { + return {v[c0], v[c1]}; +} +vector3f32_swizzle3 :: proc(v: Vector3f32, c0, c1, c2: Vector3_Components) -> Vector3f32 { + return {v[c0], v[c1], v[c2]}; +} +vector3f32_swizzle4 :: proc(v: Vector3f32, c0, c1, c2, c3: Vector3_Components) -> Vector4f32 { + return {v[c0], v[c1], v[c2], v[c3]}; +} + +vector4f32_swizzle1 :: proc(v: Vector4f32, c0: Vector4_Components) -> f32 { + return v[c0]; +} +vector4f32_swizzle2 :: proc(v: Vector4f32, c0, c1: Vector4_Components) -> Vector2f32 { + return {v[c0], v[c1]}; +} +vector4f32_swizzle3 :: proc(v: Vector4f32, c0, c1, c2: Vector4_Components) -> Vector3f32 { + return {v[c0], v[c1], v[c2]}; +} +vector4f32_swizzle4 :: proc(v: Vector4f32, c0, c1, c2, c3: Vector4_Components) -> Vector4f32 { + return {v[c0], v[c1], v[c2], v[c3]}; +} + + +scalar_f64_swizzle1 :: proc(f: f64, c0: Scalar_Components) -> f64 { + return f; +} +scalar_f64_swizzle2 :: proc(f: f64, c0, c1: Scalar_Components) -> Vector2f64 { + return {f, f}; +} +scalar_f64_swizzle3 :: proc(f: f64, c0, c1, c2: Scalar_Components) -> Vector3f64 { + return {f, f, f}; +} +scalar_f64_swizzle4 :: proc(f: f64, c0, c1, c2, c3: Scalar_Components) -> Vector4f64 { + return {f, f, f, f}; +} + +vector2f64_swizzle1 :: proc(v: Vector2f64, c0: Vector2_Components) -> f64 { + return v[c0]; +} +vector2f64_swizzle2 :: proc(v: Vector2f64, c0, c1: Vector2_Components) -> Vector2f64 { + return {v[c0], v[c1]}; +} +vector2f64_swizzle3 :: proc(v: Vector2f64, c0, c1, c2: Vector2_Components) -> Vector3f64 { + return {v[c0], v[c1], v[c2]}; +} +vector2f64_swizzle4 :: proc(v: Vector2f64, c0, c1, c2, c3: Vector2_Components) -> Vector4f64 { + return {v[c0], v[c1], v[c2], v[c3]}; +} + + +vector3f64_swizzle1 :: proc(v: Vector3f64, c0: Vector3_Components) -> f64 { + return v[c0]; +} +vector3f64_swizzle2 :: proc(v: Vector3f64, c0, c1: Vector3_Components) -> Vector2f64 { + return {v[c0], v[c1]}; +} +vector3f64_swizzle3 :: proc(v: Vector3f64, c0, c1, c2: Vector3_Components) -> Vector3f64 { + return {v[c0], v[c1], v[c2]}; +} +vector3f64_swizzle4 :: proc(v: Vector3f64, c0, c1, c2, c3: Vector3_Components) -> Vector4f64 { + return {v[c0], v[c1], v[c2], v[c3]}; +} + +vector4f64_swizzle1 :: proc(v: Vector4f64, c0: Vector4_Components) -> f64 { + return v[c0]; +} +vector4f64_swizzle2 :: proc(v: Vector4f64, c0, c1: Vector4_Components) -> Vector2f64 { + return {v[c0], v[c1]}; +} +vector4f64_swizzle3 :: proc(v: Vector4f64, c0, c1, c2: Vector4_Components) -> Vector3f64 { + return {v[c0], v[c1], v[c2]}; +} +vector4f64_swizzle4 :: proc(v: Vector4f64, c0, c1, c2, c3: Vector4_Components) -> Vector4f64 { + return {v[c0], v[c1], v[c2], v[c3]}; +} + + + + +scalar_swizzle :: proc{ + scalar_f32_swizzle1, + scalar_f32_swizzle2, + scalar_f32_swizzle3, + scalar_f32_swizzle4, + scalar_f64_swizzle1, + scalar_f64_swizzle2, + scalar_f64_swizzle3, + scalar_f64_swizzle4, +}; + +vector2_swizzle :: proc{ + vector2f32_swizzle1, + vector2f32_swizzle2, + vector2f32_swizzle3, + vector2f32_swizzle4, + vector2f64_swizzle1, + vector2f64_swizzle2, + vector2f64_swizzle3, + vector2f64_swizzle4, +}; + +vector3_swizzle :: proc{ + vector3f32_swizzle1, + vector3f32_swizzle2, + vector3f32_swizzle3, + vector3f32_swizzle4, + vector3f64_swizzle1, + vector3f64_swizzle2, + vector3f64_swizzle3, + vector3f64_swizzle4, +}; + +vector4_swizzle :: proc{ + vector4f32_swizzle1, + vector4f32_swizzle2, + vector4f32_swizzle3, + vector4f32_swizzle4, + vector4f64_swizzle1, + vector4f64_swizzle2, + vector4f64_swizzle3, + vector4f64_swizzle4, +}; + +swizzle :: proc{ + scalar_f32_swizzle1, + scalar_f32_swizzle2, + scalar_f32_swizzle3, + scalar_f32_swizzle4, + scalar_f64_swizzle1, + scalar_f64_swizzle2, + scalar_f64_swizzle3, + scalar_f64_swizzle4, + vector2f32_swizzle1, + vector2f32_swizzle2, + vector2f32_swizzle3, + vector2f32_swizzle4, + vector2f64_swizzle1, + vector2f64_swizzle2, + vector2f64_swizzle3, + vector2f64_swizzle4, + vector3f32_swizzle1, + vector3f32_swizzle2, + vector3f32_swizzle3, + vector3f32_swizzle4, + vector3f64_swizzle1, + vector3f64_swizzle2, + vector3f64_swizzle3, + vector3f64_swizzle4, + vector4f32_swizzle1, + vector4f32_swizzle2, + vector4f32_swizzle3, + vector4f32_swizzle4, + vector4f64_swizzle1, + vector4f64_swizzle2, + vector4f64_swizzle3, + vector4f64_swizzle4, +};