retrofire_core/math/

mat.rs

#![allow(clippy::needless_range_loop)]

//! Matrices and linear transforms.

use core::array;
use core::fmt::{self, Debug, Formatter};
use core::marker::PhantomData;
use core::ops::Range;

use crate::render::{NdcToScreen, ViewToProj};

use super::space::{Linear, Proj4, Real};
use super::vec::{ProjVec4, Vec2, Vec2u, Vec3, Vector};

/// A linear transform from one space (or basis) to another.
///
/// This is a tag trait with no functionality in itself. It is used to
/// statically ensure that only compatible maps can be composed, and that
/// only compatible vectors can be transformed.
pub trait LinearMap {
    /// The source space, or domain, of `Self`.
    type Source;
    /// The destination space, or range, of `Self`.
    type Dest;
}

/// Composition of two `LinearMap`s, `Self` ∘ `Inner`.
/// If `Self` maps from `B` to `C`, and `Inner` maps from `A` to `B`,
/// `Self::Result` maps from `A` to `C`.
pub trait Compose<Inner: LinearMap>: LinearMap<Source = Inner::Dest> {
    /// The result of composing `Self` with `Inner`.
    type Result: LinearMap<Source = Inner::Source, Dest = Self::Dest>;
}

/// A change of basis in real vector space of dimension `DIM`.
#[derive(Copy, Clone, Default, Eq, PartialEq)]
pub struct RealToReal<const DIM: usize, SrcBasis = (), DstBasis = ()>(
    PhantomData<(SrcBasis, DstBasis)>,
);

/// Mapping from real to projective space.
#[derive(Copy, Clone, Debug, Default, Eq, PartialEq)]
pub struct RealToProj<SrcBasis>(PhantomData<SrcBasis>);

/// A generic matrix type.
#[repr(transparent)]
#[derive(Copy, Eq, PartialEq)]
pub struct Matrix<Repr, Map>(pub Repr, PhantomData<Map>);

/// Type alias for a 3x3 float matrix.
pub type Mat3x3<Map = ()> = Matrix<[[f32; 3]; 3], Map>;
/// Type alias for a 4x4 float matrix.
pub type Mat4x4<Map = ()> = Matrix<[[f32; 4]; 4], Map>;

//
// Inherent impls
//

impl<Repr, Map> Matrix<Repr, Map> {
    /// Returns a matrix with the given elements.
    #[inline]
    pub const fn new(els: Repr) -> Self {
        Self(els, PhantomData)
    }

    /// Returns a matrix equal to `self` but with mapping `M`.
    ///
    /// This method can be used to coerce a matrix to a different
    /// mapping in case it is needed to make types match.
    #[inline]
    pub fn to<M>(&self) -> Matrix<Repr, M>
    where
        Repr: Clone,
    {
        Matrix::new(self.0.clone())
    }
}

impl<Sc, const N: usize, const M: usize, Map> Matrix<[[Sc; N]; M], Map>
where
    Sc: Copy,
    Map: LinearMap,
{
    /// Returns the row vector of `self` with index `i`.
    /// The returned vector is in space `Map::Source`.
    ///
    /// # Panics
    /// If `i >= M`.
    #[inline]
    pub fn row_vec(&self, i: usize) -> Vector<[Sc; N], Map::Source> {
        Vector::new(self.0[i])
    }
    /// Returns the column vector of `self` with index `i`.
    ///
    /// The returned vector is in space `Map::Dest`.
    ///
    /// # Panics
    /// If `i >= N`.
    #[inline]
    pub fn col_vec(&self, i: usize) -> Vector<[Sc; M], Map::Dest> {
        Vector::new(self.0.map(|row| row[i]))
    }
}
impl<Sc: Copy, const N: usize, const DIM: usize, S, D>
    Matrix<[[Sc; N]; N], RealToReal<DIM, S, D>>
{
    /// Returns `self` with its rows and columns swapped.
    pub fn transpose(self) -> Matrix<[[Sc; N]; N], RealToReal<DIM, D, S>> {
        const { assert!(N >= DIM, "map dimension >= matrix dimension") }
        array::from_fn(|j| array::from_fn(|i| self.0[i][j])).into()
    }
}

