math.h

Go to the documentation of this file.

/*
** Jo Sega Saturn Engine
** Copyright (c) 2012-2026, Johannes Fetz (johannesfetz@gmail.com)
** All rights reserved.
**
** Redistribution and use in source and binary forms, with or without
** modification, are permitted provided that the following conditions are met:
**     * Redistributions of source code must retain the above copyright
**       notice, this list of conditions and the following disclaimer.
**     * Redistributions in binary form must reproduce the above copyright
**       notice, this list of conditions and the following disclaimer in the
**       documentation and/or other materials provided with the distribution.
**     * Neither the name of the Johannes Fetz nor the
**       names of its contributors may be used to endorse or promote products
**       derived from this software without specific prior written permission.
**
** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
** ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
** WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
** DISCLAIMED. IN NO EVENT SHALL Johannes Fetz BE LIABLE FOR ANY
** DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
** (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
** LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
** ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
** (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
** SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
 
#ifndef __JO_MATH_H__
# define __JO_MATH_H__
 
/*
███╗   ███╗ █████╗ ████████╗██╗  ██╗     ██████╗ ██████╗ ███╗   ██╗███████╗████████╗███████╗
████╗ ████║██╔══██╗╚══██╔══╝██║  ██║    ██╔════╝██╔═══██╗████╗  ██║██╔════╝╚══██╔══╝██╔════╝
██╔████╔██║███████║   ██║   ███████║    ██║     ██║   ██║██╔██╗ ██║███████╗   ██║   ███████╗
██║╚██╔╝██║██╔══██║   ██║   ██╔══██║    ██║     ██║   ██║██║╚██╗██║╚════██║   ██║   ╚════██║
██║ ╚═╝ ██║██║  ██║   ██║   ██║  ██║    ╚██████╗╚██████╔╝██║ ╚████║███████║   ██║   ███████║
╚═╝     ╚═╝╚═╝  ╚═╝   ╚═╝   ╚═╝  ╚═╝     ╚═════╝ ╚═════╝ ╚═╝  ╚═══╝╚══════╝   ╚═╝   ╚══════╝
*/

# define JO_PI                      (3.1415927f)

# define JO_PI_2                    (1.5707963f)
 
/*
███╗   ███╗██╗   ██╗██╗  ████████╗ ██╗██████╗ ██╗██╗   ██╗
████╗ ████║██║   ██║██║  ╚══██╔══╝██╔╝██╔══██╗██║██║   ██║
██╔████╔██║██║   ██║██║     ██║  ██╔╝ ██║  ██║██║██║   ██║
██║╚██╔╝██║██║   ██║██║     ██║ ██╔╝  ██║  ██║██║╚██╗ ██╔╝
██║ ╚═╝ ██║╚██████╔╝███████╗██║██╔╝   ██████╔╝██║ ╚████╔╝
╚═╝     ╚═╝ ╚═════╝ ╚══════╝╚═╝╚═╝    ╚═════╝ ╚═╝  ╚═══╝
*/

# define JO_MULT_BY_2(X)            ((X) << 1)

# define JO_MULT_BY_4(X)            ((X) << 2)

# define JO_MULT_BY_8(X)            ((X) << 3)

# define JO_MULT_BY_16(X)           ((X) << 4)

# define JO_MULT_BY_32(X)           ((X) << 5)

# define JO_MULT_BY_64(X)           ((X) << 6)

# define JO_MULT_BY_128(X)          ((X) << 7)

# define JO_MULT_BY_256(X)          ((X) << 8)

# define JO_MULT_BY_512(X)          ((X) << 9)

# define JO_MULT_BY_1024(X)         ((X) << 10)

# define JO_MULT_BY_2048(X)         ((X) << 11)

# define JO_MULT_BY_4096(X)         ((X) << 12)

# define JO_MULT_BY_32768(X)            ((X) << 15)

# define JO_MULT_BY_65536(X)            ((X) << 16)

# define JO_DIV_BY_2(X)             ((X) >> 1)

# define JO_DIV_BY_4(X)             ((X) >> 2)

# define JO_DIV_BY_8(X)             ((X) >> 3)

# define JO_DIV_BY_16(X)            ((X) >> 4)

# define JO_DIV_BY_32(X)            ((X) >> 5)

# define JO_DIV_BY_64(X)            ((X) >> 6)

# define JO_DIV_BY_128(X)           ((X) >> 7)

# define JO_DIV_BY_256(X)           ((X) >> 8)

# define JO_DIV_BY_512(X)           ((X) >> 9)

# define JO_DIV_BY_1024(X)          ((X) >> 10)

# define JO_DIV_BY_32768(X)         ((X) >> 15)

# define JO_DIV_BY_65536(X)         ((X) >> 16)

# define JO_DIV_BY_2147483648(X)    ((X) >> 31)
 
/*
████████╗ ██████╗  ██████╗ ██╗     ███████╗
╚══██╔══╝██╔═══██╗██╔═══██╗██║     ██╔════╝
   ██║   ██║   ██║██║   ██║██║     ███████╗
   ██║   ██║   ██║██║   ██║██║     ╚════██║
   ██║   ╚██████╔╝╚██████╔╝███████╗███████║
   ╚═╝    ╚═════╝  ╚═════╝ ╚══════╝╚══════╝
*/

# define JO_ZERO(X)                 X ^= X

# define JO_MOD_POW2(N, M)          ((N) & ((M) - 1))

# define JO_IS_POW2(N)              ((N) != 0 && (((N) & ((N) - 1)) == 0))

# define JO_ABS(X)                  ((X) < 0 ? -(X) : (X))

# define JO_FABS(X)                 ((X) < 0.0f ? -(X) : (X))

# define JO_MIN(A, B)               (((A) < (B)) ? (A) : (B))

# define JO_MAX(A, B)               (((A) > (B)) ? (A) : (B))

# define JO_CHANGE_SIGN(X)          (-(X))

# define JO_FLOAT_EPSILON           (0.00001f)

# define JO_FLOAT_NEARLY_EQUALS(A, B)     ((A) <= ((B) + JO_FLOAT_EPSILON) && (A) >= ((B) - JO_FLOAT_EPSILON))

# define JO_IS_FLOAT_NULL(A)              ((A) <= JO_FLOAT_EPSILON && (A) >= (-JO_FLOAT_EPSILON))

# define JO_RAD_TO_DEG(A)                  ((A) * 57.295780f)

# define JO_DEG_TO_RAD(A)                  ((A) * 0.017453292f)

# define JO_IS_ODD(A)                       ((A) & 1)

# define JO_SWAP(A, B)                      { __typeof__((A) + 0) __jo_swap_tmp = (A); (A) = (B); (B) = __jo_swap_tmp; }

# define JO_OPAQUE(VAR)                     __asm__ ("" : "+r" (VAR))

# define JO_PERCENT_USED(TOTAL, FREE)       ((TOTAL) == 0 ? 0 : (int)((unsigned int)((TOTAL) - (FREE)) * 100u / (unsigned int)(TOTAL)))

# define JO_SQUARE(A)                       ((A) * (A))

# define JO_BCD_INT(BCD)                    (((BCD & 0xF0) >> 4) * 10 + (BCD & 0x0F))
 
