diff --git a/sub_crates/rmath/src/wide4.rs b/sub_crates/rmath/src/wide4.rs index 0bb3aa0..466c9fd 100644 --- a/sub_crates/rmath/src/wide4.rs +++ b/sub_crates/rmath/src/wide4.rs @@ -1,5 +1,7 @@ use std::ops::{AddAssign, DivAssign, MulAssign, SubAssign}; +use approx::relative_eq; + use crate::{difference_of_products, two_prod, two_sum}; pub use fallback::Float4; @@ -351,16 +353,6 @@ impl Float4 { /// Returns `None` if not invertible. #[inline] pub fn invert_3x3(m: [Self; 3]) -> Option<[Self; 3]> { - // let a = difference_of_products(m[1].b(), m[2].c(), m[1].c(), m[2].b()); - // let b = difference_of_products(m[1].c(), m[2].a(), m[1].a(), m[2].c()); - // let c = difference_of_products(m[1].a(), m[2].b(), m[1].b(), m[2].a()); - // let d = difference_of_products(m[2].b(), m[0].c(), m[2].c(), m[0].b()); - // let e = difference_of_products(m[2].c(), m[0].a(), m[2].a(), m[0].c()); - // let f = difference_of_products(m[2].a(), m[0].b(), m[2].b(), m[0].a()); - // let g = difference_of_products(m[0].b(), m[1].c(), m[0].c(), m[1].b()); - // let h = difference_of_products(m[0].c(), m[1].a(), m[0].a(), m[1].c()); - // let i = difference_of_products(m[0].a(), m[1].b(), m[0].b(), m[1].a()); - let m0_bca = m[0].bcad(); let m1_bca = m[1].bcad(); let m2_bca = m[2].bcad(); @@ -371,7 +363,6 @@ impl Float4 { let def = difference_of_products(m2_bca, m0_cab, m2_cab, m0_bca); let ghi = difference_of_products(m0_bca, m1_cab, m0_cab, m1_bca); - // TODO: use precise inner product. let det = Self::dot_3( Self::new(abc.a(), def.a(), ghi.a(), 0.0), Self::new(m[0].a(), m[1].a(), m[2].a(), 0.0), @@ -389,15 +380,6 @@ impl Float4 { /// Returns `None` if not invertible. #[inline] pub fn invert_3x3_fast(m: [Self; 3]) -> Option<[Self; 3]> { - // let a = (m[1].b() * m[2].c()) - (m[1].c() * m[2].b()); - // let b = (m[1].c() * m[2].a()) - (m[1].a() * m[2].c()); - // let c = (m[1].a() * m[2].b()) - (m[1].b() * m[2].a()); - // let e = (m[2].c() * m[0].a()) - (m[2].a() * m[0].c()); - // let f = (m[2].a() * m[0].b()) - (m[2].b() * m[0].a()); - // let g = (m[0].b() * m[1].c()) - (m[0].c() * m[1].b()); - // let h = (m[0].c() * m[1].a()) - (m[0].a() * m[1].c()); - // let i = (m[0].a() * m[1].b()) - (m[0].b() * m[1].a()); - let m0_bca = m[0].bcad(); let m1_bca = m[1].bcad(); let m2_bca = m[2].bcad(); @@ -408,7 +390,7 @@ impl Float4 { let def = (m2_bca * m0_cab) - (m2_cab * m0_bca); let ghi = (m0_bca * m1_cab) - (m0_cab * m1_bca); - let det = Self::dot_3( + let det = Self::dot_3_fast( Self::new(abc.a(), def.a(), ghi.a(), 0.0), Self::new(m[0].a(), m[1].a(), m[2].a(), 0.0), ); @@ -422,24 +404,63 @@ impl Float4 { /// Multiplies a 3D vector with a 3x3 matrix. #[inline] - pub fn vec3_mul_3x3(_v: Self, _m: &[Self; 3]) -> Self { - todo!() + pub fn vec_mul_3x3(self, m: &[Self; 3]) -> Self { + let x = self.aaaa(); + let y = self.bbbb(); + let z = self.cccc(); + + // Products. + let (a, a_err) = two_prod(x, m[0]); + let (b, b_err) = two_prod(y, m[1]); + let (c, c_err) = two_prod(z, m[2]); + + // Sums. + let (s1, s1_err) = two_sum(a, b); + let err1 = a_err + (b_err + s1_err); + + let (s2, s2_err) = two_sum(c, s1); + let err2 = c_err + (err1 + s2_err); + + s2 + err2 } /// Multiplies a 3D vector with a 3x3 matrix. /// /// Faster but less precise version. #[inline] - pub fn vec3_mul_3x3_fast(_v: Self, _m: &[Self; 3]) -> Self { - todo!() + pub fn vec_mul_3x3_fast(self, m: &[Self; 3]) -> Self { + let x = self.aaaa(); + let y = self.bbbb(); + let z = self.cccc(); + + (x * m[0]) + (y * m[1]) + (z * m[2]) } /// Transforms a 3d point by an affine transform. /// /// `m` is the 3x3 part of the affine transform, `t` is the translation part. #[inline] - pub fn pnt3_mul_affine(_p: Self, _m: &[Self; 3], _t: Self) -> Self { - todo!() + pub fn vec_mul_affine(self, m: &[Self; 3], t: Self) -> Self { + let x = self.aaaa(); + let y = self.bbbb(); + let z = self.cccc(); + + // Products. + let (a, a_err) = two_prod(x, m[0]); + let (b, b_err) = two_prod(y, m[1]); + let (c, c_err) = two_prod(z, m[2]); + + // Sums. + let (s1, s1_err) = two_sum(a, b); + let err1 = a_err + (b_err + s1_err); + + let (s2, s2_err) = two_sum(c, s1); + let err2 = c_err + (err1 + s2_err); + + let (s3, s3_err) = two_sum(t, s2); + let err3 = err2 + s3_err; + + s3 + err3 } /// Transforms a 3d point by an affine transform, except it applies @@ -450,8 +471,46 @@ impl Float4 { /// /// `m` is the 3x3 part of the affine transform, `t` is the translation part. #[inline] - pub fn pnt3_mul_affine_rev(_p: Self, _m: &[Self; 3], _t: Self) -> Self { - todo!() + pub fn vec_mul_affine_rev(self, m: &[Self; 3], t: Self) -> Self { + let (v, v_err) = two_sum(self, t); + + let (x, x_err) = (v.aaaa(), v_err.aaaa()); + let (y, y_err) = (v.bbbb(), v_err.bbbb()); + let (z, z_err) = (v.cccc(), v_err.cccc()); + + // Products. + let ((a, a_err1), a_err2) = (two_prod(x, m[0]), x_err * m[0]); + let ((b, b_err1), b_err2) = (two_prod(y, m[1]), y_err * m[1]); + let ((c, c_err1), c_err2) = (two_prod(z, m[2]), z_err * m[2]); + let a_err = a_err1 + a_err2; + let b_err = b_err1 + b_err2; + let c_err = c_err1 + c_err2; + + // Sums. + let (s1, s1_err) = two_sum(a, b); + let err1 = a_err + (b_err + s1_err); + + let (s2, s2_err) = two_sum(c, s1); + let err2 = c_err + (err1 + s2_err); + + let (s3, s3_err) = two_sum(t, s2); + let err3 = err2 + s3_err; + + s3 + err3 + } + + /// Returns whether the `Float4`s are approximately equal to each + /// other. + /// + /// Each corresponding element cannot have a relative error exceeding + /// `epsilon`. + pub(crate) fn aprx_eq(a: Self, b: Self, epsilon: f32) -> bool { + let mut eq = true; + eq &= relative_eq!(a.a(), b.a(), epsilon = epsilon); + eq &= relative_eq!(a.b(), b.b(), epsilon = epsilon); + eq &= relative_eq!(a.c(), b.c(), epsilon = epsilon); + eq &= relative_eq!(a.d(), b.d(), epsilon = epsilon); + eq } } diff --git a/sub_crates/rmath/src/xform.rs b/sub_crates/rmath/src/xform.rs index b0a3593..6d4e186 100644 --- a/sub_crates/rmath/src/xform.rs +++ b/sub_crates/rmath/src/xform.rs @@ -2,8 +2,6 @@ use std::ops::{Add, Mul}; -use approx::relative_eq; - use crate::point::Point; use crate::wide4::Float4; @@ -80,19 +78,12 @@ impl Xform { /// Returns whether the matrices are approximately equal to each other. /// Each corresponding element in the matrices cannot have a relative /// error exceeding epsilon. - #[inline] - pub fn aprx_eq(&self, other: Xform, epsilon: f32) -> bool { + pub(crate) fn aprx_eq(&self, other: Xform, epsilon: f32) -> bool { let mut eq = true; - for (t1, t2) in self - .m - .iter() - .chain(&[self.t]) - .zip(other.m.iter().chain(&[other.t])) - { - eq &= relative_eq!(t1.a(), t2.a(), epsilon = epsilon); - eq &= relative_eq!(t1.b(), t2.b(), epsilon = epsilon); - eq &= relative_eq!(t1.c(), t2.c(), epsilon = epsilon); - } + eq &= Float4::aprx_eq(self.m[0], other.m[0], epsilon); + eq &= Float4::aprx_eq(self.m[1], other.m[1], epsilon); + eq &= Float4::aprx_eq(self.m[2], other.m[2], epsilon); + eq &= Float4::aprx_eq(self.t, other.t, epsilon); eq }