impl<const N: usize, Map> Matrix<[[f32; N]; N], Map> {
    /// Returns the `N`×`N` identity matrix.
    pub const fn identity() -> Self {
        let mut els = [[0.0; N]; N];
        let mut i = 0;
        while i < N {
            els[i][i] = 1.0;
            i += 1;
        }
        Self::new(els)
    }

    /// Returns whether every element of `self` is finite
    /// (ie. neither `Inf`, `-Inf`, or `NaN`).
    fn is_finite(&self) -> bool {
        self.0.iter().flatten().all(|e| e.is_finite())
    }
}

impl<M: LinearMap> Mat4x4<M> {
    /// Constructs a matrix from a set of basis vectors.
    pub const fn from_basis(i: Vec3, j: Vec3, k: Vec3) -> Self {
        Self::new([
            [i.0[0], i.0[1], i.0[2], 0.0],
            [j.0[0], j.0[1], j.0[2], 0.0],
            [k.0[0], k.0[1], k.0[2], 0.0],
            [0.0, 0.0, 0.0, 1.0],
        ])
    }
}

impl<Sc, const N: usize, Map> Matrix<[[Sc; N]; N], Map>
where
    Sc: Linear<Scalar = Sc> + Copy,
    Map: LinearMap,
{
    /// Returns the composite transform of `self` and `other`.
    ///
    /// Computes the matrix product of `self` and `other` such that applying
    /// the resulting transformation is equivalent to first applying `other`
    /// and then `self`. More succinctly,
    /// ```text
    /// (𝗠 ∘ 𝗡)𝘃 = 𝗠(𝗡𝘃)
    /// ```
    /// for some matrices 𝗠 and 𝗡 and a vector 𝘃.
    #[must_use]
    pub fn compose<Inner: LinearMap>(
        &self,
        other: &Matrix<[[Sc; N]; N], Inner>,
    ) -> Matrix<[[Sc; N]; N], <Map as Compose<Inner>>::Result>
    where
        Map: Compose<Inner>,
    {
        let cols: [_; N] = array::from_fn(|i| other.col_vec(i));
        array::from_fn(|j| {
            let row = self.row_vec(j);
            array::from_fn(|i| row.dot(&cols[i]))
        })
        .into()
    }
    /// Returns the composite transform of `other` and `self`.
    ///
    /// Computes the matrix product of `self` and `other` such that applying
    /// the resulting matrix is equivalent to first applying `self` and then
    /// `other`. The call `self.then(other)` is thus equivalent to
    /// `other.compose(self)`.
    #[must_use]
    pub fn then<Outer: Compose<Map>>(
        &self,
        other: &Matrix<[[Sc; N]; N], Outer>,
    ) -> Matrix<[[Sc; N]; N], <Outer as Compose<Map>>::Result> {
        other.compose(self)
    }
}

impl<Src, Dst> Mat3x3<RealToReal<2, Src, Dst>> {
    /// Maps the real 2-vector 𝘃 from basis `Src` to basis `Dst`.
    ///
    /// Computes the matrix–vector multiplication 𝝡𝘃 where 𝘃 is interpreted as
    /// a column vector with an implicit 𝘃<sub>2</sub> component with value 1:
    ///
    /// ```text
    ///         / M00 ·  ·  \ / v0 \
    ///  Mv  =  |  ·  ·  ·  | | v1 |  =  ( v0' v1' 1 )
    ///         \  ·  · M22 / \  1 /
    /// ```
    #[must_use]
    pub fn apply(&self, v: &Vec2<Src>) -> Vec2<Dst> {
        let v = [v.x(), v.y(), 1.0].into();
        array::from_fn(|i| self.row_vec(i).dot(&v)).into()
    }
}

impl<Src, Dst> Mat4x4<RealToReal<3, Src, Dst>> {
    /// Maps the real 3-vector 𝘃 from basis `Src` to basis `Dst`.
    ///
    /// Computes the matrix–vector multiplication 𝝡𝘃 where 𝘃 is interpreted as
    /// a column vector with an implicit 𝘃<sub>3</sub> component with value 1:
    ///
    /// ```text
    ///         / M00 ·  ·  ·  \ / v0 \
    ///  Mv  =  |  ·  ·  ·  ·  | | v1 |  =  ( v0' v1' v2' 1 )
    ///         |  ·  ·  ·  ·  | | v2 |
    ///         \  ·  ·  · M33 / \  1 /
    /// ```
    #[must_use]
    pub fn apply(&self, v: &Vec3<Src>) -> Vec3<Dst> {
        let v = [v.x(), v.y(), v.z(), 1.0].into();
        array::from_fn(|i| self.row_vec(i).dot(&v)).into()
    }

