Centripetal Catmull-Rom curves

CentripetalCatmullRom.cpp

#include <cmath>

#include <iostream>
#include <utility>

#include <SFML/Graphics.hpp>

// https://stackoverflow.com/questions/9489736/catmull-rom-curve-with-no-cusps-and-no-self-intersections/23980479#23980479

class CentripetalCatmullRom {
    struct CubicPolynomial {
        struct Vector4f {
            Vector4f ( const float x0_, const float x1_, const float x2_, const float x3_ ) noexcept : x0 ( x0_ ), x1 ( x1_ ), x2 ( x2_ ), x3 ( x3_ ) { }
            float x0, x1, x2, x3;
        };
        float c0, c1, c2, c3;
        CubicPolynomial ( const float x0, const float x1, const float t0, const float t1 ) noexcept :
            c0 { x0 },
            c1 { t0 },
            c2 { -3.0f * x0 + 3.0f * x1 - 2.0f * t0 - t1 },
            c3 { +2.0f * x0 - 2.0f * x1 + 1.0f * t0 + t1 } {
        }
        CubicPolynomial ( const Vector4f & x, const sf::Vector3f & dt ) noexcept :
            CubicPolynomial { x.x1, x.x2, ( ( x.x1 - x.x0 ) / dt.x - ( x.x2 - x.x0 ) / ( dt.x + dt.y ) + ( x.x2 - x.x1 ) / dt.y ) * dt.y, ( ( x.x2 - x.x1 ) / dt.y - ( x.x3 - x.x1 ) / ( dt.y + dt.z ) + ( x.x3 - x.x2 ) / dt.z ) * dt.y } { }
        float evaluate ( const float t ) noexcept {
            const float t2 = t * t;
            const float t3 = t2 * t;
            return c0 + c1 * t + c2 * t2 + c3 * t3;
        }
    };
    CubicPolynomial x, y;
    CentripetalCatmullRom ( CubicPolynomial && x_, CubicPolynomial && y_ ) noexcept :
        x { std::forward<CubicPolynomial> ( x_ ) },
        y { std::forward<CubicPolynomial> ( y_ ) } {
    }
    static float distanceSquared ( const sf::Vector2f & p, const sf::Vector2f & q ) noexcept {
        const float dx = q.x - p.x;
        const float dy = q.y - p.y;
        return dx * dx + dy * dy;
    }
    public:
    static CentripetalCatmullRom make ( const sf::Vector2f & p0, const sf::Vector2f & p1, const sf::Vector2f & p2, const sf::Vector2f & p3 ) noexcept {
        sf::Vector3f dt { std::powf ( distanceSquared ( p0, p1 ), 0.25f ), std::powf ( distanceSquared ( p1, p2 ), 0.25f ), std::powf ( distanceSquared ( p2, p3 ), 0.25f ) };
        // Safety check for repeated points.
        if ( dt.y < 1e-4f ) { dt.y = 1.0f; }
        if ( dt.x < 1e-4f ) { dt.x = dt.y; }
        if ( dt.z < 1e-4f ) { dt.z = dt.y; }
        return { { { p0.x, p1.x, p2.x, p3.x }, dt }, { { p0.y, p1.y, p2.y, p3.y }, dt } };
    }
    sf::Vector2f evaluate ( const float t ) noexcept {
        return { x.evaluate ( t ), y.evaluate ( t ) };
    }
};


int main ( ) {
    sf::Vector2f p0 { 0, 0 }, p1 { 1, 1 }, p2 { 1.1, 1 }, p3 { 2, 0 };
    CentripetalCatmullRom p { CentripetalCatmullRom::make ( p0, p1, p2, p3 ) };
    for ( int i = 0; i <= 10; ++i ) {
        const auto v = p.evaluate ( 0.1f * i );
        std::cout << v.x << " " << v.y << std::endl;
    }
    return 0;
}