poly34f.cpp

// poly34f.cpp : solution of cubic and quartic equations (single-precision port)
//
// Original double-precision implementation:
//   (c) Sergei I. Khashin, http://math.ivanovo.ac.ru/dalgebra/Khashin/poly/
//   khash2 (at) gmail.com  -- released by the author into the public domain.
//   Thanks to Alexandr Rakhmanin, John Fairman, Sebastien Berthet (per the
//   original source).
//
// Single-precision (float) port for execution on a single-precision DSP
// target, used as reference [7] of:
//   A. Glac, V. Smidl, Z. Peroutka, "Optimal Feedforward Torque Control of
//   Synchronous Machines with Time-Varying Parameters," IECON 2018.
//
// Changes vs. the original: all `double` replaced by `float`; the working
// tolerance `eps` is raised to a float-appropriate value; `CSqrt` is renamed
// `CxxSqrt`. The algorithms (Vieta for cubics, Descartes-Euler for quartics,
// Newton polishing) are unchanged.

#include <math.h>
#include "poly34f.h"

#define TwoPi 6.28318530717958648f
static const float eps = 1.0e-6f;   // float-appropriate tolerance

//=============================================================================
// real cube root
//=============================================================================
static float _root3(float x) {
    float s = 1.0f;
    while (x < 1.0f) { x *= 8.0f; s *= 0.5f; }
    while (x > 8.0f) { x *= 0.125f; s *= 2.0f; }
    float r = 1.5f;
    r -= (1.0f / 3.0f) * (r - x / (r * r));
    r -= (1.0f / 3.0f) * (r - x / (r * r));
    r -= (1.0f / 3.0f) * (r - x / (r * r));
    r -= (1.0f / 3.0f) * (r - x / (r * r));
    r -= (1.0f / 3.0f) * (r - x / (r * r));
    r -= (1.0f / 3.0f) * (r - x / (r * r));
    return r * s;
}

static float root3(float x) {
    // Guard against non-finite input. _root3 scales the argument into [1,8]
    // with `while (x > 8) x *= 0.125`, which never terminates for +Inf (and
    // `while (x < 1) x *= 8` never terminates for a flushed-to-zero value).
    // In single precision an extreme/degenerate operating point can overflow
    // the quartic coefficients to +/-Inf, so this guard is required to avoid
    // an infinite loop (the original double-precision code rarely hit it).
    if (!isfinite(x)) return x;
    if (x > 0) return _root3(x);
    if (x < 0) return -_root3(-x);
    return 0.0f;
}

int SolveP2(float *x, float a, float b) {            // x^2 + a*x + b = 0
    float D = 0.25f * a * a - b;
    if (D >= 0) {
        D = sqrtf(D);
        x[0] = -0.5f * a + D;
        x[1] = -0.5f * a - D;
        return 2;
    }
    x[0] = -0.5f * a;
    x[1] = sqrtf(-D);
    return 0;
}

int SolveP3(float *x, float a, float b, float c) {   // x^3 + a*x^2 + b*x + c = 0
    float a2 = a * a;
    float q = (a2 - 3 * b) / 9;
    float r = (a * (2 * a2 - 9 * b) + 27 * c) / 54;
    if (fabsf(q) < eps) {                 // y^3 = -r/2
        if (fabsf(r) < eps) {             // three identical roots
            x[0] = x[1] = x[2] = -a / 3;
            return 3;
        }
        x[0] = root3(-2 * r);
        x[1] = x[0] * 0.5f;
        x[2] = x[0] * sqrtf(3.0f) / 2;
        return 1;
    }
    float r2 = r * r;
    float q3 = q * q * q;
    float A, B;
    if (r2 <= (q3 + eps)) {
        float t = r / sqrtf(q3 + eps);
        if (t < -1) t = -1;
        if (t > 1) t = 1;
        t = acosf(t);
        a /= 3;
        q = -2 * sqrtf(q);
        x[0] = q * cosf(t / 3) - a;
        x[1] = q * cosf((t + TwoPi) / 3) - a;
        x[2] = q * cosf((t - TwoPi) / 3) - a;
        return 3;
    } else {
        A = -root3(fabsf(r) + sqrtf(r2 - q3));
        if (r < 0) A = -A;
        B = (A == 0) ? 0 : q / A;
        a /= 3;
        x[0] = (A + B) - a;
        x[1] = -0.5f * (A + B) - a;
        x[2] = 0.5f * sqrtf(3.0f) * (A - B);
        if (fabsf(x[2]) < eps) { x[2] = x[1]; return 2; }
        return 1;
    }
}

