1 /* -*- c++ -*- */
2 /*
3 * Copyright 2005,2013 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 <gnuradio/math.h> // declaration is in here
24 #include <cmath>
25
26 namespace gr {
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 255
35
36 /* arctangents from 0 to pi/4 radians */
37 static float fast_atan_table[257] = {
38 0.000000e+00, 3.921549e-03, 7.842976e-03, 1.176416e-02, 1.568499e-02, 1.960533e-02,
39 2.352507e-02, 2.744409e-02, 3.136226e-02, 3.527947e-02, 3.919560e-02, 4.311053e-02,
40 4.702413e-02, 5.093629e-02, 5.484690e-02, 5.875582e-02, 6.266295e-02, 6.656816e-02,
41 7.047134e-02, 7.437238e-02, 7.827114e-02, 8.216752e-02, 8.606141e-02, 8.995267e-02,
42 9.384121e-02, 9.772691e-02, 1.016096e-01, 1.054893e-01, 1.093658e-01, 1.132390e-01,
43 1.171087e-01, 1.209750e-01, 1.248376e-01, 1.286965e-01, 1.325515e-01, 1.364026e-01,
44 1.402496e-01, 1.440924e-01, 1.479310e-01, 1.517652e-01, 1.555948e-01, 1.594199e-01,
45 1.632403e-01, 1.670559e-01, 1.708665e-01, 1.746722e-01, 1.784728e-01, 1.822681e-01,
46 1.860582e-01, 1.898428e-01, 1.936220e-01, 1.973956e-01, 2.011634e-01, 2.049255e-01,
47 2.086818e-01, 2.124320e-01, 2.161762e-01, 2.199143e-01, 2.236461e-01, 2.273716e-01,
48 2.310907e-01, 2.348033e-01, 2.385093e-01, 2.422086e-01, 2.459012e-01, 2.495869e-01,
49 2.532658e-01, 2.569376e-01, 2.606024e-01, 2.642600e-01, 2.679104e-01, 2.715535e-01,
50 2.751892e-01, 2.788175e-01, 2.824383e-01, 2.860514e-01, 2.896569e-01, 2.932547e-01,
51 2.968447e-01, 3.004268e-01, 3.040009e-01, 3.075671e-01, 3.111252e-01, 3.146752e-01,
52 3.182170e-01, 3.217506e-01, 3.252758e-01, 3.287927e-01, 3.323012e-01, 3.358012e-01,
53 3.392926e-01, 3.427755e-01, 3.462497e-01, 3.497153e-01, 3.531721e-01, 3.566201e-01,
54 3.600593e-01, 3.634896e-01, 3.669110e-01, 3.703234e-01, 3.737268e-01, 3.771211e-01,
55 3.805064e-01, 3.838825e-01, 3.872494e-01, 3.906070e-01, 3.939555e-01, 3.972946e-01,
56 4.006244e-01, 4.039448e-01, 4.072558e-01, 4.105574e-01, 4.138496e-01, 4.171322e-01,
57 4.204054e-01, 4.236689e-01, 4.269229e-01, 4.301673e-01, 4.334021e-01, 4.366272e-01,
58 4.398426e-01, 4.430483e-01, 4.462443e-01, 4.494306e-01, 4.526070e-01, 4.557738e-01,
59 4.589307e-01, 4.620778e-01, 4.652150e-01, 4.683424e-01, 4.714600e-01, 4.745676e-01,
60 4.776654e-01, 4.807532e-01, 4.838312e-01, 4.868992e-01, 4.899573e-01, 4.930055e-01,
61 4.960437e-01, 4.990719e-01, 5.020902e-01, 5.050985e-01, 5.080968e-01, 5.110852e-01,
62 5.140636e-01, 5.170320e-01, 5.199904e-01, 5.229388e-01, 5.258772e-01, 5.288056e-01,
63 5.317241e-01, 5.346325e-01, 5.375310e-01, 5.404195e-01, 5.432980e-01, 5.461666e-01,
64 5.490251e-01, 5.518738e-01, 5.547124e-01, 5.575411e-01, 5.603599e-01, 5.631687e-01,
65 5.659676e-01, 5.687566e-01, 5.715357e-01, 5.743048e-01, 5.770641e-01, 5.798135e-01,
66 5.825531e-01, 5.852828e-01, 5.880026e-01, 5.907126e-01, 5.934128e-01, 5.961032e-01,
67 5.987839e-01, 6.014547e-01, 6.041158e-01, 6.067672e-01, 6.094088e-01, 6.120407e-01,
68 6.146630e-01, 6.172755e-01, 6.198784e-01, 6.224717e-01, 6.250554e-01, 6.276294e-01,
69 6.301939e-01, 6.327488e-01, 6.352942e-01, 6.378301e-01, 6.403565e-01, 6.428734e-01,
70 6.453808e-01, 6.478788e-01, 6.503674e-01, 6.528466e-01, 6.553165e-01, 6.577770e-01,
71 6.602282e-01, 6.626701e-01, 6.651027e-01, 6.675261e-01, 6.699402e-01, 6.723452e-01,
72 6.747409e-01, 6.771276e-01, 6.795051e-01, 6.818735e-01, 6.842328e-01, 6.865831e-01,
73 6.889244e-01, 6.912567e-01, 6.935800e-01, 6.958943e-01, 6.981998e-01, 7.004964e-01,
74 7.027841e-01, 7.050630e-01, 7.073330e-01, 7.095943e-01, 7.118469e-01, 7.140907e-01,
75 7.163258e-01, 7.185523e-01, 7.207701e-01, 7.229794e-01, 7.251800e-01, 7.273721e-01,
76 7.295557e-01, 7.317307e-01, 7.338974e-01, 7.360555e-01, 7.382053e-01, 7.403467e-01,
77 7.424797e-01, 7.446045e-01, 7.467209e-01, 7.488291e-01, 7.509291e-01, 7.530208e-01,
78 7.551044e-01, 7.571798e-01, 7.592472e-01, 7.613064e-01, 7.633576e-01, 7.654008e-01,
79 7.674360e-01, 7.694633e-01, 7.714826e-01, 7.734940e-01, 7.754975e-01, 7.774932e-01,
80 7.794811e-01, 7.814612e-01, 7.834335e-01, 7.853982e-01, 7.853982e-01
81 };
82
83
84 /*****************************************************************************
85 Function: Arc tangent
86
87 Syntax: angle = fast_atan2(y, x);
88 float y y component of input vector
89 float x x component of input vector
90 float angle angle of vector (x, y) in radians
91
92 Description: This function calculates the angle of the vector (x,y)
93 based on a table lookup and linear interpolation. The table uses a
94 256 point table covering -45 to +45 degrees and uses symmetry to
95 determine the final angle value in the range of -180 to 180
96 degrees. Note that this function uses the small angle approximation
97 for values close to zero. This routine calculates the arc tangent
98 with an average error of +/- 3.56e-5 degrees (6.21e-7 radians).
99 *****************************************************************************/
100
fast_atan2f(float y,float x)101 float fast_atan2f(float y, float x)
102 {
103 float x_abs, y_abs, z;
104 float alpha, angle, base_angle;
105 int index;
106
107 /* normalize to +/- 45 degree range */
108 y_abs = fabsf(y);
109 x_abs = fabsf(x);
110 /* don't divide by zero! */
111 if (!((y_abs > 0.0f) || (x_abs > 0.0f)))
112 return 0.0;
113
114 if (y_abs < x_abs)
115 z = y_abs / x_abs;
116 else
117 z = x_abs / y_abs;
118
119 /* when ratio approaches the table resolution, the angle is */
120 /* best approximated with the argument itself... */
121 if (z < TAN_MAP_RES)
122 base_angle = z;
123 else {
124 /* find index and interpolation value */
125 alpha = z * (float)TAN_MAP_SIZE;
126 index = ((int)alpha) & 0xff;
127 alpha -= (float)index;
128 /* determine base angle based on quadrant and */
129 /* add or subtract table value from base angle based on quadrant */
130 base_angle = fast_atan_table[index];
131 base_angle += (fast_atan_table[index + 1] - fast_atan_table[index]) * alpha;
132 }
133
134 if (x_abs > y_abs) { /* -45 -> 45 or 135 -> 225 */
135 if (x >= 0.0) { /* -45 -> 45 */
136 if (y >= 0.0)
137 angle = base_angle; /* 0 -> 45, angle OK */
138 else
139 angle = -base_angle; /* -45 -> 0, angle = -angle */
140 } else { /* 135 -> 180 or 180 -> -135 */
141 angle = 3.14159265358979323846;
142 if (y >= 0.0)
143 angle -= base_angle; /* 135 -> 180, angle = 180 - angle */
144 else
145 angle = base_angle - angle; /* 180 -> -135, angle = angle - 180 */
146 }
147 } else { /* 45 -> 135 or -135 -> -45 */
148 if (y >= 0.0) { /* 45 -> 135 */
149 angle = 1.57079632679489661923;
150 if (x >= 0.0)
151 angle -= base_angle; /* 45 -> 90, angle = 90 - angle */
152 else
153 angle += base_angle; /* 90 -> 135, angle = 90 + angle */
154 } else { /* -135 -> -45 */
155 angle = -1.57079632679489661923;
156 if (x >= 0.0)
157 angle += base_angle; /* -90 -> -45, angle = -90 + angle */
158 else
159 angle -= base_angle; /* -135 -> -90, angle = -90 - angle */
160 }
161 }
162
163 #ifdef ZERO_TO_TWOPI
164 if (angle < 0)
165 return (angle + TWOPI);
166 else
167 return (angle);
168 #else
169 return (angle);
170 #endif
171 }
172
173 } /* namespace gr */
174