|
123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676 |
- #![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: Vector2<T>) -> Self {
- 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(&self, other: &Self) -> Self {
- let m1 = self;
- let m2 = other;
-
- 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)),
- )
- }
- }
-
- 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: Vector3<T>) -> 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(v.x, v.y, v.z, one),
- )
- }
-
- #[inline]
- pub fn scale(v: Vector3<T>) -> Self {
- 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(axis: Vector3<T>, angle: Angle<T>) -> Self {
- let axis = 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(&self, other: &Self) -> Self {
- let m1 = self;
- let m2 = other;
-
- 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(&self, v: &Vector4<T>) -> Vector4<T> {
- let m = self;
-
- 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),
- )
- }
- }
-
- impl<T, M> Mul<M> for Matrix4<T>
- where
- T: Float,
- M: Borrow<Matrix4<T>>,
- {
- type Output = Self;
-
- fn mul(self, rhs: M) -> Self::Output {
- self.multiply(rhs.borrow())
- }
- }
-
- #[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);
- }
- }
|