geop_core_math/
matrix.rs

1//! A dense, fixed-size, row-major matrix — [`Vector`]'s 2-D counterpart.
2//!
3//! Exists so callers building a normal-equations system (`JᵀJ`, `Jᵀf`, …)
4//! work with actual matrix/vector values and named operations
5//! (`transpose`, `mul_vec`, `mul_mat`) instead of raw `[[Scalar; N]; M]` arrays
6//! and hand-rolled index loops at every call site — see
7//! [`crate::solvers::linear::solve_linear_system`].
8
9use std::ops::{Index, IndexMut};
10
11use crate::{geop_error::GeopResult, scalars::Scalar, vector::Vector};
12
13/// Dense `R×C` matrix, row-major (`self[(row, col)]`).
14#[derive(Debug, Clone, Copy)]
15pub struct Matrix<const R: usize, const C: usize> {
16    data: [[Scalar; C]; R],
17}
18
19impl<const R: usize, const C: usize> Matrix<R, C> {
20    pub fn from_rows(data: [[Scalar; C]; R]) -> Self {
21        Self { data }
22    }
23
24    pub fn zero() -> Self {
25        Self {
26            data: [[Scalar::ZERO; C]; R],
27        }
28    }
29
30    /// Build from `C` column vectors, each an `R`-vector.
31    pub fn from_columns(cols: [Vector<R>; C]) -> Self {
32        let mut out = Self::zero();
33        for (c, col) in cols.iter().enumerate() {
34            for r in 0..R {
35                out.data[r][c] = col[r];
36            }
37        }
38        out
39    }
40
41    pub fn row(&self, r: usize) -> Vector<C> {
42        Vector::from_array(self.data[r])
43    }
44
45    pub fn col(&self, c: usize) -> Vector<R> {
46        let mut out = Vector::<R>::zero();
47        for r in 0..R {
48            out[r] = self.data[r][c];
49        }
50        out
51    }
52
53    pub fn transpose(&self) -> Matrix<C, R> {
54        let mut out = Matrix::<C, R>::zero();
55        for r in 0..R {
56            for c in 0..C {
57                out[(c, r)] = self[(r, c)];
58            }
59        }
60        out
61    }
62
63    /// `self · v`.
64    pub fn mul_vec(&self, v: &Vector<C>) -> Vector<R> {
65        let mut out = Vector::<R>::zero();
66        for r in 0..R {
67            out[r] = self.row(r).prod_dot(v);
68        }
69        out
70    }
71
72    /// `self · other`.
73    pub fn mul_mat<const K: usize>(&self, other: &Matrix<C, K>) -> Matrix<R, K> {
74        let mut out = Matrix::<R, K>::zero();
75        for r in 0..R {
76            for k in 0..K {
77                out[(r, k)] = self.row(r).prod_dot(&other.col(k));
78            }
79        }
80        out
81    }
82
83    pub(crate) fn swap_rows(&mut self, i: usize, j: usize) {
84        self.data.swap(i, j);
85    }
86}
87
88impl<const R: usize, const C: usize> Index<(usize, usize)> for Matrix<R, C> {
89    type Output = Scalar;
90    fn index(&self, (r, c): (usize, usize)) -> &Scalar {
91        &self.data[r][c]
92    }
93}
94impl<const R: usize, const C: usize> IndexMut<(usize, usize)> for Matrix<R, C> {
95    fn index_mut(&mut self, (r, c): (usize, usize)) -> &mut Scalar {
96        &mut self.data[r][c]
97    }
98}
99
100/// The eigenvalues of the symmetric `a`, ascending — each enclosed — and a
101/// unit eigenvector of each.
102///
103/// Cyclic Jacobi rotations diagonalize `a`'s midpoint in `f64`; which
104/// rotations to take is a free choice, so that is not where the enclosure
105/// comes from. It comes from two bounds every computed pair `(ρ, v)`
106/// satisfies: a symmetric matrix has an eigenvalue within `|M v - ρ v|` of
107/// the Rayleigh quotient `ρ` of a unit `v` (evaluated here in interval arithmetic), and
108/// moving `M` to any matrix `a` holds moves each eigenvalue, in order, by
109/// at most the Frobenius norm of the difference (Weyl) — bounded by `a`'s
110/// half-widths. Where two enclosures overlap their eigenvalues cannot be
111/// told apart, so each gets their hull. The vectors are the midpoint's:
112/// where eigenvalues coincide, as for a cylinder's two transverse moments,
113/// any vector of their plane is one, and these are a free choice of it.
114#[allow(clippy::needless_range_loop)]
115pub fn symmetric_eigen3(a: &Matrix<3, 3>) -> GeopResult<([Scalar; 3], [Vector<3>; 3])> {
116    let mut m = [[0.0f64; 3]; 3];
117    for r in 0..3 {
118        for c in 0..3 {
119            // Symmetric by construction: the mean of the two midpoints.
120            m[r][c] = 0.5 * (a[(r, c)].to_f64() + a[(c, r)].to_f64());
121        }
122    }
123    let midpoint = m;
124    let mut v = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0f64]];
125    for _ in 0..64 {
126        let off = m[0][1].abs() + m[0][2].abs() + m[1][2].abs();
127        if off == 0.0 {
128            break;
129        }
130        for (p, q) in [(0, 1), (0, 2), (1, 2)] {
131            if m[p][q] == 0.0 {
132                continue;
133            }
134            let theta = (m[q][q] - m[p][p]) / (2.0 * m[p][q]);
135            let t = theta.signum() / (theta.abs() + (theta * theta + 1.0).sqrt());
136            let c = 1.0 / (t * t + 1.0).sqrt();
137            let s = t * c;
138            for k in 0..3 {
139                let (mkp, mkq) = (m[k][p], m[k][q]);
140                m[k][p] = c * mkp - s * mkq;
141                m[k][q] = s * mkp + c * mkq;
142            }
143            for k in 0..3 {
144                let (mpk, mqk) = (m[p][k], m[q][k]);
145                m[p][k] = c * mpk - s * mqk;
146                m[q][k] = s * mpk + c * mqk;
147            }
148            for row in &mut v {
149                let (vp, vq) = (row[p], row[q]);
150                row[p] = c * vp - s * vq;
151                row[q] = s * vp + c * vq;
152            }
153        }
154    }
155    // Weyl: how far `a` may be from its midpoint.
156    let mut spread = Scalar::ZERO;
157    for r in 0..3 {
158        for c in 0..3 {
159            let half = a[(r, c)].width().div(Scalar::TWO)?;
160            spread = spread.add(half.mul(half));
161        }
162    }
163    let spread = spread.sqrt()?.upper();
164    let sharp = Matrix::<3, 3>::from_rows(midpoint.map(|row| row.map(Scalar::from_f64)));
165    let mut pairs = Vec::with_capacity(3);
166    for k in 0..3 {
167        let column = Vector::from_array([v[0][k], v[1][k], v[2][k]].map(Scalar::from_f64));
168        let unit = column.normalize()?;
169        let image = sharp.mul_vec(&unit);
170        let rayleigh = unit.prod_dot(&image);
171        let residual = image.sub(&unit.prod_scalar(rayleigh)).norm().upper();
172        let reach = residual.add(spread);
173        let enclosure = rayleigh
174            .sub(reach)
175            .lower()
176            .union(rayleigh.add(reach).upper());
177        pairs.push((enclosure, unit));
178    }
179    pairs.sort_by(|a, b| a.0.to_f64().total_cmp(&b.0.to_f64()));
180    // Overlapping enclosures share their hull.
181    let mut values: Vec<Scalar> = pairs.iter().map(|(e, _)| *e).collect();
182    for _ in 0..2 {
183        for i in 0..2 {
184            if values[i].could_be_equal(values[i + 1]) {
185                let hull = values[i].union(values[i + 1]);
186                values[i] = hull;
187                values[i + 1] = hull;
188            }
189        }
190    }
191    Ok((
192        [values[0], values[1], values[2]],
193        [pairs[0].1, pairs[1].1, pairs[2].1],
194    ))
195}
196
197#[cfg(test)]
198mod tests {
199    use super::*;
200
201    /// A rotated diagonal matrix: its eigenvalues are enclosed, a repeated
202    /// pair shares one enclosure, and each vector is one of its value.
203    #[test]
204    fn eigen_of_a_rotated_diagonal() {
205        let (c, s) = (0.6, 0.8);
206        // R diag(2, 5, 5) R^T, R a rotation about z.
207        let r = [[c, -s, 0.0], [s, c, 0.0], [0.0, 0.0, 1.0]];
208        let d = [2.0, 5.0, 5.0];
209        let mut rows = [[Scalar::ZERO; 3]; 3];
210        for i in 0..3 {
211            for j in 0..3 {
212                let x: f64 = (0..3).map(|k| r[i][k] * d[k] * r[j][k]).sum();
213                rows[i][j] = Scalar::from_f64(x);
214            }
215        }
216        let a = Matrix::from_rows(rows);
217        let (values, vectors) = symmetric_eigen3(&a).unwrap();
218        for (value, want) in values.iter().zip(d) {
219            assert!(value.could_be_equal(Scalar::from_f64(want)), "{values:?}");
220            assert!(value.width().to_f64() < 1e-3, "{values:?}");
221        }
222        let image = a.mul_vec(&vectors[0]);
223        assert!(image.could_be_equal(&vectors[0].prod_scalar(values[0])));
224    }
225
226    #[test]
227    fn transpose_and_mul() {
228        let m = Matrix::<2, 3>::from_rows([
229            [
230                Scalar::from_f64(1.0),
231                Scalar::from_f64(2.0),
232                Scalar::from_f64(3.0),
233            ],
234            [
235                Scalar::from_f64(4.0),
236                Scalar::from_f64(5.0),
237                Scalar::from_f64(6.0),
238            ],
239        ]);
240        let mt = m.transpose();
241        assert!(mt[(0, 1)].could_be_equal(Scalar::from_f64(4.0)));
242        assert!(mt[(2, 0)].could_be_equal(Scalar::from_f64(3.0)));
243
244        let v = Vector::<3>::from_array([Scalar::ONE, Scalar::ONE, Scalar::ONE]);
245        let mv = m.mul_vec(&v);
246        assert!(mv[0].could_be_equal(Scalar::from_f64(6.0)));
247        assert!(mv[1].could_be_equal(Scalar::from_f64(15.0)));
248
249        let mtm = mt.mul_mat(&m); // 3x3
250        assert!(mtm[(0, 0)].could_be_equal(Scalar::from_f64(1.0 + 16.0)));
251    }
252}
geop_core_math/matrix.rs

geop_core_math/
matrix.rs
