jDttSP/fastrig.c

   1
   2 /****************************************************************
   3  *   Fast Trigonometric Routines Used for Imbedded 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     /* when ratio approaches the table resolution, the angle is */
 208     /*      best approximated with the argument itself...       */
 209     if (z < TAN_MAP_RES)  base_angle = z;
 210     else {
 211       /* find index and interpolation value */
 212       alpha = z * (REAL) TAN_MAP_SIZE - .5;
 213       index = (int) alpha;
 214       alpha -= (REAL) index;
 215       /* determine base angle based on quadrant and */
 216       /* add or subtract table value from base angle based on quadrant */
 217       base_angle = fast_atan_table[index];
 218       base_angle +=
 219         (fast_atan_table[index + 1] - fast_atan_table[index]) * alpha;
 220     }
 221
 222     if (x_abs > y_abs) {        /* -45 -> 45 or 135 -> 225 */
 223       if (x >= 0.0) {           /* -45 -> 45 */
 224         if (y >= 0.0)
 225           angle = base_angle;   /* 0 -> 45, angle OK */
 226         else
 227           angle = -base_angle;  /* -45 -> 0, angle = -angle */
 228       } else {                  /* 135 -> 180 or 180 -> -135 */
 229         angle = 3.14159265358979323846;
 230         if (y >= 0.0)
 231           angle -= base_angle;  /* 135 -> 180, angle = 180 - angle */
 232         else
 233           angle = base_angle - angle;   /* 180 -> -135, angle = angle - 180 */
 234       }
 235     } else {                    /* 45 -> 135 or -135 -> -45 */
 236       if (y >= 0.0) {           /* 45 -> 135 */
 237         angle = 1.57079632679489661923;
 238         if (x >= 0.0)
 239           angle -= base_angle;  /* 45 -> 90, angle = 90 - angle */
 240         else
 241           angle += base_angle;  /* 90 -> 135, angle = 90 + angle */
 242       } else {                  /* -135 -> -45 */
 243         angle = -1.57079632679489661923;
 244         if (x >= 0.0)
 245           angle += base_angle;  /* -90 -> -45, angle = -90 + angle */
 246         else
 247           angle -= base_angle;  /* -135 -> -90, angle = -90 - angle */
 248       }
 249     }
 250   }
 251 #ifdef ZERO_TO_TWOPI
 252   if (angle < 0) return (angle + TWOPI);
 253   else return (angle);
 254 #else
 255   return (angle);
 256 #endif
 257 }
 258
 259 #endif