From 6d7fe7392ae331d6cbd1bca1d6c6a00593621895 Mon Sep 17 00:00:00 2001 From: veclavtlica Date: Fri, 11 Sep 2026 15:09:47 +0300 Subject: [PATCH] fixed cos sin --- .gitignore | 4 ++ Common/bits.h | 1 - Common/fixed.c | 65 ++++++++++++++++++++- Common/fixed.h | 64 +++++++++++++++++++- Common/mat.h | 50 ++++++++++++++++ Common/vec.h | 6 ++ Helpers/fixedgen.out | 135 +++++++++++++++++++++++++++++++++++++++++++ Helpers/fixedgen.py | 19 ++++++ Player/Display/x11.c | 5 ++ Player/Makefile | 2 +- 10 files changed, 345 insertions(+), 6 deletions(-) create mode 100644 Helpers/fixedgen.out create mode 100755 Helpers/fixedgen.py diff --git a/.gitignore b/.gitignore index 17f0620..fb0a831 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,8 @@ **/*.o +**/*.err +**/*.EXE +**/*.exe *.sublime-* Player/player +Master/master diff --git a/Common/bits.h b/Common/bits.h index 152dc2c..8ba5c87 100644 --- a/Common/bits.h +++ b/Common/bits.h @@ -9,7 +9,6 @@ /* Alternitive semntic: nearest upper power of two */ static inline int com_bits_needed(uint32_t v) { #ifdef COM_DEF_COMPILE_MODERN - /* Use intrinsic, required for fast sqrt impl */ return v == 0 ? 1 : 32 - __builtin_clz(v); #else #endif diff --git a/Common/fixed.c b/Common/fixed.c index 315c383..872a226 100644 --- a/Common/fixed.c +++ b/Common/fixed.c @@ -1,8 +1,14 @@ +/* + Fixed point arithmetic. + https://github.com/howerj/q/blob/master/q.c +*/ + #include "fixed.h" -#include "bits.h" #include +#include #include #include +#include // 4-bit LUT (16 entries) for the normalized range [0.5, 2.0) // It stores the initial guess scaled to Q16.16. @@ -10,6 +16,22 @@ static const uint32_t com_fixed_sqrt_lut[16] = { 46340, 49547, 52521, 55314, 57954, 60464, 62862, 65161, 67373, 69508, 71572, 73572, 75514, 77402, 79240, 81033}; +/* Used for other quadrants as well as cosine eval, all from the same table. */ +/* Only fractional part is present, reducing the cache footprint. */ +const uint16_t com_fixed_sin_lut[128] = { + 0, 804, 1608, 2412, 3215, 4018, 4821, 5622, 6423, 7223, 8022, + 8819, 9616, 10410, 11204, 11995, 12785, 13573, 14359, 15142, 15923, 16702, + 17479, 18253, 19024, 19792, 20557, 21319, 22078, 22833, 23586, 24334, 25079, + 25820, 26557, 27291, 28020, 28745, 29465, 30181, 30893, 31600, 32302, 32999, + 33692, 34379, 35061, 35738, 36409, 37075, 37736, 38390, 39039, 39682, 40319, + 40950, 41575, 42194, 42806, 43412, 44011, 44603, 45189, 45768, 46340, 46906, + 47464, 48015, 48558, 49095, 49624, 50146, 50660, 51166, 51665, 52155, 52639, + 53114, 53581, 54040, 54491, 54933, 55368, 55794, 56212, 56621, 57022, 57414, + 57797, 58172, 58538, 58895, 59243, 59583, 59913, 60235, 60547, 60850, 61144, + 61429, 61705, 61971, 62228, 62475, 62714, 62942, 63162, 63371, 63571, 63762, + 63943, 64115, 64276, 64428, 64571, 64703, 64826, 64939, 65043, 65136, 65220, + 65294, 65358, 65412, 65457, 65491, 65516, 65531}; + com_fixed_t com_fixed_sqrt(com_fixed_t a) { // 1. Handle sign and edge cases assert(a >= 0); @@ -18,11 +40,11 @@ com_fixed_t com_fixed_sqrt(com_fixed_t a) { // 2. Normalize input to the range [0.5, 2.0) to maximize LUT precision // clz = count leading zeros. On modern hardware, use __builtin_clz - int leading_zeros = com_bits_needed(a); + uint32_t leading_zeros = __builtin_clz(a); // Calculate how much we need to shift to place the highest bit properly // We want the value to land squarely within an optimal window - int shift = (31 - leading_zeros) - COM_FIXED_FRACBITS; + int32_t shift = (31 - leading_zeros) - COM_FIXED_FRACBITS; // Normalize shift to always be even so we can cleanly pull out 2^(shift/2) if (shift & 1) @@ -69,3 +91,40 @@ void com_fixed_print(com_fixed_t a) { printf("%d.%04u\n", int_part, decimal_val); } + +void com_fixed_run_tests(void) { + com_fixed_print(com_fixed_sqrt(COM_FIXED_FRACUNIT * 2)); + com_fixed_print(com_fixed_sqrt(COM_FIXED_FRACUNIT * 3)); + com_fixed_print(com_fixed_sqrt(COM_FIXED_FRACUNIT * 4)); + com_fixed_print(com_fixed_sqrt(COM_FIXED_FRACUNIT * 200)); + com_fixed_print(com_fixed_sqrt(COM_FIXED_FRACUNIT * 1000)); + + com_fixed_print(COM_FIXED_PI); + com_fixed_print(com_fixed_div(COM_FIXED_PI + 234, COM_FIXED_PI2) << 2); + + double max_sin_deviation = 0.0f; + com_fixed_t sin_accumulator = -COM_FIXED_PI * 4; + com_fixed_t sin_test_step = (COM_FIXED_PI << 1) >> 10; + for (int i = 0; i < 1024 * 32; ++i) { + com_fixed_t sin = com_fixed_sin(sin_accumulator); + double deviation = fabs(sin(com_fixed_as_float(sin_accumulator)) - + com_fixed_as_float(sin)); + if (deviation > max_sin_deviation) + max_sin_deviation = deviation; + sin_accumulator += sin_test_step; + } + printf("max sin deviation: %f\n", max_sin_deviation); + + double max_cos_deviation = 0.0f; + com_fixed_t cos_accumulator = -COM_FIXED_PI * 4; + com_fixed_t cos_test_step = (COM_FIXED_PI << 1) >> 10; + for (int i = 0; i < 1024 * 32; ++i) { + com_fixed_t cos = com_fixed_cos(cos_accumulator); + double deviation = fabs(cos(com_fixed_as_float(cos_accumulator)) - + com_fixed_as_float(cos)); + if (deviation > max_cos_deviation) + max_cos_deviation = deviation; + cos_accumulator += cos_test_step; + } + printf("max cos deviation: %f\n", max_cos_deviation); +} diff --git a/Common/fixed.h b/Common/fixed.h index 6eb6c27..3937f79 100644 --- a/Common/fixed.h +++ b/Common/fixed.h @@ -7,12 +7,22 @@ #define COM_FIXED_H #include +#include #define COM_FIXED_FRACBITS 16 #define COM_FIXED_FRACUNIT (1 << COM_FIXED_FRACBITS) +#define COM_FIXED_FRACHALF (COM_FIXED_FRACUNIT >> 1) +#define COM_FIXED_FRACQRTR (COM_FIXED_FRACHALF >> 1) + +#define COM_FIXED_PI (com_fixed_t)205887 +#define COM_FIXED_PI2 (com_fixed_t)411774 +#define COM_FIXED_PIHALF (com_fixed_t)102943 +#define COM_FIXED_PIONEANDAHALF (COM_FIXED_PI + COM_FIXED_PIHALF) typedef int32_t com_fixed_t; +extern const uint16_t com_fixed_sin_lut[128]; + static inline com_fixed_t com_fixed_add(com_fixed_t a, com_fixed_t b) { return a + b; } @@ -30,9 +40,61 @@ static inline com_fixed_t com_fixed_div(com_fixed_t a, com_fixed_t b) { return (com_fixed_t)(((int64_t)a << COM_FIXED_FRACBITS) / (int64_t)b); } -/* Approximate square root using Newton-Raphson in 2 iterations over small LUT*/ +/* Is supposed to be only used for debugging, not in real code. */ +/* Because of that we default to maximum precision here. */ +static inline double com_fixed_as_float(com_fixed_t a) { + return (a >> COM_FIXED_FRACBITS) * (double)1.0 + + (a & 0xFFFF) * (double)(1.0 / COM_FIXED_FRACUNIT); +} + +/* Approximated square root by Newton-Raphson in 2 iterations over small LUT. */ com_fixed_t com_fixed_sqrt(com_fixed_t a); +/* Approximated using LUT. */ +/* TODO: test whether linearly interpolated version could be default. */ +/* things like camera movement can be jarring if done without.*/ +/* https://namoseley.wordpress.com/2015/07/26/sincos-generation-using-table-lookup-and-iterpolation/ + */ +static inline com_fixed_t com_fixed_sin(com_fixed_t a) { + int32_t sign = 1; + + /* Wrap to (-Pi2,+Pi2) */ + com_fixed_t q = a % COM_FIXED_PI2; + + /* Wrap to [0,+Pi2) */ + if (q < 0) + q = COM_FIXED_PI2 + q; + + /* Limit to [0,+1) */ + q = com_fixed_div(q, COM_FIXED_PI2); + + /* Handle cases of 3rd and 4th quadrant. */ + if (q >= COM_FIXED_FRACHALF) { + q -= COM_FIXED_FRACHALF; + sign = -1; + } + + /* Finally clculate the index into LUT. */ + uint16_t const idx = q >= COM_FIXED_FRACQRTR ? 255 - (q >> 7) : q >> 7; + + return com_fixed_sin_lut[idx] * sign; +} + +/* Implemented over sin, as to share one single LUT. */ +/* Additionally, optimizer probably can collapse some math for combined sincos + * case. */ +static inline com_fixed_t com_fixed_cos(com_fixed_t a) { + return com_fixed_sin(a + COM_FIXED_PIHALF); +} + +static inline void com_fixed_sincos(com_fixed_t a, com_fixed_t *restrict s, + com_fixed_t *restrict c) { + *s = com_fixed_sin(a); + *c = com_fixed_cos(a); +} + void com_fixed_print(com_fixed_t a); +void com_fixed_run_tests(void); + #endif diff --git a/Common/mat.h b/Common/mat.h index 33fd952..23098f5 100644 --- a/Common/mat.h +++ b/Common/mat.h @@ -134,4 +134,54 @@ static inline com_mat_t com_mat_reoder(com_mat_t a) { return result; } +/* TODO: move to .c file */ +/* Produces a view matrix needed for camera work. */ +static inline com_mat_t com_mat_look_at(com_vec_t pos, com_vec_t up, + com_vec_t target) { + com_vec_t const r = com_vec_nrm(com_vec_crs(target, up)); + com_vec_t const u = com_vec_crs(r, target); + + com_mat_t result; + + result.a[0] = r.s.x; + result.a[1] = u.s.x; + result.a[2] = -target.s.x; + result.a[3] = 0; + result.a[4] = r.s.y; + result.a[5] = u.s.y; + result.a[6] = -target.s.y; + result.a[7] = 0; + result.a[8] = r.s.z; + result.a[9] = u.s.z; + result.a[10] = -target.s.z; + result.a[11] = 0; + result.a[12] = -com_vec_dot(r, pos); + result.a[13] = -com_vec_dot(u, pos); + result.a[14] = com_vec_dot(target, pos); + result.a[15] = COM_FIXED_FRACUNIT; + + return result; +} + +/* TODO: move to .c file */ +/* Produces a projection matrix needed for camera work. */ +static inline com_mat_t com_mat_perspective(uint16_t rwidth, uint16_t rheight, + com_fixed_t nearz, com_fixed_t farz, + com_fixed_t fov) { + com_mat_t result = {0}; + + // com_fixed_t const aspect = com_fixed_div(rwidth, rheight); + + // const float f = 1.0f / tanf(camera->fov * 0.5f); + // const float fn = 1.0f / (CAMERA_NEAR_Z - camera->far_z); + + // result.row[0].x = f / aspect; + // result.row[1].y = f; + // result.row[2].z = (CAMERA_NEAR_Z + camera->far_z) * fn; + // result.row[2].w = -1.0f; + // result.row[3].z = 2.0f * CAMERA_NEAR_Z * camera->far_z * fn; + + // return result; +} + #endif diff --git a/Common/vec.h b/Common/vec.h index 5a9c906..12e4415 100644 --- a/Common/vec.h +++ b/Common/vec.h @@ -20,6 +20,12 @@ typedef union { } s; } com_vec_t; +static inline com_vec_t com_vec_identity(void) { + return (com_vec_t){.s = {.x = COM_FIXED_FRACUNIT, + .y = COM_FIXED_FRACUNIT, + .z = COM_FIXED_FRACUNIT}}; +} + static inline com_vec_t com_vec_add(com_vec_t a, com_vec_t b) { return (com_vec_t){.s = {.x = com_fixed_add(a.s.x, b.s.x), .y = com_fixed_add(a.s.y, b.s.y), diff --git a/Helpers/fixedgen.out b/Helpers/fixedgen.out new file mode 100644 index 0000000..237f9c2 --- /dev/null +++ b/Helpers/fixedgen.out @@ -0,0 +1,135 @@ +#define COM_FIXED_PI (com_fixed_t)205887 + +#define COM_FIXED_PI2 (com_fixed_t)411774 + +#define COM_FIXED_PIHALF (com_fixed_t)102943 + +static const uint16_t com_fixed_sin_lut[128] = { 0, +804, +1608, +2412, +3215, +4018, +4821, +5622, +6423, +7223, +8022, +8819, +9616, +10410, +11204, +11995, +12785, +13573, +14359, +15142, +15923, +16702, +17479, +18253, +19024, +19792, +20557, +21319, +22078, +22833, +23586, +24334, +25079, +25820, +26557, +27291, +28020, +28745, +29465, +30181, +30893, +31600, +32302, +32999, +33692, +34379, +35061, +35738, +36409, +37075, +37736, +38390, +39039, +39682, +40319, +40950, +41575, +42194, +42806, +43412, +44011, +44603, +45189, +45768, +46340, +46906, +47464, +48015, +48558, +49095, +49624, +50146, +50660, +51166, +51665, +52155, +52639, +53114, +53581, +54040, +54491, +54933, +55368, +55794, +56212, +56621, +57022, +57414, +57797, +58172, +58538, +58895, +59243, +59583, +59913, +60235, +60547, +60850, +61144, +61429, +61705, +61971, +62228, +62475, +62714, +62942, +63162, +63371, +63571, +63762, +63943, +64115, +64276, +64428, +64571, +64703, +64826, +64939, +65043, +65136, +65220, +65294, +65358, +65412, +65457, +65491, +65516, +65531 }; + diff --git a/Helpers/fixedgen.py b/Helpers/fixedgen.py new file mode 100755 index 0000000..cad0606 --- /dev/null +++ b/Helpers/fixedgen.py @@ -0,0 +1,19 @@ +#!/usr/bin/env python3 +# Generator for sin/cos fixed point LUT tables. +import math + +def generate_pi(): + print(f"#define COM_FIXED_PI (com_fixed_t){int(math.pi * (1 << 16))}\n") + print(f"#define COM_FIXED_PI2 (com_fixed_t){int(math.pi*2 * (1 << 16))}\n") + print(f"#define COM_FIXED_PIHALF (com_fixed_t){int(math.pi/2 * (1 << 16))}\n") + +# Idea is to generate only one quadrant, as the values repeat later with different sign and order. +def generate_sin_table(n_entries=128): + r = [] + for n in range(n_entries): + r += [math.sin(math.pi*n/(n_entries*2))] + # r[n_entries-1]=1 # Make it converge to unit precisely + print(f"static const uint16_t com_fixed_sin_lut[{n_entries}] =", "{", ",\n".join((str(int(a * (1 << 16))) for a in r)), "};\n") + +generate_pi() +generate_sin_table() diff --git a/Player/Display/x11.c b/Player/Display/x11.c index ab19606..48080d8 100644 --- a/Player/Display/x11.c +++ b/Player/Display/x11.c @@ -38,6 +38,11 @@ extern int plr_display_x11_main(int argc, char *argv[]) { com_fixed_print(t0.a[4]); com_fixed_print(t1.a[4]); + com_vec_t v0 = com_mat_vec_project(com_mat_reoder(m0), com_vec_identity()); + com_fixed_print(v0.a[0]); + + com_fixed_run_tests(); + Display *display = XOpenDisplay(NULL); if (NULL == display) { fprintf(stderr, "Failed to initialize display"); diff --git a/Player/Makefile b/Player/Makefile index 4c83639..e073f76 100644 --- a/Player/Makefile +++ b/Player/Makefile @@ -1,5 +1,5 @@ CC=clang -CFLAGS=-lX11 -Wall -std=c99 -g3 -O3 -fno-inline -msse2 +CFLAGS=-lX11 -Wall -std=c99 -g3 -O3 -fno-inline -msse2 -lm DEPS = Display/display.h ../Common/lzw.h ../Common/mat.h ../Common/vec.h ../Common/fixed.h ../Common/def.h ../Common/bits.h OBJ = ../Common/lzw.o ../Common/fixed.o LINUX = main.o Display/x11.o