math_utils/algebra/
linear.rs

1//! Linear algebra routines
2
3use crate::{approx, num};
4use crate::traits::*;
5use crate::types::*;
6
7#[derive(Clone, Copy, Debug, Eq, PartialEq)]
8pub struct UpperHessenberg3 <S> (Matrix3 <S>);
9
10/// Returns matrices `(q, h)` of the decomposition `m = qhq^T` where `q` is an
11/// orthogonal matrix and `h` is an upper-Hessenberg matrix.
12///
13/// Returns `None` if `m` is the zero matrix:
14///
15/// ```
16/// # use math_utils::Matrix3;
17/// # use math_utils::algebra::linear::decompose_hessenberg_3;
18/// let mat = Matrix3::<f32>::zero();
19/// assert_eq!(decompose_hessenberg_3 (mat), None);
20/// ```
21pub fn decompose_hessenberg_3 <S : Real> (m : Matrix3 <S>)
22  -> Option <(Matrix3 <S>, UpperHessenberg3 <S>)>
23{
24  // algorithm from mathru crate:
25  // <https://gitlab.com/rustmath/mathru/-/blob/a8feca07b9f171ce494363634b78d55178193cf8/src/algebra/linear/matrix/general/hessenbergdec/native.rs>
26  if m == Matrix3::zero() {
27    return None
28  }
29  let v = m.cols[0];
30  let househ = Matrix3::elementary_reflector (v, 1);
31  let q = househ;
32  let h = UpperHessenberg3 (q.transpose() * m * q);
33  Some ((q, h))
34}
35
36/// Returns matrices `(q, u)` of the decomposition `h = quq^{-1}` where `q` is an
37/// orthogonal matrix and `u` is an upper quasi-triangular matrix
38pub fn decompose_schur_3 <S> (h : UpperHessenberg3 <S>) -> (Matrix3 <S>, Matrix3 <S>)
39  where S : Real + approx::AbsDiffEq <Epsilon=S>
40{
41  // algorithm from mathru crate:
42  // <https://gitlab.com/rustmath/mathru/-/blob/a8feca07b9f171ce494363634b78d55178193cf8/src/algebra/linear/matrix/upperhessenberg/schurdec/native.rs>
43  // TODO: to avoid errors we are using row-major arrays to match the indexing in the
44  // original algorithm; it would probably be more performant to do everything with
45  // column-major matrices
46  fn get_slice <S : Ring> (
47    rows : [[S; 3]; 3], row_s : usize, row_e : usize, column_s : usize, column_e : usize
48  ) -> [[S; 3]; 3] {
49    debug_assert!(row_s < 3);
50    debug_assert!(row_e < 3);
51    debug_assert!(column_s < 3);
52    debug_assert!(column_e < 3);
53    let mut m = [[S::zero(); 3]; 3];
54    for r in row_s..=row_e {
55      for c in column_s..=column_e {
56        m[r - row_s][c - column_s] = rows[r][c];
57      }
58    }
59    m
60  }
61  fn set_slice <S : Copy> (
62    mut rows : [[S; 3]; 3], slice : [[S; 3]; 3],
63    row_s : usize, row_e : usize, column_s : usize, column_e : usize
64  ) -> [[S; 3]; 3] {
65    debug_assert!(row_s < 3);
66    debug_assert!(row_e < 3);
67    debug_assert!(column_s < 3);
68    debug_assert!(column_e < 3);
69    for r in row_s..=row_e {
70      for c in column_s..=column_e {
71        rows[r][c] = slice[r - row_s][c - column_s];
72      }
73    }
74    rows
75  }
76  fn givens_cosine_sine_pair <S : Real> (a : S, b : S) -> (S, S) {
77    if b == S::zero() {
78      (S::one(), S::zero())
79    } else if a == S::zero() {
80      (S::zero(), S::one())
81    } else {
82      let l = (a * a + b * b).sqrt();
83      (a.abs() / l, a.signum_or_zero() * b / l)
84    }
85  }
86  // Francis QR algorithm
87  let mut h = h.mat().into_row_arrays();
88  let mut u = Matrix3::<S>::identity().into_row_arrays();
89  loop {
90    // bulge generation
91    let h_qq = h[1][1];
92    let h_pp = h[2][2];
93    let s = h_qq + h_pp;
94    let h_qp = h[1][2];
95    let h_pq = h[2][1];
96    let t = h_qq * h_pp - h_qp * h_pq;
97    // first 3 elements of first column of m
98    let h_11 = h[0][0];
99    let h_12 = h[0][1];
100    let h_21 = h[1][0];
101    let mut x = h_11 * h_11 + h_12 * h_21 - s * h_11 + t;
102    let h_22 = h[1][1];
103    let mut y = h_21 * (h_11 + h_22 - s);
104    let h_32 = h[2][1];
105    let z = h_21 * h_32;
106    let b = vector3 (x, y, z);
107    let hr = Matrix3::elementary_reflector (b, 0);
108    // determine the householder reflector P with P^T [x; y; z;]^T = αe1
109    h = (hr.transpose() * Matrix3::from_row_arrays (get_slice (h, 0, 2, 0, 2)))
110      .into_row_arrays();
111    h = (Matrix3::from_row_arrays (get_slice (h, 0, 2, 0, 2)) * hr)
112      .into_row_arrays();
113    u = (Matrix3::from_row_arrays (get_slice (u, 0, 2, 0, 2)) * hr)
114      .into_row_arrays();
115
116    let h_k2_k1 = h[1][0];
117    x = h_k2_k1;
118    let h_k3_k1 = h[2][0];
119    y = h_k3_k1;
120    // determine the Givens rotation P with P [x; y]^T = αe1
121    let (c, s) = givens_cosine_sine_pair (x, y);
122    // 2x2
123    let g = Matrix3::fill_zeros (Matrix2::from_row_arrays ([
124      [c, s],
125      [-s ,c]
126    ]));
127    {
128      // 2x2 @ 2x3 -> 2x3
129      let temp = (g * Matrix3::from_row_arrays (get_slice (h, 1, 2, 0, 2)))
130        .into_row_arrays();
131      // get 2 rows and 3 cols from temp
132      h = set_slice (h, temp, 1, 2, 0, 2);
133    }
134    {
135      // 2x2
136      let g_trans = g.transpose();
137      // 3x2 @ 2x2 -> 3x2
138      let temp = (Matrix3::from_row_arrays (get_slice (h, 0, 2, 1, 2)) * g_trans)
139        .into_row_arrays();
140      // get 3 rows and 2 cols from temp
141      h = set_slice (h, temp, 0, 2, 1, 2);
142      // 3x2 @ 2x2 -> 3x2
143      let u_slice =
144        (Matrix3::from_row_arrays (get_slice (h, 0, 2, 1, 2)) * g_trans)
145          .into_row_arrays();
146      // get 3 rows and 2 cols from u_slice
147      u = set_slice (u, u_slice, 0, 2, 1, 2);
148    }
149    // check for convergence
150    let m = h[1][1].abs();
151    let n = h[2][2].abs();
152    if h[2][1].abs() < S::default_epsilon() * (m + n) {
153      h[2][1] = S::zero();
154      break
155    } else {
156      let k = h[0][0].abs();
157      let l = h[1][1].abs();
158      if h[1][0].abs() < S::default_epsilon() * (k + l) {
159        h[1][0] = S::zero();
160        break
161      }
162    }
163  }
164  (Matrix3::from_row_arrays (u), Matrix3::from_row_arrays (h))
165}
166
167/// Return the (real-valued) eigenvalues and eigenvectors (eigenpairs) of a
168/// diagonalizable 2x2 matrix.
169///
170/// Will be ordered by descending eigenvalues.
171///
172/// Returns `[Some((0, Vector2::zero())), None]` if the matrix is the zero matrix:
173///
174/// ```
175/// # use math_utils::{Matrix2, Vector2};
176/// # use math_utils::algebra::linear::eigensystem_2;
177/// let mat = Matrix2::<f32>::zero();
178/// assert_eq!(eigensystem_2 (mat), [Some ((0.0, Vector2::zero())), None]);
179/// ```
180///
181/// Returns `[None, None]` if eigenvalues are not real values (matrix is orthogonal,
182/// e.g. a rotation):
183///
184/// ```
185/// # use math_utils::Matrix2;
186/// # use math_utils::algebra::linear::eigensystem_2;
187/// let mat = Matrix2::<f32>::from_row_arrays ([
188///   [0.0, -1.0],
189///   [1.0,  0.0]
190/// ]);
191/// assert_eq!(eigensystem_2 (mat), [None, None]);
192/// ```
193///
194/// Returns `[Some, None]` if the matrix has only one eigenvector direction (matrix is
195/// not diagonalizable):
196///
197/// ```
198/// # use math_utils::{Matrix2, Vector2};
199/// # use math_utils::algebra::linear::eigensystem_2;
200/// let mat = Matrix2::<f32>::from_row_arrays ([
201///   [ 3.0, 1.0],
202///   [-1.0, 1.0]
203/// ]);
204/// assert!(matches!(eigensystem_2 (mat), [Some(_), None]));
205/// ```
206pub fn eigensystem_2 <S> (matrix : Matrix2 <S>) -> [Option <(S, Vector2 <S>)>; 2] where
207  S : OrderedField + Sqrt + approx::AbsDiffEq <Epsilon=S> + std::fmt::Debug
208{
209  if matrix == Matrix2::zero() {
210    return [Some ((S::zero(), Vector2::zero())), None]
211  }
212  let mut eigenpairs = if matrix[(0,1)] == S::zero() && matrix[(1,0)] == S::zero() {
213    // diagonal matrix: eigenvalues are the values of the diagonal and the eigenvectors
214    // are the standard basis vectors
215    let eigenvalues  = matrix.diagonal();
216    let eigenvectors = Matrix2::identity().cols;
217    std::array::from_fn (|i| Some((eigenvalues[i], eigenvectors[i])))
218  } else {
219    // eigenvalues will be the solution of the quadratic equation:
220    // x^2 - trace * x + determinant = 0
221    // eigenvectors will be solution to (matrix - e*I)v = 0 for each eigenvalue e
222    let trace         = matrix.trace();
223    let determinant   = matrix.determinant();
224    let system_matrix = |eigenvalue| matrix - Matrix2::broadcast_diagonal (eigenvalue);
225    super::solve_quadratic_equation (S::one(), -trace, determinant)
226      .map (|maybe_eigenvalue| maybe_eigenvalue.map (|eigenvalue|
227        (eigenvalue, solve_system_2_homogeneous (system_matrix (eigenvalue)).unwrap())))
228  };
229  // reverse sort by eigenvalue
230  eigenpairs.sort_by (|a, b| match (a, b) {
231    (Some (a), Some (b)) => b.0.partial_cmp (&a.0).unwrap(),
232    (None,     Some (_)) => std::cmp::Ordering::Greater,
233    (Some (_), None)     => std::cmp::Ordering::Less,
234    (None,     None)     => std::cmp::Ordering::Equal
235  });
236  eigenpairs
237}
238
239/// Return the (real-valued) eigenvalues and eigenvectors (eigenpairs) of a
240/// diagonalizable 3x3 matrix.
241///
242/// Uses the Francis (implicit QR) algorithm.
243///
244/// Returns `[Some((0, Vector2::zero())), None]` if the matrix is the zero matrix:
245///
246/// ```
247/// # use math_utils::{Matrix3, Vector3};
248/// # use math_utils::algebra::linear::eigensystem_3;
249/// let mat = Matrix3::<f32>::zero();
250/// assert_eq!(eigensystem_3 (mat), [Some ((0.0, Vector3::zero())), None, None]);
251/// ```
252pub fn eigensystem_3 <S> (matrix : Matrix3 <S>) -> [Option <(S, Vector3 <S>)>; 3] where
253  S : Real + num::NumCast + approx::RelativeEq <Epsilon=S> + std::fmt::Debug
254{
255  if matrix == Matrix3::zero() {
256    return [Some ((S::zero(), Vector3::zero())), None, None]
257  }
258  let mut eigenpairs = if Matrix3::with_diagonal (matrix.diagonal()) == matrix {
259    // diagonal matrix: eigenvalues are the values of the diagonal and the eigenvectors
260    // are the standard basis vectors
261    let eigenvalues  = matrix.diagonal();
262    let eigenvectors = Matrix3::identity().cols;
263    std::array::from_fn (|i| Some((eigenvalues[i], eigenvectors[i])))
264  } else {
265    // algorithm from mathru crate:
266    // <https://gitlab.com/rustmath/mathru/-/blob/a8feca07b9f171ce494363634b78d55178193cf8/src/algebra/linear/matrix/general/eigendec/native.rs>
267    fn eigen_2by2 <S : Real> (a11 : S, a12 : S, a21 : S, a22 : S) -> (S, S) {
268      let m = S::half() * (a11 + a22);
269      let p = a11 * a22 - a12 * a21;
270      let k = (m * m - p).sqrt();
271      let l1 = m + k;
272      let l2 = m - k;
273      (l1, l2)
274    }
275    let (_q, h) = decompose_hessenberg_3 (matrix).unwrap();
276    let (_q, u) = decompose_schur_3 (h);
277    let u = u.into_row_arrays();
278    let mut eigenvalues = Vec::with_capacity (3);
279    let mut i = 0;
280    while i < 3 {
281      if i == 2 || u[i+1][i] == S::zero() {
282        eigenvalues.push (u[i][i]);
283        i += 1;
284      } else {
285        let a_ii     = u[i][i];
286        let a_ii1    = u[i][i+1];
287        let a_i1i    = u[i+1][i];
288        let a_i1i1   = u[i+1][i+1];
289        let (l1, l2) = eigen_2by2 (a_ii, a_ii1, a_i1i, a_i1i1);
290        eigenvalues.push (l1);
291        eigenvalues.push (l2);
292        i += 2;
293      }
294    }
295    // TODO: by de-duping we are assuming that a duplicate eigenvalue will return more
296    // than one eigenvector below, is this correct ?
297    eigenvalues.dedup_by (|a, b| approx::relative_eq!(a, b,
298      max_relative = S::eight() * S::default_max_relative()));
299    let mut eigenpairs = [None; 3];
300    let mut i = 0;
301    for eigenvalue in eigenvalues {
302      let diff = matrix - Matrix3::identity() * eigenvalue;
303      for eigenvector in solve_system_3_homogeneous (diff) {
304        eigenpairs[i] = Some ((eigenvalue, eigenvector));
305        i += 1;
306      }
307    }
308    eigenpairs
309  };
310  // reverse sort by eigenvalue
311  eigenpairs.sort_by (|a, b| match (a, b) {
312    (Some (a), Some (b)) => b.0.partial_cmp (&a.0).unwrap(),
313    (None,     Some (_)) => std::cmp::Ordering::Greater,
314    (Some (_), None)     => std::cmp::Ordering::Less,
315    (None,     None)     => std::cmp::Ordering::Equal
316  });
317  eigenpairs
318}
319
320/// Return the (real-valued) eigenvalues and eigenvectors (eigenpairs) of a
321/// diagonalizable 3x3 matrix as computed by the classical method of computing the roots
322/// of the characteristic polynomial. Note that the root-finding problem is
323/// ill-conditioned so this may not return accurate results.
324///
325/// Returns `[Some, None, None]` if matrix is orthogonal, e.g. a rotation):
326///
327/// ```
328/// # use math_utils::Matrix3;
329/// # use math_utils::algebra::linear::eigensystem_3_classical;
330/// let mat = Matrix3::<f32>::from_row_arrays ([
331///   [0.0, -1.0, 0.0],
332///   [0.0,  0.0, 1.0],
333///   [1.0,  0.0, 0.0]
334/// ]);
335/// assert!(matches!(eigensystem_3_classical (mat), [Some (_), None, None]));
336/// ```
337///
338/// Returns `[Some, None, None]` or `[Some, Some, None]` if the matrix has only one or
339/// two eigenvector directions:
340///
341/// ```
342/// # use math_utils::Matrix3;
343/// # use math_utils::algebra::linear::eigensystem_3_classical;
344/// let mat = Matrix3::<f32>::from_row_arrays ([
345///   [0.0, 1.0, 0.0],
346///   [0.0, 0.0, 1.0],
347///   [0.0, 0.0, 0.0]
348/// ]);
349/// assert!(matches!(eigensystem_3_classical (mat), [Some (_), None, None]));
350/// let mat = Matrix3::<f32>::from_row_arrays ([
351///   [2.0, 1.0, 0.0],
352///   [0.0, 2.0, 0.0],
353///   [0.0, 0.0, 2.0]
354/// ]);
355/// assert!(matches!(eigensystem_3_classical (mat), [Some (_), Some (_), None]));
356/// ```
357pub fn eigensystem_3_classical <S> (matrix : Matrix3 <S>)
358  -> [Option <(S, Vector3 <S>)>; 3]
359where S : Real + Cbrt + num::NumCast + approx::RelativeEq <Epsilon=S> + std::fmt::Debug {
360  use num::One;
361  let mut eigenpairs = if Matrix3::with_diagonal (matrix.diagonal()) == matrix {
362    // diagonal matrix: eigenvalues are the values of the diagonal and the eigenvectors
363    // are the standard basis vectors
364    let eigenvalues  = matrix.diagonal();
365    let eigenvectors = Matrix3::identity().cols;
366    std::array::from_fn (|i| Some((eigenvalues[i], eigenvectors[i])))
367  } else {
368    // eigenvalues will be the solution of the cubic equation:
369    // x^3 - trace * x^2 + sum_of_principal_minors * x - determinant = 0
370    // eigenvectors will be solution to (matrix - e*I)v = 0 for each eigenvalue e
371    let trace = matrix.trace();
372    let sum_of_principal_minors = (0..3).map (|i| matrix.minor (i, i)).sum::<S>();
373    let determinant = matrix.determinant();
374    let system_matrix = |eigenvalue| matrix - Matrix3::broadcast_diagonal (eigenvalue);
375    let eigenvalues = super::solve_cubic_equation (
376      NonZero::one(), -trace, sum_of_principal_minors, -determinant);
377    let mut eigenpairs = vec![];
378    for eigenvalue in eigenvalues.iter().flatten().copied() {
379      let eigenvectors = solve_system_3_homogeneous (system_matrix (eigenvalue));
380      for eigenvector in eigenvectors {
381        eigenpairs.push ((eigenvalue, eigenvector));
382      }
383    }
384    std::array::from_fn (|i| eigenpairs.get (i).copied())
385  };
386  // reverse sort by eigenvalue
387  eigenpairs.sort_by (|a, b| match (a, b) {
388    (Some (a), Some (b)) => b.0.partial_cmp (&a.0).unwrap(),
389    (None,     Some (_)) => std::cmp::Ordering::Greater,
390    (Some (_), None)     => std::cmp::Ordering::Less,
391    (None,     None)     => std::cmp::Ordering::Equal
392  });
393  eigenpairs
394}
395
396/// Reduce a 3x3 matrix to row echelon form using Gaussian elimination
397pub fn row_reduce3 <S> (m : Matrix3 <S>) -> Matrix3 <S> where
398  S : OrderedField + approx::RelativeEq <Epsilon=S> + std::fmt::Debug
399{
400  // algorithm from wikipedia article on Gaussian elimination (25-09-26)
401  let mut rows = m.into_row_arrays();
402  let mut h = 0;  // pivot row
403  let mut k = 0;  // pivot column
404  while h <= 2 && k <= 2 {
405    // find the k-th pivot
406    // find the row with the largest value in the k-th column
407    let abs_row = |i : usize| rows[i][k].abs();
408    let i_max = (h..3).max_by (|i, j| abs_row (*i).partial_cmp (&abs_row (*j)).unwrap())
409      .unwrap();
410    if !approx::abs_diff_eq!(rows[i_max][k], S::zero(),
411      epsilon = S::four() * S::default_epsilon()
412    ) {
413      // pivot in this column
414      rows.swap (h, i_max);
415      // for all rows below pivot
416      for i in h+1..3 {
417        let f = rows[i][k] / rows[h][k];
418        // fill lower part of pivot column with zeros
419        rows[i][k] = S::zero();
420        // for all remaining elements in current row
421        #[expect(clippy::needless_range_loop)]
422        for j in k+1..3 {
423          let sub = rows[h][j] * f;
424          if approx::relative_eq!(rows[i][j], sub,
425            max_relative = S::eight() * S::default_max_relative()
426          ) {
427            rows[i][j] = S::zero();
428          } else {
429            rows[i][j] -= rows[h][j] * f;
430          }
431        }
432      }
433      // increase pivot row and column
434      h += 1;
435    }
436    k += 1;
437  }
438  Matrix3::from_row_arrays (rows)
439}
440
441/// Solve a 2D homogeneous system of linear equations represented by the matrix equation
442/// `Ax = 0`.
443///
444/// If the system matrix is zero, any vector in $R^2$ is a solution and `None` is
445/// returned:
446///
447/// ```
448/// # use math_utils::{Matrix2, Vector2};
449/// # use math_utils::algebra::linear::solve_system_2_homogeneous;
450/// assert_eq!(solve_system_2_homogeneous (Matrix2::<f32>::zero()), None);
451/// ```
452///
453/// If the system matrix is invertible, the unique solution is the zero vector:
454///
455/// ```
456/// # use math_utils::{Matrix2, Vector2};
457/// # use math_utils::algebra::linear::solve_system_2_homogeneous;
458/// let mat = Matrix2::<f32>::from_col_arrays ([
459///   [0.0, -1.0],
460///   [1.0,  0.0]
461/// ]);
462/// assert_eq!(solve_system_2_homogeneous (mat), Some (Vector2::zero()));
463/// ```
464///
465/// Otherwise the solution spans a linear subspace and a member of that subspace is
466/// returned.
467pub fn solve_system_2_homogeneous <S> (system_matrix : Matrix2 <S>)
468  -> Option <Vector2 <S>>
469where S : Field + approx::AbsDiffEq <Epsilon=S> {
470  if system_matrix == Matrix2::zero() {
471    return None
472  }
473  if LinearIso::is_invertible_approx (system_matrix, None) {
474    Some (Vector2::zero())
475  } else {
476    let [
477      [a, c],
478      [b, d]
479    ] = system_matrix.into_col_arrays();
480    if b != S::zero() {
481      Some (vector2 (S::one(), -a/b))
482    } else if d != S::zero() {
483      Some (vector2 (S::one(), -c/d))
484    } else {
485      Some (vector2 (S::zero(), S::one()))
486    }
487  }
488}
489
490/// Solve a 3D homogeneous system of linear equations represented by the matrix equation
491/// `Ax = 0`.
492///
493/// If the system matrix is zero, any vector in $R^3$ is a solution and `None` is
494/// returned:
495///
496/// ```
497/// # use math_utils::{Matrix3, Vector3};
498/// # use math_utils::algebra::linear::solve_system_3_homogeneous;
499/// assert!(solve_system_3_homogeneous (Matrix3::<f32>::zero()).is_empty());
500/// ```
501///
502/// If the system matrix is invertible, the unique solution is the zero vector:
503///
504/// ```
505/// # use math_utils::{Matrix3, Vector3};
506/// # use math_utils::algebra::linear::solve_system_3_homogeneous;
507/// let mat = Matrix3::<f32>::from_row_arrays ([
508///   [0.0, -1.0, 0.0],
509///   [0.0,  0.0, 1.0],
510///   [1.0,  0.0, 0.0]
511/// ]);
512/// assert_eq!(solve_system_3_homogeneous (mat), vec![Vector3::zero()]);
513/// ```
514///
515/// Otherwise the solution spans a linear subspace and a member of that subspace is
516/// returned.
517pub fn solve_system_3_homogeneous <S> (system_matrix : Matrix3 <S>)
518  -> Vec <Vector3 <S>>
519where S : OrderedField + num::NumCast + approx::RelativeEq <Epsilon=S> + std::fmt::Debug {
520  if system_matrix == Matrix3::zero() {
521    return vec![]
522  }
523  if LinearIso::is_invertible_approx (system_matrix, Some (S::from (0.001).unwrap())) {
524    vec![Vector3::zero()]
525  } else {
526    let row_echelon_form = row_reduce3 (system_matrix).into_row_arrays();
527    let mut pivot_cols = [usize::MAX; 3];
528    for i in 0..3 {
529      #[expect(clippy::needless_range_loop)]
530      for j in 0..3 {
531        if approx::abs_diff_ne!(row_echelon_form[i][j], S::zero(),
532          epsilon = S::four() * S::default_epsilon()
533        ) {
534          pivot_cols[i] = j;
535          break
536        }
537      }
538    }
539    debug_assert!(pivot_cols.iter().any (|i| *i != usize::MAX),
540      "we checked that the system matrix is non-zero, so there should be at least 1 \
541      pivot");
542    debug_assert!(!pivot_cols.iter().all (|i| *i != usize::MAX),
543      "we checked that the system matrix is singular, so there should never be 3 pivots"
544    );
545    let mut basis = vec![];
546    for free_col in (0..3).filter (|col| !pivot_cols.contains (col)) {
547      let mut vec = Vector3::zero();
548      vec[free_col] = S::one();
549      for i in (0..3).rev().filter (|i| pivot_cols[*i] != usize::MAX) {
550        let pivot_col = pivot_cols[i];
551        let row = row_echelon_form[i];
552        vec[pivot_col] = -(pivot_col+1..3).map (|j| row[j] * vec[j]).sum::<S>()
553          / row[pivot_col];
554      }
555      basis.push (vec);
556    }
557    debug_assert!(!basis.is_empty());
558    basis
559  }
560}
561
562impl <S> ElementaryReflector for Matrix3 <S> where S : OrderedField + Sqrt {
563  type Vector = Vector3 <S>;
564
565  /// Householder transformation
566  fn elementary_reflector (v : Vector3 <S>, index : usize) -> Matrix3 <S> {
567    debug_assert!(index < 3);
568    let d      = &v[index..];
569    let d_0    = d[0];
570    let d_norm = d.iter().copied().map (|x| x * x).sum::<S>().sqrt();
571    let alpha = if d_0 >= S::zero() {
572      -d_norm
573    } else {
574      d_norm
575    };
576    if alpha == S::zero() {
577      return Matrix3::identity()
578    }
579    let d_m = d.len();
580    let mut v = vec![S::zero(); d_m];
581    v[0] = (S::half() * (S::one() - d_0 / alpha)).sqrt();
582    let p = -alpha * v[0];
583    if d_m > 1 {
584      for i in 1..d_m {
585        v[i] = d[i] / (S::two() * p);
586      }
587    }
588    let mut w = Vector3::zero();
589    for i in index..3 {
590      w[i] = v[i - index];
591    }
592    let w_dyadp = w.outer_product (w);
593    Matrix3::<S>::identity() - (w_dyadp * S::two())
594  }
595}
596
597impl <S> UpperHessenberg3 <S> {
598  pub fn mat (self) -> Matrix3 <S> {
599    self.0
600  }
601}
602
603#[cfg(test)]
604mod tests {
605  #![expect(clippy::unreadable_literal)]
606
607  use super::*;
608  use approx::RelativeEq;
609
610  #[test]
611  fn eigensystem_2d() {
612    let mat = Matrix2::from_row_arrays ([
613      [1.0, 1.0],
614      [0.0, 0.0] ]);
615    let (eigenvalues, eigenvectors) : (Vec<_>, Vec<_>) =
616      eigensystem_2 (mat).map (Option::unwrap).iter().copied().unzip();
617    assert_eq!(eigenvalues, [1.0, 0.0]);
618    assert_eq!(eigenvectors, [
619      [1.0,  0.0],
620      [1.0, -1.0]
621    ].map (Vector2::from));
622
623    let mat = Matrix2::from_row_arrays ([
624      [1.0, 0.0],
625      [1.0, 0.0] ]);
626    let (eigenvalues, eigenvectors) : (Vec<_>, Vec<_>) =
627      eigensystem_2 (mat).map (Option::unwrap).iter().copied().unzip();
628    assert_eq!(eigenvalues, [1.0, 0.0]);
629    assert_eq!(eigenvectors, [
630      [1.0, 1.0],
631      [0.0, 1.0]
632    ].map (Vector2::from));
633
634    let mat = Matrix2::from_row_arrays ([
635      [2.0, 1.0],
636      [1.0, 2.0] ]);
637    let (eigenvalues, eigenvectors) : (Vec<_>, Vec<_>) =
638      eigensystem_2 (mat).map (Option::unwrap).iter().copied().unzip();
639    assert_eq!(eigenvalues, [3.0, 1.0]);
640    assert_eq!(eigenvectors, [
641      [1.0,  1.0],
642      [1.0, -1.0]
643    ].map (Vector2::from));
644
645    let mat = Matrix2::from_row_arrays ([
646      [1.0, 0.0],
647      [1.0, 3.0] ]);
648    let (eigenvalues, eigenvectors) : (Vec<_>, Vec<_>) =
649      eigensystem_2 (mat).map (Option::unwrap).iter().copied().unzip();
650    assert_eq!(eigenvalues, [3.0, 1.0]);
651    assert_eq!(eigenvectors, [
652      [0.0,  1.0],
653      [1.0, -0.5]
654    ].map (Vector2::from));
655  }
656
657  #[test]
658  fn eigensystem_3d() {
659    let mat = Matrix3::<f32>::from_row_arrays ([
660      [0.0, 1.0, 0.0],
661      [0.0, 0.0, 1.0],
662      [0.0, 0.0, 0.0]
663    ]);
664    let (eigenvalues, eigenvectors) : (Vec<_>, Vec<_>) =
665      eigensystem_3_classical (mat).iter().flatten().copied().unzip();
666    assert_eq!(eigenvalues, [0.0]);
667    assert_eq!(eigenvectors, [
668      [1.0, 0.0, 0.0]
669    ].map (Vector3::from));
670    let mat = Matrix3::<f32>::from_row_arrays ([
671      [2.0, 1.0, 0.0],
672      [0.0, 2.0, 0.0],
673      [0.0, 0.0, 2.0]
674    ]);
675    let (eigenvalues, eigenvectors) : (Vec<_>, Vec<_>) =
676      eigensystem_3_classical (mat).iter().flatten().copied().unzip();
677    assert_eq!(eigenvalues, [2.0, 2.0]);
678    assert_eq!(eigenvectors, [
679      [1.0, 0.0, 0.0],
680      [0.0, 0.0, 1.0]
681    ].map (Vector3::from));
682    let mat = Matrix3::<f32>::from_row_arrays ([
683      [9.714286, 0.0, 8.571428], [0.0, 1.1428572, 0.0], [8.571428, 0.0, 9.714286]
684    ]);
685    /* classical root-finding fails for this matrix
686    let (eigenvalues, eigenvectors) : (Vec<_>, Vec<_>) =
687      eigensystem_3_classical (mat).iter().flatten().copied().unzip();
688    */
689    let (eigenvalues, eigenvectors) : (Vec<_>, Vec<_>) =
690      eigensystem_3 (mat).iter().flatten().copied().unzip();
691    assert_eq!(eigenvalues, [18.285717, 1.1428576, 1.1428576]);
692    approx::assert_relative_eq!(eigenvectors[0], [ 1.0, 0.0, 1.0].into(),
693      max_relative = 4.0 * f32::default_max_relative());
694    assert_eq!(eigenvectors[1], [ 0.0, 1.0, 0.0].into());
695    assert_eq!(eigenvectors[2], [-1.0, 0.0, 1.0].into());
696  }
697
698  #[test]
699  fn row_reduce_3d() {
700    let mat = Matrix3::from_row_arrays ([
701      [0.0, 1.0, 0.0],
702      [0.0, 0.0, 1.0],
703      [1.0, 0.0, 0.0] ]);
704    let reduced = row_reduce3 (mat);
705    assert_eq!(reduced, Matrix3::identity());
706
707    let mat = Matrix3::from_row_arrays ([
708      [0.0, 1.0,  0.0],
709      [0.0, 0.0, -1.0],
710      [1.0, 0.0,  0.0] ]);
711    let reduced = row_reduce3 (mat);
712    assert_eq!(reduced, Matrix3::from_row_arrays ([
713      [1.0, 0.0,  0.0],
714      [0.0, 1.0,  0.0],
715      [0.0, 0.0, -1.0] ]));
716
717    let mat = Matrix3::from_row_arrays ([
718      [1.0, 2.0, 3.0],
719      [2.0, 4.0, 6.0],
720      [1.0, 1.0, 1.0] ]);
721    let reduced = row_reduce3 (mat);
722    assert_eq!(reduced, Matrix3::from_row_arrays ([
723      [2.0,  4.0,  6.0],
724      [0.0, -1.0, -2.0],
725      [0.0,  0.0,  0.0] ]));
726  }
727
728  #[test]
729  fn solve_homogeneous_2d() {
730    let mat = Matrix2::from_row_arrays ([
731      [1.0, 1.0],
732      [0.0, 0.0] ]);
733    assert_eq!(solve_system_2_homogeneous (mat), Some (vector2 (1.0, -1.0)));
734    let mat = Matrix2::from_row_arrays ([
735      [1.0, 0.0],
736      [1.0, 0.0] ]);
737    assert_eq!(solve_system_2_homogeneous (mat), Some (vector2 (0.0, 1.0)));
738  }
739
740  #[test]
741  fn solve_homogeneous_3d() {
742    // rank 2: linear nullspace
743    let mat = Matrix3::from_row_arrays ([
744      [1.0, 2.0, 3.0],
745      [4.0, 5.0, 6.0],
746      [7.0, 8.0, 9.0] ]);
747    let solution = solve_system_3_homogeneous (mat);
748    debug_assert_eq!(solution.len(), 1);
749    solution.into_iter().for_each (|basis| assert_eq!(mat * basis, Vector3::zero()));
750    let mat = Matrix3::from_row_arrays ([
751      [1.0, 2.0, 3.0],
752      [2.0, 4.0, 2.0],
753      [3.0, 6.0, 9.0] ]);
754    let solution = solve_system_3_homogeneous (mat);
755    debug_assert_eq!(solution.len(), 1);
756    solution.into_iter().for_each (|basis| assert_eq!(mat * basis, Vector3::zero()));
757    // rank 1: planar nullspace
758    let mat = Matrix3::from_row_arrays ([
759      [2.0,  4.0,  6.0],
760      [4.0,  8.0, 12.0],
761      [6.0, 12.0, 18.0] ]);
762    let solution = solve_system_3_homogeneous (mat);
763    debug_assert_eq!(solution.len(), 2);
764    solution.into_iter().for_each (|basis| assert_eq!(mat * basis, Vector3::zero()));
765    let mat = Matrix3::from_row_arrays ([
766      [ 4.0, -1.0, 2.0],
767      [ 8.0, -2.0, 4.0],
768      [12.0, -3.0, 6.0] ]);
769    let solution = solve_system_3_homogeneous (mat);
770    debug_assert_eq!(solution.len(), 2);
771    solution.into_iter().for_each (|basis| assert_eq!(mat * basis, Vector3::zero()));
772  }
773}
math_utils/algebra/linear.rs

math_utils/algebra/
linear.rs
