lib/lib8tion/trig8.h

   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.
  30 LIB8STATIC 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.
  88 LIB8STATIC 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.
 120 LIB8STATIC 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
 150 const uint8_t b_m16_interleave[] = { 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
 159 LIB8STATIC 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
 217 LIB8STATIC 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
 253 LIB8STATIC uint8_t cos8( uint8_t theta)
 254 {
 255     return sin8( theta + 64);
 256 }
 257
 258 ///@}
 259 #endif