space-crush/glc/src/matrix.rs

#![allow(dead_code)]

use std::borrow::Borrow;
use std::convert::{AsMut, AsRef};
use std::fmt::{Debug, Formatter, Result as FmtResult};
use std::ops::{Deref, DerefMut, Mul};

#[cfg(feature = "serde")]
use serde::{
    de::{Deserializer, Error, MapAccess, SeqAccess, Visitor},
    ser::{SerializeStruct, Serializer},
    Deserialize, Serialize,
};

use super::vector::{Vector2, Vector3, Vector4};

pub use super::{
    angle::Angle,
    numeric::{Float, Numeric},
};

macro_rules! first_ptr {
    ($this:ident, $first:ident $(, $other:ident)*) => {
        unsafe { $this.$first.as_ptr() }
    };
}

macro_rules! define_mat {
    ($Name:ident, $Vector:ident, $size:tt, $($T:ident => $i:tt => $f:ident),*) => {
        #[repr(C, packed)]
        pub struct $Name<T> {
            $(pub $f: $Vector<T>,)+
        }

        impl<T> $Name<T> {
            #[inline]
            pub fn new($($f: $Vector<T>,)+) -> Self {
                Self { $($f,)+ }
            }

            pub fn as_ptr(&self) -> * const T {
                first_ptr!(self $(,$f)+)
            }
        }

        impl<T> Default for $Name<T>
        where
            T: Numeric
        {
            #[inline]
            fn default() -> Self {
                Self::identity()
            }
        }

        impl<T> Debug for $Name<T>
        where
            T: Debug
        {
            fn fmt(&self, fmt: &mut Formatter<'_>) -> FmtResult {
                fmt.debug_list().entries(self.as_ref().iter()).finish()
            }
        }

        impl<T, S> PartialEq<$Name<S>> for $Name<T>
        where
            $Vector<S>: PartialEq<$Vector<T>>,
        {
            fn eq(&self, other: &$Name<S>) -> bool {
                unsafe { true $(&& other.$f.eq(&self.$f))+ }
            }
        }

        impl<T> Eq for $Name<T>
        where
            T: Eq
            { }

        impl<T> Deref for $Name<T> {
            type Target = [$Vector<T>; $size];

            #[inline]
            fn deref(&self) -> &Self::Target {
                self.as_ref()
            }
        }

        impl<T> DerefMut for $Name<T> {
            #[inline]
            fn deref_mut(&mut self) -> &mut Self::Target {
                self.as_mut()
            }
        }

        impl<T> AsRef<[$Vector<T>; $size]> for $Name<T> {
            #[inline]
            fn as_ref(&self) -> &[$Vector<T>; $size] {
                unsafe {
                    let raw: * const _ = self;
                    let raw = raw as * const [$Vector<T>; $size];

                    &*raw
                }
            }
        }

        impl<T> AsMut<[$Vector<T>; $size]> for $Name<T> {
            #[inline]
            fn as_mut(&mut self) -> &mut [$Vector<T>; $size] {
                unsafe {
                    let raw: * mut _ = self;
                    let raw = raw as * mut [$Vector<T>; $size];

                    &mut *raw
                }
            }
        }

        impl<T> Clone for $Name<T>
        where
            T: Clone
        {
            #[inline]
            fn clone(&self) -> Self {
                unsafe {
                    Self {
                        $($f: self.$f.clone(),)+
                    }
                }
            }
        }

        impl<T> Copy for $Name<T>
        where
            T: Copy
            { }

        impl<T> From<[$Vector<T>; $size]> for $Name<T>
        where
            T: Copy,
        {
            #[inline]
            fn from(arr: [$Vector<T>; $size]) -> Self {
                Self {
                    $($f: arr[$i],)+
                }
            }
        }

        impl<T> From<($($Vector<$T>,)+)> for $Name<T> {
            #[inline]
            fn from(($($f,)+): ($($Vector<$T>,)+)) -> Self {
                Self {
                    $($f,)+
                }
            }
        }

        #[cfg(feature = "serde")]
        impl<T> Serialize for $Name<T>
        where
            T: Serialize,
        {
            fn serialize<S>(&self, serializer: S) -> Result<S::Ok, S::Error>
            where
                S: Serializer
            {
                let mut s = serializer.serialize_struct(stringify!($Name), $size)?;
                unsafe { $(s.serialize_field(stringify!($f), &self.$f)?;)+ }
                s.end()
            }
        }

        #[cfg(feature = "serde")]
        impl<'de, T> Deserialize<'de> for $Name<T>
        where
            T: Deserialize<'de>,
        {
            fn deserialize<D>(deserializer: D) -> Result<Self, D::Error>
            where
                D: Deserializer<'de>
            {
                struct MatrixVisitor<T>(std::marker::PhantomData<T>);

                impl<'de, X> Visitor<'de> for MatrixVisitor<X>
                where X: Deserialize<'de>,
                {
                    type Value = $Name<X>;

                    fn expecting(&self, formatter: &mut Formatter) -> FmtResult {
                        formatter.write_fmt(format_args!("struct {}", stringify!($Name)))
                    }

                    fn visit_seq<V>(self, mut seq: V) -> Result<$Name<X>, V::Error>
                    where
                        V: SeqAccess<'de>,
                    {
                        Ok($Name {
                            $($f: seq.next_element()?.ok_or_else(|| Error::invalid_length($i, &self))?,)+
                        })
                    }

                    fn visit_map<V>(self, mut map: V) -> Result<$Name<X>, V::Error>
                    where
                        V: MapAccess<'de>,
                    {
                        $(let mut $f = None;)+

                        while let Some(key) = map.next_key()? {
                            match key {
                                $(
                                    stringify!($f) => {
                                        if $f.is_some() {
                                            return Err(Error::duplicate_field(stringify!($f)));
                                        }
                                        $f = Some(map.next_value()?);
                                    }
                                )+
                                value => return Err(Error::unknown_field(value, FIELDS)),
                            }
                        }

                        Ok($Name {
                            $($f: $f.ok_or_else(|| Error::missing_field(stringify!($f)))?,)+
                        })
                    }
                }

                const FIELDS: &'static [&'static str] = &[$(stringify!($f),)+];

                deserializer.deserialize_struct(stringify!($Name), FIELDS, MatrixVisitor::<T>(std::marker::PhantomData))
            }
        }
    };
}

