Eclipse SUMO - Simulation of Urban MObility
Loading...
Searching...
No Matches
PositionVector.cpp
Go to the documentation of this file.
1/****************************************************************************/
2// Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3// Copyright (C) 2001-2023 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/****************************************************************************/
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 <cassert>
30#include <iterator>
31#include <limits>
35#include "AbstractPoly.h"
36#include "Position.h"
37#include "PositionVector.h"
38#include "GeomHelper.h"
39#include "Boundary.h"
40
41//#define DEBUG_MOVE2SIDE
42
43// ===========================================================================
44// static members
45// ===========================================================================
47
48// ===========================================================================
49// method definitions
50// ===========================================================================
51
53
54
55PositionVector::PositionVector(const std::vector<Position>& v) {
56 std::copy(v.begin(), v.end(), std::back_inserter(*this));
57}
58
59
60PositionVector::PositionVector(const std::vector<Position>::const_iterator beg, const std::vector<Position>::const_iterator end) {
61 std::copy(beg, end, std::back_inserter(*this));
62}
63
64
66 push_back(p1);
67 push_back(p2);
68}
69
70
72
73
74bool
75PositionVector::around(const Position& p, double offset) const {
76 if (size() < 2) {
77 return false;
78 }
79 if (offset != 0) {
80 PositionVector tmp(*this);
81 tmp.scaleAbsolute(offset);
82 return tmp.around(p);
83 }
84 double angle = 0;
85 // iterate over all points, and obtain angle between current and next
86 for (const_iterator i = begin(); i != (end() - 1); i++) {
87 Position p1(
88 i->x() - p.x(),
89 i->y() - p.y());
90 Position p2(
91 (i + 1)->x() - p.x(),
92 (i + 1)->y() - p.y());
93 angle += GeomHelper::angle2D(p1, p2);
94 }
95 // add angle between last and first point
96 Position p1(
97 (end() - 1)->x() - p.x(),
98 (end() - 1)->y() - p.y());
99 Position p2(
100 begin()->x() - p.x(),
101 begin()->y() - p.y());
102 angle += GeomHelper::angle2D(p1, p2);
103 // if angle is less than PI, then point lying in Polygon
104 return (!(fabs(angle) < M_PI));
105}
106
107
108bool
109PositionVector::overlapsWith(const AbstractPoly& poly, double offset) const {
110 if (
111 // check whether one of my points lies within the given poly
112 partialWithin(poly, offset) ||
113 // check whether the polygon lies within me
114 poly.partialWithin(*this, offset)) {
115 return true;
116 }
117 if (size() >= 2) {
118 for (const_iterator i = begin(); i != end() - 1; i++) {
119 if (poly.crosses(*i, *(i + 1))) {
120 return true;
121 }
122 }
123 if (size() > 2 && poly.crosses(back(), front())) {
124 return true;
125 }
126 }
127 return false;
128}
129
130
131double
132PositionVector::getOverlapWith(const PositionVector& poly, double zThreshold) const {
133 double result = 0;
134 if ((size() == 0) || (poly.size() == 0)) {
135 return result;
136 }
137 // this points within poly
138 for (const_iterator i = begin(); i != end() - 1; i++) {
139 if (poly.around(*i)) {
141 if (fabs(closest.z() - (*i).z()) < zThreshold) {
142 result = MAX2(result, poly.distance2D(*i));
143 }
144 }
145 }
146 // polys points within this
147 for (const_iterator i = poly.begin(); i != poly.end() - 1; i++) {
148 if (around(*i)) {
150 if (fabs(closest.z() - (*i).z()) < zThreshold) {
151 result = MAX2(result, distance2D(*i));
152 }
153 }
154 }
155 return result;
156}
157
158
159bool
160PositionVector::intersects(const Position& p1, const Position& p2) const {
161 if (size() < 2) {
162 return false;
163 }
164 for (const_iterator i = begin(); i != end() - 1; i++) {
165 if (intersects(*i, *(i + 1), p1, p2)) {
166 return true;
167 }
168 }
169 return false;
170}
171
172
173bool
175 if (size() < 2) {
176 return false;
177 }
178 for (const_iterator i = begin(); i != end() - 1; i++) {
179 if (v1.intersects(*i, *(i + 1))) {
180 return true;
181 }
182 }
183 return false;
184}
185
186
188PositionVector::intersectionPosition2D(const Position& p1, const Position& p2, const double withinDist) const {
189 for (const_iterator i = begin(); i != end() - 1; i++) {
190 double x, y, m;
191 if (intersects(*i, *(i + 1), p1, p2, withinDist, &x, &y, &m)) {
192 return Position(x, y);
193 }
194 }
195 return Position::INVALID;
196}
197
198
201 for (const_iterator i = begin(); i != end() - 1; i++) {
202 if (v1.intersects(*i, *(i + 1))) {
203 return v1.intersectionPosition2D(*i, *(i + 1));
204 }
205 }
206 return Position::INVALID;
207}
208
209
210const Position&
212 /* bracket operators works as in Python. Examples:
213 - A = {'a', 'b', 'c', 'd'} (size 4)
214 - A [2] returns 'c' because 0 < 2 < 4
215 - A [100] throws an exception because 100 > 4
216 - A [-1] returns 'd' because 4 - 1 = 3
217 - A [-100] throws an exception because (4-100) < 0
218 */
219 if (index >= 0 && index < (int)size()) {
220 return at(index);
221 } else if (index < 0 && -index <= (int)size()) {
222 return at((int)size() + index);
223 } else {
224 throw OutOfBoundsException("Index out of range in bracket operator of PositionVector");
225 }
226}
227
228
231 /* bracket operators works as in Python. Examples:
232 - A = {'a', 'b', 'c', 'd'} (size 4)
233 - A [2] returns 'c' because 0 < 2 < 4
234 - A [100] throws an exception because 100 > 4
235 - A [-1] returns 'd' because 4 - 1 = 3
236 - A [-100] throws an exception because (4-100) < 0
237 */
238 if (index >= 0 && index < (int)size()) {
239 return at(index);
240 } else if (index < 0 && -index <= (int)size()) {
241 return at((int)size() + index);
242 } else {
243 throw OutOfBoundsException("Index out of range in bracket operator of PositionVector");
244 }
245}
246
247
249PositionVector::positionAtOffset(double pos, double lateralOffset) const {
250 if (size() == 0) {
251 return Position::INVALID;
252 }
253 if (size() == 1) {
254 return front();
255 }
256 const_iterator i = begin();
257 double seenLength = 0;
258 do {
259 const double nextLength = (*i).distanceTo(*(i + 1));
260 if (seenLength + nextLength > pos) {
261 return positionAtOffset(*i, *(i + 1), pos - seenLength, lateralOffset);
262 }
263 seenLength += nextLength;
264 } while (++i != end() - 1);
265 if (lateralOffset == 0 || size() < 2) {
266 return back();
267 } else {
268 return positionAtOffset(*(end() - 2), *(end() - 1), (*(end() - 2)).distanceTo(*(end() - 1)), lateralOffset);
269 }
270}
271
272
274PositionVector::sidePositionAtAngle(double pos, double lateralOffset, double angle) const {
275 if (size() == 0) {
276 return Position::INVALID;
277 }
278 if (size() == 1) {
279 return front();
280 }
281 const_iterator i = begin();
282 double seenLength = 0;
283 do {
284 const double nextLength = (*i).distanceTo(*(i + 1));
285 if (seenLength + nextLength > pos) {
286 return sidePositionAtAngle(*i, *(i + 1), pos - seenLength, lateralOffset, angle);
287 }
288 seenLength += nextLength;
289 } while (++i != end() - 1);
290 return sidePositionAtAngle(*(end() - 2), *(end() - 1), (*(end() - 2)).distanceTo(*(end() - 1)), lateralOffset, angle);
291}
292
293
295PositionVector::positionAtOffset2D(double pos, double lateralOffset) const {
296 if (size() == 0) {
297 return Position::INVALID;
298 }
299 if (size() == 1) {
300 return front();
301 }
302 const_iterator i = begin();
303 double seenLength = 0;
304 do {
305 const double nextLength = (*i).distanceTo2D(*(i + 1));
306 if (seenLength + nextLength > pos) {
307 return positionAtOffset2D(*i, *(i + 1), pos - seenLength, lateralOffset);
308 }
309 seenLength += nextLength;
310 } while (++i != end() - 1);
311 return back();
312}
313
314
315double
317 if ((size() == 0) || (size() == 1)) {
318 return INVALID_DOUBLE;
319 }
320 if (pos < 0) {
321 pos += length();
322 }
323 const_iterator i = begin();
324 double seenLength = 0;
325 do {
326 const Position& p1 = *i;
327 const Position& p2 = *(i + 1);
328 const double nextLength = p1.distanceTo(p2);
329 if (seenLength + nextLength > pos) {
330 return p1.angleTo2D(p2);
331 }
332 seenLength += nextLength;
333 } while (++i != end() - 1);
334 const Position& p1 = (*this)[-2];
335 const Position& p2 = back();
336 return p1.angleTo2D(p2);
337}
338
339
340double
344
345
346double
348 if (size() == 0) {
349 return INVALID_DOUBLE;
350 }
351 const_iterator i = begin();
352 double seenLength = 0;
353 do {
354 const Position& p1 = *i;
355 const Position& p2 = *(i + 1);
356 const double nextLength = p1.distanceTo(p2);
357 if (seenLength + nextLength > pos) {
358 return RAD2DEG(p1.slopeTo2D(p2));
359 }
360 seenLength += nextLength;
361 } while (++i != end() - 1);
362 const Position& p1 = (*this)[-2];
363 const Position& p2 = back();
364 return RAD2DEG(p1.slopeTo2D(p2));
365}
366
367
369PositionVector::positionAtOffset(const Position& p1, const Position& p2, double pos, double lateralOffset) {
370 const double dist = p1.distanceTo(p2);
371 if (pos < 0. || dist < pos) {
372 return Position::INVALID;
373 }
374 if (lateralOffset != 0) {
375 if (dist == 0.) {
376 return Position::INVALID;
377 }
378 const Position offset = sideOffset(p1, p2, -lateralOffset); // move in the same direction as Position::move2side
379 if (pos == 0.) {
380 return p1 + offset;
381 }
382 return p1 + (p2 - p1) * (pos / dist) + offset;
383 }
384 if (pos == 0.) {
385 return p1;
386 }
387 return p1 + (p2 - p1) * (pos / dist);
388}
389
390
392PositionVector::sidePositionAtAngle(const Position& p1, const Position& p2, double pos, double lateralOffset, double angle) {
393 const double dist = p1.distanceTo(p2);
394 if (pos < 0. || dist < pos || dist == 0) {
395 return Position::INVALID;
396 }
397 angle -= DEG2RAD(90);
398 const Position offset(cos(angle) * lateralOffset, sin(angle) * lateralOffset);
399 return p1 + (p2 - p1) * (pos / dist) + offset;
400}
401
402
404PositionVector::positionAtOffset2D(const Position& p1, const Position& p2, double pos, double lateralOffset) {
405 const double dist = p1.distanceTo2D(p2);
406 if (pos < 0 || dist < pos) {
407 return Position::INVALID;
408 }
409 if (lateralOffset != 0) {
410 const Position offset = sideOffset(p1, p2, -lateralOffset); // move in the same direction as Position::move2side
411 if (pos == 0.) {
412 return p1 + offset;
413 }
414 return p1 + (p2 - p1) * (pos / dist) + offset;
415 }
416 if (pos == 0.) {
417 return p1;
418 }
419 return p1 + (p2 - p1) * (pos / dist);
420}
421
422
425 Boundary ret;
426 for (const Position& i : *this) {
427 ret.add(i);
428 }
429 return ret;
430}
431
432
435 if (size() == 0) {
436 return Position::INVALID;
437 }
438 double x = 0;
439 double y = 0;
440 double z = 0;
441 for (const Position& i : *this) {
442 x += i.x();
443 y += i.y();
444 z += i.z();
445 }
446 return Position(x / (double) size(), y / (double) size(), z / (double)size());
447}
448
449
452 if (size() == 0) {
453 return Position::INVALID;
454 } else if (size() == 1) {
455 return (*this)[0];
456 } else if (size() == 2) {
457 return ((*this)[0] + (*this)[1]) * 0.5;
458 }
459 PositionVector tmp = *this;
460 if (!isClosed()) { // make sure its closed
461 tmp.push_back(tmp[0]);
462 }
463 // shift to origin to increase numerical stability
464 Position offset = tmp[0];
465 Position result;
466 tmp.sub(offset);
467 const int endIndex = (int)tmp.size() - 1;
468 double div = 0; // 6 * area including sign
469 double x = 0;
470 double y = 0;
471 if (tmp.area() != 0) { // numerical instability ?
472 // http://en.wikipedia.org/wiki/Polygon
473 for (int i = 0; i < endIndex; i++) {
474 const double z = tmp[i].x() * tmp[i + 1].y() - tmp[i + 1].x() * tmp[i].y();
475 div += z; // area formula
476 x += (tmp[i].x() + tmp[i + 1].x()) * z;
477 y += (tmp[i].y() + tmp[i + 1].y()) * z;
478 }
479 div *= 3; // 6 / 2, the 2 comes from the area formula
480 result = Position(x / div, y / div);
481 } else {
482 // compute by decomposing into line segments
483 // http://en.wikipedia.org/wiki/Centroid#By_geometric_decomposition
484 double lengthSum = 0;
485 for (int i = 0; i < endIndex; i++) {
486 double length = tmp[i].distanceTo(tmp[i + 1]);
487 x += (tmp[i].x() + tmp[i + 1].x()) * length / 2;
488 y += (tmp[i].y() + tmp[i + 1].y()) * length / 2;
489 lengthSum += length;
490 }
491 if (lengthSum == 0) {
492 // it is probably only one point
493 result = tmp[0];
494 }
495 result = Position(x / lengthSum, y / lengthSum) + offset;
496 }
497 return result + offset;
498}
499
500
501void
503 Position centroid = getCentroid();
504 for (int i = 0; i < static_cast<int>(size()); i++) {
505 (*this)[i] = centroid + (((*this)[i] - centroid) * factor);
506 }
507}
508
509
510void
512 Position centroid = getCentroid();
513 for (int i = 0; i < static_cast<int>(size()); i++) {
514 (*this)[i] = centroid + (((*this)[i] - centroid) + offset);
515 }
516}
517
518
521 if (size() == 1) {
522 return (*this)[0];
523 } else {
524 return positionAtOffset(double((length() / 2.)));
525 }
526}
527
528
529double
531 if (size() == 0) {
532 return 0;
533 }
534 double len = 0;
535 for (const_iterator i = begin(); i != end() - 1; i++) {
536 len += (*i).distanceTo(*(i + 1));
537 }
538 return len;
539}
540
541
542double
544 if (size() == 0) {
545 return 0;
546 }
547 double len = 0;
548 for (const_iterator i = begin(); i != end() - 1; i++) {
549 len += (*i).distanceTo2D(*(i + 1));
550 }
551 return len;
552}
553
554
555double
557 if (size() < 3) {
558 return 0;
559 }
560 double area = 0;
561 PositionVector tmp = *this;
562 if (!isClosed()) { // make sure its closed
563 tmp.push_back(tmp[0]);
564 }
565 const int endIndex = (int)tmp.size() - 1;
566 // http://en.wikipedia.org/wiki/Polygon
567 for (int i = 0; i < endIndex; i++) {
568 area += tmp[i].x() * tmp[i + 1].y() - tmp[i + 1].x() * tmp[i].y();
569 }
570 if (area < 0) { // we whether we had cw or ccw order
571 area *= -1;
572 }
573 return area / 2;
574}
575
576
577bool
578PositionVector::partialWithin(const AbstractPoly& poly, double offset) const {
579 if (size() < 2) {
580 return false;
581 }
582 for (const_iterator i = begin(); i != end(); i++) {
583 if (poly.around(*i, offset)) {
584 return true;
585 }
586 }
587 return false;
588}
589
590
591bool
592PositionVector::crosses(const Position& p1, const Position& p2) const {
593 return intersects(p1, p2);
594}
595
596
597std::pair<PositionVector, PositionVector>
598PositionVector::splitAt(double where, bool use2D) const {
599 const double len = use2D ? length2D() : length();
600 if (size() < 2) {
601 throw InvalidArgument("Vector to short for splitting");
602 }
603 if (where < 0 || where > len) {
604 throw InvalidArgument("Invalid split position " + toString(where) + " for vector of length " + toString(len));
605 }
606 if (where <= POSITION_EPS || where >= len - POSITION_EPS) {
607 WRITE_WARNINGF(TL("Splitting vector close to end (pos: %, length: %)"), toString(where), toString(len));
608 }
609 PositionVector first, second;
610 first.push_back((*this)[0]);
611 double seen = 0;
612 const_iterator it = begin() + 1;
613 double next = use2D ? first.back().distanceTo2D(*it) : first.back().distanceTo(*it);
614 // see how many points we can add to first
615 while (where >= seen + next + POSITION_EPS) {
616 seen += next;
617 first.push_back(*it);
618 it++;
619 next = use2D ? first.back().distanceTo2D(*it) : first.back().distanceTo(*it);
620 }
621 if (fabs(where - (seen + next)) > POSITION_EPS || it == end() - 1) {
622 // we need to insert a new point because 'where' is not close to an
623 // existing point or it is to close to the endpoint
624 const Position p = (use2D
625 ? positionAtOffset2D(first.back(), *it, where - seen)
626 : positionAtOffset(first.back(), *it, where - seen));
627 first.push_back(p);
628 second.push_back(p);
629 } else {
630 first.push_back(*it);
631 }
632 // add the remaining points to second
633 for (; it != end(); it++) {
634 second.push_back(*it);
635 }
636 assert(first.size() >= 2);
637 assert(second.size() >= 2);
638 assert(first.back() == second.front());
639 assert(fabs((use2D ? first.length2D() + second.length2D() : first.length() + second.length()) - len) < 2 * POSITION_EPS);
640 return std::pair<PositionVector, PositionVector>(first, second);
641}
642
643
644std::ostream&
645operator<<(std::ostream& os, const PositionVector& geom) {
646 for (PositionVector::const_iterator i = geom.begin(); i != geom.end(); i++) {
647 if (i != geom.begin()) {
648 os << " ";
649 }
650 os << (*i);
651 }
652 return os;
653}
654
655
656void
658 std::sort(begin(), end(), as_poly_cw_sorter());
659}
660
661
662void
663PositionVector::add(double xoff, double yoff, double zoff) {
664 for (int i = 0; i < (int)size(); i++) {
665 (*this)[i].add(xoff, yoff, zoff);
666 }
667}
668
669
670void
672 add(-offset.x(), -offset.y(), -offset.z());
673}
674
675
676void
678 add(offset.x(), offset.y(), offset.z());
679}
680
681
683PositionVector::added(const Position& offset) const {
685 for (auto i1 = begin(); i1 != end(); ++i1) {
686 pv.push_back(*i1 + offset);
687 }
688 return pv;
689}
690
691
692void
694 for (int i = 0; i < (int)size(); i++) {
695 (*this)[i].mul(1, -1);
696 }
697}
698
699
701
702
703int
705 return atan2(p1.x(), p1.y()) < atan2(p2.x(), p2.y());
706}
707
708
709void
711 std::sort(begin(), end(), increasing_x_y_sorter());
712}
713
714
716
717
718int
720 if (p1.x() != p2.x()) {
721 return p1.x() < p2.x();
722 }
723 return p1.y() < p2.y();
724}
725
726
727double
728PositionVector::isLeft(const Position& P0, const Position& P1, const Position& P2) const {
729 return (P1.x() - P0.x()) * (P2.y() - P0.y()) - (P2.x() - P0.x()) * (P1.y() - P0.y());
730}
731
732
733void
734PositionVector::append(const PositionVector& v, double sameThreshold) {
735 if ((size() > 0) && (v.size() > 0) && (back().distanceTo(v[0]) < sameThreshold)) {
736 copy(v.begin() + 1, v.end(), back_inserter(*this));
737 } else {
738 copy(v.begin(), v.end(), back_inserter(*this));
739 }
740}
741
742
743void
744PositionVector::prepend(const PositionVector& v, double sameThreshold) {
745 if ((size() > 0) && (v.size() > 0) && (front().distanceTo(v.back()) < sameThreshold)) {
746 insert(begin(), v.begin(), v.end() - 1);
747 } else {
748 insert(begin(), v.begin(), v.end());
749 }
750}
751
752
754PositionVector::getSubpart(double beginOffset, double endOffset) const {
755 PositionVector ret;
756 Position begPos = front();
757 if (beginOffset > POSITION_EPS) {
758 begPos = positionAtOffset(beginOffset);
759 }
760 Position endPos = back();
761 if (endOffset < length() - POSITION_EPS) {
762 endPos = positionAtOffset(endOffset);
763 }
764 ret.push_back(begPos);
765
766 double seen = 0;
767 const_iterator i = begin();
768 // skip previous segments
769 while ((i + 1) != end()
770 &&
771 seen + (*i).distanceTo(*(i + 1)) < beginOffset) {
772 seen += (*i).distanceTo(*(i + 1));
773 i++;
774 }
775 // append segments in between
776 while ((i + 1) != end()
777 &&
778 seen + (*i).distanceTo(*(i + 1)) < endOffset) {
779
780 ret.push_back_noDoublePos(*(i + 1));
781 seen += (*i).distanceTo(*(i + 1));
782 i++;
783 }
784 // append end
785 ret.push_back_noDoublePos(endPos);
786 if (ret.size() == 1) {
787 ret.push_back(endPos);
788 }
789 return ret;
790}
791
792
794PositionVector::getSubpart2D(double beginOffset, double endOffset) const {
795 if (size() == 0) {
796 return PositionVector();
797 }
798 PositionVector ret;
799 Position begPos = front();
800 if (beginOffset > POSITION_EPS) {
801 begPos = positionAtOffset2D(beginOffset);
802 }
803 Position endPos = back();
804 if (endOffset < length2D() - POSITION_EPS) {
805 endPos = positionAtOffset2D(endOffset);
806 }
807 ret.push_back(begPos);
808
809 double seen = 0;
810 const_iterator i = begin();
811 // skip previous segments
812 while ((i + 1) != end()
813 &&
814 seen + (*i).distanceTo2D(*(i + 1)) < beginOffset) {
815 seen += (*i).distanceTo2D(*(i + 1));
816 i++;
817 }
818 // append segments in between
819 while ((i + 1) != end()
820 &&
821 seen + (*i).distanceTo2D(*(i + 1)) < endOffset) {
822
823 ret.push_back_noDoublePos(*(i + 1));
824 seen += (*i).distanceTo2D(*(i + 1));
825 i++;
826 }
827 // append end
828 ret.push_back_noDoublePos(endPos);
829 if (ret.size() == 1) {
830 ret.push_back(endPos);
831 }
832 return ret;
833}
834
835
837PositionVector::getSubpartByIndex(int beginIndex, int count) const {
838 if (size() == 0) {
839 return PositionVector();
840 }
841 if (beginIndex < 0) {
842 beginIndex += (int)size();
843 }
844 assert(count >= 0);
845 assert(beginIndex < (int)size());
846 assert(beginIndex + count <= (int)size());
847 PositionVector result;
848 for (int i = beginIndex; i < beginIndex + count; ++i) {
849 result.push_back((*this)[i]);
850 }
851 return result;
852}
853
854
855double
857 if (size() == 0) {
858 return INVALID_DOUBLE;
859 }
860 return front().angleTo2D(back());
861}
862
863
864double
865PositionVector::nearest_offset_to_point2D(const Position& p, bool perpendicular) const {
866 if (size() == 0) {
867 return INVALID_DOUBLE;
868 }
869 double minDist = std::numeric_limits<double>::max();
870 double nearestPos = GeomHelper::INVALID_OFFSET;
871 double seen = 0;
872 for (const_iterator i = begin(); i != end() - 1; i++) {
873 const double pos =
874 GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, perpendicular);
875 const double dist = pos == GeomHelper::INVALID_OFFSET ? minDist : p.distanceTo2D(positionAtOffset2D(*i, *(i + 1), pos));
876 if (dist < minDist) {
877 nearestPos = pos + seen;
878 minDist = dist;
879 }
880 if (perpendicular && i != begin() && pos == GeomHelper::INVALID_OFFSET) {
881 // even if perpendicular is set we still need to check the distance to the inner points
882 const double cornerDist = p.distanceTo2D(*i);
883 if (cornerDist < minDist) {
884 const double pos1 =
885 GeomHelper::nearest_offset_on_line_to_point2D(*(i - 1), *i, p, false);
886 const double pos2 =
887 GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, false);
888 if (pos1 == (*(i - 1)).distanceTo2D(*i) && pos2 == 0.) {
889 nearestPos = seen;
890 minDist = cornerDist;
891 }
892 }
893 }
894 seen += (*i).distanceTo2D(*(i + 1));
895 }
896 return nearestPos;
897}
898
899
900double
901PositionVector::nearest_offset_to_point25D(const Position& p, bool perpendicular) const {
902 if (size() == 0) {
903 return INVALID_DOUBLE;
904 }
905 double minDist = std::numeric_limits<double>::max();
906 double nearestPos = GeomHelper::INVALID_OFFSET;
907 double seen = 0;
908 for (const_iterator i = begin(); i != end() - 1; i++) {
909 const double pos =
910 GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, perpendicular);
911 const double dist = pos == GeomHelper::INVALID_OFFSET ? minDist : p.distanceTo2D(positionAtOffset2D(*i, *(i + 1), pos));
912 if (dist < minDist) {
913 const double pos25D = pos * (*i).distanceTo(*(i + 1)) / (*i).distanceTo2D(*(i + 1));
914 nearestPos = pos25D + seen;
915 minDist = dist;
916 }
917 if (perpendicular && i != begin() && pos == GeomHelper::INVALID_OFFSET) {
918 // even if perpendicular is set we still need to check the distance to the inner points
919 const double cornerDist = p.distanceTo2D(*i);
920 if (cornerDist < minDist) {
921 const double pos1 =
922 GeomHelper::nearest_offset_on_line_to_point2D(*(i - 1), *i, p, false);
923 const double pos2 =
924 GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, false);
925 if (pos1 == (*(i - 1)).distanceTo2D(*i) && pos2 == 0.) {
926 nearestPos = seen;
927 minDist = cornerDist;
928 }
929 }
930 }
931 seen += (*i).distanceTo(*(i + 1));
932 }
933 return nearestPos;
934}
935
936
939 if (size() == 0) {
940 return Position::INVALID;
941 }
942 // @toDo this duplicates most of the code in nearest_offset_to_point2D. It should be refactored
943 if (extend) {
944 PositionVector extended = *this;
945 const double dist = 2 * distance2D(p);
946 extended.extrapolate(dist);
947 return extended.transformToVectorCoordinates(p) - Position(dist, 0);
948 }
949 double minDist = std::numeric_limits<double>::max();
950 double nearestPos = -1;
951 double seen = 0;
952 int sign = 1;
953 for (const_iterator i = begin(); i != end() - 1; i++) {
954 const double pos =
956 const double dist = pos < 0 ? minDist : p.distanceTo2D(positionAtOffset(*i, *(i + 1), pos));
957 if (dist < minDist) {
958 nearestPos = pos + seen;
959 minDist = dist;
960 sign = isLeft(*i, *(i + 1), p) >= 0 ? -1 : 1;
961 }
962 if (i != begin() && pos == GeomHelper::INVALID_OFFSET) {
963 // even if perpendicular is set we still need to check the distance to the inner points
964 const double cornerDist = p.distanceTo2D(*i);
965 if (cornerDist < minDist) {
966 const double pos1 =
967 GeomHelper::nearest_offset_on_line_to_point2D(*(i - 1), *i, p, false);
968 const double pos2 =
969 GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, false);
970 if (pos1 == (*(i - 1)).distanceTo2D(*i) && pos2 == 0.) {
971 nearestPos = seen;
972 minDist = cornerDist;
973 sign = isLeft(*(i - 1), *i, p) >= 0 ? -1 : 1;
974 }
975 }
976 }
977 seen += (*i).distanceTo2D(*(i + 1));
978 }
979 if (nearestPos != -1) {
980 return Position(nearestPos, sign * minDist);
981 } else {
982 return Position::INVALID;
983 }
984}
985
986
987int
988PositionVector::indexOfClosest(const Position& p, bool twoD) const {
989 if (size() == 0) {
990 return -1;
991 }
992 double minDist = std::numeric_limits<double>::max();
993 double dist;
994 int closest = 0;
995 for (int i = 0; i < (int)size(); i++) {
996 const Position& p2 = (*this)[i];
997 dist = twoD ? p.distanceTo2D(p2) : p.distanceTo(p2);
998 if (dist < minDist) {
999 closest = i;
1000 minDist = dist;
1001 }
1002 }
1003 return closest;
1004}
1005
1006
1007int
1009 if (size() == 0) {
1010 return -1;
1011 }
1012 double minDist = std::numeric_limits<double>::max();
1013 int insertionIndex = 1;
1014 for (int i = 0; i < (int)size() - 1; i++) {
1015 const double length = GeomHelper::nearest_offset_on_line_to_point2D((*this)[i], (*this)[i + 1], p, false);
1016 const Position& outIntersection = PositionVector::positionAtOffset2D((*this)[i], (*this)[i + 1], length);
1017 const double dist = p.distanceTo2D(outIntersection);
1018 if (dist < minDist) {
1019 insertionIndex = i + 1;
1020 minDist = dist;
1021 }
1022 }
1023 // check if we have to adjust Position Z
1024 if (interpolateZ) {
1025 // obtain previous and next Z
1026 const double previousZ = (begin() + (insertionIndex - 1))->z();
1027 const double nextZ = (begin() + insertionIndex)->z();
1028 // insert new position using x and y of p, and the new z
1029 insert(begin() + insertionIndex, Position(p.x(), p.y(), ((previousZ + nextZ) / 2.0)));
1030 } else {
1031 insert(begin() + insertionIndex, p);
1032 }
1033 return insertionIndex;
1034}
1035
1036
1037int
1039 if (size() == 0) {
1040 return -1;
1041 }
1042 double minDist = std::numeric_limits<double>::max();
1043 int removalIndex = 0;
1044 for (int i = 0; i < (int)size(); i++) {
1045 const double dist = p.distanceTo2D((*this)[i]);
1046 if (dist < minDist) {
1047 removalIndex = i;
1048 minDist = dist;
1049 }
1050 }
1051 erase(begin() + removalIndex);
1052 return removalIndex;
1053}
1054
1055
1056std::vector<double>
1058 std::vector<double> ret;
1059 if (other.size() == 0) {
1060 return ret;
1061 }
1062 for (const_iterator i = other.begin(); i != other.end() - 1; i++) {
1063 std::vector<double> atSegment = intersectsAtLengths2D(*i, *(i + 1));
1064 copy(atSegment.begin(), atSegment.end(), back_inserter(ret));
1065 }
1066 return ret;
1067}
1068
1069
1070std::vector<double>
1072 std::vector<double> ret;
1073 if (size() == 0) {
1074 return ret;
1075 }
1076 double pos = 0;
1077 for (const_iterator i = begin(); i != end() - 1; i++) {
1078 const Position& p1 = *i;
1079 const Position& p2 = *(i + 1);
1080 double x, y, m;
1081 if (intersects(p1, p2, lp1, lp2, 0., &x, &y, &m)) {
1082 ret.push_back(Position(x, y).distanceTo2D(p1) + pos);
1083 }
1084 pos += p1.distanceTo2D(p2);
1085 }
1086 return ret;
1087}
1088
1089
1090void
1091PositionVector::extrapolate(const double val, const bool onlyFirst, const bool onlyLast) {
1092 if (size() > 0) {
1093 Position& p1 = (*this)[0];
1094 Position& p2 = (*this)[1];
1095 const Position offset = (p2 - p1) * (val / p1.distanceTo(p2));
1096 if (!onlyLast) {
1097 p1.sub(offset);
1098 }
1099 if (!onlyFirst) {
1100 if (size() == 2) {
1101 p2.add(offset);
1102 } else {
1103 const Position& e1 = (*this)[-2];
1104 Position& e2 = (*this)[-1];
1105 e2.sub((e1 - e2) * (val / e1.distanceTo(e2)));
1106 }
1107 }
1108 }
1109}
1110
1111
1112void
1113PositionVector::extrapolate2D(const double val, const bool onlyFirst) {
1114 if (size() > 0) {
1115 Position& p1 = (*this)[0];
1116 Position& p2 = (*this)[1];
1117 if (p1.distanceTo2D(p2) > 0) {
1118 const Position offset = (p2 - p1) * (val / p1.distanceTo2D(p2));
1119 p1.sub(offset);
1120 if (!onlyFirst) {
1121 if (size() == 2) {
1122 p2.add(offset);
1123 } else {
1124 const Position& e1 = (*this)[-2];
1125 Position& e2 = (*this)[-1];
1126 e2.sub((e1 - e2) * (val / e1.distanceTo2D(e2)));
1127 }
1128 }
1129 }
1130 }
1131}
1132
1133
1136 PositionVector ret;
1137 for (const_reverse_iterator i = rbegin(); i != rend(); i++) {
1138 ret.push_back(*i);
1139 }
1140 return ret;
1141}
1142
1143
1145PositionVector::sideOffset(const Position& beg, const Position& end, const double amount) {
1146 const double scale = amount / beg.distanceTo2D(end);
1147 return Position((beg.y() - end.y()) * scale, (end.x() - beg.x()) * scale);
1148}
1149
1150
1151void
1152PositionVector::move2side(double amount, double maxExtension) {
1153 if (size() < 2) {
1154 return;
1155 }
1156 removeDoublePoints(POSITION_EPS, true);
1157 if (length2D() == 0 || amount == 0) {
1158 return;
1159 }
1160 PositionVector shape;
1161 std::vector<int> recheck;
1162 for (int i = 0; i < static_cast<int>(size()); i++) {
1163 if (i == 0) {
1164 const Position& from = (*this)[i];
1165 const Position& to = (*this)[i + 1];
1166 if (from != to) {
1167 shape.push_back(from - sideOffset(from, to, amount));
1168#ifdef DEBUG_MOVE2SIDE
1169 if (gDebugFlag1) {
1170 std::cout << " " << i << "a=" << shape.back() << "\n";
1171 }
1172#endif
1173 }
1174 } else if (i == static_cast<int>(size()) - 1) {
1175 const Position& from = (*this)[i - 1];
1176 const Position& to = (*this)[i];
1177 if (from != to) {
1178 shape.push_back(to - sideOffset(from, to, amount));
1179#ifdef DEBUG_MOVE2SIDE
1180 if (gDebugFlag1) {
1181 std::cout << " " << i << "b=" << shape.back() << "\n";
1182 }
1183#endif
1184 }
1185 } else {
1186 const Position& from = (*this)[i - 1];
1187 const Position& me = (*this)[i];
1188 const Position& to = (*this)[i + 1];
1189 PositionVector fromMe(from, me);
1190 fromMe.extrapolate2D(me.distanceTo2D(to));
1191 const double extrapolateDev = fromMe[1].distanceTo2D(to);
1192 if (fabs(extrapolateDev) < POSITION_EPS) {
1193 // parallel case, just shift the middle point
1194 shape.push_back(me - sideOffset(from, to, amount));
1195#ifdef DEBUG_MOVE2SIDE
1196 if (gDebugFlag1) {
1197 std::cout << " " << i << "c=" << shape.back() << "\n";
1198 }
1199#endif
1200 } else if (fabs(extrapolateDev - 2 * me.distanceTo2D(to)) < POSITION_EPS) {
1201 // counterparallel case, just shift the middle point
1202 PositionVector fromMe2(from, me);
1203 fromMe2.extrapolate2D(amount);
1204 shape.push_back(fromMe2[1]);
1205#ifdef DEBUG_MOVE2SIDE
1206 if (gDebugFlag1) {
1207 std::cout << " " << i << "d=" << shape.back() << " " << i << "_from=" << from << " " << i << "_me=" << me << " " << i << "_to=" << to << "\n";
1208 }
1209#endif
1210 } else {
1211 Position offsets = sideOffset(from, me, amount);
1212 Position offsets2 = sideOffset(me, to, amount);
1213 PositionVector l1(from - offsets, me - offsets);
1214 PositionVector l2(me - offsets2, to - offsets2);
1215 Position meNew = l1.intersectionPosition2D(l2[0], l2[1], maxExtension);
1216 if (meNew == Position::INVALID) {
1217 recheck.push_back(i);
1218 continue;
1219 }
1220 meNew = meNew + Position(0, 0, me.z());
1221 shape.push_back(meNew);
1222#ifdef DEBUG_MOVE2SIDE
1223 if (gDebugFlag1) {
1224 std::cout << " " << i << "e=" << shape.back() << "\n";
1225 }
1226#endif
1227 }
1228 // copy original z value
1229 shape.back().set(shape.back().x(), shape.back().y(), me.z());
1230 const double angle = localAngle(from, me, to);
1231 if (fabs(angle) > NUMERICAL_EPS) {
1232 const double length = from.distanceTo2D(me) + me.distanceTo2D(to);
1233 const double radius = length / angle;
1234#ifdef DEBUG_MOVE2SIDE
1235 if (gDebugFlag1) {
1236 std::cout << " i=" << i << " a=" << RAD2DEG(angle) << " l=" << length << " r=" << radius << " t=" << amount * 1.8 << "\n";
1237 }
1238#endif
1239 if ((radius < 0 && -radius < amount * 1.8) || fabs(RAD2DEG(angle)) > 170) {
1240 recheck.push_back(i);
1241 }
1242 }
1243 }
1244 }
1245 if (!recheck.empty()) {
1246 // try to adjust positions to avoid clipping
1247 shape = *this;
1248 for (int i = (int)recheck.size() - 1; i >= 0; i--) {
1249 shape.erase(shape.begin() + recheck[i]);
1250 }
1251 shape.move2side(amount, maxExtension);
1252 }
1253 *this = shape;
1254}
1255
1256
1257void
1258PositionVector::move2sideCustom(std::vector<double> amount, double maxExtension) {
1259 if (size() < 2) {
1260 return;
1261 }
1262 if (length2D() == 0) {
1263 return;
1264 }
1265 if (size() != amount.size()) {
1266 throw InvalidArgument("Numer of offsets (" + toString(amount.size())
1267 + ") does not match number of points (" + toString(size()) + ")");
1268 }
1269 PositionVector shape;
1270 for (int i = 0; i < static_cast<int>(size()); i++) {
1271 if (i == 0) {
1272 const Position& from = (*this)[i];
1273 const Position& to = (*this)[i + 1];
1274 if (from != to) {
1275 shape.push_back(from - sideOffset(from, to, amount[i]));
1276 }
1277 } else if (i == static_cast<int>(size()) - 1) {
1278 const Position& from = (*this)[i - 1];
1279 const Position& to = (*this)[i];
1280 if (from != to) {
1281 shape.push_back(to - sideOffset(from, to, amount[i]));
1282 }
1283 } else {
1284 const Position& from = (*this)[i - 1];
1285 const Position& me = (*this)[i];
1286 const Position& to = (*this)[i + 1];
1287 PositionVector fromMe(from, me);
1288 fromMe.extrapolate2D(me.distanceTo2D(to));
1289 const double extrapolateDev = fromMe[1].distanceTo2D(to);
1290 if (fabs(extrapolateDev) < POSITION_EPS) {
1291 // parallel case, just shift the middle point
1292 shape.push_back(me - sideOffset(from, to, amount[i]));
1293 } else if (fabs(extrapolateDev - 2 * me.distanceTo2D(to)) < POSITION_EPS) {
1294 // counterparallel case, just shift the middle point
1295 PositionVector fromMe2(from, me);
1296 fromMe2.extrapolate2D(amount[i]);
1297 shape.push_back(fromMe2[1]);
1298 } else {
1299 Position offsets = sideOffset(from, me, amount[i]);
1300 Position offsets2 = sideOffset(me, to, amount[i]);
1301 PositionVector l1(from - offsets, me - offsets);
1302 PositionVector l2(me - offsets2, to - offsets2);
1303 Position meNew = l1.intersectionPosition2D(l2[0], l2[1], maxExtension);
1304 if (meNew == Position::INVALID) {
1305 continue;
1306 }
1307 meNew = meNew + Position(0, 0, me.z());
1308 shape.push_back(meNew);
1309 }
1310 // copy original z value
1311 shape.back().set(shape.back().x(), shape.back().y(), me.z());
1312 }
1313 }
1314 *this = shape;
1315}
1316
1317double
1318PositionVector::localAngle(const Position& from, const Position& pos, const Position& to) {
1319 return GeomHelper::angleDiff(from.angleTo2D(pos), pos.angleTo2D(to));
1320}
1321
1322double
1324 if ((pos + 1) < (int)size()) {
1325 return (*this)[pos].angleTo2D((*this)[pos + 1]);
1326 } else {
1327 return INVALID_DOUBLE;
1328 }
1329}
1330
1331
1332void
1334 if ((size() != 0) && ((*this)[0] != back())) {
1335 push_back((*this)[0]);
1336 }
1337}
1338
1339
1340std::vector<double>
1341PositionVector::distances(const PositionVector& s, bool perpendicular) const {
1342 std::vector<double> ret;
1343 const_iterator i;
1344 for (i = begin(); i != end(); i++) {
1345 const double dist = s.distance2D(*i, perpendicular);
1346 if (dist != GeomHelper::INVALID_OFFSET) {
1347 ret.push_back(dist);
1348 }
1349 }
1350 for (i = s.begin(); i != s.end(); i++) {
1351 const double dist = distance2D(*i, perpendicular);
1352 if (dist != GeomHelper::INVALID_OFFSET) {
1353 ret.push_back(dist);
1354 }
1355 }
1356 return ret;
1357}
1358
1359
1360double
1361PositionVector::distance2D(const Position& p, bool perpendicular) const {
1362 if (size() == 0) {
1363 return std::numeric_limits<double>::max();
1364 } else if (size() == 1) {
1365 return front().distanceTo(p);
1366 }
1367 const double nearestOffset = nearest_offset_to_point2D(p, perpendicular);
1368 if (nearestOffset == GeomHelper::INVALID_OFFSET) {
1370 } else {
1371 return p.distanceTo2D(positionAtOffset2D(nearestOffset));
1372 }
1373}
1374
1375
1376void
1378 if (empty()) {
1379 push_back(p);
1380 } else {
1381 insert(begin(), p);
1382 }
1383}
1384
1385
1386void
1388 if (empty()) {
1389 throw ProcessError("PositionVector is empty");
1390 } else {
1391 erase(begin());
1392 }
1393}
1394
1395
1396void
1398 if (size() == 0 || !p.almostSame(back())) {
1399 push_back(p);
1400 }
1401}
1402
1403
1404void
1406 if ((size() == 0) || !p.almostSame(front())) {
1407 push_front(p);
1408 }
1409}
1410
1411
1412void
1413PositionVector::insert_noDoublePos(const std::vector<Position>::iterator& at, const Position& p) {
1414 if (at == begin()) {
1416 } else if (at == end()) {
1418 } else {
1419 if (!p.almostSame(*at) && !p.almostSame(*(at - 1))) {
1420 insert(at, p);
1421 }
1422 }
1423}
1424
1425
1426bool
1428 return (size() >= 2) && ((*this)[0] == back());
1429}
1430
1431
1432bool
1434 // iterate over all positions and check if is NAN
1435 for (auto i = begin(); i != end(); i++) {
1436 if (i->isNAN()) {
1437 return true;
1438 }
1439 }
1440 // all ok, then return false
1441 return false;
1442}
1443
1444
1445void
1446PositionVector::removeDoublePoints(double minDist, bool assertLength, int beginOffset, int endOffset, bool resample) {
1447 int curSize = (int)size() - beginOffset - endOffset;
1448 if (curSize > 1) {
1449 iterator last = begin() + beginOffset;
1450 for (iterator i = last + 1; i != (end() - endOffset) && (!assertLength || curSize > 2);) {
1451 if (last->almostSame(*i, minDist)) {
1452 if (i + 1 == end() - endOffset) {
1453 // special case: keep the last point and remove the next-to-last
1454 if (resample && last > begin() && (last - 1)->distanceTo(*i) >= 2 * minDist) {
1455 // resample rather than remove point after a long segment
1456 const double shiftBack = minDist - last->distanceTo(*i);
1457 //if (gDebugFlag1) std::cout << " resample endOffset beforeLast=" << *(last - 1) << " last=" << *last << " i=" << *i;
1458 (*last) = positionAtOffset(*(last - 1), *last, (last - 1)->distanceTo(*last) - shiftBack);
1459 //if (gDebugFlag1) std::cout << " lastNew=" << *last;
1460 last = i;
1461 ++i;
1462 } else {
1463 erase(last);
1464 i = end() - endOffset;
1465 }
1466 } else {
1467 if (resample && i + 1 != end() && last->distanceTo(*(i + 1)) >= 2 * minDist) {
1468 // resample rather than remove points before a long segment
1469 const double shiftForward = minDist - last->distanceTo(*i);
1470 //if (gDebugFlag1) std::cout << " resample last=" << *last << " i=" << *i << " next=" << *(i + 1);
1471 (*i) = positionAtOffset(*i, *(i + 1), shiftForward);
1472 //if (gDebugFlag1) std::cout << " iNew=" << *i << "\n";
1473 last = i;
1474 ++i;
1475 } else {
1476 i = erase(i);
1477 }
1478 }
1479 curSize--;
1480 } else {
1481 last = i;
1482 ++i;
1483 }
1484 }
1485 }
1486}
1487
1488
1489bool
1491 return static_cast<vp>(*this) == static_cast<vp>(v2);
1492}
1493
1494
1495bool
1497 return static_cast<vp>(*this) != static_cast<vp>(v2);
1498}
1499
1502 if (length() != v2.length()) {
1503 WRITE_ERROR(TL("Trying to subtract PositionVectors of different lengths."));
1504 }
1505 PositionVector pv;
1506 auto i1 = begin();
1507 auto i2 = v2.begin();
1508 while (i1 != end()) {
1509 pv.add(*i1 - *i2);
1510 }
1511 return pv;
1512}
1513
1516 if (length() != v2.length()) {
1517 WRITE_ERROR(TL("Trying to subtract PositionVectors of different lengths."));
1518 }
1519 PositionVector pv;
1520 auto i1 = begin();
1521 auto i2 = v2.begin();
1522 while (i1 != end()) {
1523 pv.add(*i1 + *i2);
1524 }
1525 return pv;
1526}
1527
1528bool
1529PositionVector::almostSame(const PositionVector& v2, double maxDiv) const {
1530 if (size() != v2.size()) {
1531 return false;
1532 }
1533 auto i2 = v2.begin();
1534 for (auto i1 = begin(); i1 != end(); i1++) {
1535 if (!i1->almostSame(*i2, maxDiv)) {
1536 return false;
1537 }
1538 i2++;
1539 }
1540 return true;
1541}
1542
1543bool
1545 if (size() < 2) {
1546 return false;
1547 }
1548 for (const_iterator i = begin(); i != end(); i++) {
1549 if ((*i).z() != 0) {
1550 return true;
1551 }
1552 }
1553 return false;
1554}
1555
1556
1557bool
1558PositionVector::intersects(const Position& p11, const Position& p12, const Position& p21, const Position& p22, const double withinDist, double* x, double* y, double* mu) {
1559 const double eps = std::numeric_limits<double>::epsilon();
1560 const double denominator = (p22.y() - p21.y()) * (p12.x() - p11.x()) - (p22.x() - p21.x()) * (p12.y() - p11.y());
1561 const double numera = (p22.x() - p21.x()) * (p11.y() - p21.y()) - (p22.y() - p21.y()) * (p11.x() - p21.x());
1562 const double numerb = (p12.x() - p11.x()) * (p11.y() - p21.y()) - (p12.y() - p11.y()) * (p11.x() - p21.x());
1563 /* Are the lines coincident? */
1564 if (fabs(numera) < eps && fabs(numerb) < eps && fabs(denominator) < eps) {
1565 double a1;
1566 double a2;
1567 double a3;
1568 double a4;
1569 double a = -1e12;
1570 if (p11.x() != p12.x()) {
1571 a1 = p11.x() < p12.x() ? p11.x() : p12.x();
1572 a2 = p11.x() < p12.x() ? p12.x() : p11.x();
1573 a3 = p21.x() < p22.x() ? p21.x() : p22.x();
1574 a4 = p21.x() < p22.x() ? p22.x() : p21.x();
1575 } else {
1576 a1 = p11.y() < p12.y() ? p11.y() : p12.y();
1577 a2 = p11.y() < p12.y() ? p12.y() : p11.y();
1578 a3 = p21.y() < p22.y() ? p21.y() : p22.y();
1579 a4 = p21.y() < p22.y() ? p22.y() : p21.y();
1580 }
1581 if (a1 <= a3 && a3 <= a2) {
1582 if (a4 < a2) {
1583 a = (a3 + a4) / 2;
1584 } else {
1585 a = (a2 + a3) / 2;
1586 }
1587 }
1588 if (a3 <= a1 && a1 <= a4) {
1589 if (a2 < a4) {
1590 a = (a1 + a2) / 2;
1591 } else {
1592 a = (a1 + a4) / 2;
1593 }
1594 }
1595 if (a != -1e12) {
1596 if (x != nullptr) {
1597 if (p11.x() != p12.x()) {
1598 *mu = (a - p11.x()) / (p12.x() - p11.x());
1599 *x = a;
1600 *y = p11.y() + (*mu) * (p12.y() - p11.y());
1601 } else {
1602 *x = p11.x();
1603 *y = a;
1604 if (p12.y() == p11.y()) {
1605 *mu = 0;
1606 } else {
1607 *mu = (a - p11.y()) / (p12.y() - p11.y());
1608 }
1609 }
1610 }
1611 return true;
1612 }
1613 return false;
1614 }
1615 /* Are the lines parallel */
1616 if (fabs(denominator) < eps) {
1617 return false;
1618 }
1619 /* Is the intersection along the segments */
1620 double mua = numera / denominator;
1621 /* reduce rounding errors for lines ending in the same point */
1622 if (fabs(p12.x() - p22.x()) < eps && fabs(p12.y() - p22.y()) < eps) {
1623 mua = 1.;
1624 } else {
1625 const double offseta = withinDist / p11.distanceTo2D(p12);
1626 const double offsetb = withinDist / p21.distanceTo2D(p22);
1627 const double mub = numerb / denominator;
1628 if (mua < -offseta || mua > 1 + offseta || mub < -offsetb || mub > 1 + offsetb) {
1629 return false;
1630 }
1631 }
1632 if (x != nullptr) {
1633 *x = p11.x() + mua * (p12.x() - p11.x());
1634 *y = p11.y() + mua * (p12.y() - p11.y());
1635 *mu = mua;
1636 }
1637 return true;
1638}
1639
1640
1641void
1643 const double s = sin(angle);
1644 const double c = cos(angle);
1645 for (int i = 0; i < (int)size(); i++) {
1646 const double x = (*this)[i].x();
1647 const double y = (*this)[i].y();
1648 const double z = (*this)[i].z();
1649 const double xnew = x * c - y * s;
1650 const double ynew = x * s + y * c;
1651 (*this)[i].set(xnew, ynew, z);
1652 }
1653}
1654
1655
1658 PositionVector result = *this;
1659 bool changed = true;
1660 while (changed && result.size() > 3) {
1661 changed = false;
1662 for (int i = 0; i < (int)result.size(); i++) {
1663 const Position& p1 = result[i];
1664 const Position& p2 = result[(i + 2) % result.size()];
1665 const int middleIndex = (i + 1) % result.size();
1666 const Position& p0 = result[middleIndex];
1667 // https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line#Line_defined_by_two_points
1668 const double triangleArea2 = fabs((p2.y() - p1.y()) * p0.x() - (p2.x() - p1.x()) * p0.y() + p2.x() * p1.y() - p2.y() * p1.x());
1669 const double distIK = p1.distanceTo2D(p2);
1670 if (distIK > NUMERICAL_EPS && triangleArea2 / distIK < NUMERICAL_EPS) {
1671 changed = true;
1672 result.erase(result.begin() + middleIndex);
1673 break;
1674 }
1675 }
1676 }
1677 return result;
1678}
1679
1680
1682PositionVector::getOrthogonal(const Position& p, double extend, bool before, double length, double deg) const {
1683 PositionVector result;
1684 PositionVector tmp = *this;
1685 tmp.extrapolate2D(extend);
1686 const double baseOffset = tmp.nearest_offset_to_point2D(p);
1687 if (baseOffset == GeomHelper::INVALID_OFFSET || size() < 2) {
1688 // fail
1689 return result;
1690 }
1691 Position base = tmp.positionAtOffset2D(baseOffset);
1692 const int closestIndex = tmp.indexOfClosest(base);
1693 const double closestOffset = tmp.offsetAtIndex2D(closestIndex);
1694 result.push_back(base);
1695 if (fabs(baseOffset - closestOffset) > NUMERICAL_EPS) {
1696 result.push_back(tmp[closestIndex]);
1697 if ((closestOffset < baseOffset) != before) {
1698 deg *= -1;
1699 }
1700 } else if (before) {
1701 // take the segment before closestIndex if possible
1702 if (closestIndex > 0) {
1703 result.push_back(tmp[closestIndex - 1]);
1704 } else {
1705 result.push_back(tmp[1]);
1706 deg *= -1;
1707 }
1708 } else {
1709 // take the segment after closestIndex if possible
1710 if (closestIndex < (int)size() - 1) {
1711 result.push_back(tmp[closestIndex + 1]);
1712 } else {
1713 result.push_back(tmp[-1]);
1714 deg *= -1;
1715 }
1716 }
1717 result = result.getSubpart2D(0, length);
1718 // rotate around base
1719 result.add(base * -1);
1720 result.rotate2D(DEG2RAD(deg));
1721 result.add(base);
1722 return result;
1723}
1724
1725
1728 PositionVector result = *this;
1729 if (size() == 0) {
1730 return result;
1731 }
1732 const double z0 = (*this)[0].z();
1733 // the z-delta of the first segment
1734 const double dz = (*this)[1].z() - z0;
1735 // if the shape only has 2 points it is as smooth as possible already
1736 if (size() > 2 && dz != 0) {
1737 dist = MIN2(dist, length2D());
1738 // check wether we need to insert a new point at dist
1739 Position pDist = positionAtOffset2D(dist);
1740 int iLast = indexOfClosest(pDist);
1741 // prevent close spacing to reduce impact of rounding errors in z-axis
1742 if (pDist.distanceTo2D((*this)[iLast]) > POSITION_EPS * 20) {
1743 iLast = result.insertAtClosest(pDist, false);
1744 }
1745 double dist2 = result.offsetAtIndex2D(iLast);
1746 const double dz2 = result[iLast].z() - z0;
1747 double seen = 0;
1748 for (int i = 1; i < iLast; ++i) {
1749 seen += result[i].distanceTo2D(result[i - 1]);
1750 result[i].set(result[i].x(), result[i].y(), z0 + dz2 * seen / dist2);
1751 }
1752 }
1753 return result;
1754
1755}
1756
1757
1759PositionVector::interpolateZ(double zStart, double zEnd) const {
1760 PositionVector result = *this;
1761 if (size() == 0) {
1762 return result;
1763 }
1764 result[0].setz(zStart);
1765 result[-1].setz(zEnd);
1766 const double dz = zEnd - zStart;
1767 const double length = length2D();
1768 double seen = 0;
1769 for (int i = 1; i < (int)size() - 1; ++i) {
1770 seen += result[i].distanceTo2D(result[i - 1]);
1771 result[i].setz(zStart + dz * seen / length);
1772 }
1773 return result;
1774}
1775
1776
1778PositionVector::resample(double maxLength, const bool adjustEnd) const {
1779 PositionVector result;
1780 if (maxLength == 0) {
1781 return result;
1782 }
1783 const double length = length2D();
1784 if (length < POSITION_EPS) {
1785 return result;
1786 }
1787 maxLength = length / ceil(length / maxLength);
1788 for (double pos = 0; pos <= length; pos += maxLength) {
1789 result.push_back(positionAtOffset2D(pos));
1790 }
1791 // check if we have to adjust last element
1792 if (adjustEnd && !result.empty() && (result.back() != back())) {
1793 // add last element
1794 result.push_back(back());
1795 }
1796 return result;
1797}
1798
1799
1800double
1802 if (index < 0 || index >= (int)size()) {
1804 }
1805 double seen = 0;
1806 for (int i = 1; i <= index; ++i) {
1807 seen += (*this)[i].distanceTo2D((*this)[i - 1]);
1808 }
1809 return seen;
1810}
1811
1812
1813double
1814PositionVector::getMaxGrade(double& maxJump) const {
1815 double result = 0;
1816 for (int i = 1; i < (int)size(); ++i) {
1817 const Position& p1 = (*this)[i - 1];
1818 const Position& p2 = (*this)[i];
1819 const double distZ = fabs(p1.z() - p2.z());
1820 const double dist2D = p1.distanceTo2D(p2);
1821 if (dist2D == 0) {
1822 maxJump = MAX2(maxJump, distZ);
1823 } else {
1824 result = MAX2(result, distZ / dist2D);
1825 }
1826 }
1827 return result;
1828}
1829
1830
1833 // inspired by David F. Rogers
1834 assert(size() < 33);
1835 static const double fac[33] = {
1836 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,
1837 6227020800.0, 87178291200.0, 1307674368000.0, 20922789888000.0, 355687428096000.0, 6402373705728000.0,
1838 121645100408832000.0, 2432902008176640000.0, 51090942171709440000.0, 1124000727777607680000.0,
1839 25852016738884976640000.0, 620448401733239439360000.0, 15511210043330985984000000.0,
1840 403291461126605635584000000.0, 10888869450418352160768000000.0, 304888344611713860501504000000.0,
1841 8841761993739701954543616000000.0, 265252859812191058636308480000000.0,
1842 8222838654177922817725562880000000.0, 263130836933693530167218012160000000.0
1843 };
1844 PositionVector ret;
1845 const int npts = (int)size();
1846 // calculate the points on the Bezier curve
1847 const double step = (double) 1.0 / (numPoints - 1);
1848 double t = 0.;
1849 Position prev;
1850 for (int i1 = 0; i1 < numPoints; i1++) {
1851 if ((1.0 - t) < 5e-6) {
1852 t = 1.0;
1853 }
1854 double x = 0., y = 0., z = 0.;
1855 for (int i = 0; i < npts; i++) {
1856 const double ti = (i == 0) ? 1.0 : pow(t, i);
1857 const double tni = (npts == i + 1) ? 1.0 : pow(1 - t, npts - i - 1);
1858 const double basis = fac[npts - 1] / (fac[i] * fac[npts - 1 - i]) * ti * tni;
1859 x += basis * at(i).x();
1860 y += basis * at(i).y();
1861 z += basis * at(i).z();
1862 }
1863 t += step;
1864 Position current(x, y, z);
1865 if (prev != current && !std::isnan(x) && !std::isnan(y) && !std::isnan(z)) {
1866 ret.push_back(current);
1867 }
1868 prev = current;
1869 }
1870 return ret;
1871}
1872
1873
1874/****************************************************************************/
#define DEG2RAD(x)
Definition GeomHelper.h:35
#define RAD2DEG(x)
Definition GeomHelper.h:36
#define WRITE_WARNINGF(...)
Definition MsgHandler.h:271
#define WRITE_ERROR(msg)
Definition MsgHandler.h:279
#define TL(string)
Definition MsgHandler.h:287
std::ostream & operator<<(std::ostream &os, const PositionVector &geom)
bool gDebugFlag1
global utility flags for debugging
Definition StdDefs.cpp:35
const double INVALID_DOUBLE
invalid double
Definition StdDefs.h:64
T MIN2(T a, T b)
Definition StdDefs.h:76
T MAX2(T a, T b)
Definition StdDefs.h:82
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
Definition ToString.h:46
virtual bool partialWithin(const AbstractPoly &poly, double offset=0) const =0
Returns whether the AbstractPoly is partially within the given polygon.
virtual bool crosses(const Position &p1, const Position &p2) const =0
Returns whether the AbstractPoly crosses the given line.
virtual bool around(const Position &p, double offset=0) const =0
Returns whether the AbstractPoly the given coordinate.
A class that stores a 2D geometrical boundary.
Definition Boundary.h:39
void add(double x, double y, double z=0)
Makes the boundary include the given coordinate.
Definition Boundary.cpp:78
static double angle2D(const Position &p1, const Position &p2)
Returns the angle between two vectors on a plane The angle is from vector 1 to vector 2,...
static const double INVALID_OFFSET
a value to signify offsets outside the range of [0, Line.length()]
Definition GeomHelper.h:50
static double nearest_offset_on_line_to_point2D(const Position &lineStart, const Position &lineEnd, const Position &p, bool perpendicular=true)
static double legacyDegree(const double angle, const bool positive=false)
static double angleDiff(const double angle1, const double angle2)
Returns the difference of the second angle to the first angle in radiants.
A point in 2D or 3D with translation and scaling methods.
Definition Position.h:37
double slopeTo2D(const Position &other) const
returns the slope of the vector pointing from here to the other position
Definition Position.h:269
static const Position INVALID
used to indicate that a position is valid
Definition Position.h:300
double distanceTo2D(const Position &p2) const
returns the euclidean distance in the x-y-plane
Definition Position.h:254
double distanceTo(const Position &p2) const
returns the euclidean distance in 3 dimension
Definition Position.h:244
void sub(double dx, double dy)
Subtracts the given position from this one.
Definition Position.h:145
double x() const
Returns the x-position.
Definition Position.h:55
void add(const Position &pos)
Adds the given position to this one.
Definition Position.h:125
double z() const
Returns the z-position.
Definition Position.h:65
double angleTo2D(const Position &other) const
returns the angle in the plane of the vector pointing from here to the other position
Definition Position.h:264
bool almostSame(const Position &p2, double maxDiv=POSITION_EPS) const
check if two position is almost the sme as other
Definition Position.h:239
double y() const
Returns the y-position.
Definition Position.h:60
int operator()(const Position &p1, const Position &p2) const
comparing operation for sort
clase for increasing Sorter
int operator()(const Position &p1, const Position &p2) const
comparing operation
A list of positions.
PositionVector operator-(const PositionVector &v2) const
subtracts two vectors (requires vectors of the same length)
void scaleAbsolute(double offset)
enlarges/shrinks the polygon by an absolute offset based at the centroid
double length2D() const
Returns the length.
void append(const PositionVector &v, double sameThreshold=2.0)
bool overlapsWith(const AbstractPoly &poly, double offset=0) const
Returns the information whether the given polygon overlaps with this.
PositionVector added(const Position &offset) const
double isLeft(const Position &P0, const Position &P1, const Position &P2) const
get left
double beginEndAngle() const
returns the angle in radians of the line connecting the first and the last position
double length() const
Returns the length.
void move2sideCustom(std::vector< double > amount, double maxExtension=100)
move position vector to side using a custom offset for each geometry point
void sortAsPolyCWByAngle()
sort as polygon CW by angle
PositionVector simplified() const
return the same shape with intermediate colinear points removed
void rotate2D(double angle)
PositionVector()
Constructor. Creates an empty position vector.
Position getPolygonCenter() const
Returns the arithmetic of all corner points.
Position intersectionPosition2D(const Position &p1, const Position &p2, const double withinDist=0.) const
Returns the position of the intersection.
const Position & operator[](int index) const
returns the constant position at the given index, negative indices are interpreted python style
double rotationAtOffset(double pos) const
Returns the rotation at the given length.
std::vector< Position > vp
vector of position
void push_front_noDoublePos(const Position &p)
insert in front a non double position
bool operator!=(const PositionVector &v2) const
comparing operation
PositionVector resample(double maxLength, const bool adjustEnd) const
resample shape (i.e. transform to segments, equal spacing)
void sortByIncreasingXY()
sort by increasing X-Y Positions
double rotationDegreeAtOffset(double pos) const
Returns the rotation at the given length.
bool isNAN() const
check if PositionVector is NAN
Position positionAtOffset(double pos, double lateralOffset=0) const
Returns the position at the given length.
void add(double xoff, double yoff, double zoff)
void closePolygon()
ensures that the last position equals the first
static Position sideOffset(const Position &beg, const Position &end, const double amount)
get a side position of position vector using a offset
std::vector< double > intersectsAtLengths2D(const PositionVector &other) const
For all intersections between this vector and other, return the 2D-length of the subvector from this ...
double distance2D(const Position &p, bool perpendicular=false) const
closest 2D-distance to point p (or -1 if perpendicular is true and the point is beyond this vector)
void prepend(const PositionVector &v, double sameThreshold=2.0)
double nearest_offset_to_point2D(const Position &p, bool perpendicular=true) const
return the nearest offest to point 2D
std::vector< double > distances(const PositionVector &s, bool perpendicular=false) const
distances of all my points to s and all of s points to myself
PositionVector getOrthogonal(const Position &p, double extend, bool before, double length=1.0, double deg=90) const
return orthogonal through p (extending this vector if necessary)
int indexOfClosest(const Position &p, bool twoD=false) const
std::pair< PositionVector, PositionVector > splitAt(double where, bool use2D=false) const
Returns the two lists made when this list vector is splitted at the given point.
void move2side(double amount, double maxExtension=100)
move position vector to side using certain ammount
bool almostSame(const PositionVector &v2, double maxDiv=POSITION_EPS) const
check if the two vectors have the same length and pairwise similar positions
bool crosses(const Position &p1, const Position &p2) const
Returns whether the AbstractPoly crosses the given line.
PositionVector getSubpart2D(double beginOffset, double endOffset) const
get subpart of a position vector in two dimensions (Z is ignored)
PositionVector interpolateZ(double zStart, double zEnd) const
returned vector that varies z smoothly over its length
Boundary getBoxBoundary() const
Returns a boundary enclosing this list of lines.
double offsetAtIndex2D(int index) const
return the offset at the given index
PositionVector smoothedZFront(double dist=std::numeric_limits< double >::max()) const
returned vector that is smoothed at the front (within dist)
double angleAt2D(int pos) const
get angle in certain position of position vector
void insert_noDoublePos(const std::vector< Position >::iterator &at, const Position &p)
insert in front a non double position
double slopeDegreeAtOffset(double pos) const
Returns the slope at the given length.
bool hasElevation() const
return whether two positions differ in z-coordinate
static const PositionVector EMPTY
empty Vector
void extrapolate(const double val, const bool onlyFirst=false, const bool onlyLast=false)
extrapolate position vector
PositionVector bezier(int numPoints)
return a bezier interpolation
Position getLineCenter() const
get line center
Position getCentroid() const
Returns the centroid (closes the polygon if unclosed)
double getOverlapWith(const PositionVector &poly, double zThreshold) const
Returns the maximum overlaps between this and the given polygon (when not separated by at least zThre...
PositionVector operator+(const PositionVector &v2) const
adds two vectors (requires vectors of the same length)
void extrapolate2D(const double val, const bool onlyFirst=false)
extrapolate position vector in two dimensions (Z is ignored)
int insertAtClosest(const Position &p, bool interpolateZ)
inserts p between the two closest positions
void push_front(const Position &p)
insert in front a Position
void scaleRelative(double factor)
enlarges/shrinks the polygon by a factor based at the centroid
void push_back_noDoublePos(const Position &p)
insert in back a non double position
void removeDoublePoints(double minDist=POSITION_EPS, bool assertLength=false, int beginOffset=0, int endOffset=0, bool resample=false)
Removes positions if too near.
bool partialWithin(const AbstractPoly &poly, double offset=0) const
Returns the information whether this polygon lies partially within the given polygon.
double getMaxGrade(double &maxJump) const
double area() const
Returns the area (0 for non-closed)
bool isClosed() const
check if PositionVector is closed
void pop_front()
pop first Position
double nearest_offset_to_point25D(const Position &p, bool perpendicular=true) const
return the nearest offest to point 2D projected onto the 3D geometry
int removeClosest(const Position &p)
removes the point closest to p and return the removal index
static double localAngle(const Position &from, const Position &pos, const Position &to)
Position sidePositionAtAngle(double pos, double lateralOffset, double angle) const
bool intersects(const Position &p1, const Position &p2) const
Returns the information whether this list of points interesects the given line.
PositionVector reverse() const
reverse position vector
PositionVector getSubpartByIndex(int beginIndex, int count) const
get subpart of a position vector using index and a cout
Position positionAtOffset2D(double pos, double lateralOffset=0) const
Returns the position at the given length.
bool operator==(const PositionVector &v2) const
comparing operation
void sub(const Position &offset)
PositionVector getSubpart(double beginOffset, double endOffset) const
get subpart of a position vector
~PositionVector()
Destructor.
bool around(const Position &p, double offset=0) const
Returns the information whether the position vector describes a polygon lying around the given point.
Position transformToVectorCoordinates(const Position &p, bool extend=false) const
return position p within the length-wise coordinate system defined by this position vector....
#define M_PI
Definition odrSpiral.cpp:45