603 lines
16 KiB
C++
603 lines
16 KiB
C++
|
|
||
|
|
//
|
||
|
|
// Copyright (C) : Please refer to the COPYRIGHT file distributed
|
||
|
|
// with this source distribution.
|
||
|
|
//
|
||
|
|
// This program is free software; you can redistribute it and/or
|
||
|
|
// modify it under the terms of the GNU General Public License
|
||
|
|
// as published by the Free Software Foundation; either version 2
|
||
|
|
// of the License, or (at your option) any later version.
|
||
|
|
//
|
||
|
|
// This program is distributed in the hope that it will be useful,
|
||
|
|
// but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||
|
|
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||
|
|
// GNU General Public License for more details.
|
||
|
|
//
|
||
|
|
// You should have received a copy of the GNU General Public License
|
||
|
|
// along with this program; if not, write to the Free Software
|
||
|
|
// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
|
||
|
|
//
|
||
|
|
///////////////////////////////////////////////////////////////////////////////
|
||
|
|
|
||
|
|
#include <stdio.h>
|
||
|
|
#include <math.h>
|
||
|
|
#include "FitCurve.h"
|
||
|
|
|
||
|
|
using namespace std;
|
||
|
|
|
||
|
|
typedef Vector2 *BezierCurve;
|
||
|
|
|
||
|
|
#ifdef __cplusplus
|
||
|
|
extern "C"
|
||
|
|
{
|
||
|
|
#endif
|
||
|
|
|
||
|
|
/* Forward declarations */
|
||
|
|
static double *Reparameterize(Vector2 *d, int first, int last, double *u, BezierCurve bezCurve);
|
||
|
|
static double NewtonRaphsonRootFind(BezierCurve Q, Vector2 P, double u);
|
||
|
|
static Vector2 BezierII(int degree, Vector2 *V, double t);
|
||
|
|
static double B0(double u);
|
||
|
|
static double B1(double u);
|
||
|
|
static double B2(double u);
|
||
|
|
static double B3(double u);
|
||
|
|
static Vector2 ComputeLeftTangent(Vector2 *d, int end);
|
||
|
|
static Vector2 ComputeLeftTangent(Vector2 *d, int end);
|
||
|
|
static Vector2 ComputeLeftTangent(Vector2 *d, int end);
|
||
|
|
static double ComputeMaxError(Vector2 *d, int first, int last, BezierCurve bezCurve, double *u, int *splitPoint);
|
||
|
|
static double *ChordLengthParameterize(Vector2 *d, int first, int last);
|
||
|
|
static BezierCurve GenerateBezier(Vector2 *d, int first, int last, double *uPrime, Vector2 tHat1, Vector2 tHat2);
|
||
|
|
static Vector2 V2AddII(Vector2 a, Vector2 b);
|
||
|
|
static Vector2 V2ScaleIII(Vector2 v, double s);
|
||
|
|
static Vector2 V2SubII(Vector2 a, Vector2 b);
|
||
|
|
|
||
|
|
|
||
|
|
#define MAXPOINTS 1000 /* The most points you can have */
|
||
|
|
|
||
|
|
/* returns squared length of input vector */
|
||
|
|
double V2SquaredLength(Vector2 *a)
|
||
|
|
{ return(((*a)[0] * (*a)[0])+((*a)[1] * (*a)[1]));
|
||
|
|
}
|
||
|
|
|
||
|
|
/* returns length of input vector */
|
||
|
|
double V2Length(Vector2 *a)
|
||
|
|
{
|
||
|
|
return(sqrt(V2SquaredLength(a)));
|
||
|
|
}
|
||
|
|
|
||
|
|
Vector2 *V2Scale(Vector2 *v, double newlen)
|
||
|
|
{
|
||
|
|
double len = V2Length(v);
|
||
|
|
if (len != 0.0) { (*v)[0] *= newlen/len; (*v)[1] *= newlen/len; }
|
||
|
|
return(v);
|
||
|
|
}
|
||
|
|
|
||
|
|
/* return the dot product of vectors a and b */
|
||
|
|
double V2Dot(Vector2 *a, Vector2 *b)
|
||
|
|
{
|
||
|
|
return(((*a)[0]*(*b)[0])+((*a)[1]*(*b)[1]));
|
||
|
|
}
|
||
|
|
|
||
|
|
/* return the distance between two points */
|
||
|
|
double V2DistanceBetween2Points(Vector2 *a, Vector2 *b)
|
||
|
|
{
|
||
|
|
double dx = (*a)[0] - (*b)[0];
|
||
|
|
double dy = (*a)[1] - (*b)[1];
|
||
|
|
return(sqrt((dx*dx)+(dy*dy)));
|
||
|
|
}
|
||
|
|
|
||
|
|
/* return vector sum c = a+b */
|
||
|
|
Vector2 *V2Add(Vector2 *a, Vector2 *b, Vector2 *c)
|
||
|
|
{
|
||
|
|
(*c)[0] = (*a)[0]+(*b)[0]; (*c)[1] = (*a)[1]+(*b)[1];
|
||
|
|
return(c);
|
||
|
|
}
|
||
|
|
|
||
|
|
/* normalizes the input vector and returns it */
|
||
|
|
Vector2 *V2Normalize(Vector2 *v)
|
||
|
|
{
|
||
|
|
double len = V2Length(v);
|
||
|
|
if (len != 0.0) { (*v)[0] /= len; (*v)[1] /= len; }
|
||
|
|
return(v);
|
||
|
|
}
|
||
|
|
|
||
|
|
/* negates the input vector and returns it */
|
||
|
|
Vector2 *V2Negate(Vector2 *v)
|
||
|
|
{
|
||
|
|
(*v)[0] = -(*v)[0]; (*v)[1] = -(*v)[1];
|
||
|
|
return(v);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
/*
|
||
|
|
* GenerateBezier :
|
||
|
|
* Use least-squares method to find Bezier control points for region.
|
||
|
|
*
|
||
|
|
*/
|
||
|
|
static BezierCurve GenerateBezier(Vector2 *d, int first, int last, double *uPrime, Vector2 tHat1, Vector2 tHat2)
|
||
|
|
// Vector2 *d; /* Array of digitized points */
|
||
|
|
// int first, last; /* Indices defining region */
|
||
|
|
// double *uPrime; /* Parameter values for region */
|
||
|
|
// Vector2 tHat1, tHat2; /* Unit tangents at endpoints */
|
||
|
|
{
|
||
|
|
int i;
|
||
|
|
Vector2 A[MAXPOINTS][2]; /* Precomputed rhs for eqn */
|
||
|
|
int nPts; /* Number of pts in sub-curve */
|
||
|
|
double C[2][2]; /* Matrix C */
|
||
|
|
double X[2]; /* Matrix X */
|
||
|
|
double det_C0_C1, /* Determinants of matrices */
|
||
|
|
det_C0_X,
|
||
|
|
det_X_C1;
|
||
|
|
double alpha_l, /* Alpha values, left and right */
|
||
|
|
alpha_r;
|
||
|
|
Vector2 tmp; /* Utility variable */
|
||
|
|
BezierCurve bezCurve; /* RETURN bezier curve ctl pts */
|
||
|
|
|
||
|
|
bezCurve = (Vector2 *)malloc(4 * sizeof(Vector2));
|
||
|
|
nPts = last - first + 1;
|
||
|
|
|
||
|
|
|
||
|
|
/* Compute the A's */
|
||
|
|
for (i = 0; i < nPts; i++) {
|
||
|
|
Vector2 v1, v2;
|
||
|
|
v1 = tHat1;
|
||
|
|
v2 = tHat2;
|
||
|
|
V2Scale(&v1, B1(uPrime[i]));
|
||
|
|
V2Scale(&v2, B2(uPrime[i]));
|
||
|
|
A[i][0] = v1;
|
||
|
|
A[i][1] = v2;
|
||
|
|
}
|
||
|
|
|
||
|
|
/* Create the C and X matrices */
|
||
|
|
C[0][0] = 0.0;
|
||
|
|
C[0][1] = 0.0;
|
||
|
|
C[1][0] = 0.0;
|
||
|
|
C[1][1] = 0.0;
|
||
|
|
X[0] = 0.0;
|
||
|
|
X[1] = 0.0;
|
||
|
|
|
||
|
|
for (i = 0; i < nPts; i++) {
|
||
|
|
C[0][0] += V2Dot(&A[i][0], &A[i][0]);
|
||
|
|
C[0][1] += V2Dot(&A[i][0], &A[i][1]);
|
||
|
|
/* C[1][0] += V2Dot(&A[i][0], &A[i][1]);*/
|
||
|
|
C[1][0] = C[0][1];
|
||
|
|
C[1][1] += V2Dot(&A[i][1], &A[i][1]);
|
||
|
|
|
||
|
|
tmp = V2SubII(d[first + i],
|
||
|
|
V2AddII(
|
||
|
|
V2ScaleIII(d[first], B0(uPrime[i])),
|
||
|
|
V2AddII(
|
||
|
|
V2ScaleIII(d[first], B1(uPrime[i])),
|
||
|
|
V2AddII(
|
||
|
|
V2ScaleIII(d[last], B2(uPrime[i])),
|
||
|
|
V2ScaleIII(d[last], B3(uPrime[i]))))));
|
||
|
|
|
||
|
|
|
||
|
|
X[0] += V2Dot(&((A[i])[0]), &tmp);
|
||
|
|
X[1] += V2Dot(&((A[i])[1]), &tmp);
|
||
|
|
}
|
||
|
|
|
||
|
|
/* Compute the determinants of C and X */
|
||
|
|
det_C0_C1 = C[0][0] * C[1][1] - C[1][0] * C[0][1];
|
||
|
|
det_C0_X = C[0][0] * X[1] - C[0][1] * X[0];
|
||
|
|
det_X_C1 = X[0] * C[1][1] - X[1] * C[0][1];
|
||
|
|
|
||
|
|
/* Finally, derive alpha values */
|
||
|
|
if (det_C0_C1 == 0.0) {
|
||
|
|
det_C0_C1 = (C[0][0] * C[1][1]) * 10e-12;
|
||
|
|
}
|
||
|
|
alpha_l = det_X_C1 / det_C0_C1;
|
||
|
|
alpha_r = det_C0_X / det_C0_C1;
|
||
|
|
|
||
|
|
|
||
|
|
/* If alpha negative, use the Wu/Barsky heuristic (see text) */
|
||
|
|
/* (if alpha is 0, you get coincident control points that lead to
|
||
|
|
* divide by zero in any subsequent NewtonRaphsonRootFind() call. */
|
||
|
|
if (alpha_l < 1.0e-6 || alpha_r < 1.0e-6) {
|
||
|
|
double dist = V2DistanceBetween2Points(&d[last], &d[first]) /
|
||
|
|
3.0;
|
||
|
|
|
||
|
|
bezCurve[0] = d[first];
|
||
|
|
bezCurve[3] = d[last];
|
||
|
|
V2Add(&(bezCurve[0]), V2Scale(&(tHat1), dist), &(bezCurve[1]));
|
||
|
|
V2Add(&(bezCurve[3]), V2Scale(&(tHat2), dist), &(bezCurve[2]));
|
||
|
|
return (bezCurve);
|
||
|
|
}
|
||
|
|
|
||
|
|
/* First and last control points of the Bezier curve are */
|
||
|
|
/* positioned exactly at the first and last data points */
|
||
|
|
/* Control points 1 and 2 are positioned an alpha distance out */
|
||
|
|
/* on the tangent vectors, left and right, respectively */
|
||
|
|
bezCurve[0] = d[first];
|
||
|
|
bezCurve[3] = d[last];
|
||
|
|
V2Add(&bezCurve[0], V2Scale(&tHat1, alpha_l), &bezCurve[1]);
|
||
|
|
V2Add(&bezCurve[3], V2Scale(&tHat2, alpha_r), &bezCurve[2]);
|
||
|
|
return (bezCurve);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
/*
|
||
|
|
* Reparameterize:
|
||
|
|
* Given set of points and their parameterization, try to find
|
||
|
|
* a better parameterization.
|
||
|
|
*
|
||
|
|
*/
|
||
|
|
static double *Reparameterize(Vector2 *d, int first, int last, double *u, BezierCurve bezCurve)
|
||
|
|
// Vector2 *d; /* Array of digitized points */
|
||
|
|
// int first, last; /* Indices defining region */
|
||
|
|
// double *u; /* Current parameter values */
|
||
|
|
// BezierCurve bezCurve; /* Current fitted curve */
|
||
|
|
{
|
||
|
|
int nPts = last-first+1;
|
||
|
|
int i;
|
||
|
|
double *uPrime; /* New parameter values */
|
||
|
|
|
||
|
|
uPrime = (double *)malloc(nPts * sizeof(double));
|
||
|
|
for (i = first; i <= last; i++) {
|
||
|
|
uPrime[i-first] = NewtonRaphsonRootFind(bezCurve, d[i], u[i-
|
||
|
|
first]);
|
||
|
|
}
|
||
|
|
return (uPrime);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
/*
|
||
|
|
* NewtonRaphsonRootFind :
|
||
|
|
* Use Newton-Raphson iteration to find better root.
|
||
|
|
*/
|
||
|
|
static double NewtonRaphsonRootFind(BezierCurve Q, Vector2 P, double u)
|
||
|
|
// BezierCurve Q; /* Current fitted curve */
|
||
|
|
// Vector2 P; /* Digitized point */
|
||
|
|
// double u; /* Parameter value for "P" */
|
||
|
|
{
|
||
|
|
double numerator, denominator;
|
||
|
|
Vector2 Q1[3], Q2[2]; /* Q' and Q'' */
|
||
|
|
Vector2 Q_u, Q1_u, Q2_u; /*u evaluated at Q, Q', & Q'' */
|
||
|
|
double uPrime; /* Improved u */
|
||
|
|
int i;
|
||
|
|
|
||
|
|
/* Compute Q(u) */
|
||
|
|
Q_u = BezierII(3, Q, u);
|
||
|
|
|
||
|
|
/* Generate control vertices for Q' */
|
||
|
|
for (i = 0; i <= 2; i++) {
|
||
|
|
Q1[i][0] = (Q[i+1][0] - Q[i][0]) * 3.0;
|
||
|
|
Q1[i][1] = (Q[i+1][1] - Q[i][1]) * 3.0;
|
||
|
|
}
|
||
|
|
|
||
|
|
/* Generate control vertices for Q'' */
|
||
|
|
for (i = 0; i <= 1; i++) {
|
||
|
|
Q2[i][0] = (Q1[i+1][0] - Q1[i][0]) * 2.0;
|
||
|
|
Q2[i][1] = (Q1[i+1][1] - Q1[i][1]) * 2.0;
|
||
|
|
}
|
||
|
|
|
||
|
|
/* Compute Q'(u) and Q''(u) */
|
||
|
|
Q1_u = BezierII(2, Q1, u);
|
||
|
|
Q2_u = BezierII(1, Q2, u);
|
||
|
|
|
||
|
|
/* Compute f(u)/f'(u) */
|
||
|
|
numerator = (Q_u[0] - P[0]) * (Q1_u[0]) + (Q_u[1] - P[1]) * (Q1_u[1]);
|
||
|
|
denominator = (Q1_u[0]) * (Q1_u[0]) + (Q1_u[1]) * (Q1_u[1]) +
|
||
|
|
(Q_u[0] - P[0]) * (Q2_u[0]) + (Q_u[1] - P[1]) * (Q2_u[1]);
|
||
|
|
|
||
|
|
/* u = u - f(u)/f'(u) */
|
||
|
|
if(denominator == 0) // FIXME
|
||
|
|
return u;
|
||
|
|
uPrime = u - (numerator/denominator);
|
||
|
|
return (uPrime);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
/*
|
||
|
|
* Bezier :
|
||
|
|
* Evaluate a Bezier curve at a particular parameter value
|
||
|
|
*
|
||
|
|
*/
|
||
|
|
static Vector2 BezierII(int degree, Vector2 *V, double t)
|
||
|
|
// int degree; /* The degree of the bezier curve */
|
||
|
|
// Vector2 *V; /* Array of control points */
|
||
|
|
// double t; /* Parametric value to find point for */
|
||
|
|
{
|
||
|
|
int i, j;
|
||
|
|
Vector2 Q; /* Point on curve at parameter t */
|
||
|
|
Vector2 *Vtemp; /* Local copy of control points */
|
||
|
|
|
||
|
|
/* Copy array */
|
||
|
|
Vtemp = (Vector2 *)malloc((unsigned)((degree+1)
|
||
|
|
* sizeof (Vector2)));
|
||
|
|
for (i = 0; i <= degree; i++) {
|
||
|
|
Vtemp[i] = V[i];
|
||
|
|
}
|
||
|
|
|
||
|
|
/* Triangle computation */
|
||
|
|
for (i = 1; i <= degree; i++) {
|
||
|
|
for (j = 0; j <= degree-i; j++) {
|
||
|
|
Vtemp[j][0] = (1.0 - t) * Vtemp[j][0] + t * Vtemp[j+1][0];
|
||
|
|
Vtemp[j][1] = (1.0 - t) * Vtemp[j][1] + t * Vtemp[j+1][1];
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
Q = Vtemp[0];
|
||
|
|
free((void *)Vtemp);
|
||
|
|
return Q;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
/*
|
||
|
|
* B0, B1, B2, B3 :
|
||
|
|
* Bezier multipliers
|
||
|
|
*/
|
||
|
|
static double B0(double u)
|
||
|
|
{
|
||
|
|
double tmp = 1.0 - u;
|
||
|
|
return (tmp * tmp * tmp);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
static double B1(double u)
|
||
|
|
{
|
||
|
|
double tmp = 1.0 - u;
|
||
|
|
return (3 * u * (tmp * tmp));
|
||
|
|
}
|
||
|
|
|
||
|
|
static double B2(double u)
|
||
|
|
{
|
||
|
|
double tmp = 1.0 - u;
|
||
|
|
return (3 * u * u * tmp);
|
||
|
|
}
|
||
|
|
|
||
|
|
static double B3(double u)
|
||
|
|
{
|
||
|
|
return (u * u * u);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
/*
|
||
|
|
* ComputeLeftTangent, ComputeRightTangent, ComputeCenterTangent :
|
||
|
|
*Approximate unit tangents at endpoints and "center" of digitized curve
|
||
|
|
*/
|
||
|
|
static Vector2 ComputeLeftTangent(Vector2 *d, int end)
|
||
|
|
// Vector2 *d; /* Digitized points*/
|
||
|
|
// int end; /* Index to "left" end of region */
|
||
|
|
{
|
||
|
|
Vector2 tHat1;
|
||
|
|
tHat1 = V2SubII(d[end+1], d[end]);
|
||
|
|
tHat1 = *V2Normalize(&tHat1);
|
||
|
|
return tHat1;
|
||
|
|
}
|
||
|
|
|
||
|
|
static Vector2 ComputeRightTangent(Vector2 *d, int end)
|
||
|
|
// Vector2 *d; /* Digitized points */
|
||
|
|
// int end; /* Index to "right" end of region */
|
||
|
|
{
|
||
|
|
Vector2 tHat2;
|
||
|
|
tHat2 = V2SubII(d[end-1], d[end]);
|
||
|
|
tHat2 = *V2Normalize(&tHat2);
|
||
|
|
return tHat2;
|
||
|
|
}
|
||
|
|
|
||
|
|
static Vector2 ComputeCenterTangent(Vector2 *d, int center)
|
||
|
|
// Vector2 *d; /* Digitized points */
|
||
|
|
// int center; /* Index to point inside region */
|
||
|
|
{
|
||
|
|
Vector2 V1, V2, tHatCenter;
|
||
|
|
|
||
|
|
V1 = V2SubII(d[center-1], d[center]);
|
||
|
|
V2 = V2SubII(d[center], d[center+1]);
|
||
|
|
tHatCenter[0] = (V1[0] + V2[0])/2.0;
|
||
|
|
tHatCenter[1] = (V1[1] + V2[1])/2.0;
|
||
|
|
tHatCenter = *V2Normalize(&tHatCenter);
|
||
|
|
return tHatCenter;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
/*
|
||
|
|
* ChordLengthParameterize :
|
||
|
|
* Assign parameter values to digitized points
|
||
|
|
* using relative distances between points.
|
||
|
|
*/
|
||
|
|
static double *ChordLengthParameterize(Vector2 *d, int first, int last)
|
||
|
|
// Vector2 *d; /* Array of digitized points */
|
||
|
|
// int first, last; /* Indices defining region */
|
||
|
|
{
|
||
|
|
int i;
|
||
|
|
double *u; /* Parameterization */
|
||
|
|
|
||
|
|
u = (double *)malloc((unsigned)(last-first+1) * sizeof(double));
|
||
|
|
|
||
|
|
u[0] = 0.0;
|
||
|
|
for (i = first+1; i <= last; i++) {
|
||
|
|
u[i-first] = u[i-first-1] +
|
||
|
|
V2DistanceBetween2Points(&d[i], &d[i-1]);
|
||
|
|
}
|
||
|
|
|
||
|
|
for (i = first + 1; i <= last; i++) {
|
||
|
|
u[i-first] = u[i-first] / u[last-first];
|
||
|
|
}
|
||
|
|
|
||
|
|
return(u);
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
|
||
|
|
/*
|
||
|
|
* ComputeMaxError :
|
||
|
|
* Find the maximum squared distance of digitized points
|
||
|
|
* to fitted curve.
|
||
|
|
*/
|
||
|
|
static double ComputeMaxError(Vector2 *d, int first, int last, BezierCurve bezCurve, double *u, int *splitPoint)
|
||
|
|
// Vector2 *d; /* Array of digitized points */
|
||
|
|
// int first, last; /* Indices defining region */
|
||
|
|
// BezierCurve bezCurve; /* Fitted Bezier curve */
|
||
|
|
// double *u; /* Parameterization of points */
|
||
|
|
// int *splitPoint; /* Point of maximum error */
|
||
|
|
{
|
||
|
|
int i;
|
||
|
|
double maxDist; /* Maximum error */
|
||
|
|
double dist; /* Current error */
|
||
|
|
Vector2 P; /* Point on curve */
|
||
|
|
Vector2 v; /* Vector from point to curve */
|
||
|
|
|
||
|
|
*splitPoint = (last - first + 1)/2;
|
||
|
|
maxDist = 0.0;
|
||
|
|
for (i = first + 1; i < last; i++) {
|
||
|
|
P = BezierII(3, bezCurve, u[i-first]);
|
||
|
|
v = V2SubII(P, d[i]);
|
||
|
|
dist = V2SquaredLength(&v);
|
||
|
|
if (dist >= maxDist) {
|
||
|
|
maxDist = dist;
|
||
|
|
*splitPoint = i;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
return (maxDist);
|
||
|
|
}
|
||
|
|
static Vector2 V2AddII(Vector2 a, Vector2 b)
|
||
|
|
{
|
||
|
|
Vector2 c;
|
||
|
|
c[0] = a[0] + b[0]; c[1] = a[1] + b[1];
|
||
|
|
return (c);
|
||
|
|
}
|
||
|
|
static Vector2 V2ScaleIII(Vector2 v, double s)
|
||
|
|
{
|
||
|
|
Vector2 result;
|
||
|
|
result[0] = v[0] * s; result[1] = v[1] * s;
|
||
|
|
return (result);
|
||
|
|
}
|
||
|
|
|
||
|
|
static Vector2 V2SubII(Vector2 a, Vector2 b)
|
||
|
|
{
|
||
|
|
Vector2 c;
|
||
|
|
c[0] = a[0] - b[0]; c[1] = a[1] - b[1];
|
||
|
|
return (c);
|
||
|
|
}
|
||
|
|
|
||
|
|
#ifdef __cplusplus
|
||
|
|
}
|
||
|
|
#endif
|
||
|
|
|
||
|
|
|
||
|
|
//------------------------- WRAPPER -----------------------------//
|
||
|
|
|
||
|
|
FitCurveWrapper::FitCurveWrapper()
|
||
|
|
{
|
||
|
|
}
|
||
|
|
|
||
|
|
FitCurveWrapper::~FitCurveWrapper()
|
||
|
|
{
|
||
|
|
_vertices.clear();
|
||
|
|
}
|
||
|
|
|
||
|
|
void FitCurveWrapper::DrawBezierCurve(int n, Vector2 *curve )
|
||
|
|
{
|
||
|
|
for(int i=0; i<n+1; ++i)
|
||
|
|
_vertices.push_back(curve[i]);
|
||
|
|
}
|
||
|
|
|
||
|
|
void FitCurveWrapper::FitCurve(vector<Vec2d>& data, vector<Vec2d>& oCurve, double error)
|
||
|
|
{
|
||
|
|
int size = data.size();
|
||
|
|
Vector2 *d = new Vector2[size];
|
||
|
|
for(int i=0; i<size; ++i)
|
||
|
|
{
|
||
|
|
d[i][0] = data[i][0];
|
||
|
|
d[i][1] = data[i][1];
|
||
|
|
}
|
||
|
|
|
||
|
|
FitCurve(d,size,error);
|
||
|
|
|
||
|
|
// copy results
|
||
|
|
for(vector<Vector2>::iterator v=_vertices.begin(), vend=_vertices.end();
|
||
|
|
v!=vend;
|
||
|
|
++v)
|
||
|
|
{
|
||
|
|
oCurve.push_back(Vec2d(v->x(), v->y())) ;
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
void FitCurveWrapper::FitCurve(Vector2 *d, int nPts, double error)
|
||
|
|
{
|
||
|
|
Vector2 tHat1, tHat2; /* Unit tangent vectors at endpoints */
|
||
|
|
|
||
|
|
tHat1 = ComputeLeftTangent(d, 0);
|
||
|
|
tHat2 = ComputeRightTangent(d, nPts - 1);
|
||
|
|
FitCubic(d, 0, nPts - 1, tHat1, tHat2, error);
|
||
|
|
}
|
||
|
|
|
||
|
|
void FitCurveWrapper::FitCubic(Vector2 *d, int first, int last, Vector2 tHat1, Vector2 tHat2, double error)
|
||
|
|
{
|
||
|
|
BezierCurve bezCurve; /*Control points of fitted Bezier curve*/
|
||
|
|
double *u; /* Parameter values for point */
|
||
|
|
double *uPrime; /* Improved parameter values */
|
||
|
|
double maxError; /* Maximum fitting error */
|
||
|
|
int splitPoint; /* Point to split point set at */
|
||
|
|
int nPts; /* Number of points in subset */
|
||
|
|
double iterationError; /*Error below which you try iterating */
|
||
|
|
int maxIterations = 4; /* Max times to try iterating */
|
||
|
|
Vector2 tHatCenter; /* Unit tangent vector at splitPoint */
|
||
|
|
int i;
|
||
|
|
|
||
|
|
iterationError = error * error;
|
||
|
|
nPts = last - first + 1;
|
||
|
|
|
||
|
|
/* Use heuristic if region only has two points in it */
|
||
|
|
if (nPts == 2) {
|
||
|
|
double dist = V2DistanceBetween2Points(&d[last], &d[first]) / 3.0;
|
||
|
|
|
||
|
|
bezCurve = (Vector2 *)malloc(4 * sizeof(Vector2));
|
||
|
|
bezCurve[0] = d[first];
|
||
|
|
bezCurve[3] = d[last];
|
||
|
|
V2Add(&bezCurve[0], V2Scale(&tHat1, dist), &bezCurve[1]);
|
||
|
|
V2Add(&bezCurve[3], V2Scale(&tHat2, dist), &bezCurve[2]);
|
||
|
|
DrawBezierCurve(3, bezCurve);
|
||
|
|
free((void *)bezCurve);
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
/* Parameterize points, and attempt to fit curve */
|
||
|
|
u = ChordLengthParameterize(d, first, last);
|
||
|
|
bezCurve = GenerateBezier(d, first, last, u, tHat1, tHat2);
|
||
|
|
|
||
|
|
/* Find max deviation of points to fitted curve */
|
||
|
|
maxError = ComputeMaxError(d, first, last, bezCurve, u, &splitPoint);
|
||
|
|
if (maxError < error) {
|
||
|
|
DrawBezierCurve(3, bezCurve);
|
||
|
|
free((void *)u);
|
||
|
|
free((void *)bezCurve);
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
/* If error not too large, try some reparameterization */
|
||
|
|
/* and iteration */
|
||
|
|
if (maxError < iterationError) {
|
||
|
|
for (i = 0; i < maxIterations; i++) {
|
||
|
|
uPrime = Reparameterize(d, first, last, u, bezCurve);
|
||
|
|
bezCurve = GenerateBezier(d, first, last, uPrime, tHat1, tHat2);
|
||
|
|
maxError = ComputeMaxError(d, first, last,
|
||
|
|
bezCurve, uPrime, &splitPoint);
|
||
|
|
if (maxError < error) {
|
||
|
|
DrawBezierCurve(3, bezCurve);
|
||
|
|
free((void *)u);
|
||
|
|
free((void *)bezCurve);
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
free((void *)u);
|
||
|
|
u = uPrime;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/* Fitting failed -- split at max error point and fit recursively */
|
||
|
|
free((void *)u);
|
||
|
|
free((void *)bezCurve);
|
||
|
|
tHatCenter = ComputeCenterTangent(d, splitPoint);
|
||
|
|
FitCubic(d, first, splitPoint, tHat1, tHatCenter, error);
|
||
|
|
V2Negate(&tHatCenter);
|
||
|
|
FitCubic(d, splitPoint, last, tHatCenter, tHat2, error);
|
||
|
|
|
||
|
|
}
|
||
|
|
|