Square root approx function and normalise.

2 files changed, 31 insertions, 0 deletions

diff --git a/maths.c b/maths.c
index 861643a..706c12b 100644
--- a/maths.c
+++ b/maths.c
@@ -73,6 +73,25 @@ int fpow(int a, int p) {
 	return r;
 }
 
+int fsqrt(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;
+}
+
 int* mtx_iden(int* m) {
 	register int o = f1;
 	m[0]  = o;
@@ -254,6 +273,15 @@ int vec_dot(const int* a, const int* b, int d) {
 	return r;
 }
 
+int* vec_nrm(int* d, const int* a, int c) {
+	int l = vec_dot(a, a, c), i;
+	l = fsqrt(l);
+	for (i = 0; i < c; i++) {
+		d[i] = (a[i] << fbits) / l;
+	}
+	return d;
+}
+
 int* persp(int* v, int asp) {
 	v[2] += !v[2];
 	v[0] = ((v[0] << fbits) / v[2]);
diff --git a/maths.h b/maths.h
index af2a027..e72f8fd 100644
--- a/maths.h
+++ b/maths.h
@@ -21,6 +21,7 @@ extern int sqrt_table[sqrt_table_count];
 void init_maths(void);
 
 int fpow(int a, int p);
+int fsqrt(int a);
 
 int* mtx_iden(int* m);
 int* mtx_cpy(int* d, const int* s);
@@ -41,6 +42,8 @@ int* mtx_apply(const int* m, int* v);
 int* vec_cpy(int* d, const int* s, int c);
 int* vec_ref(int* d, const int* i, const int* n, int c);
 int vec_dot(const int* a, const int* b, int d);
+int* vec_nrm(int* d, const int* a, int c);
+int* vec_minise(int* d, const int* a, int c);
 int* persp(int* v, int asp);
 int* ndc2clip(const int* c, int* p);