nsys-math-utils 1.0.0

//! Linear algebra routines

use crate::{approx, num};
use crate::traits::*;
use crate::types::*;

#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct UpperHessenberg3 <S> (Matrix3 <S>);

/// Returns matrices `(q, h)` of the decomposition `m = qhq^T` where `q` is an
/// orthogonal matrix and `h` is an upper-Hessenberg matrix.
///
/// Returns `None` if `m` is the zero matrix:
///
/// ```
/// # use math_utils::Matrix3;
/// # use math_utils::algebra::linear::decompose_hessenberg_3;
/// let mat = Matrix3::<f32>::zero();
/// assert_eq!(decompose_hessenberg_3 (mat), None);
/// ```
pub fn decompose_hessenberg_3 <S : Real> (m : Matrix3 <S>)
  -> Option <(Matrix3 <S>, UpperHessenberg3 <S>)>
{
  // algorithm from mathru crate:
  // <https://gitlab.com/rustmath/mathru/-/blob/a8feca07b9f171ce494363634b78d55178193cf8/src/algebra/linear/matrix/general/hessenbergdec/native.rs>
  if m == Matrix3::zero() {
    return None
  }
  let v = m.cols[0];
  let househ = Matrix3::elementary_reflector (v, 1);
  let q = househ;
  let h = UpperHessenberg3 (q.transpose() * m * q);
  Some ((q, h))
}

/// Returns matrices `(q, u)` of the decomposition `h = quq^{-1}` where `q` is an
/// orthogonal matrix and `u` is an upper quasi-triangular matrix
pub fn decompose_schur_3 <S> (h : UpperHessenberg3 <S>) -> (Matrix3 <S>, Matrix3 <S>)
  where S : Real + approx::AbsDiffEq <Epsilon=S>
{
  // algorithm from mathru crate:
  // <https://gitlab.com/rustmath/mathru/-/blob/a8feca07b9f171ce494363634b78d55178193cf8/src/algebra/linear/matrix/upperhessenberg/schurdec/native.rs>
  // TODO: to avoid errors we are using row-major arrays to match the indexing in the
  // original algorithm; it would probably be more performant to do everything with
  // column-major matrices
  fn get_slice <S : Ring> (
    rows : [[S; 3]; 3], row_s : usize, row_e : usize, column_s : usize, column_e : usize
  ) -> [[S; 3]; 3] {
    debug_assert!(row_s < 3);
    debug_assert!(row_e < 3);
    debug_assert!(column_s < 3);
    debug_assert!(column_e < 3);
    let mut m = [[S::zero(); 3]; 3];
    for r in row_s..=row_e {
      for c in column_s..=column_e {
        m[r - row_s][c - column_s] = rows[r][c];
      }
    }
    m
  }
  fn set_slice <S : Copy> (
    mut rows : [[S; 3]; 3], slice : [[S; 3]; 3],
    row_s : usize, row_e : usize, column_s : usize, column_e : usize
  ) -> [[S; 3]; 3] {
    debug_assert!(row_s < 3);
    debug_assert!(row_e < 3);
    debug_assert!(column_s < 3);
    debug_assert!(column_e < 3);
    for r in row_s..=row_e {
      for c in column_s..=column_e {
        rows[r][c] = slice[r - row_s][c - column_s];
      }
    }
    rows
  }
  fn givens_cosine_sine_pair <S : Real> (a : S, b : S) -> (S, S) {
    if b == S::zero() {
      (S::one(), S::zero())
    } else if a == S::zero() {
      (S::zero(), S::one())
    } else {
      let l = (a * a + b * b).sqrt();
      (a.abs() / l, a.signum_or_zero() * b / l)
    }
  }
  // Francis QR algorithm
  let mut h = h.mat().into_row_arrays();
  let mut u = Matrix3::<S>::identity().into_row_arrays();
  loop {
    // bulge generation
    let h_qq = h[1][1];
    let h_pp = h[2][2];
    let s = h_qq + h_pp;
    let h_qp = h[1][2];
    let h_pq = h[2][1];
    let t = h_qq * h_pp - h_qp * h_pq;
    // first 3 elements of first column of m
    let h_11 = h[0][0];
    let h_12 = h[0][1];
    let h_21 = h[1][0];
    let mut x = h_11 * h_11 + h_12 * h_21 - s * h_11 + t;
    let h_22 = h[1][1];
    let mut y = h_21 * (h_11 + h_22 - s);
    let h_32 = h[2][1];
    let z = h_21 * h_32;
    let b = vector3 (x, y, z);
    let hr = Matrix3::elementary_reflector (b, 0);
    // determine the householder reflector P with P^T [x; y; z;]^T = αe1
    h = (hr.transpose() * Matrix3::from_row_arrays (get_slice (h, 0, 2, 0, 2)))
      .into_row_arrays();
    h = (Matrix3::from_row_arrays (get_slice (h, 0, 2, 0, 2)) * hr)
      .into_row_arrays();
    u = (Matrix3::from_row_arrays (get_slice (u, 0, 2, 0, 2)) * hr)
      .into_row_arrays();

    let h_k2_k1 = h[1][0];
    x = h_k2_k1;
    let h_k3_k1 = h[2][0];
    y = h_k3_k1;
    // determine the Givens rotation P with P [x; y]^T = αe1
    let (c, s) = givens_cosine_sine_pair (x, y);
    // 2x2
    let g = Matrix3::fill_zeros (Matrix2::from_row_arrays ([
      [c, s],
      [-s ,c]
    ]));
    {
      // 2x2 @ 2x3 -> 2x3
      let temp = (g * Matrix3::from_row_arrays (get_slice (h, 1, 2, 0, 2)))
        .into_row_arrays();
      // get 2 rows and 3 cols from temp
      h = set_slice (h, temp, 1, 2, 0, 2);
    }
    {
      // 2x2
      let g_trans = g.transpose();
      // 3x2 @ 2x2 -> 3x2
      let temp = (Matrix3::from_row_arrays (get_slice (h, 0, 2, 1, 2)) * g_trans)
        .into_row_arrays();
      // get 3 rows and 2 cols from temp
      h = set_slice (h, temp, 0, 2, 1, 2);
      // 3x2 @ 2x2 -> 3x2
      let u_slice =
        (Matrix3::from_row_arrays (get_slice (h, 0, 2, 1, 2)) * g_trans)
          .into_row_arrays();
      // get 3 rows and 2 cols from u_slice
      u = set_slice (u, u_slice, 0, 2, 1, 2);
    }
    // check for convergence
    let m = h[1][1].abs();
    let n = h[2][2].abs();
    if h[2][1].abs() < S::default_epsilon() * (m + n) {
      h[2][1] = S::zero();
      break
    } else {
      let k = h[0][0].abs();
      let l = h[1][1].abs();
      if h[1][0].abs() < S::default_epsilon() * (k + l) {
        h[1][0] = S::zero();
        break
      }
    }
  }
  (Matrix3::from_row_arrays (u), Matrix3::from_row_arrays (h))
}

