rust/fpnum/src/lib.rs
changeset 15235 58a0f2a6527b
parent 15234 517f3a1dd5c2
child 15239 b2c086629fb8
equal deleted inserted replaced
15234:517f3a1dd5c2 15235:58a0f2a6527b
    71     }
    71     }
    72 
    72 
    73     pub fn sqrt(&self) -> Self {
    73     pub fn sqrt(&self) -> Self {
    74         debug_assert!(self.is_positive());
    74         debug_assert!(self.is_positive());
    75 
    75 
    76         let mut t: u64 = 0x4000_0000_0000_0000;
       
    77         let mut r: u64 = 0;
       
    78         let mut q = self.value;
       
    79 
       
    80         for _ in 0..32 {
       
    81             let s = r + t;
       
    82             r >>= 1;
       
    83 
       
    84             if s <= q {
       
    85                 q -= s;
       
    86                 r += t;
       
    87             }
       
    88             t >>= 2;
       
    89         }
       
    90 
       
    91         Self {
    76         Self {
    92             sign_mask: POSITIVE_MASK,
    77             sign_mask: POSITIVE_MASK,
    93             value: r << 16,
    78             value: integral_sqrt(self.value) << 16,
    94         }
    79         }
    95     }
    80     }
    96 
    81 
    97     #[inline]
    82     #[inline]
    98     pub fn with_sign(&self, is_negative: bool) -> FPNum {
    83     pub fn with_sign(&self, is_negative: bool) -> FPNum {
   352     pub fn distance(&self) -> FPNum {
   337     pub fn distance(&self) -> FPNum {
   353         let r = self.x_value | self.y_value;
   338         let r = self.x_value | self.y_value;
   354         if r < LINEARIZE_TRESHOLD {
   339         if r < LINEARIZE_TRESHOLD {
   355             FPNum::from(r as u32)
   340             FPNum::from(r as u32)
   356         } else {
   341         } else {
   357             let mut sqr: u128 = (self.x_value as u128).pow(2) + (self.y_value as u128).pow(2);
   342             let sqr: u128 = (self.x_value as u128).pow(2) + (self.y_value as u128).pow(2);
   358 
       
   359             let mut t: u128 = 0x40000000_00000000_00000000_00000000;
       
   360             let mut r: u128 = 0;
       
   361 
       
   362             for _ in 0..64 {
       
   363                 let s = r + t;
       
   364                 r >>= 1;
       
   365 
       
   366                 if s <= sqr {
       
   367                     sqr -= s;
       
   368                     r += t;
       
   369                 }
       
   370                 t >>= 2;
       
   371             }
       
   372 
   343 
   373             FPNum {
   344             FPNum {
   374                 sign_mask: POSITIVE_MASK,
   345                 sign_mask: POSITIVE_MASK,
   375                 value: r as u64,
   346                 value: integral_sqrt_ext(sqr) as u64,
   376             }
   347             }
   377         }
   348         }
   378     }
   349     }
   379 
   350 
   380     #[inline]
   351     #[inline]
   498 bin_assign_op_impl!(FPPoint, ops::AddAssign<FPNum>, add_assign, +);
   469 bin_assign_op_impl!(FPPoint, ops::AddAssign<FPNum>, add_assign, +);
   499 bin_assign_op_impl!(FPPoint, ops::SubAssign<FPNum>, sub_assign, -);
   470 bin_assign_op_impl!(FPPoint, ops::SubAssign<FPNum>, sub_assign, -);
   500 bin_assign_op_impl!(FPPoint, ops::MulAssign<FPNum>, mul_assign, *);
   471 bin_assign_op_impl!(FPPoint, ops::MulAssign<FPNum>, mul_assign, *);
   501 bin_assign_op_impl!(FPPoint, ops::DivAssign<FPNum>, div_assign, /);
   472 bin_assign_op_impl!(FPPoint, ops::DivAssign<FPNum>, div_assign, /);
   502 
   473 
       
   474 pub fn integral_sqrt(mut value: u64) -> u64 {
       
   475     let mut digit_sqr = 0x4000_0000_0000_0000u64.wrapping_shr(value.leading_zeros() & 0xFFFF_FFFE);
       
   476     let mut result = 0;
       
   477 
       
   478     while digit_sqr != 0 {
       
   479         let approx = digit_sqr + result;
       
   480         result >>= 1;
       
   481 
       
   482         if approx <= value {
       
   483             value -= approx;
       
   484             result += digit_sqr;
       
   485         }
       
   486         digit_sqr >>= 2;
       
   487     }
       
   488     result
       
   489 }
       
   490 
       
   491 pub fn integral_sqrt_ext(mut value: u128) -> u128 {
       
   492     let mut digit_sqr =
       
   493         0x40000000_00000000_00000000_00000000u128.wrapping_shr(value.leading_zeros() & 0xFFFF_FFFE);
       
   494     let mut result = 0u128;
       
   495 
       
   496     while digit_sqr != 0 {
       
   497         let approx = result + digit_sqr;
       
   498         result >>= 1;
       
   499 
       
   500         if approx <= value {
       
   501             value -= approx;
       
   502             result += digit_sqr;
       
   503         }
       
   504         digit_sqr >>= 2;
       
   505     }
       
   506     result
       
   507 }
       
   508 
   503 pub fn distance<T>(x: T, y: T) -> FPNum
   509 pub fn distance<T>(x: T, y: T) -> FPNum
   504 where
   510 where
   505     T: Into<i64> + std::fmt::Debug,
   511     T: Into<i64> + std::fmt::Debug,
   506 {
   512 {
   507     let mut sqr: u128 = (x.into().pow(2) as u128).shl(64) + (y.into().pow(2) as u128).shl(64);
   513     let sqr: u128 = (x.into().pow(2) as u128).shl(64) + (y.into().pow(2) as u128).shl(64);
   508 
       
   509     let mut t: u128 = 0x40000000_00000000_00000000_00000000;
       
   510     let mut r: u128 = 0;
       
   511 
       
   512     for _ in 0..64 {
       
   513         let s = r + t;
       
   514         r >>= 1;
       
   515 
       
   516         if s <= sqr {
       
   517             sqr -= s;
       
   518             r += t;
       
   519         }
       
   520         t >>= 2;
       
   521     }
       
   522 
   514 
   523     FPNum {
   515     FPNum {
   524         sign_mask: POSITIVE_MASK,
   516         sign_mask: POSITIVE_MASK,
   525         value: r as u64,
   517         value: integral_sqrt_ext(sqr) as u64,
   526     }
   518     }
   527 }
   519 }
   528 
   520 
   529 /* TODO:
   521 /* TODO:
   530  AngleSin
   522  AngleSin