void CxxSqrt(float x, float y, float &a, float &b) {  // a + i*b = sqrt(x + i*y)
    float r = sqrtf(x * x + y * y);
    if (y == 0) {
        r = sqrtf(r);
        if (x >= 0) { a = r; b = 0; } else { a = 0; b = r; }
    } else {
        a = sqrtf(0.5f * (x + r));
        b = 0.5f * y / a;
    }
}

int SolveP4Bi(float *x, float b, float d) {           // x^4 + b*x^2 + d = 0
    float D = b * b - 4 * d;
    if (D >= 0) {
        float sD = sqrtf(D);
        float x1 = (-b + sD) / 2;
        float x2 = (-b - sD) / 2;
        if (x2 >= 0) {                    // 4 real roots
            float sx1 = sqrtf(x1);
            float sx2 = sqrtf(x2);
            x[0] = -sx1; x[1] = sx1; x[2] = -sx2; x[3] = sx2;
            return 4;
        }
        if (x1 < 0) {                     // two pairs of imaginary roots
            float sx1 = sqrtf(-x1);
            float sx2 = sqrtf(-x2);
            x[0] = 0; x[1] = sx1; x[2] = 0; x[3] = sx2;
            return 0;
        }
        // x2 < 0 <= x1
        float sx1 = sqrtf(x1);
        float sx2 = sqrtf(-x2);
        x[0] = -sx1; x[1] = sx1; x[2] = 0; x[3] = sx2;
        return 2;
    } else {
        float sD2 = 0.5f * sqrtf(-D);
        CxxSqrt(-0.5f * b, sD2, x[0], x[1]);
        CxxSqrt(-0.5f * b, -sD2, x[2], x[3]);
        return 0;
    }
}

#define SWAPF(a, b) { t = b; b = a; a = t; }
static void fltSort3(float &a, float &b, float &c) {  // make a <= b <= c
    float t;
    if (a > b) SWAPF(a, b);
    if (c < b) {
        SWAPF(b, c);
        if (a > b) SWAPF(a, b);
    }
}

int SolveP4De(float *x, float b, float c, float d) {  // x^4 + b*x^2 + c*x + d = 0
    if (fabsf(c) < 1e-6f * (fabsf(b) + fabsf(d))) return SolveP4Bi(x, b, d);

    int res3 = SolveP3(x, 2 * b, b * b - 4 * d, -c * c);  // resolvent
    if (res3 > 1) {                                       // 3 real roots
        fltSort3(x[0], x[1], x[2]);
        if (x[0] > 0) {                                   // all positive
            float sz1 = sqrtf(x[0]);
            float sz2 = sqrtf(x[1]);
            float sz3 = sqrtf(x[2]);
            if (c > 0) {
                x[0] = (-sz1 - sz2 - sz3) / 2;
                x[1] = (-sz1 + sz2 + sz3) / 2;
                x[2] = (+sz1 - sz2 + sz3) / 2;
                x[3] = (+sz1 + sz2 - sz3) / 2;
                return 4;
            }
            x[0] = (-sz1 - sz2 + sz3) / 2;
            x[1] = (-sz1 + sz2 - sz3) / 2;
            x[2] = (+sz1 - sz2 - sz3) / 2;
            x[3] = (+sz1 + sz2 + sz3) / 2;
            return 4;
        }
        // x[0] <= x[1] < 0 < x[2] : two pairs of complex roots
        float sz1 = sqrtf(-x[0]);
        float sz2 = sqrtf(-x[1]);
        float sz3 = sqrtf(x[2]);
        if (c > 0) {
            x[0] = -sz3 / 2;
            x[1] = (sz1 - sz2) / 2;
            x[2] = sz3 / 2;
            x[3] = (-sz1 - sz2) / 2;
            return 0;
        }
        x[0] = sz3 / 2;
        x[1] = (-sz1 + sz2) / 2;
        x[2] = -sz3 / 2;
        x[3] = (sz1 + sz2) / 2;
        return 0;
    }
    // 1 real and a pair of complex resolvent roots
    if (x[0] < 0) x[0] = 0;
    float sz1 = sqrtf(x[0]);
    float szr, szi;
    CxxSqrt(x[1], x[2], szr, szi);
    if (c > 0) {
        x[0] = -sz1 / 2 - szr;
        x[1] = -sz1 / 2 + szr;
        x[2] = sz1 / 2;
        x[3] = szi;
        return 2;
    }
    x[0] = sz1 / 2 - szr;
    x[1] = sz1 / 2 + szr;
    x[2] = -sz1 / 2;
    x[3] = szi;
    return 2;
}

