Plaquette
 
Loading...
Searching...
No Matches
pq_fixed32_trig.h
1/*
2 * pq_fixed32_trig.h
3 *
4 * Fixed-point trigonometric functions.
5 *
6 * (c) 2025 Sofian Audry :: info(@)sofianaudry(.)com
7 *
8 * sin32() and cos32() functions adapted from Teensy Audio library
9 * (c) 2014 Paul Stoffregen :: paul(@)pjrc.com
10 *
11 * sin16() and cos16() functions adapted from FastLED library
12 * https://fastled.io/
13 *
14 * This program is free software: you can redistribute it and/or modify
15 * it under the terms of the GNU General Public License as published by
16 * the Free Software Foundation, either version 3 of the License, or
17 * (at your option) any later version.
18 *
19 * This program is distributed in the hope that it will be useful,
20 * but WITHOUT ANY WARRANTY; without even the implied warranty of
21 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
22 * GNU General Public License for more details.
23 *
24 * You should have received a copy of the GNU General Public License
25 * along with this program. If not, see <http://www.gnu.org/licenses/>.
26 */
27
28#ifndef PQ_q0_32u_tRIG_H_
29#define PQ_q0_32u_tRIG_H_
30
31#include <stdint.h>
32#include "pq_fixed32_math.h"
33
34namespace pq {
35
41static inline int32_t sin32(uint32_t ph) __attribute__((unused));
42static inline int32_t sin32(uint32_t ph)
43{
44 int32_t angle, sum, p1, p2, p3, p5, p7, p9, p11;
45
46 if (ph >= 0xC0000000 || ph < 0x40000000) { // ph: 0.32
47 angle = (int32_t)ph; // valid from -90 to +90 degrees
48 } else {
49 angle = (int32_t)(0x80000000u - ph); // angle: 2.30
50 }
51 p1 = multiply_32x32_rshift32_rounded(angle, 1686629713) << 2; // p1: 2.30
52 p2 = multiply_32x32_rshift32_rounded(p1, p1) << 1; // p2: 3.29
53 p3 = multiply_32x32_rshift32_rounded(p2, p1) << 2; // p3: 3.29
54 sum = multiply_subtract_32x32_rshift32_rounded(p1, p3, 1431655765); // sum: 2.30
55 p5 = multiply_32x32_rshift32_rounded(p3, p2); // p5: 6.26
56 sum = multiply_accumulate_32x32_rshift32_rounded(sum, p5, 572662306);
57 p7 = multiply_32x32_rshift32_rounded(p5, p2); // p7: 9.23
58 sum = multiply_subtract_32x32_rshift32_rounded(sum, p7, 109078534);
59 p9 = multiply_32x32_rshift32_rounded(p7, p2); // p9: 12.20
60 sum = multiply_accumulate_32x32_rshift32_rounded(sum, p9, 12119837);
61 p11 = multiply_32x32_rshift32_rounded(p9, p2); // p11: 15.17
62 sum = multiply_subtract_32x32_rshift32_rounded(sum, p11, 881443);
63 return sum <<= 1; // return: 1.31
64}
65
67static inline int32_t cos32(uint32_t ph) __attribute__((always_inline, unused));
68static inline int32_t cos32(uint32_t ph) {
69 return sin32(ph + 0x40000000);
70}
71
72// Fast trigonometric functions for AVR.
73// Source: http://fastled.io/docs/3.1/trig8_8h_source.html
74
83
84static inline int16_t sin16( uint16_t theta ) __attribute__ ((unused));
85
86#if defined(__AVR__)
87
95static inline int16_t sin16(uint16_t theta)
96{
97 static const uint8_t data[] =
98 { 0, 0, 49, 0, 6393%256, 6393/256, 48, 0,
99 12539%256, 12539/256, 44, 0, 18204%256, 18204/256, 38, 0,
100 23170%256, 23170/256, 31, 0, 27245%256, 27245/256, 23, 0,
101 30273%256, 30273/256, 14, 0, 32137%256, 32137/256, 4 /*,0*/ };
102
103 uint16_t offset = (theta & 0x3FFF);
104
105 // AVR doesn't have a multi-bit shift instruction,
106 // so if we say "offset >>= 3", gcc makes a tiny loop.
107 // Inserting empty volatile statements between each
108 // bit shift forces gcc to unroll the loop.
109 offset >>= 1; // 0..8191
110 asm volatile("");
111 offset >>= 1; // 0..4095
112 asm volatile("");
113 offset >>= 1; // 0..2047
114
115 if( theta & 0x4000 ) offset = 2047 - offset;
116
117 uint8_t sectionX4;
118 sectionX4 = offset / 256;
119 sectionX4 *= 4;
120
121 uint8_t m;
122
123 union {
124 uint16_t b;
125 struct {
126 uint8_t blo;
127 uint8_t bhi;
128 };
129 } u;
130
131 //in effect u.b = blo + (256 * bhi);
132 u.blo = data[ sectionX4 ];
133 u.bhi = data[ sectionX4 + 1];
134 m = data[ sectionX4 + 2];
135
136 uint8_t secoffset8 = (uint8_t)(offset) / 2;
137
138 uint16_t mx = m * secoffset8;
139
140 int16_t y = mx + u.b;
141 if( theta & 0x8000 ) y = -y;
142
143 return y;
144}
145
146#else
154static inline int16_t sin16(uint16_t theta)
155{
156 static const uint16_t base[] =
157 { 0, 6393, 12539, 18204, 23170, 27245, 30273, 32137 };
158 static const uint8_t slope[] =
159 { 49, 48, 44, 38, 31, 23, 14, 4 };
160
161 uint16_t offset = (theta & 0x3FFF) >> 3; // 0..2047
162 if( theta & 0x4000 ) offset = 2047 - offset;
163
164 uint8_t section = offset / 256; // 0..7
165 uint16_t b = base[section];
166 uint8_t m = slope[section];
167
168 uint8_t secoffset8 = (uint8_t)(offset) / 2;
169
170 uint16_t mx = m * secoffset8;
171 int16_t y = mx + b;
172
173 if( theta & 0x8000 ) y = -y;
174
175 return y;
176}
177
178#endif
179
187static inline int16_t cos16(uint16_t theta) __attribute__ ((unused));
188static inline int16_t cos16(uint16_t theta)
189{
190 return sin16( theta + 16384);
191}
192
193// ///////////////////////////////////////////////////////////////////////
194
195// // sin8 & cos8
196// // Fast 8-bit approximations of sin(x) & cos(x).
197// // Input angle is an unsigned int from 0-255.
198// // Output is an unsigned int from 0 to 255.
199// //
200// // This approximation can vary to to 2%
201// // from the floating point value you'd get by doing
202// // float s = (sin( x ) * 128.0) + 128;
203// //
204// // Don't use this approximation for calculating the
205// // "real" trigonometric calculations, but it's great
206// // for art projects and LED displays.
207// //
208// // On Arduino/AVR, this approximation is more than
209// // 20X faster than floating point sin(x) and cos(x)
210
211// const uint8_t b_m16_interleave[] = { 0, 49, 49, 41, 90, 27, 117, 10 };
212
213// #if defined(__AVR__) && !defined(LIB8_ATTINY)
214// #define sin8 sin8_avr
215
216
217// /// Fast 8-bit approximation of sin(x). This approximation never varies more than
218// /// 2% from the floating point value you'd get by doing
219// ///
220// /// float s = (sin(x) * 128.0) + 128;
221// ///
222// /// @param theta input angle from 0-255
223// /// @returns sin of theta, value between 0 and 255
224// __attribute__ ((unused)) static inline uint8_t sin8_avr( uint8_t theta)
225// {
226// uint8_t offset = theta;
227
228// asm volatile(
229// "sbrc %[theta],6 \n\t"
230// "com %[offset] \n\t"
231// : [theta] "+r" (theta), [offset] "+r" (offset)
232// );
233
234// offset &= 0x3F; // 0..63
235
236// uint8_t secoffset = offset & 0x0F; // 0..15
237// if( theta & 0x40) secoffset++;
238
239// uint8_t m16; uint8_t b;
240
241// uint8_t section = offset >> 4; // 0..3
242// uint8_t s2 = section * 2;
243
244// const uint8_t* p = b_m16_interleave;
245// p += s2;
246// b = *p;
247// p++;
248// m16 = *p;
249
250// uint8_t mx;
251// uint8_t xr1;
252// asm volatile(
253// "mul %[m16],%[secoffset] \n\t"
254// "mov %[mx],r0 \n\t"
255// "mov %[xr1],r1 \n\t"
256// "eor r1, r1 \n\t"
257// "swap %[mx] \n\t"
258// "andi %[mx],0x0F \n\t"
259// "swap %[xr1] \n\t"
260// "andi %[xr1], 0xF0 \n\t"
261// "or %[mx], %[xr1] \n\t"
262// : [mx] "=d" (mx), [xr1] "=d" (xr1)
263// : [m16] "d" (m16), [secoffset] "d" (secoffset)
264// );
265
266// int8_t y = mx + b;
267// if( theta & 0x80 ) y = -y;
268
269// y += 128;
270
271// return y;
272// }
273
274// #else
275// #define sin8 sin8_C
276
277
278// /// Fast 8-bit approximation of sin(x). This approximation never varies more than
279// /// 2% from the floating point value you'd get by doing
280// ///
281// /// float s = (sin(x) * 128.0) + 128;
282// ///
283// /// @param theta input angle from 0-255
284// /// @returns sin of theta, value between 0 and 255
285// __attribute__ ((unused)) static inline uint8_t sin8_C( uint8_t theta)
286// {
287// uint8_t offset = theta;
288// if( theta & 0x40 ) {
289// offset = (uint8_t)255 - offset;
290// }
291// offset &= 0x3F; // 0..63
292
293// uint8_t secoffset = offset & 0x0F; // 0..15
294// if( theta & 0x40) secoffset++;
295
296// uint8_t section = offset >> 4; // 0..3
297// uint8_t s2 = section * 2;
298// const uint8_t* p = b_m16_interleave;
299// p += s2;
300// uint8_t b = *p;
301// p++;
302// uint8_t m16 = *p;
303
304// uint8_t mx = (m16 * secoffset) >> 4;
305
306// int8_t y = mx + b;
307// if( theta & 0x80 ) y = -y;
308
309// y += 128;
310
311// return y;
312// }
313
314// #endif
315
316// /// Fast 8-bit approximation of cos(x). This approximation never varies more than
317// /// 2% from the floating point value you'd get by doing
318// ///
319// /// float s = (cos(x) * 128.0) + 128;
320// ///
321// /// @param theta input angle from 0-255
322// /// @returns sin of theta, value between 0 and 255
323// __attribute__ ((unused)) static inline uint8_t cos8( uint8_t theta)
324// {
325// return sin8( theta + 64);
326// }
327
328// ///@}
329
330} // namespace pq
331
332#endif