| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /* | ||
| 2 | * Contains some trigonometric functions. | ||
| 3 | * The ANSI C equivalents are likely faster, but using any sin/cos/tan function incurs a memory penalty of 460 bytes on ESP8266, likely for lookup tables. | ||
| 4 | * This implementation has no extra static memory usage. | ||
| 5 | * | ||
| 6 | * Source of the cos_t() function: https://web.eecs.utk.edu/~azh/blog/cosine.html (cos_taylor_literal_6terms) | ||
| 7 | */ | ||
| 8 | |||
| 9 | |||
| 10 | #include <stdint.h> | ||
| 11 | #include <stdlib.h> | ||
| 12 | #include <algorithm> | ||
| 13 | #include <cmath> | ||
| 14 | |||
| 15 | #define M_TWOPI 6.283185307179586476925286766559 | ||
| 16 | |||
| 17 | //#define WLED_DEBUG_MATH | ||
| 18 | |||
| 19 | // Note: cos_t, sin_t and tan_t are very accurate but slow | ||
| 20 | // the math.h functions use several kB of flash and are to be avoided if possible | ||
| 21 | // sin16_t / cos16_t are faster and much more accurate than the fastled variants | ||
| 22 | // sin_approx and cos_approx are float wrappers for sin16_t/cos16_t and have an accuracy better than +/-0.0015 compared to sinf() | ||
| 23 | // sin8_t / cos8_t are fastled replacements and use sin16_t / cos16_t. Slightly slower than fastled version but very accurate | ||
| 24 | |||
| 25 | |||
| 26 | // Taylor series approximations, replaced with Bhaskara I's approximation | ||
| 27 | /* | ||
| 28 | #define modd(x, y) ((x) - (int)((x) / (y)) * (y)) | ||
| 29 | |||
| 30 | float cos_t(float phi) | ||
| 31 | { | ||
| 32 | float x = modd(phi, M_TWOPI); | ||
| 33 | if (x < 0) x = -1 * x; | ||
| 34 | int8_t sign = 1; | ||
| 35 | if (x > M_PI) | ||
| 36 | { | ||
| 37 | x -= M_PI; | ||
| 38 | sign = -1; | ||
| 39 | } | ||
| 40 | float xx = x * x; | ||
| 41 | |||
| 42 | float res = sign * (1 - ((xx) / (2)) + ((xx * xx) / (24)) - ((xx * xx * xx) / (720)) + ((xx * xx * xx * xx) / (40320)) - ((xx * xx * xx * xx * xx) / (3628800)) + ((xx * xx * xx * xx * xx * xx) / (479001600))); | ||
| 43 | #ifdef WLED_DEBUG_MATH | ||
| 44 | Serial.printf("cos: %f,%f,%f,(%f)\n",phi,res,cos(x),res-cos(x)); | ||
| 45 | #endif | ||
| 46 | return res; | ||
| 47 | } | ||
| 48 | |||
| 49 | float sin_t(float phi) { | ||
| 50 | float res = cos_t(M_PI_2 - phi); | ||
| 51 | #ifdef WLED_DEBUG_MATH | ||
| 52 | Serial.printf("sin: %f,%f,%f,(%f)\n",x,res,sin(x),res-sin(x)); | ||
| 53 | #endif | ||
| 54 | return res; | ||
| 55 | } | ||
| 56 | |||
| 57 | float tan_t(float x) { | ||
| 58 | float c = cos_t(x); | ||
| 59 | if (c==0.0f) return 0; | ||
| 60 | float res = sin_t(x) / c; | ||
| 61 | #ifdef WLED_DEBUG_MATH | ||
| 62 | Serial.printf("tan: %f,%f,%f,(%f)\n",x,res,tan(x),res-tan(x)); | ||
| 63 | #endif | ||
| 64 | return res; | ||
| 65 | } | ||
| 66 | */ | ||
| 67 | |||
| 68 | // 16-bit, integer based Bhaskara I's sine approximation: 16*x*(pi - x) / (5*pi^2 - 4*x*(pi - x)) | ||
| 69 | // input is 16bit unsigned (0-65535), output is 16bit signed (-32767 to +32767) | ||
| 70 | // optimized integer implementation by @dedehai | ||
| 71 | 627995 | int16_t sin16_t(uint16_t theta) { | |
| 72 | 627995 | int scale = 1; | |
| 73 |
2/2✓ Branch 0 taken 218821 times.
✓ Branch 1 taken 409174 times.
|
627995 | if (theta > 0x7FFF) { |
| 74 | 218821 | theta = 0xFFFF - theta; | |
| 75 | 218821 | scale = -1; // second half of the sine function is negative (pi - 2*pi) | |
| 76 | } | ||
| 77 | 627995 | uint32_t precal = theta * (0x7FFF - theta); | |
| 78 | 627995 | uint64_t numerator = (uint64_t)precal * (4 * 0x7FFF); // 64bit required | |
| 79 | 627995 | int32_t denominator = 1342095361 - precal; // 1342095361 is 5 * 0x7FFF^2 / 4 | |
| 80 | 627995 | int16_t result = numerator / denominator; | |
| 81 | 627995 | return result * scale; | |
| 82 | } | ||
| 83 | |||
| 84 | 274 | int16_t cos16_t(uint16_t theta) { | |
| 85 | 274 | return sin16_t(theta + 0x4000); //cos(x) = sin(x+pi/2) | |
| 86 | } | ||
| 87 | |||
| 88 | 369895 | uint8_t sin8_t(uint8_t theta) { | |
| 89 |
1/2✓ Branch 0 taken 369895 times.
✗ Branch 1 not taken.
|
369895 | int32_t sin16 = sin16_t((uint16_t)theta * 257); // 255 * 257 = 0xFFFF |
| 90 | 369895 | sin16 += 0x7FFF + 128; //shift result to range 0-0xFFFF, +128 for rounding | |
| 91 | 369895 | return std::min(sin16, int32_t(0xFFFF)) >> 8; // min performs saturation, and prevents overflow | |
| 92 | } | ||
| 93 | |||
| 94 | 121795 | uint8_t cos8_t(uint8_t theta) { | |
| 95 | 121795 | return sin8_t(theta + 64); //cos(x) = sin(x+pi/2) | |
| 96 | } | ||
| 97 | |||
| 98 | 79420 | float sin_approx(float theta) { | |
| 99 | 79420 | uint16_t scaled_theta = (int)(theta * (float)(0xFFFF / M_TWOPI)); // note: do not cast negative float to uint! cast to int first (undefined on C3) | |
| 100 | 79420 | int32_t result = sin16_t(scaled_theta); | |
| 101 | 79420 | float sin = float(result) / 0x7FFF; | |
| 102 | 79420 | return sin; | |
| 103 | } | ||
| 104 | |||
| 105 | 79402 | float cos_approx(float theta) { | |
| 106 | 79402 | uint16_t scaled_theta = (int)(theta * (float)(0xFFFF / M_TWOPI)); // note: do not cast negative float to uint! cast to int first (undefined on C3) | |
| 107 | 79402 | int32_t result = sin16_t(scaled_theta + 0x4000); | |
| 108 | 79402 | float cos = float(result) / 0x7FFF; | |
| 109 | 79402 | return cos; | |
| 110 | } | ||
| 111 | |||
| 112 | ✗ | float tan_approx(float x) { | |
| 113 | ✗ | float c = cos_approx(x); | |
| 114 | ✗ | if (c==0.0f) return 0; | |
| 115 | ✗ | float res = sin_approx(x) / c; | |
| 116 | ✗ | return res; | |
| 117 | } | ||
| 118 | |||
| 119 | #define ATAN2_CONST_A 0.1963f | ||
| 120 | #define ATAN2_CONST_B 0.9817f | ||
| 121 | |||
| 122 | // atan2_t approximation, with the idea from https://gist.github.com/volkansalma/2972237?permalink_comment_id=3872525#gistcomment-3872525 | ||
| 123 | 1557 | float atan2_t(float y, float x) { | |
| 124 | 1557 | float abs_y = fabs(y); | |
| 125 | 1557 | float abs_x = fabs(x); | |
| 126 | 1557 | float r = (abs_x - abs_y) / (abs_y + abs_x + 1e-10f); // avoid division by zero by adding a small nubmer | |
| 127 | float angle; | ||
| 128 |
2/2✓ Branch 0 taken 761 times.
✓ Branch 1 taken 796 times.
|
1557 | if(x < 0) { |
| 129 | 761 | r = -r; | |
| 130 | 761 | angle = M_PI_2 + M_PI_4; | |
| 131 | } | ||
| 132 | else | ||
| 133 | 796 | angle = M_PI_2 - M_PI_4; | |
| 134 | |||
| 135 | 1557 | float add = (ATAN2_CONST_A * (r * r) - ATAN2_CONST_B) * r; | |
| 136 | 1557 | angle += add; | |
| 137 |
2/2✓ Branch 0 taken 743 times.
✓ Branch 1 taken 814 times.
|
1557 | angle = y < 0 ? -angle : angle; |
| 138 | 1557 | return angle; | |
| 139 | } | ||
| 140 | |||
| 141 | //https://stackoverflow.com/questions/3380628 | ||
| 142 | // Absolute error <= 6.7e-5 | ||
| 143 | ✗ | float acos_t(float x) { | |
| 144 | ✗ | float negate = float(x < 0); | |
| 145 | ✗ | float xabs = std::abs(x); | |
| 146 | ✗ | float ret = -0.0187293f; | |
| 147 | ✗ | ret = ret * xabs; | |
| 148 | ✗ | ret = ret + 0.0742610f; | |
| 149 | ✗ | ret = ret * xabs; | |
| 150 | ✗ | ret = ret - 0.2121144f; | |
| 151 | ✗ | ret = ret * xabs; | |
| 152 | ✗ | ret = ret + M_PI_2; | |
| 153 | ✗ | ret = ret * sqrt(1.0f-xabs); | |
| 154 | ✗ | ret = ret - 2 * negate * ret; | |
| 155 | ✗ | float res = negate * M_PI + ret; | |
| 156 | #ifdef WLED_DEBUG_MATH | ||
| 157 | Serial.printf("acos: %f,%f,%f,(%f)\n",x,res,acos(x),res-acos(x)); | ||
| 158 | #endif | ||
| 159 | ✗ | return res; | |
| 160 | } | ||
| 161 | |||
| 162 | ✗ | float asin_t(float x) { | |
| 163 | ✗ | float res = M_PI_2 - acos_t(x); | |
| 164 | #ifdef WLED_DEBUG_MATH | ||
| 165 | Serial.printf("asin: %f,%f,%f,(%f)\n",x,res,asin(x),res-asin(x)); | ||
| 166 | #endif | ||
| 167 | ✗ | return res; | |
| 168 | } | ||
| 169 | |||
| 170 | // declare a template with no implementation, and only one specialization | ||
| 171 | // this allows hiding the constants, while ensuring ODR causes optimizations | ||
| 172 | // to still apply. (Fixes issues with conflicting 3rd party #define's) | ||
| 173 | template <typename T> T atan_t(T x); | ||
| 174 | template<> | ||
| 175 | ✗ | float atan_t(float x) { | |
| 176 | //For A/B/C, see https://stackoverflow.com/a/42542593 | ||
| 177 | static const double A { 0.0776509570923569 }; | ||
| 178 | static const double B { -0.287434475393028 }; | ||
| 179 | static const double C { ((M_PI_4) - A - B) }; | ||
| 180 | // polynominal factors for approximation between 1 and 5 | ||
| 181 | static const float C0 { 0.089494f }; | ||
| 182 | static const float C1 { 0.974207f }; | ||
| 183 | static const float C2 { -0.326175f }; | ||
| 184 | static const float C3 { 0.05375f }; | ||
| 185 | static const float C4 { -0.003445f }; | ||
| 186 | |||
| 187 | #ifdef WLED_DEBUG_MATH | ||
| 188 | float xinput = x; | ||
| 189 | #endif | ||
| 190 | ✗ | bool neg = (x < 0); | |
| 191 | ✗ | x = std::abs(x); | |
| 192 | float res; | ||
| 193 | ✗ | if (x > 5.0f) { // atan(x) converges to pi/2 - (1/x) for large values | |
| 194 | ✗ | res = M_PI_2 - (1.0f/x); | |
| 195 | ✗ | } else if (x > 1.0f) { //1 < x < 5 | |
| 196 | ✗ | float xx = x * x; | |
| 197 | ✗ | res = (C4*xx*xx)+(C3*xx*x)+(C2*xx)+(C1*x)+C0; | |
| 198 | } else { // this approximation is only for x <= 1 | ||
| 199 | ✗ | float xx = x * x; | |
| 200 | ✗ | res = ((A*xx + B)*xx + C)*x; | |
| 201 | } | ||
| 202 | ✗ | if (neg) { | |
| 203 | ✗ | res = -res; | |
| 204 | } | ||
| 205 | #ifdef WLED_DEBUG_MATH | ||
| 206 | Serial.printf("atan: %f,%f,%f,(%f)\n",xinput,res,atan(xinput),res-atan(xinput)); | ||
| 207 | #endif | ||
| 208 | ✗ | return res; | |
| 209 | } | ||
| 210 | |||
| 211 | ✗ | float floor_t(float x) { | |
| 212 | ✗ | bool neg = x < 0; | |
| 213 | ✗ | int val = x; | |
| 214 | ✗ | if (neg) val--; | |
| 215 | #ifdef WLED_DEBUG_MATH | ||
| 216 | Serial.printf("floor: %f,%f,%f\n",x,(float)val,floor(x)); | ||
| 217 | #endif | ||
| 218 | ✗ | return val; | |
| 219 | } | ||
| 220 | |||
| 221 | ✗ | float fmod_t(float num, float denom) { | |
| 222 | ✗ | int tquot = num / denom; | |
| 223 | ✗ | float res = num - tquot * denom; | |
| 224 | #ifdef WLED_DEBUG_MATH | ||
| 225 | Serial.printf("fmod: %f,%f,(%f)\n",res,fmod(num,denom),res-fmod(num,denom)); | ||
| 226 | #endif | ||
| 227 | ✗ | return res; | |
| 228 | } | ||
| 229 | |||
| 230 | // bit-wise integer square root calculation (exact) | ||
| 231 | 20522 | uint32_t sqrt32_bw(uint32_t x) { | |
| 232 | 20522 | uint32_t res = 0; | |
| 233 | uint32_t bit; | ||
| 234 | 20522 | uint32_t num = x; // use 32bit for faster calculation | |
| 235 | |||
| 236 |
2/2✓ Branch 0 taken 20347 times.
✓ Branch 1 taken 175 times.
|
20522 | if(num < 1 << 10) bit = 1 << 10; // speed optimization for small numbers < 32^2 |
| 237 |
1/2✓ Branch 0 taken 175 times.
✗ Branch 1 not taken.
|
175 | else if (num < 1 << 20) bit = 1 << 20; // speed optimization for medium numbers < 1024^2 |
| 238 | ✗ | else bit = 1 << 30; // start with highest power of 4 <= 2^32 | |
| 239 | |||
| 240 |
2/2✓ Branch 0 taken 44903 times.
✓ Branch 1 taken 20522 times.
|
65425 | while (bit > num) bit >>= 2; // reduce iterations |
| 241 | |||
| 242 |
2/2✓ Branch 0 taken 79104 times.
✓ Branch 1 taken 20522 times.
|
99626 | while (bit != 0) { |
| 243 |
2/2✓ Branch 0 taken 46614 times.
✓ Branch 1 taken 32490 times.
|
79104 | if (num >= res + bit) { |
| 244 | 46614 | num -= res + bit; | |
| 245 | 46614 | res = (res >> 1) + bit; | |
| 246 | } else { | ||
| 247 | 32490 | res >>= 1; | |
| 248 | } | ||
| 249 | 79104 | bit >>= 2; | |
| 250 | } | ||
| 251 | 20522 | return res; | |
| 252 | } | ||
| 253 |