mirrors/Odin
1
0
Fork 0
mirror of https://github.com/odin-lang/Odin.git synced 2026-05-25 21:28:13 +00:00
Code Issues Packages Projects Releases Wiki Activity
Files
539ea7a003cd8b507e9da73cb352827f73699dce
Odin/core/math/linalg/general.odin
gingerBill ad940e40ca Add linalg.matrix_pseudo_inverse
2026-04-28 15:41:50 +01:00

1216 lines
32 KiB
Odin
Raw Blame History

package linalg
import "core:math"
import "base:builtin"
import "base:intrinsics"
@require import "base:runtime"
// Generic
TAU :: 6.28318530717958647692528676655900576
PI :: 3.14159265358979323846264338327950288
E :: 2.71828182845904523536
τ :: TAU
π :: PI
e :: E
SQRT_TWO :: 1.41421356237309504880168872420969808
SQRT_THREE :: 1.73205080756887729352744634150587236
SQRT_FIVE :: 2.23606797749978969640917366873127623
LN2 :: 0.693147180559945309417232121458176568
LN10 :: 2.30258509299404568401799145468436421
MAX_F64_PRECISION :: 16 // Maximum number of meaningful digits after the decimal point for 'f64'
MAX_F32_PRECISION :: 8 // Maximum number of meaningful digits after the decimal point for 'f32'
RAD_PER_DEG :: TAU/360.0
DEG_PER_RAD :: 360.0/TAU
@private IS_NUMERIC :: intrinsics.type_is_numeric
@private IS_QUATERNION :: intrinsics.type_is_quaternion
@private IS_ARRAY :: intrinsics.type_is_array
@private IS_FLOAT :: intrinsics.type_is_float
@private BASE_TYPE :: intrinsics.type_base_type
@private ELEM_TYPE :: intrinsics.type_elem_type
@(require_results)
scalar_dot :: proc "contextless" (a, b: $T) -> T where IS_FLOAT(T), !IS_ARRAY(T) {
return a * b
}
@(require_results)
vector_dot :: proc "contextless" (a, b: $T/[$N]$E) -> (c: E) where IS_NUMERIC(E) #no_bounds_check {
ab := a * b
when N == 1 {
return ab.x
} else when N == 2 {
return ab.x + ab.y
} else when N == 3 {
return ab.x + ab.y + ab.z
} else when N == 4 {
return ab.x + ab.y + ab.z + ab.w
} else {
for elem in ab {
c += elem
}
return c
}
}
@(require_results)
quaternion64_dot :: proc "contextless" (a, b: $T/quaternion64) -> (c: f16) {
return a.w*b.w + a.x*b.x + a.y*b.y + a.z*b.z
}
@(require_results)
quaternion128_dot :: proc "contextless" (a, b: $T/quaternion128) -> (c: f32) {
return a.w*b.w + a.x*b.x + a.y*b.y + a.z*b.z
}
@(require_results)
quaternion256_dot :: proc "contextless" (a, b: $T/quaternion256) -> (c: f64) {
return a.w*b.w + a.x*b.x + a.y*b.y + a.z*b.z
}
dot :: proc{scalar_dot, vector_dot, quaternion64_dot, quaternion128_dot, quaternion256_dot}
inner_product :: dot
outer_product :: intrinsics.outer_product
@(require_results)
quaternion_inverse :: proc "contextless" (q: $Q) -> Q where IS_QUATERNION(Q) {
return conj(q) * quaternion(w=1.0/dot(q, q), x=0, y=0, z=0)
}
@(require_results)
scalar_cross :: proc "contextless" (a, b: $T) -> T where IS_FLOAT(T), !IS_ARRAY(T) {
return a * b
}
@(require_results)
vector_cross2 :: proc "contextless" (a, b: $T/[2]$E) -> E where IS_NUMERIC(E) {
return a[0]*b[1] - b[0]*a[1]
}
@(require_results)
vector_cross3 :: proc "contextless" (a, b: $T/[3]$E) -> (c: T) where IS_NUMERIC(E) #no_bounds_check {
return a.yzx*b.zxy - b.yzx*a.zxy
}
@(require_results)
quaternion_cross :: proc "contextless" (q1, q2: $Q) -> (q3: Q) where IS_QUATERNION(Q) {
q3.x = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y
q3.y = q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z
q3.z = q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x
q3.w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z
return
}
vector_cross :: proc{scalar_cross, vector_cross2, vector_cross3}
cross :: proc{scalar_cross, vector_cross2, vector_cross3, quaternion_cross}
@(require_results)
vector_normalize :: proc "contextless" (v: $T/[$N]$E) -> T where IS_FLOAT(E) {
return v / length(v)
}
@(require_results)
quaternion_normalize :: proc "contextless" (q: $Q) -> Q where IS_QUATERNION(Q) {
return q/abs(q)
}
normalize :: proc{vector_normalize, quaternion_normalize}
@(require_results)
vector_normalize0 :: proc "contextless" (v: $T/[$N]$E) -> T where IS_FLOAT(E) {
m := length(v)
return 0 if m == 0 else v/m
}
@(require_results)
quaternion_normalize0 :: proc "contextless" (q: $Q) -> Q where IS_QUATERNION(Q) {
m := abs(q)
return 0 if m == 0 else q/m
}
normalize0 :: proc{vector_normalize0, quaternion_normalize0}
@(require_results)
vector_length :: proc "contextless" (v: $T/[$N]$E) -> E where IS_FLOAT(E) {
return #force_inline math.sqrt(#force_inline dot(v, v))
}
@(require_results)
vector_length2 :: proc "contextless" (v: $T/[$N]$E) -> E where IS_NUMERIC(E) {
return #force_inline dot(v, v)
}
@(require_results)
quaternion_length :: proc "contextless" (q: $Q) -> Q where IS_QUATERNION(Q) {
return abs(q)
}
@(require_results)
quaternion_length2 :: proc "contextless" (q: $Q) -> Q where IS_QUATERNION(Q) {
return dot(q, q)
}
@(require_results)
scalar_triple_product :: proc "contextless" (a, b, c: $T/[$N]$E) -> E where IS_NUMERIC(E) {
// a . (b x c)
// b . (c x a)
// c . (a x b)
return dot(a, cross(b, c))
}
@(require_results)
vector_triple_product :: proc "contextless" (a, b, c: $T/[$N]$E) -> T where IS_NUMERIC(E) {
// a x (b x c)
// (a . c)b - (a . b)c
return cross(a, cross(b, c))
}
length :: proc{vector_length, quaternion_length}
length2 :: proc{vector_length2, quaternion_length2}
@(require_results)
clamp_length :: proc "contextless" (v: $T/[$N]$E, a: E) -> T where IS_FLOAT(E) {
if a <= 0 {
return 0
}
m2 := length2(v)
return v if (m2 <= a*a) else (v / sqrt(m2) * a) // returns original when m2 is 0
}
@(require_results)
projection :: proc "contextless" (x, normal: $T/[$N]$E) -> T where IS_NUMERIC(E) {
return dot(x, normal) / dot(normal, normal) * normal
}
@(require_results)
identity_array_based_matrix :: proc "contextless" ($T: typeid/[$N][N]$E) -> (m: T) #no_bounds_check {
for i in 0..<N {
m[i][i] = E(1)
}
return m
}
@(require_results)
identity_matrix :: proc "contextless" ($T: typeid/matrix[$N, N]$E) -> T #no_bounds_check {
return 1
}
identity :: proc{
identity_array_based_matrix,
identity_matrix,
}
transpose :: intrinsics.transpose
@(require_results)
matrix_mul :: proc "contextless" (a, b: $M/matrix[$N, N]$E) -> (c: M)
where !IS_ARRAY(E), IS_NUMERIC(E) #no_bounds_check {
return a * b
}
@(require_results)
matrix_comp_mul :: proc "contextless" (a, b: $M/matrix[$I, $J]$E) -> (c: M)
where !IS_ARRAY(E), IS_NUMERIC(E) #no_bounds_check {
return hadamard_product(a, b)
}
@(require_results)
matrix_mul_differ :: proc "contextless" (a: $A/matrix[$I, $J]$E, b: $B/matrix[J, $K]E) -> (c: matrix[I, K]E)
where !IS_ARRAY(E), IS_NUMERIC(E), I != K #no_bounds_check {
return a * b
}
@(require_results)
matrix_mul_vector :: proc "contextless" (a: $A/matrix[$I, $J]$E, b: $B/[J]E) -> (c: B)
where !IS_ARRAY(E), IS_NUMERIC(E) #no_bounds_check {
return a * b
}
@(require_results)
quaternion_mul_quaternion :: proc "contextless" (q1, q2: $Q) -> Q where IS_QUATERNION(Q) {
return q1 * q2
}
@(require_results)
quaternion64_mul_vector3 :: proc "contextless" (q: $Q/quaternion64, v: $V/[3]$F/f16) -> V {
q := transmute(runtime.Raw_Quaternion64_Vector_Scalar)q
v := v
t := cross(2*q.vector, v)
return V(v + q.scalar*t + cross(q.vector, t))
}
@(require_results)
quaternion128_mul_vector3 :: proc "contextless" (q: $Q/quaternion128, v: $V/[3]$F/f32) -> V {
q := transmute(runtime.Raw_Quaternion128_Vector_Scalar)q
v := v
t := cross(2*q.vector, v)
return V(v + q.scalar*t + cross(q.vector, t))
}
@(require_results)
quaternion256_mul_vector3 :: proc "contextless" (q: $Q/quaternion256, v: $V/[3]$F/f64) -> V {
q := transmute(runtime.Raw_Quaternion256_Vector_Scalar)q
v := v
t := cross(2*q.vector, v)
return V(v + q.scalar*t + cross(q.vector, t))
}
quaternion_mul_vector3 :: proc{quaternion64_mul_vector3, quaternion128_mul_vector3, quaternion256_mul_vector3}
mul :: proc{
matrix_mul,
matrix_mul_differ,
matrix_mul_vector,
quaternion64_mul_vector3,
quaternion128_mul_vector3,
quaternion256_mul_vector3,
quaternion_mul_quaternion,
}
@(require_results)
vector_to_ptr :: proc "contextless" (v: ^$V/[$N]$E) -> ^E where IS_NUMERIC(E), N > 0 #no_bounds_check {
return &v[0]
}
@(require_results)
matrix_to_ptr :: proc "contextless" (m: ^$A/matrix[$I, $J]$E) -> ^E where IS_NUMERIC(E), I > 0, J > 0 #no_bounds_check {
return &m[0, 0]
}
to_ptr :: proc{vector_to_ptr, matrix_to_ptr}
vector_angle_between :: proc "contextless" (a, b: $V/[$N]$E) -> E {
a0 := normalize0(a)
b0 := normalize0(b)
d := clamp(dot(a0, b0), -1, +1)
return math.acos(d)
}
quaternion64_angle_between :: proc "contextless" (a, b: $Q/quaternion64) -> f16 {
c := normalize0(conj(a) * b)
return math.acos(c.w)
}
quaternion128_angle_between :: proc "contextless" (a, b: $Q/quaternion128) -> f32 {
c := normalize0(conj(a) * b)
return math.acos(c.w)
}
quaternion256_angle_between :: proc "contextless" (a, b: $Q/quaternion256) -> f64 {
c := normalize0(conj(a) * b)
return math.acos(c.w)
}
angle_between :: proc{
vector_angle_between,
quaternion64_angle_between,
quaternion128_angle_between,
quaternion256_angle_between,
}
// Splines
@(require_results)
vector_slerp :: proc "contextless" (x, y: $T/[$N]$E, a: E) -> T #no_bounds_check {
cos_alpha := dot(x, y)
alpha := math.acos(cos_alpha)
sin_alpha := math.sin(alpha)
t1 := math.sin((1 - a) * alpha) / sin_alpha
t2 := math.sin(a * alpha) / sin_alpha
return x * t1 + y * t2
}
@(require_results)
catmull_rom :: proc "contextless" (v1, v2, v3, v4: $T/[$N]$E, s: E) -> T #no_bounds_check {
s2 := s*s
s3 := s2*s
f1 := -s3 + 2 * s2 - s
f2 := 3 * s3 - 5 * s2 + 2
f3 := -3 * s3 + 4 * s2 + s
f4 := s3 - s2
return (f1 * v1 + f2 * v2 + f3 * v3 + f4 * v4) * 0.5
}
@(require_results)
hermite :: proc "contextless" (v1, t1, v2, t2: $T/[$N]$E, s: E) -> T #no_bounds_check {
s2 := s*s
s3 := s2*s
f1 := 2 * s3 - 3 * s2 + 1
f2 := -2 * s3 + 3 * s2
f3 := s3 - 2 * s2 + s
f4 := s3 - s2
return f1 * v1 + f2 * v2 + f3 * t1 + f4 * t2
}
@(require_results)
cubic :: proc "contextless" (v1, v2, v3, v4: $T/[$N]$E, s: E) -> T #no_bounds_check {
return ((v1 * s + v2) * s + v3) * s + v4
}
@(require_results)
array_cast :: proc "contextless" (v: $A/[$N]$T, $Elem_Type: typeid) -> [N]Elem_Type #no_bounds_check {
return ([N]Elem_Type)(v)
}
@(require_results)
matrix_cast :: proc "contextless" (v: $A/matrix[$M, $N]$T, $Elem_Type: typeid) -> (w: matrix[M, N]Elem_Type) #no_bounds_check {
for j in 0..<N {
for i in 0..<M {
w[i, j] = Elem_Type(v[i, j])
}
}
return
}
@(require_results) to_f16 :: #force_inline proc(v: $A/[$N]$T) -> [N]f16 { return ([N]f16) (v) }
@(require_results) to_f32 :: #force_inline proc(v: $A/[$N]$T) -> [N]f32 { return ([N]f32) (v) }
@(require_results) to_f64 :: #force_inline proc(v: $A/[$N]$T) -> [N]f64 { return ([N]f64) (v) }
@(require_results) to_i8 :: #force_inline proc(v: $A/[$N]$T) -> [N]i8 { return ([N]i8) (v) }
@(require_results) to_i16 :: #force_inline proc(v: $A/[$N]$T) -> [N]i16 { return ([N]i16) (v) }
@(require_results) to_i32 :: #force_inline proc(v: $A/[$N]$T) -> [N]i32 { return ([N]i32) (v) }
@(require_results) to_i64 :: #force_inline proc(v: $A/[$N]$T) -> [N]i64 { return ([N]i64) (v) }
@(require_results) to_int :: #force_inline proc(v: $A/[$N]$T) -> [N]int { return ([N]int) (v) }
@(require_results) to_u8 :: #force_inline proc(v: $A/[$N]$T) -> [N]u8 { return ([N]u8) (v) }
@(require_results) to_u16 :: #force_inline proc(v: $A/[$N]$T) -> [N]u16 { return ([N]u16) (v) }
@(require_results) to_u32 :: #force_inline proc(v: $A/[$N]$T) -> [N]u32 { return ([N]u32) (v) }
@(require_results) to_u64 :: #force_inline proc(v: $A/[$N]$T) -> [N]u64 { return ([N]u64) (v) }
@(require_results) to_uint :: #force_inline proc(v: $A/[$N]$T) -> [N]uint { return ([N]uint)(v) }
@(require_results) to_complex32 :: #force_inline proc(v: $A/[$N]$T) -> [N]complex32 { return ([N]complex32) (v) }
@(require_results) to_complex64 :: #force_inline proc(v: $A/[$N]$T) -> [N]complex64 { return ([N]complex64) (v) }
@(require_results) to_complex128 :: #force_inline proc(v: $A/[$N]$T) -> [N]complex128 { return ([N]complex128) (v) }
@(require_results) to_quaternion64 :: #force_inline proc(v: $A/[$N]$T) -> [N]quaternion64 { return ([N]quaternion64) (v) }
@(require_results) to_quaternion128 :: #force_inline proc(v: $A/[$N]$T) -> [N]quaternion128 { return ([N]quaternion128)(v) }
@(require_results) to_quaternion256 :: #force_inline proc(v: $A/[$N]$T) -> [N]quaternion256 { return ([N]quaternion256)(v) }
hadamard_product :: intrinsics.hadamard_product
matrix_flatten :: intrinsics.matrix_flatten
determinant :: proc{
matrix1x1_determinant,
matrix2x2_determinant,
matrix3x3_determinant,
matrix4x4_determinant,
matrix5x5_determinant,
matrix6x6_determinant,
matrix7x7_determinant,
matrix8x8_determinant,
}
adjugate :: proc{
matrix1x1_adjugate,
matrix2x2_adjugate,
matrix3x3_adjugate,
matrix4x4_adjugate,
matrix5x5_adjugate,
matrix6x6_adjugate,
matrix7x7_adjugate,
matrix8x8_adjugate,
}
cofactor :: proc{
matrix1x1_cofactor,
matrix2x2_cofactor,
matrix3x3_cofactor,
matrix4x4_cofactor,
matrix5x5_cofactor,
matrix6x6_cofactor,
matrix7x7_cofactor,
matrix8x8_cofactor,
}
inverse_transpose :: proc{
matrix1x1_inverse_transpose,
matrix2x2_inverse_transpose,
matrix3x3_inverse_transpose,
matrix4x4_inverse_transpose,
matrix5x5_inverse_transpose,
matrix6x6_inverse_transpose,
matrix7x7_inverse_transpose,
matrix8x8_inverse_transpose,
}
inverse :: proc{
matrix1x1_inverse,
matrix2x2_inverse,
matrix3x3_inverse,
matrix4x4_inverse,
matrix5x5_inverse,
matrix6x6_inverse,
matrix7x7_inverse,
matrix8x8_inverse,
}
@(require_results)
hermitian_adjoint :: proc "contextless" (m: $M/matrix[$N, N]$T) -> M where intrinsics.type_is_complex(T), N >= 1 #no_bounds_check {
return conj(transpose(m))
}
@(require_results)
trace :: proc "contextless" (m: $M/matrix[$N, N]$T) -> (trace: T) #no_bounds_check {
for i in 0..<N {
trace += m[i, i]
}
return
}
@(require_results)
matrix_minor :: proc "contextless" (m: $M/matrix[$N, N]$T, #any_int row, column: int) -> (minor: T) where N > 1 #no_bounds_check {
K :: int(N-1)
cut_down: matrix[K, K]T
for col_idx in 0..<K {
j := col_idx + int(col_idx >= column)
for row_idx in 0..<K {
i := row_idx + int(row_idx >= row)
cut_down[row_idx, col_idx] = m[i, j]
}
}
return determinant(cut_down)
}
@(require_results)
matrix1x1_determinant :: proc "contextless" (m: $M/matrix[1, 1]$T) -> (det: T) #no_bounds_check {
return m[0, 0]
}
@(require_results)
matrix2x2_determinant :: proc "contextless" (m: $M/matrix[2, 2]$T) -> (det: T) #no_bounds_check {
return m[0, 0]*m[1, 1] - m[0, 1]*m[1, 0]
}
@(require_results)
matrix3x3_determinant :: proc "contextless" (m: $M/matrix[3, 3]$T) -> (det: T) #no_bounds_check {
a := +m[0, 0] * (m[1, 1] * m[2, 2] - m[1, 2] * m[2, 1])
b := -m[0, 1] * (m[1, 0] * m[2, 2] - m[1, 2] * m[2, 0])
c := +m[0, 2] * (m[1, 0] * m[2, 1] - m[1, 1] * m[2, 0])
return a + b + c
}
@(require_results)
matrix4x4_determinant :: proc "contextless" (m: $M/matrix[4, 4]$T) -> (det: T) #no_bounds_check {
s0 := m[0, 0] * m[1, 1] - m[1, 0] * m[0, 1]
s1 := m[0, 0] * m[1, 2] - m[1, 0] * m[0, 2]
s2 := m[0, 0] * m[1, 3] - m[1, 0] * m[0, 3]
s3 := m[0, 1] * m[1, 2] - m[1, 1] * m[0, 2]
s4 := m[0, 1] * m[1, 3] - m[1, 1] * m[0, 3]
s5 := m[0, 2] * m[1, 3] - m[1, 2] * m[0, 3]
c5 := m[2, 2] * m[3, 3] - m[3, 2] * m[2, 3]
c4 := m[2, 1] * m[3, 3] - m[3, 1] * m[2, 3]
c3 := m[2, 1] * m[3, 2] - m[3, 1] * m[2, 2]
c2 := m[2, 0] * m[3, 3] - m[3, 0] * m[2, 3]
c1 := m[2, 0] * m[3, 2] - m[3, 0] * m[2, 2]
c0 := m[2, 0] * m[3, 1] - m[3, 0] * m[2, 1]
det = s0 * c5 - s1 * c4 + s2 * c3 + s3 * c2 - s4 * c1 + s5 * c0
return
}
@(require_results)
matrix5x5_determinant :: proc "contextless" (m: $M/matrix[5, 5]$T) -> (det: T) #no_bounds_check {
return matrix_determinant_generic(m)
}
@(require_results)
matrix6x6_determinant :: proc "contextless" (m: $M/matrix[6, 6]$T) -> (det: T) #no_bounds_check {
return matrix_determinant_generic(m)
}
@(require_results)
matrix7x7_determinant :: proc "contextless" (m: $M/matrix[7, 7]$T) -> (det: T) #no_bounds_check {
return matrix_determinant_generic(m)
}
@(require_results)
matrix8x8_determinant :: proc "contextless" (m: $M/matrix[8, 8]$T) -> (det: T) #no_bounds_check {
return matrix_determinant_generic(m)
}
@(require_results)
matrix_determinant_generic :: proc "contextless" (a: $M/matrix[$N, N]$T) -> T #no_bounds_check {
when N == 1 {
return matrix1x1_determinant(a)
} else when N == 2 {
return matrix2x2_determinant(a)
} else when N == 3 {
return matrix3x3_determinant(a)
} else when N == 4 {
return matrix4x4_determinant(a)
} else {
a := a
det: T = 1
for col in 0..<N {
pivot_row := col
pivot_val := abs(a[col, col])
for row in (col + 1)..<N {
val := abs(a[row, col])
if val > pivot_val {
pivot_val = val
pivot_row = row
}
}
if pivot_val == 0 {
return 0
}
if pivot_row != col {
for k in 0..<N {
t := a[col, k]
a[col, k] = a[pivot_row, k]
a[pivot_row, k] = t
}
det = -det
}
det *= a[col, col]
inv_pivot := 1.0 / a[col, col]
for row in (col + 1)..<N {
factor := a[row, col] * inv_pivot
for k in (col + 1)..<N {
a[row, k] -= factor * a[col, k]
}
}
}
return det
}
}
@(require_results)
matrix1x1_adjugate :: proc "contextless" (x: $M/matrix[1, 1]$T) -> (y: M) #no_bounds_check {
y = x
return
}
@(require_results)
matrix2x2_adjugate :: proc "contextless" (x: $M/matrix[2, 2]$T) -> (y: M) #no_bounds_check {
y[0, 0] = +x[1, 1]
y[0, 1] = -x[0, 1]
y[1, 0] = -x[1, 0]
y[1, 1] = +x[0, 0]
return
}
@(require_results)
matrix3x3_adjugate :: proc "contextless" (m: $M/matrix[3, 3]$T) -> (y: M) #no_bounds_check {
y[0, 0] = +(m[1, 1] * m[2, 2] - m[2, 1] * m[1, 2])
y[1, 0] = -(m[1, 0] * m[2, 2] - m[2, 0] * m[1, 2])
y[2, 0] = +(m[1, 0] * m[2, 1] - m[2, 0] * m[1, 1])
y[0, 1] = -(m[0, 1] * m[2, 2] - m[2, 1] * m[0, 2])
y[1, 1] = +(m[0, 0] * m[2, 2] - m[2, 0] * m[0, 2])
y[2, 1] = -(m[0, 0] * m[2, 1] - m[2, 0] * m[0, 1])
y[0, 2] = +(m[0, 1] * m[1, 2] - m[1, 1] * m[0, 2])
y[1, 2] = -(m[0, 0] * m[1, 2] - m[1, 0] * m[0, 2])
y[2, 2] = +(m[0, 0] * m[1, 1] - m[1, 0] * m[0, 1])
return
}
@(require_results)
matrix4x4_adjugate :: proc "contextless" (x: $M/matrix[4, 4]$T) -> (y: M) #no_bounds_check {
s0 := x[0, 0] * x[1, 1] - x[1, 0] * x[0, 1]
s1 := x[0, 0] * x[1, 2] - x[1, 0] * x[0, 2]
s2 := x[0, 0] * x[1, 3] - x[1, 0] * x[0, 3]
s3 := x[0, 1] * x[1, 2] - x[1, 1] * x[0, 2]
s4 := x[0, 1] * x[1, 3] - x[1, 1] * x[0, 3]
s5 := x[0, 2] * x[1, 3] - x[1, 2] * x[0, 3]
c5 := x[2, 2] * x[3, 3] - x[3, 2] * x[2, 3]
c4 := x[2, 1] * x[3, 3] - x[3, 1] * x[2, 3]
c3 := x[2, 1] * x[3, 2] - x[3, 1] * x[2, 2]
c2 := x[2, 0] * x[3, 3] - x[3, 0] * x[2, 3]
c1 := x[2, 0] * x[3, 2] - x[3, 0] * x[2, 2]
c0 := x[2, 0] * x[3, 1] - x[3, 0] * x[2, 1]
y[0, 0] = x[1, 1] * c5 - x[1, 2] * c4 + x[1, 3] * c3
y[0, 1] = -x[0, 1] * c5 + x[0, 2] * c4 - x[0, 3] * c3
y[0, 2] = x[3, 1] * s5 - x[3, 2] * s4 + x[3, 3] * s3
y[0, 3] = -x[2, 1] * s5 + x[2, 2] * s4 - x[2, 3] * s3
y[1, 0] = -x[1, 0] * c5 + x[1, 2] * c2 - x[1, 3] * c1
y[1, 1] = x[0, 0] * c5 - x[0, 2] * c2 + x[0, 3] * c1
y[1, 2] = -x[3, 0] * s5 + x[3, 2] * s2 - x[3, 3] * s1
y[1, 3] = x[2, 0] * s5 - x[2, 2] * s2 + x[2, 3] * s1
y[2, 0] = x[1, 0] * c4 - x[1, 1] * c2 + x[1, 3] * c0
y[2, 1] = -x[0, 0] * c4 + x[0, 1] * c2 - x[0, 3] * c0
y[2, 2] = x[3, 0] * s4 - x[3, 1] * s2 + x[3, 3] * s0
y[2, 3] = -x[2, 0] * s4 + x[2, 1] * s2 - x[2, 3] * s0
y[3, 0] = -x[1, 0] * c3 + x[1, 1] * c1 - x[1, 2] * c0
y[3, 1] = x[0, 0] * c3 - x[0, 1] * c1 + x[0, 2] * c0
y[3, 2] = -x[3, 0] * s3 + x[3, 1] * s1 - x[3, 2] * s0
y[3, 3] = x[2, 0] * s3 - x[2, 1] * s1 + x[2, 2] * s0
return
}
@(require_results)
matrix5x5_adjugate :: proc "contextless" (x: $M/matrix[5, 5]$T) -> (y: M) #no_bounds_check {
return matrix_adjugate_general(x)
}
@(require_results)
matrix6x6_adjugate :: proc "contextless" (x: $M/matrix[6, 6]$T) -> (y: M) #no_bounds_check {
return matrix_adjugate_general(x)
}
@(require_results)
matrix7x7_adjugate :: proc "contextless" (x: $M/matrix[7, 7]$T) -> (y: M) #no_bounds_check {
return matrix_adjugate_general(x)
}
@(require_results)
matrix8x8_adjugate :: proc "contextless" (x: $M/matrix[8, 8]$T) -> (y: M) #no_bounds_check {
return matrix_adjugate_general(x)
}
@(require_results)
matrix_adjugate_general :: proc "contextless" (x: $M/matrix[$N, N]$T) -> (y: M) where N >= 2 #no_bounds_check {
for row in 0..<N {
for col in 0..<N {
// Build (N-1)x(N-1) minor by excluding row and col
minor: matrix[N - 1, N - 1]T
mi := 0
for r in 0..<N {
if r == row {
continue
}
mj := 0
for c in 0..<N {
if c == col {
continue
}
minor[mi, mj] = x[r, c]
mj += 1
}
mi += 1
}
// Adjugate is the transpose of the cofactor matrix
// so cofactor[row, col] goes into y[col, row]
sign: T = -1 if (row + col) % 2 == 1 else 1
y[col, row] = sign * matrix_determinant_generic(minor)
}
}
return
}
@(require_results)
matrix1x1_cofactor :: proc "contextless" (x: $M/matrix[1, 1]$T) -> (y: M) #no_bounds_check {
y = x
return
}
@(require_results)
matrix2x2_cofactor :: proc "contextless" (x: $M/matrix[2, 2]$T) -> (y: M) #no_bounds_check {
y[0, 0] = +x[1, 1]
y[0, 1] = -x[1, 0]
y[1, 0] = -x[0, 1]
y[1, 1] = +x[0, 0]
return
}
@(require_results)
matrix3x3_cofactor :: proc "contextless" (m: $M/matrix[3, 3]$T) -> (y: M) #no_bounds_check {
y[0, 0] = +(m[1, 1] * m[2, 2] - m[2, 1] * m[1, 2])
y[0, 1] = -(m[1, 0] * m[2, 2] - m[2, 0] * m[1, 2])
y[0, 2] = +(m[1, 0] * m[2, 1] - m[2, 0] * m[1, 1])
y[1, 0] = -(m[0, 1] * m[2, 2] - m[2, 1] * m[0, 2])
y[1, 1] = +(m[0, 0] * m[2, 2] - m[2, 0] * m[0, 2])
y[1, 2] = -(m[0, 0] * m[2, 1] - m[2, 0] * m[0, 1])
y[2, 0] = +(m[0, 1] * m[1, 2] - m[1, 1] * m[0, 2])
y[2, 1] = -(m[0, 0] * m[1, 2] - m[1, 0] * m[0, 2])
y[2, 2] = +(m[0, 0] * m[1, 1] - m[1, 0] * m[0, 1])
return
}
@(require_results)
matrix4x4_cofactor :: proc "contextless" (x: $M/matrix[4, 4]$T) -> (y: M) #no_bounds_check {
s0 := x[0, 0] * x[1, 1] - x[1, 0] * x[0, 1]
s1 := x[0, 0] * x[1, 2] - x[1, 0] * x[0, 2]
s2 := x[0, 0] * x[1, 3] - x[1, 0] * x[0, 3]
s3 := x[0, 1] * x[1, 2] - x[1, 1] * x[0, 2]
s4 := x[0, 1] * x[1, 3] - x[1, 1] * x[0, 3]
s5 := x[0, 2] * x[1, 3] - x[1, 2] * x[0, 3]
c5 := x[2, 2] * x[3, 3] - x[3, 2] * x[2, 3]
c4 := x[2, 1] * x[3, 3] - x[3, 1] * x[2, 3]
c3 := x[2, 1] * x[3, 2] - x[3, 1] * x[2, 2]
c2 := x[2, 0] * x[3, 3] - x[3, 0] * x[2, 3]
c1 := x[2, 0] * x[3, 2] - x[3, 0] * x[2, 2]
c0 := x[2, 0] * x[3, 1] - x[3, 0] * x[2, 1]
// Cofactor = transpose of adjugate
y[0, 0] = x[1, 1] * c5 - x[1, 2] * c4 + x[1, 3] * c3
y[1, 0] = -x[0, 1] * c5 + x[0, 2] * c4 - x[0, 3] * c3
y[2, 0] = x[3, 1] * s5 - x[3, 2] * s4 + x[3, 3] * s3
y[3, 0] = -x[2, 1] * s5 + x[2, 2] * s4 - x[2, 3] * s3
y[0, 1] = -x[1, 0] * c5 + x[1, 2] * c2 - x[1, 3] * c1
y[1, 1] = x[0, 0] * c5 - x[0, 2] * c2 + x[0, 3] * c1
y[2, 1] = -x[3, 0] * s5 + x[3, 2] * s2 - x[3, 3] * s1
y[3, 1] = x[2, 0] * s5 - x[2, 2] * s2 + x[2, 3] * s1
y[0, 2] = x[1, 0] * c4 - x[1, 1] * c2 + x[1, 3] * c0
y[1, 2] = -x[0, 0] * c4 + x[0, 1] * c2 - x[0, 3] * c0
y[2, 2] = x[3, 0] * s4 - x[3, 1] * s2 + x[3, 3] * s0
y[3, 2] = -x[2, 0] * s4 + x[2, 1] * s2 - x[2, 3] * s0
y[0, 3] = -x[1, 0] * c3 + x[1, 1] * c1 - x[1, 2] * c0
y[1, 3] = x[0, 0] * c3 - x[0, 1] * c1 + x[0, 2] * c0
y[2, 3] = -x[3, 0] * s3 + x[3, 1] * s1 - x[3, 2] * s0
y[3, 3] = x[2, 0] * s3 - x[2, 1] * s1 + x[2, 2] * s0
return
}
@(require_results)
matrix5x5_cofactor :: proc "contextless" (x: $M/matrix[5, 5]$T) -> (y: M) #no_bounds_check {
return matrix_cofactor_general(x)
}
@(require_results)
matrix6x6_cofactor :: proc "contextless" (x: $M/matrix[6, 6]$T) -> (y: M) #no_bounds_check {
return matrix_cofactor_general(x)
}
@(require_results)
matrix7x7_cofactor :: proc "contextless" (x: $M/matrix[7, 7]$T) -> (y: M) #no_bounds_check {
return matrix_cofactor_general(x)
}
@(require_results)
matrix8x8_cofactor :: proc "contextless" (x: $M/matrix[8, 8]$T) -> (y: M) #no_bounds_check {
return matrix_cofactor_general(x)
}
@(require_results)
matrix_cofactor_general :: proc "contextless" (x: $M/matrix[$N, N]$T) -> (y: M) where N >= 2 #no_bounds_check {
for row in 0..<N {
for col in 0..<N {
// Build (N-1)x(N-1) minor by excluding row and col
minor: matrix[N - 1, N - 1]T
mi := 0
for r in 0..<N {
if r == row {
continue
}
mj := 0
for c in 0..<N {
if c == col {
continue
}
minor[mi, mj] = x[r, c]
mj += 1
}
mi += 1
}
// Cofactor sign: (-1)^(row+col)
sign: T = -1 if (row + col) % 2 == 1 else 1
y[row, col] = sign * matrix_determinant_generic(minor)
}
}
return
}
@(require_results)
matrix1x1_inverse_transpose :: proc "contextless" (x: $M/matrix[1, 1]$T) -> (y: M) #no_bounds_check {
y[0, 0] = 1/x[0, 0]
return
}
@(require_results)
matrix2x2_inverse_transpose :: proc "contextless" (x: $M/matrix[2, 2]$T) -> (y: M) #no_bounds_check {
d := x[0, 0]*x[1, 1] - x[0, 1]*x[1, 0]
when intrinsics.type_is_integer(T) {
y[0, 0] = +x[1, 1] / d
y[1, 0] = -x[0, 1] / d
y[0, 1] = -x[1, 0] / d
y[1, 1] = +x[0, 0] / d
} else {
id := 1 / d
y[0, 0] = +x[1, 1] * id
y[1, 0] = -x[0, 1] * id
y[0, 1] = -x[1, 0] * id
y[1, 1] = +x[0, 0] * id
}
return
}
@(require_results)
matrix3x3_inverse_transpose :: proc "contextless" (x: $M/matrix[3, 3]$T) -> (y: M) #no_bounds_check {
c := cofactor(x)
d := determinant(x)
when intrinsics.type_is_integer(T) {
for i in 0..<3 {
for j in 0..<3 {
y[i, j] = c[i, j] / d
}
}
} else {
id := 1/d
for i in 0..<3 {
for j in 0..<3 {
y[i, j] = c[i, j] * id
}
}
}
return
}
@(require_results)
matrix4x4_inverse_transpose :: proc "contextless" (x: $M/matrix[4, 4]$T) -> (y: M) #no_bounds_check {
c := cofactor(x)
d: T
for i in 0..<4 {
d += x[0, i] * c[0, i]
}
when intrinsics.type_is_integer(T) {
for i in 0..<4 {
for j in 0..<4 {
y[i, j] = c[i, j] / d
}
}
} else {
id := 1/d
for i in 0..<4 {
for j in 0..<4 {
y[i, j] = c[i, j] * id
}
}
}
return
}
@(require_results)
matrix5x5_inverse_transpose :: proc "contextless" (x: $M/matrix[5, 5]$T) -> (y: M) #no_bounds_check {
y, _ = matrix_inverse_lu_decomposition(x)
return transpose(y)
}
@(require_results)
matrix6x6_inverse_transpose :: proc "contextless" (x: $M/matrix[6, 6]$T) -> (y: M) #no_bounds_check {
y, _ = matrix_inverse_lu_decomposition(x)
return transpose(y)
}
@(require_results)
matrix7x7_inverse_transpose :: proc "contextless" (x: $M/matrix[7, 7]$T) -> (y: M) #no_bounds_check {
y, _ = matrix_inverse_lu_decomposition(x)
return transpose(y)
}
@(require_results)
matrix8x8_inverse_transpose :: proc "contextless" (x: $M/matrix[8, 8]$T) -> (y: M) #no_bounds_check {
y, _ = matrix_inverse_lu_decomposition(x)
return transpose(y)
}
@(require_results)
matrix1x1_inverse :: proc "contextless" (x: $M/matrix[1, 1]$T) -> (y: M) #no_bounds_check {
y[0, 0] = 1/x[0, 0]
return transpose(y)
}
@(require_results)
matrix2x2_inverse :: proc "contextless" (x: $M/matrix[2, 2]$T) -> (y: M) #no_bounds_check {
d := x[0, 0]*x[1, 1] - x[0, 1]*x[1, 0]
when intrinsics.type_is_integer(T) {
y[0, 0] = +x[1, 1] / d
y[0, 1] = -x[0, 1] / d
y[1, 0] = -x[1, 0] / d
y[1, 1] = +x[0, 0] / d
} else {
id := 1 / d
y[0, 0] = +x[1, 1] * id
y[0, 1] = -x[0, 1] * id
y[1, 0] = -x[1, 0] * id
y[1, 1] = +x[0, 0] * id
}
return
}
@(require_results)
matrix3x3_inverse :: proc "contextless" (x: $M/matrix[3, 3]$T) -> (y: M) #no_bounds_check {
c := cofactor(x)
d := determinant(x)
when intrinsics.type_is_integer(T) {
for i in 0..<3 {
for j in 0..<3 {
y[i, j] = c[j, i] / d
}
}
} else {
id := 1/d
for i in 0..<3 {
for j in 0..<3 {
y[i, j] = c[j, i] * id
}
}
}
return
}
@(require_results)
matrix4x4_inverse :: proc "contextless" (x: $M/matrix[4, 4]$T) -> (y: M) #no_bounds_check {
c := cofactor(x)
d: T
for i in 0..<4 {
d += x[0, i] * c[0, i]
}
when intrinsics.type_is_integer(T) {
for i in 0..<4 {
for j in 0..<4 {
y[i, j] = c[j, i] / d
}
}
} else {
id := 1/d
for i in 0..<4 {
for j in 0..<4 {
y[i, j] = c[j, i] * id
}
}
}
return
}
@(require_results)
matrix5x5_inverse :: proc "contextless" (x: $M/matrix[5, 5]$T) -> (y: M) #no_bounds_check {
y, _ = matrix_inverse_lu_decomposition(x)
return
}
@(require_results)
matrix6x6_inverse :: proc "contextless" (x: $M/matrix[6, 6]$T) -> (y: M) #no_bounds_check {
y, _ = matrix_inverse_lu_decomposition(x)
return
}
@(require_results)
matrix7x7_inverse :: proc "contextless" (x: $M/matrix[7, 7]$T) -> (y: M) #no_bounds_check {
y, _ = matrix_inverse_lu_decomposition(x)
return
}
@(require_results)
matrix8x8_inverse :: proc "contextless" (x: $M/matrix[8, 8]$T) -> (y: M) #no_bounds_check {
y, _ = matrix_inverse_lu_decomposition(x)
return
}
@(private="file")
swap :: #force_inline proc "contextless" (a, b: ^$T) {
t := a^
a^ = b^
b^ = t
}
@(require_results)
matrix_inverse_gauss_jordan :: proc "contextless" (A: $M/matrix[$N, N]$T) -> (b: M, non_singular: bool) #no_bounds_check {
a := A
b = 1
for col in 0..<N {
pivot_row := col
pivot_val := abs(a[col, col])
for row in (col + 1)..<N {
val := abs(a[row, col])
if val > pivot_val {
pivot_val = val
pivot_row = row
}
}
if pivot_val == 0 {
return
}
if pivot_row != col {
for k in 0..<N {
swap(&a[col, k], &a[pivot_row, k])
swap(&b[col, k], &b[pivot_row, k])
}
}
when intrinsics.type_is_integer(T) {
for k in 0..<N {
a[col, k] /= a[col, col]
b[col, k] /= a[col, col]
}
} else {
inv_pivot := 1.0 / a[col, col]
for k in 0..<N {
a[col, k] *= inv_pivot
b[col, k] *= inv_pivot
}
}
for row in 0..<N {
if row == col {
continue
}
factor := a[row, col]
for k in 0..<N {
a[row, k] -= factor * a[col, k]
b[row, k] -= factor * b[col, k]
}
}
}
non_singular = true
return
}
@(require_results)
matrix_inverse_lu_decomposition :: proc "contextless" (A: $M/matrix[$N, N]$T) -> (b: M, non_singular: bool) #no_bounds_check {
// LU decomposition with partial pivoting
a := A
pivot: [N]int = ---
for i in 0..<N {
pivot[i] = i
}
for col in 0..<N {
pivot_row := col
pivot_val := abs(a[col, col])
for row in (col + 1)..<N {
val := abs(a[row, col])
if val > pivot_val {
pivot_val = val
pivot_row = row
}
}
if pivot_val == 0 {
return
}
// Swap pivot indices
if pivot_row != col {
t := pivot[col]
pivot[col] = pivot[pivot_row]
pivot[pivot_row] = t
for k in 0..<N {
swap(&a[col, k], &a[pivot_row, k])
}
}
// Compute L and U in place
when intrinsics.type_is_integer(T) {
for row in (col + 1)..<N {
a[row, col] /= a[col, col]
for k in (col + 1)..<N {
a[row, k] -= a[row, col] * a[col, k]
}
}
} else {
inv_pivot := 1 / a[col, col]
for row in (col + 1)..<N {
a[row, col] *= inv_pivot
for k in (col + 1)..<N {
a[row, k] -= a[row, col] * a[col, k]
}
}
}
}
// Solve for each column of the inverse
b = 0
for col in 0..<N {
// Apply pivot permutation to identity column
x: [N]T = ---
for i in 0..<N {
x[i] = 1 if pivot[i] == col else 0
}
// Forward substitution (L * y = Pb)
for row in 1..<N {
sum: T = 0
for k in 0..<row {
sum += a[row, k] * x[k]
}
x[row] -= sum
}
// Back substitution (U * x = y)
for i in 0..<N {
row := N - 1 - i
sum: T = 0
for k in (row + 1)..<N {
sum += a[row, k] * x[k]
}
x[row] = (x[row] - sum) / a[row, row]
}
// Store result column
for row in 0..<N {
b[row, col] = x[row]
}
}
non_singular = true
return
}
pseudo_inverse :: proc{
matrix_pseudo_inverse,
}
// Moore-Penrose Pseudo-Inverse
@(require_results)
matrix_pseudo_inverse :: proc "contextless" (A: $M/matrix[$R, $C]$T) -> (B: matrix[C, R]T, not_singular: bool) #no_bounds_check {
At := transpose(A)
when R >= C {
// Overdetermined (tall):
// pseudo_inverse(A) = inverse(transpose(A)*A)*transpose(A)
AtA := At * A // [C, C]
AtA_inv := matrix_inverse_generic(AtA) or_return
B = AtA_inv * At
not_singular = true
} else {
// Underdetermined (wide):
// pseudo_inverse(A) = transpose(A)*inverse(A*transpose(A))
AAt := A * At // [R, R]
AAt_inv := matrix_inverse_generic(AAt) or_return
B = At * AAt_inv
not_singular = true
}
return
}
Reference in New Issue View Git Blame Copy Permalink