]> git.rkrishnan.org Git - dttsp.git/blob - jDttSP/fastrig.c
multiple receivers implemented
[dttsp.git] / 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