/*
██████╗ ██╗   ██╗████████╗███████╗███████╗██╗███████╗██╗     ██████╗
██╔══██╗╚██╗ ██╔╝╚══██╔══╝██╔════╝██╔════╝██║██╔════╝██║     ██╔══██╗
██████╔╝ ╚████╔╝    ██║   █████╗  █████╗  ██║█████╗  ██║     ██║  ██║
██╔══██╗  ╚██╔╝     ██║   ██╔══╝  ██╔══╝  ██║██╔══╝  ██║     ██║  ██║
██████╔╝   ██║      ██║   ███████╗██║     ██║███████╗███████╗██████╔╝
╚═════╝    ╚═╝      ╚═╝   ╚══════╝╚═╝     ╚═╝╚══════╝╚══════╝╚═════╝
*/

# define JO_SET_FLAGS(BYTEFIELD, FLAGS)         BYTEFIELD = (FLAGS)

# define JO_SET_ALL_FLAGS(BYTEFIELD)            BYTEFIELD = (~0)

# define JO_ADD_FLAG(BYTEFIELD, FLAG)           BYTEFIELD |= (FLAG)

# define JO_REMOVE_FLAG(BYTEFIELD, FLAG)        BYTEFIELD &= ~(FLAG)

# define JO_REMOVE_ALL_FLAGS(BYTEFIELD)         BYTEFIELD = 0

# define JO_HAS_FLAG(BYTEFIELD, FLAG)           ((BYTEFIELD & FLAG) == FLAG)
 
/*
███████╗██╗██╗  ██╗███████╗██████╗      ██████╗ ██████╗ ███╗   ██╗██╗   ██╗███████╗██████╗ ████████╗██╗ ██████╗ ███╗   ██╗
██╔════╝██║╚██╗██╔╝██╔════╝██╔══██╗    ██╔════╝██╔═══██╗████╗  ██║██║   ██║██╔════╝██╔══██╗╚══██╔══╝██║██╔═══██╗████╗  ██║
█████╗  ██║ ╚███╔╝ █████╗  ██║  ██║    ██║     ██║   ██║██╔██╗ ██║██║   ██║█████╗  ██████╔╝   ██║   ██║██║   ██║██╔██╗ ██║
██╔══╝  ██║ ██╔██╗ ██╔══╝  ██║  ██║    ██║     ██║   ██║██║╚██╗██║╚██╗ ██╔╝██╔══╝  ██╔══██╗   ██║   ██║██║   ██║██║╚██╗██║
██║     ██║██╔╝ ██╗███████╗██████╔╝    ╚██████╗╚██████╔╝██║ ╚████║ ╚████╔╝ ███████╗██║  ██║   ██║   ██║╚██████╔╝██║ ╚████║
╚═╝     ╚═╝╚═╝  ╚═╝╚══════╝╚═════╝      ╚═════╝ ╚═════╝ ╚═╝  ╚═══╝  ╚═══╝  ╚══════╝╚═╝  ╚═╝   ╚═╝   ╚═╝ ╚═════╝ ╚═╝  ╚═══╝
 
 ▲ NOTE ABOUT FIXED NUMBER ▲
 Only values between -32767.99998 and 32767.99998 can be converted to fixed number.
 
*/

# define JO_FIXED_0                 (0)
# define JO_FIXED_1_DIV_2           (32768)
# define JO_FIXED_ALMOST_1          (65470)
# define JO_FIXED_1                 (65536)
# define JO_FIXED_2                 JO_INT_TO_FIXED(2)
# define JO_FIXED_3                 JO_INT_TO_FIXED(3)
# define JO_FIXED_4                 JO_INT_TO_FIXED(4)
# define JO_FIXED_8                 JO_INT_TO_FIXED(8)
# define JO_FIXED_16                JO_INT_TO_FIXED(16)
# define JO_FIXED_32                JO_INT_TO_FIXED(32)
# define JO_FIXED_120               JO_INT_TO_FIXED(120)
# define JO_FIXED_150               JO_INT_TO_FIXED(150)
# define JO_FIXED_180               JO_INT_TO_FIXED(180)
# define JO_FIXED_360               JO_INT_TO_FIXED(360)

# define JO_FIXED_MIN               (-2147483647)
# define JO_FIXED_MAX               (2147483647)
# define JO_FIXED_EPSILON           (1)
# define JO_FIXED_OVERFLOW          (0x80000000)
# define JO_FIXED_PI                (205887)
# define JO_FIXED_PI_2              (411775)
# define JO_FIXED_180_DIV_PI        (3754936)
# define JO_FIXED_PI_DIV_180        (1144)
# define JO_FIXED_PI_DIV_2          (102944)
# define JO_FIXED_PI_OVER_2         JO_FIXED_PI_DIV_2
# define JO_FIXED_3_PI_OVER_2       (308331)
# define JO_FIXED_1_DIV             (1.0f / 65536.0f)

# define JO_INT_TO_FIXED(X)         JO_MULT_BY_65536(X)
# define JO_FIXED_TO_INT(X)         JO_DIV_BY_65536(X)
# define JO_FIXED_DECIMAL_MASK      (0xFFFF)
# define JO_FLOAT_TO_FIXED(X)       ((jo_fixed)(65536.0f * (X)))
# define JO_DEG_TO_ANGLE(X)         ((jo_angle)((65536.0 * (X)) / 360.0))
# define JO_RAD_TO_ANGLE(X)         ((jo_angle)((65536.0 * (X)) / (2.0 * 3.14159265358979)))

static __jo_force_inline jo_fixed       jo_int2fixed(const int x)
{
    return JO_INT_TO_FIXED(x);
}

static __jo_force_inline int            jo_fixed2int(const jo_fixed x)
{
    return JO_FIXED_TO_INT(x);
}

static __jo_force_inline jo_fixed       jo_float2fixed(const float x)
{
    return ((jo_fixed)(x * (float)JO_FIXED_1));
}

static __jo_force_inline float          jo_fixed2float(const jo_fixed x)
{
    return ((float)x * JO_FIXED_1_DIV);
}

static __jo_force_inline jo_fixed       jo_fixed_wrap_to_pi(jo_fixed rad)
{
    while (rad > JO_FIXED_PI) rad -= JO_FIXED_PI_2;
    while (rad <= -JO_FIXED_PI) rad += JO_FIXED_PI_2;
    return (rad);
}

static __jo_force_inline jo_fixed       jo_fixed_wrap_to_180(jo_fixed deg)
{
    while (deg > JO_FIXED_180) deg -= JO_FIXED_360;
    while (deg <= -JO_FIXED_180) deg += JO_FIXED_360;
    return (deg);
}

jo_fixed                                jo_fixed_pow(jo_fixed x, jo_fixed y);

static __jo_force_inline jo_fixed       jo_fixed_mult(const jo_fixed x, const jo_fixed y)
{
    return ((jo_fixed)(((long long)x * y) >> 16));
}

static __jo_force_inline int            jo_fixed_mult_to_int(const jo_fixed x, const jo_fixed y)
{
    int                                 high;
 
    __asm__ ("dmuls.l %1, %2\n\t"
             "sts mach, %0"
             : "=r" (high)
             : "r" (x), "r" (y)
             : "mach", "macl");
    return (high);
}

static __jo_force_inline jo_fixed       jo_fixed_dot(const jo_fixed ptA[3], const jo_fixed ptB[3])
{
    const jo_fixed                      *a;
    const jo_fixed                      *b;
    jo_fixed                            high;
    jo_fixed                            low;
 
    a = ptA;
    b = ptB;
    __asm__ ("clrmac\n\t"
             "mac.l @%2+, @%3+\n\t"
             "mac.l @%2+, @%3+\n\t"
             "mac.l @%2+, @%3+\n\t"
             "sts mach, %0\n\t"
             "sts macl, %1\n\t"
             "xtrct %0, %1"
             : "=&r" (high), "=&r" (low), "+r" (a), "+r" (b)
             : "m" (*(const jo_fixed (*)[3])ptA), "m" (*(const jo_fixed (*)[3])ptB)
             : "mach", "macl");
    return (low);
}

