Equation.h

Go to the documentation of this file.

/*
 * Copyright (c) Meta Platforms, Inc. and affiliates.
 *
 * This source code is licensed under the MIT license found in the
 * LICENSE file in the root directory of this source tree.
 */
 
#ifndef META_OCEAN_MATH_EQUATION_H
#define META_OCEAN_MATH_EQUATION_H
 
#include "ocean/math/Math.h"
#include "ocean/math/Numeric.h"
 
#include "ocean/base/Utilities.h"
 
#include <complex>
 
namespace Ocean
{
 
// Forward declaration.
template <typename T> class EquationT;
 
/**
 * Definition of the Equation object, depending on the OCEAN_MATH_USE_SINGLE_PRECISION either with single or double precision float data type.
 * @see EquationT
 * @ingroup math
 */
using Equation = EquationT<Scalar>;
 
/**
 * Definition of the Equation class using double values
 * @see EquationT
 * @ingroup math
 */
using EquationD = EquationT<double>;
 
/**
 * Definition of the Equation class using float values
 * @see EquationT
 * @ingroup math
 */
using EquationF = EquationT<float>;
 
/**
 * This class provides several functions to solve equations with different degree using floating point values with the precission specified by type T.
 * @tparam T Type of passed floating point values
 * @see Equation, EquationF, EquationD.
 * @ingroup math
 */
template <typename T>
class EquationT
{
    public:
 
        /**
         * Solves a linear eqation with the form:
         * <pre>
         * ax + b = 0
         * </pre>
         * @param a A parameter, with range (-infinity, infinity) \ {0}, (must not be 0)
         * @param b B parameter, with range (-infinity, infinity)
         * @param x Resulting solution
         * @return True, if succeeded
         */
        static bool solveLinear(const T a, const T b, T& x);
 
        /**
         * Solves an quadratic equation with the form:
         * <pre>
         * ax^2 + bx + c = 0
         * </pre>
         * @param a A parameter, with range (-infinity, infinity) \ {0}, (must not be 0)
         * @param b B parameter, with range (-infinity, infinity)
         * @param c C parameter, with range (-infinity, infinity)
         * @param x1 First resulting solution, with range (-infinity, infinity)
         * @param x2 Second resulting solution, with range (-infinity, infinity)
         * @return True, if succeeded
         */
        static bool solveQuadratic(const T a, const T b, const T c, T& x1, T& x2);
 
        /**
         * Solves a cubic equation with the from:
         * <pre>
         * ax^3 + bx^2 + cx + d = 0
         * </pre>
         * @param a A parameter, with range (-infinity, infinity) \ {0}, (must not be 0)
         * @param b B parameter, with range (-infinity, infinity)
         * @param c C parameter, with range (-infinity, infinity)
         * @param d D parameter, with range (-infinity, infinity)
         * @param x1 First resulting solution, with range (-infinity, infinity)
         * @param x2 Second resulting solution, with range (-infinity, infinity)
         * @param x3 Third resulting solution, with range (-infinity, infinity)
         * @param refine True, to refine the calculated solutions with an iterative algorithm; False, to use the roots as calculated
         * @return The number of solutions, with range [0, 3]
         */
        static unsigned int solveCubic(const T a, const T b, const T c, const T d, T& x1, T& x2, T& x3, const bool refine = true);
 
        /**
         * Solves a quartic equation with the form:
         * <pre>
         * ax^4 + bx^3 + cx^2 + dx + e = 0
         * </pre>
         * @param a A parameter, with range (-infinity, infinity) \ {0}, (must not be 0)
         * @param b B parameter, with range (-infinity, infinity)
         * @param c C parameter, with range (-infinity, infinity)
         * @param d D parameter, with range (-infinity, infinity)
         * @param e E parameter, with range (-infinity, infinity)
         * @param x Array with at least four scalar values receiving the (at most) four solutions
         * @param refine True, to refine the calculated solutions with an iterative algorithm; False, to use the roots as calculated
         * @return The number of solutions, with range [0, 4]
         */
        static unsigned int solveQuartic(const T a, const T b, const T c, const T d, const T e, T* x, const bool refine = true);
 
