diff --git a/Cargo.lock b/Cargo.lock index 9f52b24..efa4183 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -11,15 +11,6 @@ dependencies = [ "winapi", ] -[[package]] -name = "approx" -version = "0.4.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3f2a05fd1bd10b2527e20a2cd32d8873d115b8b39fe219ee25f42a8aca6ba278" -dependencies = [ - "num-traits", -] - [[package]] name = "arrayvec" version = "0.5.2" @@ -566,9 +557,6 @@ dependencies = [ [[package]] name = "rmath" version = "0.1.0" -dependencies = [ - "approx", -] [[package]] name = "rustc-serialize" diff --git a/sub_crates/rmath/Cargo.toml b/sub_crates/rmath/Cargo.toml index 963a83c..15a9d3a 100644 --- a/sub_crates/rmath/Cargo.toml +++ b/sub_crates/rmath/Cargo.toml @@ -8,6 +8,3 @@ license = "MIT, Apache 2.0" [lib] name = "rmath" path = "src/lib.rs" - -[dependencies] -approx = "0.4" diff --git a/sub_crates/rmath/src/lib.rs b/sub_crates/rmath/src/lib.rs index e382266..d097132 100644 --- a/sub_crates/rmath/src/lib.rs +++ b/sub_crates/rmath/src/lib.rs @@ -4,6 +4,7 @@ mod normal; mod point; +mod utils; mod vector; pub mod wide4; mod xform; diff --git a/sub_crates/rmath/src/normal.rs b/sub_crates/rmath/src/normal.rs index ab73bf8..950ac38 100644 --- a/sub_crates/rmath/src/normal.rs +++ b/sub_crates/rmath/src/normal.rs @@ -207,8 +207,9 @@ mod tests { #[test] fn xform() { let n = Normal::new(1.0, 2.5, 4.0); - let m = - Xform::new(1.0, 3.0, 9.0, 2.0, 6.0, 2.0, 2.0, 7.0, 11.0, 1.5, 8.0, 12.0).into_full(); + let m = Xform::new(1.0, 3.0, 9.0, 2.0, 6.0, 2.0, 2.0, 7.0, 11.0, 1.5, 8.0, 12.0) + .into_full() + .unwrap(); assert_eq!(n.xform(&m), Normal::new(-4.0625, 1.78125, -0.03125)); assert_eq!(n.xform(&m).xform_inv(&m), n); @@ -217,8 +218,9 @@ mod tests { #[test] fn xform_fast() { let n = Normal::new(1.0, 2.5, 4.0); - let m = - Xform::new(1.0, 3.0, 9.0, 2.0, 6.0, 2.0, 2.0, 7.0, 11.0, 1.5, 8.0, 12.0).into_full(); + let m = Xform::new(1.0, 3.0, 9.0, 2.0, 6.0, 2.0, 2.0, 7.0, 11.0, 1.5, 8.0, 12.0) + .into_full() + .unwrap(); assert_eq!(n.xform_fast(&m), Normal::new(-4.0625, 1.78125, -0.03125)); assert_eq!(n.xform_fast(&m).xform_inv_fast(&m), n); diff --git a/sub_crates/rmath/src/point.rs b/sub_crates/rmath/src/point.rs index b43a575..669335b 100644 --- a/sub_crates/rmath/src/point.rs +++ b/sub_crates/rmath/src/point.rs @@ -147,8 +147,9 @@ mod tests { #[test] fn xform() { let p = Point::new(1.0, 2.5, 4.0); - let m = - Xform::new(1.0, 3.0, 9.0, 2.0, 6.0, 2.0, 2.0, 7.0, 11.0, 1.5, 8.0, 12.0).into_full(); + let m = Xform::new(1.0, 3.0, 9.0, 2.0, 6.0, 2.0, 2.0, 7.0, 11.0, 1.5, 8.0, 12.0) + .into_full() + .unwrap(); assert_eq!(p.xform(&m), Point::new(15.5, 54.0, 70.0)); assert_eq!(p.xform(&m).xform_inv(&m), p); } @@ -156,8 +157,9 @@ mod tests { #[test] fn xform_fast() { let p = Point::new(1.0, 2.5, 4.0); - let m = - Xform::new(1.0, 3.0, 9.0, 2.0, 6.0, 2.0, 2.0, 7.0, 11.0, 1.5, 8.0, 12.0).into_full(); + let m = Xform::new(1.0, 3.0, 9.0, 2.0, 6.0, 2.0, 2.0, 7.0, 11.0, 1.5, 8.0, 12.0) + .into_full() + .unwrap(); assert_eq!(p.xform_fast(&m), Point::new(15.5, 54.0, 70.0)); assert_eq!(p.xform_fast(&m).xform_inv_fast(&m), p); } diff --git a/sub_crates/rmath/src/utils.rs b/sub_crates/rmath/src/utils.rs new file mode 100644 index 0000000..454da83 --- /dev/null +++ b/sub_crates/rmath/src/utils.rs @@ -0,0 +1,73 @@ +const TOP_BIT: u32 = 1 << 31; + +/// Compute how different two floats are in ulps. +#[inline(always)] +pub(crate) fn ulp_diff(a: f32, b: f32) -> u32 { + let a = a.to_bits(); + let b = b.to_bits(); + + let a_sign = a & TOP_BIT; + let b_sign = b & TOP_BIT; + let a_abs = a & !TOP_BIT; + let b_abs = b & !TOP_BIT; + + if a_sign == b_sign { + a_abs.max(b_abs) - a_abs.min(b_abs) + } else { + a_abs + b_abs + } +} + +/// Checks if two floats are approximately equal, within `max_ulps`. +#[inline(always)] +pub(crate) fn ulps_eq(a: f32, b: f32, max_ulps: u32) -> bool { + !a.is_nan() && !b.is_nan() && (ulp_diff(a, b) <= max_ulps) +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn ulp_diff_test() { + assert_eq!(ulp_diff(1.0, 1.0), 0); + assert_eq!(ulp_diff(0.0, 0.0), 0); + assert_eq!(ulp_diff(0.0, -0.0), 0); + assert_eq!(ulp_diff(1.0, 2.0), 1 << 23); + assert_eq!(ulp_diff(0.0, f32::from_bits(0.0f32.to_bits() + 1)), 1); + assert_eq!(ulp_diff(-0.0, f32::from_bits(0.0f32.to_bits() + 1)), 1); + assert_eq!( + ulp_diff(std::f32::INFINITY, -std::f32::INFINITY), + 0xff000000 + ); + } + + #[test] + fn ulps_eq_test() { + assert!(ulps_eq(1.0, 1.0, 0)); + assert!(ulps_eq(1.0, 1.0, 1)); + assert!(ulps_eq(0.0, 0.0, 0)); + assert!(ulps_eq(0.0, -0.0, 0)); + + assert!(ulps_eq(1.0, 2.0, 1 << 23)); + assert!(!ulps_eq(1.0, 2.0, (1 << 23) - 1)); + + assert!(ulps_eq(0.0, f32::from_bits(0.0f32.to_bits() + 1), 1)); + assert!(!ulps_eq(0.0, f32::from_bits(0.0f32.to_bits() + 1), 0)); + + assert!(ulps_eq(-0.0, f32::from_bits(0.0f32.to_bits() + 1), 1)); + assert!(!ulps_eq(-0.0, f32::from_bits(0.0f32.to_bits() + 1), 0)); + + assert!(ulps_eq(std::f32::INFINITY, -std::f32::INFINITY, 0xff000000)); + assert!(!ulps_eq( + std::f32::INFINITY, + -std::f32::INFINITY, + 0xff000000 - 1 + )); + + assert!(!ulps_eq(std::f32::NAN, std::f32::NAN, 0)); + assert!(!ulps_eq(-std::f32::NAN, -std::f32::NAN, 0)); + assert!(!ulps_eq(std::f32::NAN, std::f32::INFINITY, 1 << 31)); + assert!(!ulps_eq(std::f32::INFINITY, std::f32::NAN, 1 << 31)); + } +} diff --git a/sub_crates/rmath/src/vector.rs b/sub_crates/rmath/src/vector.rs index 4ff2a97..8cdf424 100644 --- a/sub_crates/rmath/src/vector.rs +++ b/sub_crates/rmath/src/vector.rs @@ -213,8 +213,9 @@ mod tests { #[test] fn xform() { let v = Vector::new(1.0, 2.5, 4.0); - let m = - Xform::new(1.0, 3.0, 9.0, 2.0, 6.0, 2.0, 2.0, 7.0, 11.0, 1.5, 8.0, 12.0).into_full(); + let m = Xform::new(1.0, 3.0, 9.0, 2.0, 6.0, 2.0, 2.0, 7.0, 11.0, 1.5, 8.0, 12.0) + .into_full() + .unwrap(); assert_eq!(v.xform(&m), Vector::new(14.0, 46.0, 58.0)); assert_eq!(v.xform(&m).xform_inv(&m), v); @@ -223,8 +224,9 @@ mod tests { #[test] fn xform_fast() { let v = Vector::new(1.0, 2.5, 4.0); - let m = - Xform::new(1.0, 3.0, 9.0, 2.0, 6.0, 2.0, 2.0, 7.0, 11.0, 1.5, 8.0, 12.0).into_full(); + let m = Xform::new(1.0, 3.0, 9.0, 2.0, 6.0, 2.0, 2.0, 7.0, 11.0, 1.5, 8.0, 12.0) + .into_full() + .unwrap(); assert_eq!(v.xform_fast(&m), Vector::new(14.0, 46.0, 58.0)); assert_eq!(v.xform_fast(&m).xform_inv_fast(&m), v); diff --git a/sub_crates/rmath/src/wide4.rs b/sub_crates/rmath/src/wide4.rs index e846753..6398d41 100644 --- a/sub_crates/rmath/src/wide4.rs +++ b/sub_crates/rmath/src/wide4.rs @@ -2,8 +2,9 @@ use std::ops::{ AddAssign, BitAndAssign, BitOrAssign, BitXorAssign, DivAssign, MulAssign, SubAssign, }; -use approx::ulps_eq; +use std::cmp::PartialEq; +use crate::utils::ulps_eq; use crate::{difference_of_products, two_prod, two_sum}; pub use fallback::{Bool4, Float4}; @@ -646,12 +647,55 @@ impl Float4 { /// `epsilon`. pub(crate) fn aprx_eq(a: Self, b: Self, max_ulps: u32) -> bool { let mut eq = true; - eq &= ulps_eq!(a.a(), b.a(), max_ulps = max_ulps); - eq &= ulps_eq!(a.b(), b.b(), max_ulps = max_ulps); - eq &= ulps_eq!(a.c(), b.c(), max_ulps = max_ulps); - eq &= ulps_eq!(a.d(), b.d(), max_ulps = max_ulps); + eq &= ulps_eq(a.a(), b.a(), max_ulps); + eq &= ulps_eq(a.b(), b.b(), max_ulps); + eq &= ulps_eq(a.c(), b.c(), max_ulps); + eq &= ulps_eq(a.d(), b.d(), max_ulps); eq } + + /// Transforms one affine transform by another. + /// + /// The result is an affine transform that acts as a sequence of the + /// first followed by the second. + /// + /// `m#` is the 3x3 part of the affine transform, `t#` is the translation part. + #[inline] + pub fn affine_mul_affine( + m1: &[Self; 3], + t1: Self, + m2: &[Self; 3], + t2: Self, + ) -> ([Self; 3], Self) { + ( + [ + m1[0].vec_mul_3x3(&m2), + m1[1].vec_mul_3x3(&m2), + m1[2].vec_mul_3x3(&m2), + ], + t1.vec_mul_affine(&m2, t2), + ) + } + + /// Transforms one affine transform by another. + /// + /// Faster but less precise version. + #[inline] + pub fn affine_mul_affine_fast( + m1: &[Self; 3], + t1: Self, + m2: &[Self; 3], + t2: Self, + ) -> ([Self; 3], Self) { + ( + [ + m1[0].vec_mul_3x3_fast(&m2), + m1[1].vec_mul_3x3_fast(&m2), + m1[2].vec_mul_3x3_fast(&m2), + ], + t1.vec_mul_affine_fast(&m2, t2), + ) + } } impl AddAssign for Float4 { @@ -696,6 +740,15 @@ impl DivAssign for Float4 { } } +impl PartialEq for Float4 { + #[inline(always)] + fn eq(&self, rhs: &Self) -> bool { + Self::eq(*self, *rhs).to_bitmask() == 0b1111 + } +} + +//-------- + impl BitAndAssign for Bool4 { #[inline(always)] fn bitand_assign(&mut self, rhs: Self) { @@ -716,3 +769,26 @@ impl BitXorAssign for Bool4 { *self = *self ^ rhs; } } + +//------------------------------------------------------------- + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn approximate_equality_test() { + let a = Float4::new(1.0, 2.0, 3.0, 4.0); + let b = Float4::new(1.00001, 2.00002, 3.00003, 4.00004); + let c = Float4::new(1.0e-43, 2.0e-43, 3.0e-43, 4.0e-43); + let d = Float4::new(-1.0e-43, -2.0e-43, -3.0e-43, -4.0e-43); + + assert!(Float4::aprx_eq(a, a, 0)); + + assert!(Float4::aprx_eq(a, b, 130)); + assert!(!Float4::aprx_eq(a, b, 120)); + + assert!(Float4::aprx_eq(c, d, 575)); + assert!(!Float4::aprx_eq(c, d, 565)); + } +} diff --git a/sub_crates/rmath/src/xform.rs b/sub_crates/rmath/src/xform.rs index bece65e..1e476ec 100644 --- a/sub_crates/rmath/src/xform.rs +++ b/sub_crates/rmath/src/xform.rs @@ -6,7 +6,7 @@ use crate::point::Point; use crate::wide4::Float4; /// An affine transform. -#[derive(Debug, Copy, Clone)] +#[derive(Debug, Copy, Clone, PartialEq)] #[repr(C)] pub struct Xform { pub m: [Float4; 3], // Linear matrix. @@ -90,31 +90,50 @@ impl Xform { /// Computes a "full" version of the transform, which can do both /// forward and inverse transforms. #[inline] - pub fn into_full(self) -> XformFull { - XformFull { - m: self.m, - m_inv: Float4::invert_3x3(self.m).unwrap_or([ - Float4::new(1.0, 0.0, 0.0, 0.0), - Float4::new(0.0, 1.0, 0.0, 0.0), - Float4::new(0.0, 0.0, 1.0, 0.0), - ]), - t: self.t, + pub fn into_full(self) -> Option { + if let Some(m_inv) = Float4::invert_3x3(self.m) { + Some(XformFull { + m: self.m, + m_inv: m_inv, + t: self.t, + }) + } else { + None } } /// Faster but less precise version of `compute_full()`. #[inline] - pub fn into_full_fast(self) -> XformFull { - XformFull { - m: self.m, - m_inv: Float4::invert_3x3_fast(self.m).unwrap_or([ - Float4::new(1.0, 0.0, 0.0, 0.0), - Float4::new(0.0, 1.0, 0.0, 0.0), - Float4::new(0.0, 0.0, 1.0, 0.0), - ]), - t: self.t, + pub fn into_full_fast(self) -> Option { + if let Some(m_inv) = Float4::invert_3x3_fast(self.m) { + Some(XformFull { + m: self.m, + m_inv: m_inv, + t: self.t, + }) + } else { + None } } + + /// Composes two transforms together. + /// + /// The resulting transform is the same as doing `self` and then + /// `rhs` in sequence. + #[inline] + pub fn compose(&self, rhs: &Self) -> Self { + let (m, t) = Float4::affine_mul_affine(&self.m, self.t, &rhs.m, rhs.t); + Self { m: m, t: t } + } + + /// Composes two transforms together. + /// + /// Faster but less precise version. + #[inline] + pub fn compose_fast(&self, rhs: &Self) -> Self { + let (m, t) = Float4::affine_mul_affine_fast(&self.m, self.t, &rhs.m, rhs.t); + Self { m: m, t: t } + } } impl Default for Xform { @@ -123,16 +142,6 @@ impl Default for Xform { } } -// /// Multiply two matrices together -// impl Mul for Xform { -// type Output = Self; - -// #[inline] -// fn mul(self, rhs: Self) -> Self { -// Self(rhs.0 * self.0) -// } -// } - /// Multiply a matrix by a f32 impl Mul for Xform { type Output = Self; @@ -177,62 +186,48 @@ pub struct XformFull { //------------------------------------------------------------- -// #[cfg(test)] -// mod tests { -// use super::*; +#[cfg(test)] +mod tests { + use super::*; -// #[test] -// fn equality_test() { -// let a = Xform::new(); -// let b = Xform::new(); -// let c = -// Xform::new_from_values(1.1, 0.0, 0.0, 0.0, 0.0, 1.1, 0.0, 0.0, 0.0, 0.0, 1.1, 0.0); + #[test] + fn equality() { + let a = Xform::identity(); + let b = Xform::identity(); + let c = Xform::new(1.1, 0.0, 0.0, 0.0, 0.0, 1.1, 0.0, 0.0, 0.0, 0.0, 1.1, 0.0); -// assert_eq!(a, b); -// assert!(a != c); -// } + assert_eq!(a, b); + assert!(a != c); + } -// #[test] -// fn approximate_equality_test() { -// let a = Xform::new(); -// let b = Xform::new_from_values( -// 1.000001, 0.0, 0.0, 0.0, 0.0, 1.000001, 0.0, 0.0, 0.0, 0.0, 1.000001, 0.0, -// ); -// let c = Xform::new_from_values( -// 1.000003, 0.0, 0.0, 0.0, 0.0, 1.000003, 0.0, 0.0, 0.0, 0.0, 1.000003, 0.0, -// ); -// let d = Xform::new_from_values( -// -1.000001, 0.0, 0.0, 0.0, 0.0, -1.000001, 0.0, 0.0, 0.0, 0.0, -1.000001, 0.0, -// ); + #[test] + fn approximate_equality() { + let a = Xform::identity(); + let b = Xform::new( + 1.000001, 0.0, 0.0, 0.0, 1.000001, 0.0, 0.0, 0.0, 1.000001, 0.0, 0.0, 0.0, + ); + let c = Xform::new( + 1.000003, 0.0, 0.0, 0.0, 1.000003, 0.0, 0.0, 0.0, 1.000003, 0.0, 0.0, 0.0, + ); -// assert!(a.aprx_eq(b, 0.000001)); -// assert!(!a.aprx_eq(c, 0.000001)); -// assert!(!a.aprx_eq(d, 0.000001)); -// } + assert!(a.aprx_eq(b, 10)); + assert!(!a.aprx_eq(b, 6)); -// #[test] -// fn multiply_test() { -// let a = Xform::new_from_values( -// 1.0, 2.0, 2.0, 1.5, 3.0, 6.0, 7.0, 8.0, 9.0, 2.0, 11.0, 12.0, -// ); -// let b = Xform::new_from_values( -// 1.0, 5.0, 9.0, 13.0, 2.0, 6.0, 10.0, 14.0, 3.0, 7.0, 11.0, 15.0, -// ); -// let c = Xform::new_from_values( -// 97.0, 50.0, 136.0, 162.5, 110.0, 60.0, 156.0, 185.0, 123.0, 70.0, 176.0, 207.5, -// ); + assert!(a.aprx_eq(c, 27)); + assert!(!a.aprx_eq(c, 23)); + } -// assert_eq!(a * b, c); -// } + #[test] + fn compose() { + let a = Xform::new(1.0, 3.0, 9.0, 2.0, 6.0, 2.0, 2.0, 7.0, 11.0, 1.5, 8.0, 12.0); + let b = Xform::new( + 1.0, 2.0, 3.0, 5.0, 6.0, 7.0, 9.0, 10.0, 11.0, 13.0, 14.0, 15.0, + ); + let c = Xform::new( + 97.0, 110.0, 123.0, 50.0, 60.0, 70.0, 136.0, 156.0, 176.0, 162.5, 185.0, 207.5, + ); -// #[test] -// fn inverse_test() { -// let a = Xform::new_from_values( -// 1.0, 0.33, 0.0, -2.0, 0.0, 1.0, 0.0, 0.0, 2.1, 0.7, 1.3, 0.0, -// ); -// let b = a.inverse(); -// let c = Xform::new(); - -// assert!((dbg!(a * b)).aprx_eq(dbg!(c), 0.0000001)); -// } -// } + assert_eq!(a.compose(&b), c); + assert_eq!(a.compose_fast(&b), c); + } +}