fixedpoint.c

一个做移动机器人方面的外国人的个人网站上的程序源码

/*
  fixedpoint (c) 2006 Kris Beevers

  This file is part of fixedpoint.

  fixedpoint 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 2 of the License, or
  (at your option) any later version.

  fixedpoint 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 fixedpoint; if not, write to the Free Software
  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301
  USA

  $Id: fixedpoint.c,v 1.5 2007/02/06 03:27:04 beevek Exp $
*/

#include <stdlib.h>
#include <math.h>
#include "fixedpoint.h"
#include "tables.h"

#ifndef _USE_DOUBLE

int fp_check_overflow(long_fixed_t f)
{
  if(f>>FP_DEC_BITS > fp_max_value)
    return 1;
  else
    return 0;
}

fixed_t int2fp(fixed_t i) { return i << FP_DEC_BITS; }
int32_t fp2int(fixed_t f) { return f >> FP_DEC_BITS; }

fixed_t float2fp(float f) { return (fixed_t)(f * (((long_fixed_t)1)<<FP_DEC_BITS)); }
float fp2float(fixed_t f) { return (float)f / (float)(((long_fixed_t)1)<<FP_DEC_BITS); }

fixed_t fp_mul(fixed_t a, fixed_t b)
{
  long_fixed_t i = (long_fixed_t)a * (long_fixed_t)b;
  return (fixed_t)(i>>FP_DEC_BITS);
}

fixed_t fp_div(fixed_t a, fixed_t b)
{
  long_fixed_t i = ((long_fixed_t)a << FP_DEC_BITS) / (long_fixed_t)b;
  return (fixed_t)i;
}

void fp_srand(int seed)
{
  srand(seed);
}

fixed_t fp_rand(void)
{
#if !defined(_USE_SHORT_FIXED_POINT) && defined(__AVR__)
  // sizeof(int) == 2 on the avr so to generate a 32-bit random number
  // we need two calls to rand()
  return (fixed_t)((((long_fixed_t)rand()) << 16) | rand());
#else
  return (fixed_t)rand();
#endif
}

fixed_t fp_rand_0_1(void)
{
  // only keep the random bits representing the decimal part
  return fp_rand() & FP_DEC_MASK;
}

fixed_t fp_clamp_angle(fixed_t a)
{
  if(a < -fp_pi)
    return a + fp_2_pi * ((-a + fp_pi)/fp_2_pi);
  else if(a > fp_pi)
    return a - fp_2_pi * ((a + fp_pi)/fp_2_pi);
  else
    return a;
}

fixed_t fp_abs(fixed_t x)
{
  return (x < 0) ? -x : x;
}

fixed_t fp_min(fixed_t a, fixed_t b)
{
  return (a < b) ? a : b;
}

fixed_t fp_max(fixed_t a, fixed_t b)
{
  return (a > b) ? a : b;
}

fixed_t fp_floor(fixed_t x)
{
  return (x >= 0) ? (x>>FP_DEC_BITS) : ~((~x)>>FP_DEC_BITS);
}

fixed_t fp_cos(fixed_t x)
{
  x = fp_clamp_angle(x); // verify -pi <= x <= pi
  if(x < 0)
    return pgm_read_fixed(&cos_table[(-x) >> fp_trig_shift]);
  else
    return pgm_read_fixed(&cos_table[x >> fp_trig_shift]);
}

fixed_t fp_sin(fixed_t x)
{
  x = fp_clamp_angle(x); // verify -pi <= x <= pi
  if(x < 0)
    return -pgm_read_fixed(&sin_table[(-x) >> fp_trig_shift]);
  else
    return pgm_read_fixed(&sin_table[x >> fp_trig_shift]);
}

fixed_t fp_tan(fixed_t x)
{
  x = fp_clamp_angle(x); // verify -pi <= x <= pi
  if(x < 0)
    return -pgm_read_fixed(&tan_table[(-x) >> fp_trig_shift]);
  else
    return pgm_read_fixed(&tan_table[x >> fp_trig_shift]);
}