define_mat!(Matrix2, Vector2, 2, T => 0 => axis_x, T => 1 => axis_y);
define_mat!(Matrix3, Vector3, 3, T => 0 => axis_x, T => 1 => axis_y, T => 2 => axis_z);
define_mat!(Matrix4, Vector4, 4, T => 0 => axis_x, T => 1 => axis_y, T => 2 => axis_z, T => 3 => position);

pub type Matrix2f = Matrix2<gl::GLfloat>;
pub type Matrix3f = Matrix3<gl::GLfloat>;
pub type Matrix4f = Matrix4<gl::GLfloat>;

pub type Matrix2d = Matrix2<gl::GLdouble>;
pub type Matrix3d = Matrix3<gl::GLdouble>;
pub type Matrix4d = Matrix4<gl::GLdouble>;

pub type Matrix2i = Matrix2<gl::GLint>;
pub type Matrix3i = Matrix3<gl::GLint>;
pub type Matrix4i = Matrix4<gl::GLint>;

/* Matrix2 */

impl<T> Matrix2<T>
where
    T: Numeric,
{
    #[inline]
    pub fn identity() -> Self {
        let one = T::one();
        let zero = T::zero();

        Self::new(Vector2::new(one, zero), Vector2::new(zero, one))
    }
}

/* Matrix3 */

