src/2geom/solve-bezier-one-d.cpp

   1 #include <2geom/solver.h>
   2 #include <2geom/point.h>
   3 #include <algorithm>
   4 #include <valarray>
   5
   6 /*** Find the zeros of the bernstein function.  The code subdivides until it is happy with the
   7  * linearity of the function.  This requires an O(degree^2) subdivision for each step, even when
   8  * there is only one solution.
   9  */
  10
  11 namespace Geom{
  12
  13 template<class t>
  14 static int SGN(t x) { return (x > 0 ? 1 : (x < 0 ? -1 : 0)); }
  15
  16 const unsigned MAXDEPTH = 23; // Maximum depth for recursion.  Using floats means 23 bits precision max
  17
  18 const double BEPSILON = ldexp(1.0,(-MAXDEPTH-1)); /*Flatness control value */
  19 const double SECANT_EPSILON = 1e-13; // secant method converges much faster, get a bit more precision
  20 /**
  21  * This function is called _a lot_.  We have included various manual memory management stuff to reduce the amount of mallocing that goes on.  In the future it is possible that this will hurt performance.
  22  **/
  23 class Bernsteins{
  24 public:
  25     double *Vtemp;
  26     unsigned N,degree;
  27     std::vector<double> &solutions;
  28     bool use_secant;
  29     Bernsteins(int degr, std::vector<double> &so) : N(degr+1), degree(degr),solutions(so), use_secant(false) {
  30         Vtemp = new double[N*2];
  31     }
  32     ~Bernsteins() {
  33         delete[] Vtemp;
  34     }
  35     void subdivide(double const *V,
  36                    double t,
  37                    double *Left,
  38                    double *Right);
  39
  40     double horner(const double *b, double t);
  41
  42     unsigned
  43     control_poly_flat_enough(double const *V);
  44
  45     void
  46     find_bernstein_roots(double const *w, /* The control points  */
  47                          unsigned depth,  /* The depth of the recursion */
  48                          double left_t, double right_t);
  49 };
  50 /*
  51  *  find_bernstein_roots : Given an equation in Bernstein-Bernstein form, find all
  52  *    of the roots in the open interval (0, 1).  Return the number of roots found.
  53  */
  54 void
  55 find_bernstein_roots(double const *w, /* The control points  */
  56                      unsigned degree,   /* The degree of the polynomial */
  57                      std::vector<double> &solutions, /* RETURN candidate t-values */
  58                      unsigned depth,    /* The depth of the recursion */
  59                      double left_t, double right_t, bool use_secant)
  60 {
  61     Bernsteins B(degree, solutions);
  62     B.use_secant = use_secant;
  63     B.find_bernstein_roots(w, depth, left_t, right_t);
  64 }
  65
  66 void
  67 Bernsteins::find_bernstein_roots(double const *w, /* The control points  */
  68                      unsigned depth,    /* The depth of the recursion */
  69                      double left_t, double right_t)
  70 {
  71
  72     unsigned    n_crossings = 0;        /*  Number of zero-crossings */
  73
  74     int old_sign = SGN(w[0]);
  75     for (unsigned i = 1; i < N; i++) {
  76         int sign = SGN(w[i]);
  77         if (sign) {
  78             if (sign != old_sign && old_sign) {
  79                n_crossings++;
  80             }
  81             old_sign = sign;
  82         }
  83     }
  84
  85     if (n_crossings == 0) // no solutions here
  86         return;
  87
  88     if (n_crossings == 1) {
  89         /* Unique solution      */
  90         /* Stop recursion when the tree is deep enough  */
  91         /* if deep enough, return 1 solution at midpoint  */
  92         if (depth >= MAXDEPTH) {
  93             //printf("bottom out %d\n", depth);
  94             const double Ax = right_t - left_t;
  95             const double Ay = w[degree] - w[0];
  96
  97             solutions.push_back(left_t - Ax*w[0] / Ay);
  98             return;
  99             solutions.push_back((left_t + right_t) / 2.0);
 100             return;
 101         }
 102
 103         // I thought secant method would be faster here, but it'aint. -- njh
 104         // Actually, it was, I just was using the wrong method for bezier evaluation.  Horner's rule results in a very efficient algorithm - 10* faster (20080816)
 105         // Future work: try using brent's method
 106         if(use_secant) { // false position
 107             double s = 0;double t = 1;
 108             double e = 1e-10;
 109             int n,side=0;
 110             double r,fr,fs = w[0],ft = w[degree];
 111
 112             for (n = 1; n <= 100; n++)
 113             {
 114                 r = (fs*t - ft*s) / (fs - ft);
 115                 if (fabs(t-s) < e*fabs(t+s)) break;
 116                 fr = horner(w, r);
 117
 118                 if (fr * ft > 0)
 119                 {
 120                     t = r; ft = fr;
 121                     if (side==-1) fs /= 2;
 122                     side = -1;
 123                 }
 124                 else if (fs * fr > 0)
 125                 {
 126                     s = r;  fs = fr;
 127                     if (side==+1) ft /= 2;
 128                     side = +1;
 129                 }
 130                 else break;
 131             }
 132             solutions.push_back(r*right_t + (1-r)*left_t);
 133             return;
 134         }
 135     }
 136
 137     /* Otherwise, solve recursively after subdividing control polygon  */
 138     std::valarray<double> new_controls(2*N); // New left and right control polygons
 139     const double t = 0.5;
 140
 141
 142 /*
 143  *  Bernstein :
 144  *      Evaluate a Bernstein function at a particular parameter value
 145  *      Fill in control points for resulting sub-curves.
 146  *
 147  */
 148     for (unsigned i = 0; i < N; i++)
 149         Vtemp[i] = w[i];
 150
 151     /* Triangle computation     */
 152     const double omt = (1-t);
 153     new_controls[0] = Vtemp[0];
 154     new_controls[N+degree] = Vtemp[degree];
 155     double *prev_row = Vtemp;
 156     double *row = Vtemp + N;
 157     for (unsigned i = 1; i < N; i++) {
 158         for (unsigned j = 0; j < N - i; j++) {
 159             row[j] = omt*prev_row[j] + t*prev_row[j+1];
 160         }
 161         new_controls[i] = row[0];
 162         new_controls[N+degree-i] = row[degree-i];
 163         std::swap(prev_row, row);
 164     }
 165
 166     double mid_t = left_t*(1-t) + right_t*t;
 167
 168     find_bernstein_roots(&new_controls[0], depth+1, left_t, mid_t);
 169
 170     /* Solution is exactly on the subdivision point. */
 171     if (new_controls[N] == 0)
 172         solutions.push_back(mid_t);
 173
 174     find_bernstein_roots(&new_controls[N], depth+1, mid_t, right_t);
 175 }
 176
 177 /*
 178  *  control_poly_flat_enough :
 179  *      Check if the control polygon of a Bernstein curve is flat enough
 180  *      for recursive subdivision to bottom out.
 181  *
 182  */
 183 unsigned
 184 Bernsteins::control_poly_flat_enough(double const *V)
 185 {
 186     /* Find the perpendicular distance from each interior control point to line connecting V[0] and
 187      * V[degree] */
 188
 189     /* Derive the implicit equation for line connecting first */
 190     /*  and last control points */
 191     const double a = V[0] - V[degree];
 192
 193     double max_distance_above = 0.0;
 194     double max_distance_below = 0.0;
 195     double ii = 0, dii = 1./degree;
 196     for (unsigned i = 1; i < degree; i++) {
 197         ii += dii;
 198         /* Compute distance from each of the points to that line */
 199         const double d = (a + V[i]) * ii  - a;
 200         double dist = d*d;
 201     // Find the largest distance
 202         if (d < 0.0)
 203             max_distance_below = std::min(max_distance_below, -dist);
 204         else
 205             max_distance_above = std::max(max_distance_above, dist);
 206     }
 207
 208     const double abSquared = 1./((a * a) + 1);
 209
 210     const double intercept_1 = (a - max_distance_above * abSquared);
 211     const double intercept_2 = (a - max_distance_below * abSquared);
 212
 213     /* Compute bounding interval*/
 214     const double left_intercept = std::min(intercept_1, intercept_2);
 215     const double right_intercept = std::max(intercept_1, intercept_2);
 216
 217     const double error = 0.5 * (right_intercept - left_intercept);
 218     //printf("error %g %g %g\n", error, a, BEPSILON * a);
 219     return error < BEPSILON * a;
 220 }
 221
 222 // suggested by Sederberg.
 223 double Bernsteins::horner(const double *b, double t) {
 224     int n = degree;
 225     double u, bc, tn, tmp;
 226     int i;
 227     u = 1.0 - t;
 228     bc = 1;
 229     tn = 1;
 230     tmp = b[0]*u;
 231     for(i=1; i<n; i++){
 232         tn = tn*t;
 233         bc = bc*(n-i+1)/i;
 234         tmp = (tmp + tn*bc*b[i])*u;
 235     }
 236     return (tmp + tn*t*b[n]);
 237 }
 238
 239
 240 };
 241
 242 /*
 243   Local Variables:
 244   mode:c++
 245   c-file-style:"stroustrup"
 246   c-file-offsets:((innamespace . 0)(inline-open . 0)(case-label . +))
 247   indent-tabs-mode:nil
 248   fill-column:99
 249   End:
 250 */
 251 // vim: filetype=cpp:expandtab:shiftwidth=4:tabstop=8:softtabstop=4:encoding=utf-8:textwidth=99 :