/// Return the (real-valued) eigenvalues and eigenvectors (eigenpairs) of a
/// diagonalizable 2x2 matrix.
///
/// Will be ordered by descending eigenvalues.
///
/// Returns `[Some((0, Vector2::zero())), None]` if the matrix is the zero matrix:
///
/// ```
/// # use math_utils::{Matrix2, Vector2};
/// # use math_utils::algebra::linear::eigensystem_2;
/// let mat = Matrix2::<f32>::zero();
/// assert_eq!(eigensystem_2 (mat), [Some ((0.0, Vector2::zero())), None]);
/// ```
///
/// Returns `[None, None]` if eigenvalues are not real values (matrix is orthogonal,
/// e.g. a rotation):
///
/// ```
/// # use math_utils::Matrix2;
/// # use math_utils::algebra::linear::eigensystem_2;
/// let mat = Matrix2::<f32>::from_row_arrays ([
///   [0.0, -1.0],
///   [1.0,  0.0]
/// ]);
/// assert_eq!(eigensystem_2 (mat), [None, None]);
/// ```
///
/// Returns `[Some, None]` if the matrix has only one eigenvector direction (matrix is
/// not diagonalizable):
///
/// ```
/// # use math_utils::{Matrix2, Vector2};
/// # use math_utils::algebra::linear::eigensystem_2;
/// let mat = Matrix2::<f32>::from_row_arrays ([
///   [ 3.0, 1.0],
///   [-1.0, 1.0]
/// ]);
/// assert!(matches!(eigensystem_2 (mat), [Some(_), None]));
/// ```
pub fn eigensystem_2 <S> (matrix : Matrix2 <S>) -> [Option <(S, Vector2 <S>)>; 2] where
  S : OrderedField + Sqrt + approx::AbsDiffEq <Epsilon=S> + std::fmt::Debug
{
  if matrix == Matrix2::zero() {
    return [Some ((S::zero(), Vector2::zero())), None]
  }
  let mut eigenpairs = if matrix[(0,1)] == S::zero() && matrix[(1,0)] == S::zero() {
    // diagonal matrix: eigenvalues are the values of the diagonal and the eigenvectors
    // are the standard basis vectors
    let eigenvalues  = matrix.diagonal();
    let eigenvectors = Matrix2::identity().cols;
    std::array::from_fn (|i| Some((eigenvalues[i], eigenvectors[i])))
  } else {
    // eigenvalues will be the solution of the quadratic equation:
    // x^2 - trace * x + determinant = 0
    // eigenvectors will be solution to (matrix - e*I)v = 0 for each eigenvalue e
    let trace         = matrix.trace();
    let determinant   = matrix.determinant();
    let system_matrix = |eigenvalue| matrix - Matrix2::broadcast_diagonal (eigenvalue);
    super::solve_quadratic_equation (S::one(), -trace, determinant)
      .map (|maybe_eigenvalue| maybe_eigenvalue.map (|eigenvalue|
        (eigenvalue, solve_system_2_homogeneous (system_matrix (eigenvalue)).unwrap())))
  };
  // reverse sort by eigenvalue
  eigenpairs.sort_by (|a, b| match (a, b) {
    (Some (a), Some (b)) => b.0.partial_cmp (&a.0).unwrap(),
    (None,     Some (_)) => std::cmp::Ordering::Greater,
    (Some (_), None)     => std::cmp::Ordering::Less,
    (None,     None)     => std::cmp::Ordering::Equal
  });
  eigenpairs
}

