1 |
gwlarson |
3.1 |
#ifndef lint |
2 |
greg |
3.8 |
static const char RCSid[] = "$Id: tcos.c,v 3.7 2013/02/07 20:19:52 greg Exp $"; |
3 |
gwlarson |
3.1 |
#endif |
4 |
|
|
/* |
5 |
|
|
* Table-based cosine approximation. |
6 |
|
|
* |
7 |
|
|
* Use doubles in table even though we're not nearly that accurate just |
8 |
|
|
* to avoid conversion and guarantee that tsin(x)^2 + tcos(x)^2 == 1. |
9 |
|
|
* |
10 |
|
|
* No interpolation in this version. |
11 |
greg |
3.2 |
* |
12 |
greg |
3.5 |
* External symbols declared in rtmath.h |
13 |
greg |
3.2 |
*/ |
14 |
|
|
|
15 |
greg |
3.3 |
#include "copyright.h" |
16 |
gwlarson |
3.1 |
|
17 |
|
|
#include <math.h> |
18 |
schorsch |
3.4 |
|
19 |
|
|
#include "rtmath.h" |
20 |
gwlarson |
3.1 |
|
21 |
|
|
#ifndef NCOSENTRY |
22 |
greg |
3.6 |
#define NCOSENTRY 1024 |
23 |
gwlarson |
3.1 |
#endif |
24 |
|
|
|
25 |
|
|
|
26 |
|
|
double |
27 |
greg |
3.5 |
tcos(double x) /* approximate cosine */ |
28 |
gwlarson |
3.1 |
{ |
29 |
|
|
static double costab[NCOSENTRY+1]; |
30 |
|
|
register int i; |
31 |
|
|
|
32 |
|
|
if (costab[0] < 0.5) /* initialize table */ |
33 |
|
|
for (i = 0; i <= NCOSENTRY; i++) |
34 |
|
|
costab[i] = cos((PI/2./NCOSENTRY)*i); |
35 |
|
|
/* normalize angle */ |
36 |
|
|
if (x < 0.) |
37 |
|
|
x = -x; |
38 |
|
|
i = (NCOSENTRY*2./PI) * x + 0.5; |
39 |
greg |
3.5 |
while (i >= 4*NCOSENTRY) |
40 |
|
|
i -= 4*NCOSENTRY; |
41 |
gwlarson |
3.1 |
switch (i / NCOSENTRY) { |
42 |
|
|
case 0: |
43 |
|
|
return(costab[i]); |
44 |
|
|
case 1: |
45 |
|
|
return(-costab[(2*NCOSENTRY)-i]); |
46 |
|
|
case 2: |
47 |
|
|
return(-costab[i-(2*NCOSENTRY)]); |
48 |
|
|
case 3: |
49 |
|
|
return(costab[(4*NCOSENTRY)-i]); |
50 |
|
|
} |
51 |
|
|
return(0.); /* should never be reached */ |
52 |
|
|
} |
53 |
greg |
3.7 |
|
54 |
|
|
|
55 |
|
|
/* Fast arctangent approximation due to Rajan et al. 2006 */ |
56 |
|
|
double |
57 |
greg |
3.8 |
atan2a(double y, double x) |
58 |
greg |
3.7 |
{ |
59 |
|
|
double ratio, aratio, val; |
60 |
|
|
|
61 |
|
|
if (x == 0) |
62 |
|
|
return (y > 0) ? PI/2. : 3./2.*PI; |
63 |
|
|
|
64 |
|
|
aratio = (ratio = y/x) >= 0 ? ratio : -ratio; |
65 |
|
|
|
66 |
|
|
if (aratio > 1.01) |
67 |
|
|
return PI/2. - aatan2(x, y); |
68 |
|
|
|
69 |
|
|
val = PI/4.*ratio - ratio*(aratio - 1.)*(0.2447 + 0.0663*aratio); |
70 |
|
|
|
71 |
|
|
return val + PI*(x < 0); |
72 |
|
|
} |