cubic.h

Go to the documentation of this file.

// Copyright 2019, Philipp Zabel
// Copyright 2026, Beyley Cardellio
// SPDX-License-Identifier: BSL-1.0
/*!
 * @file
 * @brief  Cubic root finder, specialized for P3P solving.
 * @author Philipp Zabel <philipp.zabel@gmail.com>
 * @author Beyley Cardellio <ep1cm1n10n123@gmail.com>
 * @ingroup xrt_iface
 */
 
#pragma once
 
#include <math.h>
 
 
/* Tuning variable for maximum loops and stopping
 * criteria */
#ifndef CUBIC_MAX_ITERS
#define CUBIC_MAX_ITERS 100
#endif
#ifndef CUBIC_ROOT_EPSILON
#define CUBIC_ROOT_EPSILON 1e-10
#endif
 
/*
 * Given a cubic equation with coefficients b,c,d:
 *   f(x) = x^3 + bx^2 + cx + d
 *
 * return one root of the equation, preferably the
 * one with the highest derivative. There is always
 * at least 1 real root for such a cubic.
 *
 * We do that by choosing a starting point and using
 * the Newton-Raphson method to find the root.
 *
 * The derivative of the cubic equation is:
 *   f'(x) = 3x^2 + 2bx + c
 *
 * Using the quadratic formula on the derivative, we can find
 * the critical points:
 * x = (-2*b ± sqrt(4*b^2 - 4*3*c) / 2*3
 * x = (-b ± sqrt(b^2 - 3*c) / 3.0
 *
 * From the sqrt part, if b^2 < 3*c, there are no real critical
 * points. If b^2 == 3*c, there is 1 critical point @ -b/3.0.
 * In both these cases, the cubic is strictly monotonic, with only 1 root.
 *
 * Othwise, there are up to 3 roots, and we want the one
 * with the larger derivative, so we choose a starting point
 * as explained below.
 *
 * It's also worth remembering that given
 * roots x1, x2, x3 then x1 + x2 + x3 = -b,
 * which is why it is good to start at x = -b/3.0 for the
 * single-root case.
 *
 * cubic b=0.000144 c=-2.460081 d=1.747825 -> r0 = 0.961102 f(r0) = 0.271356 (FAIL)
 *
 * t1 = 2.0736e-08
 * t2 = -7.380243
 */
static inline double
reduced_cubic_real_root(double b, double c, double d)
{
        double t1 = b * b;
        double t2 = 3.0 * c;
        double t3 = -b / 3.0;
        double r0;
 
        if (t1 <= t2) {
                r0 = t3;
        } else {
                /* We need to decide which root to iterate toward,
                 * by starting to the left of the left critical point,
                 * the right of the right critical point, or in between
                 * the 2.
                 *
                 * We know the cubic is increasing, because coefficient a = 1,
                 * so the left critical point (cl) is a maxima, and the right (cr) is a minima.
                 *
                 * If f(cl) > 0, go left from there, else go right from cr.
                 */
                double t4 = sqrt(t1 - t2) / 3.0;
                double cl = t3 - t4;
                double fx = cl * (cl * (cl + b) + c) + d;
                if (fx > 0) {
                        r0 = cl - t4 / 4.0;
                } else {
                        double cr = t3 + t4;
                        r0 = cr + t4 / 4.0;
                }
        }
 
        /* Newton-Raphson - r0 = r0 - f(r0)/f'(r0) */
        int i = CUBIC_MAX_ITERS;
        do {
                double fx = r0 * (r0 * (r0 + b) + c) + d;
                double dx = fx / (r0 * (3 * r0 + 2 * b) + c);
 
                /* If we converged, exit or if the delta gets really small, break out too,
                 * because we're not moving any closer */
                if (fabs(fx) < CUBIC_ROOT_EPSILON || fabs(dx) < CUBIC_ROOT_EPSILON) {
                        return r0;
                }
 
                r0 -= dx;
        } while (i-- > 0);
 
        return r0;
}