/// Return the (real-valued) eigenvalues and eigenvectors (eigenpairs) of a
/// diagonalizable 3x3 matrix.
///
/// Uses the Francis (implicit QR) algorithm.
///
/// Returns `[Some((0, Vector2::zero())), None]` if the matrix is the zero matrix:
///
/// ```
/// # use math_utils::{Matrix3, Vector3};
/// # use math_utils::algebra::linear::eigensystem_3;
/// let mat = Matrix3::<f32>::zero();
/// assert_eq!(eigensystem_3 (mat), [Some ((0.0, Vector3::zero())), None, None]);
/// ```
pub fn eigensystem_3 <S> (matrix : Matrix3 <S>) -> [Option <(S, Vector3 <S>)>; 3] where
  S : Real + num::NumCast + approx::RelativeEq <Epsilon=S> + std::fmt::Debug
{
  if matrix == Matrix3::zero() {
    return [Some ((S::zero(), Vector3::zero())), None, None]
  }
  let mut eigenpairs = if Matrix3::with_diagonal (matrix.diagonal()) == matrix {
    // diagonal matrix: eigenvalues are the values of the diagonal and the eigenvectors
    // are the standard basis vectors
    let eigenvalues  = matrix.diagonal();
    let eigenvectors = Matrix3::identity().cols;
    std::array::from_fn (|i| Some((eigenvalues[i], eigenvectors[i])))
  } else {
    // algorithm from mathru crate:
    // <https://gitlab.com/rustmath/mathru/-/blob/a8feca07b9f171ce494363634b78d55178193cf8/src/algebra/linear/matrix/general/eigendec/native.rs>
    fn eigen_2by2 <S : Real> (a11 : S, a12 : S, a21 : S, a22 : S) -> (S, S) {
      let m = S::half() * (a11 + a22);
      let p = a11 * a22 - a12 * a21;
      let k = (m * m - p).sqrt();
      let l1 = m + k;
      let l2 = m - k;
      (l1, l2)
    }
    let (_q, h) = decompose_hessenberg_3 (matrix).unwrap();
    let (_q, u) = decompose_schur_3 (h);
    let u = u.into_row_arrays();
    let mut eigenvalues = Vec::with_capacity (3);
    let mut i = 0;
    while i < 3 {
      if i == 2 || u[i+1][i] == S::zero() {
        eigenvalues.push (u[i][i]);
        i += 1;
      } else {
        let a_ii     = u[i][i];
        let a_ii1    = u[i][i+1];
        let a_i1i    = u[i+1][i];
        let a_i1i1   = u[i+1][i+1];
        let (l1, l2) = eigen_2by2 (a_ii, a_ii1, a_i1i, a_i1i1);
        eigenvalues.push (l1);
        eigenvalues.push (l2);
        i += 2;
      }
    }
    // TODO: by de-duping we are assuming that a duplicate eigenvalue will return more
    // than one eigenvector below, is this correct ?
    eigenvalues.dedup_by (|a, b| approx::relative_eq!(a, b,
      max_relative = S::eight() * S::default_max_relative()));
    let mut eigenpairs = [None; 3];
    let mut i = 0;
    for eigenvalue in eigenvalues {
      let diff = matrix - Matrix3::identity() * eigenvalue;
      for eigenvector in solve_system_3_homogeneous (diff) {
        eigenpairs[i] = Some ((eigenvalue, eigenvector));
        i += 1;
      }
    }
    eigenpairs
  };
  // reverse sort by eigenvalue
  eigenpairs.sort_by (|a, b| match (a, b) {
    (Some (a), Some (b)) => b.0.partial_cmp (&a.0).unwrap(),
    (None,     Some (_)) => std::cmp::Ordering::Greater,
    (Some (_), None)     => std::cmp::Ordering::Less,
    (None,     None)     => std::cmp::Ordering::Equal
  });
  eigenpairs
}