    /// Returns the determinant of `self`.
    ///
    /// Given a matrix M,
    /// ```text
    ///         / a  b  c  d \
    ///  M  =   | e  f  g  h |
    ///         | i  j  k  l |
    ///         \ m  n  o  p /
    /// ```
    /// its determinant can be computed by recursively computing
    /// the determinants of sub-matrices on rows 1.. and multiplying
    /// them by the elements on row 0:
    /// ```text
    ///              | f g h |       | e g h |
    /// det(M) = a · | j k l | - b · | i k l |  + - ···
    ///              | n o p |       | m o p |
    /// ```
    pub fn determinant(&self) -> f32 {
        let [[a, b, c, d], r, s, t] = self.0;
        let det2 = |m, n| s[m] * t[n] - s[n] * t[m];
        let det3 =
            |j, k, l| r[j] * det2(k, l) - r[k] * det2(j, l) + r[l] * det2(j, k);

        a * det3(1, 2, 3) - b * det3(0, 2, 3) + c * det3(0, 1, 3)
            - d * det3(0, 1, 2)
    }

    /// Returns the inverse matrix of `self`.
    ///
    /// The inverse 𝝡<sup>-1</sup> of matrix 𝝡 is a matrix that, when
    /// composed with 𝝡, results in the identity matrix:
    ///
    /// 𝝡 ∘ 𝝡<sup>-1</sup> = 𝝡<sup>-1</sup> ∘ 𝝡 = 𝐈
    ///
    /// In other words, it applies the transform of 𝝡 in reverse.
    /// Given vectors 𝘃 and 𝘂,
    ///
    /// 𝝡𝘃 = 𝘂 ⇔ 𝝡<sup>-1</sup> 𝘂 = 𝘃.
    ///
    /// Only matrices with a nonzero determinant have a defined inverse.
    /// A matrix without an inverse is said to be singular.
    ///
    /// Note: This method uses naive Gauss–Jordan elimination and may
    /// suffer from imprecision or numerical instability in certain cases.
    ///
    /// # Panics
    /// If `self` is singular or near-singular:
    /// * Panics in debug mode.
    /// * Does not panic in release mode, but the result may be inaccurate
    /// or contain `Inf`s or `NaN`s.
    #[must_use]
    pub fn inverse(&self) -> Mat4x4<RealToReal<3, Dst, Src>> {
        use super::float::f32;
        if cfg!(debug_assertions) {
            let det = self.determinant();
            assert!(
                f32::abs(det) > f32::EPSILON,
                "a singular, near-singular, or non-finite matrix does not \
                 have a well-defined inverse (determinant = {det})"
            );
        }

        // Elementary row operation subtracting one row,
        // multiplied by a scalar, from another
        fn sub_row(m: &mut Mat4x4, from: usize, to: usize, mul: f32) {
            m.0[to] = (m.row_vec(to) - m.row_vec(from) * mul).0;
        }

        // Elementary row operation multiplying one row with a scalar
        fn mul_row(m: &mut Mat4x4, row: usize, mul: f32) {
            m.0[row] = (m.row_vec(row) * mul).0;
        }

        // Elementary row operation swapping two rows
        fn swap_rows(m: &mut Mat4x4, r: usize, s: usize) {
            m.0.swap(r, s);
        }

        // This algorithm attempts to reduce `this` to the identity matrix
        // by simultaneously applying elementary row operations to it and
        // another matrix `inv` which starts as the identity matrix. Once
        // `this` is reduced, the value of `inv` has become the inverse of
        // `this` and thus of `self`.

        let inv = &mut Mat4x4::identity();
        let this = &mut self.to();

        // Apply row operations to reduce the matrix to an upper echelon form
        for idx in 0..4 {
            let pivot = (idx..4)
                .max_by(|&r1, &r2| {
                    let v1 = f32::abs(this.0[r1][idx]);
                    let v2 = f32::abs(this.0[r2][idx]);
                    v1.partial_cmp(&v2).unwrap()
                })
                .unwrap();

            if this.0[pivot][idx] != 0.0 {
                swap_rows(this, idx, pivot);
                swap_rows(inv, idx, pivot);

                let div = 1.0 / this.0[idx][idx];
                for r in (idx + 1)..4 {
                    let x = this.0[r][idx] * div;
                    sub_row(this, idx, r, x);
                    sub_row(inv, idx, r, x);
                }
            }
        }
        // now in upper echelon form, back-substitute variables
        for &idx in &[3, 2, 1] {
            let diag = this.0[idx][idx];
            for r in 0..idx {
                let x = this.0[r][idx] / diag;

                sub_row(this, idx, r, x);
                sub_row(inv, idx, r, x);
            }
        }
        // normalize
        for r in 0..4 {
            let x = 1.0 / this.0[r][r];
            mul_row(this, r, x);
            mul_row(inv, r, x);
        }
        debug_assert!(inv.is_finite());
        inv.to()
    }
}

