lager/
alg.rs

1use std::{
2    fmt::Debug,
3    ops::{Add, AddAssign, Div, DivAssign, Mul, Neg, Sub, SubAssign},
4};
5
6use crate::isclose::IsClose;
7use crate::{abs::Abs, LUDecomposition};
8
9use crate::{argmax, Matrix};
10
11impl<T, const M: usize, const N: usize> Matrix<T, M, N>
12where
13    T: Copy
14        + Add<Output = T>
15        + Sub<Output = T>
16        + Mul<Output = T>
17        + Div<Output = T>
18        + Neg<Output = T>
19        + Default
20        + AddAssign
21        + SubAssign
22        + DivAssign
23        + Abs
24        + IsClose
25        + Debug
26        + PartialOrd
27        + From<f64>,
28    f64: From<T>,
29{
30    pub fn into_row_echelon(&mut self) {
31        let mut h = 0; /* Initialization of the pivot row */
32        let mut k = 0; /* Initialization of the pivot column */
33
34        while h < M && k < N {
35            /* Find the k-th pivot: */
36
37            let i_max = h + argmax(
38                self.view::<M, 1>([h..M, k..k])
39                    .iter()
40                    .take(M - h) // Takes only the values within the valid range
41                    .map(|el| el.abs()),
42            )
43            .unwrap();
44
45            //let i_max = argmax (i = h..m, A[[i, k]].abs());
46            if self[[i_max, k]] == 0.0.into() {
47                /* No pivot in this column, pass to next column */
48            } else {
49                self.swap_rows(h, i_max);
50                /* Do for all rows below pivot: */
51                for i in h + 1..M {
52                    let f = self[[i, k]] / self[[h, k]];
53                    /* Fill with zeros the lower part of pivot column: */
54                    self[[i, k]] = 0.0.into();
55                    /* Do for all remaining elements in current row: */
56                    for j in (k + 1)..N {
57                        let el = self[[h, j]];
58                        self[[i, j]] -= el * f;
59                    }
60                }
61                /* Increase pivot row and column */
62                h += 1;
63            }
64            k += 1;
65        }
66    }
67
68    pub fn into_reduced_row_echelon(&mut self) {
69        self.into_row_echelon();
70
71        let mut h = M - 1; // Initialization of the pivot row
72        let mut k = 0; // Initialization of the pivot column
73
74        // Find first non-zero element in the last row
75        while self[[h, k]] == 0.0.into() {
76            k += 1;
77        }
78
79        loop {
80            // Make leading coefficient 1
81            let f = self[[h, k]];
82            for i in k..N {
83                self[[h, i]] /= f;
84            }
85
86            // Don't reduce above rows if on the first row
87            if h == 0 {
88                break;
89            }
90
91            // Set each element in each row above to zero
92            for i in 0..h {
93                // For each row above
94                let f = self[[i, k]] / self[[h, k]];
95                for j in k..N {
96                    // For each element in the row that isn't to the left of the coefficient
97                    let el = self[[h, j]];
98                    self[[i, j]] -= el * f;
99                }
100            }
101            h -= 1;
102            k -= 1;
103            while self[[h, k]] == 0.0.into() && k > 0 {
104                k -= 1;
105            }
106        }
107    }
108
109    pub fn inv(&self) -> Matrix<T, M, N>
110    where
111        [(); N + N]: Sized,
112    {
113        // Stack identity matrix to the right
114        let mut augmented = self.hstack(Self::identity());
115
116        // Convert augmented matrix into reduced row echelon form
117        augmented.into_reduced_row_echelon();
118
119        // Remove identity matrix from the left
120        augmented.view([0..M, N..2 * N]).into()
121    }
122
123    pub fn lu(&self) -> LUDecomposition<T, M, N> {
124        let mut mtx = self.clone();
125
126        let mut perms = Matrix::identity();
127        let mut lower = Matrix::zeros();
128        let mut upper = Matrix::zeros();
129
130        for col in 0..N {
131            let i_max = col
132                + argmax(
133                    self.view::<M, 1>([col..M, col..col])
134                        .iter()
135                        .take(M - col) // Takes only the values within the valid range
136                        .map(|el| el.abs()),
137                )
138                .unwrap();
139
140            mtx.swap_rows(col, i_max);
141            perms.swap_rows(col, i_max);
142        }
143
144        // Doolittle's algorithm
145        for i in 0..M {
146            for j in i..N {
147                upper[[i, j]] = mtx[[i, j]];
148                for k in 0..i {
149                    let el = upper[[k, j]];
150                    upper[[i, j]] -= lower[[i, k]] * el;
151                }
152            }
153            for j in i..N {
154                lower[[j, i]] = mtx[[j, i]];
155                for k in 0..i {
156                    let el = lower[[j, k]];
157                    lower[[j, i]] -= el * upper[[k, i]];
158                }
159                lower[[j, i]] /= upper[[i, i]];
160            }
161        }
162
163        LUDecomposition {
164            l: lower,
165            u: upper,
166            p: perms,
167        }
168    }
169
170    pub fn det(&self) -> T {
171        if M == 2 && N == 2 {
172            self[[0, 0]] * self[[1, 1]] - self[[0, 1]] * self[[1, 0]]
173        } else {
174            let decomp = self.lu();
175
176            let num_perms = M - decomp.p.diag().count() - 1;
177            let det_pinv: T = ((-1_f64).powf(num_perms as f64)).into();
178            dbg!(num_perms);
179
180            let lower_det = decomp.l.diag().prod();
181            let upper_det = decomp.u.diag().prod();
182
183            dbg!(det_pinv, lower_det, upper_det);
184
185            det_pinv * lower_det * upper_det
186        }
187    }
188}
lager/alg.rs

lager/
alg.rs
