1 #include <2geom/solver.h>
2 #include <2geom/point.h>
3 #include <algorithm>
4 #include <valarray>
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 */
11 namespace Geom{
13 template<class t>
14 static int SGN(t x) { return (x > 0 ? 1 : (x < 0 ? -1 : 0)); }
16 const unsigned MAXDEPTH = 23; // Maximum depth for recursion. Using floats means 23 bits precision max
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);
40 double horner(const double *b, double t);
42 unsigned
43 control_poly_flat_enough(double const *V);
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 }
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 {
72 unsigned n_crossings = 0; /* Number of zero-crossings */
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 }
85 if (n_crossings == 0) // no solutions here
86 return;
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];
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 }
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];
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);
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 }
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;
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];
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 }
166 double mid_t = left_t*(1-t) + right_t*t;
168 find_bernstein_roots(&new_controls[0], depth+1, left_t, mid_t);
170 /* Solution is exactly on the subdivision point. */
171 if (new_controls[N] == 0)
172 solutions.push_back(mid_t);
174 find_bernstein_roots(&new_controls[N], depth+1, mid_t, right_t);
175 }
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] */
189 /* Derive the implicit equation for line connecting first */
190 /* and last control points */
191 const double a = V[0] - V[degree];
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 }
208 const double abSquared = 1./((a * a) + 1);
210 const double intercept_1 = (a - max_distance_above * abSquared);
211 const double intercept_2 = (a - max_distance_below * abSquared);
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);
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 }
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 }
240 };
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 :