impl<T> Matrix3<T>
where
    T: Numeric,
{
    #[inline]
    pub fn identity() -> Self {
        let one = T::one();
        let zero = T::zero();

        Self::new(
            Vector3::new(one, zero, zero),
            Vector3::new(zero, one, zero),
            Vector3::new(zero, zero, one),
        )
    }

    #[inline]
    pub fn translate<V: Into<Vector2<T>>>(v: V) -> Self {
        let v = V::into(v);
        let one = T::one();
        let zro = T::zero();

        Self::new(
            Vector3::new(one, zro, zro),
            Vector3::new(zro, one, zro),
            Vector3::new(v.x, v.y, one),
        )
    }

    #[inline]
    pub fn determinant(&self) -> T {
        let m = self;

        m[0][0] * m[1][1] * m[2][2] + m[1][0] * m[2][1] * m[0][2] + m[2][0] * m[0][1] * m[1][2]
            - m[2][0] * m[1][1] * m[0][2]
            - m[1][0] * m[0][1] * m[2][2]
            - m[0][0] * m[2][1] * m[1][2]
    }

    #[inline]
    pub fn multiply<M: Borrow<Self>>(&self, other: M) -> Self {
        let m1 = self;
        let m2 = other.borrow();

        macro_rules! mul {
            ($x:tt, $y:tt) => {
                m1[0][$y] * m2[$x][0] + m1[1][$y] * m2[$x][1] + m1[2][$y] * m2[$x][2]
            };
        }

        Self::new(
            Vector3::new(mul!(0, 0), mul!(0, 1), mul!(0, 2)),
            Vector3::new(mul!(1, 0), mul!(1, 1), mul!(1, 2)),
            Vector3::new(mul!(2, 0), mul!(2, 1), mul!(2, 2)),
        )
    }

    #[inline]
    pub fn transform<V: Into<Vector2<T>>>(&self, v: V) -> Vector2<T> {
        let m = self;
        let v = V::into(v);

        macro_rules! mul {
            ($i:tt) => {
                m[0][$i] * v[0] + m[1][$i] * v[1] + m[2][$i] * T::one()
            };
        }

        Vector2::new(mul!(0), mul!(1))
    }
}

impl<T> Matrix3<T>
where
    T: Float,
{
    #[inline]
    pub fn rotate(a: Angle<T>) -> Self {
        let one = T::one();
        let zro = T::zero();

        Self::new(
            Vector3::new(a.cos(), -a.sin(), zro),
            Vector3::new(a.sin(), a.cos(), zro),
            Vector3::new(zro, zro, one),
        )
    }
}

impl<T, M> Mul<M> for Matrix3<T>
where
    T: Float,
    M: Borrow<Matrix3<T>>,
{
    type Output = Self;

    fn mul(self, rhs: M) -> Self::Output {
        self.multiply(rhs.borrow())
    }
}

/* Matrix4 */

impl<T> Matrix4<T>
where
    T: Numeric,
{
    #[inline]
    pub fn identity() -> Self {
        let one = T::one();
        let zero = T::zero();

        Self::new(
            Vector4::new(one, zero, zero, zero),
            Vector4::new(zero, one, zero, zero),
            Vector4::new(zero, zero, one, zero),
            Vector4::new(zero, zero, zero, one),
        )
    }
}

