math_utils/algebra/
linear.rs

1//! Linear algebra routines
2
3use crate::{approx, num_traits as 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  let mut eigenpairs = if Matrix3::with_diagonal (matrix.diagonal()) == matrix {
361    // diagonal matrix: eigenvalues are the values of the diagonal and the eigenvectors
362    // are the standard basis vectors
363    let eigenvalues  = matrix.diagonal();
364    let eigenvectors = Matrix3::identity().cols;
365    std::array::from_fn (|i| Some((eigenvalues[i], eigenvectors[i])))
366  } else {
367    // eigenvalues will be the solution of the cubic equation:
368    // x^3 - trace * x^2 + sum_of_principal_minors * x - determinant = 0
369    // eigenvectors will be solution to (matrix - e*I)v = 0 for each eigenvalue e
370    let trace = matrix.trace();
371    let sum_of_principal_minors = (0..3).map (|i| matrix.minor (i, i)).sum::<S>();
372    let determinant = matrix.determinant();
373    let system_matrix = |eigenvalue| matrix - Matrix3::broadcast_diagonal (eigenvalue);
374    let eigenvalues = super::solve_cubic_equation (
375      S::one(), -trace, sum_of_principal_minors, -determinant);
376    let mut eigenpairs = vec![];
377    for eigenvalue in eigenvalues.iter().flatten().copied() {
378      let eigenvectors = solve_system_3_homogeneous (system_matrix (eigenvalue));
379      for eigenvector in eigenvectors {
380        eigenpairs.push ((eigenvalue, eigenvector));
381      }
382    }
383    std::array::from_fn (|i| eigenpairs.get (i).copied())
384  };
385  // reverse sort by eigenvalue
386  eigenpairs.sort_by (|a, b| match (a, b) {
387    (Some (a), Some (b)) => b.0.partial_cmp (&a.0).unwrap(),
388    (None,     Some (_)) => std::cmp::Ordering::Greater,
389    (Some (_), None)     => std::cmp::Ordering::Less,
390    (None,     None)     => std::cmp::Ordering::Equal
391  });
392  eigenpairs
393}
394
395/// Reduce a 3x3 matrix to row echelon form using Gaussian elimination
396pub fn row_reduce3 <S> (m : Matrix3 <S>) -> Matrix3 <S> where
397  S : OrderedField + approx::RelativeEq <Epsilon=S> + std::fmt::Debug
398{
399  // algorithm from wikipedia article on Gaussian elimination (25-09-26)
400  let mut rows = m.into_row_arrays();
401  let mut h = 0;  // pivot row
402  let mut k = 0;  // pivot column
403  while h <= 2 && k <= 2 {
404    // find the k-th pivot
405    // find the row with the largest value in the k-th column
406    let abs_row = |i : usize| rows[i][k].abs();
407    let i_max = (h..3).max_by (|i, j| abs_row (*i).partial_cmp (&abs_row (*j)).unwrap())
408      .unwrap();
409    if !approx::abs_diff_eq!(rows[i_max][k], S::zero(),
410      epsilon = S::four() * S::default_epsilon()
411    ) {
412      // pivot in this column
413      rows.swap (h, i_max);
414      // for all rows below pivot
415      for i in h+1..3 {
416        let f = rows[i][k] / rows[h][k];
417        // fill lower part of pivot column with zeros
418        rows[i][k] = S::zero();
419        // for all remaining elements in current row
420        #[expect(clippy::needless_range_loop)]
421        for j in k+1..3 {
422          let sub = rows[h][j] * f;
423          if approx::relative_eq!(rows[i][j], sub,
424            max_relative = S::eight() * S::default_epsilon()
425          ) {
426            rows[i][j] = S::zero();
427          } else {
428            rows[i][j] -= rows[h][j] * f;
429          }
430        }
431      }
432      // increase pivot row and column
433      h += 1;
434    }
435    k += 1;
436  }
437  Matrix3::from_row_arrays (rows)
438}
439
440/// Solve a 2D homogeneous system of linear equations represented by the matrix equation
441/// `Ax = 0`.
442///
443/// If the system matrix is zero, any vector in $R^2$ is a solution and `None` is
444/// returned:
445///
446/// ```
447/// # use math_utils::{Matrix2, Vector2};
448/// # use math_utils::algebra::linear::solve_system_2_homogeneous;
449/// assert_eq!(solve_system_2_homogeneous (Matrix2::<f32>::zero()), None);
450/// ```
451///
452/// If the system matrix is invertible, the unique solution is the zero vector:
453///
454/// ```
455/// # use math_utils::{Matrix2, Vector2};
456/// # use math_utils::algebra::linear::solve_system_2_homogeneous;
457/// let mat = Matrix2::<f32>::from_col_arrays ([
458///   [0.0, -1.0],
459///   [1.0,  0.0]
460/// ]);
461/// assert_eq!(solve_system_2_homogeneous (mat), Some (Vector2::zero()));
462/// ```
463///
464/// Otherwise the solution spans a linear subspace and a member of that subspace is
465/// returned.
466pub fn solve_system_2_homogeneous <S> (system_matrix : Matrix2 <S>)
467  -> Option <Vector2 <S>>
468where S : Field + approx::AbsDiffEq <Epsilon=S> {
469  if system_matrix == Matrix2::zero() {
470    return None
471  }
472  if LinearIso::is_invertible_approx (system_matrix, None) {
473    Some (Vector2::zero())
474  } else {
475    let [
476      [a, c],
477      [b, d]
478    ] = system_matrix.into_col_arrays();
479    if b != S::zero() {
480      Some (vector2 (S::one(), -a/b))
481    } else if d != S::zero() {
482      Some (vector2 (S::one(), -c/d))
483    } else {
484      Some (vector2 (S::zero(), S::one()))
485    }
486  }
487}
488
489/// Solve a 3D homogeneous system of linear equations represented by the matrix equation
490/// `Ax = 0`.
491///
492/// If the system matrix is zero, any vector in $R^3$ is a solution and `None` is
493/// returned:
494///
495/// ```
496/// # use math_utils::{Matrix3, Vector3};
497/// # use math_utils::algebra::linear::solve_system_3_homogeneous;
498/// assert!(solve_system_3_homogeneous (Matrix3::<f32>::zero()).is_empty());
499/// ```
500///
501/// If the system matrix is invertible, the unique solution is the zero vector:
502///
503/// ```
504/// # use math_utils::{Matrix3, Vector3};
505/// # use math_utils::algebra::linear::solve_system_3_homogeneous;
506/// let mat = Matrix3::<f32>::from_row_arrays ([
507///   [0.0, -1.0, 0.0],
508///   [0.0,  0.0, 1.0],
509///   [1.0,  0.0, 0.0]
510/// ]);
511/// assert_eq!(solve_system_3_homogeneous (mat), vec![Vector3::zero()]);
512/// ```
513///
514/// Otherwise the solution spans a linear subspace and a member of that subspace is
515/// returned.
516pub fn solve_system_3_homogeneous <S> (system_matrix : Matrix3 <S>)
517  -> Vec <Vector3 <S>>
518where S : OrderedField + num::NumCast + approx::RelativeEq <Epsilon=S> + std::fmt::Debug {
519  if system_matrix == Matrix3::zero() {
520    return vec![]
521  }
522  if LinearIso::is_invertible_approx (system_matrix, Some (S::from (0.001).unwrap())) {
523    vec![Vector3::zero()]
524  } else {
525    let row_echelon_form = row_reduce3 (system_matrix).into_row_arrays();
526    let mut pivot_cols = [usize::MAX; 3];
527    for i in 0..3 {
528      #[expect(clippy::needless_range_loop)]
529      for j in 0..3 {
530        if approx::abs_diff_ne!(row_echelon_form[i][j], S::zero(),
531          epsilon = S::four() * S::default_epsilon()
532        ) {
533          pivot_cols[i] = j;
534          break
535        }
536      }
537    }
538    debug_assert!(pivot_cols.iter().any (|i| *i != usize::MAX),
539      "we checked that the system matrix is non-zero, so there should be at least 1 \
540      pivot");
541    debug_assert!(!pivot_cols.iter().all (|i| *i != usize::MAX),
542      "we checked that the system matrix is singular, so there should never be 3 pivots"
543    );
544    let mut basis = vec![];
545    for free_col in (0..3).filter (|col| !pivot_cols.contains (col)) {
546      let mut vec = Vector3::zero();
547      vec[free_col] = S::one();
548      for i in (0..3).rev().filter (|i| pivot_cols[*i] != usize::MAX) {
549        let pivot_col = pivot_cols[i];
550        let row = row_echelon_form[i];
551        vec[pivot_col] = -(pivot_col+1..3).map (|j| row[j] * vec[j]).sum::<S>()
552          / row[pivot_col];
553      }
554      basis.push (vec);
555    }
556    debug_assert!(!basis.is_empty());
557    basis
558  }
559}
560
561impl <S : Real> ElementaryReflector for Matrix3 <S> {
562  type Vector = Vector3 <S>;
563
564  /// Householder transformation
565  fn elementary_reflector (v : Vector3 <S>, index : usize) -> Matrix3 <S> {
566    debug_assert!(index < 3);
567    let d      = &v[index..];
568    let d_0    = d[0];
569    let d_norm = d.iter().copied().map (|x| x * x).sum::<S>().sqrt();
570    let alpha = if d_0 >= S::zero() {
571      -d_norm
572    } else {
573      d_norm
574    };
575    if alpha == S::zero() {
576      return Matrix3::identity()
577    }
578    let d_m = d.len();
579    let mut v = vec![S::zero(); d_m];
580    v[0] = (S::half() * (S::one() - d_0 / alpha)).sqrt();
581    let p = -alpha * v[0];
582    if d_m > 1 {
583      for i in 1..d_m {
584        v[i] = d[i] / (S::two() * p);
585      }
586    }
587    let mut w = Vector3::zero();
588    for i in index..3 {
589      w[i] = v[i - index];
590    }
591    let w_dyadp = w.outer_product (w);
592    Matrix3::<S>::identity() - (w_dyadp * S::two())
593  }
594}
595
596impl <S> UpperHessenberg3 <S> {
597  pub fn mat (self) -> Matrix3 <S> {
598    self.0
599  }
600}
601
602#[cfg(test)]
603mod tests {
604  #![expect(clippy::unreadable_literal)]
605
606  use super::*;
607
608  #[test]
609  fn eigensystem_2d() {
610    let mat = Matrix2::from_row_arrays ([
611      [1.0, 1.0],
612      [0.0, 0.0] ]);
613    let (eigenvalues, eigenvectors) : (Vec<_>, Vec<_>) =
614      eigensystem_2 (mat).map (Option::unwrap).iter().copied().unzip();
615    assert_eq!(eigenvalues, [1.0, 0.0]);
616    assert_eq!(eigenvectors, [
617      [1.0,  0.0],
618      [1.0, -1.0]
619    ].map (Vector2::from));
620
621    let mat = Matrix2::from_row_arrays ([
622      [1.0, 0.0],
623      [1.0, 0.0] ]);
624    let (eigenvalues, eigenvectors) : (Vec<_>, Vec<_>) =
625      eigensystem_2 (mat).map (Option::unwrap).iter().copied().unzip();
626    assert_eq!(eigenvalues, [1.0, 0.0]);
627    assert_eq!(eigenvectors, [
628      [1.0, 1.0],
629      [0.0, 1.0]
630    ].map (Vector2::from));
631
632    let mat = Matrix2::from_row_arrays ([
633      [2.0, 1.0],
634      [1.0, 2.0] ]);
635    let (eigenvalues, eigenvectors) : (Vec<_>, Vec<_>) =
636      eigensystem_2 (mat).map (Option::unwrap).iter().copied().unzip();
637    assert_eq!(eigenvalues, [3.0, 1.0]);
638    assert_eq!(eigenvectors, [
639      [1.0,  1.0],
640      [1.0, -1.0]
641    ].map (Vector2::from));
642
643    let mat = Matrix2::from_row_arrays ([
644      [1.0, 0.0],
645      [1.0, 3.0] ]);
646    let (eigenvalues, eigenvectors) : (Vec<_>, Vec<_>) =
647      eigensystem_2 (mat).map (Option::unwrap).iter().copied().unzip();
648    assert_eq!(eigenvalues, [3.0, 1.0]);
649    assert_eq!(eigenvectors, [
650      [0.0,  1.0],
651      [1.0, -0.5]
652    ].map (Vector2::from));
653  }
654
655  #[test]
656  fn eigensystem_3d() {
657    let mat = Matrix3::<f32>::from_row_arrays ([
658      [0.0, 1.0, 0.0],
659      [0.0, 0.0, 1.0],
660      [0.0, 0.0, 0.0]
661    ]);
662    let (eigenvalues, eigenvectors) : (Vec<_>, Vec<_>) =
663      eigensystem_3_classical (mat).iter().flatten().copied().unzip();
664    assert_eq!(eigenvalues, [0.0]);
665    assert_eq!(eigenvectors, [
666      [1.0, 0.0, 0.0]
667    ].map (Vector3::from));
668    let mat = Matrix3::<f32>::from_row_arrays ([
669      [2.0, 1.0, 0.0],
670      [0.0, 2.0, 0.0],
671      [0.0, 0.0, 2.0]
672    ]);
673    let (eigenvalues, eigenvectors) : (Vec<_>, Vec<_>) =
674      eigensystem_3_classical (mat).iter().flatten().copied().unzip();
675    assert_eq!(eigenvalues, [2.0, 2.0]);
676    assert_eq!(eigenvectors, [
677      [1.0, 0.0, 0.0],
678      [0.0, 0.0, 1.0]
679    ].map (Vector3::from));
680    let mat = Matrix3::<f32>::from_row_arrays ([
681      [9.714286, 0.0, 8.571428], [0.0, 1.1428572, 0.0], [8.571428, 0.0, 9.714286]
682    ]);
683    /* classical root-finding fails for this matrix
684    let (eigenvalues, eigenvectors) : (Vec<_>, Vec<_>) =
685      eigensystem_3_classical (mat).iter().flatten().copied().unzip();
686    */
687    let (eigenvalues, eigenvectors) : (Vec<_>, Vec<_>) =
688      eigensystem_3 (mat).iter().flatten().copied().unzip();
689    assert_eq!(eigenvalues, [18.285717, 1.1428576, 1.1428576]);
690    approx::assert_relative_eq!(eigenvectors[0], [ 1.0, 0.0, 1.0].into(),
691      max_relative = 4.0 * f32::EPSILON);
692    assert_eq!(eigenvectors[1], [ 0.0, 1.0, 0.0].into());
693    assert_eq!(eigenvectors[2], [-1.0, 0.0, 1.0].into());
694  }
695
696  #[test]
697  fn row_reduce_3d() {
698    let mat = Matrix3::from_row_arrays ([
699      [0.0, 1.0, 0.0],
700      [0.0, 0.0, 1.0],
701      [1.0, 0.0, 0.0] ]);
702    let reduced = row_reduce3 (mat);
703    assert_eq!(reduced, Matrix3::identity());
704
705    let mat = Matrix3::from_row_arrays ([
706      [0.0, 1.0,  0.0],
707      [0.0, 0.0, -1.0],
708      [1.0, 0.0,  0.0] ]);
709    let reduced = row_reduce3 (mat);
710    assert_eq!(reduced, Matrix3::from_row_arrays ([
711      [1.0, 0.0,  0.0],
712      [0.0, 1.0,  0.0],
713      [0.0, 0.0, -1.0] ]));
714
715    let mat = Matrix3::from_row_arrays ([
716      [1.0, 2.0, 3.0],
717      [2.0, 4.0, 6.0],
718      [1.0, 1.0, 1.0] ]);
719    let reduced = row_reduce3 (mat);
720    assert_eq!(reduced, Matrix3::from_row_arrays ([
721      [2.0,  4.0,  6.0],
722      [0.0, -1.0, -2.0],
723      [0.0,  0.0,  0.0] ]));
724  }
725
726  #[test]
727  fn solve_homogeneous_2d() {
728    let mat = Matrix2::from_row_arrays ([
729      [1.0, 1.0],
730      [0.0, 0.0] ]);
731    assert_eq!(solve_system_2_homogeneous (mat), Some (vector2 (1.0, -1.0)));
732    let mat = Matrix2::from_row_arrays ([
733      [1.0, 0.0],
734      [1.0, 0.0] ]);
735    assert_eq!(solve_system_2_homogeneous (mat), Some (vector2 (0.0, 1.0)));
736  }
737
738  #[test]
739  fn solve_homogeneous_3d() {
740    // rank 2: linear nullspace
741    let mat = Matrix3::from_row_arrays ([
742      [1.0, 2.0, 3.0],
743      [4.0, 5.0, 6.0],
744      [7.0, 8.0, 9.0] ]);
745    let solution = solve_system_3_homogeneous (mat);
746    debug_assert_eq!(solution.len(), 1);
747    solution.into_iter().for_each (|basis| assert_eq!(mat * basis, Vector3::zero()));
748    let mat = Matrix3::from_row_arrays ([
749      [1.0, 2.0, 3.0],
750      [2.0, 4.0, 2.0],
751      [3.0, 6.0, 9.0] ]);
752    let solution = solve_system_3_homogeneous (mat);
753    debug_assert_eq!(solution.len(), 1);
754    solution.into_iter().for_each (|basis| assert_eq!(mat * basis, Vector3::zero()));
755    // rank 1: planar nullspace
756    let mat = Matrix3::from_row_arrays ([
757      [2.0,  4.0,  6.0],
758      [4.0,  8.0, 12.0],
759      [6.0, 12.0, 18.0] ]);
760    let solution = solve_system_3_homogeneous (mat);
761    debug_assert_eq!(solution.len(), 2);
762    solution.into_iter().for_each (|basis| assert_eq!(mat * basis, Vector3::zero()));
763    let mat = Matrix3::from_row_arrays ([
764      [ 4.0, -1.0, 2.0],
765      [ 8.0, -2.0, 4.0],
766      [12.0, -3.0, 6.0] ]);
767    let solution = solve_system_3_homogeneous (mat);
768    debug_assert_eq!(solution.len(), 2);
769    solution.into_iter().for_each (|basis| assert_eq!(mat * basis, Vector3::zero()));
770  }
771}
math_utils/algebra/linear.rs

math_utils/algebra/
linear.rs
