diff --git a/agb/src/number.rs b/agb/src/number.rs index 497293eb..2e91c6a9 100644 --- a/agb/src/number.rs +++ b/agb/src/number.rs @@ -7,6 +7,13 @@ use core::{ }, }; +#[macro_export] +macro_rules! num { + ($value:literal) => {{ + $crate::number::Num::new_from_parts(agb_macros::num!($value)) + }}; +} + pub trait Number: Sized + Copy @@ -299,11 +306,37 @@ impl Num { } } -#[macro_export] -macro_rules! num { - ($value:literal) => {{ - $crate::number::Num::new_from_parts(agb_macros::num!($value)) - }}; +impl Num { + pub fn sqrt(self) -> Self { + assert_eq!(N % 2, 0, "N must be even to be able to square root"); + assert!(self.0 >= 0, "sqrt is only valid for positive numbers"); + let mut d = 1 << 30; + let mut x = self.0; + let mut c = 0; + + while d > self.0 { + d >>= 2; + } + + while d != 0 { + if x >= c + d { + x -= c + d; + c = (c >> 1) + d; + } else { + c >>= 1; + } + d >>= 2; + } + Self(c << (N / 2)) + } +} + +#[test_case] +fn sqrt(_gba: &mut crate::Gba) { + for x in 1..1024 { + let n: Num = Num::new(x * x); + assert_eq!(n.sqrt(), x.into()); + } } #[test_case] @@ -687,11 +720,15 @@ impl Vector2D> { self.x.abs() + self.y.abs() } + pub fn magnitude(self) -> Num { + self.magnitude_squared().sqrt() + } + // calculates the magnitude of a vector using the alpha max plus beta min // algorithm https://en.wikipedia.org/wiki/Alpha_max_plus_beta_min_algorithm // this has a maximum error of less than 4% of the true magnitude, probably // depending on the size of your fixed point approximation - pub fn magnitude(self) -> Num { + pub fn fast_magnitude(self) -> Num { let max = core::cmp::max(self.x, self.y); let min = core::cmp::min(self.x, self.y); @@ -701,6 +738,10 @@ impl Vector2D> { pub fn normalise(self) -> Self { self / self.magnitude() } + + pub fn fast_normalise(self) -> Self { + self / self.fast_magnitude() + } } #[test_case]