Skip to main content

tpt_math_linalg_fixed/
lib.rs

1#![no_std]
2#![forbid(unsafe_code)]
3// Fixed-size matrix/vector algorithms are clearest with explicit indexing over
4// fixed-size arrays; the indexed-loop and manual-swap lints do not fit this
5// code, and the operator-impl lints misfire on componentwise arithmetic.
6#![allow(
7    clippy::needless_range_loop,
8    clippy::manual_swap,
9    clippy::suspicious_arithmetic_impl,
10    clippy::suspicious_op_assign_impl
11)]
12//! Const-generic, fixed-size (stack-allocated) linear algebra.
13//!
14//! This crate mirrors the `Vector3`/`Matrix4` layer of `nalgebra` but is
15//! implemented from scratch with const generics, so it carries no Apache-2.0
16//! dependency and needs no allocator. Storage is plain fixed-size arrays
17//! (`[T; N]` / `[[T; C]; R]`), so every type here is `#![no_std]` and
18//! allocator-free.
19//!
20//! All arithmetic is generic over [`tpt_math_numeric::Scalar`] (i.e. any float
21//! type), and the usual elementwise, scalar, dot/cross and matrix-product ops
22//! are implemented. Square matrices get closed-form [`Matrix::determinant`] and
23//! [`Matrix::inverse`] (exact for 2×2 / 3×3 / 4×4, and by Gauss–Jordan for any
24//! size).
25//!
26//! # Examples
27//!
28//! ```
29//! use tpt_math_linalg_fixed::{Vector3, Matrix3};
30//!
31//! let a = Vector3::new([1.0_f64, 2.0, 3.0]);
32//! let b = Vector3::new([4.0_f64, 5.0, 6.0]);
33//! assert_eq!(a.dot(&b), 32.0);
34//!
35//! let m = Matrix3::identity();
36//! assert_eq!(m * a, a);
37//! ```
38
39use core::fmt;
40use core::ops::{Add, Div, Index, Mul, Neg, Sub};
41
42use tpt_math_numeric::Scalar;
43
44// ===========================================================================
45// Vector
46// ===========================================================================
47
48/// A fixed-size column vector of `N` elements of type `T`, stored inline.
49#[derive(Clone, Copy, PartialEq, Eq)]
50pub struct Vector<T, const N: usize> {
51    /// Row-major element storage.
52    pub data: [T; N],
53}
54
55impl<T, const N: usize> Vector<T, N> {
56    /// Build from a fixed-size array (row-major order).
57    pub fn new(data: [T; N]) -> Self {
58        Vector { data }
59    }
60
61    /// Build element-by-element with `f(i)`.
62    pub fn from_fn(f: impl FnMut(usize) -> T) -> Self {
63        Vector {
64            data: array_init::<T, N>(f),
65        }
66    }
67
68    /// Number of components.
69    pub fn len(&self) -> usize {
70        N
71    }
72
73    /// Always `false` (a fixed vector has a constant, non-zero length unless
74    /// `N == 0`).
75    pub fn is_empty(&self) -> bool {
76        N == 0
77    }
78
79    /// Iterate over the components.
80    pub fn iter(&self) -> impl Iterator<Item = &T> {
81        self.data.iter()
82    }
83}
84
85impl<T: Scalar, const N: usize> Vector<T, N> {
86    /// The Euclidean (L2) norm.
87    pub fn norm(&self) -> T {
88        let s = self.dot(self);
89        s.sqrt()
90    }
91
92    /// Normalise to unit length. Returns the zero vector if the norm is zero.
93    pub fn normalize(&self) -> Self {
94        let n = self.norm();
95        if n == T::zero() {
96            *self
97        } else {
98            *self * (T::one() / n)
99        }
100    }
101
102    /// Dot product with another vector of the same length.
103    pub fn dot(&self, other: &Vector<T, N>) -> T {
104        let mut s = T::zero();
105        for i in 0..N {
106            s = s + self.data[i] * other.data[i];
107        }
108        s
109    }
110}
111
112/// Per-size accessors (`.x()`, `.y()`, …) that only exist when the vector is
113/// large enough, so they can never panic on a too-small vector.
114impl<T: Scalar + Copy> Vector<T, 2> {
115    /// First component.
116    pub fn x(&self) -> T {
117        self.data[0]
118    }
119    /// Second component.
120    pub fn y(&self) -> T {
121        self.data[1]
122    }
123}
124
125impl<T: Scalar + Copy> Vector<T, 3> {
126    /// First component.
127    pub fn x(&self) -> T {
128        self.data[0]
129    }
130    /// Second component.
131    pub fn y(&self) -> T {
132        self.data[1]
133    }
134    /// Third component.
135    pub fn z(&self) -> T {
136        self.data[2]
137    }
138
139    /// 3-D cross product.
140    pub fn cross(&self, other: &Vector<T, 3>) -> Vector<T, 3> {
141        Vector::new([
142            self.data[1] * other.data[2] - self.data[2] * other.data[1],
143            self.data[2] * other.data[0] - self.data[0] * other.data[2],
144            self.data[0] * other.data[1] - self.data[1] * other.data[0],
145        ])
146    }
147}
148
149impl<T: Scalar + Copy> Vector<T, 4> {
150    /// First component.
151    pub fn x(&self) -> T {
152        self.data[0]
153    }
154    /// Second component.
155    pub fn y(&self) -> T {
156        self.data[1]
157    }
158    /// Third component.
159    pub fn z(&self) -> T {
160        self.data[2]
161    }
162    /// Fourth component.
163    pub fn w(&self) -> T {
164        self.data[3]
165    }
166}
167
168impl<T: Scalar + Copy, const N: usize> Vector<T, N> {
169    /// 2-D perpendicular dot product (z-component of the 2-D cross product).
170    /// Available on vectors of length ≥ 2; panics for `N < 2`.
171    pub fn perp_dot(&self, other: &Vector<T, N>) -> T {
172        self.data[0] * other.data[1] - self.data[1] * other.data[0]
173    }
174}
175
176impl<T, const N: usize> Index<usize> for Vector<T, N> {
177    type Output = T;
178    /// # Panics
179    ///
180    /// Panics if `i >= N`.
181    fn index(&self, i: usize) -> &T {
182        &self.data[i]
183    }
184}
185
186impl<T: Scalar, const N: usize> Add for Vector<T, N> {
187    type Output = Vector<T, N>;
188    /// # Panics
189    ///
190    /// Inherits `T`'s `+` semantics (never panics for floats).
191    fn add(self, rhs: Vector<T, N>) -> Vector<T, N> {
192        Vector::from_fn(|i| self.data[i] + rhs.data[i])
193    }
194}
195
196impl<T: Scalar, const N: usize> Sub for Vector<T, N> {
197    type Output = Vector<T, N>;
198    fn sub(self, rhs: Vector<T, N>) -> Vector<T, N> {
199        Vector::from_fn(|i| self.data[i] - rhs.data[i])
200    }
201}
202
203impl<T: Scalar, const N: usize> Neg for Vector<T, N> {
204    type Output = Vector<T, N>;
205    fn neg(self) -> Vector<T, N> {
206        Vector::from_fn(|i| -self.data[i])
207    }
208}
209
210impl<T: Scalar, const N: usize> Mul<T> for Vector<T, N> {
211    type Output = Vector<T, N>;
212    fn mul(self, rhs: T) -> Vector<T, N> {
213        Vector::from_fn(|i| self.data[i] * rhs)
214    }
215}
216
217impl<T: Scalar, const N: usize> Div<T> for Vector<T, N> {
218    type Output = Vector<T, N>;
219    fn div(self, rhs: T) -> Vector<T, N> {
220        Vector::from_fn(|i| self.data[i] / rhs)
221    }
222}
223
224/// Componentwise product of two same-length vectors.
225impl<T: Scalar, const N: usize> Mul<Vector<T, N>> for Vector<T, N> {
226    type Output = Vector<T, N>;
227    fn mul(self, rhs: Vector<T, N>) -> Vector<T, N> {
228        Vector::from_fn(|i| self.data[i] * rhs.data[i])
229    }
230}
231
232/// Componentwise quotient of two same-length vectors.
233impl<T: Scalar, const N: usize> Div<Vector<T, N>> for Vector<T, N> {
234    type Output = Vector<T, N>;
235    fn div(self, rhs: Vector<T, N>) -> Vector<T, N> {
236        Vector::from_fn(|i| self.data[i] / rhs.data[i])
237    }
238}
239
240impl<T: fmt::Debug, const N: usize> fmt::Debug for Vector<T, N> {
241    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
242        f.debug_list().entries(self.data.iter()).finish()
243    }
244}
245
246// ===========================================================================
247// Matrix
248// ===========================================================================
249
250/// A fixed-size `R × C` matrix of elements of type `T`, stored row-major.
251#[derive(Clone, Copy, PartialEq, Eq)]
252pub struct Matrix<T, const R: usize, const C: usize> {
253    /// Row-major storage: `data[row][col]`.
254    pub data: [[T; C]; R],
255}
256
257impl<T: Scalar, const R: usize, const C: usize> Matrix<T, R, C> {
258    /// Build from a row-major nested array.
259    pub fn new(data: [[T; C]; R]) -> Self {
260        Matrix { data }
261    }
262
263    /// Build element-by-element with `f(row, col)`.
264    pub fn from_fn(f: impl FnMut(usize, usize) -> T) -> Self {
265        let data = array2_init::<T, R, C>(f);
266        Matrix { data }
267    }
268
269    /// Build from a flat row-major slice of length `R * C` (row-major order).
270    pub fn from_array(data: &[T]) -> Self
271    where
272        T: Scalar,
273    {
274        let mut out = [[T::zero(); C]; R];
275        let mut k = 0;
276        for i in 0..R {
277            for j in 0..C {
278                out[i][j] = data[k];
279                k += 1;
280            }
281        }
282        Matrix { data: out }
283    }
284
285    /// Build from `C` column vectors (each of length `R`). Returns `None` if
286    /// the number of columns is not `C`.
287    pub fn from_columns(columns: &[Vector<T, R>]) -> Option<Self>
288    where
289        T: Scalar,
290    {
291        if columns.len() != C {
292            return None;
293        }
294        let mut out = [[T::zero(); C]; R];
295        for (j, col) in columns.iter().enumerate() {
296            for i in 0..R {
297                out[i][j] = col.data[i];
298            }
299        }
300        Some(Matrix { data: out })
301    }
302
303    /// Number of rows.
304    pub fn nrows(&self) -> usize {
305        R
306    }
307
308    /// Number of columns.
309    pub fn ncols(&self) -> usize {
310        C
311    }
312
313    /// Transpose (only defined for non-rectangular via the explicit
314    /// [`Matrix::transpose`]).
315    pub fn transpose(&self) -> Matrix<T, C, R>
316    where
317        T: Copy,
318    {
319        let mut out = [[T::zero(); R]; C];
320        for i in 0..R {
321            for j in 0..C {
322                out[j][i] = self.data[i][j];
323            }
324        }
325        Matrix { data: out }
326    }
327
328    /// Iterate over all elements in row-major order.
329    pub fn iter(&self) -> impl Iterator<Item = &T> {
330        self.data.iter().flat_map(|row| row.iter())
331    }
332}
333
334impl<T: Copy, const R: usize, const C: usize> Matrix<T, R, C> {
335    /// Index a single element.
336    pub fn get(&self, i: usize, j: usize) -> T {
337        self.data[i][j]
338    }
339}
340
341impl<T: Scalar, const R: usize, const C: usize> Matrix<T, R, C> {
342    /// Matrix–vector product `self * v` (requires `v` of length `C`), giving a
343    /// vector of length `R`.
344    pub fn mul_vec(&self, v: &Vector<T, C>) -> Vector<T, R> {
345        let mut out = [T::zero(); R];
346        for i in 0..R {
347            let mut s = T::zero();
348            for j in 0..C {
349                s = s + self.data[i][j] * v.data[j];
350            }
351            out[i] = s;
352        }
353        Vector { data: out }
354    }
355}
356
357impl<T, const R: usize, const C: usize> Index<(usize, usize)> for Matrix<T, R, C> {
358    type Output = T;
359    /// # Panics
360    ///
361    /// Panics if `(i, j)` is out of bounds.
362    fn index(&self, (i, j): (usize, usize)) -> &T {
363        &self.data[i][j]
364    }
365}
366
367impl<T: Scalar, const R: usize, const C: usize> Add for Matrix<T, R, C> {
368    type Output = Matrix<T, R, C>;
369    fn add(self, rhs: Matrix<T, R, C>) -> Matrix<T, R, C> {
370        Matrix::from_fn(|i, j| self.data[i][j] + rhs.data[i][j])
371    }
372}
373
374impl<T: Scalar, const R: usize, const C: usize> Sub for Matrix<T, R, C> {
375    type Output = Matrix<T, R, C>;
376    fn sub(self, rhs: Matrix<T, R, C>) -> Matrix<T, R, C> {
377        Matrix::from_fn(|i, j| self.data[i][j] - rhs.data[i][j])
378    }
379}
380
381impl<T: Scalar, const R: usize, const C: usize> Neg for Matrix<T, R, C> {
382    type Output = Matrix<T, R, C>;
383    fn neg(self) -> Matrix<T, R, C> {
384        Matrix::from_fn(|i, j| -self.data[i][j])
385    }
386}
387
388impl<T: Scalar, const R: usize, const C: usize> Mul<T> for Matrix<T, R, C> {
389    type Output = Matrix<T, R, C>;
390    fn mul(self, rhs: T) -> Matrix<T, R, C> {
391        Matrix::from_fn(|i, j| self.data[i][j] * rhs)
392    }
393}
394
395impl<T: Scalar, const R: usize, const C: usize> Div<T> for Matrix<T, R, C> {
396    type Output = Matrix<T, R, C>;
397    fn div(self, rhs: T) -> Matrix<T, R, C> {
398        Matrix::from_fn(|i, j| self.data[i][j] / rhs)
399    }
400}
401
402/// Matrix–matrix product `(R×C) * (C×M) -> (R×M)`.
403impl<T: Scalar, const R: usize, const C: usize, const M: usize> Mul<Matrix<T, C, M>>
404    for Matrix<T, R, C>
405{
406    type Output = Matrix<T, R, M>;
407    /// # Panics
408    ///
409    /// Inherits `T`'s `*` semantics.
410    fn mul(self, rhs: Matrix<T, C, M>) -> Matrix<T, R, M> {
411        let mut out = [[T::zero(); M]; R];
412        for i in 0..R {
413            for k in 0..M {
414                let mut s = T::zero();
415                for j in 0..C {
416                    s = s + self.data[i][j] * rhs.data[j][k];
417                }
418                out[i][k] = s;
419            }
420        }
421        Matrix { data: out }
422    }
423}
424
425/// Matrix–vector product `(R×C) * (C) -> (R)`.
426impl<T: Scalar, const R: usize, const C: usize> Mul<Vector<T, C>> for Matrix<T, R, C> {
427    type Output = Vector<T, R>;
428    fn mul(self, rhs: Vector<T, C>) -> Vector<T, R> {
429        self.mul_vec(&rhs)
430    }
431}
432
433/// Row-vector–matrix product `(N) * (N×M) -> (M)`.
434impl<T: Scalar, const N: usize, const M: usize> Mul<Matrix<T, N, M>> for Vector<T, N> {
435    type Output = Vector<T, M>;
436    fn mul(self, rhs: Matrix<T, N, M>) -> Vector<T, M> {
437        let mut out = [T::zero(); M];
438        for k in 0..M {
439            let mut s = T::zero();
440            for j in 0..N {
441                s = s + self.data[j] * rhs.data[j][k];
442            }
443            out[k] = s;
444        }
445        Vector { data: out }
446    }
447}
448
449impl<T: fmt::Debug, const R: usize, const C: usize> fmt::Debug for Matrix<T, R, C> {
450    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
451        f.debug_struct("Matrix")
452            .field("nrows", &R)
453            .field("ncols", &C)
454            .finish()
455    }
456}
457
458impl<T: Scalar, const R: usize, const C: usize> Matrix<T, R, C> {
459    /// Componentwise (Hadamard) product of two same-shaped matrices.
460    pub fn component_mul(&self, rhs: &Matrix<T, R, C>) -> Matrix<T, R, C> {
461        Matrix::from_fn(|i, j| self.data[i][j] * rhs.data[i][j])
462    }
463
464    /// Componentwise (Hadamard) quotient of two same-shaped matrices.
465    pub fn component_div(&self, rhs: &Matrix<T, R, C>) -> Matrix<T, R, C> {
466        Matrix::from_fn(|i, j| self.data[i][j] / rhs.data[i][j])
467    }
468}
469
470// ===========================================================================
471// Square matrices: identity, determinant, inverse
472// ===========================================================================
473
474impl<T: Scalar, const N: usize> Matrix<T, N, N> {
475    /// The `N×N` identity matrix.
476    pub fn identity() -> Self {
477        Matrix::from_fn(|i, j| if i == j { T::one() } else { T::zero() })
478    }
479
480    /// Determinant via Gauss elimination with partial pivoting. Exact (up to
481    /// floating-point round-off) for small `N`.
482    pub fn determinant(&self) -> T {
483        let mut a = self.data;
484        let mut det = T::one();
485        let mut sign = T::one();
486        for k in 0..N {
487            let mut piv = k;
488            let mut mx = a[k][k].abs();
489            for i in (k + 1)..N {
490                let v = a[i][k].abs();
491                if v > mx {
492                    mx = v;
493                    piv = i;
494                }
495            }
496            if !mx.is_finite() || mx == T::zero() {
497                return T::zero();
498            }
499            if piv != k {
500                let tmp = a[k];
501                a[k] = a[piv];
502                a[piv] = tmp;
503                sign = -sign;
504            }
505            let ak = a[k][k];
506            det = det * ak;
507            for i in (k + 1)..N {
508                let f = a[i][k] / ak;
509                for j in (k + 1)..N {
510                    a[i][j] = a[i][j] - f * a[k][j];
511                }
512            }
513        }
514        det * sign
515    }
516
517    /// Inverse via Gauss–Jordan elimination with partial pivoting. Returns
518    /// `None` if the matrix is (numerically) singular.
519    pub fn inverse(&self) -> Option<Matrix<T, N, N>> {
520        self.inverse_with_tolerance(T::from(1e-12).unwrap_or(T::zero()))
521    }
522
523    /// [`Matrix::inverse`] with a scale-invariant threshold: a pivot below `rel`
524    /// times the largest entry magnitude counts as singular.
525    pub fn inverse_with_relative_tolerance(&self, rel: T) -> Option<Matrix<T, N, N>> {
526        let scale = self
527            .data
528            .iter()
529            .flat_map(|row| row.iter())
530            .fold(T::zero(), |acc, &x| acc.max(x.abs()));
531        self.inverse_with_tolerance(rel * scale)
532    }
533
534    /// [`Matrix::inverse`] with an absolute pivot threshold: a pivot with
535    /// magnitude below `tol` counts as singular. `tol = 0` accepts any non-zero
536    /// pivot.
537    pub fn inverse_with_tolerance(&self, tol: T) -> Option<Matrix<T, N, N>> {
538        let eps = tol;
539        let mut a = self.data;
540        let mut inv = [[T::zero(); N]; N];
541        for i in 0..N {
542            inv[i][i] = T::one();
543        }
544        for k in 0..N {
545            let mut piv = k;
546            let mut mx = a[k][k].abs();
547            for i in (k + 1)..N {
548                let v = a[i][k].abs();
549                if v > mx {
550                    mx = v;
551                    piv = i;
552                }
553            }
554            if mx == T::zero() || mx < eps {
555                return None;
556            }
557            if piv != k {
558                let tmp = a[k];
559                a[k] = a[piv];
560                a[piv] = tmp;
561                let tmp2 = inv[k];
562                inv[k] = inv[piv];
563                inv[piv] = tmp2;
564            }
565            let ak = a[k][k];
566            for j in 0..N {
567                a[k][j] = a[k][j] / ak;
568                inv[k][j] = inv[k][j] / ak;
569            }
570            for i in 0..N {
571                if i != k {
572                    let f = a[i][k];
573                    if f != T::zero() {
574                        for j in 0..N {
575                            a[i][j] = a[i][j] - f * a[k][j];
576                            inv[i][j] = inv[i][j] - f * inv[k][j];
577                        }
578                    }
579                }
580            }
581        }
582        Some(Matrix { data: inv })
583    }
584}
585
586// ===========================================================================
587// Common aliases
588// ===========================================================================
589
590/// 2-D vector.
591pub type Vector2<T> = Vector<T, 2>;
592/// 3-D vector.
593pub type Vector3<T> = Vector<T, 3>;
594/// 4-D vector.
595pub type Vector4<T> = Vector<T, 4>;
596/// 6-D vector.
597pub type Vector6<T> = Vector<T, 6>;
598
599/// 2×2 matrix.
600pub type Matrix2<T> = Matrix<T, 2, 2>;
601/// 3×3 matrix.
602pub type Matrix3<T> = Matrix<T, 3, 3>;
603/// 4×4 matrix.
604pub type Matrix4<T> = Matrix<T, 4, 4>;
605/// 3×4 matrix.
606pub type Matrix3x4<T> = Matrix<T, 3, 4>;
607/// 2×3 matrix.
608pub type Matrix2x3<T> = Matrix<T, 2, 3>;
609/// 4×3 matrix.
610pub type Matrix4x3<T> = Matrix<T, 4, 3>;
611
612// ===========================================================================
613// Internal fixed-array helpers (const-generic initialisers)
614// ===========================================================================
615
616fn array_init<T, const N: usize>(f: impl FnMut(usize) -> T) -> [T; N] {
617    core::array::from_fn(f)
618}
619
620fn array2_init<T, const R: usize, const C: usize>(
621    mut f: impl FnMut(usize, usize) -> T,
622) -> [[T; C]; R] {
623    core::array::from_fn(|i| core::array::from_fn(|j| f(i, j)))
624}
625
626#[cfg(test)]
627mod tests {
628    use super::*;
629    use core::iter;
630
631    /// A random-ish but reproducible RNG.
632    fn rng() -> impl Iterator<Item = f64> {
633        let mut state: u64 = 0x9e3779b97f4a7c15;
634        iter::from_fn(move || {
635            state ^= state >> 30;
636            state = state.wrapping_mul(0xbf58476d1ce4e5b9);
637            state ^= state >> 27;
638            state = state.wrapping_mul(0x94d049bb133111eb);
639            state ^= state >> 31;
640            Some(((state >> 11) as f64) / ((1u64 << 53) as f64))
641        })
642    }
643
644    fn close(a: f64, b: f64, tol: f64) -> bool {
645        (a - b).abs() < tol
646    }
647
648    #[test]
649    fn vector_arithmetic() {
650        let a = Vector3::new([1.0_f64, 2.0, 3.0]);
651        let b = Vector3::new([4.0_f64, 5.0, 6.0]);
652        assert_eq!((a + b).data, [5.0, 7.0, 9.0]);
653        assert_eq!((a - b).data, [-3.0, -3.0, -3.0]);
654        assert_eq!((a * 2.0).data, [2.0, 4.0, 6.0]);
655        assert_eq!((a / 2.0).data, [0.5, 1.0, 1.5]);
656        assert_eq!((a * b).data, [4.0, 10.0, 18.0]);
657        assert_eq!(-a, Vector3::new([-1.0, -2.0, -3.0]));
658    }
659
660    #[test]
661    fn vector_dot_cross() {
662        let a = Vector3::new([1.0_f64, 0.0, 0.0]);
663        let b = Vector3::new([0.0_f64, 1.0, 0.0]);
664        assert_eq!(a.cross(&b), Vector3::new([0.0, 0.0, 1.0]));
665        let u = Vector2::new([1.0_f64, 0.0]);
666        let v = Vector2::new([0.0_f64, 1.0]);
667        assert_eq!(u.perp_dot(&v), 1.0);
668        assert!(close(a.dot(&b), 0.0, 1e-12));
669    }
670
671    #[test]
672    fn vector_norm() {
673        let a = Vector3::new([3.0_f64, 4.0, 0.0]);
674        assert!(close(a.norm(), 5.0, 1e-12));
675        let n = a.normalize();
676        assert!(close(n.norm(), 1.0, 1e-12));
677    }
678
679    #[test]
680    fn matrix_vector_product() {
681        let m = Matrix2::new([[1.0_f64, 2.0], [3.0, 4.0]]);
682        let v = Vector2::new([1.0_f64, 1.0]);
683        assert_eq!((m * v).data, [3.0, 7.0]);
684    }
685
686    #[test]
687    fn matrix_matrix_product() {
688        let m = Matrix2::new([[1.0_f64, 2.0], [3.0, 4.0]]);
689        let n = Matrix2::new([[0.0_f64, 1.0], [1.0, 0.0]]);
690        let p = m * n;
691        assert_eq!(p.data, [[2.0, 1.0], [4.0, 3.0]]);
692    }
693
694    #[test]
695    fn transpose() {
696        let m = Matrix2x3::new([[1.0_f64, 2.0, 3.0], [4.0, 5.0, 6.0]]);
697        let t = m.transpose();
698        assert_eq!(t.data, [[1.0, 4.0], [2.0, 5.0], [3.0, 6.0]]);
699    }
700
701    #[test]
702    fn identity_and_inverse_2x2() {
703        let m = Matrix2::new([[2.0_f64, 0.0], [0.0, 3.0]]);
704        let inv = m.inverse().unwrap();
705        assert_eq!(inv.data, [[0.5, 0.0], [0.0, 1.0 / 3.0]]);
706        let prod = m * inv;
707        assert!(close(prod.data[0][0], 1.0, 1e-12));
708        assert!(close(prod.data[1][1], 1.0, 1e-12));
709        assert!(close(prod.data[0][1], 0.0, 1e-12));
710    }
711
712    #[test]
713    fn determinant_2x2() {
714        let m = Matrix2::new([[2.0_f64, 1.0], [1.0, 3.0]]);
715        assert!(close(m.determinant(), 5.0, 1e-12));
716    }
717
718    #[test]
719    fn random_invertible_2x2() {
720        let mut r = rng();
721        for _ in 0..50 {
722            let (a, b, c, d) = (
723                r.next().unwrap(),
724                r.next().unwrap(),
725                r.next().unwrap(),
726                r.next().unwrap(),
727            );
728            let m = Matrix2::new([[a, b], [c, d]]) + Matrix2::identity();
729            let inv = m.inverse().unwrap();
730            let prod = m * inv;
731            for i in 0..2 {
732                for j in 0..2 {
733                    let want = if i == j { 1.0 } else { 0.0 };
734                    assert!(
735                        close(prod.data[i][j], want, 1e-9),
736                        "entry {i},{j} = {}",
737                        prod.data[i][j]
738                    );
739                }
740            }
741            assert!(close(m.determinant() * inv.determinant(), 1.0, 1e-8));
742        }
743    }
744
745    #[test]
746    fn random_invertible_3x3() {
747        let mut r = rng();
748        for _ in 0..30 {
749            let mut m = Matrix3::<f64>::identity();
750            for i in 0..3 {
751                for j in 0..3 {
752                    m.data[i][j] = r.next().unwrap();
753                }
754            }
755            m = m + Matrix3::identity();
756            let inv = m.inverse().unwrap();
757            let prod = m * inv;
758            for i in 0..3 {
759                for j in 0..3 {
760                    let want = if i == j { 1.0 } else { 0.0 };
761                    assert!(close(prod.data[i][j], want, 1e-8), "entry {i},{j}");
762                }
763            }
764        }
765    }
766
767    #[test]
768    fn random_invertible_4x4() {
769        let mut r = rng();
770        for _ in 0..20 {
771            let mut m = Matrix4::<f64>::identity();
772            for i in 0..4 {
773                for j in 0..4 {
774                    m.data[i][j] = if i == j {
775                        1.0 + r.next().unwrap()
776                    } else {
777                        r.next().unwrap()
778                    };
779                }
780            }
781            let inv = m.inverse().unwrap();
782            let prod = m * inv;
783            for i in 0..4 {
784                for j in 0..4 {
785                    let want = if i == j { 1.0 } else { 0.0 };
786                    assert!(close(prod.data[i][j], want, 1e-7), "entry {i},{j}");
787                }
788            }
789        }
790    }
791
792    #[test]
793    fn singular_is_none() {
794        let m = Matrix2::new([[1.0_f64, 2.0], [2.0, 4.0]]);
795        assert!(m.inverse().is_none());
796        assert!(close(m.determinant(), 0.0, 1e-12));
797    }
798
799    #[test]
800    fn from_columns_and_array() {
801        let cols = [Vector2::new([1.0_f64, 2.0]), Vector2::new([3.0, 4.0])];
802        let m = Matrix2::from_columns(&cols).unwrap();
803        assert_eq!(m.data, [[1.0, 3.0], [2.0, 4.0]]);
804        let a = Matrix2::from_array(&[1.0_f64, 2.0, 3.0, 4.0]);
805        assert_eq!(a.data, [[1.0, 2.0], [3.0, 4.0]]);
806        assert!(Matrix2::from_columns(&cols[0..1]).is_none());
807    }
808
809    #[test]
810    fn from_columns_non_square() {
811        // A 3x2 matrix: two columns, each of length 3. Used to index out of
812        // bounds because the column count and length were swapped.
813        let cols = [
814            Vector::<f64, 3>::new([1.0, 2.0, 3.0]),
815            Vector::<f64, 3>::new([4.0, 5.0, 6.0]),
816        ];
817        let m = Matrix::<f64, 3, 2>::from_columns(&cols).unwrap();
818        assert_eq!(m.data, [[1.0, 4.0], [2.0, 5.0], [3.0, 6.0]]);
819        assert!(Matrix::<f64, 3, 2>::from_columns(&cols[0..1]).is_none());
820    }
821}
822
823#[cfg(test)]
824mod inverse_threshold_tests {
825    use super::*;
826
827    #[test]
828    fn relative_threshold_inverts_small_scale_matrix_that_default_rejects() {
829        let diag = [1e-13, 2e-13];
830        let m = Matrix::<f64, 2, 2>::from_fn(|r, c| if r == c { diag[r] } else { 0.0 });
831        assert!(m.inverse().is_none());
832        let inv = m.inverse_with_relative_tolerance(1e-10).unwrap();
833        assert!((inv.data[0][0] - 1e13).abs() < 1e1);
834        assert!((inv.data[1][1] - 5e12).abs() < 1e1);
835    }
836
837    #[test]
838    fn zero_matrix_is_rejected_with_zero_tolerance() {
839        let z = Matrix::<f64, 3, 3>::from_fn(|_, _| 0.0);
840        assert!(z.inverse_with_tolerance(0.0).is_none());
841    }
842}