1 use std::cmp;

     2 use std::ops;

     3 

     4 #[derive(Clone, Debug, Copy)]

     5 pub struct FPNum {

     6     is_negative: bool,

     7     value: u64,

     8 }

     9 

    10 impl FPNum {

    11     fn new(numerator: i32, denominator: u32) -> Self {

    12         FPNum::from(numerator) / denominator

    13     }

    14 

    15     fn signum(&self) -> i8 {

    16         if self.is_negative {

    17             -1

    18         } else {

    19             1

    20         }

    21     }

    22 

    23     fn is_negative(&self) -> bool {

    24         self.is_negative

    25     }

    26 

    27     fn is_positive(&self) -> bool {

    28         !self.is_negative

    29     }

    30 

    31     fn is_zero(&self) -> bool {

    32         self.value == 0

    33     }

    34 

    35     fn abs(&self) -> Self {

    36         Self {

    37             is_negative: false,

    38             value: self.value,

    39         }

    40     }

    41 

    42     fn round(&self) -> i64 {

    43         if self.is_negative {

    44             -((self.value >> 32) as i64)

    45         } else {

    46             (self.value >> 32) as i64

    47         }

    48     }

    49 

    50     fn sqr(&self) -> Self {

    51         Self {

    52             is_negative: false,

    53             value: ((self.value as u128).pow(2) >> 32) as u64,

    54         }

    55     }

    56 

    57     fn sqrt(&self) -> Self {

    58         debug_assert!(!self.is_negative);

    59 

    60         let mut t: u64 = 0x4000000000000000;

    61         let mut r: u64 = 0;

    62         let mut q = self.value;

    63 

    64         for _ in 0..32 {

    65             let s = r + t;

    66             r >>= 1;

    67 

    68             if s <= q {

    69                 q -= s;

    70                 r += t;

    71             }

    72             t >>= 2;

    73         }

    74 

    75         Self {

    76             is_negative: false,

    77             value: r << 16,

    78         }

    79     }

    80 }

    81 

    82 impl From<i32> for FPNum {

    83     #[inline]

    84     fn from(n: i32) -> Self {

    85         FPNum {

    86             is_negative: n < 0,

    87             value: (n.abs() as u64) << 32,

    88         }

    89     }

    90 }

    91 

    92 impl From<u32> for FPNum {

    93     #[inline]

    94     fn from(n: u32) -> Self {

    95         Self {

    96             is_negative: false,

    97             value: (n as u64) << 32,

    98         }

    99     }

   100 }

   101 

   102 impl From<FPNum> for f64 {

   103     #[inline]

   104     fn from(n: FPNum) -> Self {

   105         if n.is_negative {

   106             n.value as f64 / (-0x10000000 as f64)

   107         } else {

   108             n.value as f64 / 0x10000000 as f64

   109         }

   110     }

   111 }

   112 

   113 impl PartialEq for FPNum {

   114     fn eq(&self, other: &Self) -> bool {

   115         self.value == other.value && (self.is_negative == other.is_negative || self.value == 0)

   116     }

   117 }

   118 

   119 impl Eq for FPNum {}

   120 

   121 impl PartialOrd for FPNum {

   122     fn partial_cmp(&self, rhs: &Self) -> std::option::Option<std::cmp::Ordering> {

   123         Some(self.cmp(rhs))

   124     }

   125 }

   126 

   127 impl Ord for FPNum {

   128     #[inline]

   129     fn cmp(&self, rhs: &Self) -> cmp::Ordering {

   130         if self.value == 0 && rhs.value == 0 {

   131             cmp::Ordering::Equal

   132         } else if self.is_negative != rhs.is_negative {

   133             if self.is_negative {

   134                 cmp::Ordering::Less

   135             } else {

   136                 cmp::Ordering::Greater

   137             }

   138         } else if self.value == rhs.value {

   139             cmp::Ordering::Equal

   140         } else if self.is_negative {

   141             if self.value > rhs.value {

   142                 cmp::Ordering::Less

   143             } else {

   144                 cmp::Ordering::Greater

   145             }

   146         } else {

   147             if self.value < rhs.value {

   148                 cmp::Ordering::Less

   149             } else {

   150                 cmp::Ordering::Greater

   151             }

   152         }

   153     }

   154 }

   155 

   156 impl ops::Add for FPNum {

   157     type Output = Self;

   158 

   159     #[inline]

   160     fn add(self, rhs: Self) -> Self {

   161         if self.is_negative == rhs.is_negative {

   162             Self {

   163                 is_negative: self.is_negative,

   164                 value: self.value + rhs.value,

   165             }

   166         } else if self.value > rhs.value {

   167             Self {

   168                 is_negative: self.is_negative,

   169                 value: self.value - rhs.value,

   170             }

   171         } else {

   172             Self {

   173                 is_negative: rhs.is_negative,

   174                 value: rhs.value - self.value,

   175             }

   176         }

   177     }

   178 }

   179 

   180 impl ops::Sub for FPNum {

   181     type Output = Self;

   182 

   183     #[inline]

   184     fn sub(self, rhs: Self) -> Self {

   185         if self.is_negative == rhs.is_negative {

   186             if self.value > rhs.value {

   187                 Self {

   188                     is_negative: self.is_negative,

   189                     value: self.value - rhs.value,

   190                 }

   191             } else {

   192                 Self {

   193                     is_negative: !rhs.is_negative,

   194                     value: rhs.value - self.value,

   195                 }

   196             }

   197         } else {

   198             Self {

   199                 is_negative: self.is_negative,

   200                 value: self.value + rhs.value,

   201             }

   202         }

   203     }

   204 }

   205 

   206 impl ops::Neg for FPNum {

   207     type Output = Self;

   208 

   209     #[inline]

   210     fn neg(self) -> Self {

   211         Self {

   212             is_negative: !self.is_negative,

   213             value: self.value,

   214         }

   215     }

   216 }

   217 

   218 impl ops::Mul for FPNum {

   219     type Output = Self;

   220 

   221     #[inline]

   222     fn mul(self, rhs: Self) -> Self {

   223         Self {

   224             is_negative: self.is_negative ^ rhs.is_negative,

   225             value: ((self.value as u128 * rhs.value as u128) >> 32) as u64,

   226         }

   227     }

   228 }

   229 

   230 impl ops::Mul<i32> for FPNum {

   231     type Output = Self;

   232 

   233     #[inline]

   234     fn mul(self, rhs: i32) -> Self {

   235         Self {

   236             is_negative: self.is_negative ^ (rhs < 0),

   237             value: self.value * rhs.abs() as u64,

   238         }

   239     }

   240 }

   241 

   242 impl ops::Div for FPNum {

   243     type Output = Self;

   244 

   245     #[inline]

   246     fn div(self, rhs: Self) -> Self {

   247         Self {

   248             is_negative: self.is_negative ^ rhs.is_negative,

   249             value: (((self.value as u128) << 32) / rhs.value as u128) as u64,

   250         }

   251     }

   252 }

   253 

   254 impl ops::Div<i32> for FPNum {

   255     type Output = Self;

   256 

   257     #[inline]

   258     fn div(self, rhs: i32) -> Self {

   259         Self {

   260             is_negative: self.is_negative ^ (rhs < 0),

   261             value: self.value / rhs.abs() as u64,

   262         }

   263     }

   264 }

   265 

   266 impl ops::Div<u32> for FPNum {

   267     type Output = Self;

   268 

   269     #[inline]

   270     fn div(self, rhs: u32) -> Self {

   271         Self {

   272             is_negative: self.is_negative,

   273             value: self.value / rhs as u64,

   274         }

   275     }

   276 }

   277 

   278 /* TODO:

   279  Distance

   280  DistanceI

   281  SignAs

   282  AngleSin

   283  AngleCos

   284 */

   285 

   286 #[cfg(test)]

   287 #[test]

   288 fn basics() {

   289     let n = FPNum::new(15, 2);

   290     assert!(n.is_positive());

   291     assert!(!n.is_negative());

   292 

   293     assert!(!(-n).is_positive());

   294     assert!((-n).is_negative());

   295 

   296     assert_eq!(-(-n), n);

   297     assert_eq!((-n).abs(), n);

   298     assert_eq!(-n, FPNum::new(-15, 2));

   299 

   300     assert_eq!(n.round(), 7);

   301     assert_eq!((-n).round(), -7);

   302 }

   303 

   304 #[test]

   305 fn zero() {

   306     let z = FPNum::from(0);

   307     let n = FPNum::new(15, 2);

   308 

   309     assert!(z.is_zero());

   310     assert!(z.is_positive());

   311     assert!((-z).is_negative);

   312     assert_eq!(n - n, z)

   313 }

   314 

   315 #[test]

   316 fn arith() {

   317     let n1_5 = FPNum::new(3, 2);

   318     let n2_25 = FPNum::new(9, 4);

   319 

   320     assert_eq!(n1_5 + n1_5, FPNum::from(3));

   321     assert_eq!(-n1_5 - n1_5, FPNum::from(-3));

   322 

   323     assert_eq!(n1_5 * n1_5, n2_25);

   324     assert_eq!(-n1_5 * -n1_5, n2_25);

   325     assert_eq!(n1_5 * -n1_5, -n2_25);

   326     assert_eq!(-n1_5 * n1_5, -n2_25);

   327 

   328     assert_eq!(n1_5.sqr(), n2_25);

   329     assert_eq!((-n1_5).sqr(), n2_25);

   330 

   331     assert_eq!(n2_25.sqrt(), n1_5);

   332 

   333     assert_eq!((n1_5 * n1_5 * n1_5.sqr()).sqrt(), n2_25);

   334 }