Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2001-2024 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 53052655 : PositionVector::PositionVector() {}
54 :
55 :
56 558 : PositionVector::PositionVector(const std::vector<Position>& v) {
57 : std::copy(v.begin(), v.end(), std::back_inserter(*this));
58 558 : }
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 7354530 : PositionVector::PositionVector(const Position& p1, const Position& p2) {
67 7354530 : push_back(p1);
68 7354530 : push_back(p2);
69 7354530 : }
70 :
71 :
72 129135627 : PositionVector::~PositionVector() {}
73 :
74 :
75 : bool
76 38632176 : PositionVector::around(const Position& p, double offset) const {
77 38632176 : if (size() < 2) {
78 : return false;
79 : }
80 38632172 : 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 161231327 : for (const_iterator i = begin(); i != (end() - 1); i++) {
88 : Position p1(
89 : i->x() - p.x(),
90 122599951 : i->y() - p.y());
91 : Position p2(
92 : (i + 1)->x() - p.x(),
93 122599951 : (i + 1)->y() - p.y());
94 122599951 : angle += GeomHelper::angle2D(p1, p2);
95 : }
96 : // add angle between last and first point
97 : Position p1(
98 : (end() - 1)->x() - p.x(),
99 38631376 : (end() - 1)->y() - p.y());
100 : Position p2(
101 : begin()->x() - p.x(),
102 38631376 : begin()->y() - p.y());
103 38631376 : angle += GeomHelper::angle2D(p1, p2);
104 : // if angle is less than PI, then point lying in Polygon
105 38631376 : return (!(fabs(angle) < M_PI));
106 : }
107 :
108 :
109 : bool
110 4742796 : PositionVector::overlapsWith(const AbstractPoly& poly, double offset) const {
111 : if (
112 : // check whether one of my points lies within the given poly
113 9463659 : partialWithin(poly, offset) ||
114 : // check whether the polygon lies within me
115 4720863 : poly.partialWithin(*this, offset)) {
116 37964 : return true;
117 : }
118 4704832 : if (size() >= 2) {
119 18840489 : for (const_iterator i = begin(); i != end() - 1; i++) {
120 14138697 : if (poly.crosses(*i, *(i + 1))) {
121 : return true;
122 : }
123 : }
124 4701792 : 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 24974688 : PositionVector::intersects(const Position& p1, const Position& p2) const {
162 24974688 : if (size() < 2) {
163 : return false;
164 : }
165 87600902 : for (const_iterator i = begin(); i != end() - 1; i++) {
166 63665989 : if (intersects(*i, *(i + 1), p1, p2)) {
167 : return true;
168 : }
169 : }
170 : return false;
171 : }
172 :
173 :
174 : bool
175 1135535 : PositionVector::intersects(const PositionVector& v1) const {
176 1135535 : if (size() < 2) {
177 : return false;
178 : }
179 1185296 : for (const_iterator i = begin(); i != end() - 1; i++) {
180 1077111 : if (v1.intersects(*i, *(i + 1))) {
181 : return true;
182 : }
183 : }
184 : return false;
185 : }
186 :
187 :
188 : Position
189 1982601 : PositionVector::intersectionPosition2D(const Position& p1, const Position& p2, const double withinDist) const {
190 1984635 : for (const_iterator i = begin(); i != end() - 1; i++) {
191 : double x, y, m;
192 1984629 : if (intersects(*i, *(i + 1), p1, p2, withinDist, &x, &y, &m)) {
193 1982595 : return Position(x, y);
194 : }
195 : }
196 6 : return Position::INVALID;
197 : }
198 :
199 :
200 : Position
201 193995 : PositionVector::intersectionPosition2D(const PositionVector& v1) const {
202 199703 : for (const_iterator i = begin(); i != end() - 1; i++) {
203 197048 : if (v1.intersects(*i, *(i + 1))) {
204 191340 : return v1.intersectionPosition2D(*i, *(i + 1));
205 : }
206 : }
207 2655 : return Position::INVALID;
208 : }
209 :
210 :
211 : const Position&
212 161614685 : 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 161614685 : if (index >= 0 && index < (int)size()) {
221 134516197 : return at(index);
222 27098488 : } else if (index < 0 && -index <= (int)size()) {
223 27098488 : 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 176378619 : 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 176378619 : if (index >= 0 && index < (int)size()) {
240 173958998 : return at(index);
241 2419621 : } else if (index < 0 && -index <= (int)size()) {
242 2419621 : 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 1433608691 : PositionVector::positionAtOffset(double pos, double lateralOffset) const {
251 1433608691 : if (size() == 0) {
252 0 : return Position::INVALID;
253 : }
254 1433608691 : if (size() == 1) {
255 0 : return front();
256 : }
257 : const_iterator i = begin();
258 : double seenLength = 0;
259 : do {
260 1715136751 : const double nextLength = (*i).distanceTo(*(i + 1));
261 1715136751 : if (seenLength + nextLength > pos) {
262 1432988832 : return positionAtOffset(*i, *(i + 1), pos - seenLength, lateralOffset);
263 : }
264 : seenLength += nextLength;
265 282147919 : } while (++i != end() - 1);
266 619859 : if (lateralOffset == 0 || size() < 2) {
267 596365 : return back();
268 : } else {
269 23494 : return positionAtOffset(*(end() - 2), *(end() - 1), (*(end() - 2)).distanceTo(*(end() - 1)), lateralOffset);
270 : }
271 : }
272 :
273 :
274 : Position
275 82923 : PositionVector::sidePositionAtAngle(double pos, double lateralOffset, double angle) const {
276 82923 : if (size() == 0) {
277 0 : return Position::INVALID;
278 : }
279 82923 : if (size() == 1) {
280 0 : return front();
281 : }
282 : const_iterator i = begin();
283 : double seenLength = 0;
284 : do {
285 199128 : const double nextLength = (*i).distanceTo(*(i + 1));
286 199128 : if (seenLength + nextLength > pos) {
287 82331 : return sidePositionAtAngle(*i, *(i + 1), pos - seenLength, lateralOffset, angle);
288 : }
289 : seenLength += nextLength;
290 116797 : } while (++i != end() - 1);
291 592 : return sidePositionAtAngle(*(end() - 2), *(end() - 1), (*(end() - 2)).distanceTo(*(end() - 1)), lateralOffset, angle);
292 : }
293 :
294 :
295 : Position
296 27323476 : PositionVector::positionAtOffset2D(double pos, double lateralOffset) const {
297 27323476 : if (size() == 0) {
298 0 : return Position::INVALID;
299 : }
300 27323476 : 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 40771704 : if (seenLength + nextLength > pos) {
308 20952552 : return positionAtOffset2D(*i, *(i + 1), pos - seenLength, lateralOffset);
309 : }
310 : seenLength += nextLength;
311 19819152 : } while (++i != end() - 1);
312 6370924 : return back();
313 : }
314 :
315 :
316 : double
317 5116241 : PositionVector::rotationAtOffset(double pos) const {
318 5116241 : if ((size() == 0) || (size() == 1)) {
319 : return INVALID_DOUBLE;
320 : }
321 5116241 : if (pos < 0) {
322 2636 : pos += length();
323 : }
324 : const_iterator i = begin();
325 : double seenLength = 0;
326 : do {
327 : const Position& p1 = *i;
328 : const Position& p2 = *(i + 1);
329 8622683 : const double nextLength = p1.distanceTo(p2);
330 8622683 : if (seenLength + nextLength > pos) {
331 5006004 : return p1.angleTo2D(p2);
332 : }
333 : seenLength += nextLength;
334 3616679 : } while (++i != end() - 1);
335 110237 : const Position& p1 = (*this)[-2];
336 : const Position& p2 = back();
337 110237 : return p1.angleTo2D(p2);
338 : }
339 :
340 :
341 : double
342 8984 : PositionVector::rotationDegreeAtOffset(double pos) const {
343 8984 : return GeomHelper::legacyDegree(rotationAtOffset(pos));
344 : }
345 :
346 :
347 : double
348 519782 : PositionVector::slopeDegreeAtOffset(double pos) const {
349 519782 : if (size() == 0) {
350 : return INVALID_DOUBLE;
351 : }
352 : const_iterator i = begin();
353 : double seenLength = 0;
354 : do {
355 : const Position& p1 = *i;
356 : const Position& p2 = *(i + 1);
357 663909 : const double nextLength = p1.distanceTo(p2);
358 663909 : if (seenLength + nextLength > pos) {
359 409362 : return RAD2DEG(p1.slopeTo2D(p2));
360 : }
361 : seenLength += nextLength;
362 254547 : } while (++i != end() - 1);
363 110420 : const Position& p1 = (*this)[-2];
364 : const Position& p2 = back();
365 110420 : return RAD2DEG(p1.slopeTo2D(p2));
366 : }
367 :
368 :
369 : Position
370 1438740092 : PositionVector::positionAtOffset(const Position& p1, const Position& p2, double pos, double lateralOffset) {
371 1438740092 : const double dist = p1.distanceTo(p2);
372 1438740092 : if (pos < 0. || dist < pos) {
373 170051 : return Position::INVALID;
374 : }
375 1438570041 : if (lateralOffset != 0) {
376 67868082 : if (dist == 0.) {
377 288 : return Position::INVALID;
378 : }
379 67867794 : const Position offset = sideOffset(p1, p2, -lateralOffset); // move in the same direction as Position::move2side
380 67867794 : if (pos == 0.) {
381 : return p1 + offset;
382 : }
383 67813255 : return p1 + (p2 - p1) * (pos / dist) + offset;
384 : }
385 1370701959 : if (pos == 0.) {
386 938624 : return p1;
387 : }
388 1369763335 : return p1 + (p2 - p1) * (pos / dist);
389 : }
390 :
391 :
392 : Position
393 82923 : PositionVector::sidePositionAtAngle(const Position& p1, const Position& p2, double pos, double lateralOffset, double angle) {
394 82923 : const double dist = p1.distanceTo(p2);
395 82923 : if (pos < 0. || dist < pos || dist == 0) {
396 6 : return Position::INVALID;
397 : }
398 82917 : angle -= DEG2RAD(90);
399 82917 : const Position offset(cos(angle) * lateralOffset, sin(angle) * lateralOffset);
400 82917 : return p1 + (p2 - p1) * (pos / dist) + offset;
401 : }
402 :
403 :
404 : Position
405 87195640 : PositionVector::positionAtOffset2D(const Position& p1, const Position& p2, double pos, double lateralOffset) {
406 : const double dist = p1.distanceTo2D(p2);
407 87195640 : if (pos < 0 || dist < pos) {
408 151 : return Position::INVALID;
409 : }
410 87195489 : if (lateralOffset != 0) {
411 144384 : const Position offset = sideOffset(p1, p2, -lateralOffset); // move in the same direction as Position::move2side
412 144384 : if (pos == 0.) {
413 : return p1 + offset;
414 : }
415 144384 : return p1 + (p2 - p1) * (pos / dist) + offset;
416 : }
417 87051105 : if (pos == 0.) {
418 43256784 : return p1;
419 : }
420 43794321 : return p1 + (p2 - p1) * (pos / dist);
421 : }
422 :
423 :
424 : Boundary
425 741041 : PositionVector::getBoxBoundary() const {
426 741041 : Boundary ret;
427 4234029 : for (const Position& i : *this) {
428 3492988 : ret.add(i);
429 : }
430 741041 : return ret;
431 0 : }
432 :
433 :
434 : Position
435 11255 : PositionVector::getPolygonCenter() const {
436 11255 : if (size() == 0) {
437 0 : return Position::INVALID;
438 : }
439 : double x = 0;
440 : double y = 0;
441 : double z = 0;
442 67566 : for (const Position& i : *this) {
443 56311 : x += i.x();
444 56311 : y += i.y();
445 56311 : z += i.z();
446 : }
447 11255 : return Position(x / (double) size(), y / (double) size(), z / (double)size());
448 : }
449 :
450 :
451 : Position
452 417283 : PositionVector::getCentroid() const {
453 417283 : if (size() == 0) {
454 0 : return Position::INVALID;
455 417283 : } else if (size() == 1) {
456 48 : return (*this)[0];
457 417235 : } else if (size() == 2) {
458 80545 : return ((*this)[0] + (*this)[1]) * 0.5;
459 : }
460 : PositionVector tmp = *this;
461 336690 : if (!isClosed()) { // make sure its closed
462 312571 : tmp.push_back(tmp[0]);
463 : }
464 : // shift to origin to increase numerical stability
465 336690 : Position offset = tmp[0];
466 : Position result;
467 336690 : tmp.sub(offset);
468 336690 : const int endIndex = (int)tmp.size() - 1;
469 : double div = 0; // 6 * area including sign
470 : double x = 0;
471 : double y = 0;
472 336690 : if (tmp.area() != 0) { // numerical instability ?
473 : // http://en.wikipedia.org/wiki/Polygon
474 4466295 : for (int i = 0; i < endIndex; i++) {
475 4143692 : const double z = tmp[i].x() * tmp[i + 1].y() - tmp[i + 1].x() * tmp[i].y();
476 4143692 : div += z; // area formula
477 4143692 : x += (tmp[i].x() + tmp[i + 1].x()) * z;
478 4143692 : y += (tmp[i].y() + tmp[i + 1].y()) * z;
479 : }
480 322603 : div *= 3; // 6 / 2, the 2 comes from the area formula
481 322603 : result = Position(x / div, y / div);
482 : } else {
483 : // compute by decomposing into line segments
484 : // http://en.wikipedia.org/wiki/Centroid#By_geometric_decomposition
485 : double lengthSum = 0;
486 56043 : for (int i = 0; i < endIndex; i++) {
487 41956 : double length = tmp[i].distanceTo(tmp[i + 1]);
488 41956 : x += (tmp[i].x() + tmp[i + 1].x()) * length / 2;
489 41956 : y += (tmp[i].y() + tmp[i + 1].y()) * length / 2;
490 41956 : lengthSum += length;
491 : }
492 14087 : if (lengthSum == 0) {
493 : // it is probably only one point
494 0 : result = tmp[0];
495 : }
496 14087 : result = Position(x / lengthSum, y / lengthSum) + offset;
497 : }
498 : return result + offset;
499 336690 : }
500 :
501 :
502 : void
503 65357 : PositionVector::scaleRelative(double factor) {
504 65357 : Position centroid = getCentroid();
505 196074 : for (int i = 0; i < static_cast<int>(size()); i++) {
506 130717 : (*this)[i] = centroid + (((*this)[i] - centroid) * factor);
507 : }
508 65357 : }
509 :
510 :
511 : void
512 796 : PositionVector::scaleAbsolute(double offset) {
513 796 : Position centroid = getCentroid();
514 6045 : for (int i = 0; i < static_cast<int>(size()); i++) {
515 5249 : (*this)[i] = centroid + (((*this)[i] - centroid) + offset);
516 : }
517 796 : }
518 :
519 :
520 : Position
521 4342 : PositionVector::getLineCenter() const {
522 4342 : if (size() == 1) {
523 0 : return (*this)[0];
524 : } else {
525 4342 : return positionAtOffset(double((length() / 2.)));
526 : }
527 : }
528 :
529 :
530 : double
531 6943777 : PositionVector::length() const {
532 6943777 : if (size() == 0) {
533 : return 0;
534 : }
535 : double len = 0;
536 20780588 : for (const_iterator i = begin(); i != end() - 1; i++) {
537 13938692 : len += (*i).distanceTo(*(i + 1));
538 : }
539 : return len;
540 : }
541 :
542 :
543 : double
544 25643161 : PositionVector::length2D() const {
545 25643161 : if (size() == 0) {
546 : return 0;
547 : }
548 : double len = 0;
549 67621356 : for (const_iterator i = begin(); i != end() - 1; i++) {
550 41978195 : len += (*i).distanceTo2D(*(i + 1));
551 : }
552 : return len;
553 : }
554 :
555 :
556 : double
557 336776 : PositionVector::area() const {
558 336776 : if (size() < 3) {
559 : return 0;
560 : }
561 : double area = 0;
562 : PositionVector tmp = *this;
563 336776 : if (!isClosed()) { // make sure its closed
564 23 : tmp.push_back(tmp[0]);
565 : }
566 336776 : const int endIndex = (int)tmp.size() - 1;
567 : // http://en.wikipedia.org/wiki/Polygon
568 4524702 : for (int i = 0; i < endIndex; i++) {
569 4187926 : area += tmp[i].x() * tmp[i + 1].y() - tmp[i + 1].x() * tmp[i].y();
570 : }
571 336776 : if (area < 0) { // we whether we had cw or ccw order
572 314446 : area *= -1;
573 : }
574 336776 : return area / 2;
575 336776 : }
576 :
577 :
578 : bool
579 9464648 : PositionVector::partialWithin(const AbstractPoly& poly, double offset) const {
580 9464648 : if (size() < 2) {
581 : return false;
582 : }
583 47308600 : for (const_iterator i = begin(); i != end(); i++) {
584 37882608 : if (poly.around(*i, offset)) {
585 : return true;
586 : }
587 : }
588 : return false;
589 : }
590 :
591 :
592 : bool
593 18841673 : PositionVector::crosses(const Position& p1, const Position& p2) const {
594 18841673 : return intersects(p1, p2);
595 : }
596 :
597 :
598 : std::pair<PositionVector, PositionVector>
599 246636 : PositionVector::splitAt(double where, bool use2D) const {
600 246636 : const double len = use2D ? length2D() : length();
601 246636 : if (size() < 2) {
602 0 : throw InvalidArgument("Vector to short for splitting");
603 : }
604 246636 : if (where < 0 || where > len) {
605 0 : throw InvalidArgument("Invalid split position " + toString(where) + " for vector of length " + toString(len));
606 : }
607 246636 : if (where <= POSITION_EPS || where >= len - POSITION_EPS) {
608 2 : WRITE_WARNINGF(TL("Splitting vector close to end (pos: %, length: %)"), toString(where), toString(len));
609 : }
610 246636 : PositionVector first, second;
611 246636 : first.push_back((*this)[0]);
612 : double seen = 0;
613 : const_iterator it = begin() + 1;
614 246636 : double next = use2D ? first.back().distanceTo2D(*it) : first.back().distanceTo(*it);
615 : // see how many points we can add to first
616 330624 : while (where >= seen + next + POSITION_EPS) {
617 : seen += next;
618 83988 : first.push_back(*it);
619 : it++;
620 83988 : next = use2D ? first.back().distanceTo2D(*it) : first.back().distanceTo(*it);
621 : }
622 246636 : if (fabs(where - (seen + next)) > POSITION_EPS || it == end() - 1) {
623 : // we need to insert a new point because 'where' is not close to an
624 : // existing point or it is to close to the endpoint
625 : const Position p = (use2D
626 236502 : ? positionAtOffset2D(first.back(), *it, where - seen)
627 236502 : : positionAtOffset(first.back(), *it, where - seen));
628 236502 : first.push_back(p);
629 236502 : second.push_back(p);
630 : } else {
631 10134 : first.push_back(*it);
632 : }
633 : // add the remaining points to second
634 615100 : for (; it != end(); it++) {
635 368464 : second.push_back(*it);
636 : }
637 : assert(first.size() >= 2);
638 : assert(second.size() >= 2);
639 : assert(first.back() == second.front());
640 : assert(fabs((use2D ? first.length2D() + second.length2D() : first.length() + second.length()) - len) < 2 * POSITION_EPS);
641 493272 : return std::pair<PositionVector, PositionVector>(first, second);
642 246636 : }
643 :
644 :
645 : std::ostream&
646 325401 : operator<<(std::ostream& os, const PositionVector& geom) {
647 1931931 : for (PositionVector::const_iterator i = geom.begin(); i != geom.end(); i++) {
648 1606530 : if (i != geom.begin()) {
649 1281134 : os << " ";
650 : }
651 1606530 : os << (*i);
652 : }
653 325401 : return os;
654 : }
655 :
656 :
657 : void
658 3 : PositionVector::sortAsPolyCWByAngle() {
659 : // We take the centroid of the points as an origin for the angle computations
660 : // that will follow but other points could be taken (the center of the bounding
661 : // box of the polygon for instance). Each of these can potentially lead
662 : // to a different result in the case of a non-convex polygon.
663 3 : const Position centroid = std::accumulate(begin(), end(), Position(0, 0)) / (double)size();
664 3 : sub(centroid);
665 3 : std::sort(begin(), end(), as_poly_cw_sorter());
666 3 : add(centroid);
667 3 : }
668 :
669 :
670 : void
671 680605 : PositionVector::add(double xoff, double yoff, double zoff) {
672 5712324 : for (int i = 0; i < (int)size(); i++) {
673 5031719 : (*this)[i].add(xoff, yoff, zoff);
674 : }
675 680605 : }
676 :
677 :
678 : void
679 336907 : PositionVector::sub(const Position& offset) {
680 336907 : add(-offset.x(), -offset.y(), -offset.z());
681 336907 : }
682 :
683 :
684 : void
685 7637 : PositionVector::add(const Position& offset) {
686 7637 : add(offset.x(), offset.y(), offset.z());
687 7637 : }
688 :
689 :
690 : PositionVector
691 0 : PositionVector::added(const Position& offset) const {
692 0 : PositionVector pv;
693 0 : for (auto i1 = begin(); i1 != end(); ++i1) {
694 0 : pv.push_back(*i1 + offset);
695 : }
696 0 : return pv;
697 0 : }
698 :
699 :
700 : void
701 9910 : PositionVector::mirrorX() {
702 27310 : for (int i = 0; i < (int)size(); i++) {
703 17400 : (*this)[i].mul(1, -1);
704 : }
705 9910 : }
706 :
707 :
708 3 : PositionVector::as_poly_cw_sorter::as_poly_cw_sorter() {}
709 :
710 :
711 : int
712 8 : PositionVector::as_poly_cw_sorter::operator()(const Position& p1, const Position& p2) const {
713 8 : double angle1 = atAngle2D(p1);
714 8 : double angle2 = atAngle2D(p2);
715 8 : if (angle1 > angle2) {
716 : return true;
717 : }
718 2 : if (angle1 == angle2) {
719 : double squaredDistance1 = p1.dotProduct(p1);
720 : double squaredDistance2 = p2.dotProduct(p2);
721 0 : if (squaredDistance1 < squaredDistance2) {
722 0 : return true;
723 : }
724 : }
725 : return false;
726 : }
727 :
728 :
729 : double
730 16 : PositionVector::as_poly_cw_sorter::atAngle2D(const Position& p) const {
731 16 : double angle = atan2(p.y(), p.x());
732 16 : return angle < 0.0 ? angle : angle + 2.0 * M_PI;
733 : }
734 :
735 : void
736 0 : PositionVector::sortByIncreasingXY() {
737 0 : std::sort(begin(), end(), increasing_x_y_sorter());
738 0 : }
739 :
740 :
741 0 : PositionVector::increasing_x_y_sorter::increasing_x_y_sorter() {}
742 :
743 :
744 : int
745 0 : PositionVector::increasing_x_y_sorter::operator()(const Position& p1, const Position& p2) const {
746 0 : if (p1.x() != p2.x()) {
747 0 : return p1.x() < p2.x();
748 : }
749 0 : return p1.y() < p2.y();
750 : }
751 :
752 :
753 : double
754 5478338 : PositionVector::isLeft(const Position& P0, const Position& P1, const Position& P2) const {
755 5478338 : return (P1.x() - P0.x()) * (P2.y() - P0.y()) - (P2.x() - P0.x()) * (P1.y() - P0.y());
756 : }
757 :
758 :
759 : void
760 7144859 : PositionVector::append(const PositionVector& v, double sameThreshold) {
761 7144859 : if ((size() > 0) && (v.size() > 0) && (back().distanceTo(v[0]) < sameThreshold)) {
762 : copy(v.begin() + 1, v.end(), back_inserter(*this));
763 : } else {
764 : copy(v.begin(), v.end(), back_inserter(*this));
765 : }
766 7144859 : }
767 :
768 :
769 : void
770 376 : PositionVector::prepend(const PositionVector& v, double sameThreshold) {
771 376 : if ((size() > 0) && (v.size() > 0) && (front().distanceTo(v.back()) < sameThreshold)) {
772 148 : insert(begin(), v.begin(), v.end() - 1);
773 : } else {
774 228 : insert(begin(), v.begin(), v.end());
775 : }
776 376 : }
777 :
778 :
779 : PositionVector
780 666004 : PositionVector::getSubpart(double beginOffset, double endOffset) const {
781 666004 : PositionVector ret;
782 666004 : Position begPos = front();
783 666004 : if (beginOffset > POSITION_EPS) {
784 45886 : begPos = positionAtOffset(beginOffset);
785 : }
786 666004 : Position endPos = back();
787 666004 : if (endOffset < length() - POSITION_EPS) {
788 160830 : endPos = positionAtOffset(endOffset);
789 : }
790 666004 : ret.push_back(begPos);
791 :
792 : double seen = 0;
793 : const_iterator i = begin();
794 : // skip previous segments
795 666004 : while ((i + 1) != end()
796 678556 : &&
797 678556 : seen + (*i).distanceTo(*(i + 1)) < beginOffset) {
798 12552 : seen += (*i).distanceTo(*(i + 1));
799 : i++;
800 : }
801 : // append segments in between
802 : while ((i + 1) != end()
803 748983 : &&
804 748554 : seen + (*i).distanceTo(*(i + 1)) < endOffset) {
805 :
806 82979 : ret.push_back_noDoublePos(*(i + 1));
807 82979 : seen += (*i).distanceTo(*(i + 1));
808 : i++;
809 : }
810 : // append end
811 666004 : ret.push_back_noDoublePos(endPos);
812 666004 : if (ret.size() == 1) {
813 18 : ret.push_back(endPos);
814 : }
815 666004 : return ret;
816 0 : }
817 :
818 :
819 : PositionVector
820 1065330 : PositionVector::getSubpart2D(double beginOffset, double endOffset) const {
821 1065330 : if (size() == 0) {
822 0 : return PositionVector();
823 : }
824 1065330 : PositionVector ret;
825 1065330 : Position begPos = front();
826 1065330 : if (beginOffset > POSITION_EPS) {
827 553339 : begPos = positionAtOffset2D(beginOffset);
828 : }
829 1065330 : Position endPos = back();
830 1065330 : if (endOffset < length2D() - POSITION_EPS) {
831 170244 : endPos = positionAtOffset2D(endOffset);
832 : }
833 1065330 : ret.push_back(begPos);
834 :
835 : double seen = 0;
836 : const_iterator i = begin();
837 : // skip previous segments
838 1065330 : while ((i + 1) != end()
839 1103774 : &&
840 1103774 : seen + (*i).distanceTo2D(*(i + 1)) < beginOffset) {
841 38444 : seen += (*i).distanceTo2D(*(i + 1));
842 : i++;
843 : }
844 : // append segments in between
845 : while ((i + 1) != end()
846 2355526 : &&
847 2077675 : seen + (*i).distanceTo2D(*(i + 1)) < endOffset) {
848 :
849 1290196 : ret.push_back_noDoublePos(*(i + 1));
850 1290196 : seen += (*i).distanceTo2D(*(i + 1));
851 : i++;
852 : }
853 : // append end
854 1065330 : ret.push_back_noDoublePos(endPos);
855 1065330 : if (ret.size() == 1) {
856 27 : ret.push_back(endPos);
857 : }
858 : return ret;
859 1065330 : }
860 :
861 :
862 : PositionVector
863 144176 : PositionVector::getSubpartByIndex(int beginIndex, int count) const {
864 144176 : if (size() == 0) {
865 0 : return PositionVector();
866 : }
867 144176 : if (beginIndex < 0) {
868 0 : beginIndex += (int)size();
869 : }
870 : assert(count >= 0);
871 : assert(beginIndex < (int)size());
872 : assert(beginIndex + count <= (int)size());
873 144176 : PositionVector result;
874 422858 : for (int i = beginIndex; i < beginIndex + count; ++i) {
875 278682 : result.push_back((*this)[i]);
876 : }
877 : return result;
878 144176 : }
879 :
880 :
881 : double
882 671526 : PositionVector::beginEndAngle() const {
883 671526 : if (size() == 0) {
884 : return INVALID_DOUBLE;
885 : }
886 671526 : return front().angleTo2D(back());
887 : }
888 :
889 :
890 : double
891 25663007 : PositionVector::nearest_offset_to_point2D(const Position& p, bool perpendicular) const {
892 25663007 : if (size() == 0) {
893 : return INVALID_DOUBLE;
894 : }
895 : double minDist = std::numeric_limits<double>::max();
896 25663007 : double nearestPos = GeomHelper::INVALID_OFFSET;
897 : double seen = 0;
898 90995445 : for (const_iterator i = begin(); i != end() - 1; i++) {
899 : const double pos =
900 65332438 : GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, perpendicular);
901 65332438 : const double dist2 = pos == GeomHelper::INVALID_OFFSET ? minDist : p.distanceSquaredTo2D(positionAtOffset2D(*i, *(i + 1), pos));
902 65332438 : if (dist2 < minDist) {
903 34902320 : nearestPos = pos + seen;
904 : minDist = dist2;
905 : }
906 65332438 : if (perpendicular && i != begin() && pos == GeomHelper::INVALID_OFFSET) {
907 : // even if perpendicular is set we still need to check the distance to the inner points
908 : const double cornerDist2 = p.distanceSquaredTo2D(*i);
909 495397 : if (cornerDist2 < minDist) {
910 : const double pos1 =
911 293731 : GeomHelper::nearest_offset_on_line_to_point2D(*(i - 1), *i, p, false);
912 : const double pos2 =
913 293731 : GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, false);
914 293731 : if (pos1 == (*(i - 1)).distanceTo2D(*i) && pos2 == 0.) {
915 : nearestPos = seen;
916 : minDist = cornerDist2;
917 : }
918 : }
919 : }
920 65332438 : seen += (*i).distanceTo2D(*(i + 1));
921 : }
922 : return nearestPos;
923 : }
924 :
925 :
926 : double
927 17968 : PositionVector::nearest_offset_to_point25D(const Position& p, bool perpendicular) const {
928 17968 : if (size() == 0) {
929 : return INVALID_DOUBLE;
930 : }
931 : double minDist = std::numeric_limits<double>::max();
932 17968 : double nearestPos = GeomHelper::INVALID_OFFSET;
933 : double seen = 0;
934 51814 : for (const_iterator i = begin(); i != end() - 1; i++) {
935 : const double pos =
936 33846 : GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, perpendicular);
937 33846 : const double dist = pos == GeomHelper::INVALID_OFFSET ? minDist : p.distanceTo2D(positionAtOffset2D(*i, *(i + 1), pos));
938 33846 : if (dist < minDist) {
939 26704 : const double pos25D = pos * (*i).distanceTo(*(i + 1)) / (*i).distanceTo2D(*(i + 1));
940 26704 : nearestPos = pos25D + seen;
941 : minDist = dist;
942 : }
943 33846 : if (perpendicular && i != begin() && pos == GeomHelper::INVALID_OFFSET) {
944 : // even if perpendicular is set we still need to check the distance to the inner points
945 : const double cornerDist = p.distanceTo2D(*i);
946 0 : if (cornerDist < minDist) {
947 : const double pos1 =
948 0 : GeomHelper::nearest_offset_on_line_to_point2D(*(i - 1), *i, p, false);
949 : const double pos2 =
950 0 : GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, false);
951 0 : if (pos1 == (*(i - 1)).distanceTo2D(*i) && pos2 == 0.) {
952 : nearestPos = seen;
953 : minDist = cornerDist;
954 : }
955 : }
956 : }
957 33846 : seen += (*i).distanceTo(*(i + 1));
958 : }
959 : return nearestPos;
960 : }
961 :
962 :
963 : Position
964 10007303 : PositionVector::transformToVectorCoordinates(const Position& p, bool extend) const {
965 10007303 : if (size() == 0) {
966 0 : return Position::INVALID;
967 : }
968 : // @toDo this duplicates most of the code in nearest_offset_to_point2D. It should be refactored
969 10007303 : if (extend) {
970 : PositionVector extended = *this;
971 4552249 : const double dist = 2 * distance2D(p);
972 4552249 : extended.extrapolate(dist);
973 4552249 : return extended.transformToVectorCoordinates(p) - Position(dist, 0);
974 4552249 : }
975 : double minDist = std::numeric_limits<double>::max();
976 : double nearestPos = -1;
977 : double seen = 0;
978 : int sign = 1;
979 13361940 : for (const_iterator i = begin(); i != end() - 1; i++) {
980 : const double pos =
981 7906886 : GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, true);
982 7906886 : const double dist = pos < 0 ? minDist : p.distanceTo2D(positionAtOffset(*i, *(i + 1), pos));
983 7906886 : if (dist < minDist) {
984 5284980 : nearestPos = pos + seen;
985 : minDist = dist;
986 5284980 : sign = isLeft(*i, *(i + 1), p) >= 0 ? -1 : 1;
987 : }
988 7906886 : if (i != begin() && pos == GeomHelper::INVALID_OFFSET) {
989 : // even if perpendicular is set we still need to check the distance to the inner points
990 : const double cornerDist = p.distanceTo2D(*i);
991 1493864 : if (cornerDist < minDist) {
992 : const double pos1 =
993 399730 : GeomHelper::nearest_offset_on_line_to_point2D(*(i - 1), *i, p, false);
994 : const double pos2 =
995 399730 : GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, false);
996 399730 : if (pos1 == (*(i - 1)).distanceTo2D(*i) && pos2 == 0.) {
997 : nearestPos = seen;
998 : minDist = cornerDist;
999 193358 : sign = isLeft(*(i - 1), *i, p) >= 0 ? -1 : 1;
1000 : }
1001 : }
1002 : }
1003 7906886 : seen += (*i).distanceTo2D(*(i + 1));
1004 : }
1005 5455054 : if (nearestPos != -1) {
1006 5454449 : return Position(nearestPos, sign * minDist);
1007 : } else {
1008 605 : return Position::INVALID;
1009 : }
1010 : }
1011 :
1012 :
1013 : int
1014 279100 : PositionVector::indexOfClosest(const Position& p, bool twoD) const {
1015 279100 : if (size() == 0) {
1016 : return -1;
1017 : }
1018 : double minDist = std::numeric_limits<double>::max();
1019 : double dist;
1020 : int closest = 0;
1021 1965973 : for (int i = 0; i < (int)size(); i++) {
1022 1686873 : const Position& p2 = (*this)[i];
1023 1686873 : dist = twoD ? p.distanceTo2D(p2) : p.distanceTo(p2);
1024 1686873 : if (dist < minDist) {
1025 : closest = i;
1026 : minDist = dist;
1027 : }
1028 : }
1029 : return closest;
1030 : }
1031 :
1032 :
1033 : int
1034 155133 : PositionVector::insertAtClosest(const Position& p, bool interpolateZ) {
1035 155133 : if (size() == 0) {
1036 : return -1;
1037 : }
1038 : double minDist = std::numeric_limits<double>::max();
1039 : int insertionIndex = 1;
1040 1179974 : for (int i = 0; i < (int)size() - 1; i++) {
1041 1024841 : const double length = GeomHelper::nearest_offset_on_line_to_point2D((*this)[i], (*this)[i + 1], p, false);
1042 1024841 : const Position& outIntersection = PositionVector::positionAtOffset2D((*this)[i], (*this)[i + 1], length);
1043 : const double dist = p.distanceTo2D(outIntersection);
1044 1024841 : if (dist < minDist) {
1045 : insertionIndex = i + 1;
1046 : minDist = dist;
1047 : }
1048 : }
1049 : // check if we have to adjust Position Z
1050 155133 : if (interpolateZ) {
1051 : // obtain previous and next Z
1052 0 : const double previousZ = (begin() + (insertionIndex - 1))->z();
1053 : const double nextZ = (begin() + insertionIndex)->z();
1054 : // insert new position using x and y of p, and the new z
1055 0 : insert(begin() + insertionIndex, Position(p.x(), p.y(), ((previousZ + nextZ) / 2.0)));
1056 : } else {
1057 155133 : insert(begin() + insertionIndex, p);
1058 : }
1059 : return insertionIndex;
1060 : }
1061 :
1062 :
1063 : int
1064 251 : PositionVector::removeClosest(const Position& p) {
1065 251 : if (size() == 0) {
1066 : return -1;
1067 : }
1068 : double minDist = std::numeric_limits<double>::max();
1069 : int removalIndex = 0;
1070 2477 : for (int i = 0; i < (int)size(); i++) {
1071 2226 : const double dist = p.distanceTo2D((*this)[i]);
1072 2226 : if (dist < minDist) {
1073 : removalIndex = i;
1074 : minDist = dist;
1075 : }
1076 : }
1077 : erase(begin() + removalIndex);
1078 251 : return removalIndex;
1079 : }
1080 :
1081 :
1082 : std::vector<double>
1083 5758084 : PositionVector::intersectsAtLengths2D(const PositionVector& other) const {
1084 : std::vector<double> ret;
1085 5758084 : if (other.size() == 0) {
1086 : return ret;
1087 : }
1088 19117124 : for (const_iterator i = other.begin(); i != other.end() - 1; i++) {
1089 13359040 : std::vector<double> atSegment = intersectsAtLengths2D(*i, *(i + 1));
1090 : copy(atSegment.begin(), atSegment.end(), back_inserter(ret));
1091 13359040 : }
1092 : return ret;
1093 0 : }
1094 :
1095 :
1096 : std::vector<double>
1097 13359040 : PositionVector::intersectsAtLengths2D(const Position& lp1, const Position& lp2) const {
1098 : std::vector<double> ret;
1099 13359040 : if (size() == 0) {
1100 : return ret;
1101 : }
1102 : double pos = 0;
1103 48241120 : for (const_iterator i = begin(); i != end() - 1; i++) {
1104 : const Position& p1 = *i;
1105 : const Position& p2 = *(i + 1);
1106 : double x, y, m;
1107 34882080 : if (intersects(p1, p2, lp1, lp2, 0., &x, &y, &m)) {
1108 3291125 : ret.push_back(Position(x, y).distanceTo2D(p1) + pos);
1109 : }
1110 34882080 : pos += p1.distanceTo2D(p2);
1111 : }
1112 : return ret;
1113 0 : }
1114 :
1115 :
1116 : void
1117 5376388 : PositionVector::extrapolate(const double val, const bool onlyFirst, const bool onlyLast) {
1118 5376388 : if (size() > 1) {
1119 5376388 : Position& p1 = (*this)[0];
1120 5376388 : Position& p2 = (*this)[1];
1121 5376388 : const Position offset = (p2 - p1) * (val / p1.distanceTo(p2));
1122 5376388 : if (!onlyLast) {
1123 : p1.sub(offset);
1124 : }
1125 5376388 : if (!onlyFirst) {
1126 5376388 : if (size() == 2) {
1127 : p2.add(offset);
1128 : } else {
1129 480282 : const Position& e1 = (*this)[-2];
1130 480282 : Position& e2 = (*this)[-1];
1131 480282 : e2.sub((e1 - e2) * (val / e1.distanceTo(e2)));
1132 : }
1133 : }
1134 : }
1135 5376388 : }
1136 :
1137 :
1138 : void
1139 4321411 : PositionVector::extrapolate2D(const double val, const bool onlyFirst) {
1140 4321411 : if (size() > 1) {
1141 4321411 : Position& p1 = (*this)[0];
1142 4321411 : Position& p2 = (*this)[1];
1143 4321411 : if (p1.distanceTo2D(p2) > 0) {
1144 4321405 : const Position offset = (p2 - p1) * (val / p1.distanceTo2D(p2));
1145 : p1.sub(offset);
1146 4321405 : if (!onlyFirst) {
1147 3415175 : if (size() == 2) {
1148 : p2.add(offset);
1149 : } else {
1150 329623 : const Position& e1 = (*this)[-2];
1151 329623 : Position& e2 = (*this)[-1];
1152 329623 : e2.sub((e1 - e2) * (val / e1.distanceTo2D(e2)));
1153 : }
1154 : }
1155 : }
1156 : }
1157 4321411 : }
1158 :
1159 :
1160 : PositionVector
1161 11272208 : PositionVector::reverse() const {
1162 11272208 : PositionVector ret;
1163 40501888 : for (const_reverse_iterator i = rbegin(); i != rend(); i++) {
1164 29229680 : ret.push_back(*i);
1165 : }
1166 11272208 : return ret;
1167 0 : }
1168 :
1169 :
1170 : Position
1171 103852094 : PositionVector::sideOffset(const Position& beg, const Position& end, const double amount) {
1172 103852094 : const double scale = amount / beg.distanceTo2D(end);
1173 103852094 : return Position((beg.y() - end.y()) * scale, (end.x() - beg.x()) * scale);
1174 : }
1175 :
1176 :
1177 : void
1178 16108660 : PositionVector::move2side(double amount, double maxExtension) {
1179 16108660 : if (size() < 2) {
1180 160439 : return;
1181 : }
1182 16108660 : removeDoublePoints(POSITION_EPS, true);
1183 16108660 : if (length2D() == 0 || amount == 0) {
1184 : return;
1185 : }
1186 15948221 : PositionVector shape;
1187 : std::vector<int> recheck;
1188 49994677 : for (int i = 0; i < static_cast<int>(size()); i++) {
1189 34046456 : if (i == 0) {
1190 15948221 : const Position& from = (*this)[i];
1191 15948221 : const Position& to = (*this)[i + 1];
1192 : if (from != to) {
1193 31896442 : shape.push_back(from - sideOffset(from, to, amount));
1194 : #ifdef DEBUG_MOVE2SIDE
1195 : if (gDebugFlag1) {
1196 : std::cout << " " << i << "a=" << shape.back() << "\n";
1197 : }
1198 : #endif
1199 : }
1200 18098235 : } else if (i == static_cast<int>(size()) - 1) {
1201 15948221 : const Position& from = (*this)[i - 1];
1202 15948221 : const Position& to = (*this)[i];
1203 : if (from != to) {
1204 31896442 : shape.push_back(to - sideOffset(from, to, amount));
1205 : #ifdef DEBUG_MOVE2SIDE
1206 : if (gDebugFlag1) {
1207 : std::cout << " " << i << "b=" << shape.back() << "\n";
1208 : }
1209 : #endif
1210 : }
1211 : } else {
1212 2150014 : const Position& from = (*this)[i - 1];
1213 2150014 : const Position& me = (*this)[i];
1214 2150014 : const Position& to = (*this)[i + 1];
1215 2150014 : PositionVector fromMe(from, me);
1216 2150014 : fromMe.extrapolate2D(me.distanceTo2D(to));
1217 2150014 : const double extrapolateDev = fromMe[1].distanceTo2D(to);
1218 2150014 : if (fabs(extrapolateDev) < POSITION_EPS) {
1219 : // parallel case, just shift the middle point
1220 716988 : shape.push_back(me - sideOffset(from, to, amount));
1221 : #ifdef DEBUG_MOVE2SIDE
1222 : if (gDebugFlag1) {
1223 : std::cout << " " << i << "c=" << shape.back() << "\n";
1224 : }
1225 : #endif
1226 1791520 : } else if (fabs(extrapolateDev - 2 * me.distanceTo2D(to)) < POSITION_EPS) {
1227 : // counterparallel case, just shift the middle point
1228 422 : PositionVector fromMe2(from, me);
1229 422 : fromMe2.extrapolate2D(amount);
1230 422 : shape.push_back(fromMe2[1]);
1231 : #ifdef DEBUG_MOVE2SIDE
1232 : if (gDebugFlag1) {
1233 : std::cout << " " << i << "d=" << shape.back() << " " << i << "_from=" << from << " " << i << "_me=" << me << " " << i << "_to=" << to << "\n";
1234 : }
1235 : #endif
1236 422 : } else {
1237 1791098 : Position offsets = sideOffset(from, me, amount);
1238 1791098 : Position offsets2 = sideOffset(me, to, amount);
1239 1791098 : PositionVector l1(from - offsets, me - offsets);
1240 1791098 : PositionVector l2(me - offsets2, to - offsets2);
1241 1791098 : Position meNew = l1.intersectionPosition2D(l2[0], l2[1], maxExtension);
1242 6 : if (meNew == Position::INVALID) {
1243 6 : recheck.push_back(i);
1244 : continue;
1245 : }
1246 1791092 : meNew = meNew + Position(0, 0, me.z());
1247 1791092 : shape.push_back(meNew);
1248 : #ifdef DEBUG_MOVE2SIDE
1249 : if (gDebugFlag1) {
1250 : std::cout << " " << i << "e=" << shape.back() << "\n";
1251 : }
1252 : #endif
1253 1791098 : }
1254 : // copy original z value
1255 : shape.back().set(shape.back().x(), shape.back().y(), me.z());
1256 2150008 : const double angle = localAngle(from, me, to);
1257 2150008 : if (fabs(angle) > NUMERICAL_EPS) {
1258 1984244 : const double length = from.distanceTo2D(me) + me.distanceTo2D(to);
1259 1984244 : const double radius = length / angle;
1260 : #ifdef DEBUG_MOVE2SIDE
1261 : if (gDebugFlag1) {
1262 : std::cout << " i=" << i << " a=" << RAD2DEG(angle) << " l=" << length << " r=" << radius << " t=" << amount * 1.8 << "\n";
1263 : }
1264 : #endif
1265 1984244 : if ((radius < 0 && -radius < amount * 1.8) || fabs(RAD2DEG(angle)) > 170) {
1266 1232 : recheck.push_back(i);
1267 : }
1268 : }
1269 2150014 : }
1270 : }
1271 15948221 : if (!recheck.empty()) {
1272 : // try to adjust positions to avoid clipping
1273 : shape = *this;
1274 2140 : for (int i = (int)recheck.size() - 1; i >= 0; i--) {
1275 1238 : shape.erase(shape.begin() + recheck[i]);
1276 : }
1277 902 : shape.move2side(amount, maxExtension);
1278 : }
1279 : *this = shape;
1280 15948221 : }
1281 :
1282 :
1283 : void
1284 52 : PositionVector::move2sideCustom(std::vector<double> amount, double maxExtension) {
1285 52 : if (size() < 2) {
1286 0 : return;
1287 : }
1288 52 : if (length2D() == 0) {
1289 : return;
1290 : }
1291 52 : if (size() != amount.size()) {
1292 0 : throw InvalidArgument("Numer of offsets (" + toString(amount.size())
1293 0 : + ") does not match number of points (" + toString(size()) + ")");
1294 : }
1295 52 : PositionVector shape;
1296 2543 : for (int i = 0; i < static_cast<int>(size()); i++) {
1297 2491 : if (i == 0) {
1298 52 : const Position& from = (*this)[i];
1299 52 : const Position& to = (*this)[i + 1];
1300 : if (from != to) {
1301 104 : shape.push_back(from - sideOffset(from, to, amount[i]));
1302 : }
1303 2439 : } else if (i == static_cast<int>(size()) - 1) {
1304 52 : const Position& from = (*this)[i - 1];
1305 52 : const Position& to = (*this)[i];
1306 : if (from != to) {
1307 104 : shape.push_back(to - sideOffset(from, to, amount[i]));
1308 : }
1309 : } else {
1310 2387 : const Position& from = (*this)[i - 1];
1311 2387 : const Position& me = (*this)[i];
1312 2387 : const Position& to = (*this)[i + 1];
1313 2387 : PositionVector fromMe(from, me);
1314 2387 : fromMe.extrapolate2D(me.distanceTo2D(to));
1315 2387 : const double extrapolateDev = fromMe[1].distanceTo2D(to);
1316 2387 : if (fabs(extrapolateDev) < POSITION_EPS) {
1317 : // parallel case, just shift the middle point
1318 4448 : shape.push_back(me - sideOffset(from, to, amount[i]));
1319 163 : } else if (fabs(extrapolateDev - 2 * me.distanceTo2D(to)) < POSITION_EPS) {
1320 : // counterparallel case, just shift the middle point
1321 0 : PositionVector fromMe2(from, me);
1322 0 : fromMe2.extrapolate2D(amount[i]);
1323 0 : shape.push_back(fromMe2[1]);
1324 0 : } else {
1325 163 : Position offsets = sideOffset(from, me, amount[i]);
1326 163 : Position offsets2 = sideOffset(me, to, amount[i]);
1327 163 : PositionVector l1(from - offsets, me - offsets);
1328 163 : PositionVector l2(me - offsets2, to - offsets2);
1329 163 : Position meNew = l1.intersectionPosition2D(l2[0], l2[1], maxExtension);
1330 0 : if (meNew == Position::INVALID) {
1331 : continue;
1332 : }
1333 163 : meNew = meNew + Position(0, 0, me.z());
1334 163 : shape.push_back(meNew);
1335 163 : }
1336 : // copy original z value
1337 : shape.back().set(shape.back().x(), shape.back().y(), me.z());
1338 2387 : }
1339 : }
1340 : *this = shape;
1341 52 : }
1342 :
1343 : double
1344 2150008 : PositionVector::localAngle(const Position& from, const Position& pos, const Position& to) {
1345 2150008 : return GeomHelper::angleDiff(from.angleTo2D(pos), pos.angleTo2D(to));
1346 : }
1347 :
1348 : double
1349 24544566 : PositionVector::angleAt2D(int pos) const {
1350 24544566 : if ((pos + 1) < (int)size()) {
1351 24544566 : return (*this)[pos].angleTo2D((*this)[pos + 1]);
1352 : }
1353 : return INVALID_DOUBLE;
1354 : }
1355 :
1356 :
1357 : void
1358 693723 : PositionVector::closePolygon() {
1359 693723 : if ((size() != 0) && ((*this)[0] != back())) {
1360 398307 : push_back((*this)[0]);
1361 : }
1362 693723 : }
1363 :
1364 :
1365 : std::vector<double>
1366 2055999 : PositionVector::distances(const PositionVector& s, bool perpendicular) const {
1367 : std::vector<double> ret;
1368 : const_iterator i;
1369 9901878 : for (i = begin(); i != end(); i++) {
1370 7845879 : const double dist = s.distance2D(*i, perpendicular);
1371 7845879 : if (dist != GeomHelper::INVALID_OFFSET) {
1372 7811572 : ret.push_back(dist);
1373 : }
1374 : }
1375 9864633 : for (i = s.begin(); i != s.end(); i++) {
1376 7808634 : const double dist = distance2D(*i, perpendicular);
1377 7808634 : if (dist != GeomHelper::INVALID_OFFSET) {
1378 7804819 : ret.push_back(dist);
1379 : }
1380 : }
1381 2055999 : return ret;
1382 0 : }
1383 :
1384 :
1385 : double
1386 25182643 : PositionVector::distance2D(const Position& p, bool perpendicular) const {
1387 25182643 : if (size() == 0) {
1388 : return std::numeric_limits<double>::max();
1389 25182463 : } else if (size() == 1) {
1390 484792 : return front().distanceTo2D(p);
1391 : }
1392 24697671 : const double nearestOffset = nearest_offset_to_point2D(p, perpendicular);
1393 24697671 : if (nearestOffset == GeomHelper::INVALID_OFFSET) {
1394 : return GeomHelper::INVALID_OFFSET;
1395 : } else {
1396 24657950 : return p.distanceTo2D(positionAtOffset2D(nearestOffset));
1397 : }
1398 : }
1399 :
1400 :
1401 : void
1402 99377 : PositionVector::push_front(const Position& p) {
1403 99377 : if (empty()) {
1404 0 : push_back(p);
1405 : } else {
1406 99377 : insert(begin(), p);
1407 : }
1408 99377 : }
1409 :
1410 :
1411 : void
1412 0 : PositionVector::pop_front() {
1413 0 : if (empty()) {
1414 0 : throw ProcessError("PositionVector is empty");
1415 : } else {
1416 : erase(begin());
1417 : }
1418 0 : }
1419 :
1420 :
1421 : void
1422 4269624 : PositionVector::push_back_noDoublePos(const Position& p) {
1423 8453042 : if (size() == 0 || !p.almostSame(back())) {
1424 3956980 : push_back(p);
1425 : }
1426 4269624 : }
1427 :
1428 :
1429 : void
1430 108515 : PositionVector::push_front_noDoublePos(const Position& p) {
1431 217030 : if ((size() == 0) || !p.almostSame(front())) {
1432 99376 : push_front(p);
1433 : }
1434 108515 : }
1435 :
1436 :
1437 : void
1438 0 : PositionVector::insert_noDoublePos(const std::vector<Position>::iterator& at, const Position& p) {
1439 0 : if (at == begin()) {
1440 0 : push_front_noDoublePos(p);
1441 0 : } else if (at == end()) {
1442 0 : push_back_noDoublePos(p);
1443 : } else {
1444 0 : if (!p.almostSame(*at) && !p.almostSame(*(at - 1))) {
1445 0 : insert(at, p);
1446 : }
1447 : }
1448 0 : }
1449 :
1450 :
1451 : bool
1452 673567 : PositionVector::isClosed() const {
1453 673567 : return (size() >= 2) && ((*this)[0] == back());
1454 : }
1455 :
1456 :
1457 : bool
1458 3190 : PositionVector::isNAN() const {
1459 : // iterate over all positions and check if is NAN
1460 12543 : for (auto i = begin(); i != end(); i++) {
1461 : if (i->isNAN()) {
1462 : return true;
1463 : }
1464 : }
1465 : // all ok, then return false
1466 : return false;
1467 : }
1468 :
1469 :
1470 : void
1471 16221048 : PositionVector::removeDoublePoints(double minDist, bool assertLength, int beginOffset, int endOffset, bool resample) {
1472 16221048 : int curSize = (int)size() - beginOffset - endOffset;
1473 16221048 : if (curSize > 1) {
1474 : iterator last = begin() + beginOffset;
1475 19576734 : for (iterator i = last + 1; i != (end() - endOffset) && (!assertLength || curSize > 2);) {
1476 3394782 : if (last->almostSame(*i, minDist)) {
1477 3118 : if (i + 1 == end() - endOffset) {
1478 : // special case: keep the last point and remove the next-to-last
1479 1044 : if (resample && last > begin() && (last - 1)->distanceTo(*i) >= 2 * minDist) {
1480 : // resample rather than remove point after a long segment
1481 3 : const double shiftBack = minDist - last->distanceTo(*i);
1482 : //if (gDebugFlag1) std::cout << " resample endOffset beforeLast=" << *(last - 1) << " last=" << *last << " i=" << *i;
1483 3 : (*last) = positionAtOffset(*(last - 1), *last, (last - 1)->distanceTo(*last) - shiftBack);
1484 : //if (gDebugFlag1) std::cout << " lastNew=" << *last;
1485 : last = i;
1486 : ++i;
1487 : } else {
1488 : erase(last);
1489 : i = end() - endOffset;
1490 : }
1491 : } else {
1492 2074 : if (resample && i + 1 != end() && last->distanceTo(*(i + 1)) >= 2 * minDist) {
1493 : // resample rather than remove points before a long segment
1494 2 : const double shiftForward = minDist - last->distanceTo(*i);
1495 : //if (gDebugFlag1) std::cout << " resample last=" << *last << " i=" << *i << " next=" << *(i + 1);
1496 2 : (*i) = positionAtOffset(*i, *(i + 1), shiftForward);
1497 : //if (gDebugFlag1) std::cout << " iNew=" << *i << "\n";
1498 : last = i;
1499 : ++i;
1500 : } else {
1501 : i = erase(i);
1502 : }
1503 : }
1504 3118 : curSize--;
1505 : } else {
1506 : last = i;
1507 : ++i;
1508 : }
1509 : }
1510 : }
1511 16221048 : }
1512 :
1513 :
1514 : bool
1515 416920 : PositionVector::operator==(const PositionVector& v2) const {
1516 416920 : return static_cast<vp>(*this) == static_cast<vp>(v2);
1517 : }
1518 :
1519 :
1520 : bool
1521 289602 : PositionVector::operator!=(const PositionVector& v2) const {
1522 289602 : return static_cast<vp>(*this) != static_cast<vp>(v2);
1523 : }
1524 :
1525 : PositionVector
1526 0 : PositionVector::operator-(const PositionVector& v2) const {
1527 0 : if (length() != v2.length()) {
1528 0 : WRITE_ERROR(TL("Trying to subtract PositionVectors of different lengths."));
1529 : }
1530 0 : PositionVector pv;
1531 : auto i1 = begin();
1532 : auto i2 = v2.begin();
1533 0 : while (i1 != end()) {
1534 0 : pv.add(*i1 - *i2);
1535 : }
1536 0 : return pv;
1537 0 : }
1538 :
1539 : PositionVector
1540 0 : PositionVector::operator+(const PositionVector& v2) const {
1541 0 : if (length() != v2.length()) {
1542 0 : WRITE_ERROR(TL("Trying to add PositionVectors of different lengths."));
1543 : }
1544 0 : PositionVector pv;
1545 : auto i1 = begin();
1546 : auto i2 = v2.begin();
1547 0 : while (i1 != end()) {
1548 0 : pv.add(*i1 + *i2);
1549 : }
1550 0 : return pv;
1551 0 : }
1552 :
1553 : bool
1554 5394 : PositionVector::almostSame(const PositionVector& v2, double maxDiv) const {
1555 5394 : if (size() != v2.size()) {
1556 : return false;
1557 : }
1558 : auto i2 = v2.begin();
1559 10629 : for (auto i1 = begin(); i1 != end(); i1++) {
1560 8116 : if (!i1->almostSame(*i2, maxDiv)) {
1561 : return false;
1562 : }
1563 : i2++;
1564 : }
1565 : return true;
1566 : }
1567 :
1568 : bool
1569 2218864 : PositionVector::hasElevation() const {
1570 2218864 : if (size() < 2) {
1571 : return false;
1572 : }
1573 9565800 : for (const_iterator i = begin(); i != end(); i++) {
1574 7349094 : if ((*i).z() != 0) {
1575 : return true;
1576 : }
1577 : }
1578 : return false;
1579 : }
1580 :
1581 :
1582 : bool
1583 100532698 : PositionVector::intersects(const Position& p11, const Position& p12, const Position& p21, const Position& p22, const double withinDist, double* x, double* y, double* mu) {
1584 : const double eps = std::numeric_limits<double>::epsilon();
1585 100532698 : const double denominator = (p22.y() - p21.y()) * (p12.x() - p11.x()) - (p22.x() - p21.x()) * (p12.y() - p11.y());
1586 100532698 : const double numera = (p22.x() - p21.x()) * (p11.y() - p21.y()) - (p22.y() - p21.y()) * (p11.x() - p21.x());
1587 100532698 : const double numerb = (p12.x() - p11.x()) * (p11.y() - p21.y()) - (p12.y() - p11.y()) * (p11.x() - p21.x());
1588 : /* Are the lines coincident? */
1589 100532698 : if (fabs(numera) < eps && fabs(numerb) < eps && fabs(denominator) < eps) {
1590 : double a1;
1591 : double a2;
1592 : double a3;
1593 : double a4;
1594 : double a = -1e12;
1595 14144 : if (p11.x() != p12.x()) {
1596 10494 : a1 = p11.x() < p12.x() ? p11.x() : p12.x();
1597 10494 : a2 = p11.x() < p12.x() ? p12.x() : p11.x();
1598 10494 : a3 = p21.x() < p22.x() ? p21.x() : p22.x();
1599 10494 : a4 = p21.x() < p22.x() ? p22.x() : p21.x();
1600 : } else {
1601 3650 : a1 = p11.y() < p12.y() ? p11.y() : p12.y();
1602 3650 : a2 = p11.y() < p12.y() ? p12.y() : p11.y();
1603 3650 : a3 = p21.y() < p22.y() ? p21.y() : p22.y();
1604 3650 : a4 = p21.y() < p22.y() ? p22.y() : p21.y();
1605 : }
1606 14144 : if (a1 <= a3 && a3 <= a2) {
1607 7900 : if (a4 < a2) {
1608 316 : a = (a3 + a4) / 2;
1609 : } else {
1610 7584 : a = (a2 + a3) / 2;
1611 : }
1612 : }
1613 14144 : if (a3 <= a1 && a1 <= a4) {
1614 7921 : if (a2 < a4) {
1615 234 : a = (a1 + a2) / 2;
1616 : } else {
1617 7687 : a = (a1 + a4) / 2;
1618 : }
1619 : }
1620 14144 : if (a != -1e12) {
1621 10968 : if (x != nullptr) {
1622 7862 : if (p11.x() != p12.x()) {
1623 6259 : *mu = (a - p11.x()) / (p12.x() - p11.x());
1624 6259 : *x = a;
1625 6259 : *y = p11.y() + (*mu) * (p12.y() - p11.y());
1626 : } else {
1627 1603 : *x = p11.x();
1628 1603 : *y = a;
1629 1603 : if (p12.y() == p11.y()) {
1630 58 : *mu = 0;
1631 : } else {
1632 1545 : *mu = (a - p11.y()) / (p12.y() - p11.y());
1633 : }
1634 : }
1635 : }
1636 10968 : return true;
1637 : }
1638 : return false;
1639 : }
1640 : /* Are the lines parallel */
1641 100518554 : if (fabs(denominator) < eps) {
1642 : return false;
1643 : }
1644 : /* Is the intersection along the segments */
1645 96787013 : double mua = numera / denominator;
1646 : /* reduce rounding errors for lines ending in the same point */
1647 96787013 : if (fabs(p12.x() - p22.x()) < eps && fabs(p12.y() - p22.y()) < eps) {
1648 : mua = 1.;
1649 : } else {
1650 96784286 : const double offseta = withinDist / p11.distanceTo2D(p12);
1651 96784286 : const double offsetb = withinDist / p21.distanceTo2D(p22);
1652 96784286 : const double mub = numerb / denominator;
1653 96784286 : if (mua < -offseta || mua > 1 + offseta || mub < -offsetb || mub > 1 + offsetb) {
1654 : return false;
1655 : }
1656 : }
1657 6302523 : if (x != nullptr) {
1658 5265858 : *x = p11.x() + mua * (p12.x() - p11.x());
1659 5265858 : *y = p11.y() + mua * (p12.y() - p11.y());
1660 5265858 : *mu = mua;
1661 : }
1662 : return true;
1663 : }
1664 :
1665 :
1666 : void
1667 6723 : PositionVector::rotate2D(double angle) {
1668 6723 : const double s = sin(angle);
1669 6723 : const double c = cos(angle);
1670 84753 : for (int i = 0; i < (int)size(); i++) {
1671 78030 : const double x = (*this)[i].x();
1672 78030 : const double y = (*this)[i].y();
1673 78030 : const double z = (*this)[i].z();
1674 78030 : const double xnew = x * c - y * s;
1675 78030 : const double ynew = x * s + y * c;
1676 78030 : (*this)[i].set(xnew, ynew, z);
1677 : }
1678 6723 : }
1679 :
1680 :
1681 : void
1682 0 : PositionVector::rotateAroundFirstElement2D(double angle) {
1683 0 : if (size() > 1) {
1684 : // translate position vector to (0,0), rotate, and traslate back again
1685 0 : const Position offset = front();
1686 0 : sub(offset);
1687 0 : rotate2D(angle);
1688 0 : add(offset);
1689 : }
1690 0 : }
1691 :
1692 :
1693 : PositionVector
1694 49449 : PositionVector::simplified() const {
1695 : PositionVector result = *this;
1696 : bool changed = true;
1697 142264 : while (changed && result.size() > 3) {
1698 : changed = false;
1699 452592 : for (int i = 0; i < (int)result.size(); i++) {
1700 419538 : const Position& p1 = result[i];
1701 419538 : const Position& p2 = result[(i + 2) % result.size()];
1702 419538 : const int middleIndex = (i + 1) % result.size();
1703 419538 : const Position& p0 = result[middleIndex];
1704 : // https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line#Line_defined_by_two_points
1705 419538 : const double triangleArea2 = fabs((p2.y() - p1.y()) * p0.x() - (p2.x() - p1.x()) * p0.y() + p2.x() * p1.y() - p2.y() * p1.x());
1706 : const double distIK = p1.distanceTo2D(p2);
1707 419538 : if (distIK > NUMERICAL_EPS && triangleArea2 / distIK < NUMERICAL_EPS) {
1708 : changed = true;
1709 : result.erase(result.begin() + middleIndex);
1710 10312 : break;
1711 : }
1712 : }
1713 : }
1714 49449 : return result;
1715 0 : }
1716 :
1717 :
1718 : const PositionVector
1719 1178 : PositionVector::simplified2(const bool closed, const double eps) const {
1720 : // this is a variation of the https://en.wikipedia.org/wiki/Visvalingam%E2%80%93Whyatt_algorithm
1721 : // which uses the distance instead of the area
1722 : // the benefits over the initial implementation are:
1723 : // 3D support, no degenerate results for a sequence of small distances, keeping the longest part of a line
1724 : // drawbacks: complexity of the code, speed
1725 1178 : if (size() < 3) {
1726 : return *this;
1727 : }
1728 14829 : auto calcScore = [&](const PositionVector & pv, int index) {
1729 14829 : if (!closed && (index == 0 || index == (int)pv.size() - 1)) {
1730 1981 : return eps + 1.;
1731 : }
1732 12848 : const Position& p = pv[index];
1733 12848 : const Position& a = pv[(index + (int)pv.size() - 1) % pv.size()];
1734 12848 : const Position& b = pv[(index + 1) % pv.size()];
1735 12848 : const double distAB = a.distanceTo(b);
1736 25696 : if (distAB < MIN2(eps, NUMERICAL_EPS)) {
1737 : // avoid division by 0 and degenerate cases due to very small baseline
1738 9 : return (a.distanceTo(p) + b.distanceTo(p)) / 2.;
1739 : }
1740 : // https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line#Vector_formulation
1741 : // calculating the distance of p to the line defined by a and b
1742 : const Position dir = (b - a) / distAB;
1743 : const double projectedLength = (a - p).dotProduct(dir);
1744 12839 : if (projectedLength <= -distAB) {
1745 5 : return b.distanceTo(p);
1746 : }
1747 12834 : if (projectedLength >= 0.) {
1748 4 : return a.distanceTo(p);
1749 : }
1750 : const Position distVector = (a - p) - dir * projectedLength;
1751 : return distVector.length();
1752 809 : };
1753 : std::vector<double> scores;
1754 809 : double minScore = eps + 1.;
1755 : int minIndex = -1;
1756 10948 : for (int i = 0; i < (int)size(); i++) {
1757 10139 : scores.push_back(calcScore(*this, i));
1758 10139 : if (scores.back() < minScore) {
1759 : minScore = scores.back();
1760 : minIndex = i;
1761 : }
1762 : }
1763 809 : if (minScore >= eps) {
1764 : return *this;
1765 : }
1766 : PositionVector result(*this);
1767 2459 : while (minScore < eps) {
1768 : result.erase(result.begin() + minIndex);
1769 2375 : if (result.size() < 3) {
1770 : break;
1771 : }
1772 : scores.erase(scores.begin() + minIndex);
1773 2345 : const int prevIndex = (minIndex + (int)result.size() - 1) % result.size();
1774 2345 : scores[prevIndex] = calcScore(result, prevIndex);
1775 2345 : scores[minIndex % result.size()] = calcScore(result, minIndex % result.size());
1776 2345 : minScore = eps + 1.;
1777 148569 : for (int i = 0; i < (int)result.size(); i++) {
1778 146224 : if (scores[i] < minScore) {
1779 : minScore = scores[i];
1780 : minIndex = i;
1781 : }
1782 : }
1783 : }
1784 : return result;
1785 809 : }
1786 :
1787 :
1788 : PositionVector
1789 1873 : PositionVector::getOrthogonal(const Position& p, double extend, bool before, double length, double deg) const {
1790 1873 : PositionVector result;
1791 : PositionVector tmp = *this;
1792 1873 : tmp.extrapolate2D(extend);
1793 1873 : const double baseOffset = tmp.nearest_offset_to_point2D(p);
1794 1873 : if (baseOffset == GeomHelper::INVALID_OFFSET || size() < 2) {
1795 : // fail
1796 : return result;
1797 : }
1798 1873 : Position base = tmp.positionAtOffset2D(baseOffset);
1799 1873 : const int closestIndex = tmp.indexOfClosest(base);
1800 1873 : const double closestOffset = tmp.offsetAtIndex2D(closestIndex);
1801 1873 : result.push_back(base);
1802 1873 : if (fabs(baseOffset - closestOffset) > NUMERICAL_EPS) {
1803 1872 : result.push_back(tmp[closestIndex]);
1804 1872 : if ((closestOffset < baseOffset) != before) {
1805 1372 : deg *= -1;
1806 : }
1807 1 : } else if (before) {
1808 : // take the segment before closestIndex if possible
1809 1 : if (closestIndex > 0) {
1810 1 : result.push_back(tmp[closestIndex - 1]);
1811 : } else {
1812 0 : result.push_back(tmp[1]);
1813 0 : deg *= -1;
1814 : }
1815 : } else {
1816 : // take the segment after closestIndex if possible
1817 0 : if (closestIndex < (int)size() - 1) {
1818 0 : result.push_back(tmp[closestIndex + 1]);
1819 : } else {
1820 0 : result.push_back(tmp[-1]);
1821 0 : deg *= -1;
1822 : }
1823 : }
1824 3746 : result = result.getSubpart2D(0, length);
1825 : // rotate around base
1826 1873 : result.add(base * -1);
1827 1873 : result.rotate2D(DEG2RAD(deg));
1828 1873 : result.add(base);
1829 : return result;
1830 1873 : }
1831 :
1832 :
1833 : PositionVector
1834 182093 : PositionVector::smoothedZFront(double dist) const {
1835 : PositionVector result = *this;
1836 182093 : if (size() == 0) {
1837 : return result;
1838 : }
1839 182093 : const double z0 = (*this)[0].z();
1840 : // the z-delta of the first segment
1841 182093 : const double dz = (*this)[1].z() - z0;
1842 : // if the shape only has 2 points it is as smooth as possible already
1843 182093 : if (size() > 2 && dz != 0) {
1844 1219 : dist = MIN2(dist, length2D());
1845 : // check wether we need to insert a new point at dist
1846 1219 : Position pDist = positionAtOffset2D(dist);
1847 1219 : int iLast = indexOfClosest(pDist);
1848 : // prevent close spacing to reduce impact of rounding errors in z-axis
1849 1219 : if (pDist.distanceTo2D((*this)[iLast]) > POSITION_EPS * 20) {
1850 11 : iLast = result.insertAtClosest(pDist, false);
1851 : }
1852 1219 : double dist2 = result.offsetAtIndex2D(iLast);
1853 1219 : const double dz2 = result[iLast].z() - z0;
1854 : double seen = 0;
1855 6561 : for (int i = 1; i < iLast; ++i) {
1856 5342 : seen += result[i].distanceTo2D(result[i - 1]);
1857 5342 : result[i].set(result[i].x(), result[i].y(), z0 + dz2 * seen / dist2);
1858 : }
1859 : }
1860 : return result;
1861 :
1862 0 : }
1863 :
1864 :
1865 : PositionVector
1866 221770 : PositionVector::interpolateZ(double zStart, double zEnd) const {
1867 : PositionVector result = *this;
1868 221770 : if (size() == 0) {
1869 : return result;
1870 : }
1871 221770 : result[0].setz(zStart);
1872 221770 : result[-1].setz(zEnd);
1873 221770 : const double dz = zEnd - zStart;
1874 221770 : const double length = length2D();
1875 : double seen = 0;
1876 351603 : for (int i = 1; i < (int)size() - 1; ++i) {
1877 129833 : seen += result[i].distanceTo2D(result[i - 1]);
1878 129833 : result[i].setz(zStart + dz * seen / length);
1879 : }
1880 : return result;
1881 0 : }
1882 :
1883 :
1884 : PositionVector
1885 0 : PositionVector::resample(double maxLength, const bool adjustEnd) const {
1886 0 : PositionVector result;
1887 0 : if (maxLength == 0) {
1888 : return result;
1889 : }
1890 0 : const double length = length2D();
1891 0 : if (length < POSITION_EPS) {
1892 : return result;
1893 : }
1894 0 : maxLength = length / ceil(length / maxLength);
1895 0 : for (double pos = 0; pos <= length; pos += maxLength) {
1896 0 : result.push_back(positionAtOffset2D(pos));
1897 : }
1898 : // check if we have to adjust last element
1899 0 : if (adjustEnd && !result.empty() && (result.back() != back())) {
1900 : // add last element
1901 0 : result.push_back(back());
1902 : }
1903 : return result;
1904 0 : }
1905 :
1906 :
1907 : double
1908 3208 : PositionVector::offsetAtIndex2D(int index) const {
1909 3208 : if (index < 0 || index >= (int)size()) {
1910 0 : return GeomHelper::INVALID_OFFSET;
1911 : }
1912 : double seen = 0;
1913 12710 : for (int i = 1; i <= index; ++i) {
1914 9502 : seen += (*this)[i].distanceTo2D((*this)[i - 1]);
1915 : }
1916 : return seen;
1917 : }
1918 :
1919 :
1920 : double
1921 4940 : PositionVector::getMaxGrade(double& maxJump) const {
1922 : double result = 0;
1923 10350 : for (int i = 1; i < (int)size(); ++i) {
1924 5410 : const Position& p1 = (*this)[i - 1];
1925 5410 : const Position& p2 = (*this)[i];
1926 5410 : const double distZ = fabs(p1.z() - p2.z());
1927 : const double dist2D = p1.distanceTo2D(p2);
1928 5410 : if (dist2D == 0) {
1929 0 : maxJump = MAX2(maxJump, distZ);
1930 : } else {
1931 5410 : result = MAX2(result, distZ / dist2D);
1932 : }
1933 : }
1934 4940 : return result;
1935 : }
1936 :
1937 :
1938 : double
1939 280 : PositionVector::getMinZ() const {
1940 : double minZ = std::numeric_limits<double>::max();
1941 840 : for (const Position& i : *this) {
1942 : minZ = MIN2(minZ, i.z());
1943 : }
1944 280 : return minZ;
1945 : }
1946 :
1947 :
1948 : PositionVector
1949 183379 : PositionVector::bezier(int numPoints) {
1950 : // inspired by David F. Rogers
1951 : assert(size() < 33);
1952 : static const double fac[33] = {
1953 : 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,
1954 : 6227020800.0, 87178291200.0, 1307674368000.0, 20922789888000.0, 355687428096000.0, 6402373705728000.0,
1955 : 121645100408832000.0, 2432902008176640000.0, 51090942171709440000.0, 1124000727777607680000.0,
1956 : 25852016738884976640000.0, 620448401733239439360000.0, 15511210043330985984000000.0,
1957 : 403291461126605635584000000.0, 10888869450418352160768000000.0, 304888344611713860501504000000.0,
1958 : 8841761993739701954543616000000.0, 265252859812191058636308480000000.0,
1959 : 8222838654177922817725562880000000.0, 263130836933693530167218012160000000.0
1960 : };
1961 183379 : PositionVector ret;
1962 183379 : const int npts = (int)size();
1963 : // calculate the points on the Bezier curve
1964 183379 : const double step = (double) 1.0 / (numPoints - 1);
1965 : double t = 0.;
1966 : Position prev;
1967 1262439 : for (int i1 = 0; i1 < numPoints; i1++) {
1968 1079060 : if ((1.0 - t) < 5e-6) {
1969 : t = 1.0;
1970 : }
1971 : double x = 0., y = 0., z = 0.;
1972 4540930 : for (int i = 0; i < npts; i++) {
1973 3461870 : const double ti = (i == 0) ? 1.0 : pow(t, i);
1974 3461870 : const double tni = (npts == i + 1) ? 1.0 : pow(1 - t, npts - i - 1);
1975 3461870 : const double basis = fac[npts - 1] / (fac[i] * fac[npts - 1 - i]) * ti * tni;
1976 3461870 : x += basis * at(i).x();
1977 3461870 : y += basis * at(i).y();
1978 3461870 : z += basis * at(i).z();
1979 : }
1980 1079060 : t += step;
1981 : Position current(x, y, z);
1982 1079060 : if (prev != current && !std::isnan(x) && !std::isnan(y) && !std::isnan(z)) {
1983 1079060 : ret.push_back(current);
1984 : }
1985 : prev = current;
1986 : }
1987 183379 : return ret;
1988 0 : }
1989 :
1990 :
1991 3 : bool PositionVector::isClockwiseOriented() {
1992 : // The test is based on the computation of a signed area enclosed by the polygon.
1993 : // If the polygon is in the upper (resp. the lower) half-plane and the area is
1994 : // negatively (resp. positively) signed, then the polygon is CW oriented. In case
1995 : // the polygon has points with both positive and negative y-coordinates, we translate
1996 : // the polygon to apply the above simple area-based test.
1997 : double area = 0.0;
1998 : const double y_min = std::min_element(begin(), end(), [](Position p1, Position p2) {
1999 : return p1.y() < p2.y();
2000 : })->y();
2001 3 : const double gap = y_min > 0.0 ? 0.0 : y_min;
2002 3 : add(0., gap, 0.);
2003 3 : const int last = (int)size() - 1;
2004 10 : for (int i = 0; i < last; i++) {
2005 7 : const Position& firstPoint = at(i);
2006 7 : const Position& secondPoint = at(i + 1);
2007 7 : area += (secondPoint.x() - firstPoint.x()) / (secondPoint.y() + firstPoint.y()) / 2.0;
2008 : }
2009 3 : area += (at(0).x() - at(last).x()) / (at(0).y() + at(last).y()) / 2.0;
2010 3 : add(0., -gap, 0.);
2011 3 : return area < 0.0;
2012 : }
2013 :
2014 :
2015 : /****************************************************************************/
|
