@woosh/meep-engine/src/core/math/spline/spline3_hermite_nearest_point.js

Pure JavaScript game engine. Fully featured and production ready.

import { spline3_hermite } from "./spline3_hermite.js";

/**
 * Helper to evaluate a polynomial given its coefficients.
 * @param {Float32Array} p - Polynomial coefficients [c0, c1, c2, ...]
 * @param {number} t - The value at which to evaluate.
 * @param {number} size
 * @returns {number}
 */
function polyval(p, t, size) {
    let result = 0;

    for (let i = size - 1; i >= 0; i--) {
        result = result * t + p[i];
    }

    return result;
}

const quintic_coeffs = new Float32Array(6);
const quartic_coeffs = new Float32Array(5);

const NEWTON_STEPS = 3;
const START_ROOT_COUNT = 5;

/**
 * Finds the parameter 't' of the nearest point on a 1D Hermite spline
 * function graph to a reference point. The curve is defined by (t, p(t)).
 * This version is optimized to avoid allocations.
 *
 * @param {number} ref_t - The 't' coordinate of the reference point.
 * @param {number} ref_p - The 'p' coordinate of the reference point.
 * @param {number} p0 - The start value of the spline (at t=0).
 * @param {number} p1 - The end value of the spline (at t=1).
 * @param {number} m0 - The tangent at the start.
 * @param {number} m1 - The tangent at the end.
 * @returns {number} The parameter 't' of the nearest point. This value is always in [0..1] range
 */
export function spline3_hermite_nearest_point(
    ref_t, ref_p,
    p0, p1, m0, m1
) {

    // 1. Calculate polynomial coefficients A, B, C for p(t)
    const A = 2 * p0 - 2 * p1 + m0 + m1;
    const B = -3 * p0 + 3 * p1 - 2 * m0 - m1;
    const C = m0;
    const D_minus_Rp = p0 - ref_p;

    // 2. Form the quintic polynomial f(t) = c5*t^5 + ... + c0

    quintic_coeffs[0] = (C * D_minus_Rp - ref_t)
    quintic_coeffs[1] = (C * C + 2 * B * D_minus_Rp + 1)
    quintic_coeffs[2] = (3 * B * C + 3 * A * D_minus_Rp)
    quintic_coeffs[3] = (4 * A * C + 2 * B * B)
    quintic_coeffs[4] = (5 * A * B)
    quintic_coeffs[5] = (3 * A * A)

    // Derivative of the quintic (a quartic) for Newton's method

    quartic_coeffs[0] = quintic_coeffs[1]
    quartic_coeffs[1] = 2 * quintic_coeffs[2]
    quartic_coeffs[2] = 3 * quintic_coeffs[3]
    quartic_coeffs[3] = 4 * quintic_coeffs[4]
    quartic_coeffs[4] = 5 * quintic_coeffs[5]


    // 3. Initialize by checking the endpoints t=0 and t=1
    let best_t = 0;
    let dt = 0 - ref_t;
    let dp = p0 - ref_p;
    let min_dist_sq = dt * dt + dp * dp;

    dt = 1 - ref_t;
    dp = p1 - ref_p;
    const dist_sq_at_1 = dt * dt + dp * dp;
    if (dist_sq_at_1 < min_dist_sq) {
        min_dist_sq = dist_sq_at_1;
        best_t = 1;
    }

    // 4. Find roots and check them on the fly
    const tolerance = 1e-7;


    // Start Newton's method from several points to find multiple roots
    for (let i = 0; i <= START_ROOT_COUNT; i++) {
        let t = i / START_ROOT_COUNT; // Starting guess for the root

        for (let j = NEWTON_STEPS; j > 0; j--) {

            const f_t = polyval(quintic_coeffs, t, 6);

            // derivative of the polynomial we are finding the root of
            const fp_t = polyval(quartic_coeffs, t, 5);

            if (Math.abs(fp_t) < tolerance) {
                break;
            }

            const t_new = t - f_t / fp_t;

            if (Math.abs(t_new - t) < tolerance) {
                break;
            }

            t = t_new;
        }

        // Check if the found root is valid and yields a better distance
        if (t > 0 && t < 1) {
            // Endpoints already checked
            const p = spline3_hermite(t, p0, p1, m0, m1);
            const current_dt = t - ref_t;
            const current_dp = p - ref_p;
            const dist_sq = current_dt * current_dt + current_dp * current_dp;

            if (dist_sq < min_dist_sq) {
                min_dist_sq = dist_sq;
                best_t = t;
            }
        }
    }

    return best_t;
}