Subversion Repositories Games.Descent

/*

 * This file is part of the DXX-Rebirth project <https://www.dxx-rebirth.com/>.

 * It is copyright by its individual contributors, as recorded in the

 * project's Git history.  See COPYING.txt at the top level for license

 * terms and a link to the Git history.

 */

/*

 *

 * C version of fixed point library

 *

 */

#include <cstdint>

#include <stdlib.h>

#include <math.h>

#include "dxxerror.h"

#include "maths.h"

namespace dcx {

#define EPSILON (F1_0/100)

namespace {

class fix_sincos_input

{

public:

        const uint8_t m_idx;

        const signed m_mul;

        fix_sincos_input(fix a) :

                m_idx(static_cast<uint8_t>(a >> 8)),

                m_mul(static_cast<uint8_t>(a))

        {

        }

};

}

fix64 fixmul64(fix a, fix b)

{

        const fix64 a64 = a;

        const fix64 b64 = b;

        return (a64 * b64) / 65536;

}

fix fixdiv(fix a, fix b)

{

        if (!b)

                return 1;

        const fix64 a64 = a;

        return static_cast<fix>((a64 * 65536) / b);

}

fix fixmuldiv(fix a, fix b, fix c)

{

        if (!c)

                return 1;

        const fix64 a64 = a;

        return static_cast<fix>((a64 * b) / c);

}

//given cos & sin of an angle, return that angle.

//parms need not be normalized, that is, the ratio of the parms cos/sin must

//equal the ratio of the actual cos & sin for the result angle, but the parms 

//need not be the actual cos & sin.  

//NOTE: this is different from the standard C atan2, since it is left-handed.

fixang fix_atan2(fix cos,fix sin)

{

        fixang t;

        //Assert(!(cos==0 && sin==0));

        //find smaller of two

        const auto dsin = static_cast<double>(sin);

        const auto dcos = static_cast<double>(cos);

        double d;

        d = sqrt((dsin * dsin) + (dcos * dcos));

        if (d==0.0)

                return 0;

        if (labs(sin) < labs(cos)) {                            //sin is smaller, use arcsin

                t = fix_asin(static_cast<fix>((dsin / d) * 65536.0));

                if (cos<0)

                        t = 0x8000 - t;

                return t;

        }

        else {

                t = fix_acos(static_cast<fix>((dcos / d) * 65536.0));

                if (sin<0)

                        t = -t;

                return t;

        }

}

static unsigned int fixdivquadlongu(quadint n, uint64_t d)

{

        return n.q / d;

}

uint32_t quad_sqrt(const quadint iq)

{

        const uint32_t low = iq.low;

        const int32_t high = iq.high;

        int i, cnt;

        uint32_t r,old_r,t;

        if (high<0)

                return 0;

        if (high==0 && static_cast<int32_t>(low)>=0)

                return long_sqrt(static_cast<int32_t>(low));

        if ((i = high >> 24)) {

                cnt=12+16;

        }

        else if ((i = high >> 16))

        {

                cnt=8+16;

        }

        else if ((i = high >> 8))

        {

                cnt=4+16;

        } else {

                cnt=0+16; i = high;

        }

        r = guess_table[i]<<cnt;

        //quad loop usually executed 4 times

        r = fixdivquadlongu(iq,r)/2 + r/2;

        r = fixdivquadlongu(iq,r)/2 + r/2;

        r = fixdivquadlongu(iq,r)/2 + r/2;

        do {

                old_r = r;

                t = fixdivquadlongu(iq,r);

                if (t==r)       //got it!

                        return r;

                r = t/2 + r/2;

        } while (!(r==t || r==old_r));

        t = fixdivquadlongu(iq,r);

        quadint tq;

        //edited 05/17/99 Matt Mueller - tq.high is undefined here.. so set them to = 0

        tq.q = 0;

        //end edit -MM

        fixmulaccum(&tq,r,t);

        if (tq.q != iq.q)

                r++;

        return r;

}

//computes the square root of a long, returning a short

ushort long_sqrt(int32_t a)

{

        int cnt,r,old_r,t;

        if (a<=0)

                return 0;

        if (a & 0xff000000)

                cnt=12;

        else if (a & 0xff0000)

                cnt=8;

        else if (a & 0xff00)

                cnt=4;

        else

                cnt=0;

        r = guess_table[(a>>cnt)&0xff]<<cnt;

        //the loop nearly always executes 3 times, so we'll unroll it 2 times and

        //not do any checking until after the third time.  By my calcutations, the

        //loop is executed 2 times in 99.97% of cases, 3 times in 93.65% of cases, 

        //four times in 16.18% of cases, and five times in 0.44% of cases.  It never

        //executes more than five times.  By timing, I determined that is is faster

        //to always execute three times and not check for termination the first two

        //times through.  This means that in 93.65% of cases, we save 6 cmp/jcc pairs,

        //and in 6.35% of cases we do an extra divide.  In real life, these numbers

        //might not be the same.

        r = ((a/r)+r)/2;

        r = ((a/r)+r)/2;

        do {

                old_r = r;

                t = a/r;

                if (t==r)       //got it!

                        return r;

                r = (t+r)/2;

        } while (!(r==t || r==old_r));

        if (a % r)

                r++;

        return r;

}

//computes the square root of a fix, returning a fix

fix fix_sqrt(fix a)

{

        return static_cast<fix>(long_sqrt(a)) << 8;

}

static fix fix_sincos(const uint8_t idx0, const signed mul)

{

        const fix t0 = sincos_table[idx0];

        const uint8_t idx1 = idx0 + 1;

        const fix t1 = sincos_table[idx1];

        return (t0 + (((t1 - t0) * mul) >> 8)) << 2;

}

static fix fix_sin(const fix_sincos_input sci)

{

        return fix_sincos(sci.m_idx, sci.m_mul);

}

__attribute_warn_unused_result

static fix fix_cos(const fix_sincos_input sci)

{

        return fix_sincos(static_cast<uint8_t>(sci.m_idx + 64), sci.m_mul);

}

//compute sine and cosine of an angle, filling in the variables

//either of the pointers can be NULL

//with interpolation

fix_sincos_result fix_sincos(fix a)

{

        fix_sincos_input i(a);

        return {fix_sin(i), fix_cos(i)};

}

fix fix_sin(fix a)

{

        return fix_sin(fix_sincos_input{a});

}

fix fix_cos(fix a)

{

        return fix_cos(fix_sincos_input{a});

}

//compute sine and cosine of an angle, filling in the variables

//either of the pointers can be NULL

//no interpolation

fix fix_fastsin(fix a)

{

        const uint8_t i = static_cast<uint8_t>(a >> 8);

        return sincos_table[i] << 2;

}

//compute inverse sine

fixang fix_asin(fix v)

{

        fix vv;

        int i,f,aa;

        vv = labs(v);

        if (vv >= f1_0)         //check for out of range

                return 0x4000;

        i = (vv>>8)&0xff;

        f = vv&0xff;

        aa = asin_table[i];

        aa = aa + (((asin_table[i+1] - aa) * f)>>8);

        if (v < 0)

                aa = -aa;

        return aa;

}

//compute inverse cosine

fixang fix_acos(fix v)

{

        fix vv;

        int i,f,aa;

        vv = labs(v);

        if (vv >= f1_0)         //check for out of range

                return 0;

        i = (vv>>8)&0xff;

        f = vv&0xff;

        aa = acos_table[i];

        aa = aa + (((acos_table[i+1] - aa) * f)>>8);

        if (v < 0)

                aa = 0x8000 - aa;

        return aa;

}

}