float N4Step(float x, float a, float b, float c, float d) {  // one Newton step
    float fxs = ((4 * x + 3 * a) * x + 2 * b) * x + c;       // f'(x)
    if (fxs == 0) return x;
    float fx = (((x + a) * x + b) * x + c) * x + d;          // f(x)
    return x - fx / fxs;
}

int SolveP4(float *x, float a, float b, float c, float d) {  // x^4 + a x^3 + b x^2 + c x + d
    // depress to a = 0
    float d1 = d + 0.25f * a * (0.25f * b * a - 3.0f / 64 * a * a * a - c);
    float c1 = c + 0.5f * a * (0.25f * a * a - b);
    float b1 = b - 0.375f * a * a;
    int res = SolveP4De(x, b1, c1, d1);
    if (res == 4) { x[0] -= a / 4; x[1] -= a / 4; x[2] -= a / 4; x[3] -= a / 4; }
    else if (res == 2) { x[0] -= a / 4; x[1] -= a / 4; x[2] -= a / 4; }
    else { x[0] -= a / 4; x[2] -= a / 4; }
    if (res > 0) {
        x[0] = N4Step(x[0], a, b, c, d);
        x[1] = N4Step(x[1], a, b, c, d);
    }
    if (res > 2) {
        x[2] = N4Step(x[2], a, b, c, d);
        x[3] = N4Step(x[3], a, b, c, d);
    }
    return res;
}

#define F5(t) (((((t + a) * t + b) * t + c) * t + d) * t + e)

float SolveP5_1(float a, float b, float c, float d, float e) {
    int cnt;
    if (fabsf(e) < eps) return 0;

    float brd = fabsf(a);
    if (fabsf(b) > brd) brd = fabsf(b);
    if (fabsf(c) > brd) brd = fabsf(c);
    if (fabsf(d) > brd) brd = fabsf(d);
    if (fabsf(e) > brd) brd = fabsf(e);
    brd++;

    float x0, f0, x1, f1, x2, f2, f2s, dx = 0;
    if (e < 0) { x0 = 0; x1 = brd; f0 = e; f1 = F5(x1); x2 = 0.01f * brd; }
    else { x0 = -brd; x1 = 0; f0 = F5(x0); f1 = e; x2 = -0.01f * brd; }

    if (fabsf(f0) < eps) return x0;
    if (fabsf(f1) < eps) return x1;

    for (cnt = 0; cnt < 10; cnt++) {
        x2 = (x0 + x1) / 2;
        f2 = F5(x2);
        if (fabsf(f2) < eps) return x2;
        if (f2 > 0) { x1 = x2; f1 = f2; }
        else { x0 = x2; f0 = f2; }
    }

    do {
        if (cnt++ > 50) break;
        if (x2 <= x0 || x2 >= x1) x2 = (x0 + x1) / 2;
        f2 = F5(x2);
        if (fabsf(f2) < eps) return x2;
        if (f2 > 0) { x1 = x2; f1 = f2; }
        else { x0 = x2; f0 = f2; }
        f2s = (((5 * x2 + 4 * a) * x2 + 3 * b) * x2 + 2 * c) * x2 + d;
        if (fabsf(f2s) < eps) { x2 = 1e30f; continue; }
        dx = f2 / f2s;
        x2 -= dx;
    } while (fabsf(dx) > eps);
    return x2;
}

int SolveP5(float *x, float a, float b, float c, float d, float e) {
    float r = x[0] = SolveP5_1(a, b, c, d, e);
    float a1 = a + r, b1 = b + r * a1, c1 = c + r * b1, d1 = d + r * c1;
    return 1 + SolveP4(x + 1, a1, b1, c1, d1);
}