Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2001-2026 German Aerospace Center (DLR) and others.
4 : // This program and the accompanying materials are made available under the
5 : // terms of the Eclipse Public License 2.0 which is available at
6 : // https://www.eclipse.org/legal/epl-2.0/
7 : // This Source Code may also be made available under the following Secondary
8 : // Licenses when the conditions for such availability set forth in the Eclipse
9 : // Public License 2.0 are satisfied: GNU General Public License, version 2
10 : // or later which is available at
11 : // https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
12 : // SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
13 : /****************************************************************************/
14 : /// @file PositionVector.cpp
15 : /// @author Daniel Krajzewicz
16 : /// @author Jakob Erdmann
17 : /// @author Michael Behrisch
18 : /// @author Walter Bamberger
19 : /// @date Sept 2002
20 : ///
21 : // A list of positions
22 : /****************************************************************************/
23 : #include <config.h>
24 :
25 : #include <queue>
26 : #include <cmath>
27 : #include <iostream>
28 : #include <algorithm>
29 : #include <numeric>
30 : #include <cassert>
31 : #include <iterator>
32 : #include <limits>
33 : #include <utils/common/StdDefs.h>
34 : #include <utils/common/MsgHandler.h>
35 : #include <utils/common/ToString.h>
36 : #include "AbstractPoly.h"
37 : #include "Position.h"
38 : #include "PositionVector.h"
39 : #include "GeomHelper.h"
40 : #include "Boundary.h"
41 :
42 : //#define DEBUG_MOVE2SIDE
43 :
44 : // ===========================================================================
45 : // static members
46 : // ===========================================================================
47 : const PositionVector PositionVector::EMPTY;
48 :
49 : // ===========================================================================
50 : // method definitions
51 : // ===========================================================================
52 :
53 59813286 : PositionVector::PositionVector() {}
54 :
55 :
56 526 : PositionVector::PositionVector(const std::vector<Position>& v) {
57 : std::copy(v.begin(), v.end(), std::back_inserter(*this));
58 526 : }
59 :
60 :
61 0 : PositionVector::PositionVector(const std::vector<Position>::const_iterator beg, const std::vector<Position>::const_iterator end) {
62 : std::copy(beg, end, std::back_inserter(*this));
63 0 : }
64 :
65 :
66 9412026 : PositionVector::PositionVector(const Position& p1, const Position& p2) {
67 9412026 : push_back(p1);
68 9412026 : push_back(p2);
69 9412026 : }
70 :
71 :
72 149907288 : PositionVector::~PositionVector() {}
73 :
74 :
75 : bool
76 44255155 : PositionVector::around(const Position& p, double offset) const {
77 44255155 : if (size() < 2) {
78 : return false;
79 : }
80 44255146 : if (offset != 0) {
81 : PositionVector tmp(*this);
82 796 : tmp.scaleAbsolute(offset);
83 796 : return tmp.around(p);
84 796 : }
85 : double angle = 0;
86 : // iterate over all points, and obtain angle between current and next
87 184159882 : for (const_iterator i = begin(); i != (end() - 1); i++) {
88 : Position p1(
89 : i->x() - p.x(),
90 139905532 : i->y() - p.y());
91 : Position p2(
92 : (i + 1)->x() - p.x(),
93 139905532 : (i + 1)->y() - p.y());
94 139905532 : angle += GeomHelper::angle2D(p1, p2);
95 : }
96 : // add angle between last and first point
97 : Position p1(
98 : (end() - 1)->x() - p.x(),
99 44254350 : (end() - 1)->y() - p.y());
100 : Position p2(
101 : begin()->x() - p.x(),
102 44254350 : begin()->y() - p.y());
103 44254350 : angle += GeomHelper::angle2D(p1, p2);
104 : // if angle is less than PI, then point lying in Polygon
105 44254350 : return (!(fabs(angle) < M_PI));
106 : }
107 :
108 :
109 : bool
110 5430806 : PositionVector::overlapsWith(const AbstractPoly& poly, double offset) const {
111 : if (
112 : // check whether one of my points lies within the given poly
113 10838423 : partialWithin(poly, offset) ||
114 : // check whether the polygon lies within me
115 5407617 : poly.partialWithin(*this, offset)) {
116 43200 : return true;
117 : }
118 5387606 : if (size() >= 2) {
119 21576768 : for (const_iterator i = begin(); i != end() - 1; i++) {
120 16191143 : if (poly.crosses(*i, *(i + 1))) {
121 : return true;
122 : }
123 : }
124 5385625 : if (size() > 2 && poly.crosses(back(), front())) {
125 : return true;
126 : }
127 : }
128 : return false;
129 : }
130 :
131 :
132 : double
133 0 : PositionVector::getOverlapWith(const PositionVector& poly, double zThreshold) const {
134 : double result = 0;
135 0 : if ((size() == 0) || (poly.size() == 0)) {
136 : return result;
137 : }
138 : // this points within poly
139 0 : for (const_iterator i = begin(); i != end() - 1; i++) {
140 0 : if (poly.around(*i)) {
141 0 : Position closest = poly.positionAtOffset2D(poly.nearest_offset_to_point2D(*i));
142 0 : if (fabs(closest.z() - (*i).z()) < zThreshold) {
143 0 : result = MAX2(result, poly.distance2D(*i));
144 : }
145 : }
146 : }
147 : // polys points within this
148 0 : for (const_iterator i = poly.begin(); i != poly.end() - 1; i++) {
149 0 : if (around(*i)) {
150 0 : Position closest = positionAtOffset2D(nearest_offset_to_point2D(*i));
151 0 : if (fabs(closest.z() - (*i).z()) < zThreshold) {
152 0 : result = MAX2(result, distance2D(*i));
153 : }
154 : }
155 : }
156 : return result;
157 : }
158 :
159 :
160 : bool
161 27907124 : PositionVector::intersects(const Position& p1, const Position& p2) const {
162 27907124 : if (size() < 2) {
163 : return false;
164 : }
165 98866202 : for (const_iterator i = begin(); i != end() - 1; i++) {
166 72166496 : if (intersects(*i, *(i + 1), p1, p2)) {
167 : return true;
168 : }
169 : }
170 : return false;
171 : }
172 :
173 :
174 : bool
175 1296471 : PositionVector::intersects(const PositionVector& v1) const {
176 1296471 : if (size() < 2) {
177 : return false;
178 : }
179 1367143 : for (const_iterator i = begin(); i != end() - 1; i++) {
180 1248380 : if (v1.intersects(*i, *(i + 1))) {
181 : return true;
182 : }
183 : }
184 : return false;
185 : }
186 :
187 :
188 : Position
189 2487410 : PositionVector::intersectionPosition2D(const Position& p1, const Position& p2, const double withinDist) const {
190 2488538 : for (const_iterator i = begin(); i != end() - 1; i++) {
191 : double x, y, m;
192 2488532 : if (intersects(*i, *(i + 1), p1, p2, withinDist, &x, &y, &m)) {
193 2487404 : return Position(x, y);
194 : }
195 : }
196 6 : return Position::INVALID;
197 : }
198 :
199 :
200 : Position
201 206977 : PositionVector::intersectionPosition2D(const PositionVector& v1) const {
202 211788 : for (const_iterator i = begin(); i != end() - 1; i++) {
203 208998 : if (v1.intersects(*i, *(i + 1))) {
204 204187 : return v1.intersectionPosition2D(*i, *(i + 1));
205 : }
206 : }
207 2790 : return Position::INVALID;
208 : }
209 :
210 :
211 : const Position&
212 190802483 : PositionVector::operator[](int index) const {
213 : /* bracket operators works as in Python. Examples:
214 : - A = {'a', 'b', 'c', 'd'} (size 4)
215 : - A [2] returns 'c' because 0 < 2 < 4
216 : - A [100] throws an exception because 100 > 4
217 : - A [-1] returns 'd' because 4 - 1 = 3
218 : - A [-100] throws an exception because (4-100) < 0
219 : */
220 190802483 : if (index >= 0 && index < (int)size()) {
221 158555429 : return at(index);
222 32247054 : } else if (index < 0 && -index <= (int)size()) {
223 32247054 : return at((int)size() + index);
224 : } else {
225 0 : throw OutOfBoundsException("Index out of range in bracket operator of PositionVector");
226 : }
227 : }
228 :
229 :
230 : Position&
231 206309356 : PositionVector::operator[](int index) {
232 : /* bracket operators works as in Python. Examples:
233 : - A = {'a', 'b', 'c', 'd'} (size 4)
234 : - A [2] returns 'c' because 0 < 2 < 4
235 : - A [100] throws an exception because 100 > 4
236 : - A [-1] returns 'd' because 4 - 1 = 3
237 : - A [-100] throws an exception because (4-100) < 0
238 : */
239 206309356 : if (index >= 0 && index < (int)size()) {
240 203482591 : return at(index);
241 2826765 : } else if (index < 0 && -index <= (int)size()) {
242 2826765 : return at((int)size() + index);
243 : } else {
244 0 : throw OutOfBoundsException("Index out of range in bracket operator of PositionVector");
245 : }
246 : }
247 :
248 :
249 : Position
250 1611272701 : PositionVector::positionAtOffset(double pos, double lateralOffset) const {
251 1611272701 : if (size() == 0) {
252 0 : return Position::INVALID;
253 : }
254 1611272701 : if (size() == 1) {
255 0 : return front();
256 : }
257 : const_iterator i = begin();
258 : double seenLength = 0;
259 : do {
260 1924365418 : const double nextLength = (*i).distanceTo(*(i + 1));
261 1924365418 : if (seenLength + nextLength > pos) {
262 1610597866 : return positionAtOffset(*i, *(i + 1), pos - seenLength, lateralOffset);
263 : }
264 : seenLength += nextLength;
265 313767552 : } while (++i != end() - 1);
266 674835 : if (lateralOffset == 0 || size() < 2) {
267 638773 : return back();
268 : } else {
269 36062 : return positionAtOffset(*(end() - 2), *(end() - 1), (*(end() - 2)).distanceTo(*(end() - 1)), lateralOffset);
270 : }
271 : }
272 :
273 :
274 : Position
275 188550 : PositionVector::sidePositionAtAngle(double pos, double lateralOffset, double angle) const {
276 188550 : if (size() == 0) {
277 0 : return Position::INVALID;
278 : }
279 188550 : if (size() == 1) {
280 0 : return front();
281 : }
282 : const_iterator i = begin();
283 : double seenLength = 0;
284 : do {
285 516651 : const double nextLength = (*i).distanceTo(*(i + 1));
286 516651 : if (seenLength + nextLength > pos) {
287 187968 : return sidePositionAtAngle(*i, *(i + 1), pos - seenLength, lateralOffset, angle);
288 : }
289 : seenLength += nextLength;
290 328683 : } while (++i != end() - 1);
291 582 : return sidePositionAtAngle(*(end() - 2), *(end() - 1), (*(end() - 2)).distanceTo(*(end() - 1)), lateralOffset, angle);
292 : }
293 :
294 :
295 : Position
296 29188901 : PositionVector::positionAtOffset2D(double pos, double lateralOffset, bool extrapolateBeyond) const {
297 29188901 : if (size() == 0) {
298 0 : return Position::INVALID;
299 : }
300 29188901 : if (size() == 1) {
301 0 : return front();
302 : }
303 : const_iterator i = begin();
304 : double seenLength = 0;
305 : do {
306 : const double nextLength = (*i).distanceTo2D(*(i + 1));
307 42769494 : if (seenLength + nextLength > pos) {
308 21940321 : return positionAtOffset2D(*i, *(i + 1), pos - seenLength, lateralOffset, extrapolateBeyond);
309 : }
310 : seenLength += nextLength;
311 20829173 : } while (++i != end() - 1);
312 7248580 : if (extrapolateBeyond) {
313 11 : return positionAtOffset2D(*(i - 1), *i, pos - seenLength + (*i).distanceTo2D(*(i - 1)), lateralOffset, extrapolateBeyond);
314 : }
315 7248569 : return back();
316 : }
317 :
318 :
319 : double
320 4329613 : PositionVector::rotationAtOffset(double pos) const {
321 4329613 : if ((size() == 0) || (size() == 1)) {
322 : return INVALID_DOUBLE;
323 : }
324 4329613 : if (pos < 0) {
325 2767 : pos += length();
326 : }
327 : const_iterator i = begin();
328 : double seenLength = 0;
329 : do {
330 : const Position& p1 = *i;
331 : const Position& p2 = *(i + 1);
332 7076401 : const double nextLength = p1.distanceTo(p2);
333 7076401 : if (seenLength + nextLength > pos) {
334 4213878 : return p1.angleTo2D(p2);
335 : }
336 : seenLength += nextLength;
337 2862523 : } while (++i != end() - 1);
338 115735 : const Position& p1 = (*this)[-2];
339 : const Position& p2 = back();
340 115735 : return p1.angleTo2D(p2);
341 : }
342 :
343 :
344 : double
345 11647 : PositionVector::rotationDegreeAtOffset(double pos) const {
346 11647 : return GeomHelper::legacyDegree(rotationAtOffset(pos));
347 : }
348 :
349 :
350 : double
351 605842 : PositionVector::slopeDegreeAtOffset(double pos) const {
352 605842 : if (size() == 0) {
353 : return INVALID_DOUBLE;
354 : }
355 : const_iterator i = begin();
356 : double seenLength = 0;
357 : do {
358 : const Position& p1 = *i;
359 : const Position& p2 = *(i + 1);
360 800746 : const double nextLength = p1.distanceTo(p2);
361 800746 : if (seenLength + nextLength > pos) {
362 490290 : return RAD2DEG(p1.slopeTo2D(p2));
363 : }
364 : seenLength += nextLength;
365 310456 : } while (++i != end() - 1);
366 115552 : const Position& p1 = (*this)[-2];
367 : const Position& p2 = back();
368 115552 : return RAD2DEG(p1.slopeTo2D(p2));
369 : }
370 :
371 :
372 : Position
373 1617498333 : PositionVector::positionAtOffset(const Position& p1, const Position& p2, double pos, double lateralOffset) {
374 1617498333 : const double dist = p1.distanceTo(p2);
375 1617498333 : if (pos < 0. || dist < pos) {
376 270336 : return Position::INVALID;
377 : }
378 1617227997 : if (lateralOffset != 0) {
379 72967550 : if (dist == 0.) {
380 321 : return Position::INVALID;
381 : }
382 72967229 : const Position offset = sideOffset(p1, p2, -lateralOffset); // move in the same direction as Position::move2side
383 72967229 : if (pos == 0.) {
384 : return p1 + offset;
385 : }
386 72873160 : return p1 + (p2 - p1) * (pos / dist) + offset;
387 : }
388 1544260447 : if (pos == 0.) {
389 1118520 : return p1;
390 : }
391 1543141927 : return p1 + (p2 - p1) * (pos / dist);
392 : }
393 :
394 :
395 : Position
396 188550 : PositionVector::sidePositionAtAngle(const Position& p1, const Position& p2, double pos, double lateralOffset, double angle) {
397 188550 : const double dist = p1.distanceTo(p2);
398 188550 : if (pos < 0. || dist < pos || dist == 0) {
399 4 : return Position::INVALID;
400 : }
401 188546 : angle -= DEG2RAD(90);
402 188546 : const Position offset(cos(angle) * lateralOffset, sin(angle) * lateralOffset);
403 188546 : return p1 + (p2 - p1) * (pos / dist) + offset;
404 : }
405 :
406 :
407 : Position
408 87543328 : PositionVector::positionAtOffset2D(const Position& p1, const Position& p2, double pos, double lateralOffset, bool extrapolateBeyond) {
409 : const double dist = p1.distanceTo2D(p2);
410 87543328 : if ((pos < 0 || dist < pos) && !extrapolateBeyond) {
411 163 : return Position::INVALID;
412 : }
413 87543165 : if (dist == 0) {
414 159401 : return p1;
415 : }
416 87383764 : if (lateralOffset != 0) {
417 144519 : const Position offset = sideOffset(p1, p2, -lateralOffset); // move in the same direction as Position::move2side
418 144519 : if (pos == 0.) {
419 : return p1 + offset;
420 : }
421 144519 : return p1 + (p2 - p1) * (pos / dist) + offset;
422 : }
423 87239245 : if (pos == 0.) {
424 42014007 : return p1;
425 : }
426 45225238 : return p1 + (p2 - p1) * (pos / dist);
427 : }
428 :
429 :
430 : Boundary
431 870729 : PositionVector::getBoxBoundary() const {
432 870729 : Boundary ret;
433 4834784 : for (const Position& i : *this) {
434 3964055 : ret.add(i);
435 : }
436 870729 : return ret;
437 : }
438 :
439 :
440 : Position
441 15303 : PositionVector::getPolygonCenter() const {
442 15303 : if (size() == 0) {
443 0 : return Position::INVALID;
444 : }
445 : double x = 0;
446 : double y = 0;
447 : double z = 0;
448 88901 : for (const Position& i : *this) {
449 73598 : x += i.x();
450 73598 : y += i.y();
451 73598 : z += i.z();
452 : }
453 15303 : return Position(x / (double) size(), y / (double) size(), z / (double)size());
454 : }
455 :
456 :
457 : Position
458 467603 : PositionVector::getCentroid() const {
459 467603 : if (size() == 0) {
460 0 : return Position::INVALID;
461 467603 : } else if (size() == 1) {
462 50 : return (*this)[0];
463 467553 : } else if (size() == 2) {
464 88557 : return ((*this)[0] + (*this)[1]) * 0.5;
465 : }
466 : PositionVector tmp = *this;
467 378996 : if (!isClosed()) { // make sure its closed
468 351367 : tmp.push_back(tmp[0]);
469 : }
470 : // shift to origin to increase numerical stability
471 378996 : Position offset = tmp[0];
472 : Position result;
473 378996 : tmp.sub(offset);
474 378996 : const int endIndex = (int)tmp.size() - 1;
475 : double div = 0; // 6 * area including sign
476 : double x = 0;
477 : double y = 0;
478 378996 : if (tmp.area() != 0) { // numerical instability ?
479 : // http://en.wikipedia.org/wiki/Polygon
480 4787816 : for (int i = 0; i < endIndex; i++) {
481 4424237 : const double z = tmp[i].x() * tmp[i + 1].y() - tmp[i + 1].x() * tmp[i].y();
482 4424237 : div += z; // area formula
483 4424237 : x += (tmp[i].x() + tmp[i + 1].x()) * z;
484 4424237 : y += (tmp[i].y() + tmp[i + 1].y()) * z;
485 : }
486 363579 : div *= 3; // 6 / 2, the 2 comes from the area formula
487 363579 : result = Position(x / div, y / div);
488 : } else {
489 : // compute by decomposing into line segments
490 : // http://en.wikipedia.org/wiki/Centroid#By_geometric_decomposition
491 : double lengthSum = 0;
492 61427 : for (int i = 0; i < endIndex; i++) {
493 46010 : double length = tmp[i].distanceTo(tmp[i + 1]);
494 46010 : x += (tmp[i].x() + tmp[i + 1].x()) * length / 2;
495 46010 : y += (tmp[i].y() + tmp[i + 1].y()) * length / 2;
496 46010 : lengthSum += length;
497 : }
498 15417 : if (lengthSum == 0) {
499 : // it is probably only one point
500 0 : result = tmp[0];
501 : }
502 15417 : result = Position(x / lengthSum, y / lengthSum) + offset;
503 : }
504 : return result + offset;
505 378996 : }
506 :
507 :
508 : void
509 72319 : PositionVector::scaleRelative(double factor) {
510 72319 : Position centroid = getCentroid();
511 216960 : for (int i = 0; i < static_cast<int>(size()); i++) {
512 144641 : (*this)[i] = centroid + (((*this)[i] - centroid) * factor);
513 : }
514 72319 : }
515 :
516 :
517 : void
518 796 : PositionVector::scaleAbsolute(double offset) {
519 796 : Position centroid = getCentroid();
520 6045 : for (int i = 0; i < static_cast<int>(size()); i++) {
521 5249 : (*this)[i] = centroid + (((*this)[i] - centroid) + offset);
522 : }
523 796 : }
524 :
525 :
526 : Position
527 5621 : PositionVector::getLineCenter() const {
528 5621 : if (size() == 1) {
529 0 : return (*this)[0];
530 : } else {
531 5621 : return positionAtOffset(double((length() / 2.)));
532 : }
533 : }
534 :
535 :
536 : double
537 6755547 : PositionVector::length() const {
538 6755547 : if (size() == 0) {
539 : return 0;
540 : }
541 : double len = 0;
542 21282440 : for (const_iterator i = begin(); i != end() - 1; i++) {
543 14539601 : len += (*i).distanceTo(*(i + 1));
544 : }
545 : return len;
546 : }
547 :
548 :
549 : double
550 30407046 : PositionVector::length2D() const {
551 30407046 : if (size() == 0) {
552 : return 0;
553 : }
554 : double len = 0;
555 81732961 : for (const_iterator i = begin(); i != end() - 1; i++) {
556 51325915 : len += (*i).distanceTo2D(*(i + 1));
557 : }
558 : return len;
559 : }
560 :
561 :
562 : double
563 379111 : PositionVector::area() const {
564 379111 : if (size() < 3) {
565 : return 0;
566 : }
567 : double area = 0;
568 : PositionVector tmp = *this;
569 379111 : if (!isClosed()) { // make sure its closed
570 28 : tmp.push_back(tmp[0]);
571 : }
572 379111 : const int endIndex = (int)tmp.size() - 1;
573 : // http://en.wikipedia.org/wiki/Polygon
574 4852544 : for (int i = 0; i < endIndex; i++) {
575 4473433 : area += tmp[i].x() * tmp[i + 1].y() - tmp[i + 1].x() * tmp[i].y();
576 : }
577 379111 : if (area < 0) { // we whether we had cw or ccw order
578 353460 : area *= -1;
579 : }
580 379111 : return area / 2;
581 379111 : }
582 :
583 :
584 : bool
585 10839412 : PositionVector::partialWithin(const AbstractPoly& poly, double offset) const {
586 10839412 : if (size() < 2) {
587 : return false;
588 : }
589 54167996 : for (const_iterator i = begin(); i != end(); i++) {
590 43372476 : if (poly.around(*i, offset)) {
591 : return true;
592 : }
593 : }
594 : return false;
595 : }
596 :
597 :
598 : bool
599 21577952 : PositionVector::crosses(const Position& p1, const Position& p2) const {
600 21577952 : return intersects(p1, p2);
601 : }
602 :
603 :
604 : std::pair<PositionVector, PositionVector>
605 251975 : PositionVector::splitAt(double where, bool use2D) const {
606 251975 : const double len = use2D ? length2D() : length();
607 251975 : if (size() < 2) {
608 0 : throw InvalidArgument("Vector to short for splitting");
609 : }
610 251975 : if (where < 0 || where > len) {
611 0 : throw InvalidArgument("Invalid split position " + toString(where) + " for vector of length " + toString(len));
612 : }
613 251975 : if (where <= POSITION_EPS || where >= len - POSITION_EPS) {
614 2 : WRITE_WARNINGF(TL("Splitting vector close to end (pos: %, length: %)"), toString(where), toString(len));
615 : }
616 251975 : PositionVector first, second;
617 251975 : first.push_back((*this)[0]);
618 : double seen = 0;
619 : const_iterator it = begin() + 1;
620 251975 : double next = use2D ? first.back().distanceTo2D(*it) : first.back().distanceTo(*it);
621 : // see how many points we can add to first
622 338382 : while (where >= seen + next + POSITION_EPS) {
623 : seen += next;
624 86407 : first.push_back(*it);
625 : it++;
626 86407 : next = use2D ? first.back().distanceTo2D(*it) : first.back().distanceTo(*it);
627 : }
628 251975 : if (fabs(where - (seen + next)) > POSITION_EPS || it == end() - 1) {
629 : // we need to insert a new point because 'where' is not close to an
630 : // existing point or it is to close to the endpoint
631 : const Position p = (use2D
632 241696 : ? positionAtOffset2D(first.back(), *it, where - seen)
633 10830 : : positionAtOffset(first.back(), *it, where - seen));
634 241696 : first.push_back(p);
635 241696 : second.push_back(p);
636 : } else {
637 10279 : first.push_back(*it);
638 : }
639 : // add the remaining points to second
640 628250 : for (; it != end(); it++) {
641 376275 : second.push_back(*it);
642 : }
643 : assert(first.size() >= 2);
644 : assert(second.size() >= 2);
645 : assert(first.back() == second.front());
646 : assert(fabs((use2D ? first.length2D() + second.length2D() : first.length() + second.length()) - len) < 2 * POSITION_EPS);
647 503950 : return std::pair<PositionVector, PositionVector>(first, second);
648 251975 : }
649 :
650 :
651 : std::ostream&
652 377531 : operator<<(std::ostream& os, const PositionVector& geom) {
653 2306862 : for (PositionVector::const_iterator i = geom.begin(); i != geom.end(); i++) {
654 1929331 : if (i != geom.begin()) {
655 1551805 : os << " ";
656 : }
657 1929331 : os << (*i);
658 : }
659 377531 : return os;
660 : }
661 :
662 :
663 : void
664 3 : PositionVector::sortAsPolyCWByAngle() {
665 : // We take the centroid of the points as an origin for the angle computations
666 : // that will follow but other points could be taken (the center of the bounding
667 : // box of the polygon for instance). Each of these can potentially lead
668 : // to a different result in the case of a non-convex polygon.
669 3 : const Position centroid = std::accumulate(begin(), end(), Position(0, 0)) / (double)size();
670 3 : sub(centroid);
671 3 : std::sort(begin(), end(), as_poly_cw_sorter());
672 3 : add(centroid);
673 3 : }
674 :
675 :
676 : void
677 729395 : PositionVector::add(double xoff, double yoff, double zoff) {
678 6078828 : for (int i = 0; i < (int)size(); i++) {
679 5349433 : (*this)[i].add(xoff, yoff, zoff);
680 : }
681 729395 : }
682 :
683 :
684 : void
685 379200 : PositionVector::sub(const Position& offset) {
686 379200 : add(-offset.x(), -offset.y(), -offset.z());
687 379200 : }
688 :
689 :
690 : void
691 6990 : PositionVector::add(const Position& offset) {
692 6990 : add(offset.x(), offset.y(), offset.z());
693 6990 : }
694 :
695 :
696 : PositionVector
697 0 : PositionVector::added(const Position& offset) const {
698 0 : PositionVector pv;
699 0 : for (auto i1 = begin(); i1 != end(); ++i1) {
700 0 : pv.push_back(*i1 + offset);
701 : }
702 0 : return pv;
703 0 : }
704 :
705 :
706 : void
707 9839 : PositionVector::mirrorX() {
708 27129 : for (int i = 0; i < (int)size(); i++) {
709 17290 : (*this)[i].mul(1, -1);
710 : }
711 9839 : }
712 :
713 :
714 3 : PositionVector::as_poly_cw_sorter::as_poly_cw_sorter() {}
715 :
716 :
717 : int
718 8 : PositionVector::as_poly_cw_sorter::operator()(const Position& p1, const Position& p2) const {
719 8 : double angle1 = atAngle2D(p1);
720 8 : double angle2 = atAngle2D(p2);
721 8 : if (angle1 > angle2) {
722 : return true;
723 : }
724 2 : if (angle1 == angle2) {
725 : double squaredDistance1 = p1.dotProduct(p1);
726 : double squaredDistance2 = p2.dotProduct(p2);
727 0 : if (squaredDistance1 < squaredDistance2) {
728 0 : return true;
729 : }
730 : }
731 : return false;
732 : }
733 :
734 :
735 : double
736 16 : PositionVector::as_poly_cw_sorter::atAngle2D(const Position& p) const {
737 16 : double angle = atan2(p.y(), p.x());
738 16 : return angle < 0.0 ? angle : angle + 2.0 * M_PI;
739 : }
740 :
741 : void
742 0 : PositionVector::sortByIncreasingXY() {
743 0 : std::sort(begin(), end(), increasing_x_y_sorter());
744 0 : }
745 :
746 :
747 0 : PositionVector::increasing_x_y_sorter::increasing_x_y_sorter() {}
748 :
749 :
750 : int
751 0 : PositionVector::increasing_x_y_sorter::operator()(const Position& p1, const Position& p2) const {
752 0 : if (p1.x() != p2.x()) {
753 0 : return p1.x() < p2.x();
754 : }
755 0 : return p1.y() < p2.y();
756 : }
757 :
758 :
759 : double
760 6540817 : PositionVector::isLeft(const Position& P0, const Position& P1, const Position& P2) const {
761 6540817 : return (P1.x() - P0.x()) * (P2.y() - P0.y()) - (P2.x() - P0.x()) * (P1.y() - P0.y());
762 : }
763 :
764 :
765 : void
766 8224778 : PositionVector::append(const PositionVector& v, double sameThreshold) {
767 8224778 : if ((size() > 0) && (v.size() > 0) && (back().distanceTo(v[0]) < sameThreshold)) {
768 : copy(v.begin() + 1, v.end(), back_inserter(*this));
769 : } else {
770 : copy(v.begin(), v.end(), back_inserter(*this));
771 : }
772 8224778 : }
773 :
774 :
775 : void
776 375 : PositionVector::prepend(const PositionVector& v, double sameThreshold) {
777 375 : if ((size() > 0) && (v.size() > 0) && (front().distanceTo(v.back()) < sameThreshold)) {
778 148 : insert(begin(), v.begin(), v.end() - 1);
779 : } else {
780 227 : insert(begin(), v.begin(), v.end());
781 : }
782 375 : }
783 :
784 :
785 : PositionVector
786 677739 : PositionVector::getSubpart(double beginOffset, double endOffset) const {
787 677739 : PositionVector ret;
788 677739 : Position begPos = front();
789 677739 : if (beginOffset > POSITION_EPS) {
790 54369 : begPos = positionAtOffset(beginOffset);
791 : }
792 677739 : Position endPos = back();
793 677739 : if (endOffset < length() - POSITION_EPS) {
794 171411 : endPos = positionAtOffset(endOffset);
795 : }
796 677739 : ret.push_back(begPos);
797 :
798 : double seen = 0;
799 : const_iterator i = begin();
800 : // skip previous segments
801 677739 : while ((i + 1) != end()
802 690736 : &&
803 690734 : seen + (*i).distanceTo(*(i + 1)) < beginOffset) {
804 12997 : seen += (*i).distanceTo(*(i + 1));
805 : i++;
806 : }
807 : // append segments in between
808 : while ((i + 1) != end()
809 762943 : &&
810 762481 : seen + (*i).distanceTo(*(i + 1)) < endOffset) {
811 :
812 85204 : ret.push_back_noDoublePos(*(i + 1));
813 85204 : seen += (*i).distanceTo(*(i + 1));
814 : i++;
815 : }
816 : // append end
817 677739 : ret.push_back_noDoublePos(endPos);
818 677739 : if (ret.size() == 1) {
819 22 : ret.push_back(endPos);
820 : }
821 677739 : return ret;
822 0 : }
823 :
824 :
825 : PositionVector
826 1243344 : PositionVector::getSubpart2D(double beginOffset, double endOffset) const {
827 1243344 : if (size() == 0) {
828 0 : return PositionVector();
829 : }
830 1243344 : PositionVector ret;
831 1243344 : Position begPos = front();
832 1243344 : if (beginOffset > POSITION_EPS) {
833 643579 : begPos = positionAtOffset2D(beginOffset);
834 : }
835 1243344 : Position endPos = back();
836 1243344 : if (endOffset < length2D() - POSITION_EPS) {
837 202060 : endPos = positionAtOffset2D(endOffset);
838 : }
839 1243344 : ret.push_back(begPos);
840 :
841 : double seen = 0;
842 : const_iterator i = begin();
843 : // skip previous segments
844 1243344 : while ((i + 1) != end()
845 1308777 : &&
846 1308777 : seen + (*i).distanceTo2D(*(i + 1)) < beginOffset) {
847 65433 : seen += (*i).distanceTo2D(*(i + 1));
848 : i++;
849 : }
850 : // append segments in between
851 : while ((i + 1) != end()
852 3162393 : &&
853 2840934 : seen + (*i).distanceTo2D(*(i + 1)) < endOffset) {
854 :
855 1919049 : ret.push_back_noDoublePos(*(i + 1));
856 1919049 : seen += (*i).distanceTo2D(*(i + 1));
857 : i++;
858 : }
859 : // append end
860 1243344 : ret.push_back_noDoublePos(endPos);
861 1243344 : if (ret.size() == 1) {
862 27 : ret.push_back(endPos);
863 : }
864 : return ret;
865 1243344 : }
866 :
867 :
868 : PositionVector
869 166409 : PositionVector::getSubpartByIndex(int beginIndex, int count) const {
870 166409 : if (size() == 0) {
871 0 : return PositionVector();
872 : }
873 166409 : if (beginIndex < 0) {
874 0 : beginIndex += (int)size();
875 : }
876 : assert(count >= 0);
877 : assert(beginIndex < (int)size());
878 : assert(beginIndex + count <= (int)size());
879 166409 : PositionVector result;
880 492255 : for (int i = beginIndex; i < beginIndex + count; ++i) {
881 325846 : result.push_back((*this)[i]);
882 : }
883 : return result;
884 166409 : }
885 :
886 :
887 : double
888 770284 : PositionVector::beginEndAngle() const {
889 770284 : if (size() == 0) {
890 : return INVALID_DOUBLE;
891 : }
892 770284 : return front().angleTo2D(back());
893 : }
894 :
895 :
896 : double
897 27160451 : PositionVector::nearest_offset_to_point2D(const Position& p, bool perpendicular) const {
898 27160451 : if (size() == 0) {
899 : return INVALID_DOUBLE;
900 : }
901 : double minDist = std::numeric_limits<double>::max();
902 27160451 : double nearestPos = GeomHelper::INVALID_OFFSET;
903 : double seen = 0;
904 92037475 : for (const_iterator i = begin(); i != end() - 1; i++) {
905 : const double pos =
906 64877024 : GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, perpendicular);
907 64877024 : const double dist2 = pos == GeomHelper::INVALID_OFFSET ? minDist : p.distanceSquaredTo2D(positionAtOffset2D(*i, *(i + 1), pos));
908 64877024 : if (dist2 < minDist) {
909 35787353 : nearestPos = pos + seen;
910 : minDist = dist2;
911 : }
912 64877024 : if (perpendicular && i != begin() && pos == GeomHelper::INVALID_OFFSET) {
913 : // even if perpendicular is set we still need to check the distance to the inner points
914 : const double cornerDist2 = p.distanceSquaredTo2D(*i);
915 689528 : if (cornerDist2 < minDist) {
916 : const double pos1 =
917 357301 : GeomHelper::nearest_offset_on_line_to_point2D(*(i - 1), *i, p, false);
918 : const double pos2 =
919 357301 : GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, false);
920 357301 : if (pos1 == (*(i - 1)).distanceTo2D(*i) && pos2 == 0.) {
921 : nearestPos = seen;
922 : minDist = cornerDist2;
923 : }
924 : }
925 : }
926 64877024 : seen += (*i).distanceTo2D(*(i + 1));
927 : }
928 : return nearestPos;
929 : }
930 :
931 :
932 : double
933 362853 : PositionVector::nearest_offset_to_point25D(const Position& p, bool perpendicular) const {
934 362853 : if (size() == 0) {
935 : return INVALID_DOUBLE;
936 : }
937 : double minDist = std::numeric_limits<double>::max();
938 362853 : double nearestPos = GeomHelper::INVALID_OFFSET;
939 : double seen = 0;
940 1103488 : for (const_iterator i = begin(); i != end() - 1; i++) {
941 : const double pos =
942 740635 : GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, perpendicular);
943 740635 : const double dist = pos == GeomHelper::INVALID_OFFSET ? minDist : p.distanceTo2D(positionAtOffset2D(*i, *(i + 1), pos));
944 740635 : if (dist < minDist) {
945 94116 : const double pos25D = pos * (*i).distanceTo(*(i + 1)) / (*i).distanceTo2D(*(i + 1));
946 94116 : nearestPos = pos25D + seen;
947 : minDist = dist;
948 : }
949 740635 : if (perpendicular && i != begin() && pos == GeomHelper::INVALID_OFFSET) {
950 : // even if perpendicular is set we still need to check the distance to the inner points
951 : const double cornerDist = p.distanceTo2D(*i);
952 349876 : if (cornerDist < minDist) {
953 : const double pos1 =
954 323148 : GeomHelper::nearest_offset_on_line_to_point2D(*(i - 1), *i, p, false);
955 : const double pos2 =
956 323148 : GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, false);
957 323148 : if (pos1 == (*(i - 1)).distanceTo2D(*i) && pos2 == 0.) {
958 : nearestPos = seen;
959 : minDist = cornerDist;
960 : }
961 : }
962 : }
963 740635 : seen += (*i).distanceTo(*(i + 1));
964 : }
965 : return nearestPos;
966 : }
967 :
968 :
969 : Position
970 12114342 : PositionVector::transformToVectorCoordinates(const Position& p, bool extend) const {
971 12114342 : if (size() == 0) {
972 0 : return Position::INVALID;
973 : }
974 : // @toDo this duplicates most of the code in nearest_offset_to_point2D. It should be refactored
975 12114342 : if (extend) {
976 : PositionVector extended = *this;
977 5597950 : const double dist = 2 * distance2D(p);
978 5597950 : extended.extrapolate(dist);
979 5597950 : return extended.transformToVectorCoordinates(p) - Position(dist, 0);
980 5597950 : }
981 : double minDist = std::numeric_limits<double>::max();
982 : double nearestPos = -1;
983 : double seen = 0;
984 : int sign = 1;
985 15609531 : for (const_iterator i = begin(); i != end() - 1; i++) {
986 : const double pos =
987 9093139 : GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, true);
988 9093139 : const double dist = pos < 0 ? minDist : p.distanceTo2D(positionAtOffset(*i, *(i + 1), pos));
989 9093139 : if (dist < minDist) {
990 6333053 : nearestPos = pos + seen;
991 : minDist = dist;
992 6333053 : sign = isLeft(*i, *(i + 1), p) >= 0 ? -1 : 1;
993 : }
994 9093139 : if (i != begin() && pos == GeomHelper::INVALID_OFFSET) {
995 : // even if perpendicular is set we still need to check the distance to the inner points
996 : const double cornerDist = p.distanceTo2D(*i);
997 1591957 : if (cornerDist < minDist) {
998 : const double pos1 =
999 425545 : GeomHelper::nearest_offset_on_line_to_point2D(*(i - 1), *i, p, false);
1000 : const double pos2 =
1001 425545 : GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, false);
1002 425545 : if (pos1 == (*(i - 1)).distanceTo2D(*i) && pos2 == 0.) {
1003 : nearestPos = seen;
1004 : minDist = cornerDist;
1005 207764 : sign = isLeft(*(i - 1), *i, p) >= 0 ? -1 : 1;
1006 : }
1007 : }
1008 : }
1009 9093139 : seen += (*i).distanceTo2D(*(i + 1));
1010 : }
1011 6516392 : if (nearestPos != -1) {
1012 6515281 : return Position(nearestPos, sign * minDist);
1013 : } else {
1014 1111 : return Position::INVALID;
1015 : }
1016 : }
1017 :
1018 :
1019 : int
1020 307995 : PositionVector::indexOfClosest(const Position& p, bool twoD) const {
1021 307995 : if (size() == 0) {
1022 : return -1;
1023 : }
1024 : double minDist = std::numeric_limits<double>::max();
1025 : double dist;
1026 : int closest = 0;
1027 2103242 : for (int i = 0; i < (int)size(); i++) {
1028 1795247 : const Position& p2 = (*this)[i];
1029 1795247 : dist = twoD ? p.distanceTo2D(p2) : p.distanceTo(p2);
1030 1795247 : if (dist < minDist) {
1031 : closest = i;
1032 : minDist = dist;
1033 : }
1034 : }
1035 : return closest;
1036 : }
1037 :
1038 :
1039 : int
1040 164352 : PositionVector::insertAtClosest(const Position& p, bool interpolateZ) {
1041 164352 : if (size() == 0) {
1042 : return -1;
1043 : }
1044 : double minDist = std::numeric_limits<double>::max();
1045 : int insertionIndex = 1;
1046 1218301 : for (int i = 0; i < (int)size() - 1; i++) {
1047 1053949 : const double length = GeomHelper::nearest_offset_on_line_to_point2D((*this)[i], (*this)[i + 1], p, false);
1048 1053949 : const Position& outIntersection = PositionVector::positionAtOffset2D((*this)[i], (*this)[i + 1], length);
1049 : const double dist = p.distanceTo2D(outIntersection);
1050 1053949 : if (dist < minDist) {
1051 : insertionIndex = i + 1;
1052 : minDist = dist;
1053 : }
1054 : }
1055 : // check if we have to adjust Position Z
1056 164352 : if (interpolateZ) {
1057 : // obtain previous and next Z
1058 0 : const double previousZ = (begin() + (insertionIndex - 1))->z();
1059 : const double nextZ = (begin() + insertionIndex)->z();
1060 : // insert new position using x and y of p, and the new z
1061 0 : insert(begin() + insertionIndex, Position(p.x(), p.y(), ((previousZ + nextZ) / 2.0)));
1062 : } else {
1063 164352 : insert(begin() + insertionIndex, p);
1064 : }
1065 : return insertionIndex;
1066 : }
1067 :
1068 :
1069 : int
1070 251 : PositionVector::removeClosest(const Position& p) {
1071 251 : if (size() == 0) {
1072 : return -1;
1073 : }
1074 : double minDist = std::numeric_limits<double>::max();
1075 : int removalIndex = 0;
1076 2477 : for (int i = 0; i < (int)size(); i++) {
1077 2226 : const double dist = p.distanceTo2D((*this)[i]);
1078 2226 : if (dist < minDist) {
1079 : removalIndex = i;
1080 : minDist = dist;
1081 : }
1082 : }
1083 251 : erase(begin() + removalIndex);
1084 251 : return removalIndex;
1085 : }
1086 :
1087 :
1088 : std::vector<double>
1089 5768647 : PositionVector::intersectsAtLengths2D(const PositionVector& other) const {
1090 : std::vector<double> ret;
1091 5768647 : if (other.size() == 0) {
1092 : return ret;
1093 : }
1094 18157812 : for (const_iterator i = other.begin(); i != other.end() - 1; i++) {
1095 12389165 : std::vector<double> atSegment = intersectsAtLengths2D(*i, *(i + 1));
1096 : copy(atSegment.begin(), atSegment.end(), back_inserter(ret));
1097 12389165 : }
1098 : return ret;
1099 0 : }
1100 :
1101 :
1102 : std::vector<double>
1103 12389165 : PositionVector::intersectsAtLengths2D(const Position& lp1, const Position& lp2) const {
1104 : std::vector<double> ret;
1105 12389165 : if (size() == 0) {
1106 : return ret;
1107 : }
1108 : double pos = 0;
1109 42575384 : for (const_iterator i = begin(); i != end() - 1; i++) {
1110 : const Position& p1 = *i;
1111 : const Position& p2 = *(i + 1);
1112 : double x, y, m;
1113 30186219 : if (intersects(p1, p2, lp1, lp2, 0., &x, &y, &m)) {
1114 3326098 : ret.push_back(Position(x, y).distanceTo2D(p1) + pos);
1115 : }
1116 30186219 : pos += p1.distanceTo2D(p2);
1117 : }
1118 : return ret;
1119 0 : }
1120 :
1121 :
1122 : void
1123 6547093 : PositionVector::extrapolate(const double val, const bool onlyFirst, const bool onlyLast) {
1124 6547093 : if (size() > 1) {
1125 6547093 : Position& p1 = (*this)[0];
1126 6547093 : Position& p2 = (*this)[1];
1127 6547093 : const Position offset = (p2 - p1) * (val / p1.distanceTo(p2));
1128 6547093 : if (!onlyLast) {
1129 : p1.sub(offset);
1130 : }
1131 6547093 : if (!onlyFirst) {
1132 6547093 : if (size() == 2) {
1133 : p2.add(offset);
1134 : } else {
1135 581305 : const Position& e1 = (*this)[-2];
1136 581305 : Position& e2 = (*this)[-1];
1137 581305 : e2.sub((e1 - e2) * (val / e1.distanceTo(e2)));
1138 : }
1139 : }
1140 : }
1141 6547093 : }
1142 :
1143 :
1144 : void
1145 5483756 : PositionVector::extrapolate2D(const double val, const bool onlyFirst) {
1146 5483756 : if (size() > 1) {
1147 5483756 : Position& p1 = (*this)[0];
1148 5483756 : Position& p2 = (*this)[1];
1149 5483756 : if (p1.distanceTo2D(p2) > 0) {
1150 5483744 : const Position offset = (p2 - p1) * (val / p1.distanceTo2D(p2));
1151 : p1.sub(offset);
1152 5483744 : if (!onlyFirst) {
1153 4497208 : if (size() == 2) {
1154 : p2.add(offset);
1155 : } else {
1156 388501 : const Position& e1 = (*this)[-2];
1157 388501 : Position& e2 = (*this)[-1];
1158 388501 : e2.sub((e1 - e2) * (val / e1.distanceTo2D(e2)));
1159 : }
1160 : }
1161 : }
1162 : }
1163 5483756 : }
1164 :
1165 :
1166 : PositionVector
1167 12307420 : PositionVector::reverse() const {
1168 12307420 : PositionVector ret;
1169 43877179 : for (const_reverse_iterator i = rbegin(); i != rend(); i++) {
1170 31569759 : ret.push_back(*i);
1171 : }
1172 12307420 : return ret;
1173 0 : }
1174 :
1175 :
1176 : Position
1177 117355342 : PositionVector::sideOffset(const Position& beg, const Position& end, const double amount) {
1178 117355342 : const double scale = amount / beg.distanceTo2D(end);
1179 117355342 : return Position((beg.y() - end.y()) * scale, (end.x() - beg.x()) * scale);
1180 : }
1181 :
1182 :
1183 : void
1184 19605304 : PositionVector::move2side(double amount, double maxExtension) {
1185 19605304 : if (size() < 2) {
1186 172777 : return;
1187 : }
1188 19605304 : removeDoublePoints(POSITION_EPS, true);
1189 19605304 : if (length2D() == 0 || amount == 0) {
1190 : return;
1191 : }
1192 19432527 : PositionVector shape;
1193 : std::vector<int> recheck;
1194 61390709 : for (int i = 0; i < static_cast<int>(size()); i++) {
1195 41958182 : if (i == 0) {
1196 19432527 : const Position& from = (*this)[i];
1197 19432527 : const Position& to = (*this)[i + 1];
1198 : if (from != to) {
1199 38865054 : shape.push_back(from - sideOffset(from, to, amount));
1200 : #ifdef DEBUG_MOVE2SIDE
1201 : if (gDebugFlag1) {
1202 : std::cout << " " << i << "a=" << shape.back() << "\n";
1203 : }
1204 : #endif
1205 : }
1206 22525655 : } else if (i == static_cast<int>(size()) - 1) {
1207 19432527 : const Position& from = (*this)[i - 1];
1208 19432527 : const Position& to = (*this)[i];
1209 : if (from != to) {
1210 38865054 : shape.push_back(to - sideOffset(from, to, amount));
1211 : #ifdef DEBUG_MOVE2SIDE
1212 : if (gDebugFlag1) {
1213 : std::cout << " " << i << "b=" << shape.back() << "\n";
1214 : }
1215 : #endif
1216 : }
1217 : } else {
1218 3093128 : const Position& from = (*this)[i - 1];
1219 3093128 : const Position& me = (*this)[i];
1220 3093128 : const Position& to = (*this)[i + 1];
1221 3093128 : PositionVector fromMe(from, me);
1222 3093128 : fromMe.extrapolate2D(me.distanceTo2D(to));
1223 3093128 : const double extrapolateDev = fromMe[1].distanceTo2D(to);
1224 3093128 : if (fabs(extrapolateDev) < POSITION_EPS) {
1225 : // parallel case, just shift the middle point
1226 1619272 : shape.push_back(me - sideOffset(from, to, amount));
1227 : #ifdef DEBUG_MOVE2SIDE
1228 : if (gDebugFlag1) {
1229 : std::cout << " " << i << "c=" << shape.back() << "\n";
1230 : }
1231 : #endif
1232 2283492 : } else if (fabs(extrapolateDev - 2 * me.distanceTo2D(to)) < POSITION_EPS) {
1233 : // counterparallel case, just shift the middle point
1234 432 : PositionVector fromMe2(from, me);
1235 432 : fromMe2.extrapolate2D(amount);
1236 432 : shape.push_back(fromMe2[1]);
1237 : #ifdef DEBUG_MOVE2SIDE
1238 : if (gDebugFlag1) {
1239 : std::cout << " " << i << "d=" << shape.back() << " " << i << "_from=" << from << " " << i << "_me=" << me << " " << i << "_to=" << to << "\n";
1240 : }
1241 : #endif
1242 432 : } else {
1243 2283060 : Position offsets = sideOffset(from, me, amount);
1244 2283060 : Position offsets2 = sideOffset(me, to, amount);
1245 2283060 : PositionVector l1(from - offsets, me - offsets);
1246 2283060 : PositionVector l2(me - offsets2, to - offsets2);
1247 2283060 : Position meNew = l1.intersectionPosition2D(l2[0], l2[1], maxExtension);
1248 6 : if (meNew == Position::INVALID) {
1249 6 : recheck.push_back(i);
1250 : continue;
1251 : }
1252 2283054 : meNew = meNew + Position(0, 0, me.z());
1253 2283054 : shape.push_back(meNew);
1254 : #ifdef DEBUG_MOVE2SIDE
1255 : if (gDebugFlag1) {
1256 : std::cout << " " << i << "e=" << shape.back() << "\n";
1257 : }
1258 : #endif
1259 2283060 : }
1260 : // copy original z value
1261 : shape.back().set(shape.back().x(), shape.back().y(), me.z());
1262 3093122 : const double angle = localAngle(from, me, to);
1263 3093122 : if (fabs(angle) > NUMERICAL_EPS) {
1264 2741617 : const double length = from.distanceTo2D(me) + me.distanceTo2D(to);
1265 2741617 : const double radius = length / angle;
1266 : #ifdef DEBUG_MOVE2SIDE
1267 : if (gDebugFlag1) {
1268 : std::cout << " i=" << i << " a=" << RAD2DEG(angle) << " l=" << length << " r=" << radius << " t=" << amount * 1.8 << "\n";
1269 : }
1270 : #endif
1271 2741617 : if ((radius < 0 && -radius < amount * 1.8) || fabs(RAD2DEG(angle)) > 170) {
1272 1267 : recheck.push_back(i);
1273 : }
1274 : }
1275 3093128 : }
1276 : }
1277 19432527 : if (!recheck.empty()) {
1278 : // try to adjust positions to avoid clipping
1279 : shape = *this;
1280 2202 : for (int i = (int)recheck.size() - 1; i >= 0; i--) {
1281 1273 : shape.erase(shape.begin() + recheck[i]);
1282 : }
1283 929 : shape.move2side(amount, maxExtension);
1284 : }
1285 : *this = shape;
1286 19432527 : }
1287 :
1288 :
1289 : void
1290 52 : PositionVector::move2sideCustom(std::vector<double> amount, double maxExtension) {
1291 52 : if (size() < 2) {
1292 0 : return;
1293 : }
1294 52 : if (length2D() == 0) {
1295 : return;
1296 : }
1297 52 : if (size() != amount.size()) {
1298 0 : throw InvalidArgument("Numer of offsets (" + toString(amount.size())
1299 0 : + ") does not match number of points (" + toString(size()) + ")");
1300 : }
1301 52 : PositionVector shape;
1302 2543 : for (int i = 0; i < static_cast<int>(size()); i++) {
1303 2491 : if (i == 0) {
1304 52 : const Position& from = (*this)[i];
1305 52 : const Position& to = (*this)[i + 1];
1306 : if (from != to) {
1307 104 : shape.push_back(from - sideOffset(from, to, amount[i]));
1308 : }
1309 2439 : } else if (i == static_cast<int>(size()) - 1) {
1310 52 : const Position& from = (*this)[i - 1];
1311 52 : const Position& to = (*this)[i];
1312 : if (from != to) {
1313 104 : shape.push_back(to - sideOffset(from, to, amount[i]));
1314 : }
1315 : } else {
1316 2387 : const Position& from = (*this)[i - 1];
1317 2387 : const Position& me = (*this)[i];
1318 2387 : const Position& to = (*this)[i + 1];
1319 2387 : PositionVector fromMe(from, me);
1320 2387 : fromMe.extrapolate2D(me.distanceTo2D(to));
1321 2387 : const double extrapolateDev = fromMe[1].distanceTo2D(to);
1322 2387 : if (fabs(extrapolateDev) < POSITION_EPS) {
1323 : // parallel case, just shift the middle point
1324 4448 : shape.push_back(me - sideOffset(from, to, amount[i]));
1325 163 : } else if (fabs(extrapolateDev - 2 * me.distanceTo2D(to)) < POSITION_EPS) {
1326 : // counterparallel case, just shift the middle point
1327 0 : PositionVector fromMe2(from, me);
1328 0 : fromMe2.extrapolate2D(amount[i]);
1329 0 : shape.push_back(fromMe2[1]);
1330 0 : } else {
1331 163 : Position offsets = sideOffset(from, me, amount[i]);
1332 163 : Position offsets2 = sideOffset(me, to, amount[i]);
1333 163 : PositionVector l1(from - offsets, me - offsets);
1334 163 : PositionVector l2(me - offsets2, to - offsets2);
1335 163 : Position meNew = l1.intersectionPosition2D(l2[0], l2[1], maxExtension);
1336 0 : if (meNew == Position::INVALID) {
1337 : continue;
1338 : }
1339 163 : meNew = meNew + Position(0, 0, me.z());
1340 163 : shape.push_back(meNew);
1341 163 : }
1342 : // copy original z value
1343 : shape.back().set(shape.back().x(), shape.back().y(), me.z());
1344 2387 : }
1345 : }
1346 : *this = shape;
1347 52 : }
1348 :
1349 : double
1350 3093122 : PositionVector::localAngle(const Position& from, const Position& pos, const Position& to) {
1351 3093122 : return GeomHelper::angleDiff(from.angleTo2D(pos), pos.angleTo2D(to));
1352 : }
1353 :
1354 : double
1355 28702658 : PositionVector::angleAt2D(int pos) const {
1356 28702658 : if ((pos + 1) < (int)size()) {
1357 28702658 : return (*this)[pos].angleTo2D((*this)[pos + 1]);
1358 : }
1359 : return INVALID_DOUBLE;
1360 : }
1361 :
1362 :
1363 : void
1364 0 : PositionVector::openPolygon() {
1365 0 : if ((size() > 1) && (front() == back())) {
1366 : pop_back();
1367 : }
1368 0 : }
1369 :
1370 :
1371 : void
1372 749157 : PositionVector::closePolygon() {
1373 749157 : if ((size() != 0) && ((*this)[0] != back())) {
1374 415942 : push_back((*this)[0]);
1375 : }
1376 749157 : }
1377 :
1378 :
1379 : std::vector<double>
1380 2130914 : PositionVector::distances(const PositionVector& s, bool perpendicular) const {
1381 : std::vector<double> ret;
1382 : const_iterator i;
1383 9923126 : for (i = begin(); i != end(); i++) {
1384 7792212 : const double dist = s.distance2D(*i, perpendicular);
1385 7792212 : if (dist != GeomHelper::INVALID_OFFSET) {
1386 7757483 : ret.push_back(dist);
1387 : }
1388 : }
1389 9946399 : for (i = s.begin(); i != s.end(); i++) {
1390 7815485 : const double dist = distance2D(*i, perpendicular);
1391 7815485 : if (dist != GeomHelper::INVALID_OFFSET) {
1392 7811214 : ret.push_back(dist);
1393 : }
1394 : }
1395 2130914 : return ret;
1396 0 : }
1397 :
1398 :
1399 : double
1400 26734573 : PositionVector::distance2D(const Position& p, bool perpendicular) const {
1401 26734573 : if (size() == 0) {
1402 : return std::numeric_limits<double>::max();
1403 26734393 : } else if (size() == 1) {
1404 497807 : return front().distanceTo2D(p);
1405 : }
1406 26236586 : const double nearestOffset = nearest_offset_to_point2D(p, perpendicular);
1407 26236586 : if (nearestOffset == GeomHelper::INVALID_OFFSET) {
1408 : return GeomHelper::INVALID_OFFSET;
1409 : } else {
1410 26115539 : return p.distanceTo2D(positionAtOffset2D(nearestOffset));
1411 : }
1412 : }
1413 :
1414 :
1415 : void
1416 127076 : PositionVector::push_front(const Position& p) {
1417 127076 : if (empty()) {
1418 0 : push_back(p);
1419 : } else {
1420 127076 : insert(begin(), p);
1421 : }
1422 127076 : }
1423 :
1424 :
1425 : void
1426 0 : PositionVector::pop_front() {
1427 0 : if (empty()) {
1428 0 : throw ProcessError("PositionVector is empty");
1429 : } else {
1430 0 : erase(begin());
1431 : }
1432 0 : }
1433 :
1434 :
1435 : void
1436 5273515 : PositionVector::push_back_noDoublePos(const Position& p) {
1437 10449503 : if (size() == 0 || !p.almostSame(back())) {
1438 4910988 : push_back(p);
1439 : }
1440 5273515 : }
1441 :
1442 :
1443 : void
1444 138601 : PositionVector::push_front_noDoublePos(const Position& p) {
1445 277202 : if ((size() == 0) || !p.almostSame(front())) {
1446 127075 : push_front(p);
1447 : }
1448 138601 : }
1449 :
1450 :
1451 : void
1452 0 : PositionVector::insert_noDoublePos(const std::vector<Position>::iterator& at, const Position& p) {
1453 0 : if (at == begin()) {
1454 0 : push_front_noDoublePos(p);
1455 0 : } else if (at == end()) {
1456 0 : push_back_noDoublePos(p);
1457 : } else {
1458 0 : if (!p.almostSame(*at) && !p.almostSame(*(at - 1))) {
1459 0 : insert(at, p);
1460 : }
1461 : }
1462 0 : }
1463 :
1464 :
1465 : bool
1466 758208 : PositionVector::isClosed() const {
1467 758208 : return (size() >= 2) && ((*this)[0] == back());
1468 : }
1469 :
1470 :
1471 : bool
1472 1032136 : PositionVector::isNAN() const {
1473 : // iterate over all positions and check if is NAN
1474 4116611 : for (auto i = begin(); i != end(); i++) {
1475 : if (i->isNAN()) {
1476 : return true;
1477 : }
1478 : }
1479 : // all ok, then return false
1480 : return false;
1481 : }
1482 :
1483 : void
1484 248092 : PositionVector::ensureMinLength(int precision) {
1485 248092 : const double limit = 2 * pow(10, -precision);
1486 248092 : if (length2D() < limit) {
1487 8 : extrapolate2D(limit);
1488 : }
1489 248092 : }
1490 :
1491 : void
1492 342930 : PositionVector::round(int precision, bool avoidDegeneration) {
1493 342930 : if (avoidDegeneration && size() > 1) {
1494 111052 : ensureMinLength(precision);
1495 : }
1496 774497 : for (int i = 0; i < (int)size(); i++) {
1497 431567 : (*this)[i].round(precision);
1498 : }
1499 342930 : }
1500 :
1501 :
1502 : void
1503 19745423 : PositionVector::removeDoublePoints(double minDist, bool assertLength, int beginOffset, int endOffset, bool resample) {
1504 19745423 : int curSize = (int)size() - beginOffset - endOffset;
1505 19745423 : if (curSize > 1) {
1506 : iterator last = begin() + beginOffset;
1507 24448750 : for (iterator i = last + 1; i != (end() - endOffset) && (!assertLength || curSize > 2);) {
1508 4746638 : if (last->almostSame(*i, minDist)) {
1509 3471 : if (i + 1 == end() - endOffset) {
1510 : // special case: keep the last point and remove the next-to-last
1511 1183 : if (resample && last > begin() && (last - 1)->distanceTo(*i) >= 2 * minDist) {
1512 : // resample rather than remove point after a long segment
1513 3 : const double shiftBack = minDist - last->distanceTo(*i);
1514 : //if (gDebugFlag1) std::cout << " resample endOffset beforeLast=" << *(last - 1) << " last=" << *last << " i=" << *i;
1515 3 : (*last) = positionAtOffset(*(last - 1), *last, (last - 1)->distanceTo(*last) - shiftBack);
1516 : //if (gDebugFlag1) std::cout << " lastNew=" << *last;
1517 : last = i;
1518 : ++i;
1519 : } else {
1520 1180 : erase(last);
1521 : i = end() - endOffset;
1522 : }
1523 : } else {
1524 2288 : if (resample && i + 1 != end() && last->distanceTo(*(i + 1)) >= 2 * minDist) {
1525 : // resample rather than remove points before a long segment
1526 2 : const double shiftForward = minDist - last->distanceTo(*i);
1527 : //if (gDebugFlag1) std::cout << " resample last=" << *last << " i=" << *i << " next=" << *(i + 1);
1528 2 : (*i) = positionAtOffset(*i, *(i + 1), shiftForward);
1529 : //if (gDebugFlag1) std::cout << " iNew=" << *i << "\n";
1530 : last = i;
1531 : ++i;
1532 : } else {
1533 2286 : i = erase(i);
1534 : }
1535 : }
1536 3471 : curSize--;
1537 : } else {
1538 : last = i;
1539 : ++i;
1540 : }
1541 : }
1542 : }
1543 19745423 : }
1544 :
1545 :
1546 : bool
1547 538348 : PositionVector::operator==(const PositionVector& v2) const {
1548 538348 : return static_cast<vp>(*this) == static_cast<vp>(v2);
1549 : }
1550 :
1551 :
1552 : bool
1553 305684 : PositionVector::operator!=(const PositionVector& v2) const {
1554 305684 : return static_cast<vp>(*this) != static_cast<vp>(v2);
1555 : }
1556 :
1557 : PositionVector
1558 0 : PositionVector::operator-(const PositionVector& v2) const {
1559 0 : if (length() != v2.length()) {
1560 0 : WRITE_ERROR(TL("Trying to subtract PositionVectors of different lengths."));
1561 : }
1562 0 : PositionVector pv;
1563 : auto i1 = begin();
1564 : auto i2 = v2.begin();
1565 0 : while (i1 != end()) {
1566 0 : pv.add(*i1 - *i2);
1567 : }
1568 0 : return pv;
1569 0 : }
1570 :
1571 : PositionVector
1572 0 : PositionVector::operator+(const PositionVector& v2) const {
1573 0 : if (length() != v2.length()) {
1574 0 : WRITE_ERROR(TL("Trying to add PositionVectors of different lengths."));
1575 : }
1576 0 : PositionVector pv;
1577 : auto i1 = begin();
1578 : auto i2 = v2.begin();
1579 0 : while (i1 != end()) {
1580 0 : pv.add(*i1 + *i2);
1581 : }
1582 0 : return pv;
1583 0 : }
1584 :
1585 : bool
1586 5320 : PositionVector::almostSame(const PositionVector& v2, double maxDiv) const {
1587 5320 : if (size() != v2.size()) {
1588 : return false;
1589 : }
1590 : auto i2 = v2.begin();
1591 10477 : for (auto i1 = begin(); i1 != end(); i1++) {
1592 8005 : if (!i1->almostSame(*i2, maxDiv)) {
1593 : return false;
1594 : }
1595 : i2++;
1596 : }
1597 : return true;
1598 : }
1599 :
1600 : bool
1601 2132523 : PositionVector::hasElevation() const {
1602 2132523 : if (size() < 2) {
1603 : return false;
1604 : }
1605 9222920 : for (const_iterator i = begin(); i != end(); i++) {
1606 7092628 : if ((*i).z() != 0) {
1607 : return true;
1608 : }
1609 : }
1610 : return false;
1611 : }
1612 :
1613 :
1614 : bool
1615 104841247 : PositionVector::intersects(const Position& p11, const Position& p12, const Position& p21, const Position& p22, const double withinDist, double* x, double* y, double* mu) {
1616 : const double eps = std::numeric_limits<double>::epsilon();
1617 104841247 : const double denominator = (p22.y() - p21.y()) * (p12.x() - p11.x()) - (p22.x() - p21.x()) * (p12.y() - p11.y());
1618 104841247 : const double numera = (p22.x() - p21.x()) * (p11.y() - p21.y()) - (p22.y() - p21.y()) * (p11.x() - p21.x());
1619 104841247 : const double numerb = (p12.x() - p11.x()) * (p11.y() - p21.y()) - (p12.y() - p11.y()) * (p11.x() - p21.x());
1620 : /* Are the lines coincident? */
1621 104841247 : if (fabs(numera) < eps && fabs(numerb) < eps && fabs(denominator) < eps) {
1622 : double a1;
1623 : double a2;
1624 : double a3;
1625 : double a4;
1626 : double a = -1e12;
1627 14138 : if (p11.x() != p12.x()) {
1628 10398 : a1 = p11.x() < p12.x() ? p11.x() : p12.x();
1629 10398 : a2 = p11.x() < p12.x() ? p12.x() : p11.x();
1630 10398 : a3 = p21.x() < p22.x() ? p21.x() : p22.x();
1631 10398 : a4 = p21.x() < p22.x() ? p22.x() : p21.x();
1632 : } else {
1633 3740 : a1 = p11.y() < p12.y() ? p11.y() : p12.y();
1634 3740 : a2 = p11.y() < p12.y() ? p12.y() : p11.y();
1635 3740 : a3 = p21.y() < p22.y() ? p21.y() : p22.y();
1636 3740 : a4 = p21.y() < p22.y() ? p22.y() : p21.y();
1637 : }
1638 14138 : if (a1 <= a3 && a3 <= a2) {
1639 7786 : if (a4 < a2) {
1640 286 : a = (a3 + a4) / 2;
1641 : } else {
1642 7500 : a = (a2 + a3) / 2;
1643 : }
1644 : }
1645 14138 : if (a3 <= a1 && a1 <= a4) {
1646 7752 : if (a2 < a4) {
1647 244 : a = (a1 + a2) / 2;
1648 : } else {
1649 7508 : a = (a1 + a4) / 2;
1650 : }
1651 : }
1652 14138 : if (a != -1e12) {
1653 11073 : if (x != nullptr) {
1654 7659 : if (p11.x() != p12.x()) {
1655 6020 : *mu = (a - p11.x()) / (p12.x() - p11.x());
1656 6020 : *x = a;
1657 6020 : *y = p11.y() + (*mu) * (p12.y() - p11.y());
1658 : } else {
1659 1639 : *x = p11.x();
1660 1639 : *y = a;
1661 1639 : if (p12.y() == p11.y()) {
1662 32 : *mu = 0;
1663 : } else {
1664 1607 : *mu = (a - p11.y()) / (p12.y() - p11.y());
1665 : }
1666 : }
1667 : }
1668 11073 : return true;
1669 : }
1670 : return false;
1671 : }
1672 : /* Are the lines parallel */
1673 104827109 : if (fabs(denominator) < eps) {
1674 : return false;
1675 : }
1676 : /* Is the intersection along the segments */
1677 100951305 : double mua = numera / denominator;
1678 : /* reduce rounding errors for lines ending in the same point */
1679 100951305 : if (fabs(p12.x() - p22.x()) < eps && fabs(p12.y() - p22.y()) < eps) {
1680 : mua = 1.;
1681 : } else {
1682 100948548 : const double offseta = withinDist / p11.distanceTo2D(p12);
1683 100948548 : const double offsetb = withinDist / p21.distanceTo2D(p22);
1684 100948548 : const double mub = numerb / denominator;
1685 100948548 : if (mua < -offseta || mua > 1 + offseta || mub < -offsetb || mub > 1 + offsetb) {
1686 : return false;
1687 : }
1688 : }
1689 7009843 : if (x != nullptr) {
1690 5805843 : *x = p11.x() + mua * (p12.x() - p11.x());
1691 5805843 : *y = p11.y() + mua * (p12.y() - p11.y());
1692 5805843 : *mu = mua;
1693 : }
1694 : return true;
1695 : }
1696 :
1697 :
1698 : void
1699 6028 : PositionVector::rotate2D(double angle) {
1700 6028 : const double s = sin(angle);
1701 6028 : const double c = cos(angle);
1702 70444 : for (int i = 0; i < (int)size(); i++) {
1703 64416 : const double x = (*this)[i].x();
1704 64416 : const double y = (*this)[i].y();
1705 64416 : const double z = (*this)[i].z();
1706 64416 : const double xnew = x * c - y * s;
1707 64416 : const double ynew = x * s + y * c;
1708 64416 : (*this)[i].set(xnew, ynew, z);
1709 : }
1710 6028 : }
1711 :
1712 :
1713 : void
1714 0 : PositionVector::rotate2D(const Position& pos, double angle) {
1715 : PositionVector aux = *this;
1716 0 : aux.sub(pos);
1717 0 : aux.rotate2D(angle);
1718 0 : aux.add(pos);
1719 : *this = aux;
1720 0 : }
1721 :
1722 :
1723 : void
1724 0 : PositionVector::rotateAroundFirstElement2D(double angle) {
1725 0 : if (size() > 1) {
1726 : // translate position vector to (0,0), rotate, and traslate back again
1727 0 : const Position offset = front();
1728 0 : sub(offset);
1729 0 : rotate2D(angle);
1730 0 : add(offset);
1731 : }
1732 0 : }
1733 :
1734 :
1735 : PositionVector
1736 60198 : PositionVector::simplified() const {
1737 : PositionVector result = *this;
1738 : bool changed = true;
1739 173209 : while (changed && result.size() > 3) {
1740 : changed = false;
1741 509157 : for (int i = 0; i < (int)result.size(); i++) {
1742 468943 : const Position& p1 = result[i];
1743 468943 : const Position& p2 = result[(i + 2) % result.size()];
1744 468943 : const int middleIndex = (i + 1) % result.size();
1745 468943 : const Position& p0 = result[middleIndex];
1746 : // https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line#Line_defined_by_two_points
1747 468943 : const double triangleArea2 = fabs((p2.y() - p1.y()) * p0.x() - (p2.x() - p1.x()) * p0.y() + p2.x() * p1.y() - p2.y() * p1.x());
1748 : const double distIK = p1.distanceTo2D(p2);
1749 468943 : if (distIK > NUMERICAL_EPS && triangleArea2 / distIK < NUMERICAL_EPS) {
1750 : changed = true;
1751 : result.erase(result.begin() + middleIndex);
1752 12599 : break;
1753 : }
1754 : }
1755 : }
1756 60198 : return result;
1757 0 : }
1758 :
1759 :
1760 : const PositionVector
1761 1216 : PositionVector::simplified2(const bool closed, const double eps) const {
1762 : // this is a variation of the https://en.wikipedia.org/wiki/Visvalingam%E2%80%93Whyatt_algorithm
1763 : // which uses the distance instead of the area
1764 : // the benefits over the initial implementation are:
1765 : // 3D support, no degenerate results for a sequence of small distances, keeping the longest part of a line
1766 : // drawbacks: complexity of the code, speed
1767 1216 : if (size() < 3) {
1768 : return *this;
1769 : }
1770 15059 : auto calcScore = [&](const PositionVector & pv, int index) {
1771 15059 : if (!closed && (index == 0 || index == (int)pv.size() - 1)) {
1772 2025 : return eps + 1.;
1773 : }
1774 13034 : const Position& p = pv[index];
1775 13034 : const Position& a = pv[(index + (int)pv.size() - 1) % pv.size()];
1776 13034 : const Position& b = pv[(index + 1) % pv.size()];
1777 13034 : const double distAB = a.distanceTo(b);
1778 26068 : if (distAB < MIN2(eps, NUMERICAL_EPS)) {
1779 : // avoid division by 0 and degenerate cases due to very small baseline
1780 9 : return (a.distanceTo(p) + b.distanceTo(p)) / 2.;
1781 : }
1782 : // https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line#Vector_formulation
1783 : // calculating the distance of p to the line defined by a and b
1784 : const Position dir = (b - a) / distAB;
1785 : const double projectedLength = (a - p).dotProduct(dir);
1786 13025 : if (projectedLength <= -distAB) {
1787 5 : return b.distanceTo(p);
1788 : }
1789 13020 : if (projectedLength >= 0.) {
1790 4 : return a.distanceTo(p);
1791 : }
1792 : const Position distVector = (a - p) - dir * projectedLength;
1793 13016 : return distVector.length();
1794 829 : };
1795 : std::vector<double> scores;
1796 829 : double minScore = eps + 1.;
1797 : int minIndex = -1;
1798 11352 : for (int i = 0; i < (int)size(); i++) {
1799 10523 : scores.push_back(calcScore(*this, i));
1800 10523 : if (scores.back() < minScore) {
1801 : minScore = scores.back();
1802 : minIndex = i;
1803 : }
1804 : }
1805 829 : if (minScore >= eps) {
1806 : return *this;
1807 : }
1808 : PositionVector result(*this);
1809 2384 : while (minScore < eps) {
1810 : result.erase(result.begin() + minIndex);
1811 2298 : if (result.size() < 3) {
1812 : break;
1813 : }
1814 : scores.erase(scores.begin() + minIndex);
1815 2268 : const int prevIndex = (minIndex + (int)result.size() - 1) % result.size();
1816 2268 : scores[prevIndex] = calcScore(result, prevIndex);
1817 2268 : scores[minIndex % result.size()] = calcScore(result, minIndex % result.size());
1818 2268 : minScore = eps + 1.;
1819 142053 : for (int i = 0; i < (int)result.size(); i++) {
1820 139785 : if (scores[i] < minScore) {
1821 : minScore = scores[i];
1822 : minIndex = i;
1823 : }
1824 : }
1825 : }
1826 : return result;
1827 829 : }
1828 :
1829 :
1830 : PositionVector
1831 1930 : PositionVector::getOrthogonal(const Position& p, double extend, bool before, double length, double deg) const {
1832 1930 : PositionVector result;
1833 : PositionVector tmp = *this;
1834 1930 : tmp.extrapolate2D(extend);
1835 1930 : const double baseOffset = tmp.nearest_offset_to_point2D(p);
1836 1930 : if (baseOffset == GeomHelper::INVALID_OFFSET || size() < 2) {
1837 : // fail
1838 : return result;
1839 : }
1840 1930 : Position base = tmp.positionAtOffset2D(baseOffset);
1841 1930 : const int closestIndex = tmp.indexOfClosest(base);
1842 1930 : const double closestOffset = tmp.offsetAtIndex2D(closestIndex);
1843 1930 : result.push_back(base);
1844 1930 : if (fabs(baseOffset - closestOffset) > NUMERICAL_EPS) {
1845 1929 : result.push_back(tmp[closestIndex]);
1846 1929 : if ((closestOffset < baseOffset) != before) {
1847 1427 : deg *= -1;
1848 : }
1849 1 : } else if (before) {
1850 : // take the segment before closestIndex if possible
1851 1 : if (closestIndex > 0) {
1852 1 : result.push_back(tmp[closestIndex - 1]);
1853 : } else {
1854 0 : result.push_back(tmp[1]);
1855 0 : deg *= -1;
1856 : }
1857 : } else {
1858 : // take the segment after closestIndex if possible
1859 0 : if (closestIndex < (int)size() - 1) {
1860 0 : result.push_back(tmp[closestIndex + 1]);
1861 : } else {
1862 0 : result.push_back(tmp[-1]);
1863 0 : deg *= -1;
1864 : }
1865 : }
1866 3860 : result = result.getSubpart2D(0, length);
1867 : // rotate around base
1868 1930 : result.add(base * -1);
1869 1930 : result.rotate2D(DEG2RAD(deg));
1870 1930 : result.add(base);
1871 : return result;
1872 1930 : }
1873 :
1874 :
1875 : PositionVector
1876 184456 : PositionVector::smoothedZFront(double dist) const {
1877 : PositionVector result = *this;
1878 184456 : if (size() == 0) {
1879 : return result;
1880 : }
1881 184456 : const double z0 = (*this)[0].z();
1882 : // the z-delta of the first segment
1883 184456 : const double dz = (*this)[1].z() - z0;
1884 : // if the shape only has 2 points it is as smooth as possible already
1885 184456 : if (size() > 2 && dz != 0) {
1886 1272 : dist = MIN2(dist, length2D());
1887 : // check wether we need to insert a new point at dist
1888 1272 : Position pDist = positionAtOffset2D(dist);
1889 1272 : int iLast = indexOfClosest(pDist);
1890 : // prevent close spacing to reduce impact of rounding errors in z-axis
1891 1272 : if (pDist.distanceTo2D((*this)[iLast]) > POSITION_EPS * 20) {
1892 13 : iLast = result.insertAtClosest(pDist, false);
1893 : }
1894 1272 : double dist2 = result.offsetAtIndex2D(iLast);
1895 1272 : const double dz2 = result[iLast].z() - z0;
1896 : double seen = 0;
1897 6849 : for (int i = 1; i < iLast; ++i) {
1898 5577 : seen += result[i].distanceTo2D(result[i - 1]);
1899 5577 : result[i].set(result[i].x(), result[i].y(), z0 + dz2 * seen / dist2);
1900 : }
1901 : }
1902 : return result;
1903 :
1904 0 : }
1905 :
1906 :
1907 : PositionVector
1908 226650 : PositionVector::interpolateZ(double zStart, double zEnd) const {
1909 : PositionVector result = *this;
1910 226650 : if (size() == 0) {
1911 : return result;
1912 : }
1913 226650 : result[0].setz(zStart);
1914 226650 : result[-1].setz(zEnd);
1915 226650 : const double dz = zEnd - zStart;
1916 226650 : const double length = length2D();
1917 : double seen = 0;
1918 360082 : for (int i = 1; i < (int)size() - 1; ++i) {
1919 133432 : seen += result[i].distanceTo2D(result[i - 1]);
1920 133432 : result[i].setz(zStart + dz * seen / length);
1921 : }
1922 : return result;
1923 0 : }
1924 :
1925 :
1926 : PositionVector
1927 0 : PositionVector::resample(double maxLength, const bool adjustEnd) const {
1928 0 : PositionVector result;
1929 0 : if (maxLength == 0) {
1930 : return result;
1931 : }
1932 0 : const double length = length2D();
1933 0 : if (length < POSITION_EPS) {
1934 : return result;
1935 : }
1936 0 : maxLength = length / ceil(length / maxLength);
1937 0 : for (double pos = 0; pos <= length; pos += maxLength) {
1938 0 : result.push_back(positionAtOffset2D(pos));
1939 : }
1940 : // check if we have to adjust last element
1941 0 : if (adjustEnd && !result.empty() && (result.back() != back())) {
1942 : // add last element
1943 0 : result.push_back(back());
1944 : }
1945 : return result;
1946 0 : }
1947 :
1948 :
1949 : double
1950 3318 : PositionVector::offsetAtIndex2D(int index) const {
1951 3318 : if (index < 0 || index >= (int)size()) {
1952 0 : return GeomHelper::INVALID_OFFSET;
1953 : }
1954 : double seen = 0;
1955 13170 : for (int i = 1; i <= index; ++i) {
1956 9852 : seen += (*this)[i].distanceTo2D((*this)[i - 1]);
1957 : }
1958 : return seen;
1959 : }
1960 :
1961 :
1962 : double
1963 5476 : PositionVector::getMaxGrade(double& maxJump) const {
1964 : double result = 0;
1965 11615 : for (int i = 1; i < (int)size(); ++i) {
1966 6139 : const Position& p1 = (*this)[i - 1];
1967 6139 : const Position& p2 = (*this)[i];
1968 6139 : const double distZ = fabs(p1.z() - p2.z());
1969 : const double dist2D = p1.distanceTo2D(p2);
1970 6139 : if (dist2D == 0) {
1971 0 : maxJump = MAX2(maxJump, distZ);
1972 : } else {
1973 6139 : result = MAX2(result, distZ / dist2D);
1974 : }
1975 : }
1976 5476 : return result;
1977 : }
1978 :
1979 :
1980 : double
1981 280 : PositionVector::getMinZ() const {
1982 : double minZ = std::numeric_limits<double>::max();
1983 840 : for (const Position& i : *this) {
1984 : minZ = MIN2(minZ, i.z());
1985 : }
1986 280 : return minZ;
1987 : }
1988 :
1989 :
1990 : PositionVector
1991 185735 : PositionVector::bezier(int numPoints) {
1992 : // inspired by David F. Rogers
1993 : assert(size() < 33);
1994 : static const double fac[33] = {
1995 : 1.0, 1.0, 2.0, 6.0, 24.0, 120.0, 720.0, 5040.0, 40320.0, 362880.0, 3628800.0, 39916800.0, 479001600.0,
1996 : 6227020800.0, 87178291200.0, 1307674368000.0, 20922789888000.0, 355687428096000.0, 6402373705728000.0,
1997 : 121645100408832000.0, 2432902008176640000.0, 51090942171709440000.0, 1124000727777607680000.0,
1998 : 25852016738884976640000.0, 620448401733239439360000.0, 15511210043330985984000000.0,
1999 : 403291461126605635584000000.0, 10888869450418352160768000000.0, 304888344611713860501504000000.0,
2000 : 8841761993739701954543616000000.0, 265252859812191058636308480000000.0,
2001 : 8222838654177922817725562880000000.0, 263130836933693530167218012160000000.0
2002 : };
2003 185735 : PositionVector ret;
2004 185735 : const int npts = (int)size();
2005 : // calculate the points on the Bezier curve
2006 185735 : const double step = (double) 1.0 / (numPoints - 1);
2007 : double t = 0.;
2008 : Position prev;
2009 1279462 : for (int i1 = 0; i1 < numPoints; i1++) {
2010 1093727 : if ((1.0 - t) < 5e-6) {
2011 : t = 1.0;
2012 : }
2013 : double x = 0., y = 0., z = 0.;
2014 4601658 : for (int i = 0; i < npts; i++) {
2015 3507931 : const double ti = (i == 0) ? 1.0 : pow(t, i);
2016 3507931 : const double tni = (npts == i + 1) ? 1.0 : pow(1 - t, npts - i - 1);
2017 3507931 : const double basis = fac[npts - 1] / (fac[i] * fac[npts - 1 - i]) * ti * tni;
2018 3507931 : x += basis * at(i).x();
2019 3507931 : y += basis * at(i).y();
2020 3507931 : z += basis * at(i).z();
2021 : }
2022 1093727 : t += step;
2023 : Position current(x, y, z);
2024 1093727 : if (prev != current && !std::isnan(x) && !std::isnan(y) && !std::isnan(z)) {
2025 1093727 : ret.push_back(current);
2026 : }
2027 : prev = current;
2028 : }
2029 185735 : return ret;
2030 0 : }
2031 :
2032 :
2033 3 : bool PositionVector::isClockwiseOriented() {
2034 : // The test is based on the computation of a signed area enclosed by the polygon.
2035 : // If the polygon is in the upper (resp. the lower) half-plane and the area is
2036 : // negatively (resp. positively) signed, then the polygon is CW oriented. In case
2037 : // the polygon has points with both positive and negative y-coordinates, we translate
2038 : // the polygon to apply the above simple area-based test.
2039 : double area = 0.0;
2040 : const double y_min = std::min_element(begin(), end(), [](Position p1, Position p2) {
2041 : return p1.y() < p2.y();
2042 : })->y();
2043 3 : const double gap = y_min > 0.0 ? 0.0 : y_min;
2044 3 : add(0., gap, 0.);
2045 3 : const int last = (int)size() - 1;
2046 10 : for (int i = 0; i < last; i++) {
2047 7 : const Position& firstPoint = at(i);
2048 7 : const Position& secondPoint = at(i + 1);
2049 7 : area += (secondPoint.x() - firstPoint.x()) / (secondPoint.y() + firstPoint.y()) / 2.0;
2050 : }
2051 3 : area += (at(0).x() - at(last).x()) / (at(0).y() + at(last).y()) / 2.0;
2052 3 : add(0., -gap, 0.);
2053 3 : return area < 0.0;
2054 : }
2055 :
2056 :
2057 : /****************************************************************************/
|