impl<Src> Mat4x4<RealToProj<Src>> {
    /// Maps the real 3-vector 𝘃 from basis 𝖡 to the projective 4-space.
    ///
    /// Computes the matrix–vector multiplication 𝝡𝘃 where 𝘃 is interpreted as
    /// a column vector with an implicit 𝘃<sub>3</sub> component with value 1:
    ///
    /// ```text
    ///         / M00  ·  · \ / v0 \
    ///  Mv  =  |    ·      | | v1 |  =  ( v0' v1' v2' v3' )
    ///         |      ·    | | v2 |
    ///         \ ·  ·  M33 / \  1 /
    /// ```
    #[must_use]
    pub fn apply(&self, v: &Vec3<Src>) -> ProjVec4 {
        let v = Vector::from([v.x(), v.y(), v.z(), 1.0]);
        [
            self.row_vec(0).dot(&v),
            self.row_vec(1).dot(&v),
            self.row_vec(2).dot(&v),
            self.row_vec(3).dot(&v),
        ]
        .into()
    }
}

//
// Local trait impls
//

impl<const DIM: usize, S, D> LinearMap for RealToReal<DIM, S, D> {
    type Source = Real<DIM, S>;
    type Dest = Real<DIM, D>;
}

impl<const DIM: usize, S, I, D> Compose<RealToReal<DIM, S, I>>
    for RealToReal<DIM, I, D>
{
    type Result = RealToReal<DIM, S, D>;
}

impl<S> LinearMap for RealToProj<S> {
    type Source = Real<3, S>;
    type Dest = Proj4;
}

impl<S, I> Compose<RealToReal<3, S, I>> for RealToProj<I> {
    type Result = RealToProj<S>;
}

/// Dummy `LinearMap` to help with generic code.
impl LinearMap for () {
    type Source = ();
    type Dest = ();
}

//
// Foreign trait impls
//

impl<R: Clone, M> Clone for Matrix<R, M> {
    fn clone(&self) -> Self {
        self.to()
    }
}

impl<const N: usize, Map> Default for Matrix<[[f32; N]; N], Map> {
    /// Returns the `N`×`N` identity matrix.
    fn default() -> Self {
        Self::identity()
    }
}

impl<S: Debug, Map: Debug + Default, const N: usize, const M: usize> Debug
    for Matrix<[[S; N]; M], Map>
{
    fn fmt(&self, f: &mut Formatter<'_>) -> fmt::Result {
        writeln!(f, "Matrix<{:?}>[", Map::default())?;
        for i in 0..M {
            writeln!(f, "    {:6.2?}", self.0[i])?;
        }
        write!(f, "]")
    }
}

impl<const DIM: usize, F, T> Debug for RealToReal<DIM, F, T>
where
    F: Debug + Default,
    T: Debug + Default,
{
    fn fmt(&self, f: &mut Formatter<'_>) -> fmt::Result {
        write!(f, "{:?}→{:?}", F::default(), T::default())
    }
}

impl<Repr, M> From<Repr> for Matrix<Repr, M> {
    fn from(repr: Repr) -> Self {
        Self(repr, PhantomData)
    }
}

//
// Free functions
//