/// Return the (real-valued) eigenvalues and eigenvectors (eigenpairs) of a
/// diagonalizable 3x3 matrix as computed by the classical method of computing the roots
/// of the characteristic polynomial. Note that the root-finding problem is
/// ill-conditioned so this may not return accurate results.
///
/// Returns `[Some, None, None]` if matrix is orthogonal, e.g. a rotation):
///
/// ```
/// # use math_utils::Matrix3;
/// # use math_utils::algebra::linear::eigensystem_3_classical;
/// let mat = Matrix3::<f32>::from_row_arrays ([
///   [0.0, -1.0, 0.0],
///   [0.0,  0.0, 1.0],
///   [1.0,  0.0, 0.0]
/// ]);
/// assert!(matches!(eigensystem_3_classical (mat), [Some (_), None, None]));
/// ```
///
/// Returns `[Some, None, None]` or `[Some, Some, None]` if the matrix has only one or
/// two eigenvector directions:
///
/// ```
/// # use math_utils::Matrix3;
/// # use math_utils::algebra::linear::eigensystem_3_classical;
/// let mat = Matrix3::<f32>::from_row_arrays ([
///   [0.0, 1.0, 0.0],
///   [0.0, 0.0, 1.0],
///   [0.0, 0.0, 0.0]
/// ]);
/// assert!(matches!(eigensystem_3_classical (mat), [Some (_), None, None]));
/// let mat = Matrix3::<f32>::from_row_arrays ([
///   [2.0, 1.0, 0.0],
///   [0.0, 2.0, 0.0],
///   [0.0, 0.0, 2.0]
/// ]);
/// assert!(matches!(eigensystem_3_classical (mat), [Some (_), Some (_), None]));
/// ```
pub fn eigensystem_3_classical <S> (matrix : Matrix3 <S>)
  -> [Option <(S, Vector3 <S>)>; 3]