static __jo_force_inline jo_fixed       jo_fixed_div(const jo_fixed dividend, const jo_fixed divisor)
{
    JO_SH2_DVSR = divisor;
    // 64 bits dividend: (dividend << 16)
    JO_SH2_DVDNTH = dividend >> 16;
    // Writing the low part starts the division
    JO_SH2_DVDNTL = dividend << 16;
    return ((jo_fixed)JO_SH2_DVDNTL);
}

static __jo_force_inline int            jo_int_div(const int dividend, const int divisor)
{
    JO_SH2_DVSR = divisor;
    // (writing the dividend starts the division. The quotient read in DVDNTL, as jo_fixed_div(): the register saved and
    // restored by the VBLANK interrupt)
    JO_SH2_DVDNT = dividend;
    return (JO_SH2_DVDNTL);
}

static __jo_force_inline int            jo_shift_right8(const int x)
{
    return (x < 0 ? (int)~(~(unsigned int)x >> 8) : (int)((unsigned int)x >> 8));
}

static __jo_force_inline int            jo_shift_right8_small(const int x)
{
    return ((int)(((unsigned int)x + (1u << 23)) >> 8) - (1 << 15));
}

static __jo_force_inline void           jo_fixed_div_start(const jo_fixed dividend, const jo_fixed divisor)
{
    JO_SH2_DVSR = divisor;
    JO_SH2_DVDNTH = dividend >> 16;
    JO_SH2_DVDNTL = dividend << 16;
}

static __jo_force_inline jo_fixed       jo_fixed_div_result(void)
{
    return ((jo_fixed)JO_SH2_DVDNTL);
}

static __jo_force_inline jo_fixed       jo_fixed_deg2rad(const jo_fixed deg)
{
    return (jo_fixed_mult(jo_fixed_wrap_to_180(deg), JO_FIXED_PI_DIV_180));
}

static __jo_force_inline jo_fixed       jo_fixed_rad2deg(const jo_fixed rad)
{
    return (jo_fixed_mult(jo_fixed_wrap_to_pi(rad), JO_FIXED_180_DIV_PI));
}

static __jo_force_inline jo_fixed       jo_fixed_ceil(const jo_fixed x)
{
    return (x & 0xFFFF0000UL) + (x & 0x0000FFFFUL ? JO_FIXED_1 : 0);
}

static __jo_force_inline jo_fixed       jo_fixed_floor(const jo_fixed x)
{
    return (x & 0xFFFF0000ul);
}
 
/*
██████╗  █████╗ ███╗   ██╗██████╗  ██████╗ ███╗   ███╗
██╔══██╗██╔══██╗████╗  ██║██╔══██╗██╔═══██╗████╗ ████║
██████╔╝███████║██╔██╗ ██║██║  ██║██║   ██║██╔████╔██║
██╔══██╗██╔══██║██║╚██╗██║██║  ██║██║   ██║██║╚██╔╝██║
██║  ██║██║  ██║██║ ╚████║██████╔╝╚██████╔╝██║ ╚═╝ ██║
╚═╝  ╚═╝╚═╝  ╚═╝╚═╝  ╚═══╝╚═════╝  ╚═════╝ ╚═╝     ╚═╝
*/

extern int                      jo_random_seed;

int                             jo_random(int max);

static  __jo_force_inline int   jo_random_using_multiple(int max, int multiple)
{
    return (jo_random(max) / multiple) * multiple;
}
 
/*
███████╗ ██████╗ ██████╗ ████████╗
██╔════╝██╔═══██╗██╔══██╗╚══██╔══╝
███████╗██║   ██║██████╔╝   ██║
╚════██║██║▄▄ ██║██╔══██╗   ██║
███████║╚██████╔╝██║  ██║   ██║
╚══════╝ ╚══▀▀═╝ ╚═╝  ╚═╝   ╚═╝
*/

static  __jo_force_inline unsigned int  jo_highest_bit(unsigned int x)
{
    unsigned int                        bit;
 
    bit = 0;
    if (x & 0xffff0000)
    {
        bit = 16;
        x >>= 16;
    }
    if (x & 0xff00)
    {
        bit += 8;
        x >>= 8;
    }
    if (x & 0xf0)
    {
        bit += 4;
        x >>= 4;
    }
    if (x & 0xc)
    {
        bit += 2;
        x >>= 2;
    }
    if (x & 0x2)
        ++bit;
    return (bit);
}

jo_fixed                        jo_fixed_sqrt(jo_fixed value);

unsigned int                    jo_sqrt(unsigned int value);

static  __jo_force_inline float jo_sqrtf(float value)
{
    // A union avoids the strict aliasing issue of *(unsigned int *)&value (undefined behavior with -O2)
    union
    {
        float           f;
        unsigned int    i;
    }                   bits;
 
    bits.f = value;
    bits.i += 127 << 23;
    bits.i >>= 1;
    return (bits.f);
}
 
/*
██╗███╗   ██╗██╗   ██╗███████╗██████╗ ███████╗███████╗    ███████╗ ██████╗ ██████╗ ████████╗
██║████╗  ██║██║   ██║██╔════╝██╔══██╗██╔════╝██╔════╝    ██╔════╝██╔═══██╗██╔══██╗╚══██╔══╝
██║██╔██╗ ██║██║   ██║█████╗  ██████╔╝███████╗█████╗      ███████╗██║   ██║██████╔╝   ██║
██║██║╚██╗██║╚██╗ ██╔╝██╔══╝  ██╔══██╗╚════██║██╔══╝      ╚════██║██║▄▄ ██║██╔══██╗   ██║
██║██║ ╚████║ ╚████╔╝ ███████╗██║  ██║███████║███████╗    ███████║╚██████╔╝██║  ██║   ██║
╚═╝╚═╝  ╚═══╝  ╚═══╝  ╚══════╝╚═╝  ╚═╝╚══════╝╚══════╝    ╚══════╝ ╚══▀▀═╝ ╚═╝  ╚═╝   ╚═╝
AKA RECIPROCAL SQUARE ROOT
*/

jo_fixed        jo_fixed_rsqrt(jo_fixed value);

float           jo_rsqrt(float value);
 
/*
███████╗ ██████╗ ██╗         ██╗███╗   ██╗████████╗███████╗██████╗  ██████╗ ██████╗
██╔════╝██╔════╝ ██║         ██║████╗  ██║╚══██╔══╝██╔════╝██╔══██╗██╔═══██╗██╔══██╗
███████╗██║  ███╗██║         ██║██╔██╗ ██║   ██║   █████╗  ██████╔╝██║   ██║██████╔╝
╚════██║██║   ██║██║         ██║██║╚██╗██║   ██║   ██╔══╝  ██╔══██╗██║   ██║██╔═══╝
███████║╚██████╔╝███████╗    ██║██║ ╚████║   ██║   ███████╗██║  ██║╚██████╔╝██║
╚══════╝ ╚═════╝ ╚══════╝    ╚═╝╚═╝  ╚═══╝   ╚═╝   ╚══════╝╚═╝  ╚═╝ ╚═════╝ ╚═╝
 
*/

# define __JO_DEG_TO_ANGLE_MAGIC    (182)

static  __jo_force_inline jo_angle     jo_DEGtoANG(const float deg)
{
    // One float multiplication (65536 / 360), not a double multiplication and a double division
    return ((jo_angle)(deg * (65536.0f / 360.0f)));
}

