| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /** @file | ||
| 2 | * @brief Nearest time routines for D2<SBasis> and Piecewise<D2<SBasis>> | ||
| 3 | *//* | ||
| 4 | * Authors: | ||
| 5 | * Marco Cecchetti <mrcekets at gmail.com> | ||
| 6 | * | ||
| 7 | * Copyright 2007-2008 authors | ||
| 8 | * | ||
| 9 | * This library is free software; you can redistribute it and/or | ||
| 10 | * modify it either under the terms of the GNU Lesser General Public | ||
| 11 | * License version 2.1 as published by the Free Software Foundation | ||
| 12 | * (the "LGPL") or, at your option, under the terms of the Mozilla | ||
| 13 | * Public License Version 1.1 (the "MPL"). If you do not alter this | ||
| 14 | * notice, a recipient may use your version of this file under either | ||
| 15 | * the MPL or the LGPL. | ||
| 16 | * | ||
| 17 | * You should have received a copy of the LGPL along with this library | ||
| 18 | * in the file COPYING-LGPL-2.1; if not, write to the Free Software | ||
| 19 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA | ||
| 20 | * You should have received a copy of the MPL along with this library | ||
| 21 | * in the file COPYING-MPL-1.1 | ||
| 22 | * | ||
| 23 | * The contents of this file are subject to the Mozilla Public License | ||
| 24 | * Version 1.1 (the "License"); you may not use this file except in | ||
| 25 | * compliance with the License. You may obtain a copy of the License at | ||
| 26 | * http://www.mozilla.org/MPL/ | ||
| 27 | * | ||
| 28 | * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY | ||
| 29 | * OF ANY KIND, either express or implied. See the LGPL or the MPL for | ||
| 30 | * the specific language governing rights and limitations. | ||
| 31 | */ | ||
| 32 | |||
| 33 | |||
| 34 | #include <2geom/nearest-time.h> | ||
| 35 | #include <algorithm> | ||
| 36 | |||
| 37 | namespace Geom | ||
| 38 | { | ||
| 39 | |||
| 40 | 23 | Coord nearest_time(Point const &p, D2<Bezier> const &input, Coord from, Coord to) | |
| 41 | { | ||
| 42 |
1/2✓ Branch 2 taken 23 times.
✗ Branch 3 not taken.
|
23 | Interval domain(from, to); |
| 43 | 23 | bool partial = false; | |
| 44 | |||
| 45 |
3/6✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 23 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 23 times.
|
23 | if (domain.min() < 0 || domain.max() > 1) { |
| 46 | ✗ | THROW_RANGEERROR("[from,to] interval out of bounds"); | |
| 47 | } | ||
| 48 | |||
| 49 |
2/4✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 23 times.
|
23 | if (input.isConstant(0)) return from; |
| 50 | |||
| 51 |
1/2✓ Branch 2 taken 23 times.
✗ Branch 3 not taken.
|
23 | D2<Bezier> bez; |
| 52 |
3/6✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 23 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 23 times.
|
23 | if (domain.min() != 0 || domain.max() != 1) { |
| 53 | ✗ | bez = portion(input, domain) - p; | |
| 54 | ✗ | partial = true; | |
| 55 | } else { | ||
| 56 |
2/4✓ Branch 2 taken 23 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 23 times.
✗ Branch 6 not taken.
|
23 | bez = input - p; |
| 57 | } | ||
| 58 | |||
| 59 | // find extrema of the function x(t)^2 + y(t)^2 | ||
| 60 | // use the fact that (f^2)' = 2 f f' | ||
| 61 | // this reduces the order of the distance function by 1 | ||
| 62 |
1/2✓ Branch 2 taken 23 times.
✗ Branch 3 not taken.
|
23 | D2<Bezier> deriv = derivative(bez); |
| 63 |
4/8✓ Branch 6 taken 23 times.
✗ Branch 7 not taken.
✓ Branch 12 taken 23 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 23 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 23 times.
✗ Branch 19 not taken.
|
23 | std::vector<Coord> ts = (multiply(bez[X], deriv[X]) + multiply(bez[Y], deriv[Y])).roots(); |
| 64 | |||
| 65 | 23 | Coord t = -1, mind = infinity(); | |
| 66 |
2/2✓ Branch 7 taken 28 times.
✓ Branch 8 taken 23 times.
|
51 | for (double i : ts) { |
| 67 |
1/2✓ Branch 2 taken 28 times.
✗ Branch 3 not taken.
|
28 | Coord droot = L2sq(bez.valueAt(i)); |
| 68 |
2/2✓ Branch 0 taken 23 times.
✓ Branch 1 taken 5 times.
|
28 | if (droot < mind) { |
| 69 | 23 | mind = droot; | |
| 70 | 23 | t = i; | |
| 71 | } | ||
| 72 | } | ||
| 73 | |||
| 74 | // also check endpoints | ||
| 75 | 23 | Coord dinitial = L2sq(bez.at0()); | |
| 76 |
1/2✓ Branch 2 taken 23 times.
✗ Branch 3 not taken.
|
23 | Coord dfinal = L2sq(bez.at1()); |
| 77 | |||
| 78 |
2/2✓ Branch 0 taken 2 times.
✓ Branch 1 taken 21 times.
|
23 | if (dinitial < mind) { |
| 79 | 2 | mind = dinitial; | |
| 80 | 2 | t = 0; | |
| 81 | } | ||
| 82 |
2/2✓ Branch 0 taken 3 times.
✓ Branch 1 taken 20 times.
|
23 | if (dfinal < mind) { |
| 83 | //mind = dfinal; | ||
| 84 | 3 | t = 1; | |
| 85 | } | ||
| 86 | |||
| 87 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 23 times.
|
23 | if (partial) { |
| 88 | ✗ | t = domain.valueAt(t); | |
| 89 | } | ||
| 90 | 23 | return t; | |
| 91 | 23 | } | |
| 92 | |||
| 93 | //////////////////////////////////////////////////////////////////////////////// | ||
| 94 | // D2<SBasis> versions | ||
| 95 | |||
| 96 | /* | ||
| 97 | * Return the parameter t of the nearest time value on the portion of the curve "c", | ||
| 98 | * related to the interval [from, to], to the point "p". | ||
| 99 | * The needed curve derivative "dc" is passed as parameter. | ||
| 100 | * The function return the first nearest time value to "p" that is found. | ||
| 101 | */ | ||
| 102 | |||
| 103 | ✗ | double nearest_time(Point const& p, | |
| 104 | D2<SBasis> const& c, | ||
| 105 | D2<SBasis> const& dc, | ||
| 106 | double from, double to ) | ||
| 107 | { | ||
| 108 | ✗ | if ( from > to ) std::swap(from, to); | |
| 109 | ✗ | if ( from < 0 || to > 1 ) | |
| 110 | { | ||
| 111 | ✗ | THROW_RANGEERROR("[from,to] interval out of bounds"); | |
| 112 | } | ||
| 113 | ✗ | if (c.isConstant()) return from; | |
| 114 | ✗ | SBasis dd = dot(c - p, dc); | |
| 115 | //std::cout << dd << std::endl; | ||
| 116 | ✗ | std::vector<double> zeros = Geom::roots(dd); | |
| 117 | |||
| 118 | ✗ | double closest = from; | |
| 119 | ✗ | double min_dist_sq = L2sq(c(from) - p); | |
| 120 | ✗ | for (double zero : zeros) | |
| 121 | { | ||
| 122 | ✗ | double distsq = L2sq(c(zero) - p); | |
| 123 | ✗ | if ( min_dist_sq > L2sq(c(zero) - p) ) | |
| 124 | { | ||
| 125 | ✗ | closest = zero; | |
| 126 | ✗ | min_dist_sq = distsq; | |
| 127 | } | ||
| 128 | } | ||
| 129 | ✗ | if ( min_dist_sq > L2sq( c(to) - p ) ) | |
| 130 | ✗ | closest = to; | |
| 131 | ✗ | return closest; | |
| 132 | |||
| 133 | ✗ | } | |
| 134 | |||
| 135 | /* | ||
| 136 | * Return the parameters t of all the nearest points on the portion of | ||
| 137 | * the curve "c", related to the interval [from, to], to the point "p". | ||
| 138 | * The needed curve derivative "dc" is passed as parameter. | ||
| 139 | */ | ||
| 140 | |||
| 141 | std::vector<double> | ||
| 142 | ✗ | all_nearest_times(Point const &p, | |
| 143 | D2<SBasis> const &c, | ||
| 144 | D2<SBasis> const &dc, | ||
| 145 | double from, double to) | ||
| 146 | { | ||
| 147 | ✗ | if (from > to) { | |
| 148 | ✗ | std::swap(from, to); | |
| 149 | } | ||
| 150 | ✗ | if (from < 0 || to > 1) { | |
| 151 | ✗ | THROW_RANGEERROR("[from,to] interval out of bounds"); | |
| 152 | } | ||
| 153 | |||
| 154 | ✗ | std::vector<double> result; | |
| 155 | ✗ | if (c.isConstant()) { | |
| 156 | ✗ | result.push_back(from); | |
| 157 | ✗ | return result; | |
| 158 | } | ||
| 159 | ✗ | SBasis dd = dot(c - p, dc); | |
| 160 | |||
| 161 | ✗ | std::vector<double> zeros = Geom::roots(dd); | |
| 162 | ✗ | std::vector<double> candidates; | |
| 163 | ✗ | candidates.push_back(from); | |
| 164 | ✗ | candidates.insert(candidates.end(), zeros.begin(), zeros.end()); | |
| 165 | ✗ | candidates.push_back(to); | |
| 166 | ✗ | std::vector<double> distsq; | |
| 167 | ✗ | distsq.reserve(candidates.size()); | |
| 168 | ✗ | for (double candidate : candidates) { | |
| 169 | ✗ | distsq.push_back(L2sq(c(candidate) - p)); | |
| 170 | } | ||
| 171 | ✗ | unsigned closest = 0; | |
| 172 | ✗ | double dsq = distsq[0]; | |
| 173 | ✗ | for (unsigned i = 1; i < candidates.size(); ++i) { | |
| 174 | ✗ | if (dsq > distsq[i]) { | |
| 175 | ✗ | closest = i; | |
| 176 | ✗ | dsq = distsq[i]; | |
| 177 | } | ||
| 178 | } | ||
| 179 | ✗ | for (unsigned i = 0; i < candidates.size(); ++i) { | |
| 180 | ✗ | if (distsq[closest] == distsq[i]) { | |
| 181 | ✗ | result.push_back(candidates[i]); | |
| 182 | } | ||
| 183 | } | ||
| 184 | ✗ | return result; | |
| 185 | ✗ | } | |
| 186 | |||
| 187 | |||
| 188 | //////////////////////////////////////////////////////////////////////////////// | ||
| 189 | // Piecewise< D2<SBasis> > versions | ||
| 190 | |||
| 191 | |||
| 192 | ✗ | double nearest_time(Point const &p, | |
| 193 | Piecewise< D2<SBasis> > const &c, | ||
| 194 | double from, double to) | ||
| 195 | { | ||
| 196 | ✗ | if (from > to) std::swap(from, to); | |
| 197 | ✗ | if (from < c.cuts[0] || to > c.cuts[c.size()]) { | |
| 198 | ✗ | THROW_RANGEERROR("[from,to] interval out of bounds"); | |
| 199 | } | ||
| 200 | |||
| 201 | ✗ | unsigned si = c.segN(from); | |
| 202 | ✗ | unsigned ei = c.segN(to); | |
| 203 | ✗ | if (si == ei) { | |
| 204 | double nearest = | ||
| 205 | ✗ | nearest_time(p, c[si], c.segT(from, si), c.segT(to, si)); | |
| 206 | ✗ | return c.mapToDomain(nearest, si); | |
| 207 | } | ||
| 208 | |||
| 209 | double t; | ||
| 210 | ✗ | double nearest = nearest_time(p, c[si], c.segT(from, si)); | |
| 211 | ✗ | unsigned int ni = si; | |
| 212 | double dsq; | ||
| 213 | ✗ | double mindistsq = distanceSq(p, c[si](nearest)); | |
| 214 | ✗ | Rect bb; | |
| 215 | ✗ | for (unsigned i = si + 1; i < ei; ++i) { | |
| 216 | ✗ | bb = *bounds_fast(c[i]); | |
| 217 | ✗ | dsq = distanceSq(p, bb); | |
| 218 | ✗ | if ( mindistsq <= dsq ) continue; | |
| 219 | |||
| 220 | ✗ | t = nearest_time(p, c[i]); | |
| 221 | ✗ | dsq = distanceSq(p, c[i](t)); | |
| 222 | ✗ | if (mindistsq > dsq) { | |
| 223 | ✗ | nearest = t; | |
| 224 | ✗ | ni = i; | |
| 225 | ✗ | mindistsq = dsq; | |
| 226 | } | ||
| 227 | } | ||
| 228 | ✗ | bb = *bounds_fast(c[ei]); | |
| 229 | ✗ | dsq = distanceSq(p, bb); | |
| 230 | ✗ | if (mindistsq > dsq) { | |
| 231 | ✗ | t = nearest_time(p, c[ei], 0, c.segT(to, ei)); | |
| 232 | ✗ | dsq = distanceSq(p, c[ei](t)); | |
| 233 | ✗ | if (mindistsq > dsq) { | |
| 234 | ✗ | nearest = t; | |
| 235 | ✗ | ni = ei; | |
| 236 | } | ||
| 237 | } | ||
| 238 | ✗ | return c.mapToDomain(nearest, ni); | |
| 239 | } | ||
| 240 | |||
| 241 | std::vector<double> | ||
| 242 | ✗ | all_nearest_times(Point const &p, | |
| 243 | Piecewise< D2<SBasis> > const &c, | ||
| 244 | double from, double to) | ||
| 245 | { | ||
| 246 | ✗ | if (from > to) { | |
| 247 | ✗ | std::swap(from, to); | |
| 248 | } | ||
| 249 | ✗ | if (from < c.cuts[0] || to > c.cuts[c.size()]) { | |
| 250 | ✗ | THROW_RANGEERROR("[from,to] interval out of bounds"); | |
| 251 | } | ||
| 252 | |||
| 253 | ✗ | unsigned si = c.segN(from); | |
| 254 | ✗ | unsigned ei = c.segN(to); | |
| 255 | ✗ | if ( si == ei ) | |
| 256 | { | ||
| 257 | ✗ | std::vector<double> all_nearest = | |
| 258 | ✗ | all_nearest_times(p, c[si], c.segT(from, si), c.segT(to, si)); | |
| 259 | ✗ | for (double & i : all_nearest) | |
| 260 | { | ||
| 261 | ✗ | i = c.mapToDomain(i, si); | |
| 262 | } | ||
| 263 | ✗ | return all_nearest; | |
| 264 | ✗ | } | |
| 265 | ✗ | std::vector<double> all_t; | |
| 266 | ✗ | std::vector< std::vector<double> > all_np; | |
| 267 | ✗ | all_np.push_back( all_nearest_times(p, c[si], c.segT(from, si)) ); | |
| 268 | ✗ | std::vector<unsigned> ni; | |
| 269 | ✗ | ni.push_back(si); | |
| 270 | double dsq; | ||
| 271 | ✗ | double mindistsq = distanceSq( p, c[si](all_np.front().front()) ); | |
| 272 | ✗ | Rect bb; | |
| 273 | |||
| 274 | ✗ | for (unsigned i = si + 1; i < ei; ++i) { | |
| 275 | ✗ | bb = *bounds_fast(c[i]); | |
| 276 | ✗ | dsq = distanceSq(p, bb); | |
| 277 | ✗ | if ( mindistsq < dsq ) continue; | |
| 278 | ✗ | all_t = all_nearest_times(p, c[i]); | |
| 279 | ✗ | dsq = distanceSq( p, c[i](all_t.front()) ); | |
| 280 | ✗ | if ( mindistsq > dsq ) | |
| 281 | { | ||
| 282 | ✗ | all_np.clear(); | |
| 283 | ✗ | all_np.push_back(all_t); | |
| 284 | ✗ | ni.clear(); | |
| 285 | ✗ | ni.push_back(i); | |
| 286 | ✗ | mindistsq = dsq; | |
| 287 | } | ||
| 288 | ✗ | else if ( mindistsq == dsq ) | |
| 289 | { | ||
| 290 | ✗ | all_np.push_back(all_t); | |
| 291 | ✗ | ni.push_back(i); | |
| 292 | } | ||
| 293 | } | ||
| 294 | ✗ | bb = *bounds_fast(c[ei]); | |
| 295 | ✗ | dsq = distanceSq(p, bb); | |
| 296 | ✗ | if (mindistsq >= dsq) { | |
| 297 | ✗ | all_t = all_nearest_times(p, c[ei], 0, c.segT(to, ei)); | |
| 298 | ✗ | dsq = distanceSq( p, c[ei](all_t.front()) ); | |
| 299 | ✗ | if (mindistsq > dsq) { | |
| 300 | ✗ | for (double & i : all_t) { | |
| 301 | ✗ | i = c.mapToDomain(i, ei); | |
| 302 | } | ||
| 303 | ✗ | return all_t; | |
| 304 | ✗ | } else if (mindistsq == dsq) { | |
| 305 | ✗ | all_np.push_back(all_t); | |
| 306 | ✗ | ni.push_back(ei); | |
| 307 | } | ||
| 308 | } | ||
| 309 | ✗ | std::vector<double> all_nearest; | |
| 310 | ✗ | for (unsigned i = 0; i < all_np.size(); ++i) { | |
| 311 | ✗ | for (unsigned int j = 0; j < all_np[i].size(); ++j) { | |
| 312 | ✗ | all_nearest.push_back( c.mapToDomain(all_np[i][j], ni[i]) ); | |
| 313 | } | ||
| 314 | } | ||
| 315 | ✗ | all_nearest.erase(std::unique(all_nearest.begin(), all_nearest.end()), | |
| 316 | ✗ | all_nearest.end()); | |
| 317 | ✗ | return all_nearest; | |
| 318 | ✗ | } | |
| 319 | |||
| 320 | } // end namespace Geom | ||
| 321 | |||
| 322 | |||
| 323 |