1 files changed, 259 insertions, 0 deletions

diff --git a/lib/lib8tion/trig8.h b/lib/lib8tion/trig8.h
new file mode 100644
index 000000000..4907c6ff3
--- /dev/null
+++ b/lib/lib8tion/trig8.h
@@ -0,0 +1,259 @@
+#ifndef __INC_LIB8TION_TRIG_H
+#define __INC_LIB8TION_TRIG_H
+
+///@ingroup lib8tion
+
+///@defgroup Trig Fast trig functions
+/// Fast 8 and 16-bit approximations of sin(x) and cos(x).
+///        Don't use these approximations for calculating the
+///        trajectory of a rocket to Mars, but they're great
+///        for art projects and LED displays.
+///
+///        On Arduino/AVR, the 16-bit approximation is more than
+///        10X faster than floating point sin(x) and cos(x), while
+/// the 8-bit approximation is more than 20X faster.
+///@{
+
+#if defined(__AVR__)
+#define sin16 sin16_avr
+#else
+#define sin16 sin16_C
+#endif
+
+/// Fast 16-bit approximation of sin(x). This approximation never varies more than
+/// 0.69% from the floating point value you'd get by doing
+///
+///     float s = sin(x) * 32767.0;
+///
+/// @param theta input angle from 0-65535
+/// @returns sin of theta, value between -32767 to 32767.
+LIB8STATIC int16_t sin16_avr( uint16_t theta )
+{
+    static const uint8_t data[] =
+    { 0,         0,         49, 0, 6393%256,   6393/256, 48, 0,
+      12539%256, 12539/256, 44, 0, 18204%256, 18204/256, 38, 0,
+      23170%256, 23170/256, 31, 0, 27245%256, 27245/256, 23, 0,
+      30273%256, 30273/256, 14, 0, 32137%256, 32137/256,  4 /*,0*/ };
+
+    uint16_t offset = (theta & 0x3FFF);
+
+    // AVR doesn't have a multi-bit shift instruction,
+    // so if we say "offset >>= 3", gcc makes a tiny loop.
+    // Inserting empty volatile statements between each
+    // bit shift forces gcc to unroll the loop.
+    offset >>= 1; // 0..8191
+    asm volatile("");
+    offset >>= 1; // 0..4095
+    asm volatile("");
+    offset >>= 1; // 0..2047
+
+    if( theta & 0x4000 ) offset = 2047 - offset;
+
+    uint8_t sectionX4;
+    sectionX4 = offset / 256;
+    sectionX4 *= 4;
+
+    uint8_t m;
+
+    union {
+        uint16_t b;
+        struct {
+            uint8_t blo;
+            uint8_t bhi;
+        };
+    } u;
+
+    //in effect u.b = blo + (256 * bhi);
+    u.blo = data[ sectionX4 ];
+    u.bhi = data[ sectionX4 + 1];
+    m     = data[ sectionX4 + 2];
+
+    uint8_t secoffset8 = (uint8_t)(offset) / 2;
+
+    uint16_t mx = m * secoffset8;
+
+    int16_t  y  = mx + u.b;
+    if( theta & 0x8000 ) y = -y;
+
+    return y;
+}
+
+/// Fast 16-bit approximation of sin(x). This approximation never varies more than
+/// 0.69% from the floating point value you'd get by doing
+///
+///     float s = sin(x) * 32767.0;
+///
+/// @param theta input angle from 0-65535
+/// @returns sin of theta, value between -32767 to 32767.
+LIB8STATIC int16_t sin16_C( uint16_t theta )
+{
+    static const uint16_t base[] =
+    { 0, 6393, 12539, 18204, 23170, 27245, 30273, 32137 };
+    static const uint8_t slope[] =
+    { 49, 48, 44, 38, 31, 23, 14, 4 };
+
+    uint16_t offset = (theta & 0x3FFF) >> 3; // 0..2047
+    if( theta & 0x4000 ) offset = 2047 - offset;
+
+    uint8_t section = offset / 256; // 0..7
+    uint16_t b   = base[section];
+    uint8_t  m   = slope[section];
+
+    uint8_t secoffset8 = (uint8_t)(offset) / 2;
+
+    uint16_t mx = m * secoffset8;
+    int16_t  y  = mx + b;
+
+    if( theta & 0x8000 ) y = -y;
+
+    return y;
+}
+
+
+/// Fast 16-bit approximation of cos(x). This approximation never varies more than
+/// 0.69% from the floating point value you'd get by doing
+///
+///     float s = cos(x) * 32767.0;
+///
+/// @param theta input angle from 0-65535
+/// @returns sin of theta, value between -32767 to 32767.
+LIB8STATIC int16_t cos16( uint16_t theta)
+{
+    return sin16( theta + 16384);
+}
+
+///////////////////////////////////////////////////////////////////////
+
+// sin8 & cos8
+//        Fast 8-bit approximations of sin(x) & cos(x).
+//        Input angle is an unsigned int from 0-255.
+//        Output is an unsigned int from 0 to 255.
+//
+//        This approximation can vary to to 2%
+//        from the floating point value you'd get by doing
+//          float s = (sin( x ) * 128.0) + 128;
+//
+//        Don't use this approximation for calculating the
+//        "real" trigonometric calculations, but it's great
+//        for art projects and LED displays.
+//
+//        On Arduino/AVR, this approximation is more than
+//        20X faster than floating point sin(x) and cos(x)
+
+#if defined(__AVR__) && !defined(LIB8_ATTINY)
+#define sin8 sin8_avr
+#else
+#define sin8 sin8_C
+#endif
+
+
+const uint8_t b_m16_interleave[] = { 0, 49, 49, 41, 90, 27, 117, 10 };
+
+/// Fast 8-bit approximation of sin(x). This approximation never varies more than
+/// 2% from the floating point value you'd get by doing
+///
+///     float s = (sin(x) * 128.0) + 128;
+///
+/// @param theta input angle from 0-255
+/// @returns sin of theta, value between 0 and 255
+LIB8STATIC uint8_t  sin8_avr( uint8_t theta)
+{
+    uint8_t offset = theta;
+
+    asm volatile(
+                 "sbrc %[theta],6         \n\t"
+                 "com  %[offset]           \n\t"
+                 : [theta] "+r" (theta), [offset] "+r" (offset)
+                 );
+
+    offset &= 0x3F; // 0..63
+
+    uint8_t secoffset  = offset & 0x0F; // 0..15
+    if( theta & 0x40) secoffset++;
+
+    uint8_t m16; uint8_t b;
+
+    uint8_t section = offset >> 4; // 0..3
+    uint8_t s2 = section * 2;
+
+    const uint8_t* p = b_m16_interleave;
+    p += s2;
+    b   = *p;
+    p++;
+    m16 = *p;
+
+    uint8_t mx;
+    uint8_t xr1;
+    asm volatile(
+                 "mul %[m16],%[secoffset]   \n\t"
+                 "mov %[mx],r0              \n\t"
+                 "mov %[xr1],r1             \n\t"
+                 "eor  r1, r1               \n\t"
+                 "swap %[mx]                \n\t"
+                 "andi %[mx],0x0F           \n\t"
+                 "swap %[xr1]               \n\t"
+                 "andi %[xr1], 0xF0         \n\t"
+                 "or   %[mx], %[xr1]        \n\t"
+                 : [mx] "=d" (mx), [xr1] "=d" (xr1)
+                 : [m16] "d" (m16), [secoffset] "d" (secoffset)
+                 );
+
+    int8_t y = mx + b;
+    if( theta & 0x80 ) y = -y;
+
+    y += 128;
+
+    return y;
+}
+
+
+/// Fast 8-bit approximation of sin(x). This approximation never varies more than
+/// 2% from the floating point value you'd get by doing
+///
+///     float s = (sin(x) * 128.0) + 128;
+///
+/// @param theta input angle from 0-255
+/// @returns sin of theta, value between 0 and 255
+LIB8STATIC uint8_t sin8_C( uint8_t theta)
+{
+    uint8_t offset = theta;
+    if( theta & 0x40 ) {
+        offset = (uint8_t)255 - offset;
+    }
+    offset &= 0x3F; // 0..63
+
+    uint8_t secoffset  = offset & 0x0F; // 0..15
+    if( theta & 0x40) secoffset++;
+
+    uint8_t section = offset >> 4; // 0..3
+    uint8_t s2 = section * 2;
+    const uint8_t* p = b_m16_interleave;
+    p += s2;
+    uint8_t b   =  *p;
+    p++;
+    uint8_t m16 =  *p;
+
+    uint8_t mx = (m16 * secoffset) >> 4;
+
+    int8_t y = mx + b;
+    if( theta & 0x80 ) y = -y;
+
+    y += 128;
+
+    return y;
+}
+
+/// Fast 8-bit approximation of cos(x). This approximation never varies more than
+/// 2% from the floating point value you'd get by doing
+///
+///     float s = (cos(x) * 128.0) + 128;
+///
+/// @param theta input angle from 0-255
+/// @returns sin of theta, value between 0 and 255
+LIB8STATIC uint8_t cos8( uint8_t theta)
+{
+    return sin8( theta + 64);
+}
+
+///@}
+#endif