static  __jo_force_inline jo_angle     jo_DEGtoANG_int(const int deg)
{
    int                                 n;
    int                                 a;
 
    // deg * 65536 / 360 = deg * 8192 / 45: multiplication by 2^32 / 45 (rounded up) instead of a division.
    // Same result as the division for |deg| < 37000
    n = deg < 0 ? -deg : deg;
    a = (int)(((long long)(n * 8192) * 95443718LL) >> 32);
    return ((jo_angle)(deg < 0 ? -a : a));
}
 

static  __jo_force_inline jo_angle     jo_fixed_rad2ANGLE(const jo_fixed rad)
{
    // rad * 65536 / (2 pi) without floating point: 65536 / (2 pi) = 683565276 / 2^32 (Q16.16 input)
    return ((jo_angle)(((long long)rad * 683565276LL) >> 32));
}
 

extern const jo_fixed               __jo_sin_table[1025];

static  __jo_force_inline jo_fixed     __jo_sin_index(const unsigned int index)
{
    if (index < 1024)
        return (__jo_sin_table[index]);
    if (index < 2048)
        return (__jo_sin_table[2048 - index]);
    if (index < 3072)
        return (-__jo_sin_table[index - 2048]);
    return (-__jo_sin_table[4096 - index]);
}

static  __jo_force_inline unsigned int __jo_angle_to_sin_index(const jo_angle angle)
{
    return (((((unsigned short)angle) + 8) >> 4) & 0xfff);
}

static  __jo_force_inline jo_fixed     jo_sin_angle(const jo_angle angle)
{
    return (__jo_sin_index(__jo_angle_to_sin_index(angle)));
}

static  __jo_force_inline jo_fixed     jo_cos_angle(const jo_angle angle)
{
    return (__jo_sin_index((__jo_angle_to_sin_index(angle) + 1024) & 0xfff));
}

static  __jo_force_inline void         jo_sincos_angle(const jo_angle angle, jo_fixed * const sin_value, jo_fixed * const cos_value)
{
    unsigned int                        index;
 
    index = __jo_angle_to_sin_index(angle);
    *sin_value = __jo_sin_index(index);
    *cos_value = __jo_sin_index((index + 1024) & 0xfff);
}

static  __jo_force_inline jo_angle     jo_fixed_deg2ANGLE(const jo_fixed deg)
{
    // deg * 65536 / 360 without floating point: 1 / 360 = 11930465 / 2^32 (Q16.16 input)
    return ((jo_angle)(((long long)deg * 11930465LL) >> 32));
}
 
/*
███████╗██╗███╗   ██╗██╗   ██╗███████╗
██╔════╝██║████╗  ██║██║   ██║██╔════╝
███████╗██║██╔██╗ ██║██║   ██║███████╗
╚════██║██║██║╚██╗██║██║   ██║╚════██║
███████║██║██║ ╚████║╚██████╔╝███████║
╚══════╝╚═╝╚═╝  ╚═══╝ ╚═════╝ ╚══════╝
*/

static  __jo_force_inline jo_fixed  jo_fixed_sin(const jo_fixed rad)
{
    return (jo_sin_angle(jo_fixed_rad2ANGLE(rad)));
}

static  __jo_force_inline jo_fixed  jo_sin(const int deg)
{
    // Table lookup, without floating point
    return (jo_sin_angle(jo_DEGtoANG_int(deg)));
}

static  __jo_force_inline float jo_sinf(const int deg)
{
    return (jo_fixed2float(jo_sin(deg)));
}

static  __jo_force_inline jo_fixed  jo_sin_rad(const float rad)
{
    return (jo_fixed_sin(jo_float2fixed(rad)));
}

static  __jo_force_inline float jo_sin_radf(const float rad)
{
    return (jo_fixed2float(jo_fixed_sin(jo_float2fixed(rad))));
}

static  __jo_force_inline int   jo_sin_mult(const int value, const int deg)
{
    return (jo_fixed_mult(value, jo_sin(deg)));
}

static  __jo_force_inline float jo_sinf_mult(const float value, const int deg)
{
    return (value * jo_sinf(deg));
}
 
/*
 ██████╗ ██████╗ ███████╗██╗███╗   ██╗██╗   ██╗███████╗
██╔════╝██╔═══██╗██╔════╝██║████╗  ██║██║   ██║██╔════╝
██║     ██║   ██║███████╗██║██╔██╗ ██║██║   ██║███████╗
██║     ██║   ██║╚════██║██║██║╚██╗██║██║   ██║╚════██║
╚██████╗╚██████╔╝███████║██║██║ ╚████║╚██████╔╝███████║
 ╚═════╝ ╚═════╝ ╚══════╝╚═╝╚═╝  ╚═══╝ ╚═════╝ ╚══════╝
 
*/

static  __jo_force_inline jo_fixed  jo_fixed_cos(const jo_fixed rad)
{
    return (jo_cos_angle(jo_fixed_rad2ANGLE(rad)));
}

static  __jo_force_inline jo_fixed  jo_cos(const int deg)
{
    // Table lookup, without floating point
    return (jo_cos_angle(jo_DEGtoANG_int(deg)));
}

static  __jo_force_inline void  jo_sincos(const int deg, jo_fixed * const sin_value, jo_fixed * const cos_value)
{
    jo_sincos_angle(jo_DEGtoANG_int(deg), sin_value, cos_value);
}

static  __jo_force_inline float jo_cosf(const int deg)
{
    return (jo_fixed2float(jo_cos(deg)));
}

static  __jo_force_inline jo_fixed  jo_cos_rad(const float rad)
{
    return (jo_fixed_cos(jo_float2fixed(rad)));
}

static  __jo_force_inline float jo_cos_radf(const float rad)
{
    return (jo_fixed2float(jo_fixed_cos(jo_float2fixed(rad))));
}

static  __jo_force_inline int   jo_cos_mult(const int value, const int deg)
{
    return (jo_fixed_mult(value, jo_cos(deg)));
}

static  __jo_force_inline float jo_cosf_mult(const float value, const int deg)
{
    return (value * jo_cosf(deg));
}
 
/*
████████╗ █████╗ ███╗   ██╗ ██████╗ ███████╗███╗   ██╗████████╗
╚══██╔══╝██╔══██╗████╗  ██║██╔════╝ ██╔════╝████╗  ██║╚══██╔══╝
   ██║   ███████║██╔██╗ ██║██║  ███╗█████╗  ██╔██╗ ██║   ██║
   ██║   ██╔══██║██║╚██╗██║██║   ██║██╔══╝  ██║╚██╗██║   ██║
   ██║   ██║  ██║██║ ╚████║╚██████╔╝███████╗██║ ╚████║   ██║
   ╚═╝   ╚═╝  ╚═╝╚═╝  ╚═══╝ ╚═════╝ ╚══════╝╚═╝  ╚═══╝   ╚═╝
 
*/

static __jo_force_inline jo_fixed    jo_tan(const int deg)
{
    jo_fixed                         sin_value;
    jo_fixed                         cos_value;
 
    jo_sincos(deg, &sin_value, &cos_value);
    return jo_fixed_div(sin_value, cos_value);
}

static __jo_force_inline float    jo_tanf(const float deg)
{
    return (jo_sinf(deg) / jo_cosf(deg));
}

static __jo_force_inline jo_fixed    jo_tan_rad(const float rad)
{
    jo_fixed                         sin_value;
    jo_fixed                         cos_value;
 
    jo_sincos_angle(jo_fixed_rad2ANGLE(jo_float2fixed(rad)), &sin_value, &cos_value);
    return jo_fixed_div(sin_value, cos_value);
}

