fixedptc.c

Go to the documentation of this file.

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


char str[25] = {0};

/* Multiplies two fixedpt numbers, returns the result. */
fixedpt fixedpt_mul(fixedpt A, fixedpt B)
{
  return (((fixedptd)A * (fixedptd)B) >> FIXEDPT_FBITS);
}

/* Divides two fixedpt numbers, returns the result. */
fixedpt fixedpt_div(fixedpt A, fixedpt B)
{
  return (((fixedptd)A << FIXEDPT_FBITS) / (fixedptd)B);
}

/*
 * 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;

  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) {
    int s = A;

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

  /* Newton's iterations */
  int l = (A >> 1) + 1;
  for (int 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)
{
  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));
}

/* 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)));
}