root / trunk / code / projects / fp_math / fp_math.c @ 1953
History | View | Annotate | Download (4.18 KB)
1 | 1579 | justin | #include "fp_math.h" |
---|---|---|---|
2 | |||
3 | #include <avr/pgmspace.h> |
||
4 | #include <math.h> |
||
5 | #include <serial.h> |
||
6 | |||
7 | #define ABS(x) (x > 0 ? x : -x) |
||
8 | 1585 | justin | #define FP_PI_OVER_TWO 102944 |
9 | #define FP_TWO_PI 411775 |
||
10 | 1579 | justin | |
11 | 1585 | justin | #define TABLE_LENGTH 64 |
12 | 1602 | justin | #define TABLE_STEP 1635//.160757 |
13 | #define EXP_MAGICONSTANT 726817 //ln(2^16) * 2^16 |
||
14 | 1579 | justin | |
15 | 1602 | justin | //#define DEBUG
|
16 | |||
17 | //TODO This can probably be removed at some point.
|
||
18 | 1579 | justin | static int32_t linspace[TABLE_LENGTH] PROGMEM = {
|
19 | 1602 | justin | 0, 1634, 3268, 4902, 6536, 8170, 9804, 11438, |
20 | 13072, 14706, 16340, 17974, 19608, 21242, 22876, 24510, |
||
21 | 26144, 27778, 29412, 31047, 32681, 34315, 35949, 37583, |
||
22 | 39217, 40851, 42485, 44119, 45753, 47387, 49021, 50655, |
||
23 | 52289, 53923, 55557, 57191, 58825, 60459, 62093, 63727, |
||
24 | 65361, 66995, 68629, 70263, 71897, 73531, 75165, 76799, |
||
25 | 78433, 80067, 81701, 83335, 84969, 86603, 88237, 89871, |
||
26 | 91506, 93140, 94774, 96408, 98042, 99676, 101310, 102944, |
||
27 | 1579 | justin | }; |
28 | |||
29 | 1585 | justin | static int32_t fp_cos_table[TABLE_LENGTH] PROGMEM = {
|
30 | 1602 | justin | 65536, 65516, 65455, 65353, 65210, 65027, 64804, 64540, |
31 | 64237, 63893, 63509, 63087, 62624, 62123, 61584, 61006, |
||
32 | 60390, 59736, 59046, 58319, 57555, 56756, 55921, 55052, |
||
33 | 54148, 53211, 52241, 51238, 50203, 49138, 48041, 46915, |
||
34 | 45760, 44576, 43364, 42126, 40861, 39571, 38256, 36918, |
||
35 | 35556, 34173, 32768, 31343, 29898, 28435, 26954, 25456, |
||
36 | 23943, 22415, 20872, 19317, 17750, 16171, 14583, 12986, |
||
37 | 11380, 9768, 8149, 6525, 4898, 3267, 1634, 0, |
||
38 | 1579 | justin | }; |
39 | |||
40 | 1585 | justin | //FIXME Lazy implementations of tangent and sine.
|
41 | int32_t fp_tan(int32_t theta) { |
||
42 | return fp_div(fp_sin(theta), fp_cos(theta));
|
||
43 | } |
||
44 | 1579 | justin | |
45 | 1585 | justin | int32_t fp_sin(int32_t theta) { |
46 | return fp_cos(theta + FP_PI_OVER_TWO);
|
||
47 | } |
||
48 | |||
49 | 1586 | justin | int32_t fp_exp(int32_t x) { |
50 | //Try with partial fractions for now.
|
||
51 | int32_t xsquare = fp_mult(x,x); |
||
52 | int32_t result, i; |
||
53 | |||
54 | 1602 | justin | if(x > EXP_MAGICONSTANT) {
|
55 | 1586 | justin | usb_puts("Output value is out of range.\n\r");
|
56 | return -1; |
||
57 | } |
||
58 | |||
59 | //This is again, massive amounts of lazyness.
|
||
60 | //The approximation fails outside this range (approximately).
|
||
61 | 1602 | justin | if(x < (-17l << 16)) |
62 | 1586 | justin | return 0; |
63 | |||
64 | result = xsquare; |
||
65 | 1602 | justin | for(i= (22l << 16); i > (2l << 16); i-= (4l << 16)) { |
66 | 1586 | justin | result += i; |
67 | result = fp_div(xsquare, result); |
||
68 | } |
||
69 | |||
70 | 1602 | justin | result += (2l << 16) - x; |
71 | return (1l << 16) + fp_div(fp_mult((2l << 16), x), result); |
||
72 | 1586 | justin | |
73 | } |
||
74 | |||
75 | 1579 | justin | //Quadratic interpolation.
|
76 | int32_t fp_cos(int32_t theta) { |
||
77 | uint8_t n; |
||
78 | 1585 | justin | int8_t negative; |
79 | 1579 | justin | int32_t x_n, x_np1, x_np2; |
80 | int32_t y_n, y_np1, y_np2; |
||
81 | 1585 | justin | int32_t dd_n, dd_np1, second_dd, result; |
82 | 1602 | justin | |
83 | #ifdef DEBUG
|
||
84 | char buf[128]; |
||
85 | #endif
|
||
86 | |||
87 | 1585 | justin | //Move theta into [0, pi/2] w/ appropriate sign.
|
88 | theta = ABS(theta) % FP_TWO_PI; |
||
89 | 1603 | justin | |
90 | //Reflecting into [0, pi] doesn't change sign.
|
||
91 | 1579 | justin | if(theta > FP_PI)
|
92 | theta = FP_TWO_PI - theta; |
||
93 | 1585 | justin | |
94 | 1603 | justin | //Reflecting into [0, pi/2] does.
|
95 | 1585 | justin | if(theta > FP_PI_OVER_TWO) {
|
96 | theta = FP_PI - theta; |
||
97 | negative = 1;
|
||
98 | } |
||
99 | 1603 | justin | |
100 | //Find n such that theta is between x_{n} and x_{n+1}
|
||
101 | 1579 | justin | n = theta / TABLE_STEP; |
102 | 1602 | justin | while( n < TABLE_LENGTH - 1 |
103 | 1603 | justin | && pgm_read_dword(&linspace[n+1]) < theta)
|
104 | 1585 | justin | n++; |
105 | 1579 | justin | |
106 | 1603 | justin | //Edge case
|
107 | 1579 | justin | if(n == TABLE_LENGTH - 1) { |
108 | 1585 | justin | //Perform linear interpolation, since we're close to zero anyway.
|
109 | x_n = pgm_read_dword(&linspace[TABLE_LENGTH - 1]);
|
||
110 | y_n = pgm_read_dword(&fp_cos_table[TABLE_LENGTH - 1]);
|
||
111 | |||
112 | 1602 | justin | result = fp_mult( |
113 | fp_div(FP_PI_OVER_TWO - x_n, 0 - y_n), theta - x_n) + y_n;
|
||
114 | |||
115 | 1585 | justin | return negative ? -result : result;
|
116 | 1579 | justin | } |
117 | 1585 | justin | |
118 | 1603 | justin | //Perform quadratic interpolation.
|
119 | |||
120 | 1579 | justin | //Load in the necessary values.
|
121 | x_n = pgm_read_dword(&linspace[n]); |
||
122 | 1602 | justin | x_np1 = pgm_read_dword(&linspace[n + 1]);
|
123 | 1579 | justin | x_np2 = pgm_read_dword(&linspace[n + 2]);
|
124 | |||
125 | 1585 | justin | y_n = pgm_read_dword(&fp_cos_table[n]); |
126 | y_np1 = pgm_read_dword(&fp_cos_table[n + 1]);
|
||
127 | y_np2 = pgm_read_dword(&fp_cos_table[n + 2]);
|
||
128 | 1579 | justin | |
129 | 1602 | justin | #ifdef DEBUG
|
130 | sprintf(buf, "x_n = %ld, theta = %ld, x_{n+1} = %ld", x_n, theta, x_np1);
|
||
131 | usb_puts(buf); |
||
132 | #endif
|
||
133 | |||
134 | 1579 | justin | //Calculate first divided differences.
|
135 | dd_n = fp_div(y_np1 - y_n, x_np1 - x_n); |
||
136 | dd_np1 = fp_div(y_np2 - y_np1, x_np2 - x_np1); |
||
137 | |||
138 | //Calculate the second divided difference.
|
||
139 | second_dd = fp_div(dd_np1 - dd_n, x_np2 - x_n); |
||
140 | |||
141 | 1585 | justin | result = fp_mult(fp_mult(second_dd, theta - x_n), theta - x_np1) |
142 | + fp_mult(dd_n, theta - x_n) + y_n; |
||
143 | |||
144 | return negative ? -result : result;
|
||
145 | 1579 | justin | } |
146 | |||
147 | //FIXME I didn't write these functions very carefully...
|
||
148 | int32_t fp_mult(int32_t a, int32_t b) { |
||
149 | return (int32_t) (((int64_t)a) * ((int64_t)b)) >> 16; |
||
150 | } |
||
151 | |||
152 | int32_t fp_div(int32_t a, int32_t b) { |
||
153 | return (int32_t) ((int64_t)a << 16) / (int64_t)b; |
||
154 | } |
||
155 | |||
156 |