QtOSG widget displays Bezier curve fitting to data points, based on Schneider's algorithm for curve fitting

main.cpp

#include <Windows.h>

#include <QtGlobal>
#include <QDebug>

#include <osg/Drawable>
#include <osg/Geode>
#include <osg/Geometry>
#include <osg/Group>
#include <osg/StateSet>
#include <osgViewer/Viewer>
#include <osg/LineWidth>
#include <osg/BlendFunc>

#include "OsgPathFitter.h"

const int OSG_WIDTH = 1280;
const int OSG_HEIGHT = 960;

osg::Vec2Array* createDataPoints()
{
    osg::ref_ptr<osg::Vec2Array> vertices = new osg::Vec2Array;

    vertices->push_back(osg::Vec2f(0, 0));
    vertices->push_back(osg::Vec2f(0.2, 0.2));
    vertices->push_back(osg::Vec2f(0.4, 0.1));
    vertices->push_back(osg::Vec2f(0.6, 0.4));
    vertices->push_back(osg::Vec2f(0.9, 0.2));
    vertices->push_back(osg::Vec2f(1.3, 0.5));
    vertices->push_back(osg::Vec2f(1.5, 0.6));
    vertices->push_back(osg::Vec2f(1.7, -1.0));
    vertices->push_back(osg::Vec2f(1.8, -1.2));

    return vertices.release();
}

osg::Vec2Array* drawCurves(osg::Vec2Array* curves, int samples = 11)
{
    osg::ref_ptr<osg::Vec2Array> sampled = new osg::Vec2Array;
    Q_ASSERT(curves->size() % 4 == 0);
    int nCurves = curves->size() / 4;

    auto delta = 1.f / float(samples);

    for (decltype(curves->size()) i=0; i<curves->size(); i=i+4){
        auto b0 = curves->at(i),
                b1 = curves->at(i+1),
                b2 = curves->at(i+2),
                b3 = curves->at(i+3);
        for (int j=0; j<samples; ++j){
            auto t = delta * float(j),
                    t2 = t*t,
                    one_minus_t = 1.f - t,
                    one_minus_t2 = one_minus_t * one_minus_t;

            auto Bt = b0 * one_minus_t2 * one_minus_t
                    + b1 * 3.f * t * one_minus_t2
                    + b2 * 3.f * t2 * one_minus_t
                    + b3 * t2 * t;

            sampled->push_back(Bt);
        }
    }
    Q_ASSERT(sampled->size() == samples*nCurves);
    return sampled.release();
}

osg::Node* createTestScene()
{
    osg::ref_ptr<osg::Vec2Array> path = createDataPoints();
    OsgPathFitter<osg::Vec2Array, osg::Vec2f, float> fitter(*path);
    float tolerance = 0.05f;
    osg::ref_ptr<osg::Vec2Array> curves = fitter.fit(tolerance);

    osg::ref_ptr<osg::Vec2Array> sampled = drawCurves(curves.get());
    qDebug() << "path=" << path->size();
    qDebug() << "curves=" << curves->size();
    qDebug() << "sampled=" << sampled->size();

    osg::Vec4Array* colors = new osg::Vec4Array;
    colors->push_back(osg::Vec4(1,0.3,0,1));
    osg::Vec4Array* colorsGT = new osg::Vec4Array;
    colorsGT->push_back(osg::Vec4(0,1,0,1));

    osg::ref_ptr<osg::Geometry> geom = new osg::Geometry;
    geom->addPrimitiveSet(new osg::DrawArrays(GL_LINE_STRIP, 0, sampled->size()));
    geom->setUseDisplayList(false);
    geom->setVertexArray(sampled.get());
    geom->setColorArray(colors, osg::Array::BIND_OVERALL);

    osg::ref_ptr<osg::Geometry> geomGT = new osg::Geometry;
    geomGT->addPrimitiveSet(new osg::DrawArrays(GL_LINE_STRIP, 0, path->size()));
    geomGT->setUseDisplayList(false);
    geomGT->setVertexArray(path.get());
    geomGT->setColorArray(colorsGT, osg::Array::BIND_OVERALL);

    osg::ref_ptr<osg::Geode> geode = new osg::Geode;
    geode->addDrawable(geom.get());
    geode->addDrawable(geomGT.get());

    osg::StateSet* state = geode->getOrCreateStateSet();
    state->setMode(GL_LIGHTING, osg::StateAttribute::OFF);
    state->setMode(GL_BLEND, osg::StateAttribute::ON);
    state->setMode(GL_LINE_SMOOTH, osg::StateAttribute::ON);
    osg::LineWidth* lw = new osg::LineWidth;
    lw->setWidth(10.f);
    state->setAttribute(lw, osg::StateAttribute::ON);
    osg::BlendFunc* blendfunc = new osg::BlendFunc();
    state->setAttributeAndModes(blendfunc, osg::StateAttribute::ON);

    return geode.release();
}

