/*
* ComputeMaxError :
* Find the maximum squared distance of digitized points
* to fitted curve.
*/
public double ComputeMaxError(
Vector2[] d, int first, int last, Vector2[] bezCurve, double[] u, int[] splitPoint)
/* d; Array of digitized points */
/* first, last; Indices defining region */
/* BezierCurve bezCurve Fitted Bezier curve */
/* u; Parameterization of points */
/* 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 */
// System.out.println("ENTERING MAX ERROR"+splitPoint[0]);
splitPoint[0] = (last - first + 1) / 2;
/* *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 = v.V2SquaredLength();
/* dist = V2SquaredLength(&v); */
if (dist >= maxDist) {
maxDist = dist;
/* *splitPoint = i; */
splitPoint[0] = i;
}
}
// System.out.println("EXITING MAX ERROR"+splitPoint[0]);
// System.out.println("splitPoint "+splitPoint[0]+" and maxDist "+maxDist);
return (maxDist);
}
public Vector2 V2ScaleIII(Vector2 a, double s) {
Vector2 result = new Vector2();
result.xset(s * a.xval());
result.yset(s * a.yval());
return (result);
}
public Vector2 ComputeRightTangent(Vector2[] d, int end)
/* d; Digitized points */
/* end; Index to "right" end of region */
{
Vector2 tHat2;
tHat2 = V2SubII(d[end - 1], d[end]);
// tHat2 = V2Normalize(tHat2);
tHat2.V2Normalize();
// System.out.println("IN RIGHT TANGENT:"+tHat2.xval()+" "+tHat2.yval() );
return tHat2;
}
public Vector2 V2Scale(Vector2 v, double newlen) {
double len = v.V2Length();
if (len != 0.0) {
double temp;
temp = v.xval();
v.xset(temp * newlen / len);
temp = v.yval();
v.yset(temp * newlen / len);
}
return (v);
}
/*
* ComputeLeftTangent, ComputeRightTangent, ComputeCenterTangent :
*Approximate unit tangents at endpoints and "center" of digitized curve
*/
public Vector2 ComputeLeftTangent(Vector2[] d, int end)
/* d; Digitized points*/
/* end; Index to "left" end of region */
{
Vector2 tHat1;
tHat1 = V2SubII(d[end + 1], d[end]);
// tHat1 = V2Normalize(tHat1);
// System.out.println("end "+end+" d[end+1] "+d[end+1].xval()+" "+d[end+1].yval()+" d[end]
// "+d[end].xval()+" "+d[end].yval()+" IN LEFT TANGENT:"+tHat1.xval()+" "+tHat1.yval() );
tHat1.V2Normalize();
// System.out.println("IN LEFT TANGENT:"+tHat1.xval()+" "+tHat1.yval() );
return tHat1;
}
public Vector2 ComputeCenterTangent(Vector2[] d, int center)
/* d; Digitized points */
/*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 = new Vector2();
tHatCenter.xset((V1.xval() + V2.xval()) / 2.0);
tHatCenter.yset((V1.yval() + V2.yval()) / 2.0);
tHatCenter.V2Normalize();
return tHatCenter;
}
public Vector2 V2AddII(Vector2 a, Vector2 b) {
Vector2 c = new Vector2();
c.xset(a.xval() + b.xval());
c.yset(a.yval() + b.yval());
return (c);
}
/* return vector sum c = a+b */
public Vector2 V2Add(Vector2 a, Vector2 b, Vector2 c) {
// c = new Vector2();
c.xset(a.xval() + b.xval());
c.yset(a.yval() + b.yval());
// System.out.println("V2Add: "+a.xval()+" + "+b.xval()+" = "+c.xval() );
// System.out.println("V2Add: "+a.yval()+" + "+b.yval()+" = "+c.yval()+"\n");
return (c);
}
/* return the distance between two points */
public double V2DistanceBetween2Points(Vector2 a, Vector2 b) {
double dx = a.xval() - b.xval();
double dy = a.yval() - b.yval();
// System.out.println("Distance: "+Math.sqrt((dx*dx)+(dy*dy))+"\n\n");
return (Math.sqrt((dx * dx) + (dy * dy)));
}
/* return the dot product of vectors a and b */
public double V2Dot(Vector2 a, Vector2 b) {
return (a.xval() * b.xval() + a.yval() * b.yval());
}
/*
* GenerateBezier :
* Use least-squares method to find Bezier control points for region.
*
*/
public Vector2[] GenerateBezier(
Vector2[] d, int first, int last, double uPrime[], Vector2 tHat1, Vector2 tHat2)
/* d: Array of digitized points */
/* first last: Indices defining region */
/* uPrime: Parameter values for region */
/* tHat1, tHat2: Unit tangents at endpoints */
{
int i;
double C[][] = new double[2][2]; /* Matrix C */
double[] X = new double[2]; /* Matrix X */
Vector2 A[][]; /* Precomputed rhs for eqn */
int nPts; /* Number of pts in sub-curve */
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 */
Vector2[] bezCurve = new Vector2[4]; /* RETURN bezier curve ctl pts */
// System.out.println("ENTERING GENERATE BEZIER "+tHat1.xval()+" "+tHat1.yval()+"
// "+tHat2.xval()+" "+tHat2.yval());
for (i = 0; i < 4; i++) bezCurve[i] = new Vector2();
/* bezCurve = (Vector2 *)malloc(4 * sizeof(Vector2)); */
nPts = last - first + 1;
A = new Vector2[nPts][2];
/* Compute the A's */
for (i = 0; i < nPts; i++) {
Vector2 v1 = new Vector2();
Vector2 v2 = new Vector2();
v1.xset(tHat1.xval());
v1.yset(tHat1.yval());
v2.xset(tHat2.xval());
v2.yset(tHat2.yval());
// System.out.println("PT: "+i);
// System.out.print("that1 length "+tHat1.V2Length()+" "+tHat1.xval()+" "+tHat1.yval() );
// System.out.print("that2 length "+tHat2.V2Length()+" "+tHat2.xval()+" "+tHat2.yval() );
// System.out.print("Before: "+v1.xval() +" "+v1.yval() );
V2Scale(v1, B1(uPrime[i]));
// System.out.println("after : "+v1.xval() +" "+v1.yval() );
// System.out.print("Before: "+v2.xval() +" "+v2.yval() );
V2Scale(v2, B2(uPrime[i]));
// System.out.println("after : "+v2.xval() +" "+v2.yval() );
A[i][0] = new Vector2();
A[i][0].xset(v1.xval());
A[i][0].yset(v1.yval());
A[i][1] = new Vector2();
A[i][1].xset(v2.xval());
A[i][1].yset(v2.yval());
// System.out.println("A[i][0] "+A[i][0].xval()+" "+A[i][0].yval());
// System.out.println("A[i][1] "+A[i][1].xval()+" "+A[i][1].yval());
// System.out.println("B1 "+B1(uPrime[i])+" B2 "+B2(uPrime[i]));
}
/* 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);
}
System.out.println("C " + C[0][0] + " " + C[0][1] + " " + C[1][0] + " " + C[1][1]);
System.out.println("X " + X[0] + " " + X[1]);
/* 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;
// System.out.println("\n\n\nalpha_l "+alpha_l+" alpha_r "+alpha_r);
/* If alpha negative, use the Wu/Barsky heuristic (see text) */
if (alpha_l < 0.0 || alpha_r < 0.0) {
int ddd;
double dist = V2DistanceBetween2Points(d[last], d[first]) / 3.0;
for (ddd = 0; ddd < 4; ddd++) bezCurve[ddd] = new Vector2();
bezCurve[0] = d[first];
bezCurve[3] = d[last];
// System.out.println("HERE!!!!");
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] = new Vector2();
bezCurve[1] = new Vector2();
bezCurve[2] = new Vector2();
bezCurve[3] = new Vector2();
bezCurve[0].xset(d[first].xval());
bezCurve[0].yset(d[first].yval());
bezCurve[3].xset(d[last].xval());
bezCurve[3].yset(d[last].yval());
// System.out.println("EDW??? 1");
V2Add(bezCurve[0], V2Scale(tHat1, alpha_l), bezCurve[1]);
// System.out.println("EDW??? 2");
V2Add(bezCurve[3], V2Scale(tHat2, alpha_r), bezCurve[2]);
// System.out.println("BEZ_CURVE is "+bezCurve[0].xval()+" "+bezCurve[0].yval()+" and
// "+bezCurve[3].xval()+" "+bezCurve[3].yval());
// System.out.println("BEZ_CURVE is "+bezCurve[1].xval()+" "+bezCurve[1].yval()+" and
// "+bezCurve[2].xval()+" "+bezCurve[2].yval());
// System.out.println("EXITING GENERATE BEZIER");
return (bezCurve);
}
/*
* FitCubic :
* Fit a Bezier curve to a (sub)set of digitized points
*/
public void FitCubic(
Vector2[] d, int first, int last, Vector2 tHat1, Vector2 tHat2, double error, PrintStream ps)
// Vector2 tHat1, Vector2 tHat2, double error, PrintStream ps, int i1, int j1)
/* d; Array of digitized points */
/* first, last; Indices of first and last pts in region */
/* tHat1, tHat2; Unit tangent vectors at endpoints */
/* error; User-defined error squared */
/* FILE *fp; */
{
Vector2[] 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;
splitPoint = nPts; /* MAKE SURE THIS IS CORRECT!! */
// printf("Error is %f (nPts = %d -- last %d first %d)\n", error, nPts, last, f irst);
// System.out.println("Error is "+error+" (nPts = "+ nPts+" -- last "+last+" first "+first+")");
/* Use heuristic if region only has two points in it */
if (nPts == 2) {
double dist = V2DistanceBetween2Points(d[last], d[first]) / 3.0;
bezCurve = new Vector2[4];
for (i = 0; i < 4; i++) bezCurve[i] = new Vector2();
/* bezCurve = (Point2 *)malloc(4 * sizeof(Point2)); */
bezCurve[0] = d[first];
bezCurve[3] = d[last];
// System.out.println("BEZ CURVE is "+bezCurve[0].xval()+" "+bezCurve[0].yval()+" and
// "+bezCurve[3].xval()+" "+bezCurve[3].yval());
// System.out.println("BEZ CURVE is "+bezCurve[1].xval()+" "+bezCurve[1].yval()+" and
// "+bezCurve[2].xval()+" "+bezCurve[2].yval());
V2Add(bezCurve[0], V2Scale(tHat1, dist), bezCurve[1]);
V2Add(bezCurve[3], V2Scale(tHat2, dist), bezCurve[2]);
// DrawBezierCurve(3, bezCurve, fp);
// DrawBezierCurve(3, bezCurve, ps, i1, j1);
DrawBezierCurve(3, bezCurve, ps);
/* printf("Returning from nPts=2\n"); */
return;
}
/* Parameterize points, and attempt to fit curve */
u = ChordLengthParameterize(d, first, last);
// for (i=1;i<5;i++) System.out.print(" "+u[i]);
// System.out.print("\n");
bezCurve = GenerateBezier(d, first, last, u, tHat1, tHat2);
// printf("Value %4.3f\n", sizeof(bezCurve)/sizeof(Point2));
/* Find max deviation of points to fitted curve */
int[] split = new int[1];
// System.out.println("BEFORE: SPLITPOINT "+splitPoint);
split[0] = splitPoint;
// maxError=1;
maxError = ComputeMaxError(d, first, last, bezCurve, u, split);
splitPoint = split[0];
// System.out.println("AFTER: SPLITPOINT "+splitPoint);
if (maxError < error) {
// DrawBezierCurve(3, bezCurve, fp);
// DrawBezierCurve(3, bezCurve, ps, i1, j1);
DrawBezierCurve(3, bezCurve, ps);
// free((void *)u);
// free(u);
// free((void *)bezCurve);
// free(bezCurve);
// printf("Returning from maxerror %f\n", maxError);
return;
}
/* If error not too large, try some reparameterization */
/* and iteration */
if (maxError < iterationError) {
// printf("Returning from maxerror %f and iteration error %f\n", maxError, it erationError);
for (i = 0; i < maxIterations; i++) {
/* int [] split = new int[1]; */
split[0] = splitPoint;
uPrime = Reparameterize(d, first, last, u, bezCurve);
bezCurve = GenerateBezier(d, first, last, uPrime, tHat1, tHat2);
maxError = ComputeMaxError(d, first, last, bezCurve, uPrime, split);
splitPoint = split[0];
if (maxError < error) {
// DrawBezierCurve(3, bezCurve, fp);
// DrawBezierCurve(3, bezCurve, ps, i1, j1);
DrawBezierCurve(3, bezCurve, ps);
// free((void *)u);
// free(u);
// free((void *)bezCurve);
// free(bezCurve);
return;
}
// free((void *)u);
// free(u);
u = uPrime;
}
}
/* Fitting failed -- split at max error point and fit recursively */
// free((void *)u);
// free(u);
// free((void *)bezCurve);
// free(bezCurve);
tHatCenter = ComputeCenterTangent(d, splitPoint);
// FitCubic(d, first, splitPoint, tHat1, tHatCenter, error, fp);
// FitCubic(d, first, splitPoint, tHat1, tHatCenter, error, ps,i1,j1);
// FitCubic(d, first, splitPoint, tHat1, tHatCenter, error, ps);
// V2Negate(&tHatCenter);
tHatCenter.V2Negate();
// FitCubic(d, splitPoint, last, tHatCenter, tHat2, error, fp);
// FitCubic(d, splitPoint, last, tHatCenter, tHat2, error, ps,i1,j1);
// FitCubic(d, splitPoint, last, tHatCenter, tHat2, error, ps);
// System.out.println("EXITING FITCUBIC");
}
/*
* NewtonRaphsonRootFind :
* Use Newton-Raphson iteration to find better root.
*/
public double NewtonRaphsonRootFind(Vector2[] Q, Vector2 P, double u)
/* Q; Current fitted curve */
/* P; Digitized point */
/* u; Parameter value for "P" */
{
double numerator, denominator;
Vector2[] Q1 = new Vector2[3];
Vector2[] Q2 = new Vector2[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++) {
double x1 = Q[i + 1].xval() - Q[i].xval();
double y1 = Q[i + 1].yval() - Q[i].yval();
Q1[i] = new Vector2();
Q1[i].xset(x1 * 3.0);
Q1[i].yset(y1 * 3.0);
}
/* Generate control vertices for Q'' */
for (i = 0; i <= 1; i++) {
double x1 = Q[i + 1].xval() - Q[i].xval();
double y1 = Q[i + 1].yval() - Q[i].yval();
Q2[i] = new Vector2();
Q2[i].xset(x1 * 2.0);
Q2[i].yset(y1 * 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.xval() - P.xval()) * (Q1_u.xval()) + (Q_u.yval() - P.yval()) * (Q1_u.yval());
denominator =
(Q1_u.xval()) * (Q1_u.xval())
+ (Q1_u.yval()) * (Q1_u.yval())
+ (Q_u.xval() - P.xval()) * (Q2_u.xval())
+ (Q_u.yval() - P.yval()) * (Q2_u.yval());
/* u = u - f(u)/f'(u) */
uPrime = u - (numerator / denominator);
return (uPrime);
}