impl<T> Matrix4<T>
where
    T: Float,
{
    #[inline]
    pub fn translate<V: Into<Vector3<T>>>(v: V) -> Self {
        let v = V::into(v);
        let one = T::one();
        let zero = T::zero();

        Self::new(
            Vector4::new(one, zero, zero, zero),
            Vector4::new(zero, one, zero, zero),
            Vector4::new(zero, zero, one, zero),
            Vector4::new(v.x, v.y, v.z, one),
        )
    }

    #[inline]
    pub fn scale<V: Into<Vector3<T>>>(v: V) -> Self {
        let v = V::into(v);
        let one = T::one();
        let zero = T::zero();

        Self::new(
            Vector4::new(v.x, zero, zero, zero),
            Vector4::new(zero, v.y, zero, zero),
            Vector4::new(zero, zero, v.z, zero),
            Vector4::new(zero, zero, zero, one),
        )
    }

    #[inline]
    #[allow(clippy::many_single_char_names)]
    pub fn rotate<V: Into<Vector3<T>>>(axis: V, angle: Angle<T>) -> Self {
        let axis = V::into(axis).normalize();
        let x = axis.x;
        let y = axis.y;
        let z = axis.z;
        let angle = angle.into_rad().into_inner();
        let s = T::sin(angle);
        let c = T::cos(angle);
        let one = T::one();
        let zero = T::zero();

        Self::new(
            Vector4::new(
                x * x + (one - y * y) * c,
                x * y * (one - c) + z * s,
                x * z * (one - c) - y * s,
                zero,
            ),
            Vector4::new(
                x * y * (one - c) - z * s,
                y * y + (one - y * y) * c,
                y * z * (one - c) + x * s,
                zero,
            ),
            Vector4::new(
                x * z * (one - c) + y * s,
                y * z * (one - c) - x * s,
                z * z + (one - z * z) * c,
                zero,
            ),
            Vector4::new(zero, zero, zero, one),
        )
    }

    #[inline]
    pub fn multiply<M: Borrow<Self>>(&self, other: M) -> Self {
        let m1 = self;
        let m2 = other.borrow();

        macro_rules! mul {
            ($x:tt, $y:tt) => {
                m1[0][$y] * m2[$x][0]
                    + m1[1][$y] * m2[$x][1]
                    + m1[2][$y] * m2[$x][2]
                    + m1[3][$y] * m2[$x][3]
            };
        }

        Self::new(
            Vector4::new(mul!(0, 0), mul!(0, 1), mul!(0, 2), mul!(0, 3)),
            Vector4::new(mul!(1, 0), mul!(1, 1), mul!(1, 2), mul!(1, 3)),
            Vector4::new(mul!(2, 0), mul!(2, 1), mul!(2, 2), mul!(2, 3)),
            Vector4::new(mul!(3, 0), mul!(3, 1), mul!(3, 2), mul!(3, 3)),
        )
    }

    #[inline]
    pub fn transform<V: Into<Vector4<T>>>(&self, v: V) -> Vector4<T> {
        let m = self;
        let v = V::into(v);

        macro_rules! mul {
            ($i:tt) => {
                m[0][$i] * v[0] + m[1][$i] * v[1] + m[2][$i] * v[2] + m[3][$i] * v[3]
            };
        }

        Vector4::new(mul!(0), mul!(1), mul!(2), mul!(3))
    }

    #[inline]
    pub fn transpose(&self) -> Self {
        let m = self;

        Self::new(
            Vector4::new(m[0][0], m[1][0], m[2][0], m[3][0]),
            Vector4::new(m[0][1], m[1][1], m[2][1], m[3][1]),
            Vector4::new(m[0][2], m[1][2], m[2][2], m[3][2]),
            Vector4::new(m[0][3], m[1][3], m[2][3], m[3][3]),
        )
    }

    #[inline]
    pub fn submatrix(&self, s: usize, z: usize) -> Matrix3<T> {
        let mut ret = Matrix3::identity();

        for i in 0..=2 {
            for j in 0..=2 {
                let x = if i >= s { i + 1 } else { i };
                let y = if j >= z { j + 1 } else { j };

                ret[i][j] = self[x][y];
            }
        }

        ret
    }

    #[inline]
    pub fn determinant(&self) -> T {
        let m = self;

        m[0][0] * m.submatrix(0, 0).determinant() - m[1][0] * m.submatrix(1, 0).determinant()
            + m[2][0] * m.submatrix(2, 0).determinant()
            + m[3][0] * m.submatrix(3, 0).determinant()
    }

    #[inline]
    pub fn adjoint(&self) -> Self {
        let m = self;

        Self::new(
            Vector4::new(
                m.submatrix(0, 0).determinant(),
                -m.submatrix(1, 0).determinant(),
                m.submatrix(2, 0).determinant(),
                -m.submatrix(3, 0).determinant(),
            ),
            Vector4::new(
                -m.submatrix(0, 1).determinant(),
                m.submatrix(1, 1).determinant(),
                -m.submatrix(2, 1).determinant(),
                m.submatrix(3, 1).determinant(),
            ),
            Vector4::new(
                m.submatrix(0, 2).determinant(),
                -m.submatrix(1, 2).determinant(),
                m.submatrix(2, 2).determinant(),
                -m.submatrix(3, 2).determinant(),
            ),
            Vector4::new(
                -m.submatrix(0, 3).determinant(),
                m.submatrix(1, 3).determinant(),
                -m.submatrix(2, 3).determinant(),
                m.submatrix(3, 3).determinant(),
            ),
        )
    }

    #[inline]
    pub fn invert(&self) -> Self {
        let d = self.determinant();
        let mut ret = self.adjoint();

        ret[0][0] = ret[0][0] / d;
        ret[0][1] = ret[0][1] / d;
        ret[0][2] = ret[0][2] / d;
        ret[0][3] = ret[0][3] / d;

        ret[1][0] = ret[1][0] / d;
        ret[1][1] = ret[1][1] / d;
        ret[1][2] = ret[1][2] / d;
        ret[1][3] = ret[1][3] / d;

        ret[2][0] = ret[2][0] / d;
        ret[2][1] = ret[2][1] / d;
        ret[2][2] = ret[2][2] / d;
        ret[2][3] = ret[2][3] / d;

        ret[3][0] = ret[3][0] / d;
        ret[3][1] = ret[3][1] / d;
        ret[3][2] = ret[3][2] / d;
        ret[3][3] = ret[3][3] / d;

        ret
    }
}