int main(int, char**)
{
    ::SetProcessDPIAware();

    osgViewer::Viewer viewer;

    osg::ref_ptr<osg::Group> root = new osg::Group();

    root->addChild(createTestScene());
    viewer.setSceneData(root.get());
    viewer.setUpViewInWindow(100,100,OSG_WIDTH, OSG_HEIGHT);
//    viewer.setUpViewOnSingleScreen(0);
    return viewer.run();
}

OsgPathFitter.cpp

#include "OsgPathFitter.h"

template <typename Container, typename Point2, typename Real>
OsgPathFitter<Container, Point2, Real>::OsgPathFitter(const osg::Vec2Array &path)
    : PathFitter<osg::Vec2Array, osg::Vec2f, float>(path)
{
}

template <typename Container, typename Point2, typename Real>
Container *OsgPathFitter<Container, Point2, Real>::fit(Real error)
{
    osg::ref_ptr<Container> curvesSet = new Container;
    auto length = this->getLength();

    if (length>0){
        if (length > 1){
            auto tan1 = this->curveAt(1) - this->curveAt(0);
            auto tan2 = this->curveAt(length-2) - this->curveAt(length-1);
            tan1.normalize();
            tan2.normalize();
            this->fitCubic(curvesSet.get(), error, 0, length-1,
                           tan1, // left tangent
                           tan2); // right tangent
        }
    }
    return curvesSet.release();
}

template class OsgPathFitter<osg::Vec2Array, osg::Vec2f, float>;

OsgPathFitter.h

#ifndef OSGPATHFITTER_H
#define OSGPATHFITTER_H

#include "PathFitter.h"

#include <osg/ref_ptr>
#include <osg/Array>

template <typename Container, typename Point2, typename Real>
class OsgPathFitter : public PathFitter<osg::Vec2Array, osg::Vec2f, float>
{
public:
    OsgPathFitter(const osg::Vec2Array &path);
    virtual Container* fit(Real error = 2.5);
};

#endif // OSGPATHFITTER_H

PathFitter-impl.cpp

#include "PathFitter.cpp"

#include <osg/Array>

/* define template instance that will be used in code
 * see more: https://isocpp.org/wiki/faq/templates#separate-template-fn-defn-from-decl
*/
template class PathFitter<osg::Vec2Array, osg::Vec2f, float>;

PathFitter.cpp

#include "PathFitter.h"

#include <math.h>
#include <algorithm>
#include <memory>

template <typename Container, typename Point2, typename Real>
PathFitter<Container, Point2, Real>::PathFitter(const Container &path)
    : m_dataPoints(0)
{
    /* copy points from path and filter out adjacent duplicates */
    Point2 prev = Point2(NAN,NAN);
    for (unsigned int i=0; i<path.size(); ++i){
        auto point = path.at(i);
        if (prev.isNaN() || prev != point)
        {
            m_dataPoints.push_back(point);
            prev = point;
        }
    }
}

