Add some maths functions

2 files changed, 31 insertions, 0 deletions

diff --git a/maths.c b/maths.c
index 74d682d..f6f1912 100644
--- a/maths.c
+++ b/maths.c
@@ -64,6 +64,15 @@ void init_maths(void) {
 	mtx_cpy(mtx_stack[0], m);
 }
 
+int fpow(int a, int p) {
+	int r = a;
+	p--;
+	while (p--) {
+		r = (r * a) >> fbits;
+	}
+	return r;
+}
+
 int* mtx_iden(int* m) {
 	register int o = f1;
 	m[0]  = o;
@@ -228,6 +237,23 @@ int* vec_cpy(int* d, const int* s, int c) {
 	return d;
 }
 
+int* vec_ref(int* d, const int* i, const int* n, int c) {
+	int r, j;
+	int dp = 2 * vec_dot(n, i, c);
+	for (j = r = 0; j < c; j++) {
+		d[j] = i[j] - ((dp * n[j]) >> fbits);
+	}
+	return d;
+}
+
+int vec_dot(const int* a, const int* b, int d) {
+	int r, i;
+	for (i = r = 0; i < d; i++) {
+		r += a[i] * b[i] >> fbits;
+	}
+	return r;
+}
+
 void 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 e63878c..6d33c6f 100644
--- a/maths.h
+++ b/maths.h
@@ -4,6 +4,7 @@
 #define mini(a_, b_) ((a_) < (b_) ? (a_) : (b_))
 #define maxi(a_, b_) ((a_) > (b_) ? (a_) : (b_))
 #define clamp(v_, min_, max_) (maxi(min_, mini(max_, v_)))
+#define absolute(v_) ((v_) < 0? -(v_): (v_))
 
 #define fbits 9
 #define f1 (1 << fbits)
@@ -19,6 +20,8 @@ extern int sqrt_table[sqrt_table_count];
 
 void init_maths(void);
 
+int fpow(int a, int p);
+
 int* mtx_iden(int* m);
 int* mtx_cpy(int* d, const int* s);
 int* mtx_mul(int* d, const int* a, const int* b);
@@ -36,6 +39,8 @@ int* mtx_rot_y(int* d, int a);
 int* mtx_rot_z(int* d, int a);
 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);
 void persp(int* v, int asp);
 void ndc2clip(int* c, int* p);