impl<T> Matrix4<T>
where
    T: Float,
{
    #[inline]
    pub fn ortho(left: T, right: T, bottom: T, top: T, near: T, far: T) -> Self {
        let one = T::one();
        let two = one + one;
        let zero = T::zero();

        Matrix4::new(
            Vector4::new(two / (right - left), zero, zero, zero),
            Vector4::new(zero, two / (top - bottom), zero, zero),
            Vector4::new(zero, zero, -two / (far - near), zero),
            Vector4::new(
                -(right + left) / (right - left),
                -(top + bottom) / (top - bottom),
                -(far + near) / (far - near),
                one,
            ),
        )
    }

    #[inline]
    pub fn perspective(fov: Angle<T>, ratio: T, near: T, far: T) -> Self {
        let one = T::one();
        let two = one + one;
        let zero = T::zero();

        let top = near * fov.into_rad().into_inner().tan();
        let bottom = -top;
        let right = ratio * top;
        let left = -right;

        Matrix4::new(
            Vector4::new(two * near / (right - left), zero, zero, zero),
            Vector4::new(zero, two * near / (top - bottom), zero, zero),
            Vector4::new(
                (right + left) / (right - left),
                (top + bottom) / (top - bottom),
                -(far + near) / (far - near),
                -one,
            ),
            Vector4::new(zero, zero, -two * far * near / (far - near), zero),
        )
    }
}

macro_rules! impl_mul_mat4 {
    ($input:ty, $output:ty, $func:ident) => {
        impl<T> Mul<$input> for Matrix4<T>
        where
            T: Float,
        {
            type Output = $output;

            fn mul(self, rhs: $input) -> Self::Output {
                self.$func(rhs)
            }
        }

        impl<T> Mul<$input> for &Matrix4<T>
        where
            T: Float,
        {
            type Output = $output;

            fn mul(self, rhs: $input) -> Self::Output {
                self.$func(rhs)
            }
        }
    };
}

impl_mul_mat4!(Matrix4<T>, Matrix4<T>, multiply);
impl_mul_mat4!(&Matrix4<T>, Matrix4<T>, multiply);
impl_mul_mat4!(Vector4<T>, Vector4<T>, transform);

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn invert() {
        let m = Matrix4d::identity()
            * Matrix4::translate(Vector3::new(1.0, 2.0, 3.0))
            * Matrix4::scale(Vector3::new(30.0, 20.0, 10.0))
            * Matrix4::rotate(Vector3::new(1.0, 0.0, 0.0), Angle::Deg(45.0));
        let m = m.invert();

        let e = Matrix4d::new(
            Vector4::new(
                0.019526214587563498,
                -0.000000000000000000,
                0.000000000000000000,
                -0.000000000000000000,
            ),
            Vector4::new(
                -0.000000000000000000,
                0.035355339059327376,
                -0.035355339059327376,
                0.000000000000000000,
            ),
            Vector4::new(
                0.00000000000000000,
                0.07071067811865475,
                0.07071067811865475,
                -0.00000000000000000,
            ),
            Vector4::new(
                -0.019526214587563498,
                -0.282842712474619000,
                -0.141421356237309560,
                1.000000000000000000,
            ),
        );

        assert_eq!(e, m);
    }
}