/// Returns a matrix applying a scaling by `s`.
///
/// Tip: use [`splat`][super::vec::splat] to scale uniformly:
/// ```
/// # use retrofire_core::math::{mat::scale, vec::splat};
/// let m = scale(splat(2.0));
/// assert_eq!(m.0[0][0], 2.0);
/// assert_eq!(m.0[1][1], 2.0);
/// assert_eq!(m.0[2][2], 2.0);
/// ```
pub const fn scale(s: Vec3) -> Mat4x4<RealToReal<3>> {
    let [x, y, z] = s.0;
    Matrix::new([
        [x, 0.0, 0.0, 0.0],
        [0.0, y, 0.0, 0.0],
        [0.0, 0.0, z, 0.0],
        [0.0, 0.0, 0.0, 1.0],
    ])
}

/// Returns a matrix applying a translation by `t`.
pub const fn translate(t: Vec3) -> Mat4x4<RealToReal<3>> {
    let [x, y, z] = t.0;
    Matrix::new([
        [1.0, 0.0, 0.0, x],
        [0.0, 1.0, 0.0, y],
        [0.0, 0.0, 1.0, z],
        [0.0, 0.0, 0.0, 1.0],
    ])
}

/// Returns a matrix applying a rotation such that the original y axis
/// is now parallel with `new_y` and the new z axis is orthogonal to
/// both `x` and `new_y`.
///
/// Returns an orthogonal basis. If `new_y` and `x` are unit vectors,
/// the result is orthonormal.
#[cfg(feature = "fp")]
pub fn orient_y(new_y: Vec3, x: Vec3) -> Mat4x4<RealToReal<3>> {
    orient(new_y, x.cross(&new_y).normalize())
}
/// Returns a matrix applying a rotation such that the original z axis
/// is now parallel with `new_z` and the new y axis is orthogonal to
/// both `new_z` and `x`.
///
/// Returns an orthogonal basis. If `new_z` and `x` are unit vectors,
/// the result is orthonormal.
#[cfg(feature = "fp")]
pub fn orient_z(new_z: Vec3, x: Vec3) -> Mat4x4<RealToReal<3>> {
    orient(new_z.cross(&x).normalize(), new_z)
}

#[cfg(feature = "fp")]
fn orient(new_y: Vec3, new_z: Vec3) -> Mat4x4<RealToReal<3>> {
    use crate::math::{space::Linear, ApproxEq};

    assert!(!new_y.approx_eq(&Vec3::zero()));
    assert!(!new_z.approx_eq(&Vec3::zero()));

    let new_x = new_y.cross(&new_z);
    Mat4x4::from_basis(new_x, new_y, new_z)
}

// TODO constify rotate_* functions once we have const trig functions

/// Returns a matrix applying a rotation by `a` about the x axis.
#[cfg(feature = "fp")]
pub fn rotate_x(a: super::angle::Angle) -> Mat4x4<RealToReal<3>> {
    let (sin, cos) = a.sin_cos();
    [
        [1.0, 0.0, 0.0, 0.0],
        [0.0, cos, sin, 0.0],
        [0.0, -sin, cos, 0.0],
        [0.0, 0.0, 0.0, 1.0],
    ]
    .into()
}
/// Returns a matrix applying a rotation by `a` about the y axis.
#[cfg(feature = "fp")]
pub fn rotate_y(a: super::angle::Angle) -> Mat4x4<RealToReal<3>> {
    let (sin, cos) = a.sin_cos();
    [
        [cos, 0.0, -sin, 0.0],
        [0.0, 1.0, 0.0, 0.0],
        [sin, 0.0, cos, 0.0],
        [0.0, 0.0, 0.0, 1.0],
    ]
    .into()
}
/// Returns a matrix applying a rotation of angle `a` about the z axis.
#[cfg(feature = "fp")]
pub fn rotate_z(a: super::angle::Angle) -> Mat4x4<RealToReal<3>> {
    let (sin, cos) = a.sin_cos();
    [
        [cos, sin, 0.0, 0.0],
        [-sin, cos, 0.0, 0.0],
        [0.0, 0.0, 1.0, 0.0],
        [0.0, 0.0, 0.0, 1.0],
    ]
    .into()
}

