From f2688bc4cddcb7da8c1fdc261590a1072c0f3a92 Mon Sep 17 00:00:00 2001 From: ickk Date: Mon, 27 May 2024 23:05:06 +1000 Subject: [PATCH] impl Matrix4x4 exomorphism - add missing approx impl for Empty --- rga/src/exomorphisms/matrix4x4.rs | 472 ++++++++++++++++++++++++++++ rga/src/exomorphisms/mod.rs | 7 + rga/src/lib.rs | 7 +- rga/src/optional_features/approx.rs | 28 ++ 4 files changed, 510 insertions(+), 4 deletions(-) create mode 100644 rga/src/exomorphisms/matrix4x4.rs diff --git a/rga/src/exomorphisms/matrix4x4.rs b/rga/src/exomorphisms/matrix4x4.rs new file mode 100644 index 0000000..1296ada --- /dev/null +++ b/rga/src/exomorphisms/matrix4x4.rs @@ -0,0 +1,472 @@ +use { + super::Exomorphism, + crate::{helpers::*, values::*, F}, +}; + +/// A matrix whose column vectors have basis `[e1, e2, e3, e4]` +/// +/// ```ignore +/// ⎡ m11, m12, m13, m14 ⎤ +/// ⎢ m21, m22, m23, m24 ⎥ +/// ⎢ m31, m32, m33, m34 ⎥ +/// ⎣ m41, m42, m43, m44 ⎦ +/// ``` +#[rustfmt::skip] +#[derive(Debug, Copy, Clone)] +pub struct Matrix4x4 { + pub m11: F, pub m12: F, pub m13: F, pub m14: F, + pub m21: F, pub m22: F, pub m23: F, pub m24: F, + pub m31: F, pub m32: F, pub m33: F, pub m34: F, + pub m41: F, pub m42: F, pub m43: F, pub m44: F, +} + +impl Matrix4x4 { + #[rustfmt::skip] + pub const ZERO: Self = Self { + m11: 0., m12: 0., m13: 0., m14: 0., + m21: 0., m22: 0., m23: 0., m24: 0., + m31: 0., m32: 0., m33: 0., m34: 0., + m41: 0., m42: 0., m43: 0., m44: 0., + }; + + #[rustfmt::skip] + pub const IDENTITY: Self = Self { + m11: 1., m12: 0., m13: 0., m14: 0., + m21: 0., m22: 1., m23: 0., m24: 0., + m31: 0., m32: 0., m33: 1., m34: 0., + m41: 0., m42: 0., m43: 0., m44: 1., + }; +} + +impl Zero for Matrix4x4 { + #[inline] + fn zero() -> Self { + Matrix4x4::ZERO + } +} + +macro_rules! impl_exomorphism { + ($($morph:ty, $arg:ty => $ret:ty: $morph_fn:ident;)*) => { + $(impl Exomorphism<$arg> for $morph { + fn morph(self, arg: $arg) -> $ret { + $morph_fn(self, arg) + } + })* + }; +} + +impl_exomorphism! { + Matrix4x4, Scalar => Scalar: morph_scalar; + Matrix4x4, Vector => Vector: morph_vector; + Matrix4x4, Bivector => Bivector: morph_bivector; + Matrix4x4, Trivector => Trivector: morph_trivector; + Matrix4x4, Antiscalar => Antiscalar: morph_antiscalar; + Matrix4x4, Multivector => Multivector: morph_multivector; + Matrix4x4, DualNumber => DualNumber: morph_dual_number; + Matrix4x4, OddGrade => OddGrade: morph_odd_grade; + Matrix4x4, EvenGrade => EvenGrade: morph_even_grade; + Matrix4x4, Empty => Empty: return_empty; +} + +#[inline] +fn morph_scalar(_: Matrix4x4, scalar: Scalar) -> Scalar { + scalar +} + +#[rustfmt::skip] +#[inline] +fn morph_vector( + Matrix4x4 { + m11, m12, m13, m14, + m21, m22, m23, m24, + m31, m32, m33, m34, + m41, m42, m43, m44, + }: Matrix4x4, + Vector { e1: v1, e2: v2, e3: v3, e4: v4 }: Vector +) -> Vector { + + let e1 = m11*v1 + m12*v2 + m13*v3 + m14*v4; + let e2 = m21*v1 + m22*v2 + m23*v3 + m24*v4; + let e3 = m31*v1 + m32*v2 + m33*v3 + m34*v4; + let e4 = m41*v1 + m42*v2 + m43*v3 + m44*v4; + + Vector { e1, e2, e3, e4 } +} + +#[rustfmt::skip] +#[inline] +fn morph_bivector( + Matrix4x4 { + m11, m12, m13, m14, + m21, m22, m23, m24, + m31, m32, m33, m34, + m41, m42, m43, m44, + }: Matrix4x4, + Bivector { + e41: b41, e42: b42, e43: b43, + e23: b23, e31: b31, e12: b12, + }: Bivector +) -> Bivector { + + let x1 = m14*b41 - m12*b12 + m13*b31; + let y1 = m14*b42 - m13*b23 + m11*b12; + let z1 = m14*b43 - m11*b31 + m12*b23; + let w1 = m11*b41 + m12*b42 + m13*b43; + + let x2 = m24*b41 - m22*b12 + m23*b31; + let y2 = m24*b42 - m23*b23 + m21*b12; + let z2 = m24*b43 - m21*b31 + m22*b23; + let w2 = m21*b41 + m22*b42 + m23*b43; + + let x3 = m34*b41 - m32*b12 + m33*b31; + let y3 = m34*b42 - m33*b23 + m31*b12; + let z3 = m34*b43 - m31*b31 + m32*b23; + let w3 = m31*b41 + m32*b42 + m33*b43; + + let e41 = m44*w1 - m41*x1 - m42*y1 - m43*z1; + let e42 = m44*w2 - m41*x2 - m42*y2 - m43*z2; + let e43 = m44*w3 - m41*x3 - m42*y3 - m43*z3; + + let e23 = m24*w3 - m21*x3 - m22*y3 - m23*z3; + let e31 = m34*w1 - m31*x1 - m32*y1 - m33*z1; + let e12 = m14*w2 - m11*x2 - m12*y2 - m13*z2; + + Bivector { e41, e42, e43, e23, e31, e12 } +} + +#[rustfmt::skip] +#[inline] +fn morph_trivector( + Matrix4x4 { + m11, m12, m13, m14, + m21, m22, m23, m24, + m31, m32, m33, m34, + m41, m42, m43, m44, + }: Matrix4x4, + Trivector { e321: t321, e423: t423, e431: t431, e412: t412 }: Trivector +) -> Trivector { + + let a1 = m22*m33 - m23*m32; + let b2 = m23*m31 - m21*m33; + let c3 = m21*m32 - m22*m31; + let d4 = m23*m34 - m24*m33; + let e5 = m22*m34 - m24*m32; + let f6 = m21*m34 - m24*m31; + + let u1 = m42*m13 - m43*m12; + let v2 = m43*m11 - m41*m13; + let w3 = m41*m12 - m42*m11; + let x4 = m43*m14 - m44*m13; + let y5 = m42*m14 - m44*m12; + let z6 = m41*m14 - m44*m11; + + let e423 = -( + t321*(m41*a1 + m42*b2 + m43*c3) + - t423*(m44*a1 + m42*d4 - m43*e5) + - t431*(m44*b2 + m43*f6 - m41*d4) + - t412*(m44*c3 + m41*e5 - m42*f6) + ); + let e431 = + t321*(m31*u1 + m32*v2 + m33*w3) + - t423*(m34*u1 + m32*x4 - m33*y5) + - t431*(m34*v2 + m33*z6 - m31*x4) + - t412*(m34*w3 + m31*y5 - m32*z6); + let e412 = -( + t321*(m21*u1 + m22*v2 + m23*w3) + - t423*(m24*u1 + m22*x4 - m23*y5) + - t431*(m24*v2 + m23*z6 - m21*x4) + - t412*(m24*w3 + m21*y5 - m22*z6) + ); + let e321 = + t321*(m11*a1 + m12*b2 + m13*c3) + - t423*(m14*a1 + m12*d4 - m13*e5) + - t431*(m14*b2 + m13*f6 - m11*d4) + - t412*(m14*c3 + m11*e5 - m12*f6); + + Trivector { e423, e431, e412, e321 } +} + +#[rustfmt::skip] +#[inline] +fn morph_antiscalar( + Matrix4x4 { + m11, m12, m13, m14, + m21, m22, m23, m24, + m31, m32, m33, m34, + m41, m42, m43, m44, + }: Matrix4x4, + Antiscalar { e1234: p1234 }: Antiscalar, +) -> Antiscalar { + + let a1 = m22*m33 - m23*m32; + let b2 = m23*m31 - m21*m33; + let c3 = m21*m32 - m22*m31; + let d4 = m23*m34 - m24*m33; + let e5 = m22*m34 - m24*m32; + let f6 = m21*m34 - m24*m31; + + let u1 = m42*m13 - m43*m12; + let v2 = m43*m11 - m41*m13; + let w3 = m41*m12 - m42*m11; + let x4 = m43*m14 - m44*m13; + let y5 = m42*m14 - m44*m12; + let z6 = m41*m14 - m44*m11; + + let e1234 = -p1234*(a1*z6 + b2*y5 + c3*x4 + d4*w3 + e5*v2 + f6*u1); + + Antiscalar { e1234 } +} + +#[rustfmt::skip] +#[inline] +fn morph_multivector( + matrix: Matrix4x4, + Multivector { + s, + e1, e2, e3, e4, + e41, e42, e43, e23, e31, e12, + e423, e431, e412, e321, + e1234, + }: Multivector, +) -> Multivector { + + let Scalar { s } = morph_scalar( + matrix, Scalar { s } + ); + let Vector { e1, e2, e3, e4 } = morph_vector( + matrix, Vector { e1, e2, e3, e4 } + ); + let Bivector { e41, e42, e43, e23, e31, e12 } = morph_bivector( + matrix, Bivector { e41, e42, e43, e23, e31, e12 }, + ); + let Trivector { e423, e431, e412, e321 } = morph_trivector( + matrix, Trivector { e423, e431, e412, e321 } + ); + let Antiscalar { e1234 } = morph_antiscalar( + matrix, Antiscalar { e1234 } + ); + + Multivector { + s, + e1, e2, e3, e4, + e41, e42, e43, e23, e31, e12, + e423, e431, e412, e321, + e1234, + } +} + +#[rustfmt::skip] +#[inline] +fn morph_dual_number( + matrix: Matrix4x4, + DualNumber { s, e1234 }: DualNumber, +) -> DualNumber { + + let Scalar { s } = morph_scalar( + matrix, Scalar { s } + ); + let Antiscalar { e1234 } = morph_antiscalar( + matrix, Antiscalar { e1234 } + ); + + DualNumber { s, e1234 } +} + +#[rustfmt::skip] +#[inline] +fn morph_odd_grade( + matrix: Matrix4x4, + OddGrade { + e1, e2, e3, e4, + e423, e431, e412, e321, + }: OddGrade, +) -> OddGrade { + + let Vector { e1, e2, e3, e4 } = morph_vector( + matrix, Vector { e1, e2, e3, e4 } + ); + let Trivector { e423, e431, e412, e321 } = morph_trivector( + matrix, Trivector { e423, e431, e412, e321 } + ); + + OddGrade { + e1, e2, e3, e4, + e423, e431, e412, e321, + } +} + +#[rustfmt::skip] +#[inline] +fn morph_even_grade( + matrix: Matrix4x4, + EvenGrade { + s, + e41, e42, e43, e23, e31, e12, + e1234, + }: EvenGrade, +) -> EvenGrade { + + let Scalar { s } = morph_scalar( + matrix, Scalar { s } + ); + let Bivector { e41, e42, e43, e23, e31, e12 } = morph_bivector( + matrix, Bivector { e41, e42, e43, e23, e31, e12 }, + ); + let Antiscalar { e1234 } = morph_antiscalar( + matrix, Antiscalar { e1234 } + ); + + EvenGrade { + s, + e41, e42, e43, e23, e31, e12, + e1234, + } +} + +#[cfg(any(test, doctest))] +mod tests { + use { + super::*, + crate::{free_functions::*, test_values::*}, + }; + + #[rustfmt::skip] + pub const MATRIX_A: Matrix4x4 = Matrix4x4 { + m11: 2., m12: 3., m13: 5., m14: 7., + m21: 11., m22: 13., m23: 17., m24: 19., + m31: 23., m32: 29., m33: 31., m34: 37., + m41: 41., m42: 43., m43: 47., m44: 53., + }; + + mod vector { + use super::*; + + // we test Vector, and then just check the linearity property of the others + // types below. + + #[test] + fn identity_morph() { + let vector = grade_1(MULTIVECTOR_A); + + assert_eq!(dbg!(Matrix4x4::IDENTITY.morph(vector)), dbg!(vector)); + } + + #[rustfmt::skip] + #[test] + fn matrix_a_morph() { + let vector = grade_1(MULTIVECTOR_A); + + assert_eq!( + dbg!(MATRIX_A.morph(vector)), + dbg!(Vector { e1: 133., e2: 426., e3: 838.,e4: 1250. }) + ); + } + } + + mod scalar { + use super::*; + + #[test] + fn identity_morph() { + let scalar = grade_0(MULTIVECTOR_A); + + assert_eq!(Matrix4x4::IDENTITY.morph(scalar), scalar); + } + + #[test] + fn matrix_a_morph() { + let scalar = grade_0(MULTIVECTOR_A); + + assert_eq!(MATRIX_A.morph(scalar), scalar); + } + } + + mod bivector { + use super::*; + + #[test] + fn identity_morph() { + let bivector = grade_2(MULTIVECTOR_A); + + assert_eq!(Matrix4x4::IDENTITY.morph(bivector), bivector); + } + + #[test] + fn matrix_a_morph() { + let vector_a = grade_1(MULTIVECTOR_A); + let vector_b = grade_1(MULTIVECTOR_B); + + assert_eq!( + wedge_product(MATRIX_A.morph(vector_a), MATRIX_A.morph(vector_b)), + MATRIX_A.morph(wedge_product(vector_a, vector_b)) + ); + } + } + + mod trivector { + use super::*; + + #[test] + fn identity_morph() { + let trivector = grade_3(MULTIVECTOR_A); + + assert_eq!(Matrix4x4::IDENTITY.morph(trivector), trivector,); + } + + #[test] + fn matrix_a_morph() { + let vector = grade_1(MULTIVECTOR_A); + let bivector = grade_2(MULTIVECTOR_B); + + assert_eq!( + wedge_product(MATRIX_A.morph(vector), MATRIX_A.morph(bivector)), + MATRIX_A.morph(wedge_product(vector, bivector)) + ); + } + } + + mod antiscalar { + use super::*; + + #[test] + fn identity_morph() { + let antiscalar = grade_4(MULTIVECTOR_A); + + assert_eq!(Matrix4x4::IDENTITY.morph(antiscalar), antiscalar); + } + + #[test] + fn matrix_a_morph() { + let vector = grade_1(MULTIVECTOR_A); + let trivector = grade_3(MULTIVECTOR_B); + + assert_eq!( + wedge_product(MATRIX_A.morph(vector), MATRIX_A.morph(trivector)), + MATRIX_A.morph(wedge_product(vector, trivector)) + ); + } + } + + mod multivector { + use super::*; + + #[test] + fn identity_morph() { + assert_eq!( + dbg!(Matrix4x4::IDENTITY.morph(MULTIVECTOR_A)), + dbg!(MULTIVECTOR_A) + ); + } + + #[test] + fn matrix_a_morph() { + assert_eq!( + dbg!(wedge_product( + MATRIX_A.morph(MULTIVECTOR_A), + MATRIX_A.morph(MULTIVECTOR_B), + )), + dbg!(MATRIX_A.morph(wedge_product(MULTIVECTOR_A, MULTIVECTOR_B))) + ); + } + } +} diff --git a/rga/src/exomorphisms/mod.rs b/rga/src/exomorphisms/mod.rs index 8b13789..ea98d82 100644 --- a/rga/src/exomorphisms/mod.rs +++ b/rga/src/exomorphisms/mod.rs @@ -1 +1,8 @@ +mod matrix4x4; +pub use matrix4x4::Matrix4x4; + +/// A linear mapping extended to multivectors +pub trait Exomorphism { + fn morph(self, arg: Arg) -> Arg; +} diff --git a/rga/src/lib.rs b/rga/src/lib.rs index e907ae2..0d4c472 100644 --- a/rga/src/lib.rs +++ b/rga/src/lib.rs @@ -7,12 +7,11 @@ use ::libm::Libm; /// floating point type pub(crate) type F = f64; -mod exomorphisms; -mod operators; +pub mod exomorphisms; +pub mod operators; mod values; -// pub use exomorphisms::*; -pub use operators::*; +pub(crate) use operators::*; pub use values::*; mod optional_features; diff --git a/rga/src/optional_features/approx.rs b/rga/src/optional_features/approx.rs index 5bf6741..136b789 100644 --- a/rga/src/optional_features/approx.rs +++ b/rga/src/optional_features/approx.rs @@ -92,3 +92,31 @@ impl_approx_eq_self! { e41, e42, e43, e23, e31, e12, e1234, } + +impl AbsDiffEq for Empty { + type Epsilon = F; + fn default_epsilon() -> Self::Epsilon { + F::default_epsilon() + } + fn abs_diff_eq(&self, _: &Self, _: F) -> bool { + true + } +} + +impl RelativeEq for Empty { + fn default_max_relative() -> F { + F::default_max_relative() + } + fn relative_eq(&self, _: &Self, _: F, _: F) -> bool { + true + } +} + +impl UlpsEq for Empty { + fn default_max_ulps() -> u32 { + F::default_max_ulps() + } + fn ulps_eq(&self, _: &Self, _: F, _: u32) -> bool { + true + } +}