math: Added fixed point sqrt and fp setters and getters

This should allow simple operations on fp vectors.

diff --git a/src/test.c b/src/test.c
index 842bad7b71c7ae51395c05ea5ae56c030ea33700..31c53c59c782ba10a15294fe35d864a0b74535f0 100644 (file)
--- a/src/test.c
+++ b/src/test.c
@@ -22,6 +22,8 @@ int main(int argc, char **argv) {
        T_TESTCASE("arena", t_test_arena);
        T_TESTCASE("tile-to-screen", t_test_tile_to_screen);
        T_TESTCASE("point-in-rect", t_test_pointinrect);
+       T_TESTCASE("fixed-point-macro", t_test_fp_macros);
+       T_TESTCASE("fixed-point-sqrt", t_test_fp_sqrt);
 
   T_TESTEND("lrts test");
 

diff --git a/src/tests/t_math.c b/src/tests/t_math.c
index 9772227be44ef8d68ffef284293f14b51648e86c..3e4e40613d7b00cb521788cc6c02036b9de7905f 100644 (file)
--- a/src/tests/t_math.c
+++ b/src/tests/t_math.c
@@ -20,3 +20,69 @@ int t_test_tile_to_screen(void) {
        T_ASSERT(res.x == 144 && res.y == 24, ("tile to screen %d/%d\n", res.x, res.y));
        return 0;
 }
+
+int t_test_fp_macros(void) {
+       i32 fp;
+       i32 r;
+
+       /* first number */
+       fp = u_fp_w(1, 2);
+       T_ASSERT(fp == 0x00000102, ("fp set positive: %x\n", fp));
+
+       r = u_fp_r_frac(fp);
+       T_ASSERT(r == 0x02, ("fp frac read: %x\n", r));
+
+       r = u_fp_r_int(fp);
+       T_ASSERT(r == 0x01, ("fp int read: %x\n", r));
+       
+       /* second number */
+       fp = u_fp_w(-1, 2);
+       T_ASSERT((u32)fp == 0xffffff02, ("fp set negative: %x\n", fp));
+       
+       r = u_fp_r_frac(fp);
+       T_ASSERT(r == 0x02, ("fp frac read: %x\n", r));
+
+       r = u_fp_r_int(fp);
+       T_ASSERT(r == -1, ("fp int read: %x\n", r));
+
+       /* third number */
+       fp = u_fp_w(-42, 255);
+       T_ASSERT((u32)fp == 0xffffd6ff, ("fp set negative: %x\n", fp));
+       
+       r = u_fp_r_frac(fp);
+       T_ASSERT(r == 0xff, ("fp frac read: %x\n", r));
+
+       r = u_fp_r_int(fp);
+       T_ASSERT(r == -42, ("fp int read: %x\n", r));
+
+       return 0;
+}
+
+int t_test_fp_sqrt(void) {
+       i32 f;
+       i32 n;
+       
+       /* first number */
+       f = u_fp_sqrt(u_fp_w(4, 0));
+       n = u_fp_r_int(f);
+       T_ASSERT(n == 2, ("int part: %d\n", n));
+       n = u_fp_r_frac(f);
+       T_ASSERT(n == 0, ("frac part: %d\n", n));
+       
+       /* second number */
+       f = u_fp_sqrt(u_fp_w(9, 0));
+       n = u_fp_r_int(f);
+       T_ASSERT(n == 3, ("int part: %d\n", n));
+       n = u_fp_r_frac(f);
+       T_ASSERT(n == 0, ("frac part: %d\n", n));
+
+       /* third number */
+       f = u_fp_sqrt(u_fp_w(5, 0));
+       n = u_fp_r_int(f);
+       T_ASSERT(n == 2, ("int part: %d\n", n));
+       n = u_fp_r_frac(f);
+       /* 60/255 ~= 0.235 */
+       T_ASSERT(n == 60, ("frac part: %d\n", n));
+       
+       return 0;
+}

diff --git a/src/u_math.c b/src/u_math.c
index 679ae4463614c9e69b0b542a48e9918176bd2fca..7e2b12225cabc2a6115eb269ca1e23b98999f861 100644 (file)
--- a/src/u_math.c
+++ b/src/u_math.c
@@ -1,51 +1,6 @@
 #include "u_math.h"
 #include "r_assets.h"
 
-struct u_fp u_fp_add(struct u_fp a, struct u_fp b) {
-       struct u_fp c = U_FP(0, 0);
-
-       LRTS_UNUSED(a);
-       LRTS_UNUSED(b);
-       lrts_todo("");
-       return c;
-}
-
-struct u_fp u_fp_sub(struct u_fp a, struct u_fp b) {
-       struct u_fp c = U_FP(0, 0);
-
-       LRTS_UNUSED(a);
-       LRTS_UNUSED(b);
-       lrts_todo("");
-       return c;
-}
-
-struct u_fp u_fp_mul(struct u_fp a, struct u_fp b) {
-       struct u_fp c = U_FP(0, 0);
-
-       LRTS_UNUSED(a);
-       LRTS_UNUSED(b);
-       lrts_todo("");
-       return c;
-}
-
-struct u_fp u_fp_div(struct u_fp a, struct u_fp b) {
-       struct u_fp c = U_FP(0, 0);
-
-       LRTS_UNUSED(a);
-       LRTS_UNUSED(b);
-       lrts_todo("");
-       return c;
-}
-
-struct u_fp u_fp_mod(struct u_fp a, struct u_fp b) {
-       struct u_fp c = U_FP(0, 0);
-
-       LRTS_UNUSED(a);
-       LRTS_UNUSED(b);
-       lrts_todo("");
-       return c;
-}
-
 struct u_vec2 u_vec2_mul(i32 n, struct u_vec2 v) {
        struct u_vec2 r;
 
@@ -68,3 +23,21 @@ lrts_bool u_point_in_rect(i32 x, i32 y, i32 rx, i32 ry, i32 rw, i32 rh) {
        return x >= rx && x <= rx + rw
                        && y >= ry && y <= ry + rh;
 }
+
+
+/* newton method for aproximating the sqrt */
+u_fp u_fp_sqrt(u_fp n) {
+       u32 x;
+       u64 n_next;
+       u64 x_prev;
+
+       x = 1 << U_FP_FRAC_BITS;
+       n_next = n << U_FP_FRAC_BITS;
+
+       do {
+               x_prev = x;
+               x = (x + n_next / x) / 2;
+       } while(x != x_prev);
+
+       return x;
+}

diff --git a/src/u_math.h b/src/u_math.h
index 4403907e1ae4106182d9762311ccb3db58914094..c299f559cee555db0057c7831840324062ca0083 100644 (file)
--- a/src/u_math.h
+++ b/src/u_math.h
@@ -3,20 +3,33 @@
 
 #include "u_defs.h"
 
-#define U_FP(n, f) {n, f}
-#define U_V2(x, y) {x, y}
-
 /* Clamps an integer value between min and max */
 #define U_CLAMP(v, min, max) if (v < min) { v = min; } if (v > max) { v = max; }
 
-/* fixed point number 
- * n: whole part
- * f: fractional part
+#define U_FP_FRAC_BITS 8
+
+/* gets fraction part of a fixed point int */
+#define u_fp_r_frac(n) (u32)n & 0xFF
+
+
+/* converts the fixed point to an integer. keeps sign biit */
+#define u_fp_r_int(n) ((u32)n & 0x80000000) \
+       | ((u32)n & 0x80000000 >> 1) \
+       | ((u32)n & 0x80000000 >> 2) \
+       | ((u32)n & 0x80000000 >> 3) \
+       | ((u32)n & 0x80000000 >> 4) \
+       | ((u32)n & 0x80000000 >> 5) \
+       | ((u32)n & 0x80000000 >> 6) \
+       | ((u32)n & 0x80000000 >> 7) \
+       | (((u32)n & 0xffffffff) >> U_FP_FRAC_BITS)
+
+/* writes a new value to a fixed point number */
+#define u_fp_w(n, f) ((((u32)n) & 0x80000000) | ((u32)n) << (U_FP_FRAC_BITS)) | (f & 0xFF)
+
+/* fixed point number
+ * s.23.8
  */
-struct u_fp {
-       u16 n;
-       u16 f;
-};
+typedef i32 u_fp;
 
 /*
  * A simple 2d vector
@@ -26,20 +39,6 @@ struct u_vec2 {
        i32 y;
 };
 
-/*
- * A fixed point vector
- */
-struct u_vecfp {
-       struct u_fp x;
-       struct u_fp y;
-};
-
-struct u_fp u_fp_add(struct u_fp a, struct u_fp b);
-struct u_fp u_fp_sub(struct u_fp a, struct u_fp b);
-struct u_fp u_fp_mul(struct u_fp a, struct u_fp b);
-struct u_fp u_fp_div(struct u_fp a, struct u_fp b);
-struct u_fp u_fp_mod(struct u_fp a, struct u_fp b);
-
 /*
  * Multiplies an integer with a vec2 reutnrs the resulting vector
  */
@@ -54,4 +53,6 @@ struct u_vec2 u_tile_to_screen(i32 x, i32 y);
 /* tests if a point is contained inside a rectangle */
 lrts_bool u_point_in_rect(i32 x, i32 y, i32 rx, i32 ry, i32 rw, i32 rh);
 
+u_fp u_fp_sqrt(u_fp n);
+
 #endif