fixed_t fp_atan(fixed_t x)
{
  int min, max, c;
  fixed_t diff;

  if(x >= 0) {
    min = 0;
    max = TRIG_TABLE_SIZE >> 1;
  } else {
    min = (TRIG_TABLE_SIZE >> 1) + 1;
    max = TRIG_TABLE_SIZE;
  }

  // do binary search on the tan table
  do {
    c = (min + max) >> 1;
    diff = x - pgm_read_fixed(&tan_table[c]);

    if(diff > 0)
      min = c + 1;
    else if(diff < 0)
      max = c - 1;
  } while((min <= max) && diff != 0);

  //  x = (x < 0) ? int2fp(c) - fp_2_pi : int2fp(c); // exploit symmetric nature of arctan
  if(x < 0)
    return (c << fp_trig_shift) - fp_pi;
  else
    return (c << fp_trig_shift);
}

fixed_t fp_atan2(fixed_t y, fixed_t x)
{
  fixed_t r;

  if(x == 0) {
    if(y == 0) return 0;
    return (y < 0) ? -fp_pi_2 : fp_pi_2;
  }

  r = fp_atan(fp_div(y, x));
  if(x >= 0)
    return r;
  if(y >= 0)
    return r + fp_pi;
  return r - fp_pi;
}

fixed_t fp_sqrt(fixed_t x)
{
  //  if(x <= 0) return 0;
  if(x <= 1) return 0; // we can't divide 0x0001 in half
  fixed_t res = x >> 1; // start at x/2
  const fixed_t max_err = fp_mul(x, fp_epsilon); // maximum allowed error
  fixed_t delta;

  do {
    delta = fp_mul(res,res) - x;
    res -= fp_div(delta, fp_mul(fp_2, res));
  } while(delta > max_err || delta < -max_err);

  return res;
}

// based on the identity:
//   ln(x) = lim_{h->0} ((x^h)-1)/h
fixed_t fp_log(fixed_t x)
{
  // FIXME: check for overflow?
  fixed_t w = int2fp(1), y, z;
  int num = 1, dec = 0;

  for(; fp_abs(x) >= fp_2; ++dec)
    x = fp_div(x, fp_e);

  x -= fp_1;
  z = x;
  y = x;

  while(y != y + w && num < 32) {
    z = 0 - fp_mul(z, x);
    ++num;
    w = fp_div(z, int2fp(num));
    y += w;
  }

  return y + int2fp(dec);
}

// based on the taylor expansion:
//   exp(x) = 1 + x + x^2/2! + x^3/3! ...
fixed_t fp_exp(fixed_t x)
{
  // FIXME: check for overflow?
  fixed_t w = fp_1, y = fp_1;
  int n;
  for(n = 1; y != y+w; ++n) {
    w = fp_mul(w, fp_div(x, int2fp(n)));
    y += w;
  }
  return y;
}

#else // _USE_DOUBLE

fixed_t int2fp(int32_t i) { return (fixed_t)i; }
int32_t fp2int(fixed_t f) { return (int32_t)f; }
fixed_t float2fp(float f) { return (fixed_t)f; }
float fp2float(fixed_t f) { return (float)f; }

fixed_t fp_mul(fixed_t a, fixed_t b) { return a*b; }
fixed_t fp_div(fixed_t a, fixed_t b) { return a/b; }

void fp_srand(int seed) { srand48(seed); }
fixed_t fp_rand(void) { return (DBL_MAX*drand48())-(DBL_MAX/2.0); } // don't want to overflow
fixed_t fp_rand_0_1(void) { return drand48(); } // 0 .. 1

fixed_t fp_clamp_angle(fixed_t a)
{
  while(a < -fp_pi) a += fp_2_pi;
  while(a > fp_pi) a -= fp_2_pi;
  return a;
}

fixed_t fp_abs(fixed_t x) { return fabs(x); }
fixed_t fp_min(fixed_t a, fixed_t b) { return (a < b) ? a : b; }
fixed_t fp_max(fixed_t a, fixed_t b) { return (a > b) ? a : b; }
fixed_t fp_floor(fixed_t x) { return floor(x); }
fixed_t fp_cos(fixed_t x) { return cos(x); }
fixed_t fp_sin(fixed_t x) { return sin(x); }
fixed_t fp_tan(fixed_t x) { return tan(x); }
fixed_t fp_atan(fixed_t x) { return atan(x); }
fixed_t fp_atan2(fixed_t y, fixed_t x) { return atan2(y, x); }
fixed_t fp_sqrt(fixed_t x) { return sqrt(x); }
fixed_t fp_log(fixed_t x) { return log(x); }
fixed_t fp_exp(fixed_t x) { return exp(x); }

#endif // _USE_DOUBLE