fixedptc.c

Go to the documentation of this file.

#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <stdint.h>
#include <assert.h>
#include "fixedptc.h"


static char str[25];
/* Multiplies two fixedpt numbers, returns the result. */
fixedpt
fixedpt_mul(fixedpt A, fixedpt B)
{
  fixedpt ret = (((fixedptd)A * (fixedptd)B) >> FIXEDPT_FBITS);
  if (A > 0 && B > 0 && ret < 0)
    assert("MUL: sign change\n");
  if (A < 0 && B < 0 && ret > 0)
    assert("MUL: sign change\n");
  return ret;
}


/* Divides two fixedpt numbers, returns the result. */
fixedpt
fixedpt_div(fixedpt A, fixedpt B)
{
  fixedpt ret = (((fixedptd)A << FIXEDPT_FBITS) / (fixedptd)B);
  if (A > 0 && B > 0 && ret < 0)
    assert("DIV: sign change\n");
  if (A < 0 && B < 0 && ret > 0)
    assert("DIV: sign change\n");
  return ret;
}

/*
 * Note: adding and substracting fixedpt numbers can be done by using
 * the regular integer operators + and -.
 */

void
fixedpt_str(fixedpt A, char *str, int max_dec)
{
    int ndec = 0, slen = 0;
    char tmp[12] = {0};
    fixedptud fr, ip;
    const fixedptud one = (fixedptud)1 << FIXEDPT_BITS;
    const fixedptud mask = one - 1;

    if (max_dec == -1)
#if FIXEDPT_BITS == 32
        max_dec = 2;
#elif FIXEDPT_BITS == 64
        max_dec = 10;
#else
#error Invalid width
#endif
    else if (max_dec == -2)
        max_dec = 15;

    if (A < 0) {
        str[slen++] = '-';
        A *= -1;
    }

    ip = fixedpt_toint(A);
    do {
        tmp[ndec++] = '0' + ip % 10;
        ip /= 10;
    } while (ip != 0);

    while (ndec > 0)
        str[slen++] = tmp[--ndec];
    str[slen++] = '.';

    //    printf("before shift left %x\n", fr);
    fr = (fixedpt_fracpart(A) << FIXEDPT_WBITS) & mask;
//        printf("before iter %x\n", fr);
    do {
        fr = (fr & mask) * 10;
  //              printf("iter %x\n", fr);
        str[slen++] = '0' + (fr >> FIXEDPT_BITS) % 10;
   //             printf("char %c\n", '0' + (fr >> FIXEDPT_BITS) % 10);
        ndec++;
    } while (fr != 0 && ndec < max_dec);

    if (ndec > 1 && str[slen-1] == '0')
        str[slen-1] = '\0'; /* cut off trailing 0 */
    else
        str[slen] = '\0';
}

/* Converts the given fixedpt number into a string, using a static
 * (non-threadsafe) string buffer */
char*
fixedpt_cstr(const fixedpt A, const int max_dec)
{

    fixedpt_str(A, str, max_dec);
    return (str);
}


/* Returns the square root of the given number, or -1 in case of error */
fixedpt
fixedpt_sqrt(fixedpt A)
{
        int invert = 0;
        int iter = FIXEDPT_FBITS;
    //        int l, i;
    int i;
    fixedpt l;

        if (A < 0)
                return (-1);
        if (A == 0 || A == FIXEDPT_ONE)
                return (A);
        if (A < FIXEDPT_ONE && A > 6) {
                invert = 1;
                A = fixedpt_div(FIXEDPT_ONE, A);
        }
        if (A > FIXEDPT_ONE) {
            fixedpt s = A; //??? janders int s = A;

                iter = 0;
                while (s > 0) {
                        s >>= 2;
                        iter++;
                }
        }

        /* Newton's iterations */
        l = (A >> 1) + 1;
        for (i = 0; i < iter; i++)
                l = (l + fixedpt_div(A, l)) >> 1;
        if (invert)
                return (fixedpt_div(FIXEDPT_ONE, l));
        return (l);
}


/* Returns the sine of the given fixedpt number.
 * Note: the loss of precision is extraordinary! */
fixedpt
fixedpt_sin(fixedpt fp)
{
    int sign = 1;
    fixedpt sqr, result;
    const fixedpt SK[2] = {
        fixedpt_rconst(7.61e-03),
        fixedpt_rconst(1.6605e-01)
    };

    fp %= 2 * FIXEDPT_PI;
    if (fp < 0)
        fp = FIXEDPT_PI * 2 + fp;
    if ((fp > FIXEDPT_HALF_PI) && (fp <= FIXEDPT_PI))
        fp = FIXEDPT_PI - fp;
    else if ((fp > FIXEDPT_PI) && (fp <= (FIXEDPT_PI + FIXEDPT_HALF_PI))) {
        fp = fp - FIXEDPT_PI;
        sign = -1;
    } else if (fp > (FIXEDPT_PI + FIXEDPT_HALF_PI)) {
        fp = (FIXEDPT_PI << 1) - fp;
        sign = -1;
    }
    sqr = fixedpt_mul(fp, fp);
    result = SK[0];
    result = fixedpt_mul(result, sqr);
    result -= SK[1];
    result = fixedpt_mul(result, sqr);
    result += FIXEDPT_ONE;
    result = fixedpt_mul(result, fp);
    return sign * result;
}


