jDttSP/fastrig.c

   1
   2 /****************************************************************
   3  *   Fast Trigonometric Routines Used for Embedded Systems      *
   4  *   Programmer:  Bob McGwier, IDA CCR-P, June, 2000            *
   5  ***************************************************************/
   6 /* This file is part of a program that implements a Software-Defined Radio.
   7
   8 Copyright (C) 2004 by Bob McGwier, N4HY
   9
  10 This program is free software; you can redistribute it and/or modify
  11 it under the terms of the GNU General Public License as published by
  12 the Free Software Foundation; either version 2 of the License, or
  13 (at your option) any later version.
  14
  15 This program is distributed in the hope that it will be useful,
  16 but WITHOUT ANY WARRANTY; without even the implied warranty of
  17 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  18 GNU General Public License for more details.
  19
  20 You should have received a copy of the GNU General Public License
  21 along with this program; if not, write to the Free Software
  22 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  23
  24 The author can be reached by email at
  25
  26 rwmcgwier@comcast.net
  27
  28 or by paper mail at
  29
  30 Robert W McGwier, N4HY
  31 64 Brooktree Road
  32 East Windsor, NJ 08520
  33 */
  34
  35 #include <fastrig.h>
  36
  37 REAL
  38 phasemod(REAL angle) {
  39    while (angle >= TWOPI) angle -= TWOPI;
  40    while (angle < 0.0) angle += TWOPI;
  41    return angle;
  42 }
  43
  44 //REAL phasemod(REAL angle)
  45 //{
  46  // return (angle - floor(angle/TWOPI)*TWOPI);
  47 //}
  48
  49 #if (TRIG_SPEED != 0)
  50
  51 static REAL TABLE_FACTOR;
  52 static REAL *sinT, *cosT;
  53
  54 /***************************************************************************/
  55 /* Constant definitions                                                    */
  56 /***************************************************************************/
  57 #define TAN_MAP_RES     0.003921569     /* (smallest non-zero value in table) */
  58 #define RAD_PER_DEG     0.017453293
  59 #define TAN_MAP_SIZE    256
  60
  61 /* arctangents from 0 to pi/4 radians */
  62 static REAL fast_atan_table[257] = {
  63   0.000000e+00, 3.921549e-03, 7.842976e-03, 1.176416e-02,
  64   1.568499e-02, 1.960533e-02, 2.352507e-02, 2.744409e-02,
  65   3.136226e-02, 3.527947e-02, 3.919560e-02, 4.311053e-02,
  66   4.702413e-02, 5.093629e-02, 5.484690e-02, 5.875582e-02,
  67   6.266295e-02, 6.656816e-02, 7.047134e-02, 7.437238e-02,
  68   7.827114e-02, 8.216752e-02, 8.606141e-02, 8.995267e-02,
  69   9.384121e-02, 9.772691e-02, 1.016096e-01, 1.054893e-01,
  70   1.093658e-01, 1.132390e-01, 1.171087e-01, 1.209750e-01,
  71   1.248376e-01, 1.286965e-01, 1.325515e-01, 1.364026e-01,
  72   1.402496e-01, 1.440924e-01, 1.479310e-01, 1.517652e-01,
  73   1.555948e-01, 1.594199e-01, 1.632403e-01, 1.670559e-01,
  74   1.708665e-01, 1.746722e-01, 1.784728e-01, 1.822681e-01,
  75   1.860582e-01, 1.898428e-01, 1.936220e-01, 1.973956e-01,
  76   2.011634e-01, 2.049255e-01, 2.086818e-01, 2.124320e-01,
  77   2.161762e-01, 2.199143e-01, 2.236461e-01, 2.273716e-01,
  78   2.310907e-01, 2.348033e-01, 2.385093e-01, 2.422086e-01,
  79   2.459012e-01, 2.495869e-01, 2.532658e-01, 2.569376e-01,
  80   2.606024e-01, 2.642600e-01, 2.679104e-01, 2.715535e-01,
  81   2.751892e-01, 2.788175e-01, 2.824383e-01, 2.860514e-01,
  82   2.896569e-01, 2.932547e-01, 2.968447e-01, 3.004268e-01,
  83   3.040009e-01, 3.075671e-01, 3.111252e-01, 3.146752e-01,
  84   3.182170e-01, 3.217506e-01, 3.252758e-01, 3.287927e-01,
  85   3.323012e-01, 3.358012e-01, 3.392926e-01, 3.427755e-01,
  86   3.462497e-01, 3.497153e-01, 3.531721e-01, 3.566201e-01,
  87   3.600593e-01, 3.634896e-01, 3.669110e-01, 3.703234e-01,
  88   3.737268e-01, 3.771211e-01, 3.805064e-01, 3.838825e-01,
  89   3.872494e-01, 3.906070e-01, 3.939555e-01, 3.972946e-01,
  90   4.006244e-01, 4.039448e-01, 4.072558e-01, 4.105574e-01,
  91   4.138496e-01, 4.171322e-01, 4.204054e-01, 4.236689e-01,
  92   4.269229e-01, 4.301673e-01, 4.334021e-01, 4.366272e-01,
  93   4.398426e-01, 4.430483e-01, 4.462443e-01, 4.494306e-01,
  94   4.526070e-01, 4.557738e-01, 4.589307e-01, 4.620778e-01,
  95   4.652150e-01, 4.683424e-01, 4.714600e-01, 4.745676e-01,
  96   4.776654e-01, 4.807532e-01, 4.838312e-01, 4.868992e-01,
  97   4.899573e-01, 4.930055e-01, 4.960437e-01, 4.990719e-01,
  98   5.020902e-01, 5.050985e-01, 5.080968e-01, 5.110852e-01,
  99   5.140636e-01, 5.170320e-01, 5.199904e-01, 5.229388e-01,
 100   5.258772e-01, 5.288056e-01, 5.317241e-01, 5.346325e-01,
 101   5.375310e-01, 5.404195e-01, 5.432980e-01, 5.461666e-01,
 102   5.490251e-01, 5.518738e-01, 5.547124e-01, 5.575411e-01,
 103   5.603599e-01, 5.631687e-01, 5.659676e-01, 5.687566e-01,
 104   5.715357e-01, 5.743048e-01, 5.770641e-01, 5.798135e-01,
 105   5.825531e-01, 5.852828e-01, 5.880026e-01, 5.907126e-01,
 106   5.934128e-01, 5.961032e-01, 5.987839e-01, 6.014547e-01,
 107   6.041158e-01, 6.067672e-01, 6.094088e-01, 6.120407e-01,
 108   6.146630e-01, 6.172755e-01, 6.198784e-01, 6.224717e-01,
 109   6.250554e-01, 6.276294e-01, 6.301939e-01, 6.327488e-01,
 110   6.352942e-01, 6.378301e-01, 6.403565e-01, 6.428734e-01,
 111   6.453808e-01, 6.478788e-01, 6.503674e-01, 6.528466e-01,
 112   6.553165e-01, 6.577770e-01, 6.602282e-01, 6.626701e-01,
 113   6.651027e-01, 6.675261e-01, 6.699402e-01, 6.723452e-01,
 114   6.747409e-01, 6.771276e-01, 6.795051e-01, 6.818735e-01,
 115   6.842328e-01, 6.865831e-01, 6.889244e-01, 6.912567e-01,
 116   6.935800e-01, 6.958943e-01, 6.981998e-01, 7.004964e-01,
 117   7.027841e-01, 7.050630e-01, 7.073330e-01, 7.095943e-01,
 118   7.118469e-01, 7.140907e-01, 7.163258e-01, 7.185523e-01,
 119   7.207701e-01, 7.229794e-01, 7.251800e-01, 7.273721e-01,
 120   7.295557e-01, 7.317307e-01, 7.338974e-01, 7.360555e-01,
 121   7.382053e-01, 7.403467e-01, 7.424797e-01, 7.446045e-01,
 122   7.467209e-01, 7.488291e-01, 7.509291e-01, 7.530208e-01,
 123   7.551044e-01, 7.571798e-01, 7.592472e-01, 7.613064e-01,
 124   7.633576e-01, 7.654008e-01, 7.674360e-01, 7.694633e-01,
 125   7.714826e-01, 7.734940e-01, 7.754975e-01, 7.774932e-01,
 126   7.794811e-01, 7.814612e-01, 7.834335e-01, 7.853983e-01,
 127   7.853983e-01
 128 };
 129
 130 void
 131 InitSPEEDTRIG(void) {
 132   int i, SIZE;
 133   TABLE_FACTOR = ONE_OVER_TWOPI * SIN_TABLE_SIZE;
 134   SIZE = sizeof(REAL) * (SIN_TABLE_SIZE + 1);
 135   sinT = (REAL *) malloc(SIZE + (SIZE >> 2) + 1);
 136   /*  cosT=(REAL *)malloc(SIZE); */
 137   for (i = 0; i < SIN_TABLE_SIZE + (SIN_TABLE_SIZE >> 2) + 1; i++) {
 138     sinT[i] = (REAL) sin((REAL) i * TWOPI / (REAL) SIN_TABLE_SIZE);
 139     /*    cosT[i] = (REAL)cos((REAL)i * TWOPI/(REAL)SIN_TABLE_SIZE); */
 140   }
 141   cosT = sinT + (SIN_TABLE_SIZE >> 2);
 142 }
 143
 144 REAL
 145 fast_sin(REAL x) {
 146 #if (TRIG_SPEED==2)
 147   x = (x * TABLE_FACTOR) + 0.5;
 148   return sinT[((int) x) & (SIN_TABLE_SIZE_M1)];
 149 #else
 150   int i, ip1;
 151   REAL frac;
 152   x = (x * TABLE_FACTOR);
 153   i = (int)(frac = floor(x));
 154   ip1 = i + 1;
 155   frac = x - frac;
 156   return (1.0 - frac) * sinT[i] + frac * sinT[ip1];
 157 #endif
 158 }
 159
 160 REAL
 161 fast_cos(REAL x) {
 162 #if (TRIG_SPEED==2)
 163   x = (x * TABLE_FACTOR) + 0.5;
 164   return cosT[((int) x) & (SIN_TABLE_SIZE - 1)];
 165 #else
 166   int i, ip1;
 167   REAL frac;
 168   x = (x * TABLE_FACTOR);
 169   i = (int)(frac = floor(x));
 170   ip1 = i + 1;
 171   frac = x - frac;
 172   return (1.0 - frac) * cosT[i] + frac * cosT[ip1];
 173 #endif
 174 }
 175
 176 /*****************************************************************************
 177  Function:      Arc tangent
 178
 179  Syntax:        angle = fast_atan2(y, x);
 180                     REAL   y       y component of input vector
 181                     REAL   x       x component of input vector
 182                     REAL   angle   angle of vector (x, y) in radians
 183
 184  Description:   This function calculates the angle of the vector (x,y) based
 185                 on a table lookup and linear interpolation. The table uses
 186                 a 256 point table covering -45 to +45 degrees and uses
 187                 symetry to determine the final angle value in the range of
 188                 -180 to 180 degrees. Note that this function uses the small
 189                 angle approximation for values close to zero. This routine
 190                 calculates the arc tangent with an average  error of
 191                 +/- 0.045 degrees.
 192 *****************************************************************************/
 193
 194 REAL
 195 fast_atan2(REAL y, REAL x) {
 196   REAL x_abs, y_abs, z;
 197   REAL alpha, angle, base_angle;
 198   int index;
 199
 200   /* don't divide by zero! */
 201   if ((y == 0.0) && (x == 0.0)) angle = 0.0;
 202   else {
 203     /* normalize to +/- 45 degree range */
 204     y_abs = fabs(y);
 205     x_abs = fabs(x);
 206     //z = (y_abs < x_abs ? y_abs / x_abs : x_abs / y_abs);
 207     if (y_abs < x_abs)
 208       z = y_abs / x_abs;
 209     else
 210       z = x_abs / y_abs;
 211     /* when ratio approaches the table resolution, the angle is */
 212     /*      best approximated with the argument itself...       */
 213     if (z < TAN_MAP_RES) base_angle = z;
 214     else {
 215       /* find index and interpolation value */
 216       alpha = z * (REAL) TAN_MAP_SIZE - .5;
 217       index = (int) alpha;
 218       alpha -= (REAL) index;
 219       /* determine base angle based on quadrant and */
 220       /* add or subtract table value from base angle based on quadrant */
 221       base_angle = fast_atan_table[index];
 222       base_angle +=
 223         (fast_atan_table[index + 1] - fast_atan_table[index]) * alpha;
 224     }
 225
 226     if (x_abs > y_abs) {        /* -45 -> 45 or 135 -> 225 */
 227       if (x >= 0.0) {           /* -45 -> 45 */
 228         if (y >= 0.0)
 229           angle = base_angle;   /* 0 -> 45, angle OK */
 230         else
 231           angle = -base_angle;  /* -45 -> 0, angle = -angle */
 232       } else {                  /* 135 -> 180 or 180 -> -135 */
 233         angle = 3.14159265358979323846;
 234         if (y >= 0.0)
 235           angle -= base_angle;  /* 135 -> 180, angle = 180 - angle */
 236         else
 237           angle = base_angle - angle;   /* 180 -> -135, angle = angle - 180 */
 238       }
 239     } else {                    /* 45 -> 135 or -135 -> -45 */
 240       if (y >= 0.0) {           /* 45 -> 135 */
 241         angle = 1.57079632679489661923;
 242         if (x >= 0.0)
 243           angle -= base_angle;  /* 45 -> 90, angle = 90 - angle */
 244         else
 245           angle += base_angle;  /* 90 -> 135, angle = 90 + angle */
 246       } else {                  /* -135 -> -45 */
 247         angle = -1.57079632679489661923;
 248         if (x >= 0.0)
 249           angle += base_angle;  /* -90 -> -45, angle = -90 + angle */
 250         else
 251           angle -= base_angle;  /* -135 -> -90, angle = -90 - angle */
 252       }
 253     }
 254   }
 255 #ifdef ZERO_TO_TWOPI
 256   if (angle < 0) return (angle + TWOPI);
 257   else return (angle);
 258 #else
 259   return (angle);
 260 #endif
 261 }
 262
 263 #endif