/*
* FitCubic :
* Fit a Bezier curve to a (sub)set of digitized points
*
* Vec2f *d Array of digitized points
* int first, last Indices of first and last pts in region
* Vec2f tHat1, tHat2 Unit tangent vectors at endpoints
* double error User-defined error squared
*/
void PathFitter::FitCubic(vector<Vec2f> const &d, vector<BezierCurve> *bezCurves, int first, int last, Vec2f tHat1, Vec2f tHat2, double error)
{
vector<Vec2f> bezCurve(4); // Control points of fitted Bezier curve
vector<double> *u = new vector<double>(last-first+1); // Parameter values for point
vector<double> *uPrime = new vector<double>(last-first+1); // 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
Vec2f 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) {
float dist = d[last].distance(d[first]) / 3.0;
bezCurve[0] = d[first];
bezCurve[3] = d[last];
bezCurve[1] = bezCurve[0] + v2Scale(tHat1, dist);
bezCurve[2] = bezCurve[3] + v2Scale(tHat2, dist);
addBezierCurve(bezCurve, bezCurves);
return;
}
// Parameterize points, and attempt to fit curve
chordLengthParameterize(d, first, last, u);
generateBezier(d, &bezCurve, first, last, *u, tHat1, tHat2);
// Find max deviation of points to fitted curve
maxError = computeMaxError(d, first, last, &bezCurve, *u, &splitPoint);
if (maxError < error) {
addBezierCurve(bezCurve, bezCurves);
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);
generateBezier(d, &bezCurve, first, last, *uPrime, tHat1, tHat2);
maxError = computeMaxError(d, first, last, &bezCurve, *uPrime, &splitPoint);
if (maxError < error) {
addBezierCurve(bezCurve, bezCurves);
return;
}
u = uPrime;
}
}
// Fitting failed -- split at max error point and fit recursively
tHatCenter = computeCenterTangent(d, splitPoint);
FitCubic(d, bezCurves, first, splitPoint, tHat1, tHatCenter, error);
FitCubic(d, bezCurves, splitPoint, last, tHatCenter.inverse(), tHat2, error);
}