template <typename Container, typename Point2, typename Real>
Point2 PathFitter<Container, Point2, Real>::curveAt(unsigned int index) const
{
    if (index >= m_dataPoints.size())
        return {NAN, NAN};
    return m_dataPoints.at(index);
}

template <typename Container, typename Point2, typename Real>
size_t PathFitter<Container, Point2, Real>::getLength() const
{
    return m_dataPoints.size();
}

template <typename Container, typename Point2, typename Real>
void PathFitter<Container, Point2, Real>::fitCubic(Container *curvesSet, Real error, int first, int last, const Point2 &tan1, const Point2 &tan2)
{
    /* 2 points case */
    if (last-first == 1){
        auto pt1 = m_dataPoints.at(first);
        auto pt2 = m_dataPoints.at(last);
        auto dist = getDistance(pt1, pt2) / 3.f;
        auto pta = pt1 + tan1 * dist;
        auto ptb = pt2 + tan2 * dist;

        Curve curve(4);
        curve[0] = pt1;
        curve[1] = pta;
        curve[2] = ptb;
        curve[3] = pt2;

        this->addCurve(curvesSet, curve);
        return;
    }

    /* parameterize points and attempt to fit the curve */
    auto uPrime = this->chordLengthParameterize(first, last);
    auto maxError = (std::max)(error, error*error);
    int split = -1;
    bool parametersInOrder =  true;

    /* 4 iterations */
    for (int i=0; i<=4; ++i){
        Curve curve = this->generateBezier(first, last, uPrime, tan1, tan2);

        int index;
        auto merr = this->findMaxError(first, last, curve, uPrime, index);
        if (merr < error && parametersInOrder){
            this->addCurve(curvesSet, curve);
            return;
        }

        split = index;
        if (merr >= maxError)
            break;

        parametersInOrder = this->reparameterize(first, last, uPrime, curve);
        maxError = merr;
    }

    /* fitting failed */
    auto tanCenter = m_dataPoints.at(split-1) - m_dataPoints.at(split+1);
    this->fitCubic(curvesSet, error, first, split, tan1, tanCenter);
    this->fitCubic(curvesSet, error, split, last, -tanCenter, tan2 );
}

template <typename Container, typename Point2, typename Real>
void PathFitter<Container, Point2, Real>::addCurve(Container *curvesSet, const Curve &curve)
{
    curvesSet->push_back( curve[0] );
    curvesSet->push_back( curve[1] );
    curvesSet->push_back( curve[2] );
    curvesSet->push_back( curve[3] );
}

template <typename Container, typename Point2, typename Real>
Real PathFitter<Container, Point2, Real>::getDistance(const Point2 &p1, const Point2 &p2)
{
    auto x = p1.x() - p2.x();
    auto y = p1.y() - p2.y();
    return sqrt(x*x + y*y);
}

