trig8.h
7671 bytes
1#ifndef __INC_LIB8TION_TRIG_H
2#define __INC_LIB8TION_TRIG_H
3
4///@ingroup lib8tion
5
6///@defgroup Trig Fast trig functions
7/// Fast 8 and 16-bit approximations of sin(x) and cos(x).
8/// Don't use these approximations for calculating the
9/// trajectory of a rocket to Mars, but they're great
10/// for art projects and LED displays.
11///
12/// On Arduino/AVR, the 16-bit approximation is more than
13/// 10X faster than floating point sin(x) and cos(x), while
14/// the 8-bit approximation is more than 20X faster.
15///@{
16
17#if defined(__AVR__)
18#define sin16 sin16_avr
19#else
20#define sin16 sin16_C
21#endif
22
23/// Fast 16-bit approximation of sin(x). This approximation never varies more than
24/// 0.69% from the floating point value you'd get by doing
25///
26/// float s = sin(x) * 32767.0;
27///
28/// @param theta input angle from 0-65535
29/// @returns sin of theta, value between -32767 to 32767.
30LIB8STATIC int16_t sin16_avr( uint16_t theta )
31{
32 static const uint8_t data[] =
33 { 0, 0, 49, 0, 6393%256, 6393/256, 48, 0,
34 12539%256, 12539/256, 44, 0, 18204%256, 18204/256, 38, 0,
35 23170%256, 23170/256, 31, 0, 27245%256, 27245/256, 23, 0,
36 30273%256, 30273/256, 14, 0, 32137%256, 32137/256, 4 /*,0*/ };
37
38 uint16_t offset = (theta & 0x3FFF);
39
40 // AVR doesn't have a multi-bit shift instruction,
41 // so if we say "offset >>= 3", gcc makes a tiny loop.
42 // Inserting empty volatile statements between each
43 // bit shift forces gcc to unroll the loop.
44 offset >>= 1; // 0..8191
45 asm volatile("");
46 offset >>= 1; // 0..4095
47 asm volatile("");
48 offset >>= 1; // 0..2047
49
50 if( theta & 0x4000 ) offset = 2047 - offset;
51
52 uint8_t sectionX4;
53 sectionX4 = offset / 256;
54 sectionX4 *= 4;
55
56 uint8_t m;
57
58 union {
59 uint16_t b;
60 struct {
61 uint8_t blo;
62 uint8_t bhi;
63 };
64 } u;
65
66 //in effect u.b = blo + (256 * bhi);
67 u.blo = data[ sectionX4 ];
68 u.bhi = data[ sectionX4 + 1];
69 m = data[ sectionX4 + 2];
70
71 uint8_t secoffset8 = (uint8_t)(offset) / 2;
72
73 uint16_t mx = m * secoffset8;
74
75 int16_t y = mx + u.b;
76 if( theta & 0x8000 ) y = -y;
77
78 return y;
79}
80
81/// Fast 16-bit approximation of sin(x). This approximation never varies more than
82/// 0.69% from the floating point value you'd get by doing
83///
84/// float s = sin(x) * 32767.0;
85///
86/// @param theta input angle from 0-65535
87/// @returns sin of theta, value between -32767 to 32767.
88LIB8STATIC int16_t sin16_C( uint16_t theta )
89{
90 static const uint16_t base[] =
91 { 0, 6393, 12539, 18204, 23170, 27245, 30273, 32137 };
92 static const uint8_t slope[] =
93 { 49, 48, 44, 38, 31, 23, 14, 4 };
94
95 uint16_t offset = (theta & 0x3FFF) >> 3; // 0..2047
96 if( theta & 0x4000 ) offset = 2047 - offset;
97
98 uint8_t section = offset / 256; // 0..7
99 uint16_t b = base[section];
100 uint8_t m = slope[section];
101
102 uint8_t secoffset8 = (uint8_t)(offset) / 2;
103
104 uint16_t mx = m * secoffset8;
105 int16_t y = mx + b;
106
107 if( theta & 0x8000 ) y = -y;
108
109 return y;
110}
111
112
113/// Fast 16-bit approximation of cos(x). This approximation never varies more than
114/// 0.69% from the floating point value you'd get by doing
115///
116/// float s = cos(x) * 32767.0;
117///
118/// @param theta input angle from 0-65535
119/// @returns sin of theta, value between -32767 to 32767.
120LIB8STATIC int16_t cos16( uint16_t theta)
121{
122 return sin16( theta + 16384);
123}
124
125///////////////////////////////////////////////////////////////////////
126
127// sin8 & cos8
128// Fast 8-bit approximations of sin(x) & cos(x).
129// Input angle is an unsigned int from 0-255.
130// Output is an unsigned int from 0 to 255.
131//
132// This approximation can vary to to 2%
133// from the floating point value you'd get by doing
134// float s = (sin( x ) * 128.0) + 128;
135//
136// Don't use this approximation for calculating the
137// "real" trigonometric calculations, but it's great
138// for art projects and LED displays.
139//
140// On Arduino/AVR, this approximation is more than
141// 20X faster than floating point sin(x) and cos(x)
142
143#if defined(__AVR__) && !defined(LIB8_ATTINY)
144#define sin8 sin8_avr
145#else
146#define sin8 sin8_C
147#endif
148
149
150static const uint8_t b_m16_interleave[8] = { 0, 49, 49, 41, 90, 27, 117, 10 };
151
152/// Fast 8-bit approximation of sin(x). This approximation never varies more than
153/// 2% from the floating point value you'd get by doing
154///
155/// float s = (sin(x) * 128.0) + 128;
156///
157/// @param theta input angle from 0-255
158/// @returns sin of theta, value between 0 and 255
159LIB8STATIC uint8_t sin8_avr( uint8_t theta)
160{
161 uint8_t offset = theta;
162
163 asm volatile(
164 "sbrc %[theta],6 \n\t"
165 "com %[offset] \n\t"
166 : [theta] "+r" (theta), [offset] "+r" (offset)
167 );
168
169 offset &= 0x3F; // 0..63
170
171 uint8_t secoffset = offset & 0x0F; // 0..15
172 if( theta & 0x40) secoffset++;
173
174 uint8_t m16; uint8_t b;
175
176 uint8_t section = offset >> 4; // 0..3
177 uint8_t s2 = section * 2;
178
179 const uint8_t* p = b_m16_interleave;
180 p += s2;
181 b = *p;
182 p++;
183 m16 = *p;
184
185 uint8_t mx;
186 uint8_t xr1;
187 asm volatile(
188 "mul %[m16],%[secoffset] \n\t"
189 "mov %[mx],r0 \n\t"
190 "mov %[xr1],r1 \n\t"
191 "eor r1, r1 \n\t"
192 "swap %[mx] \n\t"
193 "andi %[mx],0x0F \n\t"
194 "swap %[xr1] \n\t"
195 "andi %[xr1], 0xF0 \n\t"
196 "or %[mx], %[xr1] \n\t"
197 : [mx] "=d" (mx), [xr1] "=d" (xr1)
198 : [m16] "d" (m16), [secoffset] "d" (secoffset)
199 );
200
201 int8_t y = mx + b;
202 if( theta & 0x80 ) y = -y;
203
204 y += 128;
205
206 return y;
207}
208
209
210/// Fast 8-bit approximation of sin(x). This approximation never varies more than
211/// 2% from the floating point value you'd get by doing
212///
213/// float s = (sin(x) * 128.0) + 128;
214///
215/// @param theta input angle from 0-255
216/// @returns sin of theta, value between 0 and 255
217LIB8STATIC uint8_t sin8_C( uint8_t theta)
218{
219 uint8_t offset = theta;
220 if( theta & 0x40 ) {
221 offset = (uint8_t)255 - offset;
222 }
223 offset &= 0x3F; // 0..63
224
225 uint8_t secoffset = offset & 0x0F; // 0..15
226 if( theta & 0x40) secoffset++;
227
228 uint8_t section = offset >> 4; // 0..3
229 uint8_t s2 = section * 2;
230 const uint8_t* p = b_m16_interleave;
231 p += s2;
232 uint8_t b = *p;
233 p++;
234 uint8_t m16 = *p;
235
236 uint8_t mx = (m16 * secoffset) >> 4;
237
238 int8_t y = mx + b;
239 if( theta & 0x80 ) y = -y;
240
241 y += 128;
242
243 return y;
244}
245
246/// Fast 8-bit approximation of cos(x). This approximation never varies more than
247/// 2% from the floating point value you'd get by doing
248///
249/// float s = (cos(x) * 128.0) + 128;
250///
251/// @param theta input angle from 0-255
252/// @returns sin of theta, value between 0 and 255
253LIB8STATIC uint8_t cos8( uint8_t theta)
254{
255 return sin8( theta + 64);
256}
257
258/// Fast 16-bit approximation of atan2(x).
259/// @returns atan2, value between 0 and 255
260LIB8STATIC uint8_t atan2_8(int16_t dy, int16_t dx)
261{
262 if (dy == 0)
263 {
264 if (dx >= 0)
265 return 0;
266 else
267 return 128;
268 }
269
270 int16_t abs_y = dy > 0 ? dy : -dy;
271 int8_t a;
272
273 if (dx >= 0)
274 a = 32 - (32 * (dx - abs_y) / (dx + abs_y));
275 else
276 a = 96 - (32 * (dx + abs_y) / (abs_y - dx));
277
278 if (dy < 0)
279 return -a; // negate if in quad III or IV
280 return a;
281}
282
283///@}
284#endif