static __jo_force_inline float  jo_tan_radf(const float rad)
{
    return (jo_sin_radf(rad) / jo_cos_radf(rad));
}
 
/*
 █████╗  ██████╗ ██████╗ ███████╗
██╔══██╗██╔════╝██╔═══██╗██╔════╝
███████║██║     ██║   ██║███████╗
██╔══██║██║     ██║   ██║╚════██║
██║  ██║╚██████╗╚██████╔╝███████║
╚═╝  ╚═╝ ╚═════╝ ╚═════╝ ╚══════╝
*/

static  __jo_force_inline float jo_acos_radf(const float angle)
{
    return (1.5707963f - 1.5707963f * angle);
}
 
/*
 █████╗ ████████╗ █████╗ ███╗   ██╗██████╗
██╔══██╗╚══██╔══╝██╔══██╗████╗  ██║╚════██╗
███████║   ██║   ███████║██╔██╗ ██║ █████╔╝
██╔══██║   ██║   ██╔══██║██║╚██╗██║██╔═══╝
██║  ██║   ██║   ██║  ██║██║ ╚████║███████╗
╚═╝  ╚═╝   ╚═╝   ╚═╝  ╚═╝╚═╝  ╚═══╝╚══════╝
 
*/

float                           jo_atan2f_rad(const float y, const float x);

static __jo_force_inline int    jo_atan2f(const float y, const float x)
{
    return (int)JO_RAD_TO_DEG(jo_atan2f_rad(y, x));
}

static __jo_force_inline jo_angle   jo_atan2_angle(const float y, const float x)
{
    return ((jo_angle)(int)(jo_atan2f_rad(y, x) * (32768.0f / JO_PI)));
}

jo_angle                        jo_atan2_fixed(const jo_fixed y, const jo_fixed x);

int                             jo_atan2_degrees(const jo_fixed y, const jo_fixed x);

static __jo_force_inline int    jo_turn_toward(const int angle, const int target, const int max_step)
{
    int                         d;
 
    // Shortest difference: -180 to 179 (the modulo, a division by libgcc, only when the angles are not 0 to 359)
    d = target - angle;
    if (d >= 360 || d <= -360)
        d %= 360;
    if (d >= 180)
        d -= 360;
    else if (d < -180)
        d += 360;
    if (d > max_step)
        d = max_step;
    else if (d < -max_step)
        d = -max_step;
    d += angle;
    if (d >= 360 || d <= -360)
        d %= 360;
    return (d < 0 ? d + 360 : d);
}
 
/*
██╗   ██╗███████╗ ██████╗████████╗ ██████╗ ██████╗
██║   ██║██╔════╝██╔════╝╚══██╔══╝██╔═══██╗██╔══██╗
██║   ██║█████╗  ██║        ██║   ██║   ██║██████╔╝
╚██╗ ██╔╝██╔══╝  ██║        ██║   ██║   ██║██╔══██╗
 ╚████╔╝ ███████╗╚██████╗   ██║   ╚██████╔╝██║  ██║
  ╚═══╝  ╚══════╝ ╚═════╝   ╚═╝    ╚═════╝ ╚═╝  ╚═╝
*/
 
/*
    _______               __   _   __                __
   / ____(_)  _____  ____/ /  / | / /_  ______ ___  / /_  ___  _____
  / /_  / / |/_/ _ \/ __  /  /  |/ / / / / __ `__ \/ __ \/ _ \/ ___/
 / __/ / />  </  __/ /_/ /  / /|  / /_/ / / / / / / /_/ /  __/ /
/_/   /_/_/|_|\___/\__,_/  /_/ |_/\__,_/_/ /_/ /_/_.___/\___/_/
 
*/

void    jo_vector_fixed_compute_bezier_point(const jo_fixed t, jo_vector_fixed p0, jo_vector_fixed p1, jo_vector_fixed p2, jo_vector_fixed p3, jo_vector_fixed *result);

static __jo_force_inline void jo_vector_fixed_add(const jo_vector_fixed * const a, const jo_vector_fixed * const b, jo_vector_fixed * const result)
{
    result->x = a->x + b->x;
    result->y = a->y + b->y;
    result->z = a->z + b->z;
}

static __jo_force_inline void jo_vector_fixed_muls(const jo_vector_fixed * const a, const jo_fixed s, jo_vector_fixed * const result)
{
    result->x = jo_fixed_mult(a->x, s);
    result->y = jo_fixed_mult(a->y, s);
    result->z = jo_fixed_mult(a->z, s);
}

static __jo_force_inline void jo_vector4_fixed_add(const jo_vector4_fixed * const a, const jo_vector4_fixed * const b, jo_vector4_fixed * const result)
{
    result->x = a->x + b->x;
    result->y = a->y + b->y;
    result->z = a->z + b->z;
    result->w = JO_FIXED_1;
}

static __jo_force_inline void jo_vector4_fixed_sub(const jo_vector4_fixed * const a, const jo_vector4_fixed * const b, jo_vector4_fixed * const result)
{
    result->x = a->x - b->x;
    result->y = a->y - b->y;
    result->z = a->z - b->z;
    result->w = JO_FIXED_1;
}

static __jo_force_inline void jo_vector4_fixed_cross(const jo_vector4_fixed * const a, const jo_vector4_fixed * const b, jo_vector4_fixed * const result)
{
    result->x = jo_fixed_mult(a->y, b->z) - jo_fixed_mult(a->z, b->y);
    result->y = jo_fixed_mult(a->z, b->x) - jo_fixed_mult(a->x, b->z);
    result->z = jo_fixed_mult(a->x, b->y) - jo_fixed_mult(a->y, b->x);
    result->w = JO_FIXED_1;
}

static __jo_force_inline jo_fixed jo_vector4_fixed_dot(const jo_vector4_fixed * const a, const jo_vector4_fixed * const b)
{
    // x, y, z are contiguous (w is ignored)
    return (jo_fixed_dot((const jo_fixed *)a, (const jo_fixed *)b));
}

static __jo_force_inline void jo_vector4_swap(jo_vector4_fixed * const a, jo_vector4_fixed * const b)
{
    JO_SWAP(a->x, b->x);
    JO_SWAP(a->y, b->y);
    JO_SWAP(a->z, b->z);
    JO_SWAP(a->w, b->w);
}
 
/*
    ________            __  _                _   __                __
   / ____/ /___  ____ _/ /_(_)___  ____ _   / | / /_  ______ ___  / /_  ___  _____
  / /_  / / __ \/ __ `/ __/ / __ \/ __ `/  /  |/ / / / / __ `__ \/ __ \/ _ \/ ___/
 / __/ / / /_/ / /_/ / /_/ / / / / /_/ /  / /|  / /_/ / / / / / / /_/ /  __/ /
/_/   /_/\____/\__,_/\__/_/_/ /_/\__, /  /_/ |_/\__,_/_/ /_/ /_/_.___/\___/_/
                                /____/
*/

void    jo_vectorf_compute_bezier_point(const float t, jo_vectorf p0, jo_vectorf p1, jo_vectorf p2, jo_vectorf p3, jo_vectorf *result);

static __jo_force_inline void jo_vectorf_add(const jo_vectorf * const a, const jo_vectorf * const b, jo_vectorf * const result)
{
    result->x = a->x + b->x;
    result->y = a->y + b->y;
    result->z = a->z + b->z;
}

static __jo_force_inline void jo_vectorf_adds(const jo_vectorf * const a, const float s, jo_vectorf * const result)
{
    result->x = a->x + s;
    result->y = a->y + s;
    result->z = a->z + s;
}

