2019-07-01 17:20:43 +01:00
|
|
|
/*
|
2019-10-27 10:13:24 +00:00
|
|
|
support_float.ino - Small floating point support for Tasmota
|
2019-07-01 17:20:43 +01:00
|
|
|
|
2021-01-01 12:44:04 +00:00
|
|
|
Copyright (C) 2021 Theo Arends and Stephan Hadinger
|
2019-07-01 17:20:43 +01:00
|
|
|
|
|
|
|
This program is free software: you can redistribute it and/or modify
|
|
|
|
it under the terms of the GNU General Public License as published by
|
|
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
|
|
(at your option) any later version.
|
|
|
|
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
|
|
GNU General Public License for more details.
|
|
|
|
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
|
|
*/
|
|
|
|
|
2019-07-02 16:18:32 +01:00
|
|
|
float fmodf(float x, float y)
|
|
|
|
{
|
2019-07-03 11:32:44 +01:00
|
|
|
// https://github.com/micropython/micropython/blob/master/lib/libm/fmodf.c
|
|
|
|
union {float f; uint32_t i;} ux = {x}, uy = {y};
|
|
|
|
int ex = ux.i>>23 & 0xff;
|
|
|
|
int ey = uy.i>>23 & 0xff;
|
|
|
|
uint32_t sx = ux.i & 0x80000000;
|
|
|
|
uint32_t i;
|
|
|
|
uint32_t uxi = ux.i;
|
2019-07-02 16:59:40 +01:00
|
|
|
|
2019-07-03 11:32:44 +01:00
|
|
|
if (uy.i<<1 == 0 || isnan(y) || ex == 0xff)
|
|
|
|
return (x*y)/(x*y);
|
|
|
|
if (uxi<<1 <= uy.i<<1) {
|
|
|
|
if (uxi<<1 == uy.i<<1)
|
|
|
|
return 0*x;
|
|
|
|
return x;
|
2019-07-02 16:18:32 +01:00
|
|
|
}
|
|
|
|
|
2019-07-03 11:32:44 +01:00
|
|
|
// normalize x and y
|
|
|
|
if (!ex) {
|
|
|
|
for (i = uxi<<9; i>>31 == 0; ex--, i <<= 1);
|
|
|
|
uxi <<= -ex + 1;
|
2019-07-02 16:18:32 +01:00
|
|
|
} else {
|
2019-07-03 11:32:44 +01:00
|
|
|
uxi &= -1U >> 9;
|
|
|
|
uxi |= 1U << 23;
|
2019-07-02 16:18:32 +01:00
|
|
|
}
|
2019-07-03 11:32:44 +01:00
|
|
|
if (!ey) {
|
|
|
|
for (i = uy.i<<9; i>>31 == 0; ey--, i <<= 1);
|
|
|
|
uy.i <<= -ey + 1;
|
2019-07-02 16:18:32 +01:00
|
|
|
} else {
|
2019-07-03 11:32:44 +01:00
|
|
|
uy.i &= -1U >> 9;
|
|
|
|
uy.i |= 1U << 23;
|
2019-07-02 16:18:32 +01:00
|
|
|
}
|
|
|
|
|
2019-07-03 11:32:44 +01:00
|
|
|
// x mod y
|
|
|
|
for (; ex > ey; ex--) {
|
|
|
|
i = uxi - uy.i;
|
|
|
|
if (i >> 31 == 0) {
|
|
|
|
if (i == 0)
|
|
|
|
return 0*x;
|
|
|
|
uxi = i;
|
2019-07-02 16:18:32 +01:00
|
|
|
}
|
2019-07-03 11:32:44 +01:00
|
|
|
uxi <<= 1;
|
2019-07-02 16:18:32 +01:00
|
|
|
}
|
2019-07-03 11:32:44 +01:00
|
|
|
i = uxi - uy.i;
|
|
|
|
if (i >> 31 == 0) {
|
|
|
|
if (i == 0)
|
|
|
|
return 0*x;
|
|
|
|
uxi = i;
|
2019-07-02 16:18:32 +01:00
|
|
|
}
|
2019-07-03 11:32:44 +01:00
|
|
|
for (; uxi>>23 == 0; uxi <<= 1, ex--);
|
|
|
|
|
|
|
|
// scale result up
|
|
|
|
if (ex > 0) {
|
|
|
|
uxi -= 1U << 23;
|
|
|
|
uxi |= (uint32_t)ex << 23;
|
|
|
|
} else {
|
|
|
|
uxi >>= -ex + 1;
|
2019-07-02 16:18:32 +01:00
|
|
|
}
|
2019-07-03 11:32:44 +01:00
|
|
|
uxi |= sx;
|
|
|
|
ux.i = uxi;
|
|
|
|
return ux.f;
|
2019-07-02 16:18:32 +01:00
|
|
|
}
|
|
|
|
|
2019-07-01 17:31:54 +01:00
|
|
|
double FastPrecisePow(double a, double b)
|
|
|
|
{
|
|
|
|
// https://martin.ankerl.com/2012/01/25/optimized-approximative-pow-in-c-and-cpp/
|
|
|
|
// calculate approximation with fraction of the exponent
|
|
|
|
int e = abs((int)b);
|
|
|
|
union {
|
|
|
|
double d;
|
|
|
|
int x[2];
|
|
|
|
} u = { a };
|
|
|
|
u.x[1] = (int)((b - e) * (u.x[1] - 1072632447) + 1072632447);
|
|
|
|
u.x[0] = 0;
|
|
|
|
// exponentiation by squaring with the exponent's integer part
|
|
|
|
// double r = u.d makes everything much slower, not sure why
|
|
|
|
double r = 1.0;
|
|
|
|
while (e) {
|
|
|
|
if (e & 1) {
|
|
|
|
r *= a;
|
|
|
|
}
|
|
|
|
a *= a;
|
|
|
|
e >>= 1;
|
|
|
|
}
|
|
|
|
return r * u.d;
|
|
|
|
}
|
|
|
|
|
|
|
|
float FastPrecisePowf(const float x, const float y)
|
|
|
|
{
|
|
|
|
// return (float)(pow((double)x, (double)y));
|
|
|
|
return (float)FastPrecisePow(x, y);
|
|
|
|
}
|
|
|
|
|
|
|
|
double TaylorLog(double x)
|
|
|
|
{
|
|
|
|
// https://stackoverflow.com/questions/46879166/finding-the-natural-logarithm-of-a-number-using-taylor-series-in-c
|
|
|
|
|
|
|
|
if (x <= 0.0) { return NAN; }
|
2020-08-05 22:42:34 +01:00
|
|
|
if (x == 1.0) { return 0; }
|
2019-07-01 17:31:54 +01:00
|
|
|
double z = (x + 1) / (x - 1); // We start from power -1, to make sure we get the right power in each iteration;
|
|
|
|
double step = ((x - 1) * (x - 1)) / ((x + 1) * (x + 1)); // Store step to not have to calculate it each time
|
|
|
|
double totalValue = 0;
|
|
|
|
double powe = 1;
|
2019-07-02 16:59:40 +01:00
|
|
|
for (uint32_t count = 0; count < 10; count++) { // Experimental number of 10 iterations
|
2019-07-01 17:31:54 +01:00
|
|
|
z *= step;
|
2019-10-30 13:08:43 +00:00
|
|
|
double y = (1 / powe) * z;
|
2019-07-01 17:31:54 +01:00
|
|
|
totalValue = totalValue + y;
|
|
|
|
powe = powe + 2;
|
|
|
|
}
|
|
|
|
totalValue *= 2;
|
|
|
|
/*
|
|
|
|
char logxs[33];
|
|
|
|
dtostrfd(x, 8, logxs);
|
|
|
|
double log1 = log(x);
|
|
|
|
char log1s[33];
|
|
|
|
dtostrfd(log1, 8, log1s);
|
|
|
|
char log2s[33];
|
|
|
|
dtostrfd(totalValue, 8, log2s);
|
2021-01-23 15:26:23 +00:00
|
|
|
AddLog(LOG_LEVEL_DEBUG, PSTR("input %s, log %s, taylor %s"), logxs, log1s, log2s);
|
2019-07-01 17:31:54 +01:00
|
|
|
*/
|
|
|
|
return totalValue;
|
|
|
|
}
|
|
|
|
|
2019-07-02 16:59:40 +01:00
|
|
|
// Following code adapted from: http://www.ganssle.com/approx.htm
|
|
|
|
// ==============================================================
|
|
|
|
// The following code implements approximations to various trig functions.
|
2019-07-01 17:20:43 +01:00
|
|
|
//
|
2019-07-02 16:59:40 +01:00
|
|
|
// This is demo code to guide developers in implementing their own approximation
|
2019-07-01 17:20:43 +01:00
|
|
|
// software. This code is merely meant to illustrate algorithms.
|
|
|
|
|
|
|
|
inline float sinf(float x) { return sin_52(x); }
|
|
|
|
inline float cosf(float x) { return cos_52(x); }
|
|
|
|
inline float tanf(float x) { return tan_56(x); }
|
|
|
|
inline float atanf(float x) { return atan_66(x); }
|
|
|
|
inline float asinf(float x) { return asinf1(x); }
|
|
|
|
inline float acosf(float x) { return acosf1(x); }
|
|
|
|
inline float sqrtf(float x) { return sqrt1(x); }
|
|
|
|
|
|
|
|
// Math constants we'll use
|
2019-07-02 16:59:40 +01:00
|
|
|
double const f_pi = 3.1415926535897932384626433; // f_pi
|
|
|
|
double const f_twopi = 2.0 * f_pi; // f_pi times 2
|
|
|
|
double const f_two_over_pi = 2.0 / f_pi; // 2/f_pi
|
|
|
|
double const f_halfpi = f_pi / 2.0; // f_pi divided by 2
|
|
|
|
double const f_threehalfpi = 3.0 * f_pi / 2.0; // f_pi times 3/2, used in tan routines
|
|
|
|
double const f_four_over_pi = 4.0 / f_pi; // 4/f_pi, used in tan routines
|
|
|
|
double const f_qtrpi = f_pi / 4.0; // f_pi/4.0, used in tan routines
|
|
|
|
double const f_sixthpi = f_pi / 6.0; // f_pi/6.0, used in atan routines
|
|
|
|
double const f_tansixthpi = tan(f_sixthpi); // tan(f_pi/6), used in atan routines
|
|
|
|
double const f_twelfthpi = f_pi / 12.0; // f_pi/12.0, used in atan routines
|
|
|
|
double const f_tantwelfthpi = tan(f_twelfthpi); // tan(f_pi/12), used in atan routines
|
2019-12-14 11:06:59 +00:00
|
|
|
float const f_180pi = 180 / f_pi; // 180 / pi for angles in degrees
|
2019-07-02 16:59:40 +01:00
|
|
|
|
|
|
|
// *******************************************************************
|
2019-07-01 17:20:43 +01:00
|
|
|
// ***
|
2019-07-02 16:59:40 +01:00
|
|
|
// *** Routines to compute sine and cosine to 5.2 digits of accuracy.
|
2019-07-01 17:20:43 +01:00
|
|
|
// ***
|
2019-07-02 16:59:40 +01:00
|
|
|
// *******************************************************************
|
2019-07-01 17:20:43 +01:00
|
|
|
//
|
|
|
|
// cos_52s computes cosine (x)
|
|
|
|
//
|
|
|
|
// Accurate to about 5.2 decimal digits over the range [0, f_pi/2].
|
|
|
|
// The input argument is in radians.
|
|
|
|
//
|
|
|
|
// Algorithm:
|
|
|
|
// cos(x)= c1 + c2*x**2 + c3*x**4 + c4*x**6
|
|
|
|
// which is the same as:
|
|
|
|
// cos(x)= c1 + x**2(c2 + c3*x**2 + c4*x**4)
|
|
|
|
// cos(x)= c1 + x**2(c2 + x**2(c3 + c4*x**2))
|
|
|
|
//
|
|
|
|
float cos_52s(float x)
|
|
|
|
{
|
2019-07-02 16:59:40 +01:00
|
|
|
const float c1 = 0.9999932946;
|
|
|
|
const float c2 = -0.4999124376;
|
|
|
|
const float c3 = 0.0414877472;
|
|
|
|
const float c4 = -0.0012712095;
|
2019-07-01 17:20:43 +01:00
|
|
|
|
2019-07-02 16:59:40 +01:00
|
|
|
float x2 = x * x; // The input argument squared
|
|
|
|
return (c1 + x2 * (c2 + x2 * (c3 + c4 * x2)));
|
2019-07-01 17:20:43 +01:00
|
|
|
}
|
|
|
|
//
|
2019-07-02 16:59:40 +01:00
|
|
|
// This is the main cosine approximation "driver"
|
2019-07-01 17:20:43 +01:00
|
|
|
// It reduces the input argument's range to [0, f_pi/2],
|
|
|
|
// and then calls the approximator.
|
|
|
|
// See the notes for an explanation of the range reduction.
|
|
|
|
//
|
2019-07-02 16:59:40 +01:00
|
|
|
float cos_52(float x)
|
|
|
|
{
|
|
|
|
x = fmodf(x, f_twopi); // Get rid of values > 2* f_pi
|
|
|
|
if (x < 0) { x = -x; } // cos(-x) = cos(x)
|
|
|
|
int quad = int(x * (float)f_two_over_pi); // Get quadrant # (0 to 3) we're in
|
|
|
|
switch (quad) {
|
|
|
|
case 0: return cos_52s(x);
|
|
|
|
case 1: return -cos_52s((float)f_pi - x);
|
|
|
|
case 2: return -cos_52s(x-(float)f_pi);
|
|
|
|
case 3: return cos_52s((float)f_twopi - x);
|
|
|
|
}
|
2020-07-14 15:21:11 +01:00
|
|
|
return 0.0; // Never reached. Fixes compiler warning
|
2019-07-01 17:20:43 +01:00
|
|
|
}
|
|
|
|
//
|
2019-07-02 16:59:40 +01:00
|
|
|
// The sine is just cosine shifted a half-f_pi, so
|
2019-07-01 17:20:43 +01:00
|
|
|
// we'll adjust the argument and call the cosine approximation.
|
|
|
|
//
|
2019-07-02 16:59:40 +01:00
|
|
|
float sin_52(float x)
|
|
|
|
{
|
|
|
|
return cos_52((float)f_halfpi - x);
|
2019-07-01 17:20:43 +01:00
|
|
|
}
|
|
|
|
|
2019-07-02 16:59:40 +01:00
|
|
|
// *******************************************************************
|
2019-07-01 17:20:43 +01:00
|
|
|
// ***
|
2019-07-02 16:59:40 +01:00
|
|
|
// *** Routines to compute tangent to 5.6 digits of accuracy.
|
2019-07-01 17:20:43 +01:00
|
|
|
// ***
|
2019-07-02 16:59:40 +01:00
|
|
|
// *******************************************************************
|
2019-07-01 17:20:43 +01:00
|
|
|
//
|
|
|
|
// tan_56s computes tan(f_pi*x/4)
|
|
|
|
//
|
|
|
|
// Accurate to about 5.6 decimal digits over the range [0, f_pi/4].
|
|
|
|
// The input argument is in radians. Note that the function
|
|
|
|
// computes tan(f_pi*x/4), NOT tan(x); it's up to the range
|
|
|
|
// reduction algorithm that calls this to scale things properly.
|
|
|
|
//
|
|
|
|
// Algorithm:
|
|
|
|
// tan(x)= x(c1 + c2*x**2)/(c3 + x**2)
|
|
|
|
//
|
|
|
|
float tan_56s(float x)
|
|
|
|
{
|
2019-07-02 16:59:40 +01:00
|
|
|
const float c1 = -3.16783027;
|
|
|
|
const float c2 = 0.134516124;
|
|
|
|
const float c3 = -4.033321984;
|
2019-07-01 17:20:43 +01:00
|
|
|
|
2019-07-02 16:59:40 +01:00
|
|
|
float x2 = x * x; // The input argument squared
|
|
|
|
return (x * (c1 + c2 * x2) / (c3 + x2));
|
2019-07-01 17:20:43 +01:00
|
|
|
}
|
|
|
|
//
|
2019-07-02 16:59:40 +01:00
|
|
|
// This is the main tangent approximation "driver"
|
2019-07-01 17:20:43 +01:00
|
|
|
// It reduces the input argument's range to [0, f_pi/4],
|
|
|
|
// and then calls the approximator.
|
|
|
|
// See the notes for an explanation of the range reduction.
|
|
|
|
// Enter with positive angles only.
|
|
|
|
//
|
|
|
|
// WARNING: We do not test for the tangent approaching infinity,
|
|
|
|
// which it will at x=f_pi/2 and x=3*f_pi/2. If this is a problem
|
|
|
|
// in your application, take appropriate action.
|
|
|
|
//
|
2019-07-02 16:59:40 +01:00
|
|
|
float tan_56(float x)
|
|
|
|
{
|
|
|
|
x = fmodf(x, (float)f_twopi); // Get rid of values >2 *f_pi
|
|
|
|
int octant = int(x * (float)f_four_over_pi); // Get octant # (0 to 7)
|
|
|
|
switch (octant){
|
|
|
|
case 0: return tan_56s(x * (float)f_four_over_pi);
|
|
|
|
case 1: return 1.0f / tan_56s(((float)f_halfpi - x) * (float)f_four_over_pi);
|
|
|
|
case 2: return -1.0f / tan_56s((x-(float)f_halfpi) * (float)f_four_over_pi);
|
|
|
|
case 3: return - tan_56s(((float)f_pi - x) * (float)f_four_over_pi);
|
|
|
|
case 4: return tan_56s((x-(float)f_pi) * (float)f_four_over_pi);
|
|
|
|
case 5: return 1.0f / tan_56s(((float)f_threehalfpi - x) * (float)f_four_over_pi);
|
|
|
|
case 6: return -1.0f / tan_56s((x-(float)f_threehalfpi) * (float)f_four_over_pi);
|
|
|
|
case 7: return - tan_56s(((float)f_twopi - x) * (float)f_four_over_pi);
|
|
|
|
}
|
2020-07-14 15:21:11 +01:00
|
|
|
return 0.0; // Never reached. Fixes compiler warning
|
2019-07-01 17:20:43 +01:00
|
|
|
}
|
|
|
|
|
2019-07-02 16:59:40 +01:00
|
|
|
// *******************************************************************
|
2019-07-01 17:20:43 +01:00
|
|
|
// ***
|
2019-07-02 16:59:40 +01:00
|
|
|
// *** Routines to compute arctangent to 6.6 digits of accuracy.
|
2019-07-01 17:20:43 +01:00
|
|
|
// ***
|
2019-07-02 16:59:40 +01:00
|
|
|
// *******************************************************************
|
2019-07-01 17:20:43 +01:00
|
|
|
//
|
|
|
|
// atan_66s computes atan(x)
|
|
|
|
//
|
|
|
|
// Accurate to about 6.6 decimal digits over the range [0, f_pi/12].
|
|
|
|
//
|
|
|
|
// Algorithm:
|
|
|
|
// atan(x)= x(c1 + c2*x**2)/(c3 + x**2)
|
|
|
|
//
|
|
|
|
float atan_66s(float x)
|
|
|
|
{
|
2019-07-02 16:59:40 +01:00
|
|
|
const float c1 = 1.6867629106;
|
|
|
|
const float c2 = 0.4378497304;
|
|
|
|
const float c3 = 1.6867633134;
|
2019-07-01 17:20:43 +01:00
|
|
|
|
2019-07-02 16:59:40 +01:00
|
|
|
float x2 = x * x; // The input argument squared
|
|
|
|
return (x * (c1 + x2 * c2) / (c3 + x2));
|
2019-07-01 17:20:43 +01:00
|
|
|
}
|
|
|
|
//
|
2019-07-02 16:59:40 +01:00
|
|
|
// This is the main arctangent approximation "driver"
|
2019-07-01 17:20:43 +01:00
|
|
|
// It reduces the input argument's range to [0, f_pi/12],
|
|
|
|
// and then calls the approximator.
|
|
|
|
//
|
2019-07-02 16:59:40 +01:00
|
|
|
float atan_66(float x)
|
|
|
|
{
|
|
|
|
float y; // return from atan__s function
|
|
|
|
bool complement= false; // true if arg was >1
|
|
|
|
bool region= false; // true depending on region arg is in
|
|
|
|
bool sign= false; // true if arg was < 0
|
|
|
|
|
|
|
|
if (x < 0) {
|
|
|
|
x = -x;
|
|
|
|
sign = true; // arctan(-x)=-arctan(x)
|
2019-07-01 17:20:43 +01:00
|
|
|
}
|
2019-07-02 16:59:40 +01:00
|
|
|
if (x > 1.0) {
|
|
|
|
x = 1.0 / x; // keep arg between 0 and 1
|
|
|
|
complement = true;
|
2019-07-01 17:20:43 +01:00
|
|
|
}
|
2019-07-02 16:59:40 +01:00
|
|
|
if (x > (float)f_tantwelfthpi) {
|
|
|
|
x = (x - (float)f_tansixthpi) / (1 + (float)f_tansixthpi * x); // reduce arg to under tan(f_pi/12)
|
|
|
|
region = true;
|
2019-07-01 17:20:43 +01:00
|
|
|
}
|
|
|
|
|
2019-07-02 16:59:40 +01:00
|
|
|
y = atan_66s(x); // run the approximation
|
|
|
|
if (region) { y += (float)f_sixthpi; } // correct for region we're in
|
|
|
|
if (complement) { y = (float)f_halfpi-y; } // correct for 1/x if we did that
|
|
|
|
if (sign) { y = -y; } // correct for negative arg
|
2019-07-01 17:20:43 +01:00
|
|
|
return (y);
|
|
|
|
}
|
|
|
|
|
2019-07-02 16:59:40 +01:00
|
|
|
float asinf1(float x)
|
|
|
|
{
|
|
|
|
float d = 1.0f - x * x;
|
|
|
|
if (d < 0.0f) { return NAN; }
|
|
|
|
return 2 * atan_66(x / (1 + sqrt1(d)));
|
2019-07-01 17:20:43 +01:00
|
|
|
}
|
|
|
|
|
2019-07-02 16:59:40 +01:00
|
|
|
float acosf1(float x)
|
|
|
|
{
|
|
|
|
float d = 1.0f - x * x;
|
|
|
|
if (d < 0.0f) { return NAN; }
|
|
|
|
float y = asinf1(sqrt1(d));
|
|
|
|
if (x >= 0.0f) {
|
|
|
|
return y;
|
|
|
|
} else {
|
|
|
|
return (float)f_pi - y;
|
|
|
|
}
|
2019-07-01 17:20:43 +01:00
|
|
|
}
|
|
|
|
|
|
|
|
// https://www.codeproject.com/Articles/69941/Best-Square-Root-Method-Algorithm-Function-Precisi
|
|
|
|
float sqrt1(const float x)
|
|
|
|
{
|
2019-07-02 16:59:40 +01:00
|
|
|
union {
|
2019-07-01 17:20:43 +01:00
|
|
|
int i;
|
|
|
|
float x;
|
|
|
|
} u;
|
|
|
|
u.x = x;
|
2019-07-02 16:59:40 +01:00
|
|
|
u.i = (1 << 29) + (u.i >> 1) - (1 << 22);
|
2019-07-01 17:20:43 +01:00
|
|
|
|
|
|
|
// Two Babylonian Steps (simplified from:)
|
|
|
|
// u.x = 0.5f * (u.x + x/u.x);
|
|
|
|
// u.x = 0.5f * (u.x + x/u.x);
|
2019-07-02 16:59:40 +01:00
|
|
|
u.x = u.x + x / u.x;
|
|
|
|
u.x = 0.25f * u.x + x / u.x;
|
2019-07-01 17:20:43 +01:00
|
|
|
|
|
|
|
return u.x;
|
|
|
|
}
|
2019-08-05 13:24:50 +01:00
|
|
|
|
|
|
|
//
|
|
|
|
// changeUIntScale
|
|
|
|
// Change a value for range a..b to c..d, using only unsigned int math
|
|
|
|
//
|
2019-11-23 18:18:09 +00:00
|
|
|
// New version, you don't need the "to_min < to_max" precondition anymore
|
|
|
|
//
|
2023-11-23 10:31:26 +00:00
|
|
|
// PRE-CONDITIONS (if not satisfied, returns the smallest between to_min and to_max')
|
|
|
|
// from_min < from_max (checked)
|
2020-11-24 22:56:24 +00:00
|
|
|
// from_min <= num <= from_max (checked)
|
2019-08-05 13:24:50 +01:00
|
|
|
// POST-CONDITIONS
|
|
|
|
// to_min <= result <= to_max
|
|
|
|
//
|
|
|
|
uint16_t changeUIntScale(uint16_t inum, uint16_t ifrom_min, uint16_t ifrom_max,
|
|
|
|
uint16_t ito_min, uint16_t ito_max) {
|
|
|
|
// guard-rails
|
2019-11-23 18:18:09 +00:00
|
|
|
if (ifrom_min >= ifrom_max) {
|
2020-11-24 22:56:24 +00:00
|
|
|
return (ito_min > ito_max ? ito_max : ito_min); // invalid input, return arbitrary value
|
2019-08-05 13:24:50 +01:00
|
|
|
}
|
|
|
|
// convert to uint31, it's more verbose but code is more compact
|
|
|
|
uint32_t num = inum;
|
|
|
|
uint32_t from_min = ifrom_min;
|
|
|
|
uint32_t from_max = ifrom_max;
|
|
|
|
uint32_t to_min = ito_min;
|
|
|
|
uint32_t to_max = ito_max;
|
|
|
|
|
|
|
|
// check source range
|
|
|
|
num = (num > from_max ? from_max : (num < from_min ? from_min : num));
|
2019-11-23 18:18:09 +00:00
|
|
|
|
|
|
|
// check to_* order
|
|
|
|
if (to_min > to_max) {
|
|
|
|
// reverse order
|
|
|
|
num = (from_max - num) + from_min;
|
|
|
|
to_min = ito_max;
|
|
|
|
to_max = ito_min;
|
|
|
|
}
|
|
|
|
|
2021-12-23 11:48:12 +00:00
|
|
|
// short-cut if limits to avoid rounding errors
|
|
|
|
if (num == from_min) return to_min;
|
|
|
|
if (num == from_max) return to_max;
|
|
|
|
|
|
|
|
uint32_t result;
|
|
|
|
if ((num - from_min) < 0x8000L) { // no overflow possible
|
2023-11-23 09:57:47 +00:00
|
|
|
if (to_max - to_min > from_max - from_min) {
|
|
|
|
uint32_t numerator = (num - from_min) * (to_max - to_min) * 2;
|
|
|
|
result = ((numerator / (from_max - from_min)) + 1 ) / 2 + to_min;
|
|
|
|
} else {
|
|
|
|
uint32_t numerator = ((num - from_min) * 2 + 1) * (to_max - to_min + 1);
|
|
|
|
result = numerator / ((from_max - from_min + 1) * 2) + to_min;
|
|
|
|
}
|
2021-12-23 11:48:12 +00:00
|
|
|
} else { // no pre-rounding since it might create an overflow
|
|
|
|
uint32_t numerator = (num - from_min) * (to_max - to_min + 1);
|
|
|
|
result = numerator / (from_max - from_min) + to_min;
|
|
|
|
}
|
|
|
|
|
2019-08-05 13:24:50 +01:00
|
|
|
return (uint32_t) (result > to_max ? to_max : (result < to_min ? to_min : result));
|
|
|
|
}
|
2020-05-07 18:48:43 +01:00
|
|
|
|
2023-11-23 10:31:26 +00:00
|
|
|
//
|
|
|
|
// changeIntScale
|
|
|
|
// Change a value for range a..b to c..d, for signed ints (16 bits max to avoid overflow)
|
|
|
|
//
|
|
|
|
// PRE-CONDITIONS (if not satisfied, returns the smallest between to_min and to_max')
|
|
|
|
// from_min < from_max (checked)
|
|
|
|
// from_min <= num <= from_max (checked)
|
|
|
|
// POST-CONDITIONS
|
|
|
|
// to_min <= result <= to_max
|
|
|
|
//
|
|
|
|
int16_t changeIntScale(int16_t num, int16_t from_min, int16_t from_max,
|
|
|
|
int16_t to_min, int16_t to_max) {
|
|
|
|
|
|
|
|
// guard-rails
|
|
|
|
if (from_min >= from_max) {
|
|
|
|
return (to_min > to_max ? to_max : to_min); // invalid input, return arbitrary value
|
|
|
|
}
|
|
|
|
int32_t from_offset = 0;
|
|
|
|
if (from_min < 0) {
|
|
|
|
from_offset = - from_min;
|
|
|
|
}
|
|
|
|
int32_t to_offset = 0;
|
|
|
|
if (to_min < 0) {
|
|
|
|
to_offset = - to_min;
|
|
|
|
}
|
|
|
|
if (to_max < (- to_offset)) {
|
|
|
|
to_offset = - to_max;
|
|
|
|
}
|
|
|
|
|
|
|
|
return changeUIntScale(num + from_offset, from_min + from_offset, from_max + from_offset, to_min + to_offset, to_max + to_offset) - to_offset;
|
|
|
|
}
|
|
|
|
|
2020-05-07 18:48:43 +01:00
|
|
|
// Force a float value between two ranges, and adds or substract the range until we fit
|
|
|
|
float ModulusRangef(float f, float a, float b) {
|
|
|
|
if (b <= a) { return a; } // inconsistent, do what we can
|
|
|
|
float range = b - a;
|
|
|
|
float x = f - a; // now range of x should be 0..range
|
|
|
|
x = fmodf(x, range); // actual range is now -range..range
|
|
|
|
if (x < 0.0f) { x += range; } // actual range is now 0..range
|
|
|
|
return x + a; // returns range a..b
|
|
|
|
}
|
|
|
|
|
|
|
|
// Compute a n-degree polynomial for value x and an array of coefficient (by increasing order)
|
|
|
|
// Ex:
|
|
|
|
// For factors = { f0, f1, f2, f3 }
|
|
|
|
// Returns : f0 + f1 x + f2 x^2, + f3 x^3
|
|
|
|
// Internally computed as : f0 + x (f1 + x (f2 + x f3))
|
|
|
|
float Polynomialf(const float *factors, uint32_t degree, float x) {
|
|
|
|
float r = 0.0f;
|
|
|
|
for (uint32_t i = degree - 1; i >= 0; i--) {
|
|
|
|
r = r * x + factors[i];
|
|
|
|
}
|
|
|
|
return r;
|
|
|
|
}
|