where S : Real + Cbrt + num::NumCast + approx::RelativeEq <Epsilon=S> + std::fmt::Debug {
  use num::One;
  let mut eigenpairs = if Matrix3::with_diagonal (matrix.diagonal()) == matrix {
    // diagonal matrix: eigenvalues are the values of the diagonal and the eigenvectors
    // are the standard basis vectors
    let eigenvalues  = matrix.diagonal();
    let eigenvectors = Matrix3::identity().cols;
    std::array::from_fn (|i| Some((eigenvalues[i], eigenvectors[i])))
  } else {
    // eigenvalues will be the solution of the cubic equation:
    // x^3 - trace * x^2 + sum_of_principal_minors * x - determinant = 0
    // eigenvectors will be solution to (matrix - e*I)v = 0 for each eigenvalue e
    let trace = matrix.trace();
    let sum_of_principal_minors = (0..3).map (|i| matrix.minor (i, i)).sum::<S>();
    let determinant = matrix.determinant();
    let system_matrix = |eigenvalue| matrix - Matrix3::broadcast_diagonal (eigenvalue);
    let eigenvalues = super::solve_cubic_equation (
      NonZero::one(), -trace, sum_of_principal_minors, -determinant);
    let mut eigenpairs = vec![];
    for eigenvalue in eigenvalues.iter().flatten().copied() {
      let eigenvectors = solve_system_3_homogeneous (system_matrix (eigenvalue));
      for eigenvector in eigenvectors {
        eigenpairs.push ((eigenvalue, eigenvector));
      }
    }
    std::array::from_fn (|i| eigenpairs.get (i).copied())
  };
  // reverse sort by eigenvalue
  eigenpairs.sort_by (|a, b| match (a, b) {
    (Some (a), Some (b)) => b.0.partial_cmp (&a.0).unwrap(),
    (None,     Some (_)) => std::cmp::Ordering::Greater,
    (Some (_), None)     => std::cmp::Ordering::Less,
    (None,     None)     => std::cmp::Ordering::Equal
  });
  eigenpairs
}

/// Reduce a 3x3 matrix to row echelon form using Gaussian elimination
pub fn row_reduce3 <S> (m : Matrix3 <S>) -> Matrix3 <S> where
  S : OrderedField + approx::RelativeEq <Epsilon=S> + std::fmt::Debug
{
  // algorithm from wikipedia article on Gaussian elimination (25-09-26)
  let mut rows = m.into_row_arrays();
  let mut h = 0;  // pivot row
  let mut k = 0;  // pivot column
  while h <= 2 && k <= 2 {
    // find the k-th pivot
    // find the row with the largest value in the k-th column
    let abs_row = |i : usize| rows[i][k].abs();
    let i_max = (h..3).max_by (|i, j| abs_row (*i).partial_cmp (&abs_row (*j)).unwrap())
      .unwrap();
    if !approx::abs_diff_eq!(rows[i_max][k], S::zero(),
      epsilon = S::four() * S::default_epsilon()
    ) {
      // pivot in this column
      rows.swap (h, i_max);
      // for all rows below pivot
      for i in h+1..3 {
        let f = rows[i][k] / rows[h][k];
        // fill lower part of pivot column with zeros
        rows[i][k] = S::zero();
        // for all remaining elements in current row
        #[expect(clippy::needless_range_loop)]
        for j in k+1..3 {
          let sub = rows[h][j] * f;
          if approx::relative_eq!(rows[i][j], sub,
            max_relative = S::eight() * S::default_max_relative()
          ) {
            rows[i][j] = S::zero();
          } else {
            rows[i][j] -= rows[h][j] * f;
          }
        }
      }
      // increase pivot row and column
      h += 1;
    }
    k += 1;
  }
  Matrix3::from_row_arrays (rows)
}

/// Solve a 2D homogeneous system of linear equations represented by the matrix equation
/// `Ax = 0`.
///
/// If the system matrix is zero, any vector in $R^2$ is a solution and `None` is
/// returned:
///
/// ```
/// # use math_utils::{Matrix2, Vector2};
/// # use math_utils::algebra::linear::solve_system_2_homogeneous;
/// assert_eq!(solve_system_2_homogeneous (Matrix2::<f32>::zero()), None);
/// ```
///
/// If the system matrix is invertible, the unique solution is the zero vector:
///
/// ```
/// # use math_utils::{Matrix2, Vector2};
/// # use math_utils::algebra::linear::solve_system_2_homogeneous;
/// let mat = Matrix2::<f32>::from_col_arrays ([
///   [0.0, -1.0],
///   [1.0,  0.0]
/// ]);
/// assert_eq!(solve_system_2_homogeneous (mat), Some (Vector2::zero()));
/// ```
///
/// Otherwise the solution spans a linear subspace and a member of that subspace is
/// returned.
pub fn solve_system_2_homogeneous <S> (system_matrix : Matrix2 <S>)
  -> Option <Vector2 <S>>