static __jo_force_inline void jo_vectorf_sub(const jo_vectorf * const a, const jo_vectorf * const b, jo_vectorf * const result)
{
    result->x = a->x - b->x;
    result->y = a->y - b->y;
    result->z = a->z - b->z;
}

static __jo_force_inline void jo_vectorf_subs(const jo_vectorf * const a, const float s, jo_vectorf * const result)
{
    result->x = a->x - s;
    result->y = a->y - s;
    result->z = a->z - s;
}

static __jo_force_inline void jo_vectorf_mul(const jo_vectorf * const a, const jo_vectorf * const b, jo_vectorf * const result)
{
    result->x = a->x * b->x;
    result->y = a->y * b->y;
    result->z = a->z * b->z;
}

static __jo_force_inline void jo_vectorf_muls(const jo_vectorf * const a, const float s, jo_vectorf * const result)
{
    result->x = a->x * s;
    result->y = a->y * s;
    result->z = a->z * s;
}

static __jo_force_inline void jo_vectorf_div(const jo_vectorf * const a, const jo_vectorf * const b, jo_vectorf * const result)
{
    result->x = a->x / b->x;
    result->y = a->y / b->y;
    result->z = a->z / b->z;
}

static __jo_force_inline void jo_vectorf_divs(const jo_vectorf * const a, const float s, jo_vectorf * const result)
{
    float                       inv;
 
    // One division (libgcc on the SH-2) and 3 multiplications instead of 3 divisions
    inv = 1.0f / s;
    result->x = a->x * inv;
    result->y = a->y * inv;
    result->z = a->z * inv;
}

static __jo_force_inline float  jo_vectorf_length(const jo_vectorf * const a)
{
    return (jo_sqrtf(JO_SQUARE(a->x) + JO_SQUARE(a->y) + JO_SQUARE(a->z)));
}

static __jo_force_inline float  jo_vectorf_dot(const jo_vectorf * const a, const jo_vectorf * const b)
{
    return (a->x * b->x + a->y * b->y + a->z * b->z);
}

static __jo_force_inline void jo_vectorf_normalize(const jo_vectorf * const a, jo_vectorf * const result)
{
    float len = jo_vectorf_length(a);
    if (len > 0)
    {
        // One division (libgcc on the SH-2) and 3 multiplications instead of 3 divisions
        float inv = 1.0f / len;
        result->x = a->x * inv;
        result->y = a->y * inv;
        result->z = a->z * inv;
    }
    else
    {
        result->x = 0;
        result->y = 0;
        result->z = 0;
    }
}

static __jo_force_inline void jo_vectorf_proj(const jo_vectorf * const v, const jo_vectorf * const onto, jo_vectorf * const result)
{
    jo_vectorf_muls(onto, jo_vectorf_dot(v, onto) / jo_vectorf_dot(onto, onto), result);
}

static __jo_force_inline void jo_vectorf_cross(const jo_vectorf * const a, const jo_vectorf * const b, jo_vectorf * const result)
{
    result->x = a->y * b->z - a->z * b->y;
    result->y = a->z * b->x - a->x * b->z;
    result->z = a->x * b->y - a->y * b->x;
}

static __jo_force_inline float jo_vectorf_angle_between_radf(const jo_vectorf * const a, const jo_vectorf * const b)
{
    // One square root: sqrt(|a|^2 * |b|^2) = |a| * |b|
    return jo_acos_radf(jo_vectorf_dot(a, b) / jo_sqrtf(jo_vectorf_dot(a, a) * jo_vectorf_dot(b, b)));
}
 
/*
██╗  ██╗██╗  ██╗██╗  ██╗    ███╗   ███╗ █████╗ ████████╗██████╗ ██╗██╗  ██╗
██║  ██║╚██╗██╔╝██║  ██║    ████╗ ████║██╔══██╗╚══██╔══╝██╔══██╗██║╚██╗██╔╝
███████║ ╚███╔╝ ███████║    ██╔████╔██║███████║   ██║   ██████╔╝██║ ╚███╔╝
╚════██║ ██╔██╗ ╚════██║    ██║╚██╔╝██║██╔══██║   ██║   ██╔══██╗██║ ██╔██╗
     ██║██╔╝ ██╗     ██║    ██║ ╚═╝ ██║██║  ██║   ██║   ██║  ██║██║██╔╝ ██╗
     ╚═╝╚═╝  ╚═╝     ╚═╝    ╚═╝     ╚═╝╚═╝  ╚═╝   ╚═╝   ╚═╝  ╚═╝╚═╝╚═╝  ╚═╝
 
*/

static __jo_force_inline void   jo_matrix_identity(jo_matrix * const result)
{
    register int                i;
 
    for (i = 1; i < 15; ++i)
        JO_ZERO(result->table[i]);
    result->m00 = JO_FIXED_1;
    result->m11 = JO_FIXED_1;
    result->m22 = JO_FIXED_1;
    result->m33 = JO_FIXED_1;
}

static __jo_force_inline void   jo_matrixf_identity(jo_matrixf * const result)
{
    register int                i;
 
    for (i = 1; i < 15; ++i)
        result->table[i] = 0;
    result->m00 = 1;
    result->m11 = 1;
    result->m22 = 1;
    result->m33 = 1;
}

static __jo_force_inline void jo_matrix_translation(const jo_vector_fixed * const offset, jo_matrix * const result)
{
    jo_matrix_identity(result);
    result->m30 = offset->x;
    result->m31 = offset->y;
    result->m32 = offset->z;
}

static __jo_force_inline void jo_matrixf_translation(const jo_vectorf * const offset, jo_matrixf * const result)
{
    jo_matrixf_identity(result);
    result->m30 = offset->x;
    result->m31 = offset->y;
    result->m32 = offset->z;
}

static __jo_force_inline void jo_matrix_scaling(const jo_vector_fixed * const scale, jo_matrix * const result)
{
    jo_matrix_identity(result);
    result->m00 = scale->x;
    result->m11 = scale->y;
    result->m22 = scale->z;
}

static __jo_force_inline void jo_matrixf_scaling(const jo_vectorf * const scale, jo_matrixf * const result)
{
    jo_matrixf_identity(result);
    result->m00 = scale->x;
    result->m11 = scale->y;
    result->m22 = scale->z;
}

static __jo_force_inline void jo_matrix_rotation_x_rad(const float angle_in_rad, jo_matrix * const result)
{
    jo_matrix_identity(result);
    // The float angle is converted once (same values as jo_cos_rad() and jo_sin_rad())
    jo_sincos_angle(jo_fixed_rad2ANGLE(jo_float2fixed(angle_in_rad)), &result->m12, &result->m11);
    result->m21 = -result->m12;
    result->m22 = result->m11;
}

static __jo_force_inline void jo_matrixf_rotation_x_rad(const float angle_in_rad, jo_matrixf * const result)
{
    jo_matrixf_identity(result);
    result->m11 = jo_cos_radf(angle_in_rad);
    result->m12 = jo_sin_radf(angle_in_rad);
    result->m21 = -result->m12;
    result->m22 = result->m11;
}

static __jo_force_inline void jo_matrix_rotation_y_rad(const float angle_in_rad, jo_matrix * const result)
{
    jo_matrix_identity(result);
    // The float angle is converted once (same values as jo_cos_rad() and jo_sin_rad())
    jo_sincos_angle(jo_fixed_rad2ANGLE(jo_float2fixed(angle_in_rad)), &result->m20, &result->m00);
    result->m02 = -result->m20;
    result->m22 = result->m00;
}

