fit.cpp

Go to the documentation of this file.

/*
 * fit.cpp
 *
 * Copyright (C) 2007  Thomas A. Vaughan
 * All rights reserved.
 *
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the <organization> nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THOMAS A. VAUGHAN ''AS IS'' AND ANY
 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THOMAS A. VAUGHAN BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *
 * Implementation of fitting structure fit_t.  See fit.h
 */

// includes --------------------------------------------------------------------
#include "fit.h"                // always include our own header first!

#include <math.h>

#include "perf/perf.h"


namespace bezier {

typedef std::vector<point_t> vec_point_t;

////////////////////////////////////////////////////////////////////////////////
//
//      quadratic fitting
//
////////////////////////////////////////////////////////////////////////////////

static point_t
getTangent
(
IN const point_t * p,
IN int nPoints,
IN int i
)
throw()
{
        ASSERT(p, "null");
        ASSERT(nPoints > 1, "Bad array size: %d", nPoints);
        ASSERT(i >= 0 && i < nPoints, "Bad index: %d", i);

        // edge cases
        if (0 == i) {
                return p[1] - p[0];
        } else if (nPoints - 1 == i) {
                return p[nPoints - 1] - p[nPoints - 2];
        }

        // use average
        point_t delta = p[i + 1] - p[i - 1];
        return point_t(0.5 * delta.x, 0.5 * delta.y);
}



void
getQuadPathFromRawPoints
(
IN const point_t * in_p,
IN int nPoints,
IN float ds,
OUT quad_path_t& path
)
{
        perf::Timer timer("bezier::getQuadPathsFromRawPoints");
        ASSERT(in_p, "null");
        ASSERT(nPoints > 0, "Bad nPoints: %d", nPoints);
        ASSERT(ds > 0.0, "Bad line step increment: %f cm", ds);
        path.clear();

        if (nPoints < 2) {
                // DPRINTF("Not enough points!  Ignoring...");
                return;
        }

        // respace
/*
        point_t * q = NULL;
        int newPoints = 0;
        respacePath(in_p, nPoints, ds, &q, newPoints);
        ASSERT(q, "null respaced points");
        ASSERT(newPoints > 1, "Should have at least 2 points: %d", newPoints);
*/
        const point_t * q = in_p;
        int newPoints = nPoints;

        // find any cusps
        int * cusps = NULL;
        int nCusps = getCuspIndices(q, newPoints, &cusps);
        ASSERT(cusps, "should have cusp array, even if empty");
        ASSERT(nCusps >= 1, "should have at least 1 cusp (end): %d", nCusps);
        // DPRINTF("nCusps = %d", nCusps);

        // now walk through set of respaced points and calculate quadratic fits
        int start = 0;
        int end;
        for (int c = 0; c < nCusps; ++c) {
                end = cusps[c];
                ASSERT(end > start && end < newPoints, "Bad end: %d", end);
                // DPRINTF("Segment from %d to %d", start, end);

                // starting tangent?
                point_t p0 = q[start];
                for (int i = start + 1; i <= end; ++i) {
                        // DPRINTF("  point %d...", i);
                        point_t p1 = q[i];
                        point_t t0 = getTangent(q, newPoints, i - 1);
                        quad_bezier_t qb;
                        qb.setFromPointsAndSlope(p0, p1, t0);
                        path.push_back(qb);

                        // set up for next quadratic
                        p0 = p1;
                }

                // next set
                start = end;
        }

        // DPRINTF("Finished quad fitting, %d total beziers", path.size());
}



void
getCubicPathFromRawPoints
(
IN const point_t * in_p,
IN int nPoints,
IN float ds,
OUT cubic_path_t& path
)
{
        perf::Timer timer("bezier::getCubicPathsFromRawPoints");
        ASSERT(in_p, "null");
        ASSERT(nPoints > 1, "Bad nPoints: %d", nPoints);
        ASSERT(ds > 0.0, "Bad line step increment: %f cm", ds);
        path.clear();

        // respace
        point_t * q = NULL;
        int newPoints = 0;
        respacePath(in_p, nPoints, ds, &q, newPoints);
        ASSERT(q, "null respaced points");
        ASSERT(newPoints > 1, "Should have at least 2 points: %d", newPoints);

        // find any cusps
        int * cusps = NULL;
        int nCusps = getCuspIndices(q, newPoints, &cusps);
        ASSERT(cusps, "should have cusp array, even if empty");
        ASSERT(nCusps >= 1, "should have at least 1 cusp (end): %d", nCusps);
        // DPRINTF("nCusps = %d", nCusps);

        // now walk through set of respaced points and calculate cubic fits
        int start = 0;
        int end;
        for (int c = 0; c < nCusps; ++c) {
                end = cusps[c];
                ASSERT(end > start && end < newPoints, "Bad end: %d", end);
                // DPRINTF("Segment from %d to %d", start, end);

                // starting tangent
                point_t t0 = getTangent(q, newPoints, start);
                point_t p0 = q[start];
                for (int i = start + 1; i <= end; ++i) {
                        // DPRINTF("  point %d...", i);
                        point_t p1 = q[i];
                        point_t t1 = getTangent(q, newPoints, i);
                        control_points_t cp;
                        cp.setFromPointsAndTangents(p0, t0, p1, t1);
                        curve_t b;
                        getCurveFromControlPoints(cp, b);
                        path.push_back(b);

                        // set up for next cubic
                        p0 = p1;
                        t0 = t1;
                }

                // next set
                start = end;
        }

        // DPRINTF("Finished cubic fitting, %d total beziers", path.size());
}


////////////////////////////////////////////////////////////////////////////////
//
//      static helper methods
//
////////////////////////////////////////////////////////////////////////////////

static void
getCubic
(
IN float x1,
IN float x2,
IN float x3,
OUT float& a,
OUT float& b,
OUT float& c
)
throw()
{
        // assumes a fit of the form
        //  x(t) = x0 + c t + b t^2 + a t ^3
        //
        // Furthermore, assumes x0 = 0 and x1, x2, x3 are relative to x0
        //
        // See 8 March 07 journal entry

        a = (9.0 / 2.0) * (3.0 * x1 - 3.0 * x2 + x3);
        b = (9.0 / 2.0) * (x2 - 2.0 * x1 - (2.0 / 9.0) * a);
        c = 3.0 * (x1 - (b / 9.0) - (a / 27.0));
}



static void
leastSquaresFit
(
IN const point_t * p,   // array of input points
IN int nPoints,         // size of array
IN const point_t& q,    // initial slope/direction
IN const point_t& r,    // ending slope/direction
OUT control_points_t& cp // bezier control points
)
throw()
{
        ASSERT(nPoints > 0, "Bad nPoints: %d", nPoints);

        if (1 == nPoints) {
                // dot!
                cp.p0 = cp.p1 = cp.p2 = cp.p3 = p[0];
                return;
        } else if (2 == nPoints) {
                // simple line!
                curve_t c;
                c.clear();
                c.x0 = p[0].x;
                c.y0 = p[0].y;
                c.cx = p[1].x - p[0].x;
                c.cy = p[1].y - p[0].y;
                getControlPointsFromCurve(c, cp);
                return;
        } else if (3 == nPoints) {
                // simple quadratic
                curve_t c;
                c.clear();
                c.x0 = p[0].x;
                c.y0 = p[0].y;
                c.bx = 2.0 * (p[2].x - p[0].x) - 4.0 * (p[1].x - p[0].x);
                c.by = 2.0 * (p[2].y - p[0].y) - 4.0 * (p[1].y - p[0].y);
                c.cx = p[2].x - p[0].x - c.bx;
                c.cy = p[2].y - p[0].y - c.by;
                getControlPointsFromCurve(c, cp);
                return;
        } else if (4 == nPoints) {
                // simple cubic (yuck)
                curve_t c;
                c.x0 = p[0].x;
                c.y0 = p[0].y;

                getCubic(p[1].x - p[0].x, p[2].x - p[0].x, p[3].x - p[0].x,
                    c.ax, c.bx, c.cx);
                getCubic(p[1].y - p[0].y, p[2].y - p[0].y, p[3].y - p[0].y,
                    c.ay, c.by, c.cy);
                getControlPointsFromCurve(c, cp);
                return;
        }

        //
        // This is *complicated*  See my 6 march 07 entry.  Basically, we
        // have 8 variables (4 control points, 2 variables each).  We know
        // the start and end point (4 variables) and the initial and ending
        // slopes (2 variables).  That leaves 2 remaining variables, which
        // we compute via least-squares fitting.
        //

        // DPRINTF("leastSquaresFit(N = %d)", nPoints);
        // q.dump("q");
        // r.dump("r");

        float q2 = (q.x * q.x) + (q.y * q.y);
        float r2 = (r.x * r.x) + (r.y * r.y);

        float w, x, y, z, v;
        w = x = y = z = v = 0.0;        // summation variables

        // initial and final control points
        cp.p0 = p[0];
        cp.p3 = p[nPoints - 1];

        // assume all input points are evenly spaced (parameter-wise)
        float dt = 1.0 / (nPoints - 1);
        float t = dt;
        for (int i = 1; i < nPoints - 1; ++i, t += dt) {
                float alpha = 1.0 - t;
                float h = 3.0 * t * alpha * alpha;
                float k = -3.0 * t * t * alpha;

                point_t g;
                g.x = alpha * alpha * alpha * cp.p0.x + h * cp.p0.x - k * cp.p3.x + t * t * t * cp.p3.x;
                g.y = alpha * alpha * alpha * cp.p0.y + h * cp.p0.y - k * cp.p3.y + t * t * t * cp.p3.y;

                point_t gp;     // g-p
                gp.x = g.x - p[i].x;
                gp.y = g.y - p[i].y;

                w += h * (q.x * gp.x + q.y * gp.y);
                x += h * h;
                y += h * k;
                v += k * k;
                z += k * (r.x * gp.x + r.y * gp.y);
        }

        // scale any sums
        x *= q2;
        v *= r2;
        y *= (q.x * r.x + q.y * r.y);

        // DPRINTF("  w = %5.2f", w);
        // DPRINTF("  x = %5.2f", x);
        // DPRINTF("  y = %5.2f", y);
        // DPRINTF("  z = %5.2f", z);
        // DPRINTF("  v = %5.2f", v);

        // okay, now have our interesting scalars w, x, y, z, v
        float denom = v * x - y * y;
        ASSERT(denom, "null denominator--need to handle this!");

        float a = (y * z - v * w) / denom;
        float b = (w * y - z * x) / denom;

        // DPRINTF("a = %5.2f,   b = %5.2f", a, b);

        // construct middle control points
        cp.p1.x = cp.p0.x + a * q.x;
        cp.p1.y = cp.p0.y + a * q.y;

        cp.p2.x = cp.p3.x - b * r.x;
        cp.p2.y = cp.p3.y - b * r.y;

        // that's it!  p0, p1, p2, p3 are your best fit bezier control points
}



static void
getQR
(
IN const point_t * p,
IN int nPoints,
OUT point_t& q,         // initial direction (slope)
OUT point_t& r          // final direction (slope) -- actually the negative
)
throw()
{
        ASSERT(p, "null");
        ASSERT(nPoints > 1, "Bad nPoints: %d", nPoints);

        // q is initial direction
        q = p[1] - p[0];

        // r is final direction
        r = p[nPoints - 1] - p[nPoints - 2];
}



void
addPathSegments
(
IN const point_t * p,
IN const point_t& q,
IN const point_t& r,
IN int nPoints,
IN float tolerance,
IO path_t& path
)
{
        ASSERT(p, "null array");
        ASSERT(nPoints > 1, "Bad nPoints: %d", nPoints);
        ASSERT(tolerance > 0.0, "Bad tolerance: %f", tolerance);

        //DPRINTF("  addPathSegments(n=%d)", nPoints);

        // fit
        control_points_t cp;
        leastSquaresFit(p, nPoints, q, r, cp);
        // cp.dump("least squares fit");

        // compute curve
        curve_t c;
        getCurveFromControlPoints(cp, c);

        // does this match to within the desired tolerance?
        float fit = getFitQuality(p, nPoints, c);
        if (fit < tolerance) {
                path.push_back(c);      // yes, success!
                return;
        }

        // didn't fit to within tolerance.  Subdivide
        // DPRINTF("  Subdividing!");
        ASSERT(nPoints > 4, "Bad fit with %d points!?", nPoints);
        int mid = nPoints / 2;
        ASSERT(mid > 0 && mid < nPoints - 1, "Bad midpoint: %d", mid);

        // fit first half
        int newN = mid + 1;
        // DPRINTF("First half: %d - %d", 0, mid);
        point_t q2, r2;
        getQR(p, newN, q2, r2);
        addPathSegments(p, q2, r2, newN, tolerance, path);

        // fit second half
        newN = nPoints - mid;
        // DPRINTF("Second half: %d - %d", mid, nPoints - 1);
        getQR(p + mid, newN, q2, r2);
        addPathSegments(p + mid, q2, r2, newN, tolerance, path);
}



////////////////////////////////////////////////////////////////////////////////
//
//      public API
//
////////////////////////////////////////////////////////////////////////////////

void
fitPoints
(
IN const float * x,
IN int nPoints,
IN float slope,
OUT fit_t& fit
)
throw()
{
        ASSERT(x, "null x-array");
        ASSERT(nPoints > 1, "Bad nPoints: %d", nPoints);

        fit.clear();
        //DPRINTF("  slope = %5.2f", slope);

        if (2 == nPoints) {
                // just a straight line!
                fit.q0 = x[0];
                fit.c = x[1] - x[0];
                return;
        }
        ASSERT(nPoints > 2, "Bad nPoints: %d", nPoints);

        // use control points
        float x0 = x[0];
        float x1 = x0 + (slope / 3.0);
        float x3 = x[nPoints - 1];

        // least-squares fit for x2
        float sum_riti = 0.0;
        float sum_ti2 = 0.0;
        float dt = 1.0 / (nPoints - 1);
        float t = dt;

        for (int i = 1; i < nPoints - 1; ++i) {
                float s = 1.0 - t;
                float ti = t * t * s;
                float ri = (x0 * s * s * s) + (3.0 * x1 * t * s * s) + (x3 * t * t * t) - x[i];
                sum_riti += ri * ti;
                sum_ti2 += ti * ti;
                t += dt;
        }

        float x2 = (-1.0/3.0) * (sum_riti / sum_ti2);

        //DPRINTF("    Control points: x0=%5.2f  x1=%5.2f  x2=%5.2f  x3=%5.2f",
        //    x0, x1, x2, x3);

        // transform from control points back to curve params
        fit.q0 = x0;
        fit.c = 3.0 * (x1 - x0);
        fit.b = 3.0 * (x2 - x1) - fit.c;
        fit.a = x3 - x0 - fit.b - fit.c;
}



int
getCuspIndices
(
IN const point_t * p,
IN int nPoints,
OUT int ** cusps
)
{
        ASSERT(p, "null array");
        ASSERT(nPoints > 0, "Bad array");
        ASSERT(cusps, "null array");

        // cusp arrays
        static int * s_cusp = NULL;
        static int s_nPoints = 0;

        if (nPoints > s_nPoints) {
                if (s_cusp)
                        delete[] s_cusp;
                s_nPoints = nPoints + 32;
                s_cusp = new int[s_nPoints];
        }
        ASSERT(s_cusp, "out of memory");

        // degenerate case
        if (nPoints < 3) {
                s_cusp[0] = nPoints - 1;
                *cusps = s_cusp;
                return 1;
        }

        float s_threshold = 0.5;
        int nCusps = 0;

        // we run through and calculate the dot product of sequential rays.
        // A low result means the path has radically changed direction
        point_t p0 = p[1] - p[0];
        p0.normalize();
        for (int i = 1; i < nPoints - 1; ++i) {
                point_t p1 = p[i + 1] - p[i];
                p1.normalize();

                float dot = point_t::dotProduct(p0, p1);
                // DPRINTF("dot = %f", dot);
                if (dot < s_threshold) {
                        // DPRINTF("  cusp!");
                        s_cusp[nCusps] = i;
                        nCusps++;
                }

                p0 = p1;
        }

        // add final point as a cusp (I think this helps clients...)
        s_cusp[nCusps] = nPoints - 1;
        ++nCusps;

        // return
        *cusps = s_cusp;
        return nCusps;
}



float
getFitQuality
(
IN const point_t * p,
IN int nPoints,
IN const curve_t& curve
)
throw()
{
        ASSERT(p, "null array");
        ASSERT(nPoints > 1, "bad npoints");

        float dt = 1.0 / (nPoints - 1);
        float sum2 = 0.0;
        float t = 0.0;
        point_t q;
        for (int i = 1; i < nPoints - 1; ++i) {
                t += dt;
                curve.getPointAtT(t, q);
                // DPRINTF("  Actual (%5.2f, %5.2f)  vs. calculated (%5.2f, %5.2f)",
                //     p[i].x, p[i].y, q.x, q.y);
                float dx = p[i].x - q.x;
                float dy = p[i].y - q.y;
                float diff = (dx * dx) + (dy * dy);
                sum2 += diff; 
        }

        float rms = sqrt(sum2) / nPoints;
        // DPRINTF("RMS of fit: %5.2f", rms);

        return rms;
}



void
fitPath
(
IN const point_t * p,
IN int nPoints,
IN float tolerance,
OUT path_t& path
)
{
        perf::Timer timer("bezier::fitPath");

        ASSERT(p, "null array");
        ASSERT(nPoints > 1, "Bad nPoints: %d", nPoints);
        ASSERT(tolerance > 0.0, "Bad tolerance: %f", tolerance);
        path.clear();

        // get all cusps
        int * cusps = NULL;
        int nCusp = getCuspIndices(p, nPoints, &cusps);
        ASSERT(cusps, "should have non-null cusp index array");
        ASSERT(nCusp > 0, "should have at least 1 cusp (end): %d", nCusp);

        // loop through all cusps
        int start = 0;          // starting index
        int end = 0;
        for (int i = 0; i < nCusp; ++i) {
                // determine endpoint for this segment
                end = cusps[i];
                // was the final point already a cusp?
                if (start == end)
                        break;
                ASSERT(end > start, "Bad start=%d, end=%d", start, end);
                ASSERT(end < nPoints, "bad end=%d", end);

                // have an endpoint
                // DPRINTF("start=%02d  end=%02d", start, end);

                // Number of points?
                int N = end - start + 1;
                ASSERT(N > 1, "Bad N:%d", N);

                // get starting/ending directions
                point_t q,r;
                getQR(p + start, N, q, r);

                // get this segment's path elements
                addPathSegments(p + start, q, r, N, tolerance, path);

                // next segment
                start = end;
        }
}



void
respacePath
(
IN const point_t * p,
IN int nPoints,
IN float dt,
OUT point_t ** out_p,
OUT int& newPoints
)
{
        ASSERT(nPoints > 0, "not enough points!");
        ASSERT(p, "null");
        ASSERT(out_p, "null");
        ASSERT(dt > 0, "bad path step size");

        static vec_point_t s_p;
        s_p.clear();

        // first point must match
        s_p.push_back(p[0]);

        // special case n=1
        if (1 == nPoints) {
                point_t d(0.005, 0.005);
                s_p.push_back(p[0] + d);
//              *out_p = s_p.data();    // not supported in Shrike
                *out_p = &s_p[0];
                newPoints = 2;
                return;
        }

        // longer paths
        int src = 0;    // starting at source point 0
        float s = 0.0;          // total path length
        point_t p0 = p[0];
        for (float t = dt; src < nPoints - 1; t += dt) {
                // step source until we are at good start point
                float ds;
                point_t dp;
                while (true) {
                        point_t p1 = p[src + 1];
                        dp = p1 - p0;
                        ds = sqrt(point_t::dotProduct(dp, dp));
                        if (t < s + ds || src >= nPoints - 1)
                                break;
                        src++;
                        p0 = p1;
                        s += ds;
                }

                // interpolate between src and src + 1
                float d0 = t - s;
                float q = d0 / ds;
                if (q > 1.0) {
                        q = 1.0;
                }
                ASSERT(q >= 0 && q <= 1.0, "Bad q: %f", q);

                point_t p1(p0.x + q * dp.x, p0.y + q * dp.y);
                point_t d = p1 - s_p.back();
                if (sqrt(point_t::dotProduct(d, d)) > 0.2 * ds) {
                        s_p.push_back(p1);
                }
        }

        // DPRINTF("Total length s=%5.2f  dt=%5.2f  in_n=%d  out_n=%d",
        //    s, dt, nPoints, (int) s_p.size());

        // all done!  Return data
//      *out_p = s_p.data();    // not supported in Shrike!
        *out_p = &s_p[0];
        newPoints = (int) s_p.size();

        if (1 == newPoints) {
                // add a point so we always deal with two...
                point_t p1 = s_p.back();
                p1.x += 0.001;
                p1.y += 0.001;
                s_p.push_back(p1);
                newPoints = 2;
        }

        /*
        // DEBUG
        {
                int i = 0;
                while (i < nPoints || i < newPoints) {
                        point_t old(-1.0, -1.0);
                        if (i < nPoints)
                                old = p[i];
                        point_t newp(-1.0, -1.0);
                        if (i < newPoints)
                                newp = s_p[i];

                        DPRINTF("%03d: old (%5.2f, %5.2f)   new (%5.2f, %5.2f)",
                            i, old.x, old.y, newp.x, newp.y);
                        ++i;
                }
        }
        */
}



};      // bezier namespace