/* Returns the cosine of the given fixedpt number */
fixedpt
fixedpt_cos(fixedpt A)
{
    return (fixedpt_sin(FIXEDPT_HALF_PI - A));
}


/* Returns the tangens of the given fixedpt number */
fixedpt
fixedpt_tan(fixedpt A)
{
    return fixedpt_div(fixedpt_sin(A), fixedpt_cos(A));
}


/* Returns the value exp(x), i.e. e^x of the given fixedpt number. */
fixedpt
fixedpt_exp(fixedpt fp)
{
    fixedpt xabs, k, z, R, xp, ans;
    const fixedpt LN2 = fixedpt_rconst(0.69314718055994530942);
    const fixedpt LN2_INV = fixedpt_rconst(1.4426950408889634074);
    const fixedpt EXP_P[5] = {
        fixedpt_rconst(1.66666666666666019037e-01),
        fixedpt_rconst(-2.77777777770155933842e-03),
        fixedpt_rconst(6.61375632143793436117e-05),
        fixedpt_rconst(-1.65339022054652515390e-06),
        fixedpt_rconst(4.13813679705723846039e-08),
    };

    if (fp == 0){
//            printf("excuted fp = 0 ");
            return (FIXEDPT_ONE);
        }


    xabs = fixedpt_abs(fp);
//        printf("xabs = fixedpt_abs(fp); = ");
//        fixedpt_print(xabs);

    k = fixedpt_mul(xabs, LN2_INV);
        //switch the exponents to base 2
        //now calculating 2^k
//        printf("k = fixedpt_mul(xabs, LN2_INV); = ");
//        fixedpt_print(k);

    k += FIXEDPT_ONE_HALF;
//        printf("k += FIXEDPT_ONE_HALF; = ");
//        fixedpt_print(k);

    k &= ~FIXEDPT_FMASK;
//        printf("k &= ~FIXEDPT_FMASK; = ");
//        fixedpt_print(k);

    if (fp < 0){
            k = -k;
//            printf("k = -k; = ");
//            fixedpt_print(k);
        }

    fp -= fixedpt_mul(k, LN2);
//        printf("fp -= fixedpt_mul(k, LN2);");
//        fixedpt_print(fp);

    z = fixedpt_mul(fp, fp);
//        printf("z = fixedpt_mul(fp, fp); = ");
//        fixedpt_print(z);


    /* Taylor */
    R = FIXEDPT_TWO +
        fixedpt_mul(z, EXP_P[0] + fixedpt_mul(z, EXP_P[1] +
        fixedpt_mul(z, EXP_P[2] + fixedpt_mul(z, EXP_P[3] +
        fixedpt_mul(z, EXP_P[4])))));
//        printf("R = FIXEDPT_TWO +"
//        "fixedpt_mul(z, EXP_P[0] + fixedpt_mul(z, EXP_P[1] +"
//        "fixedpt_mul(z, EXP_P[2] + fixedpt_mul(z, EXP_P[3] +"
//        "fixedpt_mul(z, EXP_P[4])))));= ");
//        fixedpt_print(R);


    xp = FIXEDPT_ONE + fixedpt_div(fixedpt_mul(fp, FIXEDPT_TWO), R - fp);
//        printf("xp = FIXEDPT_ONE + fixedpt_div(fixedpt_mul(fp, FIXEDPT_TWO), R - fp); ");
//        fixedpt_print(xp);


    if (k < 0){
        k = FIXEDPT_ONE >> (-k >> FIXEDPT_FBITS);
//                printf("k = FIXEDPT_ONE >> (-k >> FIXEDPT_FBITS);");
//                fixedpt_print(k);
        }
    else{
        k = FIXEDPT_ONE << (k >> FIXEDPT_FBITS);
//                printf("k = FIXEDPT_ONE << (k >> FIXEDPT_FBITS);");
//                fixedpt_print(k);
        }
        ans = fixedpt_mul(k, xp);
//        printf("ans = fixedpt_mul(k, xp)");
//        fixedpt_print(ans);
    return (ans);
}