static __jo_force_inline void jo_matrixf_rotation_y_rad(const float angle_in_rad, jo_matrixf * const result)
{
    jo_matrixf_identity(result);
    result->m00 = jo_cos_radf(angle_in_rad);
    result->m20 = jo_sin_radf(angle_in_rad);
    result->m02 = -result->m20;
    result->m22 = result->m00;
}

static __jo_force_inline void jo_matrix_rotation_z_rad(const float angle_in_rad, jo_matrix * const result)
{
    jo_matrix_identity(result);
    // The float angle is converted once (same values as jo_cos_rad() and jo_sin_rad())
    jo_sincos_angle(jo_fixed_rad2ANGLE(jo_float2fixed(angle_in_rad)), &result->m01, &result->m00);
    result->m10 = -result->m01;
    result->m11 = result->m00;
}

static __jo_force_inline void jo_matrixf_rotation_z_rad(const float angle_in_rad, jo_matrixf * const result)
{
    jo_matrixf_identity(result);
    result->m00 = jo_cos_radf(angle_in_rad);
    result->m01 = jo_sin_radf(angle_in_rad);
    result->m10 = -result->m01;
    result->m11 = result->m00;
}

static __jo_force_inline void jo_matrix_transpose(const jo_matrix * const matrix, jo_matrix * const result)
{
    result->m00 = matrix->m00;
    result->m10 = matrix->m01;
    result->m20 = matrix->m02;
    result->m30 = matrix->m03;
    result->m01 = matrix->m10;
    result->m11 = matrix->m11;
    result->m21 = matrix->m12;
    result->m31 = matrix->m13;
    result->m02 = matrix->m20;
    result->m12 = matrix->m21;
    result->m22 = matrix->m22;
    result->m32 = matrix->m23;
    result->m03 = matrix->m30;
    result->m13 = matrix->m31;
    result->m23 = matrix->m32;
    result->m33 = matrix->m33;
}

static __jo_force_inline void jo_matrixf_transpose(const jo_matrixf * const matrix, jo_matrixf * const result)
{
    result->m00 = matrix->m00;
    result->m10 = matrix->m01;
    result->m20 = matrix->m02;
    result->m30 = matrix->m03;
    result->m01 = matrix->m10;
    result->m11 = matrix->m11;
    result->m21 = matrix->m12;
    result->m31 = matrix->m13;
    result->m02 = matrix->m20;
    result->m12 = matrix->m21;
    result->m22 = matrix->m22;
    result->m32 = matrix->m23;
    result->m03 = matrix->m30;
    result->m13 = matrix->m31;
    result->m23 = matrix->m32;
    result->m33 = matrix->m33;
}

static __jo_force_inline void   jo_matrix_mul_vector4(const jo_matrix * const m, const jo_vector4_fixed * const v, jo_vector4_fixed * const result)
{
    result->x = jo_fixed_mult(m->m00, v->x) + jo_fixed_mult(m->m10, v->y) + jo_fixed_mult(m->m20, v->z) + jo_fixed_mult(m->m30, v->w);
    result->y = jo_fixed_mult(m->m01, v->x) + jo_fixed_mult(m->m11, v->y) + jo_fixed_mult(m->m21, v->z) + jo_fixed_mult(m->m31, v->w);
    result->z = jo_fixed_mult(m->m02, v->x) + jo_fixed_mult(m->m12, v->y) + jo_fixed_mult(m->m22, v->z) + jo_fixed_mult(m->m32, v->w);
    result->w = jo_fixed_mult(m->m03, v->x) + jo_fixed_mult(m->m13, v->y) + jo_fixed_mult(m->m23, v->z) + jo_fixed_mult(m->m33, v->w);
}

static __jo_force_inline jo_fixed       __jo_fixed_dot4(const jo_fixed ptA[4], const jo_fixed ptB[4])
{
    const jo_fixed                      *a;
    const jo_fixed                      *b;
    jo_fixed                            high;
    jo_fixed                            low;
 
    a = ptA;
    b = ptB;
    __asm__ ("clrmac\n\t"
             "mac.l @%2+, @%3+\n\t"
             "mac.l @%2+, @%3+\n\t"
             "mac.l @%2+, @%3+\n\t"
             "mac.l @%2+, @%3+\n\t"
             "sts mach, %0\n\t"
             "sts macl, %1\n\t"
             "xtrct %0, %1"
             : "=&r" (high), "=&r" (low), "+r" (a), "+r" (b)
             : "m" (*(const jo_fixed (*)[4])ptA), "m" (*(const jo_fixed (*)[4])ptB)
             : "mach", "macl");
    return (low);
}

static __jo_force_inline void   jo_matrix_mul(const jo_matrix * const a, const jo_matrix * const b, jo_matrix * const result)
{
    jo_matrix                   at;
    register int                i;
    register int                j;
 
    // The columns of a become contiguous rows: result->m[i][j] = sum of a->m[k][j] * b->m[i][k]
    jo_matrix_transpose(a, &at);
    for (i = 0; i < 4; ++i)
        for (j = 0; j < 4; ++j)
            result->m[i][j] = __jo_fixed_dot4(at.m[j], b->m[i]);
}

static __jo_force_inline void   jo_matrixf_mul(const jo_matrixf * const a, const jo_matrixf * const b, jo_matrixf * const result)
{
    register int                i;
    register int                j;
    register int                k;
    register float              sum;
 
    for (i = 0; i < 4; ++i)
    {
        for (j = 0; j < 4; ++j)
        {
            sum = 0;
            for (k = 0; k < 4; ++k)
                sum += a->m[k][j] * b->m[i][k];
            result->m[i][j] = sum;
        }
    }
}

static __jo_force_inline void   jo_matrixf_rotation(const float angle_in_rad, const jo_vectorf * const axis, jo_matrixf * const result)
{
    jo_vectorf                   normalized_axis;
 
    jo_vectorf_normalize(axis, &normalized_axis);
    float c = jo_cos_radf(angle_in_rad);
    float s = jo_sin_radf(angle_in_rad);
    float a = 1 - c;
    jo_matrixf_identity(result);
    result->m00 = c + normalized_axis.x * normalized_axis.x * a;
    result->m10 = normalized_axis.x * normalized_axis.y * a - normalized_axis.z * s;
    result->m20 = normalized_axis.x * normalized_axis.z * a + normalized_axis.y * s;
    result->m01 = normalized_axis.y * normalized_axis.x * a + normalized_axis.z * s;
    result->m11 = c + normalized_axis.y * normalized_axis.y * a;
    result->m21 = normalized_axis.y * normalized_axis.z * a - normalized_axis.x * s;
    result->m02 = normalized_axis.z * normalized_axis.x * a - normalized_axis.y * s;
    result->m12 = normalized_axis.z * normalized_axis.y * a + normalized_axis.x * s;
    result->m22 = c + normalized_axis.z * normalized_axis.z * a;
}

static __jo_force_inline void jo_matrixf_ortho(const float left, const float right, const float bottom, const float top,
                                                   const float back, const float front, jo_matrixf * const result)
{
    jo_matrixf_identity(result);
    result->m00 = 2 / (right - left);
    result->m11 = 2 / (top - bottom);
    result->m22 = 2 / (back - front);
    result->m30 = -(right + left) / (right - left);
    result->m31 = -(top + bottom) / (top - bottom);
    result->m32 = -(back + front) / (back - front);
}

