Eclipse SUMO - Simulation of Urban MObility
PositionVector.cpp
Go to the documentation of this file.
1/****************************************************************************/
2// Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
3// Copyright (C) 2001-2022 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::positionAtOffset2D(double pos, double lateralOffset) 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).distanceTo2D(*(i + 1));
285 if (seenLength + nextLength > pos) {
286 return positionAtOffset2D(*i, *(i + 1), pos - seenLength, lateralOffset);
287 }
288 seenLength += nextLength;
289 } while (++i != end() - 1);
290 return back();
291}
292
293
294double
296 if ((size() == 0) || (size() == 1)) {
297 return INVALID_DOUBLE;
298 }
299 if (pos < 0) {
300 pos += length();
301 }
302 const_iterator i = begin();
303 double seenLength = 0;
304 do {
305 const Position& p1 = *i;
306 const Position& p2 = *(i + 1);
307 const double nextLength = p1.distanceTo(p2);
308 if (seenLength + nextLength > pos) {
309 return p1.angleTo2D(p2);
310 }
311 seenLength += nextLength;
312 } while (++i != end() - 1);
313 const Position& p1 = (*this)[-2];
314 const Position& p2 = back();
315 return p1.angleTo2D(p2);
316}
317
318
319double
322}
323
324
325double
327 if (size() == 0) {
328 return INVALID_DOUBLE;
329 }
330 const_iterator i = begin();
331 double seenLength = 0;
332 do {
333 const Position& p1 = *i;
334 const Position& p2 = *(i + 1);
335 const double nextLength = p1.distanceTo(p2);
336 if (seenLength + nextLength > pos) {
337 return RAD2DEG(p1.slopeTo2D(p2));
338 }
339 seenLength += nextLength;
340 } while (++i != end() - 1);
341 const Position& p1 = (*this)[-2];
342 const Position& p2 = back();
343 return RAD2DEG(p1.slopeTo2D(p2));
344}
345
346
348PositionVector::positionAtOffset(const Position& p1, const Position& p2, double pos, double lateralOffset) {
349 const double dist = p1.distanceTo(p2);
350 if (pos < 0. || dist < pos) {
351 return Position::INVALID;
352 }
353 if (lateralOffset != 0) {
354 if (dist == 0.) {
355 return Position::INVALID;
356 }
357 const Position offset = sideOffset(p1, p2, -lateralOffset); // move in the same direction as Position::move2side
358 if (pos == 0.) {
359 return p1 + offset;
360 }
361 return p1 + (p2 - p1) * (pos / dist) + offset;
362 }
363 if (pos == 0.) {
364 return p1;
365 }
366 return p1 + (p2 - p1) * (pos / dist);
367}
368
369
371PositionVector::positionAtOffset2D(const Position& p1, const Position& p2, double pos, double lateralOffset) {
372 const double dist = p1.distanceTo2D(p2);
373 if (pos < 0 || dist < pos) {
374 return Position::INVALID;
375 }
376 if (lateralOffset != 0) {
377 const Position offset = sideOffset(p1, p2, -lateralOffset); // move in the same direction as Position::move2side
378 if (pos == 0.) {
379 return p1 + offset;
380 }
381 return p1 + (p2 - p1) * (pos / dist) + offset;
382 }
383 if (pos == 0.) {
384 return p1;
385 }
386 return p1 + (p2 - p1) * (pos / dist);
387}
388
389
392 Boundary ret;
393 for (const Position& i : *this) {
394 ret.add(i);
395 }
396 return ret;
397}
398
399
402 if (size() == 0) {
403 return Position::INVALID;
404 }
405 double x = 0;
406 double y = 0;
407 double z = 0;
408 for (const Position& i : *this) {
409 x += i.x();
410 y += i.y();
411 z += i.z();
412 }
413 return Position(x / (double) size(), y / (double) size(), z / (double)size());
414}
415
416
419 if (size() == 0) {
420 return Position::INVALID;
421 } else if (size() == 1) {
422 return (*this)[0];
423 } else if (size() == 2) {
424 return ((*this)[0] + (*this)[1]) * 0.5;
425 }
426 PositionVector tmp = *this;
427 if (!isClosed()) { // make sure its closed
428 tmp.push_back(tmp[0]);
429 }
430 // shift to origin to increase numerical stability
431 Position offset = tmp[0];
432 Position result;
433 tmp.sub(offset);
434 const int endIndex = (int)tmp.size() - 1;
435 double div = 0; // 6 * area including sign
436 double x = 0;
437 double y = 0;
438 if (tmp.area() != 0) { // numerical instability ?
439 // http://en.wikipedia.org/wiki/Polygon
440 for (int i = 0; i < endIndex; i++) {
441 const double z = tmp[i].x() * tmp[i + 1].y() - tmp[i + 1].x() * tmp[i].y();
442 div += z; // area formula
443 x += (tmp[i].x() + tmp[i + 1].x()) * z;
444 y += (tmp[i].y() + tmp[i + 1].y()) * z;
445 }
446 div *= 3; // 6 / 2, the 2 comes from the area formula
447 result = Position(x / div, y / div);
448 } else {
449 // compute by decomposing into line segments
450 // http://en.wikipedia.org/wiki/Centroid#By_geometric_decomposition
451 double lengthSum = 0;
452 for (int i = 0; i < endIndex; i++) {
453 double length = tmp[i].distanceTo(tmp[i + 1]);
454 x += (tmp[i].x() + tmp[i + 1].x()) * length / 2;
455 y += (tmp[i].y() + tmp[i + 1].y()) * length / 2;
456 lengthSum += length;
457 }
458 if (lengthSum == 0) {
459 // it is probably only one point
460 result = tmp[0];
461 }
462 result = Position(x / lengthSum, y / lengthSum) + offset;
463 }
464 return result + offset;
465}
466
467
468void
470 Position centroid = getCentroid();
471 for (int i = 0; i < static_cast<int>(size()); i++) {
472 (*this)[i] = centroid + (((*this)[i] - centroid) * factor);
473 }
474}
475
476
477void
479 Position centroid = getCentroid();
480 for (int i = 0; i < static_cast<int>(size()); i++) {
481 (*this)[i] = centroid + (((*this)[i] - centroid) + offset);
482 }
483}
484
485
488 if (size() == 1) {
489 return (*this)[0];
490 } else {
491 return positionAtOffset(double((length() / 2.)));
492 }
493}
494
495
496double
498 if (size() == 0) {
499 return 0;
500 }
501 double len = 0;
502 for (const_iterator i = begin(); i != end() - 1; i++) {
503 len += (*i).distanceTo(*(i + 1));
504 }
505 return len;
506}
507
508
509double
511 if (size() == 0) {
512 return 0;
513 }
514 double len = 0;
515 for (const_iterator i = begin(); i != end() - 1; i++) {
516 len += (*i).distanceTo2D(*(i + 1));
517 }
518 return len;
519}
520
521
522double
524 if (size() < 3) {
525 return 0;
526 }
527 double area = 0;
528 PositionVector tmp = *this;
529 if (!isClosed()) { // make sure its closed
530 tmp.push_back(tmp[0]);
531 }
532 const int endIndex = (int)tmp.size() - 1;
533 // http://en.wikipedia.org/wiki/Polygon
534 for (int i = 0; i < endIndex; i++) {
535 area += tmp[i].x() * tmp[i + 1].y() - tmp[i + 1].x() * tmp[i].y();
536 }
537 if (area < 0) { // we whether we had cw or ccw order
538 area *= -1;
539 }
540 return area / 2;
541}
542
543
544bool
545PositionVector::partialWithin(const AbstractPoly& poly, double offset) const {
546 if (size() < 2) {
547 return false;
548 }
549 for (const_iterator i = begin(); i != end(); i++) {
550 if (poly.around(*i, offset)) {
551 return true;
552 }
553 }
554 return false;
555}
556
557
558bool
559PositionVector::crosses(const Position& p1, const Position& p2) const {
560 return intersects(p1, p2);
561}
562
563
564std::pair<PositionVector, PositionVector>
565PositionVector::splitAt(double where, bool use2D) const {
566 const double len = use2D ? length2D() : length();
567 if (size() < 2) {
568 throw InvalidArgument("Vector to short for splitting");
569 }
570 if (where < 0 || where > len) {
571 throw InvalidArgument("Invalid split position " + toString(where) + " for vector of length " + toString(len));
572 }
573 if (where <= POSITION_EPS || where >= len - POSITION_EPS) {
574 WRITE_WARNINGF(TL("Splitting vector close to end (pos: %, length: %)"), toString(where), toString(len));
575 }
576 PositionVector first, second;
577 first.push_back((*this)[0]);
578 double seen = 0;
579 const_iterator it = begin() + 1;
580 double next = use2D ? first.back().distanceTo2D(*it) : first.back().distanceTo(*it);
581 // see how many points we can add to first
582 while (where >= seen + next + POSITION_EPS) {
583 seen += next;
584 first.push_back(*it);
585 it++;
586 next = use2D ? first.back().distanceTo2D(*it) : first.back().distanceTo(*it);
587 }
588 if (fabs(where - (seen + next)) > POSITION_EPS || it == end() - 1) {
589 // we need to insert a new point because 'where' is not close to an
590 // existing point or it is to close to the endpoint
591 const Position p = (use2D
592 ? positionAtOffset2D(first.back(), *it, where - seen)
593 : positionAtOffset(first.back(), *it, where - seen));
594 first.push_back(p);
595 second.push_back(p);
596 } else {
597 first.push_back(*it);
598 }
599 // add the remaining points to second
600 for (; it != end(); it++) {
601 second.push_back(*it);
602 }
603 assert(first.size() >= 2);
604 assert(second.size() >= 2);
605 assert(first.back() == second.front());
606 assert(fabs((use2D ? first.length2D() + second.length2D() : first.length() + second.length()) - len) < 2 * POSITION_EPS);
607 return std::pair<PositionVector, PositionVector>(first, second);
608}
609
610
611std::ostream&
612operator<<(std::ostream& os, const PositionVector& geom) {
613 for (PositionVector::const_iterator i = geom.begin(); i != geom.end(); i++) {
614 if (i != geom.begin()) {
615 os << " ";
616 }
617 os << (*i);
618 }
619 return os;
620}
621
622
623void
625 std::sort(begin(), end(), as_poly_cw_sorter());
626}
627
628
629void
630PositionVector::add(double xoff, double yoff, double zoff) {
631 for (int i = 0; i < (int)size(); i++) {
632 (*this)[i].add(xoff, yoff, zoff);
633 }
634}
635
636
637void
639 add(-offset.x(), -offset.y(), -offset.z());
640}
641
642
643void
645 add(offset.x(), offset.y(), offset.z());
646}
647
648
650PositionVector::added(const Position& offset) const {
652 for (auto i1 = begin(); i1 != end(); ++i1) {
653 pv.push_back(*i1 + offset);
654 }
655 return pv;
656}
657
658
659void
661 for (int i = 0; i < (int)size(); i++) {
662 (*this)[i].mul(1, -1);
663 }
664}
665
666
668
669
670int
672 return atan2(p1.x(), p1.y()) < atan2(p2.x(), p2.y());
673}
674
675
676void
678 std::sort(begin(), end(), increasing_x_y_sorter());
679}
680
681
683
684
685int
687 if (p1.x() != p2.x()) {
688 return p1.x() < p2.x();
689 }
690 return p1.y() < p2.y();
691}
692
693
694double
695PositionVector::isLeft(const Position& P0, const Position& P1, const Position& P2) const {
696 return (P1.x() - P0.x()) * (P2.y() - P0.y()) - (P2.x() - P0.x()) * (P1.y() - P0.y());
697}
698
699
700void
701PositionVector::append(const PositionVector& v, double sameThreshold) {
702 if ((size() > 0) && (v.size() > 0) && (back().distanceTo(v[0]) < sameThreshold)) {
703 copy(v.begin() + 1, v.end(), back_inserter(*this));
704 } else {
705 copy(v.begin(), v.end(), back_inserter(*this));
706 }
707}
708
709
710void
711PositionVector::prepend(const PositionVector& v, double sameThreshold) {
712 if ((size() > 0) && (v.size() > 0) && (front().distanceTo(v.back()) < sameThreshold)) {
713 insert(begin(), v.begin(), v.end() - 1);
714 } else {
715 insert(begin(), v.begin(), v.end());
716 }
717}
718
719
721PositionVector::getSubpart(double beginOffset, double endOffset) const {
722 PositionVector ret;
723 Position begPos = front();
724 if (beginOffset > POSITION_EPS) {
725 begPos = positionAtOffset(beginOffset);
726 }
727 Position endPos = back();
728 if (endOffset < length() - POSITION_EPS) {
729 endPos = positionAtOffset(endOffset);
730 }
731 ret.push_back(begPos);
732
733 double seen = 0;
734 const_iterator i = begin();
735 // skip previous segments
736 while ((i + 1) != end()
737 &&
738 seen + (*i).distanceTo(*(i + 1)) < beginOffset) {
739 seen += (*i).distanceTo(*(i + 1));
740 i++;
741 }
742 // append segments in between
743 while ((i + 1) != end()
744 &&
745 seen + (*i).distanceTo(*(i + 1)) < endOffset) {
746
747 ret.push_back_noDoublePos(*(i + 1));
748 seen += (*i).distanceTo(*(i + 1));
749 i++;
750 }
751 // append end
752 ret.push_back_noDoublePos(endPos);
753 if (ret.size() == 1) {
754 ret.push_back(endPos);
755 }
756 return ret;
757}
758
759
761PositionVector::getSubpart2D(double beginOffset, double endOffset) const {
762 if (size() == 0) {
763 return PositionVector();
764 }
765 PositionVector ret;
766 Position begPos = front();
767 if (beginOffset > POSITION_EPS) {
768 begPos = positionAtOffset2D(beginOffset);
769 }
770 Position endPos = back();
771 if (endOffset < length2D() - POSITION_EPS) {
772 endPos = positionAtOffset2D(endOffset);
773 }
774 ret.push_back(begPos);
775
776 double seen = 0;
777 const_iterator i = begin();
778 // skip previous segments
779 while ((i + 1) != end()
780 &&
781 seen + (*i).distanceTo2D(*(i + 1)) < beginOffset) {
782 seen += (*i).distanceTo2D(*(i + 1));
783 i++;
784 }
785 // append segments in between
786 while ((i + 1) != end()
787 &&
788 seen + (*i).distanceTo2D(*(i + 1)) < endOffset) {
789
790 ret.push_back_noDoublePos(*(i + 1));
791 seen += (*i).distanceTo2D(*(i + 1));
792 i++;
793 }
794 // append end
795 ret.push_back_noDoublePos(endPos);
796 if (ret.size() == 1) {
797 ret.push_back(endPos);
798 }
799 return ret;
800}
801
802
804PositionVector::getSubpartByIndex(int beginIndex, int count) const {
805 if (size() == 0) {
806 return PositionVector();
807 }
808 if (beginIndex < 0) {
809 beginIndex += (int)size();
810 }
811 assert(count >= 0);
812 assert(beginIndex < (int)size());
813 assert(beginIndex + count <= (int)size());
814 PositionVector result;
815 for (int i = beginIndex; i < beginIndex + count; ++i) {
816 result.push_back((*this)[i]);
817 }
818 return result;
819}
820
821
822double
824 if (size() == 0) {
825 return INVALID_DOUBLE;
826 }
827 return front().angleTo2D(back());
828}
829
830
831double
832PositionVector::nearest_offset_to_point2D(const Position& p, bool perpendicular) const {
833 if (size() == 0) {
834 return INVALID_DOUBLE;
835 }
836 double minDist = std::numeric_limits<double>::max();
837 double nearestPos = GeomHelper::INVALID_OFFSET;
838 double seen = 0;
839 for (const_iterator i = begin(); i != end() - 1; i++) {
840 const double pos =
841 GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, perpendicular);
842 const double dist = pos == GeomHelper::INVALID_OFFSET ? minDist : p.distanceTo2D(positionAtOffset2D(*i, *(i + 1), pos));
843 if (dist < minDist) {
844 nearestPos = pos + seen;
845 minDist = dist;
846 }
847 if (perpendicular && i != begin() && pos == GeomHelper::INVALID_OFFSET) {
848 // even if perpendicular is set we still need to check the distance to the inner points
849 const double cornerDist = p.distanceTo2D(*i);
850 if (cornerDist < minDist) {
851 const double pos1 =
852 GeomHelper::nearest_offset_on_line_to_point2D(*(i - 1), *i, p, false);
853 const double pos2 =
854 GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, false);
855 if (pos1 == (*(i - 1)).distanceTo2D(*i) && pos2 == 0.) {
856 nearestPos = seen;
857 minDist = cornerDist;
858 }
859 }
860 }
861 seen += (*i).distanceTo2D(*(i + 1));
862 }
863 return nearestPos;
864}
865
866
867double
868PositionVector::nearest_offset_to_point25D(const Position& p, bool perpendicular) const {
869 if (size() == 0) {
870 return INVALID_DOUBLE;
871 }
872 double minDist = std::numeric_limits<double>::max();
873 double nearestPos = GeomHelper::INVALID_OFFSET;
874 double seen = 0;
875 for (const_iterator i = begin(); i != end() - 1; i++) {
876 const double pos =
877 GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, perpendicular);
878 const double dist = pos == GeomHelper::INVALID_OFFSET ? minDist : p.distanceTo2D(positionAtOffset2D(*i, *(i + 1), pos));
879 if (dist < minDist) {
880 const double pos25D = pos * (*i).distanceTo(*(i + 1)) / (*i).distanceTo2D(*(i + 1));
881 nearestPos = pos25D + seen;
882 minDist = dist;
883 }
884 if (perpendicular && i != begin() && pos == GeomHelper::INVALID_OFFSET) {
885 // even if perpendicular is set we still need to check the distance to the inner points
886 const double cornerDist = p.distanceTo2D(*i);
887 if (cornerDist < minDist) {
888 const double pos1 =
889 GeomHelper::nearest_offset_on_line_to_point2D(*(i - 1), *i, p, false);
890 const double pos2 =
891 GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, false);
892 if (pos1 == (*(i - 1)).distanceTo2D(*i) && pos2 == 0.) {
893 nearestPos = seen;
894 minDist = cornerDist;
895 }
896 }
897 }
898 seen += (*i).distanceTo(*(i + 1));
899 }
900 return nearestPos;
901}
902
903
906 if (size() == 0) {
907 return Position::INVALID;
908 }
909 // @toDo this duplicates most of the code in nearest_offset_to_point2D. It should be refactored
910 if (extend) {
911 PositionVector extended = *this;
912 const double dist = 2 * distance2D(p);
913 extended.extrapolate(dist);
914 return extended.transformToVectorCoordinates(p) - Position(dist, 0);
915 }
916 double minDist = std::numeric_limits<double>::max();
917 double nearestPos = -1;
918 double seen = 0;
919 int sign = 1;
920 for (const_iterator i = begin(); i != end() - 1; i++) {
921 const double pos =
923 const double dist = pos < 0 ? minDist : p.distanceTo2D(positionAtOffset(*i, *(i + 1), pos));
924 if (dist < minDist) {
925 nearestPos = pos + seen;
926 minDist = dist;
927 sign = isLeft(*i, *(i + 1), p) >= 0 ? -1 : 1;
928 }
929 if (i != begin() && pos == GeomHelper::INVALID_OFFSET) {
930 // even if perpendicular is set we still need to check the distance to the inner points
931 const double cornerDist = p.distanceTo2D(*i);
932 if (cornerDist < minDist) {
933 const double pos1 =
934 GeomHelper::nearest_offset_on_line_to_point2D(*(i - 1), *i, p, false);
935 const double pos2 =
936 GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, false);
937 if (pos1 == (*(i - 1)).distanceTo2D(*i) && pos2 == 0.) {
938 nearestPos = seen;
939 minDist = cornerDist;
940 sign = isLeft(*(i - 1), *i, p) >= 0 ? -1 : 1;
941 }
942 }
943 }
944 seen += (*i).distanceTo2D(*(i + 1));
945 }
946 if (nearestPos != -1) {
947 return Position(nearestPos, sign * minDist);
948 } else {
949 return Position::INVALID;
950 }
951}
952
953
954int
955PositionVector::indexOfClosest(const Position& p, bool twoD) const {
956 if (size() == 0) {
957 return -1;
958 }
959 double minDist = std::numeric_limits<double>::max();
960 double dist;
961 int closest = 0;
962 for (int i = 0; i < (int)size(); i++) {
963 const Position& p2 = (*this)[i];
964 dist = twoD ? p.distanceTo2D(p2) : p.distanceTo(p2);
965 if (dist < minDist) {
966 closest = i;
967 minDist = dist;
968 }
969 }
970 return closest;
971}
972
973
974int
976 if (size() == 0) {
977 return -1;
978 }
979 double minDist = std::numeric_limits<double>::max();
980 int insertionIndex = 1;
981 for (int i = 0; i < (int)size() - 1; i++) {
982 const double length = GeomHelper::nearest_offset_on_line_to_point2D((*this)[i], (*this)[i + 1], p, false);
983 const Position& outIntersection = PositionVector::positionAtOffset2D((*this)[i], (*this)[i + 1], length);
984 const double dist = p.distanceTo2D(outIntersection);
985 if (dist < minDist) {
986 insertionIndex = i + 1;
987 minDist = dist;
988 }
989 }
990 // check if we have to adjust Position Z
991 if (interpolateZ) {
992 // obtain previous and next Z
993 const double previousZ = (begin() + (insertionIndex - 1))->z();
994 const double nextZ = (begin() + insertionIndex)->z();
995 // insert new position using x and y of p, and the new z
996 insert(begin() + insertionIndex, Position(p.x(), p.y(), ((previousZ + nextZ) / 2.0)));
997 } else {
998 insert(begin() + insertionIndex, p);
999 }
1000 return insertionIndex;
1001}
1002
1003
1004int
1006 if (size() == 0) {
1007 return -1;
1008 }
1009 double minDist = std::numeric_limits<double>::max();
1010 int removalIndex = 0;
1011 for (int i = 0; i < (int)size(); i++) {
1012 const double dist = p.distanceTo2D((*this)[i]);
1013 if (dist < minDist) {
1014 removalIndex = i;
1015 minDist = dist;
1016 }
1017 }
1018 erase(begin() + removalIndex);
1019 return removalIndex;
1020}
1021
1022
1023std::vector<double>
1025 std::vector<double> ret;
1026 if (other.size() == 0) {
1027 return ret;
1028 }
1029 for (const_iterator i = other.begin(); i != other.end() - 1; i++) {
1030 std::vector<double> atSegment = intersectsAtLengths2D(*i, *(i + 1));
1031 copy(atSegment.begin(), atSegment.end(), back_inserter(ret));
1032 }
1033 return ret;
1034}
1035
1036
1037std::vector<double>
1039 std::vector<double> ret;
1040 if (size() == 0) {
1041 return ret;
1042 }
1043 double pos = 0;
1044 for (const_iterator i = begin(); i != end() - 1; i++) {
1045 const Position& p1 = *i;
1046 const Position& p2 = *(i + 1);
1047 double x, y, m;
1048 if (intersects(p1, p2, lp1, lp2, 0., &x, &y, &m)) {
1049 ret.push_back(Position(x, y).distanceTo2D(p1) + pos);
1050 }
1051 pos += p1.distanceTo2D(p2);
1052 }
1053 return ret;
1054}
1055
1056
1057void
1058PositionVector::extrapolate(const double val, const bool onlyFirst, const bool onlyLast) {
1059 if (size() > 0) {
1060 Position& p1 = (*this)[0];
1061 Position& p2 = (*this)[1];
1062 const Position offset = (p2 - p1) * (val / p1.distanceTo(p2));
1063 if (!onlyLast) {
1064 p1.sub(offset);
1065 }
1066 if (!onlyFirst) {
1067 if (size() == 2) {
1068 p2.add(offset);
1069 } else {
1070 const Position& e1 = (*this)[-2];
1071 Position& e2 = (*this)[-1];
1072 e2.sub((e1 - e2) * (val / e1.distanceTo(e2)));
1073 }
1074 }
1075 }
1076}
1077
1078
1079void
1080PositionVector::extrapolate2D(const double val, const bool onlyFirst) {
1081 if (size() > 0) {
1082 Position& p1 = (*this)[0];
1083 Position& p2 = (*this)[1];
1084 if (p1.distanceTo2D(p2) > 0) {
1085 const Position offset = (p2 - p1) * (val / p1.distanceTo2D(p2));
1086 p1.sub(offset);
1087 if (!onlyFirst) {
1088 if (size() == 2) {
1089 p2.add(offset);
1090 } else {
1091 const Position& e1 = (*this)[-2];
1092 Position& e2 = (*this)[-1];
1093 e2.sub((e1 - e2) * (val / e1.distanceTo2D(e2)));
1094 }
1095 }
1096 }
1097 }
1098}
1099
1100
1103 PositionVector ret;
1104 for (const_reverse_iterator i = rbegin(); i != rend(); i++) {
1105 ret.push_back(*i);
1106 }
1107 return ret;
1108}
1109
1110
1112PositionVector::sideOffset(const Position& beg, const Position& end, const double amount) {
1113 const double scale = amount / beg.distanceTo2D(end);
1114 return Position((beg.y() - end.y()) * scale, (end.x() - beg.x()) * scale);
1115}
1116
1117
1118void
1119PositionVector::move2side(double amount, double maxExtension) {
1120 if (size() < 2) {
1121 return;
1122 }
1123 removeDoublePoints(POSITION_EPS, true);
1124 if (length2D() == 0 || amount == 0) {
1125 return;
1126 }
1127 PositionVector shape;
1128 std::vector<int> recheck;
1129 for (int i = 0; i < static_cast<int>(size()); i++) {
1130 if (i == 0) {
1131 const Position& from = (*this)[i];
1132 const Position& to = (*this)[i + 1];
1133 if (from != to) {
1134 shape.push_back(from - sideOffset(from, to, amount));
1135#ifdef DEBUG_MOVE2SIDE
1136 if (gDebugFlag1) {
1137 std::cout << " " << i << "a=" << shape.back() << "\n";
1138 }
1139#endif
1140 }
1141 } else if (i == static_cast<int>(size()) - 1) {
1142 const Position& from = (*this)[i - 1];
1143 const Position& to = (*this)[i];
1144 if (from != to) {
1145 shape.push_back(to - sideOffset(from, to, amount));
1146#ifdef DEBUG_MOVE2SIDE
1147 if (gDebugFlag1) {
1148 std::cout << " " << i << "b=" << shape.back() << "\n";
1149 }
1150#endif
1151 }
1152 } else {
1153 const Position& from = (*this)[i - 1];
1154 const Position& me = (*this)[i];
1155 const Position& to = (*this)[i + 1];
1156 PositionVector fromMe(from, me);
1157 fromMe.extrapolate2D(me.distanceTo2D(to));
1158 const double extrapolateDev = fromMe[1].distanceTo2D(to);
1159 if (fabs(extrapolateDev) < POSITION_EPS) {
1160 // parallel case, just shift the middle point
1161 shape.push_back(me - sideOffset(from, to, amount));
1162#ifdef DEBUG_MOVE2SIDE
1163 if (gDebugFlag1) {
1164 std::cout << " " << i << "c=" << shape.back() << "\n";
1165 }
1166#endif
1167 } else if (fabs(extrapolateDev - 2 * me.distanceTo2D(to)) < POSITION_EPS) {
1168 // counterparallel case, just shift the middle point
1169 PositionVector fromMe2(from, me);
1170 fromMe2.extrapolate2D(amount);
1171 shape.push_back(fromMe2[1]);
1172#ifdef DEBUG_MOVE2SIDE
1173 if (gDebugFlag1) {
1174 std::cout << " " << i << "d=" << shape.back() << " " << i << "_from=" << from << " " << i << "_me=" << me << " " << i << "_to=" << to << "\n";
1175 }
1176#endif
1177 } else {
1178 Position offsets = sideOffset(from, me, amount);
1179 Position offsets2 = sideOffset(me, to, amount);
1180 PositionVector l1(from - offsets, me - offsets);
1181 PositionVector l2(me - offsets2, to - offsets2);
1182 Position meNew = l1.intersectionPosition2D(l2[0], l2[1], maxExtension);
1183 if (meNew == Position::INVALID) {
1184 recheck.push_back(i);
1185 continue;
1186 }
1187 meNew = meNew + Position(0, 0, me.z());
1188 shape.push_back(meNew);
1189#ifdef DEBUG_MOVE2SIDE
1190 if (gDebugFlag1) {
1191 std::cout << " " << i << "e=" << shape.back() << "\n";
1192 }
1193#endif
1194 }
1195 // copy original z value
1196 shape.back().set(shape.back().x(), shape.back().y(), me.z());
1197 const double angle = localAngle(from, me, to);
1198 if (fabs(angle) > NUMERICAL_EPS) {
1199 const double length = from.distanceTo2D(me) + me.distanceTo2D(to);
1200 const double radius = length / angle;
1201#ifdef DEBUG_MOVE2SIDE
1202 if (gDebugFlag1) {
1203 std::cout << " i=" << i << " a=" << RAD2DEG(angle) << " l=" << length << " r=" << radius << " t=" << amount * 1.8 << "\n";
1204 }
1205#endif
1206 if ((radius < 0 && -radius < amount * 1.8) || fabs(RAD2DEG(angle)) > 170) {
1207 recheck.push_back(i);
1208 }
1209 }
1210 }
1211 }
1212 if (!recheck.empty()) {
1213 // try to adjust positions to avoid clipping
1214 shape = *this;
1215 for (int i = (int)recheck.size() - 1; i >= 0; i--) {
1216 shape.erase(shape.begin() + recheck[i]);
1217 }
1218 shape.move2side(amount, maxExtension);
1219 }
1220 *this = shape;
1221}
1222
1223
1224void
1225PositionVector::move2sideCustom(std::vector<double> amount, double maxExtension) {
1226 if (size() < 2) {
1227 return;
1228 }
1229 if (length2D() == 0) {
1230 return;
1231 }
1232 if (size() != amount.size()) {
1233 throw InvalidArgument("Numer of offsets (" + toString(amount.size())
1234 + ") does not match number of points (" + toString(size()) + ")");
1235 }
1236 PositionVector shape;
1237 for (int i = 0; i < static_cast<int>(size()); i++) {
1238 if (i == 0) {
1239 const Position& from = (*this)[i];
1240 const Position& to = (*this)[i + 1];
1241 if (from != to) {
1242 shape.push_back(from - sideOffset(from, to, amount[i]));
1243 }
1244 } else if (i == static_cast<int>(size()) - 1) {
1245 const Position& from = (*this)[i - 1];
1246 const Position& to = (*this)[i];
1247 if (from != to) {
1248 shape.push_back(to - sideOffset(from, to, amount[i]));
1249 }
1250 } else {
1251 const Position& from = (*this)[i - 1];
1252 const Position& me = (*this)[i];
1253 const Position& to = (*this)[i + 1];
1254 PositionVector fromMe(from, me);
1255 fromMe.extrapolate2D(me.distanceTo2D(to));
1256 const double extrapolateDev = fromMe[1].distanceTo2D(to);
1257 if (fabs(extrapolateDev) < POSITION_EPS) {
1258 // parallel case, just shift the middle point
1259 shape.push_back(me - sideOffset(from, to, amount[i]));
1260 } else if (fabs(extrapolateDev - 2 * me.distanceTo2D(to)) < POSITION_EPS) {
1261 // counterparallel case, just shift the middle point
1262 PositionVector fromMe2(from, me);
1263 fromMe2.extrapolate2D(amount[i]);
1264 shape.push_back(fromMe2[1]);
1265 } else {
1266 Position offsets = sideOffset(from, me, amount[i]);
1267 Position offsets2 = sideOffset(me, to, amount[i]);
1268 PositionVector l1(from - offsets, me - offsets);
1269 PositionVector l2(me - offsets2, to - offsets2);
1270 Position meNew = l1.intersectionPosition2D(l2[0], l2[1], maxExtension);
1271 if (meNew == Position::INVALID) {
1272 continue;
1273 }
1274 meNew = meNew + Position(0, 0, me.z());
1275 shape.push_back(meNew);
1276 }
1277 // copy original z value
1278 shape.back().set(shape.back().x(), shape.back().y(), me.z());
1279 }
1280 }
1281 *this = shape;
1282}
1283
1284double
1285PositionVector::localAngle(const Position& from, const Position& pos, const Position& to) {
1286 return GeomHelper::angleDiff(from.angleTo2D(pos), pos.angleTo2D(to));
1287}
1288
1289double
1291 if ((pos + 1) < (int)size()) {
1292 return (*this)[pos].angleTo2D((*this)[pos + 1]);
1293 } else {
1294 return INVALID_DOUBLE;
1295 }
1296}
1297
1298
1299void
1301 if ((size() != 0) && ((*this)[0] != back())) {
1302 push_back((*this)[0]);
1303 }
1304}
1305
1306
1307std::vector<double>
1308PositionVector::distances(const PositionVector& s, bool perpendicular) const {
1309 std::vector<double> ret;
1310 const_iterator i;
1311 for (i = begin(); i != end(); i++) {
1312 const double dist = s.distance2D(*i, perpendicular);
1313 if (dist != GeomHelper::INVALID_OFFSET) {
1314 ret.push_back(dist);
1315 }
1316 }
1317 for (i = s.begin(); i != s.end(); i++) {
1318 const double dist = distance2D(*i, perpendicular);
1319 if (dist != GeomHelper::INVALID_OFFSET) {
1320 ret.push_back(dist);
1321 }
1322 }
1323 return ret;
1324}
1325
1326
1327double
1328PositionVector::distance2D(const Position& p, bool perpendicular) const {
1329 if (size() == 0) {
1330 return std::numeric_limits<double>::max();
1331 } else if (size() == 1) {
1332 return front().distanceTo(p);
1333 }
1334 const double nearestOffset = nearest_offset_to_point2D(p, perpendicular);
1335 if (nearestOffset == GeomHelper::INVALID_OFFSET) {
1337 } else {
1338 return p.distanceTo2D(positionAtOffset2D(nearestOffset));
1339 }
1340}
1341
1342
1343void
1345 if (empty()) {
1346 push_back(p);
1347 } else {
1348 insert(begin(), p);
1349 }
1350}
1351
1352
1353void
1355 if (empty()) {
1356 throw ProcessError("PositionVector is empty");
1357 } else {
1358 erase(begin());
1359 }
1360}
1361
1362
1363void
1365 if (size() == 0 || !p.almostSame(back())) {
1366 push_back(p);
1367 }
1368}
1369
1370
1371void
1373 if ((size() == 0) || !p.almostSame(front())) {
1374 push_front(p);
1375 }
1376}
1377
1378
1379void
1380PositionVector::insert_noDoublePos(const std::vector<Position>::iterator& at, const Position& p) {
1381 if (at == begin()) {
1383 } else if (at == end()) {
1385 } else {
1386 if (!p.almostSame(*at) && !p.almostSame(*(at - 1))) {
1387 insert(at, p);
1388 }
1389 }
1390}
1391
1392
1393bool
1395 return (size() >= 2) && ((*this)[0] == back());
1396}
1397
1398
1399bool
1401 // iterate over all positions and check if is NAN
1402 for (auto i = begin(); i != end(); i++) {
1403 if (i->isNAN()) {
1404 return true;
1405 }
1406 }
1407 // all ok, then return false
1408 return false;
1409}
1410
1411
1412void
1413PositionVector::removeDoublePoints(double minDist, bool assertLength, int beginOffset, int endOffset, bool resample) {
1414 int curSize = (int)size() - beginOffset - endOffset;
1415 if (curSize > 1) {
1416 iterator last = begin() + beginOffset;
1417 for (iterator i = last + 1; i != (end() - endOffset) && (!assertLength || curSize > 2);) {
1418 if (last->almostSame(*i, minDist)) {
1419 if (i + 1 == end() - endOffset) {
1420 // special case: keep the last point and remove the next-to-last
1421 if (resample && last > begin() && (last - 1)->distanceTo(*i) >= 2 * minDist) {
1422 // resample rather than remove point after a long segment
1423 const double shiftBack = minDist - last->distanceTo(*i);
1424 //if (gDebugFlag1) std::cout << " resample endOffset beforeLast=" << *(last - 1) << " last=" << *last << " i=" << *i;
1425 (*last) = positionAtOffset(*(last - 1), *last, (last - 1)->distanceTo(*last) - shiftBack);
1426 //if (gDebugFlag1) std::cout << " lastNew=" << *last;
1427 last = i;
1428 ++i;
1429 } else {
1430 erase(last);
1431 i = end() - endOffset;
1432 }
1433 } else {
1434 if (resample && i + 1 != end() && last->distanceTo(*(i + 1)) >= 2 * minDist) {
1435 // resample rather than remove points before a long segment
1436 const double shiftForward = minDist - last->distanceTo(*i);
1437 //if (gDebugFlag1) std::cout << " resample last=" << *last << " i=" << *i << " next=" << *(i + 1);
1438 (*i) = positionAtOffset(*i, *(i + 1), shiftForward);
1439 //if (gDebugFlag1) std::cout << " iNew=" << *i << "\n";
1440 last = i;
1441 ++i;
1442 } else {
1443 i = erase(i);
1444 }
1445 }
1446 curSize--;
1447 } else {
1448 last = i;
1449 ++i;
1450 }
1451 }
1452 }
1453}
1454
1455
1456bool
1458 return static_cast<vp>(*this) == static_cast<vp>(v2);
1459}
1460
1461
1462bool
1464 return static_cast<vp>(*this) != static_cast<vp>(v2);
1465}
1466
1469 if (length() != v2.length()) {
1470 WRITE_ERROR(TL("Trying to substract PositionVectors of different lengths."));
1471 }
1472 PositionVector pv;
1473 auto i1 = begin();
1474 auto i2 = v2.begin();
1475 while (i1 != end()) {
1476 pv.add(*i1 - *i2);
1477 }
1478 return pv;
1479}
1480
1483 if (length() != v2.length()) {
1484 WRITE_ERROR(TL("Trying to substract PositionVectors of different lengths."));
1485 }
1486 PositionVector pv;
1487 auto i1 = begin();
1488 auto i2 = v2.begin();
1489 while (i1 != end()) {
1490 pv.add(*i1 + *i2);
1491 }
1492 return pv;
1493}
1494
1495bool
1497 if (size() < 2) {
1498 return false;
1499 }
1500 for (const_iterator i = begin(); i != end() - 1; i++) {
1501 if ((*i).z() != (*(i + 1)).z()) {
1502 return true;
1503 }
1504 }
1505 return false;
1506}
1507
1508
1509bool
1510PositionVector::intersects(const Position& p11, const Position& p12, const Position& p21, const Position& p22, const double withinDist, double* x, double* y, double* mu) {
1511 const double eps = std::numeric_limits<double>::epsilon();
1512 const double denominator = (p22.y() - p21.y()) * (p12.x() - p11.x()) - (p22.x() - p21.x()) * (p12.y() - p11.y());
1513 const double numera = (p22.x() - p21.x()) * (p11.y() - p21.y()) - (p22.y() - p21.y()) * (p11.x() - p21.x());
1514 const double numerb = (p12.x() - p11.x()) * (p11.y() - p21.y()) - (p12.y() - p11.y()) * (p11.x() - p21.x());
1515 /* Are the lines coincident? */
1516 if (fabs(numera) < eps && fabs(numerb) < eps && fabs(denominator) < eps) {
1517 double a1;
1518 double a2;
1519 double a3;
1520 double a4;
1521 double a = -1e12;
1522 if (p11.x() != p12.x()) {
1523 a1 = p11.x() < p12.x() ? p11.x() : p12.x();
1524 a2 = p11.x() < p12.x() ? p12.x() : p11.x();
1525 a3 = p21.x() < p22.x() ? p21.x() : p22.x();
1526 a4 = p21.x() < p22.x() ? p22.x() : p21.x();
1527 } else {
1528 a1 = p11.y() < p12.y() ? p11.y() : p12.y();
1529 a2 = p11.y() < p12.y() ? p12.y() : p11.y();
1530 a3 = p21.y() < p22.y() ? p21.y() : p22.y();
1531 a4 = p21.y() < p22.y() ? p22.y() : p21.y();
1532 }
1533 if (a1 <= a3 && a3 <= a2) {
1534 if (a4 < a2) {
1535 a = (a3 + a4) / 2;
1536 } else {
1537 a = (a2 + a3) / 2;
1538 }
1539 }
1540 if (a3 <= a1 && a1 <= a4) {
1541 if (a2 < a4) {
1542 a = (a1 + a2) / 2;
1543 } else {
1544 a = (a1 + a4) / 2;
1545 }
1546 }
1547 if (a != -1e12) {
1548 if (x != nullptr) {
1549 if (p11.x() != p12.x()) {
1550 *mu = (a - p11.x()) / (p12.x() - p11.x());
1551 *x = a;
1552 *y = p11.y() + (*mu) * (p12.y() - p11.y());
1553 } else {
1554 *x = p11.x();
1555 *y = a;
1556 if (p12.y() == p11.y()) {
1557 *mu = 0;
1558 } else {
1559 *mu = (a - p11.y()) / (p12.y() - p11.y());
1560 }
1561 }
1562 }
1563 return true;
1564 }
1565 return false;
1566 }
1567 /* Are the lines parallel */
1568 if (fabs(denominator) < eps) {
1569 return false;
1570 }
1571 /* Is the intersection along the segments */
1572 double mua = numera / denominator;
1573 /* reduce rounding errors for lines ending in the same point */
1574 if (fabs(p12.x() - p22.x()) < eps && fabs(p12.y() - p22.y()) < eps) {
1575 mua = 1.;
1576 } else {
1577 const double offseta = withinDist / p11.distanceTo2D(p12);
1578 const double offsetb = withinDist / p21.distanceTo2D(p22);
1579 const double mub = numerb / denominator;
1580 if (mua < -offseta || mua > 1 + offseta || mub < -offsetb || mub > 1 + offsetb) {
1581 return false;
1582 }
1583 }
1584 if (x != nullptr) {
1585 *x = p11.x() + mua * (p12.x() - p11.x());
1586 *y = p11.y() + mua * (p12.y() - p11.y());
1587 *mu = mua;
1588 }
1589 return true;
1590}
1591
1592
1593void
1595 const double s = sin(angle);
1596 const double c = cos(angle);
1597 for (int i = 0; i < (int)size(); i++) {
1598 const double x = (*this)[i].x();
1599 const double y = (*this)[i].y();
1600 const double z = (*this)[i].z();
1601 const double xnew = x * c - y * s;
1602 const double ynew = x * s + y * c;
1603 (*this)[i].set(xnew, ynew, z);
1604 }
1605}
1606
1607
1610 PositionVector result = *this;
1611 bool changed = true;
1612 while (changed && result.size() > 3) {
1613 changed = false;
1614 for (int i = 0; i < (int)result.size(); i++) {
1615 const Position& p1 = result[i];
1616 const Position& p2 = result[(i + 2) % result.size()];
1617 const int middleIndex = (i + 1) % result.size();
1618 const Position& p0 = result[middleIndex];
1619 // https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line#Line_defined_by_two_points
1620 const double triangleArea2 = fabs((p2.y() - p1.y()) * p0.x() - (p2.x() - p1.x()) * p0.y() + p2.x() * p1.y() - p2.y() * p1.x());
1621 const double distIK = p1.distanceTo2D(p2);
1622 if (distIK > NUMERICAL_EPS && triangleArea2 / distIK < NUMERICAL_EPS) {
1623 changed = true;
1624 result.erase(result.begin() + middleIndex);
1625 break;
1626 }
1627 }
1628 }
1629 return result;
1630}
1631
1632
1634PositionVector::getOrthogonal(const Position& p, double extend, bool before, double length, double deg) const {
1635 PositionVector result;
1636 PositionVector tmp = *this;
1637 tmp.extrapolate2D(extend);
1638 const double baseOffset = tmp.nearest_offset_to_point2D(p);
1639 if (baseOffset == GeomHelper::INVALID_OFFSET || size() < 2) {
1640 // fail
1641 return result;
1642 }
1643 Position base = tmp.positionAtOffset2D(baseOffset);
1644 const int closestIndex = tmp.indexOfClosest(base);
1645 const double closestOffset = tmp.offsetAtIndex2D(closestIndex);
1646 result.push_back(base);
1647 if (fabs(baseOffset - closestOffset) > NUMERICAL_EPS) {
1648 result.push_back(tmp[closestIndex]);
1649 if ((closestOffset < baseOffset) != before) {
1650 deg *= -1;
1651 }
1652 } else if (before) {
1653 // take the segment before closestIndex if possible
1654 if (closestIndex > 0) {
1655 result.push_back(tmp[closestIndex - 1]);
1656 } else {
1657 result.push_back(tmp[1]);
1658 deg *= -1;
1659 }
1660 } else {
1661 // take the segment after closestIndex if possible
1662 if (closestIndex < (int)size() - 1) {
1663 result.push_back(tmp[closestIndex + 1]);
1664 } else {
1665 result.push_back(tmp[-1]);
1666 deg *= -1;
1667 }
1668 }
1669 result = result.getSubpart2D(0, length);
1670 // rotate around base
1671 result.add(base * -1);
1672 result.rotate2D(DEG2RAD(deg));
1673 result.add(base);
1674 return result;
1675}
1676
1677
1680 PositionVector result = *this;
1681 if (size() == 0) {
1682 return result;
1683 }
1684 const double z0 = (*this)[0].z();
1685 // the z-delta of the first segment
1686 const double dz = (*this)[1].z() - z0;
1687 // if the shape only has 2 points it is as smooth as possible already
1688 if (size() > 2 && dz != 0) {
1689 dist = MIN2(dist, length2D());
1690 // check wether we need to insert a new point at dist
1691 Position pDist = positionAtOffset2D(dist);
1692 int iLast = indexOfClosest(pDist);
1693 // prevent close spacing to reduce impact of rounding errors in z-axis
1694 if (pDist.distanceTo2D((*this)[iLast]) > POSITION_EPS * 20) {
1695 iLast = result.insertAtClosest(pDist, false);
1696 }
1697 double dist2 = result.offsetAtIndex2D(iLast);
1698 const double dz2 = result[iLast].z() - z0;
1699 double seen = 0;
1700 for (int i = 1; i < iLast; ++i) {
1701 seen += result[i].distanceTo2D(result[i - 1]);
1702 result[i].set(result[i].x(), result[i].y(), z0 + dz2 * seen / dist2);
1703 }
1704 }
1705 return result;
1706
1707}
1708
1709
1711PositionVector::interpolateZ(double zStart, double zEnd) const {
1712 PositionVector result = *this;
1713 if (size() == 0) {
1714 return result;
1715 }
1716 result[0].setz(zStart);
1717 result[-1].setz(zEnd);
1718 const double dz = zEnd - zStart;
1719 const double length = length2D();
1720 double seen = 0;
1721 for (int i = 1; i < (int)size() - 1; ++i) {
1722 seen += result[i].distanceTo2D(result[i - 1]);
1723 result[i].setz(zStart + dz * seen / length);
1724 }
1725 return result;
1726}
1727
1728
1730PositionVector::resample(double maxLength, const bool adjustEnd) const {
1731 PositionVector result;
1732 if (maxLength == 0) {
1733 return result;
1734 }
1735 const double length = length2D();
1736 if (length < POSITION_EPS) {
1737 return result;
1738 }
1739 maxLength = length / ceil(length / maxLength);
1740 for (double pos = 0; pos <= length; pos += maxLength) {
1741 result.push_back(positionAtOffset2D(pos));
1742 }
1743 // check if we have to adjust last element
1744 if (adjustEnd && !result.empty() && (result.back() != back())) {
1745 // add last element
1746 result.push_back(back());
1747 }
1748 return result;
1749}
1750
1751
1752double
1754 if (index < 0 || index >= (int)size()) {
1756 }
1757 double seen = 0;
1758 for (int i = 1; i <= index; ++i) {
1759 seen += (*this)[i].distanceTo2D((*this)[i - 1]);
1760 }
1761 return seen;
1762}
1763
1764
1765double
1766PositionVector::getMaxGrade(double& maxJump) const {
1767 double result = 0;
1768 for (int i = 1; i < (int)size(); ++i) {
1769 const Position& p1 = (*this)[i - 1];
1770 const Position& p2 = (*this)[i];
1771 const double distZ = fabs(p1.z() - p2.z());
1772 const double dist2D = p1.distanceTo2D(p2);
1773 if (dist2D == 0) {
1774 maxJump = MAX2(maxJump, distZ);
1775 } else {
1776 result = MAX2(result, distZ / dist2D);
1777 }
1778 }
1779 return result;
1780}
1781
1782
1785 // inspired by David F. Rogers
1786 assert(size() < 33);
1787 static const double fac[33] = {
1788 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,
1789 6227020800.0, 87178291200.0, 1307674368000.0, 20922789888000.0, 355687428096000.0, 6402373705728000.0,
1790 121645100408832000.0, 2432902008176640000.0, 51090942171709440000.0, 1124000727777607680000.0,
1791 25852016738884976640000.0, 620448401733239439360000.0, 15511210043330985984000000.0,
1792 403291461126605635584000000.0, 10888869450418352160768000000.0, 304888344611713860501504000000.0,
1793 8841761993739701954543616000000.0, 265252859812191058636308480000000.0,
1794 8222838654177922817725562880000000.0, 263130836933693530167218012160000000.0
1795 };
1796 PositionVector ret;
1797 const int npts = (int)size();
1798 // calculate the points on the Bezier curve
1799 const double step = (double) 1.0 / (numPoints - 1);
1800 double t = 0.;
1801 Position prev;
1802 for (int i1 = 0; i1 < numPoints; i1++) {
1803 if ((1.0 - t) < 5e-6) {
1804 t = 1.0;
1805 }
1806 double x = 0., y = 0., z = 0.;
1807 for (int i = 0; i < npts; i++) {
1808 const double ti = (i == 0) ? 1.0 : pow(t, i);
1809 const double tni = (npts == i + 1) ? 1.0 : pow(1 - t, npts - i - 1);
1810 const double basis = fac[npts - 1] / (fac[i] * fac[npts - 1 - i]) * ti * tni;
1811 x += basis * at(i).x();
1812 y += basis * at(i).y();
1813 z += basis * at(i).z();
1814 }
1815 t += step;
1816 Position current(x, y, z);
1817 if (prev != current && !ISNAN(x) && !ISNAN(y) && !ISNAN(z)) {
1818 ret.push_back(current);
1819 }
1820 prev = current;
1821 }
1822 return ret;
1823}
1824
1825
1826/****************************************************************************/
#define DEG2RAD(x)
Definition: GeomHelper.h:35
#define RAD2DEG(x)
Definition: GeomHelper.h:36
#define WRITE_WARNINGF(...)
Definition: MsgHandler.h:266
#define WRITE_ERROR(msg)
Definition: MsgHandler.h:274
#define TL(string)
Definition: MsgHandler.h:282
std::ostream & operator<<(std::ostream &os, const PositionVector &geom)
bool gDebugFlag1
global utility flags for debugging
Definition: StdDefs.cpp:33
const double INVALID_DOUBLE
Definition: StdDefs.h:60
T MIN2(T a, T b)
Definition: StdDefs.h:71
T ISNAN(T a)
Definition: StdDefs.h:112
T MAX2(T a, T b)
Definition: StdDefs.h:77
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,...
Definition: GeomHelper.cpp:82
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)
Definition: GeomHelper.cpp:88
static double legacyDegree(const double angle, const bool positive=false)
Definition: GeomHelper.cpp:215
static double angleDiff(const double angle1, const double angle2)
Returns the difference of the second angle to the first angle in radiants.
Definition: GeomHelper.cpp:179
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:267
static const Position INVALID
used to indicate that a position is valid
Definition: Position.h:298
double distanceTo2D(const Position &p2) const
returns the euclidean distance in the x-y-plane
Definition: Position.h:252
double distanceTo(const Position &p2) const
returns the euclidean distance in 3 dimension
Definition: Position.h:242
void sub(double dx, double dy)
Substracts 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:262
bool almostSame(const Position &p2, double maxDiv=POSITION_EPS) const
check if two position is almost the sme as other
Definition: Position.h:237
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
substracts 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 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)
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