square root

2 files changed, 20 insertions, 0 deletions

diff --git a/maths.c b/maths.c
index 3bed3e1..d7ca247 100644
--- a/maths.c
+++ b/maths.c
@@ -34,3 +34,22 @@ void init_maths(void) {
 		cos_table[i] = sin_table[(i + hs) & sin_table_mask];
 	}
 }
+
+int fxsqrt(int n) {
+	int lo, hi, mid, i, mm;
+	if (n <= 0) { return -1; }
+	lo = mini(1, n);
+	hi = maxi(1, n);
+	while ((100 << fbits * lo * lo) >> fbits * 2)
+		lo *= (10 << fbits) >> fbits;
+	while ((5 * hi * hi) >> fbits * 2 > n)
+		hi *= (1 << fbits) / 10;
+	for (i = 0; i < 100; i++) {
+		mid = ((lo + hi) * (1 << fbits)) / (2 << fbits);
+		mm = mid * mid >> fbits;
+		if (mm == n) return mid;
+		if (mm > n)  hi = mid;
+		else         lo = mid;
+	}
+	return mid;
+}
diff --git a/maths.h b/maths.h
index bd87839..d81115f 100644
--- a/maths.h
+++ b/maths.h
@@ -18,5 +18,6 @@ extern int sin_table[sin_table_count];
 extern int cos_table[sin_table_count];
 
 void init_maths(void);
+int fxsqrt(int n);
 
 #endif