blender-archive/source/blender/freestyle/intern/geometry/FitCurve.cpp

//
//  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);

}