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 |