RMath: implement transform composition.

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"

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"

sub_crates/rmath/src/lib.rs

@@ -4,6 +4,7 @@
 
 mod normal;
 mod point;
+mod utils;
 mod vector;
 pub mod wide4;
 mod xform;

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 =
+        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)
-            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();
+            .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 =
+        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)
-            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();
+            .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);

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 =
+        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)
-            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();
+            .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 =
+        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)
-            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();
+            .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);
     }

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));
+    }
+}

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 =
+        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)
-            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();
+            .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 =
+        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)
-            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();
+            .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);

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.a(), b.a(), max_ulps);
-        eq &= ulps_eq!(a.b(), b.b(), max_ulps = max_ulps);
+        eq &= ulps_eq(a.b(), b.b(), max_ulps);
-        eq &= ulps_eq!(a.c(), b.c(), max_ulps = max_ulps);
+        eq &= ulps_eq(a.c(), b.c(), max_ulps);
-        eq &= ulps_eq!(a.d(), b.d(), max_ulps = 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<f32> 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));
+    }
+}

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 {
+    pub fn into_full(self) -> Option<XformFull> {
-        XformFull {
+        if let Some(m_inv) = Float4::invert_3x3(self.m) {
+            Some(XformFull {
                 m: self.m,
-            m_inv: Float4::invert_3x3(self.m).unwrap_or([
+                m_inv: m_inv,
-                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,
+            })
+        } else {
+            None
         }
     }
 
     /// Faster but less precise version of `compute_full()`.
     #[inline]
-    pub fn into_full_fast(self) -> XformFull {
+    pub fn into_full_fast(self) -> Option<XformFull> {
-        XformFull {
+        if let Some(m_inv) = Float4::invert_3x3_fast(self.m) {
+            Some(XformFull {
                 m: self.m,
-            m_inv: Float4::invert_3x3_fast(self.m).unwrap_or([
+                m_inv: m_inv,
-                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,
+            })
+        } 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<f32> for Xform {
     type Output = Self;
@@ -177,62 +186,48 @@ pub struct XformFull {
 
 //-------------------------------------------------------------
 
-// #[cfg(test)]
+#[cfg(test)]
-// mod tests {
+mod tests {
-//     use super::*;
+    use super::*;
 
-//     #[test]
+    #[test]
-//     fn equality_test() {
+    fn equality() {
-//         let a = Xform::new();
+        let a = Xform::identity();
-//         let b = Xform::new();
+        let b = Xform::identity();
-//         let c =
+        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);
-//             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);
 
-//         assert_eq!(a, b);
+        assert_eq!(a, b);
-//         assert!(a != c);
+        assert!(a != c);
-//     }
+    }
 
-//     #[test]
+    #[test]
-//     fn approximate_equality_test() {
+    fn approximate_equality() {
-//         let a = Xform::new();
+        let a = Xform::identity();
-//         let b = Xform::new_from_values(
+        let b = Xform::new(
-//             1.000001, 0.0, 0.0, 0.0, 0.0, 1.000001, 0.0, 0.0, 0.0, 0.0, 1.000001, 0.0,
+            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_from_values(
+        let c = Xform::new(
-//             1.000003, 0.0, 0.0, 0.0, 0.0, 1.000003, 0.0, 0.0, 0.0, 0.0, 1.000003, 0.0,
+            1.000003, 0.0, 0.0, 0.0, 1.000003, 0.0, 0.0, 0.0, 1.000003, 0.0, 0.0, 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,
-//         );
 
-//         assert!(a.aprx_eq(b, 0.000001));
+        assert!(a.aprx_eq(b, 10));
-//         assert!(!a.aprx_eq(c, 0.000001));
+        assert!(!a.aprx_eq(b, 6));
-//         assert!(!a.aprx_eq(d, 0.000001));
-//     }
 
-//     #[test]
+        assert!(a.aprx_eq(c, 27));
-//     fn multiply_test() {
+        assert!(!a.aprx_eq(c, 23));
-//         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_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]
+        assert_eq!(a.compose(&b), c);
-//     fn inverse_test() {
+        assert_eq!(a.compose_fast(&b), c);
-//         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));
-//     }
-// }