diff options
Diffstat (limited to 'src/2geom/solve-bezier.cpp')
| -rw-r--r-- | src/2geom/solve-bezier.cpp | 304 |
1 files changed, 304 insertions, 0 deletions
diff --git a/src/2geom/solve-bezier.cpp b/src/2geom/solve-bezier.cpp new file mode 100644 index 0000000..4ff42bb --- /dev/null +++ b/src/2geom/solve-bezier.cpp @@ -0,0 +1,304 @@ + +#include <2geom/solver.h> +#include <2geom/choose.h> +#include <2geom/bezier.h> +#include <2geom/point.h> + +#include <cmath> +#include <algorithm> + +/*** Find the zeros of a Bezier. The code subdivides until it is happy with the linearity of the + * function. This requires an O(degree^2) subdivision for each step, even when there is only one + * solution. + * + * We try fairly hard to correctly handle multiple roots. + */ + +//#define debug(x) do{x;}while(0) +#define debug(x) + +namespace Geom{ + +template<class t> +static int SGN(t x) { return (x > 0 ? 1 : (x < 0 ? -1 : 0)); } + +class Bernsteins{ +public: + static const size_t MAX_DEPTH = 22; + std::vector<double> &solutions; + //std::vector<double> dsolutions; + + Bernsteins(std::vector<double> & sol) + : solutions(sol) + {} + + void subdivide(double const *V, + double t, + double *Left, + double *Right); + + double secant(Bezier const &bz); + + + void find_bernstein_roots(Bezier const &bz, unsigned depth, + double left_t, double right_t); +}; + +template <typename T> +inline std::ostream &operator<< (std::ostream &out_file, const std::vector<T> & b) { + out_file << "["; + for(unsigned i = 0; i < b.size(); i++) { + out_file << b[i] << ", "; + } + return out_file << "]"; +} + +void convex_hull_marching(Bezier const &src_bz, Bezier bz, + std::vector<double> &solutions, + double left_t, + double right_t) +{ + while(bz.order() > 0 && bz[0] == 0) { + std::cout << "deflate\n"; + bz = bz.deflate(); + solutions.push_back(left_t); + } + std::cout << std::endl; + if (bz.order() > 0) { + + int old_sign = SGN(bz[0]); + + double left_bound = 0; + double dt = 0; + for (size_t i = 1; i < bz.size(); i++) + { + int sign = SGN(bz[i]); + if (sign != old_sign) + { + dt = double(i) / bz.order(); + left_bound = dt * bz[0] / (bz[0] - bz[i]); + break; + } + old_sign = sign; + } + if (dt == 0) return; + std::cout << bz << std::endl; + std::cout << "dt = " << dt << std::endl; + std::cout << "left_t = " << left_t << std::endl; + std::cout << "right_t = " << right_t << std::endl; + std::cout << "left bound = " << left_bound + << " = " << bz(left_bound) << std::endl; + double new_left_t = left_bound * (right_t - left_t) + left_t; + std::cout << "new_left_t = " << new_left_t << std::endl; + Bezier bzr = portion(src_bz, new_left_t, 1); + while(bzr.order() > 0 && bzr[0] == 0) { + std::cout << "deflate\n"; + bzr = bzr.deflate(); + solutions.push_back(new_left_t); + } + if (left_t < new_left_t) { + convex_hull_marching(src_bz, bzr, + solutions, + new_left_t, right_t); + } else { + std::cout << "epsilon reached\n"; + while(bzr.order() > 0 && fabs(bzr[0]) <= 1e-10) { + std::cout << "deflate\n"; + bzr = bzr.deflate(); + std::cout << bzr << std::endl; + solutions.push_back(new_left_t); + } + + } + } +} + +void +Bezier::find_bezier_roots(std::vector<double> &solutions, + double left_t, double right_t) const { + Bezier bz = *this; + //convex_hull_marching(bz, bz, solutions, left_t, right_t); + //return; + + // a constant bezier, even if identically zero, has no roots + if (bz.isConstant()) { + return; + } + + while(bz[0] == 0) { + debug(std::cout << "deflate\n"); + bz = bz.deflate(); + solutions.push_back(0); + } + if (bz.degree() == 1) { + debug(std::cout << "linear\n"); + + if (SGN(bz[0]) != SGN(bz[1])) { + double d = bz[0] - bz[1]; + if(d != 0) { + double r = bz[0] / d; + if(0 <= r && r <= 1) + solutions.push_back(r); + } + } + return; + } + + //std::cout << "initial = " << bz << std::endl; + Bernsteins B(solutions); + B.find_bernstein_roots(bz, 0, left_t, right_t); + //std::cout << solutions << std::endl; +} + +void Bernsteins::find_bernstein_roots(Bezier const &bz, + unsigned depth, + double left_t, + double right_t) +{ + debug(std::cout << left_t << ", " << right_t << std::endl); + size_t n_crossings = 0; + + int old_sign = SGN(bz[0]); + //std::cout << "w[0] = " << bz[0] << std::endl; + for (size_t i = 1; i < bz.size(); i++) + { + //std::cout << "w[" << i << "] = " << w[i] << std::endl; + int sign = SGN(bz[i]); + if (sign != 0) + { + if (sign != old_sign && old_sign != 0) + { + ++n_crossings; + } + old_sign = sign; + } + } + // if last control point is zero, that counts as crossing too + if (SGN(bz[bz.size()-1]) == 0) { + ++n_crossings; + } + + //std::cout << "n_crossings = " << n_crossings << std::endl; + if (n_crossings == 0) return; // no solutions here + + if (n_crossings == 1) /* Unique solution */ + { + //std::cout << "depth = " << depth << std::endl; + /* Stop recursion when the tree is deep enough */ + /* if deep enough, return 1 solution at midpoint */ + if (depth > MAX_DEPTH) + { + //printf("bottom out %d\n", depth); + const double Ax = right_t - left_t; + const double Ay = bz.at1() - bz.at0(); + + solutions.push_back(left_t - Ax*bz.at0() / Ay); + return; + } + + double r = secant(bz); + solutions.push_back(r*right_t + (1-r)*left_t); + return; + } + /* Otherwise, solve recursively after subdividing control polygon */ + Bezier::Order o(bz); + Bezier Left(o), Right = bz; + double split_t = (left_t + right_t) * 0.5; + + // If subdivision is working poorly, split around the leftmost root of the derivative + if (depth > 2) { + debug(std::cout << "derivative mode\n"); + Bezier dbz = derivative(bz); + + debug(std::cout << "initial = " << dbz << std::endl); + std::vector<double> dsolutions = dbz.roots(Interval(left_t, right_t)); + debug(std::cout << "dsolutions = " << dsolutions << std::endl); + + double dsplit_t = 0.5; + if(!dsolutions.empty()) { + dsplit_t = dsolutions[0]; + split_t = left_t + (right_t - left_t)*dsplit_t; + debug(std::cout << "split_value = " << bz(split_t) << std::endl); + debug(std::cout << "splitting around " << dsplit_t << " = " + << split_t << "\n"); + + } + std::pair<Bezier, Bezier> LR = bz.subdivide(dsplit_t); + Left = LR.first; + Right = LR.second; + } else { + // split at midpoint, because it is cheap + Left[0] = Right[0]; + for (size_t i = 1; i < bz.size(); ++i) + { + for (size_t j = 0; j < bz.size()-i; ++j) + { + Right[j] = (Right[j] + Right[j+1]) * 0.5; + } + Left[i] = Right[0]; + } + } + debug(std::cout << "Solution is exactly on the subdivision point.\n"); + debug(std::cout << Left << " , " << Right << std::endl); + Left = reverse(Left); + while(Right.order() > 0 && fabs(Right[0]) <= 1e-10) { + debug(std::cout << "deflate\n"); + Right = Right.deflate(); + Left = Left.deflate(); + solutions.push_back(split_t); + } + Left = reverse(Left); + if (Right.order() > 0) { + debug(std::cout << Left << " , " << Right << std::endl); + find_bernstein_roots(Left, depth+1, left_t, split_t); + find_bernstein_roots(Right, depth+1, split_t, right_t); + } +} + +double Bernsteins::secant(Bezier const &bz) { + double s = 0, t = 1; + double e = 1e-14; + int side = 0; + double r, fs = bz.at0(), ft = bz.at1(); + + for (size_t n = 0; n < 100; ++n) + { + r = (fs*t - ft*s) / (fs - ft); + if (fabs(t-s) < e * fabs(t+s)) { + debug(std::cout << "error small " << fabs(t-s) + << ", accepting solution " << r + << "after " << n << "iterations\n"); + return r; + } + + double fr = bz.valueAt(r); + + if (fr * ft > 0) + { + t = r; ft = fr; + if (side == -1) fs /= 2; + side = -1; + } + else if (fs * fr > 0) + { + s = r; fs = fr; + if (side == +1) ft /= 2; + side = +1; + } + else break; + } + return r; +} + +}; + +/* + 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:fileencoding=utf-8:textwidth=99 : |