/// Creates a perspective projection matrix.
///
/// # Parameters
/// * `focal_ratio`: Focal length/aperture ratio. Larger values mean
/// a smaller angle of view, with 1.0 corresponding to a horizontal
/// field of view of 90 degrees.
/// * `aspect_ratio`: Viewport width/height ratio. Larger values mean
/// a wider field of view.
/// * `near_far`: Depth range between the near and far clipping planes.
/// Objects outside this range are clipped or culled.
///
/// # Panics
/// * If any parameter value is nonpositive.
/// * If `near_far` is an empty range.
pub fn perspective(
    focal_ratio: f32,
    aspect_ratio: f32,
    near_far: Range<f32>,
) -> Mat4x4<ViewToProj> {
    let (near, far) = (near_far.start, near_far.end);

    assert!(focal_ratio > 0.0, "focal ratio must be positive");
    assert!(aspect_ratio > 0.0, "aspect ratio must be positive");
    assert!(near > 0.0, "near must be positive");
    assert!(!near_far.is_empty(), "far must be greater than near");

    let e00 = focal_ratio;
    let e11 = e00 * aspect_ratio;
    let e22 = (far + near) / (far - near);
    let e23 = 2.0 * far * near / (near - far);
    [
        [e00, 0.0, 0.0, 0.0],
        [0.0, e11, 0.0, 0.0],
        [0.0, 0.0, e22, e23],
        [0.0, 0.0, 1.0, 0.0],
    ]
    .into()
}

/// Creates an orthographic projection matrix.
///
/// # Parameters
/// * `lbn`: The left-bottom-near corner of the projection box.
/// * `rtf`: The right-bottom-far corner of the projection box.
pub fn orthographic(lbn: Vec3, rtf: Vec3) -> Mat4x4<ViewToProj> {
    let [dx, dy, dz] = (rtf - lbn).0;
    let [sx, sy, sz] = (rtf + lbn).0;
    [
        [2.0 / dx, 0.0, 0.0, -sx / dx],
        [0.0, 2.0 / dy, 0.0, -sy / dy],
        [0.0, 0.0, 2.0 / dz, -sz / dz],
        [0.0, 0.0, 0.0, 1.0],
    ]
    .into()
}

/// Creates a viewport transform matrix. A viewport matrix is used to
/// transform points from the NDC space to screen space for rasterization.
///
/// # Parameters
/// * `bounds`: the left-top and right-bottom coordinates of the viewport.
pub fn viewport(bounds: Range<Vec2u>) -> Mat4x4<NdcToScreen> {
    let Range { start, end } = bounds;
    let h = (end.x() - start.x()) as f32 / 2.0;
    let v = (end.y() - start.y()) as f32 / 2.0;
    [
        [h, 0.0, 0.0, h + start.x() as f32],
        [0.0, v, 0.0, v + start.y() as f32],
        [0.0, 0.0, 1.0, 0.0],
        [0.0, 0.0, 0.0, 1.0],
    ]
    .into()
}

#[cfg(test)]
mod tests {
    use crate::assert_approx_eq;
    use crate::math::vec::{splat, vec2, vec3};

    #[cfg(feature = "fp")]
    use crate::math::angle::degs;

    use super::*;

    #[derive(Debug, Default, Eq, PartialEq)]
    struct Basis1;
    #[derive(Debug, Default, Eq, PartialEq)]
    struct Basis2;

    type Map<const N: usize = 3> = RealToReal<N, Basis1, Basis2>;
    type InvMap<const N: usize = 3> = RealToReal<N, Basis2, Basis1>;

    mod mat3x3 {
        use super::*;

        const MAT: Mat3x3<Map> = Matrix::new([
            [0.0, 1.0, 2.0], //
            [10.0, 11.0, 12.0],
            [20.0, 21.0, 22.0],
        ]);

        #[test]
        fn row_col_vecs() {
            assert_eq!(MAT.row_vec(2), vec3::<_, Basis1>(20.0, 21.0, 22.0));
            assert_eq!(MAT.col_vec(2), vec3::<_, Basis2>(2.0, 12.0, 22.0));
        }