where S : Field + approx::AbsDiffEq <Epsilon=S> {
  if system_matrix == Matrix2::zero() {
    return None
  }
  if LinearIso::is_invertible_approx (system_matrix, None) {
    Some (Vector2::zero())
  } else {
    let [
      [a, c],
      [b, d]
    ] = system_matrix.into_col_arrays();
    if b != S::zero() {
      Some (vector2 (S::one(), -a/b))
    } else if d != S::zero() {
      Some (vector2 (S::one(), -c/d))
    } else {
      Some (vector2 (S::zero(), S::one()))
    }
  }
}

/// Solve a 3D homogeneous system of linear equations represented by the matrix equation
/// `Ax = 0`.
///
/// If the system matrix is zero, any vector in $R^3$ is a solution and `None` is
/// returned:
///
/// ```
/// # use math_utils::{Matrix3, Vector3};
/// # use math_utils::algebra::linear::solve_system_3_homogeneous;
/// assert!(solve_system_3_homogeneous (Matrix3::<f32>::zero()).is_empty());
/// ```
///
/// If the system matrix is invertible, the unique solution is the zero vector:
///
/// ```
/// # use math_utils::{Matrix3, Vector3};
/// # use math_utils::algebra::linear::solve_system_3_homogeneous;
/// let mat = Matrix3::<f32>::from_row_arrays ([
///   [0.0, -1.0, 0.0],
///   [0.0,  0.0, 1.0],
///   [1.0,  0.0, 0.0]
/// ]);
/// assert_eq!(solve_system_3_homogeneous (mat), vec![Vector3::zero()]);
/// ```
///
/// Otherwise the solution spans a linear subspace and a member of that subspace is
/// returned.
pub fn solve_system_3_homogeneous <S> (system_matrix : Matrix3 <S>)
  -> Vec <Vector3 <S>>
where S : OrderedField + num::NumCast + approx::RelativeEq <Epsilon=S> + std::fmt::Debug {
  if system_matrix == Matrix3::zero() {
    return vec![]
  }
  if LinearIso::is_invertible_approx (system_matrix, Some (S::from (0.001).unwrap())) {
    vec![Vector3::zero()]
  } else {
    let row_echelon_form = row_reduce3 (system_matrix).into_row_arrays();
    let mut pivot_cols = [usize::MAX; 3];
    for i in 0..3 {
      #[expect(clippy::needless_range_loop)]
      for j in 0..3 {
        if approx::abs_diff_ne!(row_echelon_form[i][j], S::zero(),
          epsilon = S::four() * S::default_epsilon()
        ) {
          pivot_cols[i] = j;
          break
        }
      }
    }
    debug_assert!(pivot_cols.iter().any (|i| *i != usize::MAX),
      "we checked that the system matrix is non-zero, so there should be at least 1 \
      pivot");
    debug_assert!(!pivot_cols.iter().all (|i| *i != usize::MAX),
      "we checked that the system matrix is singular, so there should never be 3 pivots"
    );
    let mut basis = vec![];
    for free_col in (0..3).filter (|col| !pivot_cols.contains (col)) {
      let mut vec = Vector3::zero();
      vec[free_col] = S::one();
      for i in (0..3).rev().filter (|i| pivot_cols[*i] != usize::MAX) {
        let pivot_col = pivot_cols[i];
        let row = row_echelon_form[i];
        vec[pivot_col] = -(pivot_col+1..3).map (|j| row[j] * vec[j]).sum::<S>()
          / row[pivot_col];
      }
      basis.push (vec);
    }
    debug_assert!(!basis.is_empty());
    basis
  }
}

impl <S> ElementaryReflector for Matrix3 <S> where S : OrderedField + Sqrt {
  type Vector = Vector3 <S>;

  /// Householder transformation
  fn elementary_reflector (v : Vector3 <S>, index : usize) -> Matrix3 <S> {
    debug_assert!(index < 3);
    let d      = &v[index..];
    let d_0    = d[0];
    let d_norm = d.iter().copied().map (|x| x * x).sum::<S>().sqrt();
    let alpha = if d_0 >= S::zero() {
      -d_norm
    } else {
      d_norm
    };
    if alpha == S::zero() {
      return Matrix3::identity()
    }
    let d_m = d.len();
    let mut v = vec![S::zero(); d_m];
    v[0] = (S::half() * (S::one() - d_0 / alpha)).sqrt();
    let p = -alpha * v[0];
    if d_m > 1 {
      for i in 1..d_m {
        v[i] = d[i] / (S::two() * p);
      }
    }
    let mut w = Vector3::zero();
    for i in index..3 {
      w[i] = v[i - index];
    }
    let w_dyadp = w.outer_product (w);
    Matrix3::<S>::identity() - (w_dyadp * S::two())
  }
}

