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 69615658 : PositionVector::PositionVector() {}
54 :
55 :
56 582 : PositionVector::PositionVector(const std::vector<Position>& v) {
57 : std::copy(v.begin(), v.end(), std::back_inserter(*this));
58 582 : }
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 10595409 : PositionVector::PositionVector(const Position& p1, const Position& p2) {
67 10595409 : push_back(p1);
68 10595409 : push_back(p2);
69 10595409 : }
70 :
71 :
72 349041502 : PositionVector::~PositionVector() {}
73 :
74 :
75 : bool
76 44772821 : PositionVector::around(const Position& p, double offset) const {
77 44772821 : if (size() < 2) {
78 : return false;
79 : }
80 44772812 : 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 186767738 : for (const_iterator i = begin(); i != (end() - 1); i++) {
88 : Position p1(
89 : i->x() - p.x(),
90 141995722 : i->y() - p.y());
91 : Position p2(
92 : (i + 1)->x() - p.x(),
93 141995722 : (i + 1)->y() - p.y());
94 141995722 : angle += GeomHelper::angle2D(p1, p2);
95 : }
96 : // add angle between last and first point
97 : Position p1(
98 : (end() - 1)->x() - p.x(),
99 44772016 : (end() - 1)->y() - p.y());
100 : Position p2(
101 : begin()->x() - p.x(),
102 44772016 : begin()->y() - p.y());
103 44772016 : angle += GeomHelper::angle2D(p1, p2);
104 : // if angle is less than PI, then point lying in Polygon
105 44772016 : return (!(fabs(angle) < M_PI));
106 : }
107 :
108 :
109 : bool
110 5478230 : PositionVector::overlapsWith(const AbstractPoly& poly, double offset) const {
111 : if (
112 : // check whether one of my points lies within the given poly
113 10933475 : partialWithin(poly, offset) ||
114 : // check whether the polygon lies within me
115 5455245 : poly.partialWithin(*this, offset)) {
116 43090 : return true;
117 : }
118 5435140 : if (size() >= 2) {
119 21768082 : for (const_iterator i = begin(); i != end() - 1; i++) {
120 16334923 : if (poly.crosses(*i, *(i + 1))) {
121 : return true;
122 : }
123 : }
124 5433159 : 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 28256657 : PositionVector::intersects(const Position& p1, const Position& p2) const {
162 28256657 : if (size() < 2) {
163 : return false;
164 : }
165 100007408 : for (const_iterator i = begin(); i != end() - 1; i++) {
166 73096227 : if (intersects(*i, *(i + 1), p1, p2)) {
167 : return true;
168 : }
169 : }
170 : return false;
171 : }
172 :
173 :
174 : bool
175 1420694 : PositionVector::intersects(const PositionVector& v1) const {
176 1420694 : if (size() < 2) {
177 : return false;
178 : }
179 1510171 : for (const_iterator i = begin(); i != end() - 1; i++) {
180 1382197 : if (v1.intersects(*i, *(i + 1))) {
181 : return true;
182 : }
183 : }
184 : return false;
185 : }
186 :
187 :
188 : Position
189 2843731 : PositionVector::intersectionPosition2D(const Position& p1, const Position& p2, const double withinDist) const {
190 2844883 : for (const_iterator i = begin(); i != end() - 1; i++) {
191 : double x, y, m;
192 2844877 : if (intersects(*i, *(i + 1), p1, p2, withinDist, &x, &y, &m)) {
193 2843725 : return Position(x, y);
194 : }
195 : }
196 6 : return Position::INVALID;
197 : }
198 :
199 :
200 : Position
201 221878 : PositionVector::intersectionPosition2D(const PositionVector& v1) const {
202 227481 : for (const_iterator i = begin(); i != end() - 1; i++) {
203 224119 : if (v1.intersects(*i, *(i + 1))) {
204 218516 : return v1.intersectionPosition2D(*i, *(i + 1));
205 : }
206 : }
207 3362 : return Position::INVALID;
208 : }
209 :
210 :
211 : const Position&
212 494538477 : 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 494538477 : if (index >= 0 && index < (int)size()) {
221 392015607 : return at(index);
222 102522870 : } else if (index < 0 && -index <= (int)size()) {
223 102522870 : 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 714161603 : 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 714161603 : if (index >= 0 && index < (int)size()) {
240 585369554 : return at(index);
241 128792049 : } else if (index < 0 && -index <= (int)size()) {
242 128792049 : 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 1629735215 : PositionVector::positionAtOffset(double pos, double lateralOffset) const {
251 1629735215 : if (size() == 0) {
252 0 : return Position::INVALID;
253 : }
254 1629735215 : if (size() == 1) {
255 1 : return front();
256 : }
257 : const_iterator i = begin();
258 : double seenLength = 0;
259 : do {
260 1951730254 : const double nextLength = (*i).distanceTo(*(i + 1));
261 1951730254 : if (seenLength + nextLength > pos) {
262 1626939337 : return positionAtOffset(*i, *(i + 1), pos - seenLength, lateralOffset);
263 : }
264 : seenLength += nextLength;
265 324790917 : } while (++i != end() - 1);
266 2795877 : if (lateralOffset == 0 || size() < 2) {
267 2777613 : return back();
268 : } else {
269 18264 : return positionAtOffset(*(end() - 2), *(end() - 1), (*(end() - 2)).distanceTo(*(end() - 1)), lateralOffset);
270 : }
271 : }
272 :
273 :
274 : Position
275 189227 : PositionVector::sidePositionAtAngle(double pos, double lateralOffset, double angle) const {
276 189227 : if (size() == 0) {
277 0 : return Position::INVALID;
278 : }
279 189227 : if (size() == 1) {
280 0 : return front();
281 : }
282 : const_iterator i = begin();
283 : double seenLength = 0;
284 : do {
285 518048 : const double nextLength = (*i).distanceTo(*(i + 1));
286 518048 : if (seenLength + nextLength > pos) {
287 188637 : return sidePositionAtAngle(*i, *(i + 1), pos - seenLength, lateralOffset, angle);
288 : }
289 : seenLength += nextLength;
290 329411 : } while (++i != end() - 1);
291 590 : return sidePositionAtAngle(*(end() - 2), *(end() - 1), (*(end() - 2)).distanceTo(*(end() - 1)), lateralOffset, angle);
292 : }
293 :
294 :
295 : Position
296 439629130 : PositionVector::positionAtOffset2D(double pos, double lateralOffset, bool extrapolateBeyond) const {
297 439629130 : if (size() == 0) {
298 0 : return Position::INVALID;
299 : }
300 439629130 : 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 628105626 : if (seenLength + nextLength > pos) {
308 307518218 : return positionAtOffset2D(*i, *(i + 1), pos - seenLength, lateralOffset, extrapolateBeyond);
309 : }
310 : seenLength += nextLength;
311 320587408 : } while (++i != end() - 1);
312 132110912 : if (extrapolateBeyond) {
313 13 : return positionAtOffset2D(*(i - 1), *i, pos - seenLength + (*i).distanceTo2D(*(i - 1)), lateralOffset, extrapolateBeyond);
314 : }
315 132110899 : return back();
316 : }
317 :
318 :
319 : double
320 188291142 : PositionVector::rotationAtOffset(double pos) const {
321 188291142 : if ((size() == 0) || (size() == 1)) {
322 : return INVALID_DOUBLE;
323 : }
324 188291142 : if (pos < 0) {
325 2855 : 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 262761220 : const double nextLength = p1.distanceTo(p2);
333 262761220 : if (seenLength + nextLength > pos) {
334 123251024 : return p1.angleTo2D(p2);
335 : }
336 : seenLength += nextLength;
337 139510196 : } while (++i != end() - 1);
338 65040118 : const Position& p1 = (*this)[-2];
339 : const Position& p2 = back();
340 65040118 : return p1.angleTo2D(p2);
341 : }
342 :
343 :
344 : double
345 12598 : PositionVector::rotationDegreeAtOffset(double pos) const {
346 12598 : return GeomHelper::legacyDegree(rotationAtOffset(pos));
347 : }
348 :
349 :
350 : double
351 2641754 : PositionVector::slopeDegreeAtOffset(double pos) const {
352 2641754 : 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 2876391 : const double nextLength = p1.distanceTo(p2);
361 2876391 : if (seenLength + nextLength > pos) {
362 500187 : return RAD2DEG(p1.slopeTo2D(p2));
363 : }
364 : seenLength += nextLength;
365 2376204 : } while (++i != end() - 1);
366 2141567 : const Position& p1 = (*this)[-2];
367 : const Position& p2 = back();
368 2141567 : return RAD2DEG(p1.slopeTo2D(p2));
369 : }
370 :
371 :
372 : Position
373 1632762556 : PositionVector::positionAtOffset(const Position& p1, const Position& p2, double pos, double lateralOffset) {
374 1632762556 : const double dist = p1.distanceTo(p2);
375 1632762556 : if (pos < 0. || dist < pos) {
376 211417 : return Position::INVALID;
377 : }
378 1632551139 : if (lateralOffset != 0) {
379 72746576 : if (dist == 0.) {
380 289 : return Position::INVALID;
381 : }
382 72746287 : const Position offset = sideOffset(p1, p2, -lateralOffset); // move in the same direction as Position::move2side
383 72746287 : if (pos == 0.) {
384 : return p1 + offset;
385 : }
386 72620492 : return p1 + (p2 - p1) * (pos / dist) + offset;
387 : }
388 1559804563 : if (pos == 0.) {
389 1142192 : return p1;
390 : }
391 1558662371 : return p1 + (p2 - p1) * (pos / dist);
392 : }
393 :
394 :
395 : Position
396 189227 : PositionVector::sidePositionAtAngle(const Position& p1, const Position& p2, double pos, double lateralOffset, double angle) {
397 189227 : const double dist = p1.distanceTo(p2);
398 189227 : if (pos < 0. || dist < pos || dist == 0) {
399 5 : return Position::INVALID;
400 : }
401 189222 : angle -= DEG2RAD(90);
402 189222 : const Position offset(cos(angle) * lateralOffset, sin(angle) * lateralOffset);
403 189222 : return p1 + (p2 - p1) * (pos / dist) + offset;
404 : }
405 :
406 :
407 : Position
408 1449994236 : PositionVector::positionAtOffset2D(const Position& p1, const Position& p2, double pos, double lateralOffset, bool extrapolateBeyond) {
409 : const double dist = p1.distanceTo2D(p2);
410 1449994236 : if ((pos < 0 || dist < pos) && !extrapolateBeyond) {
411 171 : return Position::INVALID;
412 : }
413 1449994065 : if (dist == 0) {
414 162508 : return p1;
415 : }
416 1449831557 : if (lateralOffset != 0) {
417 143552 : const Position offset = sideOffset(p1, p2, -lateralOffset); // move in the same direction as Position::move2side
418 143552 : if (pos == 0.) {
419 : return p1 + offset;
420 : }
421 143552 : return p1 + (p2 - p1) * (pos / dist) + offset;
422 : }
423 1449688005 : if (pos == 0.) {
424 626225803 : return p1;
425 : }
426 823462202 : return p1 + (p2 - p1) * (pos / dist);
427 : }
428 :
429 :
430 : Boundary
431 2950475 : PositionVector::getBoxBoundary() const {
432 2950475 : Boundary ret;
433 9178475 : for (const Position& i : *this) {
434 6228000 : ret.add(i);
435 : }
436 2950475 : return ret;
437 : }
438 :
439 :
440 : Position
441 16483 : PositionVector::getPolygonCenter() const {
442 16483 : if (size() == 0) {
443 0 : return Position::INVALID;
444 : }
445 : double x = 0;
446 : double y = 0;
447 : double z = 0;
448 94259 : for (const Position& i : *this) {
449 77776 : x += i.x();
450 77776 : y += i.y();
451 77776 : z += i.z();
452 : }
453 16483 : return Position(x / (double) size(), y / (double) size(), z / (double)size());
454 : }
455 :
456 :
457 : Position
458 505510 : PositionVector::getCentroid() const {
459 505510 : if (size() == 0) {
460 0 : return Position::INVALID;
461 505510 : } else if (size() == 1) {
462 50 : return (*this)[0];
463 505460 : } else if (size() == 2) {
464 89640 : return ((*this)[0] + (*this)[1]) * 0.5;
465 : }
466 : PositionVector tmp = *this;
467 415820 : if (!isClosed()) { // make sure its closed
468 385779 : tmp.push_back(tmp[0]);
469 : }
470 : // shift to origin to increase numerical stability
471 415820 : Position offset = tmp[0];
472 : Position result;
473 415820 : tmp.sub(offset);
474 415820 : const int endIndex = (int)tmp.size() - 1;
475 : double div = 0.; // 6 * area including sign
476 : double x = 0.;
477 : double y = 0.;
478 415820 : if (tmp.area() != 0.) { // numerical instability ?
479 : // http://en.wikipedia.org/wiki/Polygon
480 5136911 : for (int i = 0; i < endIndex; i++) {
481 4737686 : const double z = tmp[i].x() * tmp[i + 1].y() - tmp[i + 1].x() * tmp[i].y();
482 4737686 : div += z; // area formula
483 4737686 : x += (tmp[i].x() + tmp[i + 1].x()) * z;
484 4737686 : y += (tmp[i].y() + tmp[i + 1].y()) * z;
485 : }
486 399225 : div *= 3; // 6 / 2, the 2 comes from the area formula
487 399225 : 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 66017 : for (int i = 0; i < endIndex; i++) {
493 49422 : double length = tmp[i].distanceTo(tmp[i + 1]);
494 49422 : x += (tmp[i].x() + tmp[i + 1].x()) * length / 2;
495 49422 : y += (tmp[i].y() + tmp[i + 1].y()) * length / 2;
496 49422 : lengthSum += length;
497 : }
498 : // if lengthSum == 0, it is probably only one point
499 16595 : result = lengthSum == 0. ? tmp[0] : Position(x / lengthSum, y / lengthSum);
500 : }
501 : return result + offset;
502 415820 : }
503 :
504 :
505 : void
506 72408 : PositionVector::scaleRelative(double factor) {
507 72408 : Position centroid = getCentroid();
508 217227 : for (int i = 0; i < static_cast<int>(size()); i++) {
509 144819 : (*this)[i] = centroid + (((*this)[i] - centroid) * factor);
510 : }
511 72408 : }
512 :
513 :
514 : void
515 796 : PositionVector::scaleAbsolute(double offset) {
516 796 : Position centroid = getCentroid();
517 6045 : for (int i = 0; i < static_cast<int>(size()); i++) {
518 5249 : (*this)[i] = centroid + (((*this)[i] - centroid) + offset);
519 : }
520 796 : }
521 :
522 :
523 : Position
524 5663 : PositionVector::getLineCenter() const {
525 5663 : if (size() == 1) {
526 0 : return (*this)[0];
527 : } else {
528 5663 : return positionAtOffset(double((length() / 2.)));
529 : }
530 : }
531 :
532 :
533 : double
534 183399196 : PositionVector::length() const {
535 183399196 : if (size() == 0) {
536 : return 0;
537 : }
538 : double len = 0;
539 508880727 : for (const_iterator i = begin(); i != end() - 1; i++) {
540 325494239 : len += (*i).distanceTo(*(i + 1));
541 : }
542 : return len;
543 : }
544 :
545 :
546 : double
547 37238854 : PositionVector::length2D() const {
548 37238854 : if (size() == 0) {
549 : return 0;
550 : }
551 : double len = 0;
552 99202441 : for (const_iterator i = begin(); i != end() - 1; i++) {
553 61963587 : len += (*i).distanceTo2D(*(i + 1));
554 : }
555 : return len;
556 : }
557 :
558 :
559 : double
560 415957 : PositionVector::area() const {
561 415957 : if (size() < 3) {
562 : return 0;
563 : }
564 : double area = 0;
565 : PositionVector tmp = *this;
566 415957 : if (!isClosed()) { // make sure its closed
567 28 : tmp.push_back(tmp[0]);
568 : }
569 415957 : const int endIndex = (int)tmp.size() - 1;
570 : // http://en.wikipedia.org/wiki/Polygon
571 5206979 : for (int i = 0; i < endIndex; i++) {
572 4791022 : area += tmp[i].x() * tmp[i + 1].y() - tmp[i + 1].x() * tmp[i].y();
573 : }
574 415957 : if (area < 0) { // we whether we had cw or ccw order
575 387876 : area *= -1;
576 : }
577 415957 : return area / 2;
578 415957 : }
579 :
580 :
581 : bool
582 10934464 : PositionVector::partialWithin(const AbstractPoly& poly, double offset) const {
583 10934464 : if (size() < 2) {
584 : return false;
585 : }
586 54677730 : for (const_iterator i = begin(); i != end(); i++) {
587 43787048 : if (poly.around(*i, offset)) {
588 : return true;
589 : }
590 : }
591 : return false;
592 : }
593 :
594 :
595 : bool
596 21769266 : PositionVector::crosses(const Position& p1, const Position& p2) const {
597 21769266 : return intersects(p1, p2);
598 : }
599 :
600 :
601 : std::pair<PositionVector, PositionVector>
602 265409 : PositionVector::splitAt(double where, bool use2D) const {
603 265409 : const double len = use2D ? length2D() : length();
604 265409 : if (size() < 2) {
605 0 : throw InvalidArgument("Vector to short for splitting");
606 : }
607 265409 : if (where < 0 || where > len) {
608 0 : throw InvalidArgument("Invalid split position " + toString(where) + " for vector of length " + toString(len));
609 : }
610 265409 : if (where <= POSITION_EPS || where >= len - POSITION_EPS) {
611 2 : WRITE_WARNINGF(TL("Splitting vector close to end (pos: %, length: %)"), toString(where), toString(len));
612 : }
613 265409 : PositionVector first, second;
614 265409 : first.push_back((*this)[0]);
615 : double seen = 0;
616 : const_iterator it = begin() + 1;
617 265409 : double next = use2D ? first.back().distanceTo2D(*it) : first.back().distanceTo(*it);
618 : // see how many points we can add to first
619 357575 : while (where >= seen + next + POSITION_EPS) {
620 : seen += next;
621 92166 : first.push_back(*it);
622 : it++;
623 92166 : next = use2D ? first.back().distanceTo2D(*it) : first.back().distanceTo(*it);
624 : }
625 265409 : if (fabs(where - (seen + next)) > POSITION_EPS || it == end() - 1) {
626 : // we need to insert a new point because 'where' is not close to an
627 : // existing point or it is to close to the endpoint
628 : const Position p = (use2D
629 254717 : ? positionAtOffset2D(first.back(), *it, where - seen)
630 11546 : : positionAtOffset(first.back(), *it, where - seen));
631 254717 : first.push_back(p);
632 254717 : second.push_back(p);
633 : } else {
634 10692 : first.push_back(*it);
635 : }
636 : // add the remaining points to second
637 661505 : for (; it != end(); it++) {
638 396096 : second.push_back(*it);
639 : }
640 : assert(first.size() >= 2);
641 : assert(second.size() >= 2);
642 : assert(first.back() == second.front());
643 : assert(fabs((use2D ? first.length2D() + second.length2D() : first.length() + second.length()) - len) < 2 * POSITION_EPS);
644 530818 : return std::pair<PositionVector, PositionVector>(first, second);
645 265409 : }
646 :
647 :
648 : std::ostream&
649 410228 : operator<<(std::ostream& os, const PositionVector& geom) {
650 2504361 : for (PositionVector::const_iterator i = geom.begin(); i != geom.end(); i++) {
651 2094133 : if (i != geom.begin()) {
652 1683910 : os << " ";
653 : }
654 2094133 : os << (*i);
655 : }
656 410228 : return os;
657 : }
658 :
659 :
660 : void
661 3 : PositionVector::sortAsPolyCWByAngle() {
662 : // We take the centroid of the points as an origin for the angle computations
663 : // that will follow but other points could be taken (the center of the bounding
664 : // box of the polygon for instance). Each of these can potentially lead
665 : // to a different result in the case of a non-convex polygon.
666 3 : const Position centroid = std::accumulate(begin(), end(), Position(0, 0)) / (double)size();
667 3 : sub(centroid);
668 3 : std::sort(begin(), end(), as_poly_cw_sorter());
669 3 : add(centroid);
670 3 : }
671 :
672 :
673 : void
674 767437 : PositionVector::add(double xoff, double yoff, double zoff) {
675 6482596 : for (int i = 0; i < (int)size(); i++) {
676 5715159 : (*this)[i].add(xoff, yoff, zoff);
677 : }
678 767437 : }
679 :
680 :
681 : void
682 416028 : PositionVector::sub(const Position& offset) {
683 416028 : add(-offset.x(), -offset.y(), -offset.z());
684 416028 : }
685 :
686 :
687 : void
688 7300 : PositionVector::add(const Position& offset) {
689 7300 : add(offset.x(), offset.y(), offset.z());
690 7300 : }
691 :
692 :
693 : PositionVector
694 0 : PositionVector::added(const Position& offset) const {
695 0 : PositionVector pv;
696 0 : for (auto i1 = begin(); i1 != end(); ++i1) {
697 0 : pv.push_back(*i1 + offset);
698 : }
699 0 : return pv;
700 0 : }
701 :
702 :
703 : void
704 9839 : PositionVector::mirrorX() {
705 27129 : for (int i = 0; i < (int)size(); i++) {
706 17290 : (*this)[i].mul(1, -1);
707 : }
708 9839 : }
709 :
710 :
711 3 : PositionVector::as_poly_cw_sorter::as_poly_cw_sorter() {}
712 :
713 :
714 : int
715 8 : PositionVector::as_poly_cw_sorter::operator()(const Position& p1, const Position& p2) const {
716 8 : double angle1 = atAngle2D(p1);
717 8 : double angle2 = atAngle2D(p2);
718 8 : if (angle1 > angle2) {
719 : return true;
720 : }
721 2 : if (angle1 == angle2) {
722 : double squaredDistance1 = p1.dotProduct(p1);
723 : double squaredDistance2 = p2.dotProduct(p2);
724 0 : if (squaredDistance1 < squaredDistance2) {
725 0 : return true;
726 : }
727 : }
728 : return false;
729 : }
730 :
731 :
732 : double
733 16 : PositionVector::as_poly_cw_sorter::atAngle2D(const Position& p) const {
734 16 : double angle = atan2(p.y(), p.x());
735 16 : return angle < 0.0 ? angle : angle + 2.0 * M_PI;
736 : }
737 :
738 : void
739 0 : PositionVector::sortByIncreasingXY() {
740 0 : std::sort(begin(), end(), increasing_x_y_sorter());
741 0 : }
742 :
743 :
744 0 : PositionVector::increasing_x_y_sorter::increasing_x_y_sorter() {}
745 :
746 :
747 : int
748 0 : PositionVector::increasing_x_y_sorter::operator()(const Position& p1, const Position& p2) const {
749 0 : if (p1.x() != p2.x()) {
750 0 : return p1.x() < p2.x();
751 : }
752 0 : return p1.y() < p2.y();
753 : }
754 :
755 :
756 : double
757 5329845 : PositionVector::isLeft(const Position& P0, const Position& P1, const Position& P2) const {
758 5329845 : return (P1.x() - P0.x()) * (P2.y() - P0.y()) - (P2.x() - P0.x()) * (P1.y() - P0.y());
759 : }
760 :
761 :
762 : void
763 8443182 : PositionVector::append(const PositionVector& v, double sameThreshold) {
764 8443182 : if ((size() > 0) && (v.size() > 0) && (back().distanceTo(v[0]) < sameThreshold)) {
765 : copy(v.begin() + 1, v.end(), back_inserter(*this));
766 : } else {
767 : copy(v.begin(), v.end(), back_inserter(*this));
768 : }
769 8443182 : }
770 :
771 :
772 : void
773 375 : PositionVector::prepend(const PositionVector& v, double sameThreshold) {
774 375 : if ((size() > 0) && (v.size() > 0) && (front().distanceTo(v.back()) < sameThreshold)) {
775 148 : insert(begin(), v.begin(), v.end() - 1);
776 : } else {
777 227 : insert(begin(), v.begin(), v.end());
778 : }
779 375 : }
780 :
781 :
782 : PositionVector
783 690989 : PositionVector::getSubpart(double beginOffset, double endOffset) const {
784 690989 : PositionVector ret;
785 690989 : Position begPos = front();
786 690989 : if (beginOffset > POSITION_EPS) {
787 60552 : begPos = positionAtOffset(beginOffset);
788 : }
789 690989 : Position endPos = back();
790 690989 : if (endOffset < length() - POSITION_EPS) {
791 183457 : endPos = positionAtOffset(endOffset);
792 : }
793 690989 : ret.push_back(begPos);
794 :
795 : double seen = 0;
796 : const_iterator i = begin();
797 : // skip previous segments
798 690989 : while ((i + 1) != end()
799 711924 : &&
800 711923 : seen + (*i).distanceTo(*(i + 1)) < beginOffset) {
801 20935 : seen += (*i).distanceTo(*(i + 1));
802 : i++;
803 : }
804 : // append segments in between
805 : while ((i + 1) != end()
806 787377 : &&
807 786874 : seen + (*i).distanceTo(*(i + 1)) < endOffset) {
808 :
809 96388 : ret.push_back_noDoublePos(*(i + 1));
810 96388 : seen += (*i).distanceTo(*(i + 1));
811 : i++;
812 : }
813 : // append end
814 690989 : ret.push_back_noDoublePos(endPos);
815 690989 : if (ret.size() == 1) {
816 30 : ret.push_back(endPos);
817 : }
818 690989 : return ret;
819 0 : }
820 :
821 :
822 : PositionVector
823 1389763 : PositionVector::getSubpart2D(double beginOffset, double endOffset) const {
824 1389763 : if (size() == 0) {
825 0 : return PositionVector();
826 : }
827 1389763 : PositionVector ret;
828 1389763 : Position begPos = front();
829 1389763 : if (beginOffset > POSITION_EPS) {
830 715121 : begPos = positionAtOffset2D(beginOffset);
831 : }
832 1389763 : Position endPos = back();
833 1389763 : if (endOffset < length2D() - POSITION_EPS) {
834 221787 : endPos = positionAtOffset2D(endOffset);
835 : }
836 1389763 : ret.push_back(begPos);
837 :
838 : double seen = 0;
839 : const_iterator i = begin();
840 : // skip previous segments
841 1389763 : while ((i + 1) != end()
842 1471705 : &&
843 1471705 : seen + (*i).distanceTo2D(*(i + 1)) < beginOffset) {
844 81942 : seen += (*i).distanceTo2D(*(i + 1));
845 : i++;
846 : }
847 : // append segments in between
848 : while ((i + 1) != end()
849 3617209 : &&
850 3250905 : seen + (*i).distanceTo2D(*(i + 1)) < endOffset) {
851 :
852 2227446 : ret.push_back_noDoublePos(*(i + 1));
853 2227446 : seen += (*i).distanceTo2D(*(i + 1));
854 : i++;
855 : }
856 : // append end
857 1389763 : ret.push_back_noDoublePos(endPos);
858 1389763 : if (ret.size() == 1) {
859 27 : ret.push_back(endPos);
860 : }
861 : return ret;
862 1389763 : }
863 :
864 :
865 : PositionVector
866 195626 : PositionVector::getSubpartByIndex(int beginIndex, int count) const {
867 195626 : if (size() == 0) {
868 0 : return PositionVector();
869 : }
870 195626 : if (beginIndex < 0) {
871 0 : beginIndex += (int)size();
872 : }
873 : assert(count >= 0);
874 : assert(beginIndex < (int)size());
875 : assert(beginIndex + count <= (int)size());
876 195626 : PositionVector result;
877 581315 : for (int i = beginIndex; i < beginIndex + count; ++i) {
878 385689 : result.push_back((*this)[i]);
879 : }
880 : return result;
881 195626 : }
882 :
883 :
884 : double
885 841742 : PositionVector::beginEndAngle() const {
886 841742 : if (size() == 0) {
887 : return INVALID_DOUBLE;
888 : }
889 841742 : return front().angleTo2D(back());
890 : }
891 :
892 :
893 : double
894 790180124 : PositionVector::nearest_offset_to_point2D(const Position& p, bool perpendicular) const {
895 790180124 : if (size() == 0) {
896 : return INVALID_DOUBLE;
897 : }
898 : double minDist = std::numeric_limits<double>::max();
899 790180124 : double nearestPos = GeomHelper::INVALID_OFFSET;
900 : double seen = 0;
901 2219836304 : for (const_iterator i = begin(); i != end() - 1; i++) {
902 : const double pos =
903 1429656180 : GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, perpendicular);
904 1429656180 : const double dist2 = pos == GeomHelper::INVALID_OFFSET ? minDist : p.distanceSquaredTo2D(positionAtOffset2D(*i, *(i + 1), pos));
905 1429656180 : if (dist2 < minDist) {
906 806394816 : nearestPos = pos + seen;
907 : minDist = dist2;
908 : }
909 1429656180 : if (perpendicular && i != begin() && pos == GeomHelper::INVALID_OFFSET) {
910 : // even if perpendicular is set we still need to check the distance to the inner points
911 : const double cornerDist2 = p.distanceSquaredTo2D(*i);
912 171084362 : if (cornerDist2 < minDist) {
913 : const double pos1 =
914 130133785 : GeomHelper::nearest_offset_on_line_to_point2D(*(i - 1), *i, p, false);
915 : const double pos2 =
916 130133785 : GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, false);
917 130133785 : if (pos1 == (*(i - 1)).distanceTo2D(*i) && pos2 == 0.) {
918 : nearestPos = seen;
919 : minDist = cornerDist2;
920 : }
921 : }
922 : }
923 1429656180 : seen += (*i).distanceTo2D(*(i + 1));
924 : }
925 : return nearestPos;
926 : }
927 :
928 :
929 : double
930 166611330 : PositionVector::nearest_offset_to_point25D(const Position& p, bool perpendicular) const {
931 166611330 : if (size() == 0) {
932 : return INVALID_DOUBLE;
933 : }
934 : double minDist = std::numeric_limits<double>::max();
935 166611330 : double nearestPos = GeomHelper::INVALID_OFFSET;
936 : double seen = 0;
937 410655350 : for (const_iterator i = begin(); i != end() - 1; i++) {
938 : const double pos =
939 244044020 : GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, perpendicular);
940 244044020 : const double dist = pos == GeomHelper::INVALID_OFFSET ? minDist : p.distanceTo2D(positionAtOffset2D(*i, *(i + 1), pos));
941 244044020 : if (dist < minDist) {
942 34569007 : const double pos25D = pos * (*i).distanceTo(*(i + 1)) / (*i).distanceTo2D(*(i + 1));
943 34569007 : nearestPos = pos25D + seen;
944 : minDist = dist;
945 : }
946 244044020 : if (perpendicular && i != begin() && pos == GeomHelper::INVALID_OFFSET) {
947 : // even if perpendicular is set we still need to check the distance to the inner points
948 : const double cornerDist = p.distanceTo2D(*i);
949 77404238 : if (cornerDist < minDist) {
950 : const double pos1 =
951 77376141 : GeomHelper::nearest_offset_on_line_to_point2D(*(i - 1), *i, p, false);
952 : const double pos2 =
953 77376141 : GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, false);
954 77376141 : if (pos1 == (*(i - 1)).distanceTo2D(*i) && pos2 == 0.) {
955 : nearestPos = seen;
956 : minDist = cornerDist;
957 : }
958 : }
959 : }
960 244044020 : seen += (*i).distanceTo(*(i + 1));
961 : }
962 : return nearestPos;
963 : }
964 :
965 :
966 : Position
967 9690450 : PositionVector::transformToVectorCoordinates(const Position& p, bool extend) const {
968 9690450 : if (size() == 0) {
969 0 : return Position::INVALID;
970 : }
971 : // @toDo this duplicates most of the code in nearest_offset_to_point2D. It should be refactored
972 9690450 : if (extend) {
973 : PositionVector extended = *this;
974 4385053 : const double dist = 2 * distance2D(p);
975 4385053 : extended.extrapolate(dist);
976 4385053 : return extended.transformToVectorCoordinates(p) - Position(dist, 0);
977 4385053 : }
978 : double minDist = std::numeric_limits<double>::max();
979 : double nearestPos = -1;
980 : double seen = 0;
981 : int sign = 1;
982 13191302 : for (const_iterator i = begin(); i != end() - 1; i++) {
983 : const double pos =
984 7885905 : GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, true);
985 7885905 : const double dist = pos < 0 ? minDist : p.distanceTo2D(positionAtOffset(*i, *(i + 1), pos));
986 7885905 : if (dist < minDist) {
987 5121956 : nearestPos = pos + seen;
988 : minDist = dist;
989 5121956 : sign = isLeft(*i, *(i + 1), p) >= 0 ? -1 : 1;
990 : }
991 7885905 : if (i != begin() && pos == GeomHelper::INVALID_OFFSET) {
992 : // even if perpendicular is set we still need to check the distance to the inner points
993 : const double cornerDist = p.distanceTo2D(*i);
994 1594300 : if (cornerDist < minDist) {
995 : const double pos1 =
996 426097 : GeomHelper::nearest_offset_on_line_to_point2D(*(i - 1), *i, p, false);
997 : const double pos2 =
998 426097 : GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, false);
999 426097 : if (pos1 == (*(i - 1)).distanceTo2D(*i) && pos2 == 0.) {
1000 : nearestPos = seen;
1001 : minDist = cornerDist;
1002 207889 : sign = isLeft(*(i - 1), *i, p) >= 0 ? -1 : 1;
1003 : }
1004 : }
1005 : }
1006 7885905 : seen += (*i).distanceTo2D(*(i + 1));
1007 : }
1008 5305397 : if (nearestPos != -1) {
1009 5304244 : return Position(nearestPos, sign * minDist);
1010 : } else {
1011 1153 : return Position::INVALID;
1012 : }
1013 : }
1014 :
1015 :
1016 : int
1017 341150 : PositionVector::indexOfClosest(const Position& p, bool twoD) const {
1018 341150 : if (size() == 0) {
1019 : return -1;
1020 : }
1021 : double minDist = std::numeric_limits<double>::max();
1022 : double dist;
1023 : int closest = 0;
1024 2332083 : for (int i = 0; i < (int)size(); i++) {
1025 1990933 : const Position& p2 = (*this)[i];
1026 1990933 : dist = twoD ? p.distanceTo2D(p2) : p.distanceTo(p2);
1027 1990933 : if (dist < minDist) {
1028 : closest = i;
1029 : minDist = dist;
1030 : }
1031 : }
1032 : return closest;
1033 : }
1034 :
1035 :
1036 : int
1037 175612 : PositionVector::insertAtClosest(const Position& p, bool interpolateZ) {
1038 175612 : if (size() == 0) {
1039 : return -1;
1040 : }
1041 : double minDist = std::numeric_limits<double>::max();
1042 : int insertionIndex = 1;
1043 1303803 : for (int i = 0; i < (int)size() - 1; i++) {
1044 1128191 : const double length = GeomHelper::nearest_offset_on_line_to_point2D((*this)[i], (*this)[i + 1], p, false);
1045 1128191 : const Position& outIntersection = PositionVector::positionAtOffset2D((*this)[i], (*this)[i + 1], length);
1046 : const double dist = p.distanceTo2D(outIntersection);
1047 1128191 : if (dist < minDist) {
1048 : insertionIndex = i + 1;
1049 : minDist = dist;
1050 : }
1051 : }
1052 : // check if we have to adjust Position Z
1053 175612 : if (interpolateZ) {
1054 : // obtain previous and next Z
1055 0 : const double previousZ = (begin() + (insertionIndex - 1))->z();
1056 : const double nextZ = (begin() + insertionIndex)->z();
1057 : // insert new position using x and y of p, and the new z
1058 0 : insert(begin() + insertionIndex, Position(p.x(), p.y(), ((previousZ + nextZ) / 2.0)));
1059 : } else {
1060 175612 : insert(begin() + insertionIndex, p);
1061 : }
1062 : return insertionIndex;
1063 : }
1064 :
1065 :
1066 : int
1067 251 : PositionVector::removeClosest(const Position& p) {
1068 251 : if (size() == 0) {
1069 : return -1;
1070 : }
1071 : double minDist = std::numeric_limits<double>::max();
1072 : int removalIndex = 0;
1073 2477 : for (int i = 0; i < (int)size(); i++) {
1074 2226 : const double dist = p.distanceTo2D((*this)[i]);
1075 2226 : if (dist < minDist) {
1076 : removalIndex = i;
1077 : minDist = dist;
1078 : }
1079 : }
1080 251 : erase(begin() + removalIndex);
1081 251 : return removalIndex;
1082 : }
1083 :
1084 :
1085 : std::vector<double>
1086 7261086 : PositionVector::intersectsAtLengths2D(const PositionVector& other) const {
1087 : std::vector<double> ret;
1088 7261086 : if (other.size() == 0) {
1089 : return ret;
1090 : }
1091 22973325 : for (const_iterator i = other.begin(); i != other.end() - 1; i++) {
1092 15712239 : std::vector<double> atSegment = intersectsAtLengths2D(*i, *(i + 1));
1093 : copy(atSegment.begin(), atSegment.end(), back_inserter(ret));
1094 15712239 : }
1095 : return ret;
1096 0 : }
1097 :
1098 :
1099 : std::vector<double>
1100 15712239 : PositionVector::intersectsAtLengths2D(const Position& lp1, const Position& lp2) const {
1101 : std::vector<double> ret;
1102 15712239 : if (size() == 0) {
1103 : return ret;
1104 : }
1105 : double pos = 0;
1106 53725944 : for (const_iterator i = begin(); i != end() - 1; i++) {
1107 : const Position& p1 = *i;
1108 : const Position& p2 = *(i + 1);
1109 : double x, y, m;
1110 38013705 : if (intersects(p1, p2, lp1, lp2, 0., &x, &y, &m)) {
1111 4100969 : ret.push_back(Position(x, y).distanceTo2D(p1) + pos);
1112 : }
1113 38013705 : pos += p1.distanceTo2D(p2);
1114 : }
1115 : return ret;
1116 0 : }
1117 :
1118 :
1119 : void
1120 5421775 : PositionVector::extrapolate(const double val, const bool onlyFirst, const bool onlyLast) {
1121 5421775 : if (size() > 1) {
1122 5421775 : Position& p1 = (*this)[0];
1123 5421775 : Position& p2 = (*this)[1];
1124 5421775 : const Position offset = (p2 - p1) * (val / p1.distanceTo(p2));
1125 5421775 : if (!onlyLast) {
1126 : p1.sub(offset);
1127 : }
1128 5421775 : if (!onlyFirst) {
1129 5421775 : if (size() == 2) {
1130 : p2.add(offset);
1131 : } else {
1132 619442 : const Position& e1 = (*this)[-2];
1133 619442 : Position& e2 = (*this)[-1];
1134 619442 : e2.sub((e1 - e2) * (val / e1.distanceTo(e2)));
1135 : }
1136 : }
1137 : }
1138 5421775 : }
1139 :
1140 :
1141 : void
1142 182560126 : PositionVector::extrapolate2D(const double val, const bool onlyFirst) {
1143 182560126 : if (size() > 1) {
1144 182560126 : Position& p1 = (*this)[0];
1145 182560126 : Position& p2 = (*this)[1];
1146 182560126 : if (p1.distanceTo2D(p2) > 0) {
1147 182560114 : const Position offset = (p2 - p1) * (val / p1.distanceTo2D(p2));
1148 : p1.sub(offset);
1149 182560114 : if (!onlyFirst) {
1150 181482512 : if (size() == 2) {
1151 : p2.add(offset);
1152 : } else {
1153 63289562 : const Position& e1 = (*this)[-2];
1154 63289562 : Position& e2 = (*this)[-1];
1155 63289562 : e2.sub((e1 - e2) * (val / e1.distanceTo2D(e2)));
1156 : }
1157 : }
1158 : }
1159 : }
1160 182560126 : }
1161 :
1162 :
1163 : PositionVector
1164 13410940 : PositionVector::reverse() const {
1165 13410940 : PositionVector ret;
1166 48929745 : for (const_reverse_iterator i = rbegin(); i != rend(); i++) {
1167 35518805 : ret.push_back(*i);
1168 : }
1169 13410940 : return ret;
1170 0 : }
1171 :
1172 :
1173 : Position
1174 128271017 : PositionVector::sideOffset(const Position& beg, const Position& end, const double amount) {
1175 128271017 : const double scale = amount / beg.distanceTo2D(end);
1176 128271017 : return Position((beg.y() - end.y()) * scale, (end.x() - beg.x()) * scale);
1177 : }
1178 :
1179 :
1180 : void
1181 24782051 : PositionVector::move2side(double amount, double maxExtension) {
1182 24782051 : if (size() < 2) {
1183 187186 : return;
1184 : }
1185 24782051 : removeDoublePoints(POSITION_EPS, true);
1186 24782051 : if (length2D() == 0 || amount == 0) {
1187 : return;
1188 : }
1189 24594865 : PositionVector shape;
1190 : std::vector<int> recheck;
1191 77348679 : for (int i = 0; i < static_cast<int>(size()); i++) {
1192 52753814 : if (i == 0) {
1193 24594865 : const Position& from = (*this)[i];
1194 24594865 : const Position& to = (*this)[i + 1];
1195 : if (from != to) {
1196 49189730 : shape.push_back(from - sideOffset(from, to, amount));
1197 : #ifdef DEBUG_MOVE2SIDE
1198 : if (gDebugFlag1) {
1199 : std::cout << " " << i << "a=" << shape.back() << "\n";
1200 : }
1201 : #endif
1202 : }
1203 28158949 : } else if (i == static_cast<int>(size()) - 1) {
1204 24594865 : const Position& from = (*this)[i - 1];
1205 24594865 : const Position& to = (*this)[i];
1206 : if (from != to) {
1207 49189730 : shape.push_back(to - sideOffset(from, to, amount));
1208 : #ifdef DEBUG_MOVE2SIDE
1209 : if (gDebugFlag1) {
1210 : std::cout << " " << i << "b=" << shape.back() << "\n";
1211 : }
1212 : #endif
1213 : }
1214 : } else {
1215 3564084 : const Position& from = (*this)[i - 1];
1216 3564084 : const Position& me = (*this)[i];
1217 3564084 : const Position& to = (*this)[i + 1];
1218 3564084 : PositionVector fromMe(from, me);
1219 3564084 : fromMe.extrapolate2D(me.distanceTo2D(to));
1220 3564084 : const double extrapolateDev = fromMe[1].distanceTo2D(to);
1221 3564084 : if (fabs(extrapolateDev) < POSITION_EPS) {
1222 : // parallel case, just shift the middle point
1223 1877120 : shape.push_back(me - sideOffset(from, to, amount));
1224 : #ifdef DEBUG_MOVE2SIDE
1225 : if (gDebugFlag1) {
1226 : std::cout << " " << i << "c=" << shape.back() << "\n";
1227 : }
1228 : #endif
1229 2625524 : } else if (fabs(extrapolateDev - 2 * me.distanceTo2D(to)) < POSITION_EPS) {
1230 : // counterparallel case, just shift the middle point
1231 472 : PositionVector fromMe2(from, me);
1232 472 : fromMe2.extrapolate2D(amount);
1233 472 : shape.push_back(fromMe2[1]);
1234 : #ifdef DEBUG_MOVE2SIDE
1235 : if (gDebugFlag1) {
1236 : std::cout << " " << i << "d=" << shape.back() << " " << i << "_from=" << from << " " << i << "_me=" << me << " " << i << "_to=" << to << "\n";
1237 : }
1238 : #endif
1239 472 : } else {
1240 2625052 : Position offsets = sideOffset(from, me, amount);
1241 2625052 : Position offsets2 = sideOffset(me, to, amount);
1242 2625052 : PositionVector l1(from - offsets, me - offsets);
1243 2625052 : PositionVector l2(me - offsets2, to - offsets2);
1244 2625052 : Position meNew = l1.intersectionPosition2D(l2[0], l2[1], maxExtension);
1245 6 : if (meNew == Position::INVALID) {
1246 6 : recheck.push_back(i);
1247 : continue;
1248 : }
1249 2625046 : meNew = meNew + Position(0, 0, me.z());
1250 2625046 : shape.push_back(meNew);
1251 : #ifdef DEBUG_MOVE2SIDE
1252 : if (gDebugFlag1) {
1253 : std::cout << " " << i << "e=" << shape.back() << "\n";
1254 : }
1255 : #endif
1256 2625052 : }
1257 : // copy original z value
1258 : shape.back().set(shape.back().x(), shape.back().y(), me.z());
1259 3564078 : const double angle = localAngle(from, me, to);
1260 3564078 : if (fabs(angle) > NUMERICAL_EPS) {
1261 3146665 : const double length = from.distanceTo2D(me) + me.distanceTo2D(to);
1262 3146665 : const double radius = length / angle;
1263 : #ifdef DEBUG_MOVE2SIDE
1264 : if (gDebugFlag1) {
1265 : std::cout << " i=" << i << " a=" << RAD2DEG(angle) << " l=" << length << " r=" << radius << " t=" << amount * 1.8 << "\n";
1266 : }
1267 : #endif
1268 3146665 : if ((radius < 0 && -radius < amount * 1.8) || fabs(RAD2DEG(angle)) > 170) {
1269 1358 : recheck.push_back(i);
1270 : }
1271 : }
1272 3564084 : }
1273 : }
1274 24594865 : if (!recheck.empty()) {
1275 : // try to adjust positions to avoid clipping
1276 : shape = *this;
1277 2384 : for (int i = (int)recheck.size() - 1; i >= 0; i--) {
1278 1364 : shape.erase(shape.begin() + recheck[i]);
1279 : }
1280 1020 : shape.move2side(amount, maxExtension);
1281 : }
1282 : *this = shape;
1283 24594865 : }
1284 :
1285 :
1286 : void
1287 52 : PositionVector::move2sideCustom(std::vector<double> amount, double maxExtension) {
1288 52 : if (size() < 2) {
1289 0 : return;
1290 : }
1291 52 : if (length2D() == 0) {
1292 : return;
1293 : }
1294 52 : if (size() != amount.size()) {
1295 0 : throw InvalidArgument("Numer of offsets (" + toString(amount.size())
1296 0 : + ") does not match number of points (" + toString(size()) + ")");
1297 : }
1298 52 : PositionVector shape;
1299 2543 : for (int i = 0; i < static_cast<int>(size()); i++) {
1300 2491 : if (i == 0) {
1301 52 : const Position& from = (*this)[i];
1302 52 : const Position& to = (*this)[i + 1];
1303 : if (from != to) {
1304 104 : shape.push_back(from - sideOffset(from, to, amount[i]));
1305 : }
1306 2439 : } else if (i == static_cast<int>(size()) - 1) {
1307 52 : const Position& from = (*this)[i - 1];
1308 52 : const Position& to = (*this)[i];
1309 : if (from != to) {
1310 104 : shape.push_back(to - sideOffset(from, to, amount[i]));
1311 : }
1312 : } else {
1313 2387 : const Position& from = (*this)[i - 1];
1314 2387 : const Position& me = (*this)[i];
1315 2387 : const Position& to = (*this)[i + 1];
1316 2387 : PositionVector fromMe(from, me);
1317 2387 : fromMe.extrapolate2D(me.distanceTo2D(to));
1318 2387 : const double extrapolateDev = fromMe[1].distanceTo2D(to);
1319 2387 : if (fabs(extrapolateDev) < POSITION_EPS) {
1320 : // parallel case, just shift the middle point
1321 4448 : shape.push_back(me - sideOffset(from, to, amount[i]));
1322 163 : } else if (fabs(extrapolateDev - 2 * me.distanceTo2D(to)) < POSITION_EPS) {
1323 : // counterparallel case, just shift the middle point
1324 0 : PositionVector fromMe2(from, me);
1325 0 : fromMe2.extrapolate2D(amount[i]);
1326 0 : shape.push_back(fromMe2[1]);
1327 0 : } else {
1328 163 : Position offsets = sideOffset(from, me, amount[i]);
1329 163 : Position offsets2 = sideOffset(me, to, amount[i]);
1330 163 : PositionVector l1(from - offsets, me - offsets);
1331 163 : PositionVector l2(me - offsets2, to - offsets2);
1332 163 : Position meNew = l1.intersectionPosition2D(l2[0], l2[1], maxExtension);
1333 0 : if (meNew == Position::INVALID) {
1334 : continue;
1335 : }
1336 163 : meNew = meNew + Position(0, 0, me.z());
1337 163 : shape.push_back(meNew);
1338 163 : }
1339 : // copy original z value
1340 : shape.back().set(shape.back().x(), shape.back().y(), me.z());
1341 2387 : }
1342 : }
1343 : *this = shape;
1344 52 : }
1345 :
1346 : double
1347 3564078 : PositionVector::localAngle(const Position& from, const Position& pos, const Position& to) {
1348 3564078 : return GeomHelper::angleDiff(from.angleTo2D(pos), pos.angleTo2D(to));
1349 : }
1350 :
1351 : double
1352 32482194 : PositionVector::angleAt2D(int pos) const {
1353 32482194 : if ((pos + 1) < (int)size()) {
1354 32482194 : return (*this)[pos].angleTo2D((*this)[pos + 1]);
1355 : }
1356 : return INVALID_DOUBLE;
1357 : }
1358 :
1359 :
1360 : void
1361 0 : PositionVector::openPolygon() {
1362 0 : if ((size() > 1) && (front() == back())) {
1363 : pop_back();
1364 : }
1365 0 : }
1366 :
1367 :
1368 : void
1369 812629 : PositionVector::closePolygon() {
1370 812629 : if ((size() != 0) && ((*this)[0] != back())) {
1371 451097 : push_back((*this)[0]);
1372 : }
1373 812629 : }
1374 :
1375 :
1376 : std::vector<double>
1377 2589613 : PositionVector::distances(const PositionVector& s, bool perpendicular) const {
1378 : std::vector<double> ret;
1379 : const_iterator i;
1380 12247939 : for (i = begin(); i != end(); i++) {
1381 9658326 : const double dist = s.distance2D(*i, perpendicular);
1382 9658326 : if (dist != GeomHelper::INVALID_OFFSET) {
1383 9622921 : ret.push_back(dist);
1384 : }
1385 : }
1386 12210926 : for (i = s.begin(); i != s.end(); i++) {
1387 9621313 : const double dist = distance2D(*i, perpendicular);
1388 9621313 : if (dist != GeomHelper::INVALID_OFFSET) {
1389 9616290 : ret.push_back(dist);
1390 : }
1391 : }
1392 2589613 : return ret;
1393 0 : }
1394 :
1395 :
1396 : double
1397 436954393 : PositionVector::distance2D(const Position& p, bool perpendicular) const {
1398 436954393 : if (size() == 0) {
1399 : return std::numeric_limits<double>::max();
1400 436954213 : } else if (size() == 1) {
1401 497805 : return front().distanceTo2D(p);
1402 : }
1403 436456408 : const double nearestOffset = nearest_offset_to_point2D(p, perpendicular);
1404 436456408 : if (nearestOffset == GeomHelper::INVALID_OFFSET) {
1405 : return GeomHelper::INVALID_OFFSET;
1406 : } else {
1407 436312554 : return p.distanceTo2D(positionAtOffset2D(nearestOffset));
1408 : }
1409 : }
1410 :
1411 :
1412 : void
1413 146057 : PositionVector::push_front(const Position& p) {
1414 146057 : if (empty()) {
1415 0 : push_back(p);
1416 : } else {
1417 146057 : insert(begin(), p);
1418 : }
1419 146057 : }
1420 :
1421 :
1422 : void
1423 0 : PositionVector::pop_front() {
1424 0 : if (empty()) {
1425 0 : throw ProcessError("PositionVector is empty");
1426 : } else {
1427 0 : erase(begin());
1428 : }
1429 0 : }
1430 :
1431 :
1432 : void
1433 5856400 : PositionVector::push_back_noDoublePos(const Position& p) {
1434 11602562 : if (size() == 0 || !p.almostSame(back())) {
1435 5442521 : push_back(p);
1436 : }
1437 5856400 : }
1438 :
1439 :
1440 : void
1441 159865 : PositionVector::push_front_noDoublePos(const Position& p) {
1442 319730 : if ((size() == 0) || !p.almostSame(front())) {
1443 146056 : push_front(p);
1444 : }
1445 159865 : }
1446 :
1447 :
1448 : void
1449 0 : PositionVector::insert_noDoublePos(const std::vector<Position>::iterator& at, const Position& p) {
1450 0 : if (at == begin()) {
1451 0 : push_front_noDoublePos(p);
1452 0 : } else if (at == end()) {
1453 0 : push_back_noDoublePos(p);
1454 : } else {
1455 0 : if (!p.almostSame(*at) && !p.almostSame(*(at - 1))) {
1456 0 : insert(at, p);
1457 : }
1458 : }
1459 0 : }
1460 :
1461 :
1462 : bool
1463 831878 : PositionVector::isClosed() const {
1464 831878 : return (size() >= 2) && ((*this)[0] == back());
1465 : }
1466 :
1467 :
1468 : bool
1469 1163360 : PositionVector::isNAN() const {
1470 : // iterate over all positions and check if is NAN
1471 4722276 : for (auto i = begin(); i != end(); i++) {
1472 : if (i->isNAN()) {
1473 : return true;
1474 : }
1475 : }
1476 : // all ok, then return false
1477 : return false;
1478 : }
1479 :
1480 : void
1481 282526 : PositionVector::ensureMinLength(int precision) {
1482 282526 : const double limit = 2 * pow(10, -precision);
1483 282526 : if (length2D() < limit) {
1484 8 : extrapolate2D(limit);
1485 : }
1486 282526 : }
1487 :
1488 : void
1489 414839 : PositionVector::round(int precision, bool avoidDegeneration) {
1490 414839 : if (avoidDegeneration && size() > 1) {
1491 128222 : ensureMinLength(precision);
1492 : }
1493 920240 : for (int i = 0; i < (int)size(); i++) {
1494 505401 : (*this)[i].round(precision);
1495 : }
1496 414839 : }
1497 :
1498 :
1499 : void
1500 24940680 : PositionVector::removeDoublePoints(double minDist, bool assertLength, int beginOffset, int endOffset, bool resample) {
1501 24940680 : int curSize = (int)size() - beginOffset - endOffset;
1502 24940680 : if (curSize > 1) {
1503 : iterator last = begin() + beginOffset;
1504 30388890 : for (iterator i = last + 1; i != (end() - endOffset) && (!assertLength || curSize > 2);) {
1505 5497184 : if (last->almostSame(*i, minDist)) {
1506 3665 : if (i + 1 == end() - endOffset) {
1507 : // special case: keep the last point and remove the next-to-last
1508 1205 : if (resample && last > begin() && (last - 1)->distanceTo(*i) >= 2 * minDist) {
1509 : // resample rather than remove point after a long segment
1510 3 : const double shiftBack = minDist - last->distanceTo(*i);
1511 : //if (gDebugFlag1) std::cout << " resample endOffset beforeLast=" << *(last - 1) << " last=" << *last << " i=" << *i;
1512 3 : (*last) = positionAtOffset(*(last - 1), *last, (last - 1)->distanceTo(*last) - shiftBack);
1513 : //if (gDebugFlag1) std::cout << " lastNew=" << *last;
1514 : last = i;
1515 : ++i;
1516 : } else {
1517 1202 : erase(last);
1518 : i = end() - endOffset;
1519 : }
1520 : } else {
1521 2460 : if (resample && i + 1 != end() && last->distanceTo(*(i + 1)) >= 2 * minDist) {
1522 : // resample rather than remove points before a long segment
1523 2 : const double shiftForward = minDist - last->distanceTo(*i);
1524 : //if (gDebugFlag1) std::cout << " resample last=" << *last << " i=" << *i << " next=" << *(i + 1);
1525 2 : (*i) = positionAtOffset(*i, *(i + 1), shiftForward);
1526 : //if (gDebugFlag1) std::cout << " iNew=" << *i << "\n";
1527 : last = i;
1528 : ++i;
1529 : } else {
1530 2458 : i = erase(i);
1531 : }
1532 : }
1533 3665 : curSize--;
1534 : } else {
1535 : last = i;
1536 : ++i;
1537 : }
1538 : }
1539 : }
1540 24940680 : }
1541 :
1542 :
1543 : bool
1544 623826 : PositionVector::operator==(const PositionVector& v2) const {
1545 623826 : return static_cast<vp>(*this) == static_cast<vp>(v2);
1546 : }
1547 :
1548 :
1549 : bool
1550 309520 : PositionVector::operator!=(const PositionVector& v2) const {
1551 309520 : return static_cast<vp>(*this) != static_cast<vp>(v2);
1552 : }
1553 :
1554 : PositionVector
1555 0 : PositionVector::operator-(const PositionVector& v2) const {
1556 0 : if (length() != v2.length()) {
1557 0 : WRITE_ERROR(TL("Trying to subtract PositionVectors of different lengths."));
1558 : }
1559 0 : PositionVector pv;
1560 : auto i1 = begin();
1561 : auto i2 = v2.begin();
1562 0 : while (i1 != end()) {
1563 0 : pv.add(*i1 - *i2);
1564 : }
1565 0 : return pv;
1566 0 : }
1567 :
1568 : PositionVector
1569 0 : PositionVector::operator+(const PositionVector& v2) const {
1570 0 : if (length() != v2.length()) {
1571 0 : WRITE_ERROR(TL("Trying to add PositionVectors of different lengths."));
1572 : }
1573 0 : PositionVector pv;
1574 : auto i1 = begin();
1575 : auto i2 = v2.begin();
1576 0 : while (i1 != end()) {
1577 0 : pv.add(*i1 + *i2);
1578 : }
1579 0 : return pv;
1580 0 : }
1581 :
1582 : bool
1583 10112 : PositionVector::almostSame(const PositionVector& v2, double maxDiv) const {
1584 10112 : if (size() != v2.size()) {
1585 : return false;
1586 : }
1587 : auto i2 = v2.begin();
1588 17763 : for (auto i1 = begin(); i1 != end(); i1++) {
1589 14081 : if (!i1->almostSame(*i2, maxDiv)) {
1590 : return false;
1591 : }
1592 : i2++;
1593 : }
1594 : return true;
1595 : }
1596 :
1597 : bool
1598 2214081 : PositionVector::hasElevation() const {
1599 2214081 : if (size() < 2) {
1600 : return false;
1601 : }
1602 9628283 : for (const_iterator i = begin(); i != end(); i++) {
1603 7416447 : if ((*i).z() != 0) {
1604 : return true;
1605 : }
1606 : }
1607 : return false;
1608 : }
1609 :
1610 :
1611 : bool
1612 113954809 : PositionVector::intersects(const Position& p11, const Position& p12, const Position& p21, const Position& p22, const double withinDist, double* x, double* y, double* mu) {
1613 : const double eps = std::numeric_limits<double>::epsilon();
1614 113954809 : const double denominator = (p22.y() - p21.y()) * (p12.x() - p11.x()) - (p22.x() - p21.x()) * (p12.y() - p11.y());
1615 113954809 : const double numera = (p22.x() - p21.x()) * (p11.y() - p21.y()) - (p22.y() - p21.y()) * (p11.x() - p21.x());
1616 113954809 : const double numerb = (p12.x() - p11.x()) * (p11.y() - p21.y()) - (p12.y() - p11.y()) * (p11.x() - p21.x());
1617 : /* Are the lines coincident? */
1618 113954809 : if (fabs(numera) < eps && fabs(numerb) < eps && fabs(denominator) < eps) {
1619 : double a1;
1620 : double a2;
1621 : double a3;
1622 : double a4;
1623 : double a = -1e12;
1624 54450 : if (p11.x() != p12.x()) {
1625 50710 : a1 = p11.x() < p12.x() ? p11.x() : p12.x();
1626 50710 : a2 = p11.x() < p12.x() ? p12.x() : p11.x();
1627 50710 : a3 = p21.x() < p22.x() ? p21.x() : p22.x();
1628 50710 : a4 = p21.x() < p22.x() ? p22.x() : p21.x();
1629 : } else {
1630 3740 : a1 = p11.y() < p12.y() ? p11.y() : p12.y();
1631 3740 : a2 = p11.y() < p12.y() ? p12.y() : p11.y();
1632 3740 : a3 = p21.y() < p22.y() ? p21.y() : p22.y();
1633 3740 : a4 = p21.y() < p22.y() ? p22.y() : p21.y();
1634 : }
1635 54450 : if (a1 <= a3 && a3 <= a2) {
1636 9090 : if (a4 < a2) {
1637 296 : a = (a3 + a4) / 2;
1638 : } else {
1639 8794 : a = (a2 + a3) / 2;
1640 : }
1641 : }
1642 54450 : if (a3 <= a1 && a1 <= a4) {
1643 9052 : if (a2 < a4) {
1644 252 : a = (a1 + a2) / 2;
1645 : } else {
1646 8800 : a = (a1 + a4) / 2;
1647 : }
1648 : }
1649 54450 : if (a != -1e12) {
1650 12597 : if (x != nullptr) {
1651 8951 : if (p11.x() != p12.x()) {
1652 7312 : *mu = (a - p11.x()) / (p12.x() - p11.x());
1653 7312 : *x = a;
1654 7312 : *y = p11.y() + (*mu) * (p12.y() - p11.y());
1655 : } else {
1656 1639 : *x = p11.x();
1657 1639 : *y = a;
1658 1639 : if (p12.y() == p11.y()) {
1659 32 : *mu = 0;
1660 : } else {
1661 1607 : *mu = (a - p11.y()) / (p12.y() - p11.y());
1662 : }
1663 : }
1664 : }
1665 12597 : return true;
1666 : }
1667 : return false;
1668 : }
1669 : /* Are the lines parallel */
1670 113900359 : if (fabs(denominator) < eps) {
1671 : return false;
1672 : }
1673 : /* Is the intersection along the segments */
1674 109931297 : double mua = numera / denominator;
1675 : /* reduce rounding errors for lines ending in the same point */
1676 109931297 : if (fabs(p12.x() - p22.x()) < eps && fabs(p12.y() - p22.y()) < eps) {
1677 : mua = 1.;
1678 : } else {
1679 109928378 : const double offseta = withinDist / p11.distanceTo2D(p12);
1680 109928378 : const double offsetb = withinDist / p21.distanceTo2D(p22);
1681 109928378 : const double mub = numerb / denominator;
1682 109928378 : if (mua < -offseta || mua > 1 + offseta || mub < -offsetb || mub > 1 + offsetb) {
1683 : return false;
1684 : }
1685 : }
1686 8277569 : if (x != nullptr) {
1687 6935743 : *x = p11.x() + mua * (p12.x() - p11.x());
1688 6935743 : *y = p11.y() + mua * (p12.y() - p11.y());
1689 6935743 : *mu = mua;
1690 : }
1691 : return true;
1692 : }
1693 :
1694 :
1695 : void
1696 6336 : PositionVector::rotate2D(double angle) {
1697 6336 : const double s = sin(angle);
1698 6336 : const double c = cos(angle);
1699 79622 : for (int i = 0; i < (int)size(); i++) {
1700 73286 : const double x = (*this)[i].x();
1701 73286 : const double y = (*this)[i].y();
1702 73286 : const double z = (*this)[i].z();
1703 73286 : const double xnew = x * c - y * s;
1704 73286 : const double ynew = x * s + y * c;
1705 73286 : (*this)[i].set(xnew, ynew, z);
1706 : }
1707 6336 : }
1708 :
1709 :
1710 : void
1711 0 : PositionVector::rotate2D(const Position& pos, double angle) {
1712 : PositionVector aux = *this;
1713 0 : aux.sub(pos);
1714 0 : aux.rotate2D(angle);
1715 0 : aux.add(pos);
1716 : *this = aux;
1717 0 : }
1718 :
1719 :
1720 : void
1721 0 : PositionVector::rotateAroundFirstElement2D(double angle) {
1722 0 : if (size() > 1) {
1723 : // translate position vector to (0,0), rotate, and traslate back again
1724 0 : const Position offset = front();
1725 0 : sub(offset);
1726 0 : rotate2D(angle);
1727 0 : add(offset);
1728 : }
1729 0 : }
1730 :
1731 :
1732 : PositionVector
1733 66189 : PositionVector::simplified() const {
1734 : PositionVector result = *this;
1735 : bool changed = true;
1736 190539 : while (changed && result.size() > 3) {
1737 : changed = false;
1738 541561 : for (int i = 0; i < (int)result.size(); i++) {
1739 497122 : const Position& p1 = result[i];
1740 497122 : const Position& p2 = result[(i + 2) % result.size()];
1741 497122 : const int middleIndex = (i + 1) % result.size();
1742 497122 : const Position& p0 = result[middleIndex];
1743 : // https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line#Line_defined_by_two_points
1744 497122 : const double triangleArea2 = fabs((p2.y() - p1.y()) * p0.x() - (p2.x() - p1.x()) * p0.y() + p2.x() * p1.y() - p2.y() * p1.x());
1745 : const double distIK = p1.distanceTo2D(p2);
1746 497122 : if (distIK > NUMERICAL_EPS && triangleArea2 / distIK < NUMERICAL_EPS) {
1747 : changed = true;
1748 : result.erase(result.begin() + middleIndex);
1749 13722 : break;
1750 : }
1751 : }
1752 : }
1753 66189 : return result;
1754 0 : }
1755 :
1756 :
1757 : const PositionVector
1758 1216 : PositionVector::simplified2(const bool closed, const double eps) const {
1759 : // this is a variation of the https://en.wikipedia.org/wiki/Visvalingam%E2%80%93Whyatt_algorithm
1760 : // which uses the distance instead of the area
1761 : // the benefits over the initial implementation are:
1762 : // 3D support, no degenerate results for a sequence of small distances, keeping the longest part of a line
1763 : // drawbacks: complexity of the code, speed
1764 1216 : if (size() < 3) {
1765 : return *this;
1766 : }
1767 15059 : auto calcScore = [&](const PositionVector & pv, int index) {
1768 15059 : if (!closed && (index == 0 || index == (int)pv.size() - 1)) {
1769 2025 : return eps + 1.;
1770 : }
1771 13034 : const Position& p = pv[index];
1772 13034 : const Position& a = pv[(index + (int)pv.size() - 1) % pv.size()];
1773 13034 : const Position& b = pv[(index + 1) % pv.size()];
1774 13034 : const double distAB = a.distanceTo(b);
1775 26068 : if (distAB < MIN2(eps, NUMERICAL_EPS)) {
1776 : // avoid division by 0 and degenerate cases due to very small baseline
1777 9 : return (a.distanceTo(p) + b.distanceTo(p)) / 2.;
1778 : }
1779 : // https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line#Vector_formulation
1780 : // calculating the distance of p to the line defined by a and b
1781 : const Position dir = (b - a) / distAB;
1782 : const double projectedLength = (a - p).dotProduct(dir);
1783 13025 : if (projectedLength <= -distAB) {
1784 5 : return b.distanceTo(p);
1785 : }
1786 13020 : if (projectedLength >= 0.) {
1787 4 : return a.distanceTo(p);
1788 : }
1789 : const Position distVector = (a - p) - dir * projectedLength;
1790 13016 : return distVector.length();
1791 829 : };
1792 : std::vector<double> scores;
1793 829 : double minScore = eps + 1.;
1794 : int minIndex = -1;
1795 11352 : for (int i = 0; i < (int)size(); i++) {
1796 10523 : scores.push_back(calcScore(*this, i));
1797 10523 : if (scores.back() < minScore) {
1798 : minScore = scores.back();
1799 : minIndex = i;
1800 : }
1801 : }
1802 829 : if (minScore >= eps) {
1803 : return *this;
1804 : }
1805 : PositionVector result(*this);
1806 2384 : while (minScore < eps) {
1807 : result.erase(result.begin() + minIndex);
1808 2298 : if (result.size() < 3) {
1809 : break;
1810 : }
1811 : scores.erase(scores.begin() + minIndex);
1812 2268 : const int prevIndex = (minIndex + (int)result.size() - 1) % result.size();
1813 2268 : scores[prevIndex] = calcScore(result, prevIndex);
1814 2268 : scores[minIndex % result.size()] = calcScore(result, minIndex % result.size());
1815 2268 : minScore = eps + 1.;
1816 142053 : for (int i = 0; i < (int)result.size(); i++) {
1817 139785 : if (scores[i] < minScore) {
1818 : minScore = scores[i];
1819 : minIndex = i;
1820 : }
1821 : }
1822 : }
1823 : return result;
1824 829 : }
1825 :
1826 :
1827 : PositionVector
1828 1930 : PositionVector::getOrthogonal(const Position& p, double extend, bool before, double length, double deg) const {
1829 1930 : PositionVector result;
1830 : PositionVector tmp = *this;
1831 1930 : tmp.extrapolate2D(extend);
1832 1930 : const double baseOffset = tmp.nearest_offset_to_point2D(p);
1833 1930 : if (baseOffset == GeomHelper::INVALID_OFFSET || size() < 2) {
1834 : // fail
1835 : return result;
1836 : }
1837 1930 : Position base = tmp.positionAtOffset2D(baseOffset);
1838 1930 : const int closestIndex = tmp.indexOfClosest(base);
1839 1930 : const double closestOffset = tmp.offsetAtIndex2D(closestIndex);
1840 1930 : result.push_back(base);
1841 1930 : if (fabs(baseOffset - closestOffset) > NUMERICAL_EPS) {
1842 1929 : result.push_back(tmp[closestIndex]);
1843 1929 : if ((closestOffset < baseOffset) != before) {
1844 1427 : deg *= -1;
1845 : }
1846 1 : } else if (before) {
1847 : // take the segment before closestIndex if possible
1848 1 : if (closestIndex > 0) {
1849 1 : result.push_back(tmp[closestIndex - 1]);
1850 : } else {
1851 0 : result.push_back(tmp[1]);
1852 0 : deg *= -1;
1853 : }
1854 : } else {
1855 : // take the segment after closestIndex if possible
1856 0 : if (closestIndex < (int)size() - 1) {
1857 0 : result.push_back(tmp[closestIndex + 1]);
1858 : } else {
1859 0 : result.push_back(tmp[-1]);
1860 0 : deg *= -1;
1861 : }
1862 : }
1863 3860 : result = result.getSubpart2D(0, length);
1864 : // rotate around base
1865 1930 : result.add(base * -1);
1866 1930 : result.rotate2D(DEG2RAD(deg));
1867 1930 : result.add(base);
1868 : return result;
1869 1930 : }
1870 :
1871 :
1872 : PositionVector
1873 193980 : PositionVector::smoothedZFront(double dist) const {
1874 : PositionVector result = *this;
1875 193980 : if (size() == 0) {
1876 : return result;
1877 : }
1878 193980 : const double z0 = (*this)[0].z();
1879 : // the z-delta of the first segment
1880 193980 : const double dz = (*this)[1].z() - z0;
1881 : // if the shape only has 2 points it is as smooth as possible already
1882 193980 : if (size() > 2 && dz != 0) {
1883 1272 : dist = MIN2(dist, length2D());
1884 : // check wether we need to insert a new point at dist
1885 1272 : Position pDist = positionAtOffset2D(dist);
1886 1272 : int iLast = indexOfClosest(pDist);
1887 : // prevent close spacing to reduce impact of rounding errors in z-axis
1888 1272 : if (pDist.distanceTo2D((*this)[iLast]) > POSITION_EPS * 20) {
1889 13 : iLast = result.insertAtClosest(pDist, false);
1890 : }
1891 1272 : double dist2 = result.offsetAtIndex2D(iLast);
1892 1272 : const double dz2 = result[iLast].z() - z0;
1893 : double seen = 0;
1894 6849 : for (int i = 1; i < iLast; ++i) {
1895 5577 : seen += result[i].distanceTo2D(result[i - 1]);
1896 5577 : result[i].set(result[i].x(), result[i].y(), z0 + dz2 * seen / dist2);
1897 : }
1898 : }
1899 : return result;
1900 :
1901 0 : }
1902 :
1903 :
1904 : PositionVector
1905 238332 : PositionVector::interpolateZ(double zStart, double zEnd) const {
1906 : PositionVector result = *this;
1907 238332 : if (size() == 0) {
1908 : return result;
1909 : }
1910 238332 : result[0].setz(zStart);
1911 238332 : result[-1].setz(zEnd);
1912 238332 : const double dz = zEnd - zStart;
1913 238332 : const double length = length2D();
1914 : double seen = 0;
1915 379136 : for (int i = 1; i < (int)size() - 1; ++i) {
1916 140804 : seen += result[i].distanceTo2D(result[i - 1]);
1917 140804 : result[i].setz(zStart + dz * seen / length);
1918 : }
1919 : return result;
1920 0 : }
1921 :
1922 :
1923 : PositionVector
1924 0 : PositionVector::resample(double maxLength, const bool adjustEnd) const {
1925 0 : PositionVector result;
1926 0 : if (maxLength == 0) {
1927 : return result;
1928 : }
1929 0 : const double length = length2D();
1930 0 : if (length < POSITION_EPS) {
1931 : return result;
1932 : }
1933 0 : maxLength = length / ceil(length / maxLength);
1934 0 : for (double pos = 0; pos <= length; pos += maxLength) {
1935 0 : result.push_back(positionAtOffset2D(pos));
1936 : }
1937 : // check if we have to adjust last element
1938 0 : if (adjustEnd && !result.empty() && (result.back() != back())) {
1939 : // add last element
1940 0 : result.push_back(back());
1941 : }
1942 : return result;
1943 0 : }
1944 :
1945 :
1946 : double
1947 3318 : PositionVector::offsetAtIndex2D(int index) const {
1948 3318 : if (index < 0 || index >= (int)size()) {
1949 0 : return GeomHelper::INVALID_OFFSET;
1950 : }
1951 : double seen = 0;
1952 13170 : for (int i = 1; i <= index; ++i) {
1953 9852 : seen += (*this)[i].distanceTo2D((*this)[i - 1]);
1954 : }
1955 : return seen;
1956 : }
1957 :
1958 :
1959 : double
1960 5476 : PositionVector::getMaxGrade(double& maxJump) const {
1961 : double result = 0;
1962 11615 : for (int i = 1; i < (int)size(); ++i) {
1963 6139 : const Position& p1 = (*this)[i - 1];
1964 6139 : const Position& p2 = (*this)[i];
1965 6139 : const double distZ = fabs(p1.z() - p2.z());
1966 : const double dist2D = p1.distanceTo2D(p2);
1967 6139 : if (dist2D == 0) {
1968 0 : maxJump = MAX2(maxJump, distZ);
1969 : } else {
1970 6139 : result = MAX2(result, distZ / dist2D);
1971 : }
1972 : }
1973 5476 : return result;
1974 : }
1975 :
1976 :
1977 : double
1978 288 : PositionVector::getMinZ() const {
1979 : double minZ = std::numeric_limits<double>::max();
1980 864 : for (const Position& i : *this) {
1981 : minZ = MIN2(minZ, i.z());
1982 : }
1983 288 : return minZ;
1984 : }
1985 :
1986 :
1987 : PositionVector
1988 195259 : PositionVector::bezier(int numPoints) {
1989 : // inspired by David F. Rogers
1990 : assert(size() < 33);
1991 : static const double fac[33] = {
1992 : 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,
1993 : 6227020800.0, 87178291200.0, 1307674368000.0, 20922789888000.0, 355687428096000.0, 6402373705728000.0,
1994 : 121645100408832000.0, 2432902008176640000.0, 51090942171709440000.0, 1124000727777607680000.0,
1995 : 25852016738884976640000.0, 620448401733239439360000.0, 15511210043330985984000000.0,
1996 : 403291461126605635584000000.0, 10888869450418352160768000000.0, 304888344611713860501504000000.0,
1997 : 8841761993739701954543616000000.0, 265252859812191058636308480000000.0,
1998 : 8222838654177922817725562880000000.0, 263130836933693530167218012160000000.0
1999 : };
2000 195259 : PositionVector ret;
2001 195259 : const int npts = (int)size();
2002 : // calculate the points on the Bezier curve
2003 195259 : const double step = (double) 1.0 / (numPoints - 1);
2004 : double t = 0.;
2005 : Position prev;
2006 1345968 : for (int i1 = 0; i1 < numPoints; i1++) {
2007 1150709 : if ((1.0 - t) < 5e-6) {
2008 : t = 1.0;
2009 : }
2010 : double x = 0., y = 0., z = 0.;
2011 4841228 : for (int i = 0; i < npts; i++) {
2012 3690519 : const double ti = (i == 0) ? 1.0 : pow(t, i);
2013 3690519 : const double tni = (npts == i + 1) ? 1.0 : pow(1 - t, npts - i - 1);
2014 3690519 : const double basis = fac[npts - 1] / (fac[i] * fac[npts - 1 - i]) * ti * tni;
2015 3690519 : x += basis * at(i).x();
2016 3690519 : y += basis * at(i).y();
2017 3690519 : z += basis * at(i).z();
2018 : }
2019 1150709 : t += step;
2020 : Position current(x, y, z);
2021 1150709 : if (prev != current && !std::isnan(x) && !std::isnan(y) && !std::isnan(z)) {
2022 1150709 : ret.push_back(current);
2023 : }
2024 : prev = current;
2025 : }
2026 195259 : return ret;
2027 0 : }
2028 :
2029 :
2030 3 : bool PositionVector::isClockwiseOriented() {
2031 : // The test is based on the computation of a signed area enclosed by the polygon.
2032 : // If the polygon is in the upper (resp. the lower) half-plane and the area is
2033 : // negatively (resp. positively) signed, then the polygon is CW oriented. In case
2034 : // the polygon has points with both positive and negative y-coordinates, we translate
2035 : // the polygon to apply the above simple area-based test.
2036 : double area = 0.0;
2037 : const double y_min = std::min_element(begin(), end(), [](Position p1, Position p2) {
2038 : return p1.y() < p2.y();
2039 : })->y();
2040 3 : const double gap = y_min > 0.0 ? 0.0 : y_min;
2041 3 : add(0., gap, 0.);
2042 3 : const int last = (int)size() - 1;
2043 10 : for (int i = 0; i < last; i++) {
2044 7 : const Position& firstPoint = at(i);
2045 7 : const Position& secondPoint = at(i + 1);
2046 7 : area += (secondPoint.x() - firstPoint.x()) / (secondPoint.y() + firstPoint.y()) / 2.0;
2047 : }
2048 3 : area += (at(0).x() - at(last).x()) / (at(0).y() + at(last).y()) / 2.0;
2049 3 : add(0., -gap, 0.);
2050 3 : return area < 0.0;
2051 : }
2052 :
2053 :
2054 : /****************************************************************************/
|