        #[test]
        fn composition() {
            let t = Mat3x3::<Map<2>>::new([
                [1.0, 0.0, 2.0], //
                [0.0, 1.0, -3.0],
                [0.0, 0.0, 1.0],
            ]);
            let s = Mat3x3::<InvMap<2>>::new([
                [-1.0, 0.0, 0.0], //
                [0.0, 2.0, 0.0],
                [0.0, 0.0, 1.0],
            ]);

            let ts = t.then(&s);
            let st = s.then(&t);

            assert_eq!(ts, s.compose(&t));
            assert_eq!(st, t.compose(&s));

            assert_eq!(ts.apply(&splat(0.0)), vec2(-2.0, -6.0));
            assert_eq!(st.apply(&splat(0.0)), vec2(2.0, -3.0));
        }

        #[test]
        fn scaling() {
            let m = Mat3x3::<Map<2>>::new([
                [2.0, 0.0, 0.0], //
                [0.0, -3.0, 0.0],
                [0.0, 0.0, 1.0],
            ]);
            assert_eq!(m.apply(&vec2(1.0, 2.0)), vec2(2.0, -6.0));
        }

        #[test]
        fn translation() {
            let m = Mat3x3::<Map<2>>::new([
                [1.0, 0.0, 2.0], //
                [0.0, 1.0, -3.0],
                [0.0, 0.0, 1.0],
            ]);
            assert_eq!(m.apply(&vec2(1.0, 2.0)), vec2(3.0, -1.0));
        }

