summaryrefslogtreecommitdiffstats
path: root/src/3rdparty/2geom/src/toys/pencil-2.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/3rdparty/2geom/src/toys/pencil-2.cpp')
-rw-r--r--src/3rdparty/2geom/src/toys/pencil-2.cpp1133
1 files changed, 1133 insertions, 0 deletions
diff --git a/src/3rdparty/2geom/src/toys/pencil-2.cpp b/src/3rdparty/2geom/src/toys/pencil-2.cpp
new file mode 100644
index 0000000..a312083
--- /dev/null
+++ b/src/3rdparty/2geom/src/toys/pencil-2.cpp
@@ -0,0 +1,1133 @@
+/*
+ * pencil-2 Toy - point fitting.
+ *
+ * 2009 njh
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it either under the terms of the GNU Lesser General Public
+ * License version 2.1 as published by the Free Software Foundation
+ * (the "LGPL") or, at your option, under the terms of the Mozilla
+ * Public License Version 1.1 (the "MPL"). If you do not alter this
+ * notice, a recipient may use your version of this file under either
+ * the MPL or the LGPL.
+ *
+ * You should have received a copy of the LGPL along with this library
+ * in the file COPYING-LGPL-2.1; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ * You should have received a copy of the MPL along with this library
+ * in the file COPYING-MPL-1.1
+ *
+ * The contents of this file are subject to the Mozilla Public License
+ * Version 1.1 (the "License"); you may not use this file except in
+ * compliance with the License. You may obtain a copy of the License at
+ * http://www.mozilla.org/MPL/
+ *
+ * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY
+ * OF ANY KIND, either express or implied. See the LGPL or the MPL for
+ * the specific language governing rights and limitations.
+ *
+ */
+
+#include <2geom/d2.h>
+#include <2geom/sbasis.h>
+#include <2geom/sbasis-geometric.h>
+#include <2geom/basic-intersection.h>
+#include <2geom/math-utils.h>
+
+#include <toys/path-cairo.h>
+#include <toys/toy-framework-2.h>
+
+#define SP_HUGE 1e5
+#define noBEZIER_DEBUG
+
+#ifdef HAVE_IEEEFP_H
+# include <ieeefp.h>
+#endif
+
+namespace Geom{
+
+namespace BezierFitter{
+
+typedef Point BezierCurve[];
+
+/* Forward declarations */
+static void generate_bezier(Point b[], Point const d[], double const u[], unsigned len,
+ Point const &tHat1, Point const &tHat2, double tolerance_sq);
+static void estimate_lengths(Point bezier[],
+ Point const data[], double const u[], unsigned len,
+ Point const &tHat1, Point const &tHat2);
+static void estimate_bi(Point b[4], unsigned ei,
+ Point const data[], double const u[], unsigned len);
+static void reparameterize(Point const d[], unsigned len, double u[], CubicBezier const & bezCurve);
+static double NewtonRaphsonRootFind(CubicBezier const & Q, Point const &P, double u);
+static Point darray_center_tangent(Point const d[], unsigned center, unsigned length);
+static Point darray_right_tangent(Point const d[], unsigned const len);
+static unsigned copy_without_nans_or_adjacent_duplicates(Point const src[], unsigned src_len, Point dest[]);
+static void chord_length_parameterize(Point const d[], double u[], unsigned len);
+static double compute_max_error_ratio(Point const d[], double const u[], unsigned len,
+ BezierCurve const bezCurve, double tolerance,
+ unsigned *splitPoint);
+static double compute_hook(Point const &a, Point const &b, double const u,
+ CubicBezier const & bezCurve,
+ double const tolerance);
+static double compute_hook(Point const &a, Point const &b, double const u,
+ BezierCurve const bezCurve,
+ double const tolerance) {
+ CubicBezier cb(bezCurve[0], bezCurve[1], bezCurve[2], bezCurve[3]);
+ return compute_hook(a, b, u, cb, tolerance);
+
+}
+
+
+static void reparameterize_pts(Point const d[], unsigned len, double u[], BezierCurve const bezCurve) {
+ CubicBezier cb(bezCurve[0], bezCurve[1], bezCurve[2], bezCurve[3]);
+ reparameterize(d, len, u, cb);
+}
+
+
+
+static Point const unconstrained_tangent(0, 0);
+
+
+/*
+ * B0, B1, B2, B3 : Bezier multipliers
+ */
+
+#define B0(u) ( ( 1.0 - u ) * ( 1.0 - u ) * ( 1.0 - u ) )
+#define B1(u) ( 3 * u * ( 1.0 - u ) * ( 1.0 - u ) )
+#define B2(u) ( 3 * u * u * ( 1.0 - u ) )
+#define B3(u) ( u * u * u )
+
+#ifdef BEZIER_DEBUG
+# define DOUBLE_ASSERT(x) assert( ( (x) > -SP_HUGE ) && ( (x) < SP_HUGE ) )
+# define BEZIER_ASSERT(b) do { \
+ DOUBLE_ASSERT((b)[0][X]); DOUBLE_ASSERT((b)[0][Y]); \
+ DOUBLE_ASSERT((b)[1][X]); DOUBLE_ASSERT((b)[1][Y]); \
+ DOUBLE_ASSERT((b)[2][X]); DOUBLE_ASSERT((b)[2][Y]); \
+ DOUBLE_ASSERT((b)[3][X]); DOUBLE_ASSERT((b)[3][Y]); \
+ } while(0)
+#else
+# define DOUBLE_ASSERT(x) do { } while(0)
+# define BEZIER_ASSERT(b) do { } while(0)
+#endif
+
+
+Point
+bezier_pt(unsigned const degree, Point const V[], double const t)
+{
+ return bernstein_value_at(t, V, degree);
+
+}
+
+/*
+ * ComputeLeftTangent, ComputeRightTangent, ComputeCenterTangent :
+ * Approximate unit tangents at endpoints and "center" of digitized curve
+ */
+
+/**
+ * Estimate the (forward) tangent at point d[first + 0.5].
+ *
+ * Unlike the center and right versions, this calculates the tangent in
+ * the way one might expect, i.e., wrt increasing index into d.
+ * \pre (2 \<= len) and (d[0] != d[1]).
+ **/
+Point
+darray_left_tangent(Point const d[], unsigned const len)
+{
+ assert( len >= 2 );
+ assert( d[0] != d[1] );
+ return unit_vector( d[1] - d[0] );
+}
+
+/**
+ * Estimates the (backward) tangent at d[last - 0.5].
+ *
+ * \note The tangent is "backwards", i.e. it is with respect to
+ * decreasing index rather than increasing index.
+ *
+ * \pre 2 \<= len.
+ * \pre d[len - 1] != d[len - 2].
+ * \pre all[p in d] in_svg_plane(p).
+ */
+static Point
+darray_right_tangent(Point const d[], unsigned const len)
+{
+ assert( 2 <= len );
+ unsigned const last = len - 1;
+ unsigned const prev = last - 1;
+ assert( d[last] != d[prev] );
+ return unit_vector( d[prev] - d[last] );
+}
+
+/**
+ * Estimate the (forward) tangent at point d[0].
+ *
+ * Unlike the center and right versions, this calculates the tangent in
+ * the way one might expect, i.e., wrt increasing index into d.
+ *
+ * \pre 2 \<= len.
+ * \pre d[0] != d[1].
+ * \pre all[p in d] in_svg_plane(p).
+ * \post is_unit_vector(ret).
+ **/
+Point
+darray_left_tangent(Point const d[], unsigned const len, double const tolerance_sq)
+{
+ assert( 2 <= len );
+ assert( 0 <= tolerance_sq );
+ for (unsigned i = 1;;) {
+ Point const pi(d[i]);
+ Point const t(pi - d[0]);
+ double const distsq = dot(t, t);
+ if ( tolerance_sq < distsq ) {
+ return unit_vector(t);
+ }
+ ++i;
+ if (i == len) {
+ return ( distsq == 0
+ ? darray_left_tangent(d, len)
+ : unit_vector(t) );
+ }
+ }
+}
+
+/**
+ * Estimates the (backward) tangent at d[last].
+ *
+ * \note The tangent is "backwards", i.e. it is with respect to
+ * decreasing index rather than increasing index.
+ *
+ * \pre 2 \<= len.
+ * \pre d[len - 1] != d[len - 2].
+ * \pre all[p in d] in_svg_plane(p).
+ */
+Point
+darray_right_tangent(Point const d[], unsigned const len, double const tolerance_sq)
+{
+ assert( 2 <= len );
+ assert( 0 <= tolerance_sq );
+ unsigned const last = len - 1;
+ for (unsigned i = last - 1;; i--) {
+ Point const pi(d[i]);
+ Point const t(pi - d[last]);
+ double const distsq = dot(t, t);
+ if ( tolerance_sq < distsq ) {
+ return unit_vector(t);
+ }
+ if (i == 0) {
+ return ( distsq == 0
+ ? darray_right_tangent(d, len)
+ : unit_vector(t) );
+ }
+ }
+}
+
+/**
+ * Estimates the (backward) tangent at d[center], by averaging the two
+ * segments connected to d[center] (and then normalizing the result).
+ *
+ * \note The tangent is "backwards", i.e. it is with respect to
+ * decreasing index rather than increasing index.
+ *
+ * \pre (0 \< center \< len - 1) and d is uniqued (at least in
+ * the immediate vicinity of \a center).
+ */
+static Point
+darray_center_tangent(Point const d[],
+ unsigned const center,
+ unsigned const len)
+{
+ assert( center != 0 );
+ assert( center < len - 1 );
+
+ Point ret;
+ if ( d[center + 1] == d[center - 1] ) {
+ /* Rotate 90 degrees in an arbitrary direction. */
+ Point const diff = d[center] - d[center - 1];
+ ret = rot90(diff);
+ } else {
+ ret = d[center - 1] - d[center + 1];
+ }
+ ret.normalize();
+ return ret;
+}
+
+
+int
+bezier_fit_cubic_r(Point bezier[], Point const data[], int const len, double const error, unsigned const max_beziers);
+
+int
+bezier_fit_cubic_full(Point bezier[], int split_points[],
+ Point const data[], int const len,
+ Point const &tHat1, Point const &tHat2,
+ double const error, unsigned const max_beziers);
+
+
+/**
+ * Fit a single-segment Bezier curve to a set of digitized points.
+ *
+ * \return Number of segments generated, or -1 on error.
+ */
+int
+bezier_fit_cubic(Point *bezier, Point const *data, int len, double error)
+{
+ return bezier_fit_cubic_r(bezier, data, len, error, 1);
+}
+
+/**
+ * Fit a multi-segment Bezier curve to a set of digitized points, with
+ * possible weedout of identical points and NaNs.
+ *
+ * \param max_beziers Maximum number of generated segments
+ * \param Result array, must be large enough for n. segments * 4 elements.
+ *
+ * \return Number of segments generated, or -1 on error.
+ */
+int
+bezier_fit_cubic_r(Point bezier[], Point const data[], int const len, double const error, unsigned const max_beziers)
+{
+ if(bezier == NULL ||
+ data == NULL ||
+ len <= 0 ||
+ max_beziers >= (1ul << (31 - 2 - 1 - 3)))
+ return -1;
+
+ Point *uniqued_data = new Point[len];
+ unsigned uniqued_len = copy_without_nans_or_adjacent_duplicates(data, len, uniqued_data);
+
+ if ( uniqued_len < 2 ) {
+ delete[] uniqued_data;
+ return 0;
+ }
+
+ /* Call fit-cubic function with recursion. */
+ int const ret = bezier_fit_cubic_full(bezier, NULL, uniqued_data, uniqued_len,
+ unconstrained_tangent, unconstrained_tangent,
+ error, max_beziers);
+ delete[] uniqued_data;
+ return ret;
+}
+
+
+
+/**
+ * Copy points from src to dest, filter out points containing NaN and
+ * adjacent points with equal x and y.
+ * \return length of dest
+ */
+static unsigned
+copy_without_nans_or_adjacent_duplicates(Point const src[], unsigned src_len, Point dest[])
+{
+ unsigned si = 0;
+ for (;;) {
+ if ( si == src_len ) {
+ return 0;
+ }
+ if (!std::isnan(src[si][X]) &&
+ !std::isnan(src[si][Y])) {
+ dest[0] = Point(src[si]);
+ ++si;
+ break;
+ }
+ si++;
+ }
+ unsigned di = 0;
+ for (; si < src_len; ++si) {
+ Point const src_pt = Point(src[si]);
+ if ( src_pt != dest[di]
+ && !std::isnan(src_pt[X])
+ && !std::isnan(src_pt[Y])) {
+ dest[++di] = src_pt;
+ }
+ }
+ unsigned dest_len = di + 1;
+ assert( dest_len <= src_len );
+ return dest_len;
+}
+
+/**
+ * Fit a multi-segment Bezier curve to a set of digitized points, without
+ * possible weedout of identical points and NaNs.
+ *
+ * \pre data is uniqued, i.e. not exist i: data[i] == data[i + 1].
+ * \param max_beziers Maximum number of generated segments
+ * \param Result array, must be large enough for n. segments * 4 elements.
+ */
+int
+bezier_fit_cubic_full(Point bezier[], int split_points[],
+ Point const data[], int const len,
+ Point const &tHat1, Point const &tHat2,
+ double const error, unsigned const max_beziers)
+{
+ int const maxIterations = 4; /* std::max times to try iterating */
+
+ if(!(bezier != NULL) ||
+ !(data != NULL) ||
+ !(len > 0) ||
+ !(max_beziers >= 1) ||
+ !(error >= 0.0))
+ return -1;
+
+ if ( len < 2 ) return 0;
+
+ if ( len == 2 ) {
+ /* We have 2 points, which can be fitted trivially. */
+ bezier[0] = data[0];
+ bezier[3] = data[len - 1];
+ double const dist = distance(bezier[0], bezier[3]) / 3.0;
+ if (std::isnan(dist)) {
+ /* Numerical problem, fall back to straight line segment. */
+ bezier[1] = bezier[0];
+ bezier[2] = bezier[3];
+ } else {
+ bezier[1] = ( is_zero(tHat1)
+ ? ( 2 * bezier[0] + bezier[3] ) / 3.
+ : bezier[0] + dist * tHat1 );
+ bezier[2] = ( is_zero(tHat2)
+ ? ( bezier[0] + 2 * bezier[3] ) / 3.
+ : bezier[3] + dist * tHat2 );
+ }
+ BEZIER_ASSERT(bezier);
+ return 1;
+ }
+
+ /* Parameterize points, and attempt to fit curve */
+ unsigned splitPoint; /* Point to split point set at. */
+ bool is_corner;
+ {
+ double *u = new double[len];
+ chord_length_parameterize(data, u, len);
+ if ( u[len - 1] == 0.0 ) {
+ /* Zero-length path: every point in data[] is the same.
+ *
+ * (Clients aren't allowed to pass such data; handling the case is defensive
+ * programming.)
+ */
+ delete[] u;
+ return 0;
+ }
+
+ generate_bezier(bezier, data, u, len, tHat1, tHat2, error);
+ reparameterize_pts(data, len, u, bezier);
+
+ /* Find max deviation of points to fitted curve. */
+ double const tolerance = std::sqrt(error + 1e-9);
+ double maxErrorRatio = compute_max_error_ratio(data, u, len, bezier, tolerance, &splitPoint);
+
+ if ( fabs(maxErrorRatio) <= 1.0 ) {
+ BEZIER_ASSERT(bezier);
+ delete[] u;
+ return 1;
+ }
+
+ /* If error not too large, then try some reparameterization and iteration. */
+ if ( 0.0 <= maxErrorRatio && maxErrorRatio <= 3.0 ) {
+ for (int i = 0; i < maxIterations; i++) {
+ generate_bezier(bezier, data, u, len, tHat1, tHat2, error);
+ reparameterize_pts(data, len, u, bezier);
+ maxErrorRatio = compute_max_error_ratio(data, u, len, bezier, tolerance, &splitPoint);
+ if ( fabs(maxErrorRatio) <= 1.0 ) {
+ BEZIER_ASSERT(bezier);
+ delete[] u;
+ return 1;
+ }
+ }
+ }
+ delete[] u;
+ is_corner = (maxErrorRatio < 0);
+ }
+
+ if (is_corner) {
+ assert(splitPoint < unsigned(len));
+ if (splitPoint == 0) {
+ if (is_zero(tHat1)) {
+ /* Got spike even with unconstrained initial tangent. */
+ ++splitPoint;
+ } else {
+ return bezier_fit_cubic_full(bezier, split_points, data, len, unconstrained_tangent, tHat2,
+ error, max_beziers);
+ }
+ } else if (splitPoint == unsigned(len - 1)) {
+ if (is_zero(tHat2)) {
+ /* Got spike even with unconstrained final tangent. */
+ --splitPoint;
+ } else {
+ return bezier_fit_cubic_full(bezier, split_points, data, len, tHat1, unconstrained_tangent,
+ error, max_beziers);
+ }
+ }
+ }
+
+ if ( 1 < max_beziers ) {
+ /*
+ * Fitting failed -- split at max error point and fit recursively
+ */
+ unsigned const rec_max_beziers1 = max_beziers - 1;
+
+ Point recTHat2, recTHat1;
+ if (is_corner) {
+ if(!(0 < splitPoint && splitPoint < unsigned(len - 1)))
+ return -1;
+ recTHat1 = recTHat2 = unconstrained_tangent;
+ } else {
+ /* Unit tangent vector at splitPoint. */
+ recTHat2 = darray_center_tangent(data, splitPoint, len);
+ recTHat1 = -recTHat2;
+ }
+ int const nsegs1 = bezier_fit_cubic_full(bezier, split_points, data, splitPoint + 1,
+ tHat1, recTHat2, error, rec_max_beziers1);
+ if ( nsegs1 < 0 ) {
+#ifdef BEZIER_DEBUG
+ g_print("fit_cubic[1]: recursive call failed\n");
+#endif
+ return -1;
+ }
+ assert( nsegs1 != 0 );
+ if (split_points != NULL) {
+ split_points[nsegs1 - 1] = splitPoint;
+ }
+ unsigned const rec_max_beziers2 = max_beziers - nsegs1;
+ int const nsegs2 = bezier_fit_cubic_full(bezier + nsegs1*4,
+ ( split_points == NULL
+ ? NULL
+ : split_points + nsegs1 ),
+ data + splitPoint, len - splitPoint,
+ recTHat1, tHat2, error, rec_max_beziers2);
+ if ( nsegs2 < 0 ) {
+#ifdef BEZIER_DEBUG
+ g_print("fit_cubic[2]: recursive call failed\n");
+#endif
+ return -1;
+ }
+
+#ifdef BEZIER_DEBUG
+ g_print("fit_cubic: success[nsegs: %d+%d=%d] on max_beziers:%u\n",
+ nsegs1, nsegs2, nsegs1 + nsegs2, max_beziers);
+#endif
+ return nsegs1 + nsegs2;
+ } else {
+ return -1;
+ }
+}
+
+
+/**
+ * Fill in \a bezier[] based on the given data and tangent requirements, using
+ * a least-squares fit.
+ *
+ * Each of tHat1 and tHat2 should be either a zero vector or a unit vector.
+ * If it is zero, then bezier[1 or 2] is estimated without constraint; otherwise,
+ * it bezier[1 or 2] is placed in the specified direction from bezier[0 or 3].
+ *
+ * \param tolerance_sq Used only for an initial guess as to tangent directions
+ * when \a tHat1 or \a tHat2 is zero.
+ */
+static void
+generate_bezier(Point bezier[],
+ Point const data[], double const u[], unsigned const len,
+ Point const &tHat1, Point const &tHat2,
+ double const tolerance_sq)
+{
+ bool const est1 = is_zero(tHat1);
+ bool const est2 = is_zero(tHat2);
+ Point est_tHat1( est1
+ ? darray_left_tangent(data, len, tolerance_sq)
+ : tHat1 );
+ Point est_tHat2( est2
+ ? darray_right_tangent(data, len, tolerance_sq)
+ : tHat2 );
+ estimate_lengths(bezier, data, u, len, est_tHat1, est_tHat2);
+ /* We find that darray_right_tangent tends to produce better results
+ for our current freehand tool than full estimation. */
+ if (est1) {
+ estimate_bi(bezier, 1, data, u, len);
+ if (bezier[1] != bezier[0]) {
+ est_tHat1 = unit_vector(bezier[1] - bezier[0]);
+ }
+ estimate_lengths(bezier, data, u, len, est_tHat1, est_tHat2);
+ }
+}
+
+
+static void
+estimate_lengths(Point bezier[],
+ Point const data[], double const uPrime[], unsigned const len,
+ Point const &tHat1, Point const &tHat2)
+{
+ double C[2][2]; /* Matrix C. */
+ double X[2]; /* Matrix X. */
+
+ /* 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;
+
+ /* First and last control points of the Bezier curve are positioned exactly at the first and
+ last data points. */
+ bezier[0] = data[0];
+ bezier[3] = data[len - 1];
+
+ for (unsigned i = 0; i < len; i++) {
+ /* Bezier control point coefficients. */
+ double const b0 = B0(uPrime[i]);
+ double const b1 = B1(uPrime[i]);
+ double const b2 = B2(uPrime[i]);
+ double const b3 = B3(uPrime[i]);
+
+ /* rhs for eqn */
+ Point const a1 = b1 * tHat1;
+ Point const a2 = b2 * tHat2;
+
+ C[0][0] += dot(a1, a1);
+ C[0][1] += dot(a1, a2);
+ C[1][0] = C[0][1];
+ C[1][1] += dot(a2, a2);
+
+ /* Additional offset to the data point from the predicted point if we were to set bezier[1]
+ to bezier[0] and bezier[2] to bezier[3]. */
+ Point const shortfall
+ = ( data[i]
+ - ( ( b0 + b1 ) * bezier[0] )
+ - ( ( b2 + b3 ) * bezier[3] ) );
+ X[0] += dot(a1, shortfall);
+ X[1] += dot(a2, shortfall);
+ }
+
+ /* We've constructed a pair of equations in the form of a matrix product C * alpha = X.
+ Now solve for alpha. */
+ double alpha_l, alpha_r;
+
+ /* Compute the determinants of C and X. */
+ double const det_C0_C1 = C[0][0] * C[1][1] - C[1][0] * C[0][1];
+ if ( det_C0_C1 != 0 ) {
+ /* Apparently Kramer's rule. */
+ double const det_C0_X = C[0][0] * X[1] - C[0][1] * X[0];
+ double const det_X_C1 = X[0] * C[1][1] - X[1] * C[0][1];
+ alpha_l = det_X_C1 / det_C0_C1;
+ alpha_r = det_C0_X / det_C0_C1;
+ } else {
+ /* The matrix is under-determined. Try requiring alpha_l == alpha_r.
+ *
+ * One way of implementing the constraint alpha_l == alpha_r is to treat them as the same
+ * variable in the equations. We can do this by adding the columns of C to form a single
+ * column, to be multiplied by alpha to give the column vector X.
+ *
+ * We try each row in turn.
+ */
+ double const c0 = C[0][0] + C[0][1];
+ if (c0 != 0) {
+ alpha_l = alpha_r = X[0] / c0;
+ } else {
+ double const c1 = C[1][0] + C[1][1];
+ if (c1 != 0) {
+ alpha_l = alpha_r = X[1] / c1;
+ } else {
+ /* Let the below code handle this. */
+ alpha_l = alpha_r = 0.;
+ }
+ }
+ }
+
+ /* 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.) */
+ /// \todo Check whether this special-casing is necessary now that
+ /// NewtonRaphsonRootFind handles non-positive denominator.
+ if ( alpha_l < 1.0e-6 ||
+ alpha_r < 1.0e-6 )
+ {
+ alpha_l = alpha_r = distance(data[0], data[len-1]) / 3.0;
+ }
+
+ /* Control points 1 and 2 are positioned an alpha distance out on the tangent vectors, left and
+ right, respectively. */
+ bezier[1] = alpha_l * tHat1 + bezier[0];
+ bezier[2] = alpha_r * tHat2 + bezier[3];
+
+ return;
+}
+
+static double lensq(Point const p) {
+ return dot(p, p);
+}
+
+static void
+estimate_bi(Point bezier[4], unsigned const ei,
+ Point const data[], double const u[], unsigned const len)
+{
+ if(!(1 <= ei && ei <= 2))
+ return;
+ unsigned const oi = 3 - ei;
+ double num[2] = {0., 0.};
+ double den = 0.;
+ for (unsigned i = 0; i < len; ++i) {
+ double const ui = u[i];
+ double const b[4] = {
+ B0(ui),
+ B1(ui),
+ B2(ui),
+ B3(ui)
+ };
+
+ for (unsigned d = 0; d < 2; ++d) {
+ num[d] += b[ei] * (b[0] * bezier[0][d] +
+ b[oi] * bezier[oi][d] +
+ b[3] * bezier[3][d] +
+ - data[i][d]);
+ }
+ den -= b[ei] * b[ei];
+ }
+
+ if (den != 0.) {
+ for (unsigned d = 0; d < 2; ++d) {
+ bezier[ei][d] = num[d] / den;
+ }
+ } else {
+ bezier[ei] = ( oi * bezier[0] + ei * bezier[3] ) / 3.;
+ }
+}
+
+/**
+ * Given set of points and their parameterization, try to find a better assignment of parameter
+ * values for the points.
+ *
+ * \param d Array of digitized points.
+ * \param u Current parameter values.
+ * \param bezCurve Current fitted curve.
+ * \param len Number of values in both d and u arrays.
+ * Also the size of the array that is allocated for return.
+ */
+static void
+reparameterize(Point const d[],
+ unsigned const len,
+ double u[],
+ CubicBezier const &bezCurve)
+{
+ assert( 2 <= len );
+
+ unsigned const last = len - 1;
+ assert( bezCurve[0] == d[0] );
+ assert( bezCurve[3] == d[last] );
+ assert( u[0] == 0.0 );
+ assert( u[last] == 1.0 );
+ /* Otherwise, consider including 0 and last in the below loop. */
+
+ for (unsigned i = 1; i < last; i++) {
+ u[i] = NewtonRaphsonRootFind(bezCurve, d[i], u[i]);
+ }
+}
+
+/**
+ * Use Newton-Raphson iteration to find better root.
+ *
+ * \param Q Current fitted curve
+ * \param P Digitized point
+ * \param u Parameter value for "P"
+ *
+ * \return Improved u
+ */
+static double
+NewtonRaphsonRootFind(CubicBezier const &Q, Point const &P, double const u)
+{
+ assert( 0.0 <= u );
+ assert( u <= 1.0 );
+
+ std::vector<Point> Q_u = Q.pointAndDerivatives(u, 2);
+
+ /* Compute f(u)/f'(u), where f is the derivative wrt u of distsq(u) = 0.5 * the square of the
+ distance from P to Q(u). Here we're using Newton-Raphson to find a stationary point in the
+ distsq(u), hopefully corresponding to a local minimum in distsq (and hence a local minimum
+ distance from P to Q(u)). */
+ Point const diff = Q_u[0] - P;
+ double numerator = dot(diff, Q_u[1]);
+ double denominator = dot(Q_u[1], Q_u[1]) + dot(diff, Q_u[2]);
+
+ double improved_u;
+ if ( denominator > 0. ) {
+ /* One iteration of Newton-Raphson:
+ improved_u = u - f(u)/f'(u) */
+ improved_u = u - ( numerator / denominator );
+ } else {
+ /* Using Newton-Raphson would move in the wrong direction (towards a local maximum rather
+ than local minimum), so we move an arbitrary amount in the right direction. */
+ if ( numerator > 0. ) {
+ improved_u = u * .98 - .01;
+ } else if ( numerator < 0. ) {
+ /* Deliberately asymmetrical, to reduce the chance of cycling. */
+ improved_u = .031 + u * .98;
+ } else {
+ improved_u = u;
+ }
+ }
+
+ if (!std::isfinite(improved_u)) {
+ improved_u = u;
+ } else if ( improved_u < 0.0 ) {
+ improved_u = 0.0;
+ } else if ( improved_u > 1.0 ) {
+ improved_u = 1.0;
+ }
+
+ /* Ensure that improved_u isn't actually worse. */
+ {
+ double const diff_lensq = lensq(diff);
+ for (double proportion = .125; ; proportion += .125) {
+ if ( lensq( Q.pointAt(improved_u) - P ) > diff_lensq ) {
+ if ( proportion > 1.0 ) {
+ //g_warning("found proportion %g", proportion);
+ improved_u = u;
+ break;
+ }
+ improved_u = ( ( 1 - proportion ) * improved_u +
+ proportion * u );
+ } else {
+ break;
+ }
+ }
+ }
+
+ DOUBLE_ASSERT(improved_u);
+ return improved_u;
+}
+
+
+/**
+ * Assign parameter values to digitized points using relative distances between points.
+ *
+ * \pre Parameter array u must have space for \a len items.
+ */
+static void
+chord_length_parameterize(Point const d[], double u[], unsigned const len)
+{
+ if(!( 2 <= len ))
+ return;
+
+ /* First let u[i] equal the distance travelled along the path from d[0] to d[i]. */
+ u[0] = 0.0;
+ for (unsigned i = 1; i < len; i++) {
+ double const dist = distance(d[i], d[i-1]);
+ u[i] = u[i-1] + dist;
+ }
+
+ /* Then scale to [0.0 .. 1.0]. */
+ double tot_len = u[len - 1];
+ if(!( tot_len != 0 ))
+ return;
+ if (std::isfinite(tot_len)) {
+ for (unsigned i = 1; i < len; ++i) {
+ u[i] /= tot_len;
+ }
+ } else {
+ /* We could do better, but this probably never happens anyway. */
+ for (unsigned i = 1; i < len; ++i) {
+ u[i] = i / (double) ( len - 1 );
+ }
+ }
+
+ /** \todo
+ * It's been reported that u[len - 1] can differ from 1.0 on some
+ * systems (amd64), despite it having been calculated as x / x where x
+ * is isFinite and non-zero.
+ */
+ if (u[len - 1] != 1) {
+ double const diff = u[len - 1] - 1;
+ if (fabs(diff) > 1e-13) {
+ assert(0); // No warnings in 2geom
+ //g_warning("u[len - 1] = %19g (= 1 + %19g), expecting exactly 1",
+ // u[len - 1], diff);
+ }
+ u[len - 1] = 1;
+ }
+
+#ifdef BEZIER_DEBUG
+ assert( u[0] == 0.0 && u[len - 1] == 1.0 );
+ for (unsigned i = 1; i < len; i++) {
+ assert( u[i] >= u[i-1] );
+ }
+#endif
+}
+
+
+
+
+/**
+ * Find the maximum squared distance of digitized points to fitted curve, and (if this maximum
+ * error is non-zero) set \a *splitPoint to the corresponding index.
+ *
+ * \pre 2 \<= len.
+ * \pre u[0] == 0.
+ * \pre u[len - 1] == 1.0.
+ * \post ((ret == 0.0)
+ * || ((*splitPoint \< len - 1)
+ * \&\& (*splitPoint != 0 || ret \< 0.0))).
+ */
+static double
+compute_max_error_ratio(Point const d[], double const u[], unsigned const len,
+ BezierCurve const bezCurve, double const tolerance,
+ unsigned *const splitPoint)
+{
+ assert( 2 <= len );
+ unsigned const last = len - 1;
+ assert( bezCurve[0] == d[0] );
+ assert( bezCurve[3] == d[last] );
+ assert( u[0] == 0.0 );
+ assert( u[last] == 1.0 );
+ /* I.e. assert that the error for the first & last points is zero.
+ * Otherwise we should include those points in the below loop.
+ * The assertion is also necessary to ensure 0 < splitPoint < last.
+ */
+
+ double maxDistsq = 0.0; /* Maximum error */
+ double max_hook_ratio = 0.0;
+ unsigned snap_end = 0;
+ Point prev = bezCurve[0];
+ for (unsigned i = 1; i <= last; i++) {
+ Point const curr = bezier_pt(3, bezCurve, u[i]);
+ double const distsq = lensq( curr - d[i] );
+ if ( distsq > maxDistsq ) {
+ maxDistsq = distsq;
+ *splitPoint = i;
+ }
+ double const hook_ratio = compute_hook(prev, curr, .5 * (u[i - 1] + u[i]), bezCurve, tolerance);
+ if (max_hook_ratio < hook_ratio) {
+ max_hook_ratio = hook_ratio;
+ snap_end = i;
+ }
+ prev = curr;
+ }
+
+ double const dist_ratio = std::sqrt(maxDistsq) / tolerance;
+ double ret;
+ if (max_hook_ratio <= dist_ratio) {
+ ret = dist_ratio;
+ } else {
+ assert(0 < snap_end);
+ ret = -max_hook_ratio;
+ *splitPoint = snap_end - 1;
+ }
+ assert( ret == 0.0
+ || ( ( *splitPoint < last )
+ && ( *splitPoint != 0 || ret < 0. ) ) );
+ return ret;
+}
+
+/**
+ * Whereas compute_max_error_ratio() checks for itself that each data point
+ * is near some point on the curve, this function checks that each point on
+ * the curve is near some data point (or near some point on the polyline
+ * defined by the data points, or something like that: we allow for a
+ * "reasonable curviness" from such a polyline). "Reasonable curviness"
+ * means we draw a circle centred at the midpoint of a..b, of radius
+ * proportional to the length |a - b|, and require that each point on the
+ * segment of bezCurve between the parameters of a and b be within that circle.
+ * If any point P on the bezCurve segment is outside of that allowable
+ * region (circle), then we return some metric that increases with the
+ * distance from P to the circle.
+ *
+ * Given that this is a fairly arbitrary criterion for finding appropriate
+ * places for sharp corners, we test only one point on bezCurve, namely
+ * the point on bezCurve with parameter halfway between our estimated
+ * parameters for a and b. (Alternatives are taking the farthest of a
+ * few parameters between those of a and b, or even using a variant of
+ * NewtonRaphsonFindRoot() for finding the maximum rather than minimum
+ * distance.)
+ */
+static double
+compute_hook(Point const &a, Point const &b, double const u, CubicBezier const & bezCurve,
+ double const tolerance)
+{
+ Point const P = bezCurve.pointAt(u);
+ double const dist = distance((a+b)*.5, P);
+ if (dist < tolerance) {
+ return 0;
+ }
+ double const allowed = distance(a, b) + tolerance;
+ return dist / allowed;
+ /** \todo
+ * effic: Hooks are very rare. We could start by comparing
+ * distsq, only resorting to the more expensive L2 in cases of
+ * uncertainty.
+ */
+}
+
+}
+
+}
+
+#include <2geom/bezier-utils.h>
+
+
+using std::vector;
+using namespace Geom;
+using namespace std;
+
+class PointToBezierTester: public Toy {
+ //std::vector<Slider> sliders;
+ PointHandle adjuster, adjuster2, adjuster3;
+ std::vector<Toggle> toggles;
+ Piecewise<D2<SBasis > > stroke;
+
+ void draw(cairo_t *cr, std::ostringstream *notify, int width, int height, bool save, std::ostringstream *timer_stream) override {
+ cairo_save(cr);
+
+ cairo_set_source_rgba (cr, 0., 0., 0., 1);
+ cairo_set_line_width (cr, 0.5);
+ adjuster2.pos[0]=150;
+ adjuster2.pos[1]=std::min(std::max(adjuster2.pos[1],150.),450.);
+ cairo_move_to(cr, 150, 150);
+ cairo_line_to(cr, 150, 450);
+ cairo_stroke(cr);
+ ostringstream val_s;
+ double scale0=(450-adjuster2.pos[1])/300;
+ double curve_precision = pow(10, scale0*5-2);
+ val_s << curve_precision;
+ draw_text(cr, adjuster2.pos, val_s.str().c_str());
+ cairo_restore(cr);
+
+ cairo_save(cr);
+
+ cairo_set_source_rgba (cr, 0., 0., 0., 1);
+ cairo_set_line_width (cr, 0.5);
+
+ if(!mouses.empty()) {
+ cairo_move_to(cr, mouses[0]);
+ for(auto & mouse : mouses) {
+ cairo_line_to(cr, mouse);
+ }
+ cairo_stroke(cr);
+ }
+
+ if(!mouses.empty()) {
+ Point bezier[1000];
+ int segsgenerated;
+ {
+ Timer tm;
+
+ tm.ask_for_timeslice();
+ tm.start();
+ segsgenerated = bezier_fit_cubic_r(bezier, &mouses[0],
+ mouses.size(), curve_precision, 240);
+
+ Timer::Time als_time = tm.lap();
+ *notify << "original time = " << als_time << std::endl;
+ }
+
+ if(1) {
+ cairo_save(cr);
+ cairo_set_source_rgba (cr, 0., 1., 0., 1);
+ cairo_move_to(cr, bezier[0]);
+ int bezi=1;
+ for(int i = 0; i < segsgenerated; i ++) {
+ cairo_curve_to(cr, bezier[bezi], bezier[bezi+1], bezier[bezi+2]);
+ bezi += 4;
+ }
+ cairo_stroke(cr);
+ cairo_restore(cr);
+
+ }
+ {
+ Timer tm;
+
+ tm.ask_for_timeslice();
+ tm.start();
+ segsgenerated = Geom::BezierFitter::bezier_fit_cubic_r(bezier, &mouses[0],
+ mouses.size(), curve_precision, 240);
+
+ Timer::Time als_time = tm.lap();
+ *notify << "experimental version time = " << als_time << std::endl;
+ }
+
+ if (1) {
+ cairo_save(cr);
+ cairo_set_source_rgba (cr, 0., 0., 0., 1);
+ cairo_move_to(cr, bezier[0]);
+ int bezi=1;
+ for(int i = 0; i < segsgenerated; i ++) {
+ cairo_curve_to(cr, bezier[bezi], bezier[bezi+1], bezier[bezi+2]);
+ bezi += 4;
+ }
+ cairo_stroke(cr);
+ cairo_restore(cr);
+ }
+ *notify << "segments : "<< segsgenerated <<"\n";
+ }
+ cairo_restore(cr);
+
+ /*
+ Point p(25, height - 50), d(50,25);
+ toggles[0].bounds = Rect(p, p + d);
+ p+= Point(75, 0);
+ toggles[1].bounds = Rect(p, p + d);
+ draw_toggles(cr, toggles);
+ */
+ Toy::draw(cr, notify, width, height, save,timer_stream);
+ }
+
+public:
+ void key_hit(GdkEventKey *e) override {
+ if(e->keyval == 's') toggles[0].toggle();
+ redraw();
+ }
+ vector<Point> mouses;
+ int mouse_drag;
+
+ void mouse_pressed(GdkEventButton* e) override {
+ toggle_events(toggles, e);
+ Toy::mouse_pressed(e);
+ if(!selected) {
+ mouse_drag = 1;
+ mouses.clear();
+ }
+ }
+
+
+ void mouse_moved(GdkEventMotion* e) override {
+ if(mouse_drag) {
+ mouses.emplace_back(e->x, e->y);
+ redraw();
+ } else {
+ Toy::mouse_moved(e);
+ }
+ }
+
+ void mouse_released(GdkEventButton* e) override {
+ mouse_drag = 0;
+ stroke.clear();
+ stroke.push_cut(0);
+ Path pth;
+ for(unsigned i = 2; i < mouses.size(); i+=2) {
+ pth.append(QuadraticBezier(mouses[i-2], mouses[i-1], mouses[i]));
+ }
+ stroke = pth.toPwSb();
+ Toy::mouse_released(e);
+ }
+
+ PointToBezierTester() {
+ adjuster2.pos = Geom::Point(150,300);
+ handles.push_back(&adjuster2);
+ toggles.emplace_back("Seq", false);
+ toggles.emplace_back("Linfty", true);
+ //}
+ //sliders.push_back(Slider(0.0, 1.0, 0.0, 0.0, "t"));
+ //handles.push_back(&(sliders[0]));
+ }
+};
+
+int main(int argc, char **argv) {
+ init(argc, argv, new PointToBezierTester);
+ return 0;
+}
+
+/*
+ Local Variables:
+ mode:c++
+ c-file-style:"stroustrup"
+ c-file-offsets:((innamespace . 0)(inline-open . 0)(case-label . +))
+ indent-tabs-mode:nil
+ fill-column:99
+ End:
+*/
+// vim: filetype = cpp:expandtab:shiftwidth = 4:tabstop = 8:softtabstop = 4:encoding = utf-8:textwidth = 99 :