| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /** | ||
| 2 | * @file | ||
| 3 | * @brief Root finding for sbasis functions. | ||
| 4 | *//* | ||
| 5 | * Authors: | ||
| 6 | * Nathan Hurst <njh@njhurst.com> | ||
| 7 | * JF Barraud | ||
| 8 | * Copyright 2006-2007 Authors | ||
| 9 | * | ||
| 10 | * This library is free software; you can redistribute it and/or | ||
| 11 | * modify it either under the terms of the GNU Lesser General Public | ||
| 12 | * License version 2.1 as published by the Free Software Foundation | ||
| 13 | * (the "LGPL") or, at your option, under the terms of the Mozilla | ||
| 14 | * Public License Version 1.1 (the "MPL"). If you do not alter this | ||
| 15 | * notice, a recipient may use your version of this file under either | ||
| 16 | * the MPL or the LGPL. | ||
| 17 | * | ||
| 18 | * You should have received a copy of the LGPL along with this library | ||
| 19 | * in the file COPYING-LGPL-2.1; if not, write to the Free Software | ||
| 20 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA | ||
| 21 | * You should have received a copy of the MPL along with this library | ||
| 22 | * in the file COPYING-MPL-1.1 | ||
| 23 | * | ||
| 24 | * The contents of this file are subject to the Mozilla Public License | ||
| 25 | * Version 1.1 (the "License"); you may not use this file except in | ||
| 26 | * compliance with the License. You may obtain a copy of the License at | ||
| 27 | * http://www.mozilla.org/MPL/ | ||
| 28 | * | ||
| 29 | * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY | ||
| 30 | * OF ANY KIND, either express or implied. See the LGPL or the MPL for | ||
| 31 | * the specific language governing rights and limitations. | ||
| 32 | * | ||
| 33 | */ | ||
| 34 | |||
| 35 | /* | ||
| 36 | * It is more efficient to find roots of f(t) = c_0, c_1, ... all at once, rather than iterating. | ||
| 37 | * | ||
| 38 | * Todo/think about: | ||
| 39 | * multi-roots using bernstein method, one approach would be: | ||
| 40 | sort c | ||
| 41 | take median and find roots of that | ||
| 42 | whenever a segment lies entirely on one side of the median, | ||
| 43 | find the median of the half and recurse. | ||
| 44 | |||
| 45 | in essence we are implementing quicksort on a continuous function | ||
| 46 | |||
| 47 | * the gsl poly roots finder is faster than bernstein too, but we don't use it for 3 reasons: | ||
| 48 | |||
| 49 | a) it requires conversion to poly, which is numerically unstable | ||
| 50 | |||
| 51 | b) it requires gsl (which is currently not a dependency, and would bring in a whole slew of unrelated stuff) | ||
| 52 | |||
| 53 | c) it finds all roots, even complex ones. We don't want to accidentally treat a nearly real root as a real root | ||
| 54 | |||
| 55 | From memory gsl poly roots was about 10 times faster than bernstein in the case where all the roots | ||
| 56 | are in [0,1] for polys of order 5. I spent some time working out whether eigenvalue root finding | ||
| 57 | could be done directly in sbasis space, but the maths was too hard for me. -- njh | ||
| 58 | |||
| 59 | jfbarraud: eigenvalue root finding could be done directly in sbasis space ? | ||
| 60 | |||
| 61 | njh: I don't know, I think it should. You would make a matrix whose characteristic polynomial was | ||
| 62 | correct, but do it by putting the sbasis terms in the right spots in the matrix. normal eigenvalue | ||
| 63 | root finding makes a matrix that is a diagonal + a row along the top. This matrix has the property | ||
| 64 | that its characteristic poly is just the poly whose coefficients are along the top row. | ||
| 65 | |||
| 66 | Now an sbasis function is a linear combination of the poly coeffs. So it seems to me that you | ||
| 67 | should be able to put the sbasis coeffs directly into a matrix in the right spots so that the | ||
| 68 | characteristic poly is the sbasis. You'll still have problems b) and c). | ||
| 69 | |||
| 70 | We might be able to lift an eigenvalue solver and include that directly into 2geom. Eigenvalues | ||
| 71 | also allow you to find intersections of multiple curves but require solving n*m x n*m matrices. | ||
| 72 | |||
| 73 | **/ | ||
| 74 | |||
| 75 | #include <cmath> | ||
| 76 | #include <map> | ||
| 77 | |||
| 78 | #include <2geom/sbasis.h> | ||
| 79 | #include <2geom/sbasis-to-bezier.h> | ||
| 80 | #include <2geom/solver.h> | ||
| 81 | |||
| 82 | using namespace std; | ||
| 83 | |||
| 84 | namespace Geom{ | ||
| 85 | |||
| 86 | /** Find the smallest interval that bounds a | ||
| 87 | \param a sbasis function | ||
| 88 | \returns interval | ||
| 89 | |||
| 90 | */ | ||
| 91 | |||
| 92 | #ifdef USE_SBASIS_OF | ||
| 93 | OptInterval bounds_exact(SBasisOf<double> const &a) { | ||
| 94 | Interval result = Interval(a.at0(), a.at1()); | ||
| 95 | SBasisOf<double> df = derivative(a); | ||
| 96 | vector<double>extrema = roots(df); | ||
| 97 | for (unsigned i=0; i<extrema.size(); i++){ | ||
| 98 | result.extendTo(a(extrema[i])); | ||
| 99 | } | ||
| 100 | return result; | ||
| 101 | } | ||
| 102 | #else | ||
| 103 | 24001 | OptInterval bounds_exact(SBasis const &a) { | |
| 104 |
1/2✓ Branch 4 taken 24001 times.
✗ Branch 5 not taken.
|
24001 | Interval result = Interval(a.at0(), a.at1()); |
| 105 |
1/2✓ Branch 2 taken 24001 times.
✗ Branch 3 not taken.
|
24001 | SBasis df = derivative(a); |
| 106 |
1/2✓ Branch 2 taken 24001 times.
✗ Branch 3 not taken.
|
24001 | vector<double>extrema = roots(df); |
| 107 |
2/2✓ Branch 7 taken 1501 times.
✓ Branch 8 taken 24001 times.
|
25502 | for (double i : extrema){ |
| 108 | 1501 | result.expandTo(a(i)); | |
| 109 | } | ||
| 110 |
1/2✓ Branch 1 taken 24001 times.
✗ Branch 2 not taken.
|
48002 | return result; |
| 111 | 24001 | } | |
| 112 | #endif | ||
| 113 | |||
| 114 | /** Find a small interval that bounds a | ||
| 115 | \param a sbasis function | ||
| 116 | \returns interval | ||
| 117 | |||
| 118 | */ | ||
| 119 | // I have no idea how this works, some clever bounding argument by jfb. | ||
| 120 | #ifdef USE_SBASIS_OF | ||
| 121 | OptInterval bounds_fast(const SBasisOf<double> &sb, int order) { | ||
| 122 | #else | ||
| 123 | 79247 | OptInterval bounds_fast(const SBasis &sb, int order) { | |
| 124 | #endif | ||
| 125 | 79247 | Interval res(0,0); // an empty sbasis is 0. | |
| 126 | |||
| 127 |
2/2✓ Branch 1 taken 241407 times.
✓ Branch 2 taken 79247 times.
|
320654 | for(int j = sb.size()-1; j>=order; j--) { |
| 128 | 241407 | double a=sb[j][0]; | |
| 129 | 241407 | double b=sb[j][1]; | |
| 130 | |||
| 131 | 241407 | double v, t = 0; | |
| 132 | 241407 | v = res.min(); | |
| 133 |
2/2✓ Branch 0 taken 72357 times.
✓ Branch 1 taken 169050 times.
|
241407 | if (v<0) t = ((b-a)/v+1)*0.5; |
| 134 |
6/6✓ Branch 0 taken 72357 times.
✓ Branch 1 taken 169050 times.
✓ Branch 2 taken 57950 times.
✓ Branch 3 taken 14407 times.
✓ Branch 4 taken 18910 times.
✓ Branch 5 taken 39040 times.
|
241407 | if (v>=0 || t<0 || t>1) { |
| 135 | 202367 | res.setMin(std::min(a,b)); | |
| 136 | } else { | ||
| 137 | 39040 | res.setMin(lerp(t, a+v*t, b)); | |
| 138 | } | ||
| 139 | |||
| 140 | 241407 | v = res.max(); | |
| 141 |
2/2✓ Branch 0 taken 178915 times.
✓ Branch 1 taken 62492 times.
|
241407 | if (v>0) t = ((b-a)/v+1)*0.5; |
| 142 |
6/6✓ Branch 0 taken 178915 times.
✓ Branch 1 taken 62492 times.
✓ Branch 2 taken 168090 times.
✓ Branch 3 taken 10825 times.
✓ Branch 4 taken 10816 times.
✓ Branch 5 taken 157274 times.
|
241407 | if (v<=0 || t<0 || t>1) { |
| 143 | 84133 | res.setMax(std::max(a,b)); | |
| 144 | }else{ | ||
| 145 | 157274 | res.setMax(lerp(t, a+v*t, b)); | |
| 146 | } | ||
| 147 | } | ||
| 148 |
2/2✓ Branch 0 taken 41442 times.
✓ Branch 1 taken 37805 times.
|
79247 | if (order>0) res*=std::pow(.25,order); |
| 149 |
1/2✓ Branch 1 taken 79247 times.
✗ Branch 2 not taken.
|
158494 | return res; |
| 150 | } | ||
| 151 | |||
| 152 | /** Find a small interval that bounds a(t) for t in i to order order | ||
| 153 | \param sb sbasis function | ||
| 154 | \param i domain interval | ||
| 155 | \param order number of terms | ||
| 156 | \return interval | ||
| 157 | |||
| 158 | */ | ||
| 159 | #ifdef USE_SBASIS_OF | ||
| 160 | OptInterval bounds_local(const SBasisOf<double> &sb, const OptInterval &i, int order) { | ||
| 161 | #else | ||
| 162 | 97201 | OptInterval bounds_local(const SBasis &sb, const OptInterval &i, int order) { | |
| 163 | #endif | ||
| 164 | 97201 | double t0=i->min(), t1=i->max(), lo=0., hi=0.; | |
| 165 |
2/2✓ Branch 1 taken 194402 times.
✓ Branch 2 taken 97201 times.
|
291603 | for(int j = sb.size()-1; j>=order; j--) { |
| 166 | 194402 | double a=sb[j][0]; | |
| 167 | 194402 | double b=sb[j][1]; | |
| 168 | |||
| 169 | 194402 | double t = 0; | |
| 170 |
2/2✓ Branch 0 taken 19200 times.
✓ Branch 1 taken 175202 times.
|
194402 | if (lo<0) t = ((b-a)/lo+1)*0.5; |
| 171 |
6/6✓ Branch 0 taken 19200 times.
✓ Branch 1 taken 175202 times.
✓ Branch 2 taken 9000 times.
✓ Branch 3 taken 10200 times.
✓ Branch 4 taken 8400 times.
✓ Branch 5 taken 600 times.
|
194402 | if (lo>=0 || t<t0 || t>t1) { |
| 172 | 193802 | lo = std::min(a*(1-t0)+b*t0+lo*t0*(1-t0),a*(1-t1)+b*t1+lo*t1*(1-t1)); | |
| 173 | }else{ | ||
| 174 | 600 | lo = lerp(t, a+lo*t, b); | |
| 175 | } | ||
| 176 | |||
| 177 |
2/2✓ Branch 0 taken 78001 times.
✓ Branch 1 taken 116401 times.
|
194402 | if (hi>0) t = ((b-a)/hi+1)*0.5; |
| 178 |
6/6✓ Branch 0 taken 78001 times.
✓ Branch 1 taken 116401 times.
✓ Branch 2 taken 55201 times.
✓ Branch 3 taken 22800 times.
✓ Branch 4 taken 52800 times.
✓ Branch 5 taken 2401 times.
|
194402 | if (hi<=0 || t<t0 || t>t1) { |
| 179 | 192001 | hi = std::max(a*(1-t0)+b*t0+hi*t0*(1-t0),a*(1-t1)+b*t1+hi*t1*(1-t1)); | |
| 180 | }else{ | ||
| 181 | 2401 | hi = lerp(t, a+hi*t, b); | |
| 182 | } | ||
| 183 | } | ||
| 184 |
1/2✓ Branch 2 taken 97201 times.
✗ Branch 3 not taken.
|
97201 | Interval res = Interval(lo,hi); |
| 185 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 97201 times.
|
97201 | if (order>0) res*=std::pow(.25,order); |
| 186 |
1/2✓ Branch 1 taken 97201 times.
✗ Branch 2 not taken.
|
194402 | return res; |
| 187 | } | ||
| 188 | |||
| 189 | //-- multi_roots ------------------------------------ | ||
| 190 | // goal: solve f(t)=c for several c at once. | ||
| 191 | /* algo: -compute f at both ends of the given segment [a,b]. | ||
| 192 | -compute bounds m<df(t)<M for df on the segment. | ||
| 193 | let c and C be the levels below and above f(a): | ||
| 194 | going from f(a) down to c with slope m takes at least time (f(a)-c)/m | ||
| 195 | going from f(a) up to C with slope M takes at least time (C-f(a))/M | ||
| 196 | From this we conclude there are no roots before a'=a+min((f(a)-c)/m,(C-f(a))/M). | ||
| 197 | Do the same for b: compute some b' such that there are no roots in (b',b]. | ||
| 198 | -if [a',b'] is not empty, repeat the process with [a',(a'+b')/2] and [(a'+b')/2,b']. | ||
| 199 | unfortunately, extra care is needed about rounding errors, and also to avoid the repetition of roots, | ||
| 200 | making things tricky and unpleasant... | ||
| 201 | */ | ||
| 202 | //TODO: Make sure the code is "rounding-errors proof" and take care about repetition of roots! | ||
| 203 | |||
| 204 | |||
| 205 | 259200 | static int upper_level(vector<double> const &levels,double x,double tol=0.){ | |
| 206 |
1/2✓ Branch 7 taken 259200 times.
✗ Branch 8 not taken.
|
259200 | return(upper_bound(levels.begin(),levels.end(),x-tol)-levels.begin()); |
| 207 | } | ||
| 208 | |||
| 209 | #ifdef USE_SBASIS_OF | ||
| 210 | static void multi_roots_internal(SBasis const &f, | ||
| 211 | SBasis const &df, | ||
| 212 | #else | ||
| 213 | 111000 | static void multi_roots_internal(SBasis const &f, | |
| 214 | SBasis const &df, | ||
| 215 | #endif | ||
| 216 | std::vector<double> const &levels, | ||
| 217 | std::vector<std::vector<double> > &roots, | ||
| 218 | double htol, | ||
| 219 | double vtol, | ||
| 220 | double a, | ||
| 221 | double fa, | ||
| 222 | double b, | ||
| 223 | double fb){ | ||
| 224 | |||
| 225 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 111000 times.
|
111000 | if (f.isZero(0)){ |
| 226 | int idx; | ||
| 227 | ✗ | idx=upper_level(levels,0,vtol); | |
| 228 | ✗ | if (idx<(int)levels.size()&&fabs(levels.at(idx))<=vtol){ | |
| 229 | ✗ | roots[idx].push_back(a); | |
| 230 | ✗ | roots[idx].push_back(b); | |
| 231 | } | ||
| 232 | ✗ | return; | |
| 233 | } | ||
| 234 | ////useful? | ||
| 235 | // if (f.size()==1){ | ||
| 236 | // int idxa=upper_level(levels,fa); | ||
| 237 | // int idxb=upper_level(levels,fb); | ||
| 238 | // if (fa==fb){ | ||
| 239 | // if (fa==levels[idxa]){ | ||
| 240 | // roots[a]=idxa; | ||
| 241 | // roots[b]=idxa; | ||
| 242 | // } | ||
| 243 | // return; | ||
| 244 | // } | ||
| 245 | // int idx_min=std::min(idxa,idxb); | ||
| 246 | // int idx_max=std::max(idxa,idxb); | ||
| 247 | // if (idx_max==levels.size()) idx_max-=1; | ||
| 248 | // for(int i=idx_min;i<=idx_max; i++){ | ||
| 249 | // double t=a+(b-a)*(levels[i]-fa)/(fb-fa); | ||
| 250 | // if(a<t&&t<b) roots[t]=i; | ||
| 251 | // } | ||
| 252 | // return; | ||
| 253 | // } | ||
| 254 |
2/2✓ Branch 0 taken 13800 times.
✓ Branch 1 taken 97200 times.
|
111000 | if ((b-a)<htol){ |
| 255 | //TODO: use different tol for t and f ? | ||
| 256 | //TODO: unsigned idx ? (remove int casts when fixed) | ||
| 257 |
2/4✓ Branch 2 taken 13800 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 13800 times.
✗ Branch 7 not taken.
|
13800 | int idx=std::min(upper_level(levels,fa,vtol),upper_level(levels,fb,vtol)); |
| 258 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 13800 times.
|
13800 | if (idx==(int)levels.size()) idx-=1; |
| 259 |
1/2✓ Branch 1 taken 13800 times.
✗ Branch 2 not taken.
|
13800 | double c=levels.at(idx); |
| 260 |
6/6✓ Branch 0 taken 4800 times.
✓ Branch 1 taken 9000 times.
✓ Branch 2 taken 3000 times.
✓ Branch 3 taken 1800 times.
✓ Branch 4 taken 2400 times.
✓ Branch 5 taken 600 times.
|
13800 | if((fa-c)*(fb-c)<=0||fabs(fa-c)<vtol||fabs(fb-c)<vtol){ |
| 261 |
1/2✓ Branch 3 taken 13200 times.
✗ Branch 4 not taken.
|
13200 | roots[idx].push_back((a+b)/2); |
| 262 | } | ||
| 263 | 13800 | return; | |
| 264 | } | ||
| 265 | |||
| 266 |
1/2✓ Branch 1 taken 97200 times.
✗ Branch 2 not taken.
|
97200 | int idxa=upper_level(levels,fa,vtol); |
| 267 |
1/2✓ Branch 1 taken 97200 times.
✗ Branch 2 not taken.
|
97200 | int idxb=upper_level(levels,fb,vtol); |
| 268 | |||
| 269 |
3/6✓ Branch 5 taken 97200 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 97200 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 97200 times.
✗ Branch 12 not taken.
|
97200 | Interval bs = *bounds_local(df,Interval(a,b)); |
| 270 | |||
| 271 | //first times when a level (higher or lower) can be reached from a or b. | ||
| 272 | 97200 | double ta_hi,tb_hi,ta_lo,tb_lo; | |
| 273 | 97200 | ta_hi=ta_lo=b+1;//default values => no root there. | |
| 274 | 97200 | tb_hi=tb_lo=a-1;//default values => no root there. | |
| 275 | |||
| 276 |
6/8✓ Branch 1 taken 97200 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 97200 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 2400 times.
✓ Branch 7 taken 94800 times.
✓ Branch 8 taken 2400 times.
✓ Branch 9 taken 94800 times.
|
97200 | if (idxa<(int)levels.size() && fabs(fa-levels.at(idxa))<vtol){//a can be considered a root. |
| 277 | //ta_hi=ta_lo=a; | ||
| 278 |
1/2✓ Branch 2 taken 2400 times.
✗ Branch 3 not taken.
|
2400 | roots[idxa].push_back(a); |
| 279 | 2400 | ta_hi=ta_lo=a+htol; | |
| 280 | }else{ | ||
| 281 |
5/6✓ Branch 1 taken 41400 times.
✓ Branch 2 taken 53400 times.
✓ Branch 4 taken 41400 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 41400 times.
✓ Branch 7 taken 53400 times.
|
94800 | if (bs.max()>0 && idxa<(int)levels.size()) |
| 282 |
1/2✓ Branch 1 taken 41400 times.
✗ Branch 2 not taken.
|
41400 | ta_hi=a+(levels.at(idxa )-fa)/bs.max(); |
| 283 |
5/6✓ Branch 1 taken 56400 times.
✓ Branch 2 taken 38400 times.
✓ Branch 3 taken 56400 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 56400 times.
✓ Branch 6 taken 38400 times.
|
94800 | if (bs.min()<0 && idxa>0) |
| 284 |
1/2✓ Branch 1 taken 56400 times.
✗ Branch 2 not taken.
|
56400 | ta_lo=a+(levels.at(idxa-1)-fa)/bs.min(); |
| 285 | } | ||
| 286 |
6/8✓ Branch 1 taken 97200 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 97200 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 2400 times.
✓ Branch 7 taken 94800 times.
✓ Branch 8 taken 2400 times.
✓ Branch 9 taken 94800 times.
|
97200 | if (idxb<(int)levels.size() && fabs(fb-levels.at(idxb))<vtol){//b can be considered a root. |
| 287 | //tb_hi=tb_lo=b; | ||
| 288 |
1/2✓ Branch 2 taken 2400 times.
✗ Branch 3 not taken.
|
2400 | roots[idxb].push_back(b); |
| 289 | 2400 | tb_hi=tb_lo=b-htol; | |
| 290 | }else{ | ||
| 291 |
5/6✓ Branch 1 taken 57000 times.
✓ Branch 2 taken 37800 times.
✓ Branch 4 taken 57000 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 57000 times.
✓ Branch 7 taken 37800 times.
|
94800 | if (bs.min()<0 && idxb<(int)levels.size()) |
| 292 |
1/2✓ Branch 1 taken 57000 times.
✗ Branch 2 not taken.
|
57000 | tb_hi=b+(levels.at(idxb )-fb)/bs.min(); |
| 293 |
5/6✓ Branch 1 taken 40800 times.
✓ Branch 2 taken 54000 times.
✓ Branch 3 taken 40800 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 40800 times.
✓ Branch 6 taken 54000 times.
|
94800 | if (bs.max()>0 && idxb>0) |
| 294 |
1/2✓ Branch 1 taken 40800 times.
✗ Branch 2 not taken.
|
40800 | tb_lo=b+(levels.at(idxb-1)-fb)/bs.max(); |
| 295 | } | ||
| 296 | |||
| 297 | double t0,t1; | ||
| 298 | 97200 | t0=std::min(ta_hi,ta_lo); | |
| 299 | 97200 | t1=std::max(tb_hi,tb_lo); | |
| 300 | //hum, rounding errors frighten me! so I add this +tol... | ||
| 301 |
2/2✓ Branch 0 taken 47400 times.
✓ Branch 1 taken 49800 times.
|
97200 | if (t0>t1+htol) return;//no root here. |
| 302 | |||
| 303 |
2/2✓ Branch 0 taken 12600 times.
✓ Branch 1 taken 37200 times.
|
49800 | if (fabs(t1-t0)<htol){ |
| 304 |
1/2✓ Branch 3 taken 12600 times.
✗ Branch 4 not taken.
|
12600 | multi_roots_internal(f,df,levels,roots,htol,vtol,t0,f(t0),t1,f(t1)); |
| 305 | }else{ | ||
| 306 | 37200 | double t,t_left,t_right,ft,ft_left,ft_right; | |
| 307 | 37200 | t_left =t_right =t =(t0+t1)/2; | |
| 308 | 37200 | ft_left=ft_right=ft=f(t); | |
| 309 |
1/2✓ Branch 1 taken 37200 times.
✗ Branch 2 not taken.
|
37200 | int idx=upper_level(levels,ft,vtol); |
| 310 |
4/8✓ Branch 1 taken 37200 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37200 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 37200 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 37200 times.
|
37200 | if (idx<(int)levels.size() && fabs(ft-levels.at(idx))<vtol){//t can be considered a root. |
| 311 | ✗ | roots[idx].push_back(t); | |
| 312 | //we do not want to count it twice (from the left and from the right) | ||
| 313 | ✗ | t_left =t-htol/2; | |
| 314 | ✗ | t_right=t+htol/2; | |
| 315 | ✗ | ft_left =f(t_left); | |
| 316 | ✗ | ft_right=f(t_right); | |
| 317 | } | ||
| 318 |
1/2✓ Branch 2 taken 37200 times.
✗ Branch 3 not taken.
|
37200 | multi_roots_internal(f,df,levels,roots,htol,vtol,t0 ,f(t0) ,t_left,ft_left); |
| 319 |
1/2✓ Branch 2 taken 37200 times.
✗ Branch 3 not taken.
|
37200 | multi_roots_internal(f,df,levels,roots,htol,vtol,t_right,ft_right,t1 ,f(t1) ); |
| 320 | } | ||
| 321 | } | ||
| 322 | |||
| 323 | /** Solve f(t)=c for several c at once. | ||
| 324 | \param f sbasis function | ||
| 325 | \param levels vector of 'y' values | ||
| 326 | \param htol, vtol | ||
| 327 | \param a, b left and right bounds | ||
| 328 | \returns a vector of vectors, one for each y giving roots | ||
| 329 | |||
| 330 | Effectively computes: | ||
| 331 | results = roots(f(y_i)) for all y_i | ||
| 332 | |||
| 333 | * algo: -compute f at both ends of the given segment [a,b]. | ||
| 334 | -compute bounds m<df(t)<M for df on the segment. | ||
| 335 | let c and C be the levels below and above f(a): | ||
| 336 | going from f(a) down to c with slope m takes at least time (f(a)-c)/m | ||
| 337 | going from f(a) up to C with slope M takes at least time (C-f(a))/M | ||
| 338 | From this we conclude there are no roots before a'=a+min((f(a)-c)/m,(C-f(a))/M). | ||
| 339 | Do the same for b: compute some b' such that there are no roots in (b',b]. | ||
| 340 | -if [a',b'] is not empty, repeat the process with [a',(a'+b')/2] and [(a'+b')/2,b']. | ||
| 341 | unfortunately, extra care is needed about rounding errors, and also to avoid the repetition of roots, | ||
| 342 | making things tricky and unpleasant... | ||
| 343 | |||
| 344 | TODO: Make sure the code is "rounding-errors proof" and take care about repetition of roots! | ||
| 345 | */ | ||
| 346 | 24000 | std::vector<std::vector<double> > multi_roots(SBasis const &f, | |
| 347 | std::vector<double> const &levels, | ||
| 348 | double htol, | ||
| 349 | double vtol, | ||
| 350 | double a, | ||
| 351 | double b){ | ||
| 352 | |||
| 353 |
1/2✓ Branch 5 taken 24000 times.
✗ Branch 6 not taken.
|
72000 | std::vector<std::vector<double> > roots(levels.size(), std::vector<double>()); |
| 354 | |||
| 355 |
1/2✓ Branch 2 taken 24000 times.
✗ Branch 3 not taken.
|
24000 | SBasis df=derivative(f); |
| 356 |
1/2✓ Branch 3 taken 24000 times.
✗ Branch 4 not taken.
|
24000 | multi_roots_internal(f,df,levels,roots,htol,vtol,a,f(a),b,f(b)); |
| 357 | |||
| 358 | 48000 | return(roots); | |
| 359 | 24000 | } | |
| 360 | |||
| 361 | |||
| 362 | ✗ | static bool compareIntervalMin( Interval I, Interval J ){ | |
| 363 | ✗ | return I.min()<J.min(); | |
| 364 | } | ||
| 365 | ✗ | static bool compareIntervalMax( Interval I, Interval J ){ | |
| 366 | ✗ | return I.max()<J.max(); | |
| 367 | } | ||
| 368 | |||
| 369 | //find the first interval whose max is >= x | ||
| 370 | ✗ | static unsigned upper_level(vector<Interval> const &levels, double x ){ | |
| 371 | ✗ | return( lower_bound( levels.begin(), levels.end(), Interval(x,x), compareIntervalMax) - levels.begin() ); | |
| 372 | } | ||
| 373 | |||
| 374 | ✗ | static std::vector<Interval> fuseContiguous(std::vector<Interval> const &sets, double tol=0.){ | |
| 375 | ✗ | std::vector<Interval> result; | |
| 376 | ✗ | if (sets.empty() ) return result; | |
| 377 | ✗ | result.push_back( sets.front() ); | |
| 378 | ✗ | for (unsigned i=1; i < sets.size(); i++ ){ | |
| 379 | ✗ | if ( result.back().max() + tol >= sets[i].min() ){ | |
| 380 | ✗ | result.back().unionWith( sets[i] ); | |
| 381 | }else{ | ||
| 382 | ✗ | result.push_back( sets[i] ); | |
| 383 | } | ||
| 384 | } | ||
| 385 | ✗ | return result; | |
| 386 | ✗ | } | |
| 387 | |||
| 388 | /** level_sets internal method. | ||
| 389 | * algorithm: (~adaptation of Newton method versus 'accroissements finis') | ||
| 390 | -compute f at both ends of the given segment [a,b]. | ||
| 391 | -compute bounds m<df(t)<M for df on the segment. | ||
| 392 | Suppose f(a) is between two 'levels' c and C. Then | ||
| 393 | f won't enter c before a + (f(a)-c.max())/m | ||
| 394 | f won't enter C before a + (C.min()-f(a))/M | ||
| 395 | From this we conclude nothing happens before a'=a+min((f(a)-c.max())/m,(C.min()-f(a))/M). | ||
| 396 | We do the same for b: compute some b' such that nothing happens in (b',b]. | ||
| 397 | -if [a',b'] is not empty, repeat the process with [a',(a'+b')/2] and [(a'+b')/2,b']. | ||
| 398 | |||
| 399 | If f(a) or f(b) belongs to some 'level' C, then use the same argument to find a' or b' such | ||
| 400 | that f remains in C on [a,a'] or [b',b]. In case f is monotonic, we also know f won't enter another | ||
| 401 | level before or after some time, allowing us to restrict the search a little more. | ||
| 402 | |||
| 403 | unfortunately, extra care is needed about rounding errors, and also to avoid the repetition of roots, | ||
| 404 | making things tricky and unpleasant... | ||
| 405 | */ | ||
| 406 | |||
| 407 | ✗ | static void level_sets_internal(SBasis const &f, | |
| 408 | SBasis const &df, | ||
| 409 | std::vector<Interval> const &levels, | ||
| 410 | std::vector<std::vector<Interval> > &solsets, | ||
| 411 | double a, | ||
| 412 | double fa, | ||
| 413 | double b, | ||
| 414 | double fb, | ||
| 415 | double tol=1e-5){ | ||
| 416 | |||
| 417 | ✗ | if (f.isZero(0)){ | |
| 418 | unsigned idx; | ||
| 419 | ✗ | idx=upper_level( levels, 0. ); | |
| 420 | ✗ | if (idx<levels.size() && levels[idx].contains(0.)){ | |
| 421 | ✗ | solsets[idx].push_back( Interval(a,b) ) ; | |
| 422 | } | ||
| 423 | ✗ | return; | |
| 424 | } | ||
| 425 | |||
| 426 | ✗ | unsigned idxa=upper_level(levels,fa); | |
| 427 | ✗ | unsigned idxb=upper_level(levels,fb); | |
| 428 | |||
| 429 | ✗ | Interval bs = *bounds_local(df,Interval(a,b)); | |
| 430 | |||
| 431 | //first times when a level (higher or lower) can be reached from a or b. | ||
| 432 | ✗ | double ta_hi; // f remains below next level for t<ta_hi | |
| 433 | ✗ | double ta_lo; // f remains above prev level for t<ta_lo | |
| 434 | ✗ | double tb_hi; // f remains below next level for t>tb_hi | |
| 435 | ✗ | double tb_lo; // f remains above next level for t>tb_lo | |
| 436 | |||
| 437 | ✗ | ta_hi=ta_lo=b+1;//default values => no root there. | |
| 438 | ✗ | tb_hi=tb_lo=a-1;//default values => no root there. | |
| 439 | |||
| 440 | //--- if f(a) belongs to a level.------- | ||
| 441 | ✗ | if ( idxa < levels.size() && levels[idxa].contains( fa ) ){ | |
| 442 | //find the first time when we may exit this level. | ||
| 443 | ✗ | ta_lo = a + ( levels[idxa].min() - fa)/bs.min(); | |
| 444 | ✗ | ta_hi = a + ( levels[idxa].max() - fa)/bs.max(); | |
| 445 | ✗ | if ( ta_lo < a || ta_lo > b ) ta_lo = b; | |
| 446 | ✗ | if ( ta_hi < a || ta_hi > b ) ta_hi = b; | |
| 447 | //move to that time for the next iteration. | ||
| 448 | ✗ | solsets[idxa].push_back( Interval( a, std::min( ta_lo, ta_hi ) ) ); | |
| 449 | }else{ | ||
| 450 | //--- if f(b) does not belong to a level.------- | ||
| 451 | ✗ | if ( idxa == 0 ){ | |
| 452 | ✗ | ta_lo = b; | |
| 453 | }else{ | ||
| 454 | ✗ | ta_lo = a + ( levels[idxa-1].max() - fa)/bs.min(); | |
| 455 | ✗ | if ( ta_lo < a ) ta_lo = b; | |
| 456 | } | ||
| 457 | ✗ | if ( idxa == levels.size() ){ | |
| 458 | ✗ | ta_hi = b; | |
| 459 | }else{ | ||
| 460 | ✗ | ta_hi = a + ( levels[idxa].min() - fa)/bs.max(); | |
| 461 | ✗ | if ( ta_hi < a ) ta_hi = b; | |
| 462 | } | ||
| 463 | } | ||
| 464 | |||
| 465 | //--- if f(b) belongs to a level.------- | ||
| 466 | ✗ | if (idxb<levels.size() && levels.at(idxb).contains(fb)){ | |
| 467 | //find the first time from b when we may exit this level. | ||
| 468 | ✗ | tb_lo = b + ( levels[idxb].min() - fb ) / bs.max(); | |
| 469 | ✗ | tb_hi = b + ( levels[idxb].max() - fb ) / bs.min(); | |
| 470 | ✗ | if ( tb_lo > b || tb_lo < a ) tb_lo = a; | |
| 471 | ✗ | if ( tb_hi > b || tb_hi < a ) tb_hi = a; | |
| 472 | //move to that time for the next iteration. | ||
| 473 | ✗ | solsets[idxb].push_back( Interval( std::max( tb_lo, tb_hi ), b) ); | |
| 474 | }else{ | ||
| 475 | //--- if f(b) does not belong to a level.------- | ||
| 476 | ✗ | if ( idxb == 0 ){ | |
| 477 | ✗ | tb_lo = a; | |
| 478 | }else{ | ||
| 479 | ✗ | tb_lo = b + ( levels[idxb-1].max() - fb)/bs.max(); | |
| 480 | ✗ | if ( tb_lo > b ) tb_lo = a; | |
| 481 | } | ||
| 482 | ✗ | if ( idxb == levels.size() ){ | |
| 483 | ✗ | tb_hi = a; | |
| 484 | }else{ | ||
| 485 | ✗ | tb_hi = b + ( levels[idxb].min() - fb)/bs.min(); | |
| 486 | ✗ | if ( tb_hi > b ) tb_hi = a; | |
| 487 | } | ||
| 488 | |||
| 489 | |||
| 490 | ✗ | if ( bs.min() < 0 && idxb < levels.size() ) | |
| 491 | ✗ | tb_hi = b + ( levels[idxb ].min() - fb ) / bs.min(); | |
| 492 | ✗ | if ( bs.max() > 0 && idxb > 0 ) | |
| 493 | ✗ | tb_lo = b + ( levels[idxb-1].max() - fb ) / bs.max(); | |
| 494 | } | ||
| 495 | |||
| 496 | //let [t0,t1] be the next interval where to search. | ||
| 497 | ✗ | double t0=std::min(ta_hi,ta_lo); | |
| 498 | ✗ | double t1=std::max(tb_hi,tb_lo); | |
| 499 | |||
| 500 | ✗ | if (t0>=t1) return;//no root here. | |
| 501 | |||
| 502 | //if the interval is smaller than our resolution: | ||
| 503 | //pretend f simultaneously meets all the levels between f(t0) and f(t1)... | ||
| 504 | ✗ | if ( t1 - t0 <= tol ){ | |
| 505 | ✗ | Interval f_t0t1 ( f(t0), f(t1) ); | |
| 506 | ✗ | unsigned idxmin = std::min(idxa, idxb); | |
| 507 | ✗ | unsigned idxmax = std::max(idxa, idxb); | |
| 508 | //push [t0,t1] into all crossed level. Cheat to avoid overlapping intervals on different levels? | ||
| 509 | ✗ | if ( idxmax > idxmin ){ | |
| 510 | ✗ | for (unsigned idx = idxmin; idx < idxmax; idx++){ | |
| 511 | ✗ | solsets[idx].push_back( Interval( t0, t1 ) ); | |
| 512 | } | ||
| 513 | } | ||
| 514 | ✗ | if ( idxmax < levels.size() && f_t0t1.intersects( levels[idxmax] ) ){ | |
| 515 | ✗ | solsets[idxmax].push_back( Interval( t0, t1 ) ); | |
| 516 | } | ||
| 517 | ✗ | return; | |
| 518 | } | ||
| 519 | |||
| 520 | //To make sure we finally exit the level jump at least by tol: | ||
| 521 | ✗ | t0 = std::min( std::max( t0, a + tol ), b ); | |
| 522 | ✗ | t1 = std::max( std::min( t1, b - tol ), a ); | |
| 523 | |||
| 524 | ✗ | double t =(t0+t1)/2; | |
| 525 | ✗ | double ft=f(t); | |
| 526 | ✗ | level_sets_internal( f, df, levels, solsets, t0, f(t0), t, ft ); | |
| 527 | ✗ | level_sets_internal( f, df, levels, solsets, t, ft, t1, f(t1) ); | |
| 528 | } | ||
| 529 | |||
| 530 | ✗ | std::vector<std::vector<Interval> > level_sets(SBasis const &f, | |
| 531 | std::vector<Interval> const &levels, | ||
| 532 | double a, double b, double tol){ | ||
| 533 | |||
| 534 | ✗ | std::vector<std::vector<Interval> > solsets(levels.size(), std::vector<Interval>()); | |
| 535 | |||
| 536 | ✗ | SBasis df=derivative(f); | |
| 537 | ✗ | level_sets_internal(f,df,levels,solsets,a,f(a),b,f(b),tol); | |
| 538 | // Fuse overlapping intervals... | ||
| 539 | ✗ | for (auto & solset : solsets){ | |
| 540 | ✗ | if ( solset.size() == 0 ) continue; | |
| 541 | ✗ | std::sort( solset.begin(), solset.end(), compareIntervalMin ); | |
| 542 | ✗ | solset = fuseContiguous( solset, tol ); | |
| 543 | } | ||
| 544 | ✗ | return solsets; | |
| 545 | ✗ | } | |
| 546 | |||
| 547 | ✗ | std::vector<Interval> level_set (SBasis const &f, double level, double vtol, double a, double b, double tol){ | |
| 548 | ✗ | Interval fat_level( level - vtol, level + vtol ); | |
| 549 | ✗ | return level_set(f, fat_level, a, b, tol); | |
| 550 | } | ||
| 551 | ✗ | std::vector<Interval> level_set (SBasis const &f, Interval const &level, double a, double b, double tol){ | |
| 552 | ✗ | std::vector<Interval> levels(1,level); | |
| 553 | ✗ | return level_sets(f,levels, a, b, tol).front() ; | |
| 554 | ✗ | } | |
| 555 | ✗ | std::vector<std::vector<Interval> > level_sets (SBasis const &f, std::vector<double> const &levels, double vtol, double a, double b, double tol){ | |
| 556 | ✗ | std::vector<Interval> fat_levels( levels.size(), Interval()); | |
| 557 | ✗ | for (unsigned i = 0; i < levels.size(); i++){ | |
| 558 | ✗ | fat_levels[i] = Interval( levels[i]-vtol, levels[i]+vtol); | |
| 559 | } | ||
| 560 | ✗ | return level_sets(f, fat_levels, a, b, tol); | |
| 561 | ✗ | } | |
| 562 | |||
| 563 | |||
| 564 | //------------------------------------- | ||
| 565 | //------------------------------------- | ||
| 566 | |||
| 567 | |||
| 568 | ✗ | void subdiv_sbasis(SBasis const & s, | |
| 569 | std::vector<double> & roots, | ||
| 570 | double left, double right) { | ||
| 571 | ✗ | OptInterval bs = bounds_fast(s); | |
| 572 | ✗ | if(!bs || bs->min() > 0 || bs->max() < 0) | |
| 573 | ✗ | return; // no roots here | |
| 574 | ✗ | if(s.tailError(1) < 1e-7) { | |
| 575 | ✗ | double t = s[0][0] / (s[0][0] - s[0][1]); | |
| 576 | ✗ | roots.push_back(left*(1-t) + t*right); | |
| 577 | ✗ | return; | |
| 578 | } | ||
| 579 | ✗ | double middle = (left + right)/2; | |
| 580 | ✗ | subdiv_sbasis(compose(s, Linear(0, 0.5)), roots, left, middle); | |
| 581 | ✗ | subdiv_sbasis(compose(s, Linear(0.5, 1.)), roots, middle, right); | |
| 582 | } | ||
| 583 | |||
| 584 | // It is faster to use the bernstein root finder for small degree polynomials (<100?. | ||
| 585 | |||
| 586 | 184203 | std::vector<double> roots1(SBasis const & s) { | |
| 587 | 184203 | std::vector<double> res; | |
| 588 | 184203 | double d = s[0][0] - s[0][1]; | |
| 589 |
2/2✓ Branch 0 taken 31952 times.
✓ Branch 1 taken 152251 times.
|
184203 | if(d != 0) { |
| 590 | 31952 | double r = s[0][0] / d; | |
| 591 |
2/4✓ Branch 0 taken 31952 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 31952 times.
✗ Branch 3 not taken.
|
31952 | if(0 <= r && r <= 1) |
| 592 |
1/2✓ Branch 1 taken 31952 times.
✗ Branch 2 not taken.
|
31952 | res.push_back(r); |
| 593 | } | ||
| 594 | 184203 | return res; | |
| 595 | ✗ | } | |
| 596 | |||
| 597 | ✗ | std::vector<double> roots1(SBasis const & s, Interval const ivl) { | |
| 598 | ✗ | std::vector<double> res; | |
| 599 | ✗ | double d = s[0][0] - s[0][1]; | |
| 600 | ✗ | if(d != 0) { | |
| 601 | ✗ | double r = s[0][0] / d; | |
| 602 | ✗ | if(ivl.contains(r)) | |
| 603 | ✗ | res.push_back(r); | |
| 604 | } | ||
| 605 | ✗ | return res; | |
| 606 | ✗ | } | |
| 607 | |||
| 608 | /** Find all t s.t s(t) = 0 | ||
| 609 | \param a sbasis function | ||
| 610 | \see Bezier::roots | ||
| 611 | \returns vector of zeros (roots) | ||
| 612 | |||
| 613 | */ | ||
| 614 | 251557 | std::vector<double> roots(SBasis const & s) { | |
| 615 |
2/3✗ Branch 1 not taken.
✓ Branch 2 taken 184203 times.
✓ Branch 3 taken 67354 times.
|
251557 | switch(s.size()) { |
| 616 | ✗ | case 0: | |
| 617 | ✗ | assert(false); | |
| 618 | return std::vector<double>(); | ||
| 619 | 184203 | case 1: | |
| 620 | 184203 | return roots1(s); | |
| 621 | 67354 | default: | |
| 622 | { | ||
| 623 | 67354 | Bezier bz; | |
| 624 |
1/2✓ Branch 1 taken 67354 times.
✗ Branch 2 not taken.
|
67354 | sbasis_to_bezier(bz, s); |
| 625 |
1/2✓ Branch 1 taken 67354 times.
✗ Branch 2 not taken.
|
67354 | return bz.roots(); |
| 626 | 67354 | } | |
| 627 | } | ||
| 628 | } | ||
| 629 | ✗ | std::vector<double> roots(SBasis const & s, Interval const ivl) { | |
| 630 | ✗ | switch(s.size()) { | |
| 631 | ✗ | case 0: | |
| 632 | ✗ | assert(false); | |
| 633 | return std::vector<double>(); | ||
| 634 | ✗ | case 1: | |
| 635 | ✗ | return roots1(s, ivl); | |
| 636 | ✗ | default: | |
| 637 | { | ||
| 638 | ✗ | Bezier bz; | |
| 639 | ✗ | sbasis_to_bezier(bz, s); | |
| 640 | ✗ | return bz.roots(ivl); | |
| 641 | ✗ | } | |
| 642 | } | ||
| 643 | } | ||
| 644 | |||
| 645 | }; | ||
| 646 | |||
| 647 | /* | ||
| 648 | Local Variables: | ||
| 649 | mode:c++ | ||
| 650 | c-file-style:"stroustrup" | ||
| 651 | c-file-offsets:((innamespace . 0)(inline-open . 0)(case-label . +)) | ||
| 652 | indent-tabs-mode:nil | ||
| 653 | fill-column:99 | ||
| 654 | End: | ||
| 655 | */ | ||
| 656 | // vim: filetype=cpp:expandtab:shiftwidth=4:tabstop=8:softtabstop=4:fileencoding=utf-8:textwidth=99 : | ||
| 657 |