template <typename Container, typename Point2, typename Real>
typename PathFitter<Container, Point2, Real>::Curve PathFitter<Container, Point2, Real>::generateBezier(int first, int last, const std::vector<Real> &uPrime, const Point2 &tan1, const Point2 &tan2)
{
    double epsilon = 1.0e-6;
    auto pt1 = m_dataPoints[first];
    auto pt2 = m_dataPoints[last];

    // C and X matrices
    Real C[2][2];
    Real X[2];

    for (int i=0, l=last-first+1; i<l; ++i){
        auto u = uPrime.at(i);
        auto t = 1-u,
                b = 3*u*t,
                b0 = t * t * t,
                b1 = b * t,
                b2 = b * u,
                b3 = u * u * u;
        auto a1 = tan1 * b1,
                a2 = tan2 * b2,
                tmp = m_dataPoints[first+i] - (pt1 * (b0 + b1)) - (pt2 * (b2 + b3)) ;
        C[0][0] += a1 * a2;
        C[0][1] += a1 * a2;
        C[1][0] = C[0][1];
        C[1][1] += a2 * a2;
        X[0] += a1 * tmp;
        X[1] += a2 * tmp;
    }

    /* determinant of C and X */
    auto detC0C1 = C[0][0] * C[1][1] - C[1][0] * C[0][1];
    Real alpha1, alpha2;
    if (std::fabs(detC0C1) > epsilon){
        /* Kramer's rule */
        auto detC0X = C[0][0] * X[1] - C[1][0] * X[0];
        auto detXC1 = X[0] * C[1][1] - X[1] * C[0][1];

        /* alpha values */
        alpha1 = detXC1 / detC0C1;
        alpha2 = detC0X / detC0C1;
    }
    else {
        /* matrix is under-determined, assume alpha1=alpha2 */
        auto c0 = C[0][0] + C[0][1];
        auto c1 = C[1][0] + C[1][1];
        if (std::fabs(c0) > epsilon)
            alpha1 = alpha2 = X[0] / c0;
        else if (std::fabs(c1)>epsilon)
            alpha1 = alpha2 = X[1] / c1;
        else
            alpha1 = alpha2 = 0;

    }

    auto segLength = this->getDistance(pt2, pt1);
    auto eps = epsilon * segLength;
    Point2 handle1, handle2;
    if (alpha1 < eps || alpha2 < eps)
        alpha1 = alpha2 = segLength / 3.f;
    else {
        auto line = pt2 - pt1;
        handle1 = tan1 * alpha1;
        handle2 = tan2 * alpha2;
        if (handle1 * line - handle2 * line > segLength * segLength ){
            alpha1 = alpha2 = segLength / 3.f;
            handle1 = handle2 = Point2(NAN, NAN);
        }
    }

    auto pta = handle1.isNaN()? pt1+tan1*alpha1 : pt1+handle1;
    auto ptb = handle2.isNaN() ? pt2 + tan2 * alpha2 : pt2 + handle2;

    return Curve{pt1, pta, ptb, pt2};
}

template <typename Container, typename Point2, typename Real>
bool PathFitter<Container, Point2, Real>::reparameterize(int first, int last, std::vector<Real> &u, const Curve &curve)
{
    for (int i=first; i<=last; ++i){
        u[i-first] = this->findRoot(curve, m_dataPoints[i], u[i-first]);
    }

    for (int i=1, l=u.size(); i<l; ++i ){
        if (u[i] <= u[i-1])
            return false;
    }
    return true;
}

template <typename Container, typename Point2, typename Real>
Real PathFitter<Container, Point2, Real>::findRoot(const Curve &curve, const Point2 &point, Real u)
{
    Curve curve1(3);
    Curve curve2(2);

    /* control vertices for Q' */
    for (int i=0; i<=2; ++i){
        curve1[i] = (curve[i+1] - curve[i]) * 3.f;
    }

    /* control vertices for Q'' */
    for (int i=0; i<=1; ++i){
        curve2[i] = (curve1[i+1] - curve1[i]) * 2.f;
    }

    /* compute Q(u), Q'(u) and Q''(u) */
    auto pt = this->evaluate(3, curve, u);
    auto pt1 = this->evaluate(2, curve1, u);
    auto pt2 = this->evaluate(1, curve2, u);
    auto diff = pt - point;
    auto df = pt1 * pt1 + diff * pt2;

    /* f(u) / f'(u) */
    if (std::fabs(df) < 1.0e-6)
        return u;

    /* u = u - f(u)/f'(u) */
    return u - (diff*pt1) / df;
}

template <typename Container, typename Point2, typename Real>
Point2 PathFitter<Container, Point2, Real>::evaluate(int degree, const Curve &curve, Real t)
{
    Curve tmp(curve);

    /* triangle computation */
    for (int i=1; i<=degree; ++i){
        for (int j=0; j<=degree-i; ++j){
            tmp[j] = tmp[j] * (1-t) + tmp[j+1] * t;
        }
    }
    return tmp[0];
}

