diff options
Diffstat (limited to 'source/blender/freestyle/intern/geometry/FitCurve.cpp')
| -rwxr-xr-x | source/blender/freestyle/intern/geometry/FitCurve.cpp | 603 |
1 files changed, 603 insertions, 0 deletions
diff --git a/source/blender/freestyle/intern/geometry/FitCurve.cpp b/source/blender/freestyle/intern/geometry/FitCurve.cpp new file mode 100755 index 00000000000..7754fcda32b --- /dev/null +++ b/source/blender/freestyle/intern/geometry/FitCurve.cpp @@ -0,0 +1,603 @@ + +// +// 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 <cstdlib> // for malloc and free +#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); + +} + |