58 Point const &tHat1,
Point const &tHat2,
double tolerance_sq);
71 Point const bezCurve[],
double tolerance,
72 unsigned *splitPoint);
74 double const tolerance);
84#define B0(u) ( ( 1.0 - u ) * ( 1.0 - u ) * ( 1.0 - u ) )
85#define B1(u) ( 3 * u * ( 1.0 - u ) * ( 1.0 - u ) )
86#define B2(u) ( 3 * u * u * ( 1.0 - u ) )
87#define B3(u) ( u * u * u )
90# define DOUBLE_ASSERT(x) assert( ( (x) > -SP_HUGE ) && ( (x) < SP_HUGE ) )
91# define BEZIER_ASSERT(b) do { \
92 DOUBLE_ASSERT((b)[0][X]); DOUBLE_ASSERT((b)[0][Y]); \
93 DOUBLE_ASSERT((b)[1][X]); DOUBLE_ASSERT((b)[1][Y]); \
94 DOUBLE_ASSERT((b)[2][X]); DOUBLE_ASSERT((b)[2][Y]); \
95 DOUBLE_ASSERT((b)[3][X]); DOUBLE_ASSERT((b)[3][Y]); \
98# define DOUBLE_ASSERT(x) do { } while(0)
99# define BEZIER_ASSERT(b) do { } while(0)
129 max_beziers >= (1ul << (31 - 2 - 1 - 3)))
135 if ( uniqued_len < 2 ) {
136 delete[] uniqued_data;
144 delete[] uniqued_data;
158 if ( si == src_len ) {
161 if (!std::isnan(src[si][
X]) &&
162 !std::isnan(src[si][
Y])) {
163 dest[0] =
Point(src[si]);
170 for (; si < src_len; ++si) {
172 if ( src_pt != dest[di]
173 && !std::isnan(src_pt[
X])
174 && !std::isnan(src_pt[
Y])) {
178 unsigned dest_len = di + 1;
179 assert( dest_len <= src_len );
195 double const error,
unsigned const max_beziers)
197 if(!(bezier != NULL) ||
200 !(max_beziers >= 1) ||
204 if (
len < 2 )
return 0;
210 double const dist =
distance(bezier[0], bezier[3]) / 3.0;
211 if (std::isnan(
dist)) {
213 bezier[1] = bezier[0];
214 bezier[2] = bezier[3];
217 ? ( 2 * bezier[0] + bezier[3] ) / 3.
218 : bezier[0] +
dist * tHat1 );
220 ? ( bezier[0] + 2 * bezier[3] ) / 3.
221 : bezier[3] +
dist * tHat2 );
223 BEZIER_ASSERT(bezier);
231 double *u =
new double[
len];
233 if ( u[
len - 1] == 0.0 ) {
247 double const tolerance =
sqrt(error + 1e-9);
250 if ( fabs(maxErrorRatio) <= 1.0 ) {
251 BEZIER_ASSERT(bezier);
257 if ( 0.0 <= maxErrorRatio && maxErrorRatio <= 3.0 ) {
258 int const maxIterations = 4;
259 for (
int i = 0; i < maxIterations; i++) {
263 if ( fabs(maxErrorRatio) <= 1.0 ) {
264 BEZIER_ASSERT(bezier);
271 is_corner = (maxErrorRatio < 0);
275 assert(splitPoint <
unsigned(
len));
276 if (splitPoint == 0) {
284 }
else if (splitPoint ==
unsigned(
len - 1)) {
295 if ( 1 < max_beziers ) {
299 unsigned const rec_max_beziers1 = max_beziers - 1;
301 Point recTHat2, recTHat1;
303 if(!(0 < splitPoint && splitPoint <
unsigned(
len - 1)))
309 recTHat1 = -recTHat2;
312 tHat1, recTHat2, error, rec_max_beziers1);
315 g_print(
"fit_cubic[1]: recursive call failed\n");
319 assert( nsegs1 != 0 );
320 if (split_points != NULL) {
321 split_points[nsegs1 - 1] = splitPoint;
323 unsigned const rec_max_beziers2 = max_beziers - nsegs1;
325 ( split_points == NULL
327 : split_points + nsegs1 ),
328 data + splitPoint,
len - splitPoint,
329 recTHat1, tHat2, error, rec_max_beziers2);
332 g_print(
"fit_cubic[2]: recursive call failed\n");
338 g_print(
"fit_cubic: success[nsegs: %d+%d=%d] on max_beziers:%u\n",
339 nsegs1, nsegs2, nsegs1 + nsegs2, max_beziers);
341 return nsegs1 + nsegs2;
361 Point const data[],
double const u[],
unsigned const len,
363 double const tolerance_sq)
365 bool const est1 =
is_zero(tHat1);
366 bool const est2 =
is_zero(tHat2);
367 Point est_tHat1( est1
370 Point est_tHat2( est2
378 if (bezier[1] != bezier[0]) {
388 Point const data[],
double const uPrime[],
unsigned const len,
407 for (
unsigned i = 0; i <
len; i++) {
409 double const b0 = B0(uPrime[i]);
410 double const b1 = B1(uPrime[i]);
411 double const b2 = B2(uPrime[i]);
412 double const b3 = B3(uPrime[i]);
415 Point const a1 = b1 * tHat1;
416 Point const a2 = b2 * tHat2;
418 C[0][0] +=
dot(a1, a1);
419 C[0][1] +=
dot(a1, a2);
421 C[1][1] +=
dot(a2, a2);
425 Point const shortfall
427 - ( ( b0 + b1 ) * bezier[0] )
428 - ( ( b2 + b3 ) * bezier[3] ) );
429 X[0] +=
dot(a1, shortfall);
430 X[1] +=
dot(a2, shortfall);
435 double alpha_l, alpha_r;
438 double const det_C0_C1 = C[0][0] * C[1][1] - C[1][0] * C[0][1];
439 if ( det_C0_C1 != 0 ) {
441 double const det_C0_X = C[0][0] *
X[1] - C[0][1] *
X[0];
442 double const det_X_C1 =
X[0] * C[1][1] -
X[1] * C[0][1];
443 alpha_l = det_X_C1 / det_C0_C1;
444 alpha_r = det_C0_X / det_C0_C1;
454 double const c0 = C[0][0] + C[0][1];
456 alpha_l = alpha_r =
X[0] / c0;
458 double const c1 = C[1][0] + C[1][1];
460 alpha_l = alpha_r =
X[1] / c1;
463 alpha_l = alpha_r = 0.;
473 if ( alpha_l < 1.0e-6 ||
481 bezier[1] = alpha_l * tHat1 + bezier[0];
482 bezier[2] = alpha_r * tHat2 + bezier[3];
493 Point const data[],
double const u[],
unsigned const len)
495 if(!(1 <= ei && ei <= 2))
497 unsigned const oi = 3 - ei;
498 double num[2] = {0., 0.};
500 for (
unsigned i = 0; i <
len; ++i) {
501 double const ui = u[i];
502 double const b[4] = {
509 for (
unsigned d = 0; d < 2; ++d) {
510 num[d] += b[ei] * (b[0] * bezier[0][d] +
511 b[oi] * bezier[oi][d] +
512 b[3] * bezier[3][d] +
515 den -= b[ei] * b[ei];
519 for (
unsigned d = 0; d < 2; ++d) {
520 bezier[ei][d] =
num[d] / den;
523 bezier[ei] = ( oi * bezier[0] + ei * bezier[3] ) / 3.;
541 Point const bezCurve[])
545 unsigned const last =
len - 1;
546 assert( bezCurve[0] == d[0] );
547 assert( bezCurve[3] == d[last] );
548 assert( u[0] == 0.0 );
549 assert( u[last] == 1.0 );
552 for (
unsigned i = 1; i < last; i++) {
574 for (
unsigned i = 0; i < 3; i++) {
575 Q1[i] = 3.0 * ( Q[i+1] - Q[i] );
580 for (
unsigned i = 0; i < 2; i++) {
581 Q2[i] = 2.0 * ( Q1[i+1] - Q1[i] );
593 Point const diff = Q_u - P;
594 double numerator =
dot(diff, Q1_u);
595 double denominator =
dot(Q1_u, Q1_u) +
dot(diff, Q2_u);
598 if ( denominator > 0. ) {
601 improved_u = u - ( numerator / denominator );
605 if ( numerator > 0. ) {
606 improved_u = u * .98 - .01;
607 }
else if ( numerator < 0. ) {
609 improved_u = .031 + u * .98;
615 if (!std::isfinite(improved_u)) {
617 }
else if ( improved_u < 0.0 ) {
619 }
else if ( improved_u > 1.0 ) {
625 double const diff_lensq =
lensq(diff);
626 for (
double proportion = .125; ; proportion += .125) {
628 if ( proportion > 1.0 ) {
633 improved_u = ( ( 1 - proportion ) * improved_u +
641 DOUBLE_ASSERT(improved_u);
669 static int const pascal[4][4] = {{1, 0, 0, 0},
674 double const s = 1.0 - t;
679 spow[0] = 1.0; spow[1] = s;
680 tpow[0] = 1.0; tpow[1] = t;
681 for (
unsigned i = 1; i <
degree; ++i) {
682 spow[i + 1] = spow[i] * s;
683 tpow[i + 1] = tpow[i] * t;
687 for (
unsigned i = 1; i <=
degree; ++i) {
688 ret += pascal[
degree][i] * spow[
degree - i] * tpow[i] * V[i];
709 assert( d[0] != d[1] );
727 unsigned const last =
len - 1;
728 unsigned const prev = last - 1;
729 assert( d[last] != d[prev] );
748 assert( 0 <= tolerance_sq );
749 for (
unsigned i = 1;;) {
750 Point const pi(d[i]);
751 Point const t(pi - d[0]);
752 double const distsq =
dot(t, t);
753 if ( tolerance_sq < distsq ) {
779 assert( 0 <= tolerance_sq );
780 unsigned const last =
len - 1;
781 for (
unsigned i = last - 1;; i--) {
782 Point const pi(d[i]);
783 Point const t(pi - d[last]);
784 double const distsq =
dot(t, t);
785 if ( tolerance_sq < distsq ) {
808 unsigned const center,
811 assert( center != 0 );
812 assert( center <
len - 1 );
815 if ( d[center + 1] == d[center - 1] ) {
817 Point const diff = d[center] - d[center - 1];
820 ret = d[center - 1] - d[center + 1];
840 for (
unsigned i = 1; i <
len; i++) {
842 u[i] = u[i-1] +
dist;
846 double tot_len = u[
len - 1];
847 if(!( tot_len != 0 ))
849 if (std::isfinite(tot_len)) {
850 for (
unsigned i = 1; i <
len; ++i) {
855 for (
unsigned i = 1; i <
len; ++i) {
856 u[i] = i / (double) (
len - 1 );
865 if (u[
len - 1] != 1) {
866 double const diff = u[
len - 1] - 1;
867 if (fabs(diff) > 1e-13) {
876 assert( u[0] == 0.0 && u[
len - 1] == 1.0 );
877 for (
unsigned i = 1; i <
len; i++) {
878 assert( u[i] >= u[i-1] );
899 Point const bezCurve[],
double const tolerance,
900 unsigned *
const splitPoint)
903 unsigned const last =
len - 1;
904 assert( bezCurve[0] == d[0] );
905 assert( bezCurve[3] == d[last] );
906 assert( u[0] == 0.0 );
907 assert( u[last] == 1.0 );
913 double maxDistsq = 0.0;
914 double max_hook_ratio = 0.0;
915 unsigned snap_end = 0;
916 Point prev = bezCurve[0];
917 for (
unsigned i = 1; i <= last; i++) {
919 double const distsq =
lensq( curr - d[i] );
920 if ( distsq > maxDistsq ) {
924 double const hook_ratio =
compute_hook(prev, curr, .5 * (u[i - 1] + u[i]), bezCurve, tolerance);
925 if (max_hook_ratio < hook_ratio) {
926 max_hook_ratio = hook_ratio;
932 double const dist_ratio =
sqrt(maxDistsq) / tolerance;
934 if (max_hook_ratio <= dist_ratio) {
937 assert(0 < snap_end);
938 ret = -max_hook_ratio;
939 *splitPoint = snap_end - 1;
942 || ( ( *splitPoint < last )
943 && ( *splitPoint != 0 || ret < 0. ) ) );
970 double const tolerance)
974 if (
dist < tolerance) {
977 double const allowed =
distance(a, b) + tolerance;
978 return dist / allowed;
Bezier fitting algorithms.
Two-dimensional point that doubles as a vector.
void normalize()
Normalize the vector representing the point.
Low level math functions and compatibility wrappers.
double dist(const Point &a, const Point &b)
Various utility functions.
int bezier_fit_cubic(Point bezier[], Point const data[], int len, double error)
Coord length(LineSegment const &seg)
static unsigned copy_without_nans_or_adjacent_duplicates(Point const src[], unsigned src_len, Point dest[])
Copy points from src to dest, filter out points containing NaN and adjacent points with equal x and y...
static void chord_length_parameterize(Point const d[], double u[], unsigned len)
Assign parameter values to digitized points using relative distances between points.
static double lensq(Point const p)
static double compute_max_error_ratio(Point const d[], double const u[], unsigned len, Point const bezCurve[], double tolerance, unsigned *splitPoint)
Find the maximum squared distance of digitized points to fitted curve, and (if this maximum error is ...
int bezier_fit_cubic_full(Point bezier[], int split_points[], Point const data[], int len, Point const &tHat1, Point const &tHat2, double error, unsigned max_beziers)
Fit a multi-segment Bezier curve to a set of digitized points, without possible weedout of identical ...
Angle distance(Angle const &a, Angle const &b)
static double NewtonRaphsonRootFind(Point const Q[], Point const &P, double u)
Use Newton-Raphson iteration to find better root.
static Point const unconstrained_tangent(0, 0)
bool is_zero(Point const &p)
SBasisN< n > sqrt(SBasisN< n > const &a, int k)
static void estimate_bi(Point b[4], unsigned ei, Point const data[], double const u[], unsigned len)
int bezier_fit_cubic_r(Point bezier[], Point const data[], int len, double error, unsigned max_beziers)
Fit a multi-segment Bezier curve to a set of digitized points, with possible weedout of identical poi...
static void generate_bezier(Point b[], Point const d[], double const u[], unsigned len, Point const &tHat1, Point const &tHat2, double tolerance_sq)
Fill in bezier[] based on the given data and tangent requirements, using a least-squares fit.
static void estimate_lengths(Point bezier[], Point const data[], double const u[], unsigned len, Point const &tHat1, Point const &tHat2)
Point darray_left_tangent(Point const d[], unsigned const len)
Estimate the (forward) tangent at point d[first + 0.5].
static double compute_hook(Point const &a, Point const &b, double const u, Point const bezCurve[], double const tolerance)
Whereas compute_max_error_ratio() checks for itself that each data point is near some point on the cu...
static void reparameterize(Point const d[], unsigned len, double u[], Point const bezCurve[])
Given set of points and their parameterization, try to find a better assignment of parameter values f...
Point darray_right_tangent(Point const d[], unsigned const length, double const tolerance_sq)
Estimates the (backward) tangent at d[last].
static Point darray_center_tangent(Point const d[], unsigned center, unsigned length)
Estimates the (backward) tangent at d[center], by averaging the two segments connected to d[center] (...
Point bezier_pt(unsigned degree, Point const V[], double t)
Evaluate a Bezier curve at parameter value t.
T dot(D2< T > const &a, D2< T > const &b)
Point unit_vector(Point const &a)
D2< T > rot90(D2< T > const &a)
pair< double, double > Point