template <typename Container, typename Point2, typename Real>
std::vector<Real> PathFitter<Container, Point2, Real>::chordLengthParameterize(int first, int last)
{
    std::vector<Real> u(last-first+1);

    for (int i=first+1; i<=last; ++i){
        u[i-first] = u[i-first-1] + this->getDistance( m_dataPoints[i], m_dataPoints[i-1]);
    }

    for (int i=1, m=last-first; i<=m; ++i ){
        u[i] /= u[m];
    }

    return u;
}

template <typename Container, typename Point2, typename Real>
Real PathFitter<Container, Point2, Real>::findMaxError(int first, int last, const Curve &curve, const std::vector<Real> &u, int &index)
{
    index = std::floor((last-first+1)/2);
    Real maxDist = 0;
    for (int i=first+1; i<last; ++i){
        auto P = this->evaluate(3, curve, u.at(i-first));
        auto v = P - m_dataPoints[i];
        auto dist = v.length2(); // squared distance
        if (dist >= maxDist){
            maxDist = dist;
            index = i;
        }
    }
    return maxDist;
}

PathFitter.h

#ifndef PATHFITTER_H
#define PATHFITTER_H

#include <osg/Array>

/*! \class PathFitter
 * \brief Fits a sequence of as few curves as possible through the path’s anchor points.
 * Based on the paper
 * "An Algorithm for Automatically Fitting Digitized Curves", Philip J. Schneider, Graphics Gems, Academic Press, 1990
*/

template <typename Container, typename Point2, typename Real>
class PathFitter
{
public:
    typedef std::vector<Point2> Curve; /*!< Curve is a vector of 2d points */

    /*! The constructor makes a copy of the path and stores it in m_dataPoints.
     * \param path is an stl- or like container which represents a vector of 2d points. The reason it is not explicetely defined
     * because the Container type might be user-defined. */
    PathFitter(const Container& path);

    /*! A method that performs the curve fitting, must be re-defined.
     * An example of how to re-define the method:
     * \code
     * std::unique_ptr<Container> curveset_ptr{new Container}; // or use smart pointer of your choice
     * auto tan1 = curveAt(1) - curveAt(0);
     * tan1.normalize();
     * auto tan2 = curveAt(getLength()-2) - curveAt(getLength()-1);
     * tan2.normalize();
     * fitCubic(curveset_ptr.get(), error, 0, getLength()-1, tan1, tan2);
     * return curveset_ptr.release();
     * \endcode
     * \param error is the allowed maximum error when fitting the curves through the m_dataPoints
     * \return An stl- or like container which represents a vector of 2d points.
*/
    virtual Container* fit(Real error = 2.5) = 0;

    /*! \return 2D point of the data point */
    Point2 curveAt(unsigned int index) const;

    /*! return The number of points to be fitted */
    size_t getLength() const;

protected:
    /* Fit a Bezier curve to a (sub-)set of digitized points */
    void fitCubic(Container* curves, Real error, int first, int last, const Point2& tan1, const Point2& tan2);

private:
    void addCurve(Container *curvesSet, const Curve& curve);

    Real getDistance(const Point2& p1, const Point2& p2);

    Curve generateBezier(int first, int last, const std::vector<Real>& uPrime, const Point2& tan1, const Point2& tan2);

    bool reparameterize(int first, int last, std::vector<Real> &u, const Curve& curve);
    Real findRoot(const Curve& curve, const Point2& point, Real u);
    Point2 evaluate(int degree, const Curve& curve, Real t);
    std::vector<Real> chordLengthParameterize(int first, int last);
    Real findMaxError(int first, int last, const Curve& curve, const std::vector<Real> &u, int& index);

private:
    Curve m_dataPoints;
};

#endif // PATHFITTER_H