static __jo_force_inline void jo_matrixf_perspective(const float vertical_field_of_view_in_deg, const float aspect_ratio,
                                                    const float near_view_distance, const float far_view_distance, jo_matrixf * const result)
{
    jo_matrixf_identity(result);
    result->m11 = 1.0f / jo_tan_radf(vertical_field_of_view_in_deg * (JO_PI / 360.0f));
    result->m00 = result->m11 / aspect_ratio;
    result->m22 = (far_view_distance + near_view_distance) / (near_view_distance - far_view_distance);
    result->m32 = (2 * far_view_distance * near_view_distance) / (near_view_distance - far_view_distance);
    result->m23 = -1;
    result->m33 = 0;
}

static __jo_force_inline void   jo_matrixf_look_at(const jo_vectorf * const from, const jo_vectorf * const to,
                                                     const jo_vectorf * const up, jo_matrixf * const result)
{
    jo_vectorf                   tmp;
    jo_vectorf                   x;
    jo_vectorf                   y;
    jo_vectorf                   z;
 
    jo_vectorf_sub(to, from, &tmp);
    jo_vectorf_normalize(&tmp, &tmp);
    jo_vectorf_muls(&tmp, -1, &z);
    jo_vectorf_cross(up, &z, &tmp);
    jo_vectorf_normalize(&tmp, &x);
    jo_vectorf_cross(&z, &x, &y);
 
    jo_matrixf_identity(result);
    result->m00 = x.x;
    result->m10 = x.y;
    result->m20 = x.z;
    result->m30 = -jo_vectorf_dot(from, &x);
    result->m01 = y.x;
    result->m11 = y.y;
    result->m21 = y.z;
    result->m31 = -jo_vectorf_dot(from, &y);
    result->m02 = z.x;
    result->m12 = z.y;
    result->m22 = z.z;
    result->m33 = -jo_vectorf_dot(from, &z);
}

static __jo_force_inline void jo_matrixf_invert_affine(const jo_matrixf * const matrix, jo_matrixf * const result)
{
    float c00 = matrix->m11 * matrix->m22 - matrix->m12 * matrix->m21;
    float c10 = -(matrix->m01 * matrix->m22 - matrix->m02 * matrix->m21);
    float c20 =   matrix->m01 * matrix->m12 - matrix->m02 * matrix->m11;
    float det = matrix->m00 * c00 + matrix->m10 * c10 + matrix->m20 * c20;
    jo_matrixf_identity(result);
    if (JO_FABS(det) < JO_FLOAT_EPSILON)
        return;
    float c01 = -(matrix->m10 * matrix->m22 - matrix->m12 * matrix->m20);
    float c11 = matrix->m00 * matrix->m22 - matrix->m02 * matrix->m20;
    float c21 = -(matrix->m00 * matrix->m12 - matrix->m02 * matrix->m10);
    float c02 = matrix->m10 * matrix->m21 - matrix->m11 * matrix->m20;
    float c12 = -(matrix->m00 * matrix->m21 - matrix->m01 * matrix->m20);
    float c22 = matrix->m00 * matrix->m11 - matrix->m01 * matrix->m10;
    // One division (libgcc on the SH-2) and 9 multiplications instead of 9 divisions
    float inv_det = 1.0f / det;
    float i00 = c00 * inv_det;
    float i10 = c01 * inv_det;
    float i20 = c02 * inv_det;
    float i01 = c10 * inv_det;
    float i11 = c11 * inv_det;
    float i21 = c12 * inv_det;
    float i02 = c20 * inv_det;
    float i12 = c21 * inv_det;
    float i22 = c22 * inv_det;
 
    result->m00 = i00;
    result->m10 = i10;
    result->m20 = i20;
    result->m30 = -(i00 * matrix->m30 + i10 * matrix->m31 + i20 * matrix->m32);
    result->m01 = i01;
    result->m11 = i11;
    result->m21 = i21;
    result->m31 = -(i01 * matrix->m30 + i11 * matrix->m31 + i21 * matrix->m32);
    result->m02 = i02;
    result->m12 = i12;
    result->m22 = i22;
    result->m32 = -(i02 * matrix->m30 + i12 * matrix->m31 + i22 * matrix->m32);
}

static __jo_force_inline void jo_matrixf_mul_pos(const jo_matrixf * const matrix, const jo_vectorf * const position,
                                                     jo_vectorf * const result)
{
    float w = matrix->m03 * position->x + matrix->m13 * position->y + matrix->m23 * position->z + matrix->m33;
    result->x = matrix->m00 * position->x + matrix->m10 * position->y + matrix->m20 * position->z + matrix->m30;
    result->y = matrix->m01 * position->x + matrix->m11 * position->y + matrix->m21 * position->z + matrix->m31;
    result->z = matrix->m02 * position->x + matrix->m12 * position->y + matrix->m22 * position->z + matrix->m32;
    if (w != 0 && w != 1)
    {
        // One division (libgcc on the SH-2) and 3 multiplications instead of 3 divisions
        float inv_w = 1.0f / w;
        result->x *= inv_w;
        result->y *= inv_w;
        result->z *= inv_w;
    }
}

static __jo_force_inline void jo_matrixf_mul_dir(const jo_matrixf * const matrix, const jo_vectorf * const direction,
                                                     jo_vectorf * const result)
{
    float w = matrix->m03 * direction->x + matrix->m13 * direction->y + matrix->m23 * direction->z;
    result->x = matrix->m00 * direction->x + matrix->m10 * direction->y + matrix->m20 * direction->z;
    result->y = matrix->m01 * direction->x + matrix->m11 * direction->y + matrix->m21 * direction->z;
    result->z = matrix->m02 * direction->x + matrix->m12 * direction->y + matrix->m22 * direction->z;
    if (w != 0 && w != 1)
    {
        // One division (libgcc on the SH-2) and 3 multiplications instead of 3 divisions
        float inv_w = 1.0f / w;
        result->x *= inv_w;
        result->y *= inv_w;
        result->z *= inv_w;
    }
}
 
/*
███╗   ███╗██╗███████╗ ██████╗
████╗ ████║██║██╔════╝██╔════╝
██╔████╔██║██║███████╗██║
██║╚██╔╝██║██║╚════██║██║
██║ ╚═╝ ██║██║███████║╚██████╗
╚═╝     ╚═╝╚═╝╚══════╝ ╚═════╝
 
*/

static __jo_force_inline jo_fixed           jo_lerp(const jo_fixed v0, const jo_fixed v1, const jo_fixed t)
{
    return (jo_fixed_mult((JO_FIXED_1 - t), v0) + jo_fixed_mult(t, v1));
}

static  __jo_force_inline bool  jo_is_float_equals_zero(const float f)
{
    return (JO_ABS(f) < 0.00000001f);
}

static  __jo_force_inline bool  jo_square_intersect(const int x1, const int y1, const int w1, const int h1,
        const int x2, const int y2, const int w2, const int h2)
{
    return ((x1 < x2 + w2) && (x2 < x1 + w1)) && ((y1 < y2 + h2) && (y2 < y1 + h1));
}

int     jo_gcd(int a, int b);

void            jo_planar_rotate(const jo_pos2D * const point, const jo_pos2D * const origin, const int angle, jo_pos2D * const result);

static __jo_force_inline short jo_direction_to_angle(const jo_8_directions direction)
{
    switch (direction)
    {
    case LEFT:
        return (180);
    case RIGHT:
        return (0);
    case UP:
        return (270);
    case DOWN:
        return (90);
    case UP_LEFT:
        return (225);
    case UP_RIGHT:
        return (315);
    case DOWN_LEFT:
        return (135);
    case DOWN_RIGHT:
        return (45);
    default:
        return (0);
    }
}
 
#endif /* !__JO_MATH_H__ */
 
/*
** END OF FILE
*/