rust/fpnum/src/lib.rs
changeset 14143 c27461e6a9eb
parent 14102 5d42204ac35e
child 14160 37c99587825d
--- a/rust/fpnum/src/lib.rs	Sat Nov 03 18:29:58 2018 +0100
+++ b/rust/fpnum/src/lib.rs	Sat Nov 03 21:09:01 2018 +0100
@@ -1,5 +1,6 @@
 use std::cmp;
 use std::ops;
+use std::ops::Shl;
 
 #[derive(Clone, Debug, Copy)]
 pub struct FPNum {
@@ -93,7 +94,10 @@
 
     #[inline]
     pub fn with_sign(&self, is_negative: bool) -> FPNum {
-        FPNum{ is_negative, ..*self}
+        FPNum {
+            is_negative,
+            ..*self
+        }
     }
 
     #[inline]
@@ -293,10 +297,18 @@
 
 #[macro_export]
 macro_rules! fp {
-    (-$n: tt / $d: tt) => { FPNum::new(-$n, $d) };
-    ($n: tt / $d: tt) => { FPNum::new($n, $d) };
-    (-$n: tt) => { FPNum::from(-$n) };
-    ($n: tt) => { FPNum::from($n) };
+    (-$n: tt / $d: tt) => {
+        FPNum::new(-$n, $d)
+    };
+    ($n: tt / $d: tt) => {
+        FPNum::new($n, $d)
+    };
+    (-$n: tt) => {
+        FPNum::from(-$n)
+    };
+    ($n: tt) => {
+        FPNum::from($n)
+    };
 }
 
 const LINEARIZE_TRESHOLD: u64 = 0x1_0000;
@@ -316,24 +328,30 @@
             x_is_negative: x.is_negative,
             y_is_negative: y.is_negative,
             x_value: x.value,
-            y_value: y.value
+            y_value: y.value,
         }
     }
 
     #[inline]
-    pub fn zero() -> FPPoint { FPPoint::new(fp!(0), fp!(0)) }
+    pub fn zero() -> FPPoint {
+        FPPoint::new(fp!(0), fp!(0))
+    }
 
     #[inline]
-    pub fn unit_x() -> FPPoint { FPPoint::new(fp!(1), fp!(0)) }
+    pub fn unit_x() -> FPPoint {
+        FPPoint::new(fp!(1), fp!(0))
+    }
 
     #[inline]
-    pub fn unit_y() -> FPPoint { FPPoint::new(fp!(0), fp!(1)) }
+    pub fn unit_y() -> FPPoint {
+        FPPoint::new(fp!(0), fp!(1))
+    }
 
     #[inline]
     pub fn x(&self) -> FPNum {
         FPNum {
             is_negative: self.x_is_negative,
-            value: self.x_value
+            value: self.x_value,
         }
     }
 
@@ -341,7 +359,7 @@
     pub fn y(&self) -> FPNum {
         FPNum {
             is_negative: self.y_is_negative,
-            value: self.y_value
+            value: self.y_value,
         }
     }
 
@@ -361,7 +379,26 @@
         if r < LINEARIZE_TRESHOLD {
             FPNum::from(r as u32)
         } else {
-            self.sqr_distance().sqrt()
+            let mut sqr: u128 = (self.x_value as u128).pow(2) + (self.y_value as u128).pow(2);
+
+            let mut t: u128 = 0x40000000_00000000_00000000_00000000;
+            let mut r: u128 = 0;
+
+            for _ in 0..64 {
+                let s = r + t;
+                r >>= 1;
+
+                if s <= sqr {
+                    sqr -= s;
+                    r += t;
+                }
+                t >>= 2;
+            }
+
+            FPNum {
+                is_negative: false,
+                value: r as u64,
+            }
         }
     }
 
@@ -401,11 +438,10 @@
 
             #[inline]
             fn $name(self, rhs: Self) -> Self {
-                Self::new(self.x().$name(rhs.x()),
-                          self.y().$name(rhs.y()))
+                Self::new(self.x().$name(rhs.x()), self.y().$name(rhs.y()))
             }
         }
