xref: /dports/games/shockolate/systemshock-0.8.2-43-ga9eb1b93/src/Libraries/FIX/Source/fix.c

/*

Copyright (C) 2015-2018 Night Dive Studios, LLC.

This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program.  If not, see <http://www.gnu.org/licenses/>.

*/

/*
** fix.c
**
** $Header: r:/prj/lib/src/fix/RCS/fix.c 1.21 1994/08/11 12:11:09 dfan Exp $
** $Log: fix.c $
* Revision 1.21  1994/08/11  12:11:09  dfan
* move some stuff out
*
* Revision 1.20  1994/03/16  10:29:08  dfan
* fix_safe_pyth_dist etc.
*
* Revision 1.19  1994/03/01  11:28:44  dfan
* Do it for fix24's too
*
* Revision 1.18  1994/03/01  11:27:58  dfan
* Don't need quadrant/placing code in fix_atan, and besides,
* it's buggy
*
* Revision 1.17  1994/02/16  17:33:47  dfan
* allow arbitrarily small arguments to fix_exp
*
* Revision 1.16  1993/11/30  13:02:42  dfan
* safe fix_mul
*
* Revision 1.15  1993/11/04  11:06:09  rex
* Moved fix_sprintf() stuff out into fixsprnt.c
*
* Revision 1.14  1993/10/20  13:08:22  dfan
* some more fast_pyth_dists
*
* Revision 1.13  1993/09/17  13:03:30  dfan
* fast_pyth_dist
*
* Revision 1.12  1993/07/30  12:42:55  dfan
* fix_exp
*
* Revision 1.11  1993/06/27  03:09:43  dc
* pretty up hexprint formatting, sorry, whoops. etc.
*
* Revision 1.10  1993/06/27  02:30:24  dc
* fix_sprint now returns the buffer string, also added fix_sprint_hex
*
* Revision 1.9  1993/06/01  14:14:43  dfan
* Include trigtab.h, generated by fmaketab
* Lose a bit of precision, but now all trig functions work everywhere
*
* Revision 1.8  1993/04/19  13:31:33  dfan
* individual sin & cos functions
*
* Revision 1.7  1993/03/17  12:46:45  matt
* Removed 'const' from sincos declaration so could be ref'd in fix_asm.asm
*
* Revision 1.6  1993/03/02  10:51:30  dfan
* atan2: comparisons of sin should have been unsigned, not signed
*
* Revision 1.5  1993/02/15  12:15:21  dfan
* more fix24 functions
*
* Revision 1.4  1993/02/15  11:39:57  dfan
* fix24 support, not for all fuctions though
*
* Revision 1.3  1993/01/29  16:37:22  dfan
* maybe we shouldn't printf an error message in a library, what say
*
* Revision 1.2  1993/01/29  11:10:06  dfan
* Changed fix_pyth_dist to be straightforward (and twice as fast)
* The arctrig functions worked all along
*
* Revision 1.1  1993/01/22  09:57:39  dfan
* Initial revision
*
*/

#include "fix.h"
#include "lg.h"
#include "trigtab.h"
#include <stdint.h>
#include <stdlib.h>

fix fix_mul_3_3_3(fix a, fix b) { return (fix)(((int64_t)(a) * (int64_t)(b)) >> 29); }

fix fix_mul_3_32_16(fix a, fix b) { return (fix)(((int64_t)(a) * (int64_t)(b)) >> 13); }

fix fix_mul_3_16_20(fix a, fix b) { return (fix)(((int64_t)(a) * (int64_t)(b)) >> 33); }

fix fix_mul_16_32_20(fix a, fix b) { return (fix)(((int64_t)(a) * (int64_t)(b)) >> 4); }

fix fix_div_16_16_3(fix a, fix b) { return (fix)(((int64_t)a << 29) / (int64_t)b); }

int gOVResult;

fix fix_mul(fix a, fix b) { return (fix)(((int64_t)(a) * (int64_t)(b)) >> 16); }

fix fast_fix_mul_int(fix a, fix b) { return (fix)(((int64_t)(a) * (int64_t)(b)) >> 32); }