impl <S> UpperHessenberg3 <S> {
  pub fn mat (self) -> Matrix3 <S> {
    self.0
  }
}

#[cfg(test)]
mod tests {
  #![expect(clippy::unreadable_literal)]

  use super::*;
  use approx::RelativeEq;

  #[test]
  fn eigensystem_2d() {
    let mat = Matrix2::from_row_arrays ([
      [1.0, 1.0],
      [0.0, 0.0] ]);
    let (eigenvalues, eigenvectors) : (Vec<_>, Vec<_>) =
      eigensystem_2 (mat).map (Option::unwrap).iter().copied().unzip();
    assert_eq!(eigenvalues, [1.0, 0.0]);
    assert_eq!(eigenvectors, [
      [1.0,  0.0],
      [1.0, -1.0]
    ].map (Vector2::from));

    let mat = Matrix2::from_row_arrays ([
      [1.0, 0.0],
      [1.0, 0.0] ]);
    let (eigenvalues, eigenvectors) : (Vec<_>, Vec<_>) =
      eigensystem_2 (mat).map (Option::unwrap).iter().copied().unzip();
    assert_eq!(eigenvalues, [1.0, 0.0]);
    assert_eq!(eigenvectors, [
      [1.0, 1.0],
      [0.0, 1.0]
    ].map (Vector2::from));

    let mat = Matrix2::from_row_arrays ([
      [2.0, 1.0],
      [1.0, 2.0] ]);
    let (eigenvalues, eigenvectors) : (Vec<_>, Vec<_>) =
      eigensystem_2 (mat).map (Option::unwrap).iter().copied().unzip();
    assert_eq!(eigenvalues, [3.0, 1.0]);
    assert_eq!(eigenvectors, [
      [1.0,  1.0],
      [1.0, -1.0]
    ].map (Vector2::from));

    let mat = Matrix2::from_row_arrays ([
      [1.0, 0.0],
      [1.0, 3.0] ]);
    let (eigenvalues, eigenvectors) : (Vec<_>, Vec<_>) =
      eigensystem_2 (mat).map (Option::unwrap).iter().copied().unzip();
    assert_eq!(eigenvalues, [3.0, 1.0]);
    assert_eq!(eigenvectors, [
      [0.0,  1.0],
      [1.0, -0.5]
    ].map (Vector2::from));
  }

  #[test]
  fn eigensystem_3d() {
    let mat = Matrix3::<f32>::from_row_arrays ([
      [0.0, 1.0, 0.0],
      [0.0, 0.0, 1.0],
      [0.0, 0.0, 0.0]
    ]);
    let (eigenvalues, eigenvectors) : (Vec<_>, Vec<_>) =
      eigensystem_3_classical (mat).iter().flatten().copied().unzip();
    assert_eq!(eigenvalues, [0.0]);
    assert_eq!(eigenvectors, [
      [1.0, 0.0, 0.0]
    ].map (Vector3::from));
    let mat = Matrix3::<f32>::from_row_arrays ([
      [2.0, 1.0, 0.0],
      [0.0, 2.0, 0.0],
      [0.0, 0.0, 2.0]
    ]);
    let (eigenvalues, eigenvectors) : (Vec<_>, Vec<_>) =
      eigensystem_3_classical (mat).iter().flatten().copied().unzip();
    assert_eq!(eigenvalues, [2.0, 2.0]);
    assert_eq!(eigenvectors, [
      [1.0, 0.0, 0.0],
      [0.0, 0.0, 1.0]
    ].map (Vector3::from));
    let mat = Matrix3::<f32>::from_row_arrays ([
      [9.714286, 0.0, 8.571428], [0.0, 1.1428572, 0.0], [8.571428, 0.0, 9.714286]
    ]);
    /* classical root-finding fails for this matrix
    let (eigenvalues, eigenvectors) : (Vec<_>, Vec<_>) =
      eigensystem_3_classical (mat).iter().flatten().copied().unzip();
    */
    let (eigenvalues, eigenvectors) : (Vec<_>, Vec<_>) =
      eigensystem_3 (mat).iter().flatten().copied().unzip();
    assert_eq!(eigenvalues, [18.285717, 1.1428576, 1.1428576]);
    approx::assert_relative_eq!(eigenvectors[0], [ 1.0, 0.0, 1.0].into(),
      max_relative = 4.0 * f32::default_max_relative());
    assert_eq!(eigenvectors[1], [ 0.0, 1.0, 0.0].into());
    assert_eq!(eigenvectors[2], [-1.0, 0.0, 1.0].into());
  }