        #[test]
        fn matrix_debug() {
            assert_eq!(
                alloc::format!("{MAT:?}"),
                r#"Matrix<Basis1→Basis2>[
    [  0.00,   1.00,   2.00]
    [ 10.00,  11.00,  12.00]
    [ 20.00,  21.00,  22.00]
]"#
            );
        }
    }

    mod mat4x4 {
        use super::*;

        const MAT: Mat4x4<Map> = Matrix::new([
            [0.0, 1.0, 2.0, 3.0],
            [10.0, 11.0, 12.0, 13.0],
            [20.0, 21.0, 22.0, 23.0],
            [30.0, 31.0, 32.0, 33.0],
        ]);

        #[test]
        fn row_col_vecs() {
            assert_eq!(MAT.row_vec(1), [10.0, 11.0, 12.0, 13.0].into());
            assert_eq!(MAT.col_vec(3), [3.0, 13.0, 23.0, 33.0].into());
        }

        #[test]
        fn composition() {
            let t = translate(vec3(1.0, 2.0, 3.0)).to::<Map>();
            let s = scale(vec3(3.0, 2.0, 1.0)).to::<InvMap>();

            let ts = t.then(&s);
            let st = s.then(&t);

            assert_eq!(ts, s.compose(&t));
            assert_eq!(st, t.compose(&s));

            assert_eq!(ts.apply(&splat(0.0)), vec3::<_, Basis1>(3.0, 4.0, 3.0));
            assert_eq!(st.apply(&splat(0.0)), vec3::<_, Basis2>(1.0, 2.0, 3.0));
        }

        #[test]
        fn scaling_vec3() {
            let m = scale(vec3(1.0, -2.0, 3.0));
            let v = vec3(0.0, 4.0, -3.0);
            assert_eq!(m.apply(&v), vec3(0.0, -8.0, -9.0));
        }

        #[test]
        fn translation_vec3() {
            let m = translate(vec3(1.0, 2.0, 3.0));
            let v = vec3(0.0, 5.0, -3.0);
            assert_eq!(m.apply(&v), vec3(1.0, 7.0, 0.0));
        }

        #[cfg(feature = "fp")]
        #[test]
        fn rotation_x() {
            let m = rotate_x(degs(90.0));
            assert_eq!(m.apply(&splat(0.0)), splat(0.0));
            assert_approx_eq!(
                m.apply(&vec3(0.0, 0.0, 1.0)),
                vec3(0.0, 1.0, 0.0)
            );
        }

        #[cfg(feature = "fp")]
        #[test]
        fn rotation_y() {
            let m = rotate_y(degs(90.0));
            assert_eq!(m.apply(&splat(0.0)), splat(0.0));
            assert_approx_eq!(
                m.apply(&vec3(1.0, 0.0, 0.0)),
                vec3(0.0, 0.0, 1.0)
            );
        }

        #[cfg(feature = "fp")]
        #[test]
        fn rotation_z() {
            let m = rotate_z(degs(90.0));
            assert_eq!(m.apply(&splat(0.0)), splat(0.0));
            assert_approx_eq!(
                m.apply(&vec3(0.0, 1.0, 0.0)),
                vec3(1.0, 0.0, 0.0)
            );
        }

        #[test]
        fn matrix_debug() {
            assert_eq!(
                alloc::format!("{MAT:?}"),
                r#"Matrix<Basis1→Basis2>[
    [  0.00,   1.00,   2.00,   3.00]
    [ 10.00,  11.00,  12.00,  13.00]
    [ 20.00,  21.00,  22.00,  23.00]
    [ 30.00,  31.00,  32.00,  33.00]
]"#
            );
        }
    }

    #[test]
    fn transposition() {
        let m = Matrix::<_, Map>::new([
            [0.0, 1.0, 2.0], //
            [10.0, 11.0, 12.0],
            [20.0, 21.0, 22.0],
        ]);
        assert_eq!(
            m.transpose(),
            Matrix::<_, InvMap>::new([
                [0.0, 10.0, 20.0], //
                [1.0, 11.0, 21.0],
                [2.0, 12.0, 22.0],
            ])
        );
    }

    #[test]
    fn determinant_of_identity_is_one() {
        let id: Mat4x4<Map> = Mat4x4::identity();
        assert_eq!(id.determinant(), 1.0);
    }

    #[test]
    fn determinant_of_scaling_is_product_of_diagonal() {
        let scale: Mat4x4<_> = scale(vec3(2.0, 3.0, 4.0));
        assert_eq!(scale.determinant(), 24.0);
    }

    #[cfg(feature = "fp")]
    #[test]
    fn determinant_of_rotation_is_one() {
        let rot = rotate_x(degs(73.0)).then(&rotate_y(degs(-106.0)));
        assert_approx_eq!(rot.determinant(), 1.0);
    }

    #[cfg(feature = "fp")]
    #[test]
    fn mat_times_mat_inverse_is_identity() {
        let m = translate(vec3(1.0e3, -2.0e2, 0.0))
            .then(&scale(vec3(0.5, 100.0, 42.0)))
            .to::<Map>();

        let m_inv: Mat4x4<InvMap> = m.inverse();

        assert_eq!(
            m.compose(&m_inv),
            Mat4x4::<RealToReal<3, Basis2, Basis2>>::identity()
        );
        assert_eq!(
            m_inv.compose(&m),
            Mat4x4::<RealToReal<3, Basis1, Basis1>>::identity()
        );
    }

    #[test]
    fn inverse_reverts_transform() {
        let m: Mat4x4<Map> = scale(vec3(1.0, 2.0, 0.5))
            .then(&translate(vec3(-2.0, 3.0, 0.0)))
            .to();
        let m_inv: Mat4x4<InvMap> = m.inverse();

        let v1: Vec3<Basis1> = vec3(1.0, -2.0, 3.0);
        let v2: Vec3<Basis2> = vec3(2.0, 0.0, -2.0);

        assert_eq!(m_inv.apply(&m.apply(&v1)), v1);
        assert_eq!(m.apply(&m_inv.apply(&v2)), v2);
    }

    #[test]
    fn orthographic_box_maps_to_unit_cube() {
        let lbn = vec3(-20.0, 0.0, 0.01);
        let rtf = vec3(100.0, 50.0, 100.0);

        let m = orthographic(lbn, rtf);

        assert_approx_eq!(m.apply(&lbn.to()), [-1.0, -1.0, -1.0, 1.0].into());
        assert_approx_eq!(m.apply(&rtf.to()), splat(1.0));
    }

    #[test]
    fn perspective_frustum_maps_to_unit_cube() {
        let left_bot_near = vec3(-0.125, -0.0625, 0.1);
        let right_top_far = vec3(125.0, 62.5, 100.0);

        let m = perspective(0.8, 2.0, 0.1..100.0);

        let lbn = m.apply(&left_bot_near);
        assert_approx_eq!(lbn / lbn.w(), [-1.0, -1.0, -1.0, 1.0].into());

        let rtf = m.apply(&right_top_far);
        assert_approx_eq!(rtf / rtf.w(), splat(1.0));
    }
}