fix fix_mul_asm_safe(fix a, fix b) {
    int64_t intermediate = (int64_t)(a) * (int64_t)(b);
    // Apparently PowerPC and Motorola 68000 codes differ here
    // PowerPC will check if the lower 32 bits of the intemerdiate result are
    // equal to -1 (cmpi 1,r6,-1), while 68K will check if the lower *16* bits of
    // the intermediate result are equal to -1 (cmp.w #-1,d2). The result should
    // be the same because bits 31:16 of the intermediate result already are
    // tested for being equal to -1, but I'm leaving this comment here anyway for
    // future reference
    // int16_t d2 = intermediate & 0xFFFF;         // 68K code
    int32_t d2 = intermediate & 0xFFFFFFFF; // PPC code
    fix result = (fix)(intermediate >> 16);
    if (result == -1 && d2 != -1) {
        return 0;
    }
    return result;
}

// fix fix_div(fix a, fix b)
fix fix_div(fix a, fix b) {
    if (b == 0) {
        gOVResult = 2;
        fix r = 0x7FFFFFFF;
        if (a >= 0) {
            return r;
        }
        return -r;
    }
    gOVResult = 0;
    int64_t r64 = ((int64_t)(a) << 16) / (int64_t)(b);
    int32_t r32 = (int32_t)(r64 & 0xFFFFFFFF);
    if (r64 != (int64_t)r32) {
        gOVResult = 1;
        fix r = 0x7FFFFFFF;
        if (a >= 0) {
            return r;
        }
        return -r;
    }
    return r32;
}

// fix fix_div_int(fix a, fix b)
//{
//    return (fix)(((int64_t)(a) << 16) / (int64_t)(b));
//}
fix fix_div_int(fix a, fix b) {
    int64_t r64 = ((int64_t)(a) << 16) / (int64_t)(b);
    int32_t r32 = (int32_t)((r64 >> 16) & 0xFFFFFFFF);
    return r32;
}

// fix fix_div_safe_cint(fix a, fix b)
//{
//    return (fix)(((int64_t)(a) << 16) / (int64_t)(b));
//}
fix fix_div_safe_cint(fix a, fix b) {
    int64_t r64 = ((int64_t)(a) << 16) / (int64_t)(b);
    int32_t r32 = (int32_t)((r64 >> 16) & 0xFFFFFFFF);
    if ((r64 & 0xFFFF) != 0) {
        return r32 + 1;
    }
    return r32;
}

//----------------------------------------------------------------------------
// fix_div: Divide two fixed numbers.
//----------------------------------------------------------------------------

fix fix_mul_div(fix m0, fix m1, fix d) {
    // return (fix)(((int64_t)(m0) * (int64_t)(m1)) / (int64_t)(d));
    int64_t mr = ((int64_t)(m0) * (int64_t)(m1));
    if (d == 0) {
        gOVResult = 2;
        fix r = 0x7FFFFFFF;
        if (mr >= 0) {
            return r;
        }
        return -r;
    }
    gOVResult = 0;
    int64_t r64 = mr / (int64_t)(d);
    int32_t r32 = (int32_t)(r64 & 0xFFFFFFFF);
    if (r64 != (int64_t)r32) {
        gOVResult = 1;
        fix r = 0x7FFFFFFF;
        if (mr >= 0) {
            return r;
        }
        return -r;
    }
    return r32;
}

//----------------------------------------------------------------------------
// Returns the distance from (0,0) to (a,b)
//----------------------------------------------------------------------------
fix fix_pyth_dist(fix a, fix b) {
    gOVResult = 100;

    // @@@should check for overflow!
    return fix_sqrt(fix_mul(a, a) + fix_mul(b, b));
}

//----------------------------------------------------------------------------
// Returns an approximation to the distance from (0,0) to (a,b)
//----------------------------------------------------------------------------
fix fix_fast_pyth_dist(fix a, fix b) {
    if (a < 0)
        a = -a;
    if (b < 0)
        b = -b;
    if (a > b)
        return (a + b / 2);
    else
        return (b + a / 2);
}

//----------------------------------------------------------------------------
// We can use the fix function because the difference in scale doesn't matter.
//----------------------------------------------------------------------------
int long_fast_pyth_dist(int a, int b) { return (fix_fast_pyth_dist(a, b)); }