  #[test]
  fn row_reduce_3d() {
    let mat = Matrix3::from_row_arrays ([
      [0.0, 1.0, 0.0],
      [0.0, 0.0, 1.0],
      [1.0, 0.0, 0.0] ]);
    let reduced = row_reduce3 (mat);
    assert_eq!(reduced, Matrix3::identity());

    let mat = Matrix3::from_row_arrays ([
      [0.0, 1.0,  0.0],
      [0.0, 0.0, -1.0],
      [1.0, 0.0,  0.0] ]);
    let reduced = row_reduce3 (mat);
    assert_eq!(reduced, Matrix3::from_row_arrays ([
      [1.0, 0.0,  0.0],
      [0.0, 1.0,  0.0],
      [0.0, 0.0, -1.0] ]));

    let mat = Matrix3::from_row_arrays ([
      [1.0, 2.0, 3.0],
      [2.0, 4.0, 6.0],
      [1.0, 1.0, 1.0] ]);
    let reduced = row_reduce3 (mat);
    assert_eq!(reduced, Matrix3::from_row_arrays ([
      [2.0,  4.0,  6.0],
      [0.0, -1.0, -2.0],
      [0.0,  0.0,  0.0] ]));
  }

  #[test]
  fn solve_homogeneous_2d() {
    let mat = Matrix2::from_row_arrays ([
      [1.0, 1.0],
      [0.0, 0.0] ]);
    assert_eq!(solve_system_2_homogeneous (mat), Some (vector2 (1.0, -1.0)));
    let mat = Matrix2::from_row_arrays ([
      [1.0, 0.0],
      [1.0, 0.0] ]);
    assert_eq!(solve_system_2_homogeneous (mat), Some (vector2 (0.0, 1.0)));
  }

  #[test]
  fn solve_homogeneous_3d() {
    // rank 2: linear nullspace
    let mat = Matrix3::from_row_arrays ([
      [1.0, 2.0, 3.0],
      [4.0, 5.0, 6.0],
      [7.0, 8.0, 9.0] ]);
    let solution = solve_system_3_homogeneous (mat);
    debug_assert_eq!(solution.len(), 1);
    solution.into_iter().for_each (|basis| assert_eq!(mat * basis, Vector3::zero()));
    let mat = Matrix3::from_row_arrays ([
      [1.0, 2.0, 3.0],
      [2.0, 4.0, 2.0],
      [3.0, 6.0, 9.0] ]);
    let solution = solve_system_3_homogeneous (mat);
    debug_assert_eq!(solution.len(), 1);
    solution.into_iter().for_each (|basis| assert_eq!(mat * basis, Vector3::zero()));
    // rank 1: planar nullspace
    let mat = Matrix3::from_row_arrays ([
      [2.0,  4.0,  6.0],
      [4.0,  8.0, 12.0],
      [6.0, 12.0, 18.0] ]);
    let solution = solve_system_3_homogeneous (mat);
    debug_assert_eq!(solution.len(), 2);
    solution.into_iter().for_each (|basis| assert_eq!(mat * basis, Vector3::zero()));
    let mat = Matrix3::from_row_arrays ([
      [ 4.0, -1.0, 2.0],
      [ 8.0, -2.0, 4.0],
      [12.0, -3.0, 6.0] ]);
    let solution = solve_system_3_homogeneous (mat);
    debug_assert_eq!(solution.len(), 2);
    solution.into_iter().for_each (|basis| assert_eq!(mat * basis, Vector3::zero()));
  }
}