1 #ifndef __SDR_MATH_HH__
2 #define __SDR_MATH_HH__
3
4 #include <inttypes.h>
5
6 namespace sdr {
7
8 /** Template prototype for @c fast_atan2. */
9 template <class iScalar, class oScalar> oScalar fast_atan2(iScalar a, iScalar b);
10
11 /** Implementation of atan2 approximation using integers. */
fast_atan2(int8_t a,int8_t b)12 template <> inline int16_t fast_atan2<int8_t, int16_t>(int8_t a, int8_t b) {
13 const int32_t pi4 = (1<<12);
14 const int32_t pi34 = 3*(1<<12);
15 int32_t aabs, angle;
16 if ((0 == a) && (0 == b)) { return 0; }
17 aabs = (a >= 0) ? a : -a;
18 if (b >= 0) { angle = pi4 - pi4*(b-aabs) / (b+aabs); }
19 else { angle = pi34 - pi4*(b+aabs) / (aabs-b); }
20 return (a >= 0) ? angle : -angle;
21 }
22
fast_atan2(uint8_t ua,uint8_t ub)23 template <> inline int16_t fast_atan2<uint8_t, int16_t>(uint8_t ua, uint8_t ub) {
24 int8_t a = (int16_t(ua)-(1<<7));
25 int8_t b = (int16_t(ub)-(1<<7));
26 return fast_atan2<int8_t, int16_t>(a,b);
27 }
28
29 /** Implementation of atan2 approximation using integers. */
fast_atan2(int16_t a,int16_t b)30 template <> inline int16_t fast_atan2<int16_t, int16_t>(int16_t a, int16_t b) {
31 //return (1<<15)*(std::atan2(float(a), float(b))/M_PI);
32 const int32_t pi4 = (1<<12);
33 const int32_t pi34 = 3*(1<<12);
34 int32_t aabs, angle;
35 if ((0 == a) && (0 == b)) { return 0; }
36 aabs = (a >= 0) ? a : -a;
37 if (b >= 0) { angle = pi4 - pi4*(b-aabs) / (b+aabs); }
38 else { angle = pi34 - pi4*(b+aabs) / (aabs-b); }
39 return (a >= 0) ? angle : -angle;
40 }
41
42
43 }
44 #endif // MATH_HH
45