/* Returns the natural logarithm of the given fixedpt number. */
fixedpt
fixedpt_ln(fixedpt x)
{
  const fixedpt table[] = {-363409, -317983, -291410, -272557, -257933, -245984, -235882, -227130, -219411, -212507, -206260, -200558, -195312, -190455, -185934, -181704, -177731, -173985, -170442, -167080, -163883, -160834, -157921, -155132, -152457, -149886, -147413, -145029, -142730, -140508, -138359, -136278, -134262, -132305, -130405, -128559, -126764, -125016, -123314, -121654, -120036, -118457, -116915, -115408, -113935, -112495, -111085, -109706, -108354, -107030, -105733, -104460, -103212, -101987, -100784, -99603, -98443, -97304, -96183, -95082, -93999, -92933, -91884, -90852, -89836, -88836, -87850, -86879, -85922, -84979, -84050, -83133, -82229, -81338, -80458, -79590, -78733, -77887, -77053, -76228, -75414, -74610, -73816, -73031, -72255, -71489, -70731, -69982, -69241, -68509, -67785, -67069, -66360, -65659, -64966, -64280, -63600, -62928, -62263, -61604, -60952, -60307, -59667, -59034, -58407, -57786, -57170, -56561, -55957, -55358, -54765, -54177, -53595, -53017, -52445, -51877, -51315, -50757, -50204, -49656, -49112, -48572, -48037, -47507, -46980, -46458, -45940, -45426, -44916, -44410, -43908, -43409, -42915, -42424, -41937, -41453, -40973, -40496, -40023, -39553, -39087, -38624, -38164, -37707, -37254, -36803, -36356, -35911, -35470, -35032, -34596, -34164, -33734, -33307, -32883, -32461, -32043, -31627, -31213, -30802, -30394, -29988, -29585, -29184, -28786, -28390, -27996, -27605, -27216, -26829, -26445, -26063, -25683, -25305, -24929, -24556, -24185, -23815, -23448, -23083, -22720, -22359, -22000, -21643, -21288, -20934, -20583, -20233, -19886, -19540, -19196, -18854, -18513, -18174, -17837, -17502, -17169, -16837, -16507, -16178, -15851, -15526, -15202, -14880, -14560, -14241, -13924, -13608, -13294, -12981, -12669, -12360, -12051, -11744, -11439, -11135, -10832, -10530, -10231, -9932, -9635, -9339, -9044, -8751, -8459, -8169, -7879, -7591, -7304, -7019, -6734, -6451, -6169, -5889, -5609, -5331, -5054, -4778, -4503, -4230, -3957, -3686, -3415, -3146, -2878, -2611, -2345, -2081, -1817, -1554, -1293, -1032, -773, -514, -257, 0};
  return table[(x >> (FIXEDPT_FBITS-8)) & 0xFF];
#if 0 // janders
    fixedpt log2, xi;
    fixedpt f, s, z, w, R;
    const fixedpt LN2 = fixedpt_rconst(0.69314718055994530942);
    const fixedpt LG[7] = {
        fixedpt_rconst(6.666666666666735130e-01),
        fixedpt_rconst(3.999999999940941908e-01),
        fixedpt_rconst(2.857142874366239149e-01),
        fixedpt_rconst(2.222219843214978396e-01),
        fixedpt_rconst(1.818357216161805012e-01),
        fixedpt_rconst(1.531383769920937332e-01),
        fixedpt_rconst(1.479819860511658591e-01)
    };

    if (x < 0)
        return (0);
    if (x == 0)
        return 0xffffffff;

    log2 = 0;
    xi = x;
    while (xi > FIXEDPT_TWO) {
        xi >>= 1;
        log2++;
    }
    f = xi - FIXEDPT_ONE;
    s = fixedpt_div(f, FIXEDPT_TWO + f);
    z = fixedpt_mul(s, s);
    w = fixedpt_mul(z, z);
    R = fixedpt_mul(w, LG[1] + fixedpt_mul(w, LG[3]
        + fixedpt_mul(w, LG[5]))) + fixedpt_mul(z, LG[0]
        + fixedpt_mul(w, LG[2] + fixedpt_mul(w, LG[4]
        + fixedpt_mul(w, LG[6]))));
    return (fixedpt_mul(LN2, (log2 << FIXEDPT_FBITS)) + f
        - fixedpt_mul(s, f - R));
#endif
}


/* Returns the logarithm of the given base of the given fixedpt number */
fixedpt
fixedpt_log(fixedpt x, fixedpt base)
{
    return (fixedpt_div(fixedpt_ln(x), fixedpt_ln(base)));
}


/* Return the power value (n^exp) of the given fixedpt numbers */
fixedpt
fixedpt_pow(fixedpt n, fixedpt exp)
{
    if (exp == 0)
        return (FIXEDPT_ONE);
    if (n < 0)
        return 0;
    return (fixedpt_exp(fixedpt_mul(fixedpt_ln(n), exp)));
}
/*
void
fixedpt_print(fixedpt A)
{
    char num[30];

    fixedpt_str(A, num, -2);
    puts(num);
}
*/
/*
void
fixedpt_print_file(fixedpt A)
{
    char num[30];
        int j;
        FILE *output;
        output = fopen ( "MonteCarlo.txt", "a" );

        if ( !output )
        {
            fprintf ( stderr, "\n" );
            fprintf ( stderr, "fixedpt_print_file - Fatal error!\n" );
            fprintf ( stderr, "  Could not open the output file.\n" );
            return;
        }
       //c printf("PRINT_FUNCTON **********************************************\n");
//        for ( j = 0; j <= n; j++ )
//        {
            fixedpt_str(A, num, -2);
            printf("Blahhhh");
            puts(num);
            fputs(num, output);
            fprintf(output, "\n");
            //fprintf ( output, "  %24.16g\n", num );
      //  }
}*/