diff --git a/wled00/wled_math.cpp b/wled00/wled_math.cpp index 1825ab198..a191968e1 100644 --- a/wled00/wled_math.cpp +++ b/wled00/wled_math.cpp @@ -15,17 +15,17 @@ // Note: cos_t, sin_t and tan_t are very accurate but may be slow // the math.h functions use several kB of flash and are to be avoided if possible // sin16_t / cos16_t are faster and much more accurate than the fastled variants -// sin_approx and cos_approx are float wrappers for sin16_t/cos16_t and have an accuracy of +/-0.0015 compared to sinf() +// sin_approx and cos_approx are float wrappers for sin16_t/cos16_t and have an accuracy better than +/-0.0015 compared to sinf() // sin8_t / cos8_t are fastled replacements and use sin16_t / cos16_t. Slightly slower than fastled version but very accurate float cos_t(float phi) { - float x = modd(phi, TWO_PI); + float x = modd(phi, M_TWOPI); if (x < 0) x = -1 * x; int8_t sign = 1; - if (x > PI) + if (x > M_PI) { - x -= PI; + x -= M_PI; sign = -1; } float xx = x * x; @@ -38,7 +38,7 @@ float cos_t(float phi) } float sin_t(float phi) { - float res = cos_t(HALF_PI - phi); + float res = cos_t(M_PI_2 - phi); #ifdef WLED_DEBUG_MATH Serial.printf("sin: %f,%f,%f,(%f)\n",x,res,sin(x),res-sin(x)); #endif @@ -87,9 +87,7 @@ uint8_t cos8_t(uint8_t theta) { float sin_approx(float theta) { - theta = modd(theta, TWO_PI); // modulo: bring to -2pi to 2pi range - if(theta < 0) theta += TWO_PI; // 0-2pi range - uint16_t scaled_theta = (uint16_t)(theta * (0xFFFF / TWO_PI)); + uint16_t scaled_theta = (int)(theta * (0xFFFF / M_TWOPI)); // note: do not cast negative float to uint! cast to int first (undefined on C3) int32_t result = sin16_t(scaled_theta); float sin = float(result) / 0x7FFF; return sin; @@ -118,10 +116,10 @@ float atan2_t(float y, float x) { float angle; if(x < 0) { r = -r; - angle = M_PI/2.0f + M_PI/4.f; + angle = M_PI_2 + M_PI_4; } else - angle = M_PI/2.0f - M_PI/4.f; + angle = M_PI_2 - M_PI_4; float add = (ATAN2_CONST_A * (r * r) - ATAN2_CONST_B) * r; angle += add; @@ -140,10 +138,10 @@ float acos_t(float x) { ret = ret * xabs; ret = ret - 0.2121144f; ret = ret * xabs; - ret = ret + HALF_PI; + ret = ret + M_PI_2; ret = ret * sqrt(1.0f-xabs); ret = ret - 2 * negate * ret; - float res = negate * PI + ret; + float res = negate * M_PI + ret; #ifdef WLED_DEBUG_MATH Serial.printf("acos: %f,%f,%f,(%f)\n",x,res,acos(x),res-acos(x)); #endif @@ -151,7 +149,7 @@ float acos_t(float x) { } float asin_t(float x) { - float res = HALF_PI - acos_t(x); + float res = M_PI_2 - acos_t(x); #ifdef WLED_DEBUG_MATH Serial.printf("asin: %f,%f,%f,(%f)\n",x,res,asin(x),res-asin(x)); #endif @@ -167,7 +165,7 @@ float atan_t(float x) { //For A/B/C, see https://stackoverflow.com/a/42542593 static const double A { 0.0776509570923569 }; static const double B { -0.287434475393028 }; - static const double C { ((HALF_PI/2) - A - B) }; + static const double C { ((M_PI_4) - A - B) }; // polynominal factors for approximation between 1 and 5 static const float C0 { 0.089494f }; static const float C1 { 0.974207f }; @@ -182,7 +180,7 @@ float atan_t(float x) { x = std::abs(x); float res; if (x > 5.0f) { // atan(x) converges to pi/2 - (1/x) for large values - res = HALF_PI - (1.0f/x); + res = M_PI_2 - (1.0f/x); } else if (x > 1.0f) { //1 < x < 5 float xx = x * x; res = (C4*xx*xx)+(C3*xx*x)+(C2*xx)+(C1*x)+C0;