        /**
         * Optimizes a root of a cubic equation using Newton-Raphson iterations.
         * The function optimizes a known x for the following equation:
         * <pre>
         * ax^3 + bx^2 + cx + d = 0
         * </pre>
         * @param a A parameter, with range (-infinity, infinity) \ {0}, (must not be 0)
         * @param b B parameter, with range (-infinity, infinity)
         * @param c C parameter, with range (-infinity, infinity)
         * @param d D parameter, with range (-infinity, infinity)
         * @param x The known root to optimize, with range (-infinity, infinity)
         * @param maxIterations The maximum number of Newton-Raphson iterations, with range [1, infinity)
         * @param updateFactor The update factor for the Newton-Raphson iterations, with range (0, 2]
         * @return The number of applied iterations, with range [0, maxIterations], 0 if the root could not be optimized
         */
        static unsigned int optimizeCubic(const T a, const T b, const T c, const T d, T& x, const unsigned int maxIterations = 10u, const T updateFactor = T(1.0));
 
        /**
         * Optimizes a root of a quartic equation using Newton-Raphson iterations.
         * The function optimizes a known x for the following equation:
         * <pre>
         * ax^4 + bx^3 + cx^2 + dx + e = 0
         * </pre>
         * @param a A parameter, with range (-infinity, infinity) \ {0}, (must not be 0)
         * @param b B parameter, with range (-infinity, infinity)
         * @param c C parameter, with range (-infinity, infinity)
         * @param d D parameter, with range (-infinity, infinity)
         * @param e E parameter, with range (-infinity, infinity)
         * @param x The known root to optimize, with range (-infinity, infinity)
         * @param maxIterations The maximum number of Newton-Raphson iterations, with range [1, infinity)
         * @param updateFactor The update factor for the Newton-Raphson iterations, with range (0, 2]
         * @return The number of applied iterations, with range [0, maxIterations], 0 if the root could not be optimized
         */
        static unsigned int optimizeQuartic(const T a, const T b, const T c, const T d, const T e, T& x, const unsigned int maxIterations = 10u, const T updateFactor = T(1.0));
};
 
template <typename T>
bool EquationT<T>::solveLinear(const T a, const T b, T& x)
{
    ocean_assert(NumericT<T>::isNotEqualEps(a));
 
    // ax + b = 0
 
    if (NumericT<T>::isEqualEps(a))
    {
        return false;
    }
 
    x = -b / a;
    return true;
}
 
template <typename T>
bool EquationT<T>::solveQuadratic(const T a, const T b, const T c, T& x1, T& x2)
{
    ocean_assert(NumericT<T>::isNotEqualEps(a));
 
    // ax^2 + bx + c = 0
 
    // see Numerical Recipes in C++
    // x = (-b ± sqrt(b^2 - 4ac)) / 2a
 
    if (NumericT<T>::isEqualEps(a))
    {
        return false;
    }
 
    const T value = b * b - T(4) * a * c;
 
    if (value < T(0))
    {
        return false;
    }
 
    if (NumericT<T>::isEqualEps(value))
    {
        // x = (-b ± sqrt(0)) / 2a
 
        x1 = -b / (T(2) * a);
        x2 = x1;
 
        return true;
    }
 
    const T q = T(-0.5) * (b + NumericT<T>::copySign(NumericT<T>::sqrt(value), b));
 
    if (NumericT<T>::isEqualEps(q))
    {
        x1 = 0;
        x2 = 0;
        return true;
    }
 
    x1 = q / a;
    x2 = c / q;
 
#ifdef OCEAN_DEBUG
    if (std::is_same<T, float>::value)
    {
        // for 32 bit float values we have to weaken the zero accuracy
        ocean_assert(NumericT<T>::isEqual(a * x1 * x1 + b * x1 + c, T(0), NumericT<T>::weakEps() * NumericT<T>::abs(x1)));
        ocean_assert(NumericT<T>::isEqual(a * x2 * x2 + b * x2 + c, T(0), NumericT<T>::weakEps() * NumericT<T>::abs(x2)));
    }
    else
    {
        ocean_assert(NumericT<T>::isWeakEqualEps(a * x1 * x1 + b * x1 + c));
        ocean_assert(NumericT<T>::isWeakEqualEps(a * x2 * x2 + b * x2 + c));
    }
#endif
 
    return true;
}
 
template <typename T>
unsigned int EquationT<T>::solveCubic(const T a, const T b, const T c, const T d, T& x1, T& x2, T& x3, const bool refine)
{
    ocean_assert(NumericT<T>::isNotEqualEps(a));
 
    // ax^3 + bx^2 + cx + d = 0
    // see Numerical Recipes in C++
 
    if (NumericT<T>::isEqualEps(a))
    {
        return 0u;
    }
 
    const T a1 = 1 / a;
    const T alpha = b * a1;
    const T beta = c * a1;
    const T gamma = d * a1;
 
    // x^3 + alpha x^2 + beta x + gamma = 0
 
    // alpha2 = alpha^2
    const T alpha2 = alpha * alpha;
 
    // q = (alpha^2 - 3*beta) / 9
    const T q = (alpha2 - T(3) * beta) * T(0.11111111111111111111111111111111);
 
    // r = (2*alpha^3 - 9*alpha*beta + 27*gamma) / 54
    const T r = (T(2) * alpha2 * alpha - T(9) * alpha * beta + T(27) * gamma) * T(0.018518518518518518518518518518519);
 
    // r2 = r^2
    const T r2 = r * r;
 
    // q3 = q^3
    const T q3 = q * q * q;
 
    if (r2 <= q3 + NumericT<T>::eps() && q > NumericT<T>::eps())
    {
        const T sqrtQ = NumericT<T>::sqrt(q);
 
        // angle = arccos(r / sqrt(q^3))
        // angle_3 = angle / 3
        const T angle_3 = NumericT<T>::acos(minmax<T>(-1, r / (q * sqrtQ), 1)) * T(0.33333333333333333333333333333333);
 
        // alpha_3 = alpha / 3
        const T alpha_3 = alpha * T(0.33333333333333333333333333333333);
 
        const T factor = T(-2) * sqrtQ;
 
        // x1 = -2 sqrt(q) * cos(angle / 3) - alpha / 3
        x1 = factor * NumericT<T>::cos(angle_3) - alpha_3;
 
        // x2 = -2 sqrt(q) * cos((angle + 2pi) / 3) - alpha / 3
        x2 = factor * NumericT<T>::cos(angle_3 + T(2.0943951023931954923084289221863)) - alpha_3;
 
        // x3 = -2 sqrt(q) * cos((angle - 2pi) / 3) - alpha / 3
        x3 = factor * NumericT<T>::cos(angle_3 - T(2.0943951023931954923084289221863)) - alpha_3;
 
#ifdef OCEAN_DEBUG
 
        const T value1 = a * x1 * x1 * x1 + b * x1 * x1 + c * x1 + d;
        const T value2 = a * x2 * x2 * x2 + b * x2 * x2 + c * x2 + d;
        const T value3 = a * x3 * x3 * x3 + b * x3 * x3 + c * x3 + d;
 
        // the accuracy for 32 bit float values may be very poor so that we cannot define any assert
        if (!std::is_same<T, float>::value)
        {
            ocean_assert_accuracy(NumericT<T>::isEqual(value1, T(0), T(1e-3)));
            ocean_assert_accuracy(NumericT<T>::isEqual(value2, T(0), T(1e-3)));
            ocean_assert_accuracy(NumericT<T>::isEqual(value3, T(0), T(1e-3)));
        }
#endif
 
        if (refine)
        {
            optimizeCubic(a, b, c, d, x1);
            optimizeCubic(a, b, c, d, x2);
            optimizeCubic(a, b, c, d, x3);
        }
 
        return 3u;
    }
 
    ocean_assert(r2 - q3 >= -NumericT<T>::eps());
 
    // m = -sign(r) * [abs(r) + sqrt(r^2 - q^3)]^(1/3)
    const T m = -NumericT<T>::copySign(pow(NumericT<T>::abs(r) + NumericT<T>::sqrt(std::max(T(0), r2 - q3)), T(0.33333333333333333333333333333333)), r);
 
    // n = 0, if m == 0
    // n = q / m, if m != 0
    const T n = NumericT<T>::isEqualEps(m) ? 0 : q / m;
 
    // x1 = (m + n) - alpha / 3
    x1 = m + n - alpha * T(0.33333333333333333333333333333333);
 
#ifdef OCEAN_DEBUG
 
    const T value1 = a * x1 * x1 * x1 + b * x1 * x1 + c * x1 + d;
 
    // the accuracy for 32 bit float values may be very poor so that we cannot define any assert
    if (!std::is_same<T, float>::value)
    {
        ocean_assert(NumericT<T>::isEqual(value1, T(0), T(1e-3)));
    }
 
#endif
 
    if (refine)
    {
        optimizeCubic(a, b, c, d, x1);
    }
 
    return 1u;
}
 
template <typename T>
unsigned int EquationT<T>::solveQuartic(const T a, const T b, const T c, const T d, const T e, T* x, const bool refine)
{
    ocean_assert(NumericT<T>::isNotEqualEps(a));
    ocean_assert(x != nullptr);
 
    // ax^4 + bx^3 + cx^2 + dx + e = 0
 
    if (NumericT<T>::isEqualEps(a))
    {
        return 0u;
    }
 
    // simplification using substitution:
    /// y^4 + alpha * y^2 + beta * y + gamma = 0
 
    // 1 / a
    const T a1 = T(1.0) / a;
 
    // b / a
    const T b_a = b * a1;
 
    // c / a
    const T c_a = c * a1;
 
    // (b * b) / (a * a)
    const T b_a2 = b_a * b_a;
 
    // (b * b * b) / (a * a * a)
    const T b_a3 = b_a2 * b_a;
 
    // d / a
    const T d_a = d * a1;
 
    //const T alpha = T(-0.375) * (b / a) * (b / a) + c / a;
    const T alpha = T(-0.375) * b_a2 + c_a;
 
    //const T beta = T(0.125) * (b / a) * (b / a) * (b / a) - T(0.5) * (b / a) * (c / a) + (d / a);
    const T beta = T(0.125) * b_a3 - T(0.5) * b_a * c_a + d_a;
 
    //const T gamma = T(-0.01171875) * (b / a) * (b / a) * (b / a) * (b / a) + T(0.0625) * (b / a) * (b / a) * (c / a) - T(0.25) * (b / a) * (d / a) + e / a;
    const T gamma = T(-0.01171875) * b_a3 * b_a + T(0.0625) * b_a2 * c_a - T(0.25) * b_a * d_a + e * a1;
 
    // y^4 + alpha y^2 + beta y + gamma = 0
 
    unsigned int solutions = 0u;
 
    if (NumericT<T>::isEqualEps(beta))
    {
        const std::complex<T> cx1 = std::complex<T>(T(-0.25)) * std::complex<T>(b / a) + NumericT<T>::sqrt(std::complex<T>(T(0.5)) * (std::complex<T>(-alpha) + NumericT<T>::sqrt(std::complex<T>(alpha * alpha - 4 * gamma))));
        const std::complex<T> cx2 = std::complex<T>(T(-0.25)) * std::complex<T>(b / a) + NumericT<T>::sqrt(std::complex<T>(T(0.5)) * (std::complex<T>(-alpha) - NumericT<T>::sqrt(std::complex<T>(alpha * alpha - 4 * gamma))));
        const std::complex<T> cx3 = std::complex<T>(T(-0.25)) * std::complex<T>(b / a) - NumericT<T>::sqrt(std::complex<T>(T(0.5)) * (std::complex<T>(-alpha) + NumericT<T>::sqrt(std::complex<T>(alpha * alpha - 4 * gamma))));
        const std::complex<T> cx4 = std::complex<T>(T(-0.25)) * std::complex<T>(b / a) - NumericT<T>::sqrt(std::complex<T>(T(0.5)) * (std::complex<T>(-alpha) - NumericT<T>::sqrt(std::complex<T>(alpha * alpha - 4 * gamma))));
 
        if (NumericT<T>::isEqualEps(cx1.imag()))
        {
            x[solutions++] = cx1.real();
        }
 
        if (NumericT<T>::isEqualEps(cx2.imag()))
        {
            x[solutions++] = cx2.real();
        }
 
        if (NumericT<T>::isEqualEps(cx3.imag()))
        {
            x[solutions++] = cx3.real();
        }
 
        if (NumericT<T>::isEqualEps(cx4.imag()))
        {
            x[solutions++] = cx4.real();
        }
    }
    else
    {
        //const std::complex<T> p(-(alpha * alpha) / T(12.0) - gamma);
        const std::complex<T> p(T(-0.08333333333333333333333333333333) * alpha * alpha - gamma);
 
        //const std::complex<T> q(-(alpha * alpha * alpha) / T(108.0) + (alpha * gamma) / T(3.0) - (beta * beta) / T(8.0));
        const std::complex<T> q(T(-0.00925925925925925925925925925926) * alpha * alpha * alpha + T(0.33333333333333333333333333333333) * alpha * gamma - T(0.125) * beta * beta);
 
        const std::complex<T> qqSqr = NumericT<T>::sqrt(std::complex<T>(T(0.25)) * q * q + std::complex<T>(T(0.03703703703703703703703703703704)) * p * p * p);
 
        const std::complex<T> r(std::complex<T>(T(-0.5)) * q + qqSqr);
        if (NumericT<T>::isNan(r) || NumericT<T>::isInf(r))
        {
            return 0u;
        }
 
        const std::complex<T> u = NumericT<T>::pow(std::complex<T>(r), T(0.33333333333333333333333333333333));
 
        if (NumericT<T>::isNan(u) || NumericT<T>::isInf(u))
        {
            return 0u;
        }
 
        std::complex<T> y;
        if (NumericT<T>::isEqualEps(u.real()) && NumericT<T>::isEqualEps(u.imag()))
        {
            y = std::complex<T>(T(-0.83333333333333333333333333333333) * alpha) + u - NumericT<T>::pow(std::complex<T>(q), T(0.33333333333333333333333333333333));
        }
        else
        {
            y = std::complex<T>(T(-0.83333333333333333333333333333333) * alpha) + u - p / (std::complex<T>(3) * u);
        }
 
        const std::complex<T> w(NumericT<T>::sqrt(std::complex<T>(alpha) + std::complex<T>(T(2)) * y));
 
        //const std::complex<T> cx1 = std::complex<T>(T(-0.25)) * std::complex<T>(b / a) + std::complex<T>(T(0.5)) * (w + NumericT<T>::sqrt(std::complex<T>(-1) * (std::complex<T>(3) * std::complex<T>(alpha) + std::complex<T>(2) * y + std::complex<T>(2) * std::complex<T>(beta) / w)));
 
        if (NumericT<T>::isEqualEps(w))
        {
            return 0u;
        }
 
        const std::complex<T> beta2_w(std::complex<T>(T(2) * beta) / w);
        const std::complex<T> alpha3y2 = std::complex<T>(T(3) * alpha) + std::complex<T>(T(2)) * y;
        const std::complex<T> b_a4(T(-0.25) * b_a);
 
        const std::complex<T> sqrtPositive(NumericT<T>::sqrt(-alpha3y2 - beta2_w));
        const std::complex<T> sqrtNegative(NumericT<T>::sqrt(-alpha3y2 + beta2_w));
 
        const std::complex<T> cx1 = b_a4 + std::complex<T>(T(0.5)) * (w + sqrtPositive);
        const std::complex<T> cx2 = b_a4 + std::complex<T>(T(0.5)) * (w - sqrtPositive);
        const std::complex<T> cx3 = b_a4 + std::complex<T>(T(0.5)) * (-w + sqrtNegative);
        const std::complex<T> cx4 = b_a4 + std::complex<T>(T(0.5)) * (-w - sqrtNegative);
 
        if (NumericT<T>::isEqualEps(cx1.imag()))
        {
            x[solutions++] = cx1.real();
        }
 
        if (NumericT<T>::isEqualEps(cx2.imag()))
        {
            x[solutions++] = cx2.real();
        }
 
        if (NumericT<T>::isEqualEps(cx3.imag()))
        {
            x[solutions++] = cx3.real();
        }
 
        if (NumericT<T>::isEqualEps(cx4.imag()))
        {
            x[solutions++] = cx4.real();
        }
    }
 
    // first, we try to refine/optimize the solutions
 
    if (refine)
    {
        for (unsigned int n = 0u; n < solutions; ++n)
        {
            optimizeQuartic(a, b, c, d, e, x[n]);
        }
    }
 
    // finally, we remove all solutions which are not accurate enough
 
    unsigned int nOutput = 0u;
    for (unsigned int nInput = 0u; nInput < solutions; ++nInput)
    {
        const T value = x[nInput];
 
        if (NumericT<T>::isWeakEqualEps(value * value * value * value * a + value * value * value * b + value * value * c + value * d + e))
        {
            // the solution is accurate enough
 
            if (nOutput != nInput)
            {
                x[nOutput] = x[nInput];
            }
 
            ++nOutput;
        }
    }
 
    solutions = nOutput;
 
    return solutions;
}
 
template <typename T>
unsigned int EquationT<T>::optimizeCubic(const T a, const T b, const T c, const T d, T& root, const unsigned int maxIterations, const T updateFactor)
{
    ocean_assert(maxIterations >= 1u);
    ocean_assert(updateFactor > T(0) && updateFactor <= T(2));
 
    // Newton-Raphson iteration: xNew = xOld - f(xOld) / f'(xOld)
 
    // where f(x) = ax^3 + bx^2 + cx + d
    // and f'(x) = 3ax^2 + 2bx + c
 
    for (unsigned int nIteration = 0u; nIteration < maxIterations; ++nIteration)
    {
        const T x = root;
 
        const T fx = a * x * x * x + b * x * x + c * x + d;
 
        if (NumericT<T>::isEqualEps(fx))
        {
            return nIteration;
        }
 
        const T fdx = T(3) * a * x * x + T(2) * b * x + c;
 
        if (NumericT<T>::isEqualEps(fdx))
        {
            return nIteration;
        }
 
        const T delta = fx / fdx;
        const T newX = x - delta * updateFactor;
 
        const T newFx = a * newX * newX * newX + b * newX * newX + c * newX + d;
 
        if (NumericT<T>::abs(fx) < NumericT<T>::abs(newFx))
        {
            // the optimization failed
            return nIteration;
        }
 
        root = newX;
 
        // we have converged
        if (NumericT<T>::isEqualEps(delta))
        {
            return nIteration + 1u;
        }
    }
 
    return maxIterations;
}
 
template <typename T>
unsigned int EquationT<T>::optimizeQuartic(const T a, const T b, const T c, const T d, const T e, T& root, const unsigned int maxIterations, const T updateFactor)
{
    ocean_assert(maxIterations >= 1u);
    ocean_assert(updateFactor > T(0) && updateFactor <= T(2));
 
    // Newton-Raphson iteration: xNew = xOld - f(xOld) / f'(xOld)
 
    // where f(x) = ax^4 + bx^3 + cx^2 + dx + e
    // and f'(x) = 4ax^3 + 3bx^2 + 2cx + d
 
    for (unsigned int nIteration = 0u; nIteration < maxIterations; ++nIteration)
    {
        const T x = root;
        const T x2 = x * x;
        const T x3 = x2 * x;
        const T x4 = x3 * x;
 
        const T fx = a * x4 + b * x3 + c * x2 + d * x + e;
 
        if (NumericT<T>::isEqualEps(fx))
        {
            return nIteration;
        }
 
        const T fdx = T(4) * a * x3 + T(3) * b * x2 + T(2) * c * x + d;
 
        if (NumericT<T>::isEqualEps(fdx))
        {
            return nIteration;
        }
 
        const T delta = fx / fdx;
        const T newX = x - delta * updateFactor;
 
        const T newFx = a * newX * newX * newX * newX + b * newX * newX * newX + c * newX * newX + d * newX + e;
 
        if (NumericT<T>::abs(fx) < NumericT<T>::abs(newFx))
        {
            // the optimization failed
            return nIteration;
        }
 
        root = newX;
 
        // we have converged
        if (NumericT<T>::isEqualEps(delta))
        {
            return nIteration + 1u;
        }
    }
 
    return maxIterations;
}
 
}
 
#endif // META_OCEAN_MATH_EQUATION_H