-    }
+    };
 }
 
 macro_rules! right_scalar_bin_op_impl {
@@ -477,15 +513,30 @@
 bin_assign_op_impl!(FPPoint, ops::MulAssign<FPNum>, mul_assign, *);
 bin_assign_op_impl!(FPPoint, ops::DivAssign<FPNum>, div_assign, /);
 
-#[inline]
 pub fn distance<T>(x: T, y: T) -> FPNum
-    where T: ops::Add + ops::Mul + Copy +
-             From<<T as ops::Add>::Output> +
-             From<<T as ops::Mul>::Output> +
-             Into<FPNum>
+where
+    T: Into<i64> + std::fmt::Debug,
 {
-    let sqr: FPNum = T::from(T::from(x * x) + T::from(y * y)).into();
-    sqr.sqrt()
+    let mut sqr: u128 = (x.into().pow(2) as u128).shl(64) + (y.into().pow(2) as u128).shl(64);
+
+    let mut t: u128 = 0x40000000_00000000_00000000_00000000;
+    let mut r: u128 = 0;
+
+    for _ in 0..64 {
+        let s = r + t;
+        r >>= 1;
+
+        if s <= sqr {
+            sqr -= s;
+            r += t;
+        }
+        t >>= 2;
+    }
+
+    FPNum {
+        is_negative: false,
+        value: r as u64,
+    }
 }
 
 /* TODO:
@@ -496,7 +547,7 @@
 #[cfg(test)]
 #[test]
 fn basics() {
-    let n = fp!(15/2);
+    let n = fp!(15 / 2);
     assert!(n.is_positive());
     assert!(!n.is_negative());
 
@@ -505,7 +556,7 @@
 
     assert_eq!(-(-n), n);
     assert_eq!((-n).abs(), n);
-    assert_eq!(-n, fp!(-15/2));
+    assert_eq!(-n, fp!(-15 / 2));
 
     assert_eq!(n.round(), 7);
     assert_eq!((-n).round(), -7);
@@ -514,7 +565,7 @@
 #[test]
 fn zero() {
     let z = fp!(0);
-    let n = fp!(15/2);
+    let n = fp!(15 / 2);
 
     assert!(z.is_zero());
     assert!(z.is_positive());
@@ -527,8 +578,8 @@
 #[test]
 fn ord() {
     let z = fp!(0);
-    let n1_5 = fp!(3/2);
-    let n2_25 = fp!(9/4);
+    let n1_5 = fp!(3 / 2);
+    let n2_25 = fp!(9 / 4);
 
     assert!(!(z > z));
     assert!(!(z < z));
@@ -539,9 +590,9 @@
 
 #[test]
 fn arith() {
-    let n1_5 = fp!(3/2);
-    let n2_25 = fp!(9/4);
-    let n_0_15 = fp!(-15/100);
+    let n1_5 = fp!(3 / 2);
+    let n2_25 = fp!(9 / 4);
+    let n_0_15 = fp!(-15 / 100);
 
     assert_eq!(n1_5 + n1_5, fp!(3));
     assert_eq!(-n1_5 - n1_5, fp!(-3));
@@ -563,13 +614,22 @@
 
     let mut m = fp!(1);
     m += n1_5;
-    assert_eq!(m, fp!(5/2));
+    assert_eq!(m, fp!(5 / 2));
+}
+
+#[test]
+fn test_distance_high_values() {
+    assert_eq!(distance(1_000_000i32, 0), fp!(1_000_000));
+    assert_eq!(
+        FPPoint::new(fp!(1_000_000), fp!(0)).distance(),
+        fp!(1_000_000)
+    );
 }
 
 #[test]
 fn point() {
     let z = FPPoint::zero();
-    let n = fp!(16/9);
+    let n = fp!(16 / 9);
     let p = FPPoint::new(fp!(1), fp!(-2));
 
     assert_eq!(p.sqr_distance(), fp!(5));
@@ -580,4 +640,4 @@
     assert_eq!(distance(4, 3), fp!(5));
     assert_eq!(p * fp!(-3), FPPoint::new(fp!(-3), fp!(6)));
     assert_eq!(p.max_norm(), fp!(2));
-}
\ No newline at end of file
+}