Restunts repository - Git mirror

[llm]

cleanuped version of the math.h usage with some tests

Code Select Expand

#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#define _USE_MATH_DEFINES
#include <math.h>
#include "stunts_math.hpp"

namespace math_h_test
{
#define SCALE_BITS 14
#define SCALE (1 << SCALE_BITS)

// 0x400 steps = 2Pi
#define ANGLE_TO_RAD(a) ((a) * (M_PI / 512.0))
#define RAD_TO_ANGLE(r) ((r) * (512.0 / M_PI))

#define TO_FIXED(x)   ((int16_t)lround((x) * SCALE))
#define FROM_FIXED(x) ((double)(x) / SCALE)

    int16_t int_sin_math(uint16_t angle)
    {
        return TO_FIXED(sin(ANGLE_TO_RAD(angle)));
    }

    int16_t int_cos_math(uint16_t angle)
    {
        return TO_FIXED(cos(ANGLE_TO_RAD(angle)));
    }

    int16_t int_atan2_math(int16_t x, int16_t y)
    {
        double ang = atan2((double)x, (double)y);
        return (int16_t)lround(RAD_TO_ANGLE(ang));
    }

    int16_t int_hypot_math(int16_t x, int16_t y)
    {
        return TO_FIXED(hypot(FROM_FIXED(x), FROM_FIXED(y)));
    }

    int16_t int_hypot_3d_math(const VECTOR* v)
    {
        double dx = FROM_FIXED(v->x);
        double dy = FROM_FIXED(v->y);
        double dz = FROM_FIXED(v->z);
        return TO_FIXED(sqrt(dx * dx + dy * dy + dz * dz));
    }

    void test_sin_cos(void)
    {
        printf("=== SIN/COS compare ===\n");
        int max_diff_sin = 0, max_diff_cos = 0;

        for (int a = -0x400; a <= 0x400; ++a) {
            int16_t ref_sin = int_sin((uint16_t)a);
            int16_t ref_cos = int_cos((uint16_t)a);
            int16_t new_sin = int_sin_math((uint16_t)a);
            int16_t new_cos = int_cos_math((uint16_t)a);

            int diff_sin = abs(ref_sin - new_sin);
            assert(diff_sin == 0);

            int diff_cos = abs(ref_cos - new_cos);
            assert(diff_cos == 0);
        }
    }

    void test_atan2(void)
    {
        printf("=== ATAN2 Compare ===\n");
        int max_diff = 0;

        for (int y = -0x400; y <= 0x400; y += 64) {
            for (int x = -0x400; x <= 0x400; x += 64) {
                int16_t ref = int_atan2(x, y);
                int16_t newv = int_atan2_math(x, y);
                int diff = abs(ref - newv);

                assert(diff <= 1);
            }
        }
    }

    void test_hypot(void)
    {
        printf("=== HYPOT Compare ===\n");
        int max_diff = 0;

        for (int y = -0x400; y <= 0x400; y += 64) {
            for (int x = -0x400; x <= 0x400; x += 64) {
                int16_t ref = int_hypot(x, y);
                int16_t newv = int_hypot_math(x, y);
                int diff = abs(ref - newv);

                assert(diff <= 3);

                if (diff > 1)
                    printf("hypot(%4d,%4d): ref=%5d new=%5d diff=%3d\n",
                        x, y, ref, newv, diff);
            }
        }
    }

    void test_hypot3d(void)
    {
        printf("=== HYPOT_3D Compare ===\n");
        int max_diff = 0;
        VECTOR v;

        for (int z = -0x200; z <= 0x200; z += 64)
            for (int y = -0x200; y <= 0x200; y += 64)
                for (int x = -0x200; x <= 0x200; x += 64) {
                    v.x = x; v.y = y; v.z = z;
                    int16_t ref = int_hypot_3d(&v);
                    int16_t newv = int_hypot_3d_math(&v);
                    int diff = abs(ref - newv);

                    assert(diff <= 4);

                    if (diff > 1)
                        printf("hypot3d(%4d,%4d,%4d): ref=%5d new=%5d diff=%3d\n",
                            x, y, z, ref, newv, diff);
                }
    }

    int main(void)
    {
        test_sin_cos();
        test_atan2();
        test_hypot();
        test_hypot3d();
        return 0;
    }
}