git.gag.com Git - debian/gnuradio/blob - gnuradio-core/src/lib/general/gr_fast_atan2f.cc

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