//----------------------------------------------------------------------------
// This function is safer than the other fix_pyth_dist because we don't
// have to worry about overflow.
//
// Uses algorithm from METAFONT involving reflecting (a,b) through
// line from (0,0) to (a,b/2), which keeps a^2+b^2 invariant but
// greatly reduces b.  When b reaches 0, a is the distance.
//
// Knuth credits it to Moler & Morrison, IBM Journal of Research and
// Development 27 (1983).  Good for them.
//----------------------------------------------------------------------------
fix fix_safe_pyth_dist(fix a, fix b) {
    fix tmp;

    a = abs(a);
    b = abs(b); // works fine since they're really longs
    if (a < b) {
        tmp = a;
        a = b;
        b = tmp;
    } // now 0 <= b <= a
    if (a > 0) {
        if (a > 0x2fffffff) {
            //			ssWarning (("Overflow in
            // fix_safe_pyth_dist\n"));  DebugStr("\pOverflow in fix_safe_pyth_dist");
            DEBUG("%s: Overflow in fix_safe_pyth_dist", __FUNCTION__);
            return 0;
        }
        for (;;) {
            // This is a quick way of doing the reflection
            tmp = fix_div(b, a);
            tmp = fix_mul(tmp, tmp);
            if (tmp == 0)
                break;
            tmp = fix_div(tmp, tmp + fix_make(4, 0));
            a += fix_mul(2 * a, tmp);
            b = fix_mul(b, tmp);
        }
    }
    return a;
}

//----------------------------------------------------------------------------
// Computes sin and cos of theta
//----------------------------------------------------------------------------
void fix_sincos(fixang theta, fix *sin, fix *cos) {
    uint8_t baseth, fracth;                // high and low bytes of the
    uint16_t lowsin, lowcos, hisin, hicos; // table lookups

    // divide the angle into high and low bytes
    // we will do a table lookup with the high byte and
    // interpolate with the low byte
    baseth = (uint8_t)(theta >> 8);
    fracth = (uint8_t)(theta & 0xff);

    // use the identity [cos x = sin (x + PI/2)] to look up
    // cosines in the sine table
    lowsin = sintab[baseth];
    hisin = sintab[baseth + 1];
    lowcos = sintab[baseth + 64];
    hicos = sintab[baseth + 65];

    // interpolate between low___ and hi___ according to fracth
    *sin = ((int16_t)(lowsin + ((((int16_t)hisin - (int16_t)lowsin) * fracth) >> 8))) << 2;
    *cos = ((int16_t)(lowcos + ((((int16_t)hicos - (int16_t)lowcos) * fracth) >> 8))) << 2;

    return;
}

//----------------------------------------------------------------------------
// Computes sin of theta
//----------------------------------------------------------------------------
fix fix_sin(fixang theta) {
    uint8_t baseth, fracth;
    uint16_t lowsin, hisin;

    baseth = (uint8_t)(theta >> 8);
    fracth = (uint8_t)(theta & 0xff);
    lowsin = sintab[baseth];
    hisin = sintab[baseth + 1];
    return ((int16_t)(lowsin + ((((int16_t)hisin - (int16_t)lowsin) * fracth) >> 8))) << 2;
}

//----------------------------------------------------------------------------
// Computes cos of theta
//----------------------------------------------------------------------------
fix fix_cos(fixang theta) {
    uint8_t baseth, fracth;
    uint16_t lowcos, hicos;

    baseth = (uint8_t)(theta >> 8);
    fracth = (uint8_t)(theta & 0xff);
    lowcos = sintab[baseth + 64];
    hicos = sintab[baseth + 65];
    return ((int16_t)(lowcos + ((((int16_t)hicos - (int16_t)lowcos) * fracth) >> 8))) << 2;
}

//----------------------------------------------------------------------------
// Computes sin and cos of theta
// Faster than fix_sincos() but not as accurate (does not interpolate)
//----------------------------------------------------------------------------
void fix_fastsincos(fixang theta, fix *sin, fix *cos) {
    // use the identity [cos x = sin (x + PI/2)] to look up
    // cosines in the sine table
    *sin = (((int16_t)(sintab[theta >> 8])) << 2);
    *cos = (((int16_t)(sintab[(theta >> 8) + 64])) << 2);

    return;
}

//----------------------------------------------------------------------------
// Fast sin of theta
//----------------------------------------------------------------------------
fix fix_fastsin(fixang theta) { return (((int16_t)(sintab[theta >> 8])) << 2); }

//----------------------------------------------------------------------------
// Fast cos of theta
//----------------------------------------------------------------------------
fix fix_fastcos(fixang theta) { return (((int16_t)(sintab[(theta >> 8) + 64])) << 2); }

