diff --git a/agb/src/number.rs b/agb/src/number.rs index 3db154f9..2e91c6a9 100644 --- a/agb/src/number.rs +++ b/agb/src/number.rs @@ -7,7 +7,12 @@ use core::{ }, }; -use crate::syscall; +#[macro_export] +macro_rules! num { + ($value:literal) => {{ + $crate::number::Num::new_from_parts(agb_macros::num!($value)) + }}; +} pub trait Number: Sized @@ -301,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] @@ -378,13 +409,6 @@ impl Num { } } -impl Num { - pub fn sqrt(self) -> Self { - assert_eq!(N % 2, 0, "N must be even to be able to square root"); - Self(syscall::sqrt(self.0) << (N / 2)) - } -} - #[test_case] fn test_numbers(_gba: &mut super::Gba) { // test addition @@ -699,9 +723,34 @@ impl Vector2D> { 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 fast_magnitude(self) -> Num { + let max = core::cmp::max(self.x, self.y); + let min = core::cmp::min(self.x, self.y); + + max * num!(0.960433870103) + min * num!(0.397824734759) + } + pub fn normalise(self) -> Self { self / self.magnitude() } + + pub fn fast_normalise(self) -> Self { + self / self.fast_magnitude() + } +} + +#[test_case] +fn magnitude_accuracy(_gba: &mut crate::Gba) { + let n: Vector2D> = (3, 4).into(); + assert!((n.magnitude() - 5).abs() < num!(0.1)); + + let n: Vector2D> = (3, 4).into(); + assert!((n.magnitude() - 5).abs() < num!(0.1)); } impl> From<(P, P)> for Vector2D {