From 9a25274bb03340704ac4c36145ac7c3df812716f Mon Sep 17 00:00:00 2001
From: Corwin Kuiper <corwin@kuiper.dev>
Date: Mon, 6 Dec 2021 19:49:27 +0000
Subject: [PATCH 1/4] switched the magnitude function to use the alpha max +
 beta min algorithm

---
 agb/src/number.rs | 17 +++++++++--------
 1 file changed, 9 insertions(+), 8 deletions(-)

diff --git a/agb/src/number.rs b/agb/src/number.rs
index 3db154f9..0914a2fa 100644
--- a/agb/src/number.rs
+++ b/agb/src/number.rs
@@ -378,13 +378,6 @@ impl<I: FixedWidthSignedInteger, const N: usize> Num<I, N> {
     }
 }
 
-impl<const N: usize> Num<i32, N> {
-    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
@@ -696,9 +689,17 @@ impl<const N: usize> Vector2D<Num<i32, N>> {
         self.x.abs() + self.y.abs()
     }
 
+    // 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<i32, N> {
-        self.magnitude_squared().sqrt()
+        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()
     }

From affa27c8e0fdf262bde854613eb4f6d69332dc9d Mon Sep 17 00:00:00 2001
From: Corwin Kuiper <corwin@kuiper.dev>
Date: Mon, 6 Dec 2021 22:43:58 +0000
Subject: [PATCH 2/4] remove unused crate

---
 agb/src/number.rs | 2 --
 1 file changed, 2 deletions(-)

diff --git a/agb/src/number.rs b/agb/src/number.rs
index 0914a2fa..6284e7dd 100644
--- a/agb/src/number.rs
+++ b/agb/src/number.rs
@@ -7,8 +7,6 @@ use core::{
     },
 };
 
-use crate::syscall;
-
 pub trait Number:
     Sized
     + Copy

From 3800be4dcb5cbb9906d1b05fd40727cbe7ad14ac Mon Sep 17 00:00:00 2001
From: Corwin Kuiper <corwin@kuiper.dev>
Date: Mon, 6 Dec 2021 22:51:43 +0000
Subject: [PATCH 3/4] test for accuracy of magnitude function

---
 agb/src/number.rs | 9 +++++++++
 1 file changed, 9 insertions(+)

diff --git a/agb/src/number.rs b/agb/src/number.rs
index 6284e7dd..497293eb 100644
--- a/agb/src/number.rs
+++ b/agb/src/number.rs
@@ -703,6 +703,15 @@ impl<const N: usize> Vector2D<Num<i32, N>> {
     }
 }
 
+#[test_case]
+fn magnitude_accuracy(_gba: &mut crate::Gba) {
+    let n: Vector2D<Num<i32, 16>> = (3, 4).into();
+    assert!((n.magnitude() - 5).abs() < num!(0.1));
+
+    let n: Vector2D<Num<i32, 8>> = (3, 4).into();
+    assert!((n.magnitude() - 5).abs() < num!(0.1));
+}
+
 impl<T: Number, P: Number + Into<T>> From<(P, P)> for Vector2D<T> {
     fn from(f: (P, P)) -> Self {
         Vector2D::new(f.0.into(), f.1.into())

From 38703c7cb8293331c0be99e0c46f2a391aa20b1c Mon Sep 17 00:00:00 2001
From: Corwin Kuiper <corwin@kuiper.dev>
Date: Mon, 6 Dec 2021 23:31:42 +0000
Subject: [PATCH 4/4] add different implementation of sqrt

---
 agb/src/number.rs | 53 +++++++++++++++++++++++++++++++++++++++++------
 1 file changed, 47 insertions(+), 6 deletions(-)

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<I: FixedWidthUnsignedInteger, const N: usize> Num<I, N> {
     }
 }
 
-#[macro_export]
-macro_rules! num {
-    ($value:literal) => {{
-        $crate::number::Num::new_from_parts(agb_macros::num!($value))
-    }};
+impl<const N: usize> Num<i32, N> {
+    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<i32, 8> = Num::new(x * x);
+        assert_eq!(n.sqrt(), x.into());
+    }
 }
 
 #[test_case]
@@ -687,11 +720,15 @@ impl<const N: usize> Vector2D<Num<i32, N>> {
         self.x.abs() + self.y.abs()
     }
 
+    pub fn magnitude(self) -> Num<i32, N> {
+        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<i32, N> {
+    pub fn fast_magnitude(self) -> Num<i32, N> {
         let max = core::cmp::max(self.x, self.y);
         let min = core::cmp::min(self.x, self.y);
 
@@ -701,6 +738,10 @@ impl<const N: usize> Vector2D<Num<i32, N>> {
     pub fn normalise(self) -> Self {
         self / self.magnitude()
     }
+
+    pub fn fast_normalise(self) -> Self {
+        self / self.fast_magnitude()
+    }
 }
 
 #[test_case]