//----------------------------------------------------------------------------
// Computes the arcsin of x
// Assumes -1 <= x <= 1
// Returns 0xc000..0x4000 (-PI/2..PI/2)
//----------------------------------------------------------------------------
fixang fix_asin(fix x) {
    uint8_t basex, fracx; // high and low bytes of x
    fixang lowy, hiy;     // table lookups

    // divide x into high and low bytes
    // lookup with the high byte, interpolate with the low
    // We shift basex around to make it continuous; see trigtab.h

    basex = (uint8_t)(((x >> 2) >> 8) + 0x40);
    fracx = (uint8_t)((x >> 2) & 0xff);

    lowy = asintab[basex];
    hiy = asintab[basex + 1];

    // interpolate between lowy and hiy according to fracx
    return (lowy + ((((int16_t)hiy - (int16_t)lowy) * fracx) >> 8));
}

//----------------------------------------------------------------------------
// Computes the arccos of x
// Returns 0x0000..0x8000 (0..PI)
//----------------------------------------------------------------------------
fixang fix_acos(fix x) {
    uint8_t basex, fracx;
    uint16_t lowy, hiy;
    fixang asin_answer;

    // acos(x) = PI/2 - asin(x)

    basex = (uint8_t)(((x >> 2) >> 8) + 0x40);
    fracx = (uint8_t)((x >> 2) & 0xff);

    lowy = asintab[basex];
    hiy = asintab[basex + 1];

    asin_answer = (lowy + ((((int16_t)hiy - (int16_t)lowy) * fracx) >> 8));
    return ((fixang)0x4000 - asin_answer);
}

//----------------------------------------------------------------------------
// Computes the atan of y/x, in the correct quadrant and everything
//----------------------------------------------------------------------------
fixang fix_atan2(fix y, fix x) {
    fix hyp;   // hypotenuse
    fix s, c;  // sine, cosine
    fixang th; // our answer

    // Get special cases out of the way so we don't have to deal
    // with things like making sure 1 gets converted to 0x7fff and
    // not 0x8000.  Note that we grab the y = x = 0 case here
    if (y == 0) {
        if (x >= 0)
            return 0x0000;
        else
            return 0x8000;
    } else if (x == 0) {
        if (y >= 0)
            return 0x4000;
        else
            return 0xc000;
    }

    if ((hyp = fix_pyth_dist(x, y)) == 0) {
        //		printf ("hey, dist was 0\n");

        return 0;
    }

    // Use fix_asin or fix_acos depending on where we are.  We don't want to use
    // fix_asin if the sin is close to 1 or -1
    s = fix_div(y, hyp);
    if ((uint32_t)s < 0x00004000 || (uint32_t)s > 0xffffc000) { // range is good, use asin
        th = fix_asin(s);
        if (x < 0) {
            if (th < 0x4000)
                th = 0x8000 - th;
            else
                th = ~th + 0x8000; // that is, 0xffff - th + 0x8000
        }
    } else { // use acos instead
        c = fix_div(x, hyp);
        th = fix_acos(c);
        if (y < 0) {
            th = ~th; // that is, 0xffff - th
        }
    }

        // The above (x < 0) and (y < 0) conditionals should take care of placing us
        // in the correct quadrant, so we shouldn't need the code below.
        // Additionally, the code below can cause rounding errors when (th & 0x3fff
        // == 0).  So let's try omitting it.

#ifdef NO_NEED
    // set high bits based on what quadrant we are in
    th &= 0x3fff;
    th |= (y > 0 ? (x > 0 ? 0x0000 : 0x4000) : (x > 0 ? 0xc000 : 0x8000));
#endif

    return th;
}

fix24 fix24_mul(fix24 a, fix24 b) { return (fix24)(((int64_t)a * (int64_t)b) >> 8); }

fix24 fix24_div(fix24 a, fix24 b) { return (fix24)(((int64_t)a << 8) / (int64_t)b); }

fix fix_pow(fix x, fix y) {
    int i;
    fix ans;
    fix rh, rl;
    uint16_t yh, yl;

    ans = FIX_UNIT;
    yh = (uint16_t)(fix_int(y));
    yl = fix_frac(y);
    rh = rl = x;

    // calculate hi part, leave when done
    for (i = 0; i < 16; ++i) {
        if (yh & 1)
            ans = fix_mul(ans, rh);
        if (yh != 0)
            rh = fix_mul(rh, rh);
        yh = yh >> 1;
        if (yl != 0)
            rl = fix_sqrt(rl);
        if (yl & 0x8000)
            ans = fix_mul(ans, rl);
        yl = yl << 1;
    }
    return ans;
}

int32_t fix64_div(int64_t a, int32_t b) {
    return (int32_t) (a / b);
}

int64_t fix64_mul(int32_t a, int32_t b) {
    return (int64_t)a * (int64_t)b;
}