1 /****************************************************************************/
2 // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
3 // Copyright (C) 2001-2019 German Aerospace Center (DLR) and others.
4 // This program and the accompanying materials
5 // are made available under the terms of the Eclipse Public License v2.0
6 // which accompanies this distribution, and is available at
7 // http://www.eclipse.org/legal/epl-v20.html
8 // SPDX-License-Identifier: EPL-2.0
9 /****************************************************************************/
10 /// @file    PositionVector.cpp
11 /// @author  Daniel Krajzewicz
12 /// @author  Jakob Erdmann
13 /// @author  Michael Behrisch
14 /// @author  Walter Bamberger
15 /// @date    Sept 2002
16 /// @version $Id$
17 ///
18 // A list of positions
19 /****************************************************************************/
20 
21 
22 // ===========================================================================
23 // included modules
24 // ===========================================================================
25 #include <config.h>
26 
27 #include <queue>
28 #include <cmath>
29 #include <iostream>
30 #include <algorithm>
31 #include <cassert>
32 #include <iterator>
33 #include <limits>
34 #include <utils/common/StdDefs.h>
35 #include <utils/common/MsgHandler.h>
36 #include <utils/common/ToString.h>
37 #include "AbstractPoly.h"
38 #include "Position.h"
39 #include "PositionVector.h"
40 #include "GeomHelper.h"
41 #include "Boundary.h"
42 
43 // ===========================================================================
44 // static members
45 // ===========================================================================
46 const PositionVector PositionVector::EMPTY;
47 
48 // ===========================================================================
49 // method definitions
50 // ===========================================================================
51 
PositionVector()52 PositionVector::PositionVector() {}
53 
54 
PositionVector(const std::vector<Position> & v)55 PositionVector::PositionVector(const std::vector<Position>& v) {
56     std::copy(v.begin(), v.end(), std::back_inserter(*this));
57 }
58 
59 
PositionVector(const std::vector<Position>::const_iterator beg,const std::vector<Position>::const_iterator end)60 PositionVector::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 
PositionVector(const Position & p1,const Position & p2)65 PositionVector::PositionVector(const Position& p1, const Position& p2) {
66     push_back(p1);
67     push_back(p2);
68 }
69 
70 
~PositionVector()71 PositionVector::~PositionVector() {}
72 
73 
74 bool
around(const Position & p,double offset) const75 PositionVector::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     for (const_iterator i = begin(); i != end() - 1; i++) {
86         Position p1(
87             (*i).x() - p.x(),
88             (*i).y() - p.y());
89         Position p2(
90             (*(i + 1)).x() - p.x(),
91             (*(i + 1)).y() - p.y());
92         angle += GeomHelper::angle2D(p1, p2);
93     }
94     Position p1(
95         (*(end() - 1)).x() - p.x(),
96         (*(end() - 1)).y() - p.y());
97     Position p2(
98         (*(begin())).x() - p.x(),
99         (*(begin())).y() - p.y());
100     angle += GeomHelper::angle2D(p1, p2);
101     return (!(fabs(angle) < M_PI));
102 }
103 
104 
105 bool
overlapsWith(const AbstractPoly & poly,double offset) const106 PositionVector::overlapsWith(const AbstractPoly& poly, double offset) const {
107     if (
108         // check whether one of my points lies within the given poly
109         partialWithin(poly, offset) ||
110         // check whether the polygon lies within me
111         poly.partialWithin(*this, offset)) {
112         return true;
113     }
114     if (size() >= 2) {
115         for (const_iterator i = begin(); i != end() - 1; i++) {
116             if (poly.crosses(*i, *(i + 1))) {
117                 return true;
118             }
119         }
120         if (size() > 2 && poly.crosses(back(), front())) {
121             return true;
122         }
123     }
124     return false;
125 }
126 
127 
128 double
getOverlapWith(const PositionVector & poly,double zThreshold) const129 PositionVector::getOverlapWith(const PositionVector& poly, double zThreshold) const {
130     double result = 0;
131     if ((size() == 0) || (poly.size() == 0)) {
132         return result;
133     }
134     // this points within poly
135     for (const_iterator i = begin(); i != end() - 1; i++) {
136         if (poly.around(*i)) {
137             Position closest = poly.positionAtOffset2D(poly.nearest_offset_to_point2D(*i));
138             if (fabs(closest.z() - (*i).z()) < zThreshold) {
139                 result = MAX2(result, poly.distance2D(*i));
140             }
141         }
142     }
143     // polys points within this
144     for (const_iterator i = poly.begin(); i != poly.end() - 1; i++) {
145         if (around(*i)) {
146             Position closest = positionAtOffset2D(nearest_offset_to_point2D(*i));
147             if (fabs(closest.z() - (*i).z()) < zThreshold) {
148                 result = MAX2(result, distance2D(*i));
149             }
150         }
151     }
152     return result;
153 }
154 
155 
156 bool
intersects(const Position & p1,const Position & p2) const157 PositionVector::intersects(const Position& p1, const Position& p2) const {
158     if (size() < 2) {
159         return false;
160     }
161     for (const_iterator i = begin(); i != end() - 1; i++) {
162         if (intersects(*i, *(i + 1), p1, p2)) {
163             return true;
164         }
165     }
166     return false;
167 }
168 
169 
170 bool
intersects(const PositionVector & v1) const171 PositionVector::intersects(const PositionVector& v1) const {
172     if (size() < 2) {
173         return false;
174     }
175     for (const_iterator i = begin(); i != end() - 1; i++) {
176         if (v1.intersects(*i, *(i + 1))) {
177             return true;
178         }
179     }
180     return false;
181 }
182 
183 
184 Position
intersectionPosition2D(const Position & p1,const Position & p2,const double withinDist) const185 PositionVector::intersectionPosition2D(const Position& p1, const Position& p2, const double withinDist) const {
186     for (const_iterator i = begin(); i != end() - 1; i++) {
187         double x, y, m;
188         if (intersects(*i, *(i + 1), p1, p2, withinDist, &x, &y, &m)) {
189             return Position(x, y);
190         }
191     }
192     return Position::INVALID;
193 }
194 
195 
196 Position
intersectionPosition2D(const PositionVector & v1) const197 PositionVector::intersectionPosition2D(const PositionVector& v1) const {
198     for (const_iterator i = begin(); i != end() - 1; i++) {
199         if (v1.intersects(*i, *(i + 1))) {
200             return v1.intersectionPosition2D(*i, *(i + 1));
201         }
202     }
203     return Position::INVALID;
204 }
205 
206 
207 const Position&
operator [](int index) const208 PositionVector::operator[](int index) const {
209     /* bracket operators works as in Python. Examples:
210         - A = {'a', 'b', 'c', 'd'} (size 4)
211         - A [2] returns 'c' because 0 < 2 < 4
212         - A [100] thrown an exception because 100 > 4
213         - A [-1] returns 'd' because 4 - 1 = 3
214         - A [-100] thrown an exception because (4-100) < 0
215     */
216     if (index >= 0 && index < (int)size()) {
217         return at(index);
218     } else if (index < 0 && -index <= (int)size()) {
219         return at((int)size() + index);
220     } else {
221         throw ProcessError("Index out of range in bracket operator of PositionVector");
222     }
223 }
224 
225 
226 Position&
operator [](int index)227 PositionVector::operator[](int index) {
228     /* bracket operators works as in Python. Examples:
229         - A = {'a', 'b', 'c', 'd'} (size 4)
230         - A [2] returns 'c' because 0 < 2 < 4
231         - A [100] thrown an exception because 100 > 4
232         - A [-1] returns 'd' because 4 - 1 = 3
233         - A [-100] thrown an exception because (4-100) < 0
234     */
235     if (index >= 0 && index < (int)size()) {
236         return at(index);
237     } else if (index < 0 && -index <= (int)size()) {
238         return at((int)size() + index);
239     } else {
240         throw ProcessError("Index out of range in bracket operator of PositionVector");
241     }
242 }
243 
244 
245 Position
positionAtOffset(double pos,double lateralOffset) const246 PositionVector::positionAtOffset(double pos, double lateralOffset) const {
247     if (size() == 0) {
248         return Position::INVALID;
249     }
250     const_iterator i = begin();
251     double seenLength = 0;
252     do {
253         const double nextLength = (*i).distanceTo(*(i + 1));
254         if (seenLength + nextLength > pos) {
255             return positionAtOffset(*i, *(i + 1), pos - seenLength, lateralOffset);
256         }
257         seenLength += nextLength;
258     } while (++i != end() - 1);
259     if (lateralOffset == 0 || size() < 2) {
260         return back();
261     } else {
262         return positionAtOffset(*(end() - 2), *(end() - 1), (*(end() - 2)).distanceTo(*(end() - 1)), lateralOffset);
263     }
264 }
265 
266 
267 Position
positionAtOffset2D(double pos,double lateralOffset) const268 PositionVector::positionAtOffset2D(double pos, double lateralOffset) const {
269     if (size() == 0) {
270         return Position::INVALID;
271     }
272     const_iterator i = begin();
273     double seenLength = 0;
274     do {
275         const double nextLength = (*i).distanceTo2D(*(i + 1));
276         if (seenLength + nextLength > pos) {
277             return positionAtOffset2D(*i, *(i + 1), pos - seenLength, lateralOffset);
278         }
279         seenLength += nextLength;
280     } while (++i != end() - 1);
281     return back();
282 }
283 
284 
285 double
rotationAtOffset(double pos) const286 PositionVector::rotationAtOffset(double pos) const {
287     if (size() == 0) {
288         return INVALID_DOUBLE;
289     }
290     if (pos < 0) {
291         pos += length();
292     }
293     const_iterator i = begin();
294     double seenLength = 0;
295     do {
296         const Position& p1 = *i;
297         const Position& p2 = *(i + 1);
298         const double nextLength = p1.distanceTo(p2);
299         if (seenLength + nextLength > pos) {
300             return p1.angleTo2D(p2);
301         }
302         seenLength += nextLength;
303     } while (++i != end() - 1);
304     const Position& p1 = (*this)[-2];
305     const Position& p2 = back();
306     return p1.angleTo2D(p2);
307 }
308 
309 
310 double
rotationDegreeAtOffset(double pos) const311 PositionVector::rotationDegreeAtOffset(double pos) const {
312     return GeomHelper::legacyDegree(rotationAtOffset(pos));
313 }
314 
315 
316 double
slopeDegreeAtOffset(double pos) const317 PositionVector::slopeDegreeAtOffset(double pos) const {
318     if (size() == 0) {
319         return INVALID_DOUBLE;
320     }
321     const_iterator i = begin();
322     double seenLength = 0;
323     do {
324         const Position& p1 = *i;
325         const Position& p2 = *(i + 1);
326         const double nextLength = p1.distanceTo(p2);
327         if (seenLength + nextLength > pos) {
328             return RAD2DEG(atan2(p2.z() - p1.z(), p1.distanceTo2D(p2)));
329         }
330         seenLength += nextLength;
331     } while (++i != end() - 1);
332     const Position& p1 = (*this)[-2];
333     const Position& p2 = back();
334     return RAD2DEG(atan2(p2.z() - p1.z(), p1.distanceTo2D(p2)));
335 }
336 
337 
338 Position
positionAtOffset(const Position & p1,const Position & p2,double pos,double lateralOffset)339 PositionVector::positionAtOffset(const Position& p1, const Position& p2, double pos, double lateralOffset) {
340     const double dist = p1.distanceTo(p2);
341     if (pos < 0. || dist < pos) {
342         return Position::INVALID;
343     }
344     if (lateralOffset != 0) {
345         if (dist == 0.) {
346             return Position::INVALID;
347         }
348         const Position offset = sideOffset(p1, p2, -lateralOffset); // move in the same direction as Position::move2side
349         if (pos == 0.) {
350             return p1 + offset;
351         }
352         return p1 + (p2 - p1) * (pos / dist) + offset;
353     }
354     if (pos == 0.) {
355         return p1;
356     }
357     return p1 + (p2 - p1) * (pos / dist);
358 }
359 
360 
361 Position
positionAtOffset2D(const Position & p1,const Position & p2,double pos,double lateralOffset)362 PositionVector::positionAtOffset2D(const Position& p1, const Position& p2, double pos, double lateralOffset) {
363     const double dist = p1.distanceTo2D(p2);
364     if (pos < 0 || dist < pos) {
365         return Position::INVALID;
366     }
367     if (lateralOffset != 0) {
368         const Position offset = sideOffset(p1, p2, -lateralOffset); // move in the same direction as Position::move2side
369         if (pos == 0.) {
370             return p1 + offset;
371         }
372         return p1 + (p2 - p1) * (pos / dist) + offset;
373     }
374     if (pos == 0.) {
375         return p1;
376     }
377     return p1 + (p2 - p1) * (pos / dist);
378 }
379 
380 
381 Boundary
getBoxBoundary() const382 PositionVector::getBoxBoundary() const {
383     Boundary ret;
384     for (const_iterator i = begin(); i != end(); i++) {
385         ret.add(*i);
386     }
387     return ret;
388 }
389 
390 
391 Position
getPolygonCenter() const392 PositionVector::getPolygonCenter() const {
393     double x = 0;
394     double y = 0;
395     double z = 0;
396     for (const_iterator i = begin(); i != end(); i++) {
397         x += (*i).x();
398         y += (*i).y();
399         z += (*i).z();
400     }
401     return Position(x / (double) size(), y / (double) size(), z / (double)size());
402 }
403 
404 
405 Position
getCentroid() const406 PositionVector::getCentroid() const {
407     if (size() == 0) {
408         return Position::INVALID;
409     }
410     PositionVector tmp = *this;
411     if (!isClosed()) { // make sure its closed
412         tmp.push_back(tmp[0]);
413     }
414     const int endIndex = (int)tmp.size() - 1;
415     double div = 0; // 6 * area including sign
416     double x = 0;
417     double y = 0;
418     if (tmp.area() != 0) { // numerical instability ?
419         // http://en.wikipedia.org/wiki/Polygon
420         for (int i = 0; i < endIndex; i++) {
421             const double z = tmp[i].x() * tmp[i + 1].y() - tmp[i + 1].x() * tmp[i].y();
422             div += z; // area formula
423             x += (tmp[i].x() + tmp[i + 1].x()) * z;
424             y += (tmp[i].y() + tmp[i + 1].y()) * z;
425         }
426         div *= 3; //  6 / 2, the 2 comes from the area formula
427         return Position(x / div, y / div);
428     } else {
429         // compute by decomposing into line segments
430         // http://en.wikipedia.org/wiki/Centroid#By_geometric_decomposition
431         double lengthSum = 0;
432         for (int i = 0; i < endIndex; i++) {
433             double length = tmp[i].distanceTo(tmp[i + 1]);
434             x += (tmp[i].x() + tmp[i + 1].x()) * length / 2;
435             y += (tmp[i].y() + tmp[i + 1].y()) * length / 2;
436             lengthSum += length;
437         }
438         if (lengthSum == 0) {
439             // it is probably only one point
440             return tmp[0];
441         }
442         return Position(x / lengthSum, y / lengthSum);
443     }
444 }
445 
446 
447 void
scaleRelative(double factor)448 PositionVector::scaleRelative(double factor) {
449     Position centroid = getCentroid();
450     for (int i = 0; i < static_cast<int>(size()); i++) {
451         (*this)[i] = centroid + (((*this)[i] - centroid) * factor);
452     }
453 }
454 
455 
456 void
scaleAbsolute(double offset)457 PositionVector::scaleAbsolute(double offset) {
458     Position centroid = getCentroid();
459     for (int i = 0; i < static_cast<int>(size()); i++) {
460         (*this)[i] = centroid + (((*this)[i] - centroid) + offset);
461     }
462 }
463 
464 
465 Position
getLineCenter() const466 PositionVector::getLineCenter() const {
467     if (size() == 1) {
468         return (*this)[0];
469     } else {
470         return positionAtOffset(double((length() / 2.)));
471     }
472 }
473 
474 
475 double
length() const476 PositionVector::length() const {
477     if (size() == 0) {
478         return 0;
479     }
480     double len = 0;
481     for (const_iterator i = begin(); i != end() - 1; i++) {
482         len += (*i).distanceTo(*(i + 1));
483     }
484     return len;
485 }
486 
487 
488 double
length2D() const489 PositionVector::length2D() const {
490     if (size() == 0) {
491         return 0;
492     }
493     double len = 0;
494     for (const_iterator i = begin(); i != end() - 1; i++) {
495         len += (*i).distanceTo2D(*(i + 1));
496     }
497     return len;
498 }
499 
500 
501 double
area() const502 PositionVector::area() const {
503     if (size() < 3) {
504         return 0;
505     }
506     double area = 0;
507     PositionVector tmp = *this;
508     if (!isClosed()) { // make sure its closed
509         tmp.push_back(tmp[0]);
510     }
511     const int endIndex = (int)tmp.size() - 1;
512     // http://en.wikipedia.org/wiki/Polygon
513     for (int i = 0; i < endIndex; i++) {
514         area += tmp[i].x() * tmp[i + 1].y() - tmp[i + 1].x() * tmp[i].y();
515     }
516     if (area < 0) { // we whether we had cw or ccw order
517         area *= -1;
518     }
519     return area / 2;
520 }
521 
522 
523 bool
partialWithin(const AbstractPoly & poly,double offset) const524 PositionVector::partialWithin(const AbstractPoly& poly, double offset) const {
525     if (size() < 2) {
526         return false;
527     }
528     for (const_iterator i = begin(); i != end() - 1; i++) {
529         if (poly.around(*i, offset)) {
530             return true;
531         }
532     }
533     return false;
534 }
535 
536 
537 bool
crosses(const Position & p1,const Position & p2) const538 PositionVector::crosses(const Position& p1, const Position& p2) const {
539     return intersects(p1, p2);
540 }
541 
542 
543 std::pair<PositionVector, PositionVector>
splitAt(double where,bool use2D) const544 PositionVector::splitAt(double where, bool use2D) const {
545     const double len = use2D ? length2D() : length();
546     if (size() < 2) {
547         throw InvalidArgument("Vector to short for splitting");
548     }
549     if (where < 0 || where > len) {
550         throw InvalidArgument("Invalid split position " + toString(where) + " for vector of length " + toString(len));
551     }
552     if (where <= POSITION_EPS || where >= len - POSITION_EPS) {
553         WRITE_WARNING("Splitting vector close to end (pos: " + toString(where) + ", length: " + toString(len) + ")");
554     }
555     PositionVector first, second;
556     first.push_back((*this)[0]);
557     double seen = 0;
558     const_iterator it = begin() + 1;
559     double next = use2D ? first.back().distanceTo2D(*it) : first.back().distanceTo(*it);
560     // see how many points we can add to first
561     while (where >= seen + next + POSITION_EPS) {
562         seen += next;
563         first.push_back(*it);
564         it++;
565         next = use2D ? first.back().distanceTo2D(*it) : first.back().distanceTo(*it);
566     }
567     if (fabs(where - (seen + next)) > POSITION_EPS || it == end() - 1) {
568         // we need to insert a new point because 'where' is not close to an
569         // existing point or it is to close to the endpoint
570         const Position p = (use2D
571                             ? positionAtOffset2D(first.back(), *it, where - seen)
572                             : positionAtOffset(first.back(), *it, where - seen));
573         first.push_back(p);
574         second.push_back(p);
575     } else {
576         first.push_back(*it);
577     }
578     // add the remaining points to second
579     for (; it != end(); it++) {
580         second.push_back(*it);
581     }
582     assert(first.size() >= 2);
583     assert(second.size() >= 2);
584     assert(first.back() == second.front());
585     assert(fabs((use2D ? first.length2D() + second.length2D() : first.length() + second.length()) - len) < 2 * POSITION_EPS);
586     return std::pair<PositionVector, PositionVector>(first, second);
587 }
588 
589 
590 std::ostream&
operator <<(std::ostream & os,const PositionVector & geom)591 operator<<(std::ostream& os, const PositionVector& geom) {
592     for (PositionVector::const_iterator i = geom.begin(); i != geom.end(); i++) {
593         if (i != geom.begin()) {
594             os << " ";
595         }
596         os << (*i);
597     }
598     return os;
599 }
600 
601 
602 void
sortAsPolyCWByAngle()603 PositionVector::sortAsPolyCWByAngle() {
604     std::sort(begin(), end(), as_poly_cw_sorter());
605 }
606 
607 
608 void
add(double xoff,double yoff,double zoff)609 PositionVector::add(double xoff, double yoff, double zoff) {
610     for (int i = 0; i < (int)size(); i++) {
611         (*this)[i].add(xoff, yoff, zoff);
612     }
613 }
614 
615 
616 void
sub(const Position & offset)617 PositionVector::sub(const Position& offset) {
618     sub(offset.x(), offset.y(), offset.z());
619 }
620 
621 
622 void
sub(double xoff,double yoff,double zoff)623 PositionVector::sub(double xoff, double yoff, double zoff) {
624     for (int i = 0; i < (int)size(); i++) {
625         (*this)[i].add(-xoff, -yoff, -zoff);
626     }
627 }
628 
629 
630 void
add(const Position & offset)631 PositionVector::add(const Position& offset) {
632     add(offset.x(), offset.y(), offset.z());
633 }
634 
635 
636 PositionVector
added(const Position & offset) const637 PositionVector::added(const Position& offset) const {
638     PositionVector pv;
639     for (auto i1 = begin(); i1 != end(); ++i1) {
640         pv.push_back(*i1 + offset);
641     }
642     return pv;
643 }
644 
645 
646 void
mirrorX()647 PositionVector::mirrorX() {
648     for (int i = 0; i < (int)size(); i++) {
649         (*this)[i].mul(1, -1);
650     }
651 }
652 
653 
as_poly_cw_sorter()654 PositionVector::as_poly_cw_sorter::as_poly_cw_sorter() {}
655 
656 
657 int
operator ()(const Position & p1,const Position & p2) const658 PositionVector::as_poly_cw_sorter::operator()(const Position& p1, const Position& p2) const {
659     return atan2(p1.x(), p1.y()) < atan2(p2.x(), p2.y());
660 }
661 
662 
663 void
sortByIncreasingXY()664 PositionVector::sortByIncreasingXY() {
665     std::sort(begin(), end(), increasing_x_y_sorter());
666 }
667 
668 
increasing_x_y_sorter()669 PositionVector::increasing_x_y_sorter::increasing_x_y_sorter() {}
670 
671 
672 int
operator ()(const Position & p1,const Position & p2) const673 PositionVector::increasing_x_y_sorter::operator()(const Position& p1, const Position& p2) const {
674     if (p1.x() != p2.x()) {
675         return p1.x() < p2.x();
676     }
677     return p1.y() < p2.y();
678 }
679 
680 
681 double
isLeft(const Position & P0,const Position & P1,const Position & P2) const682 PositionVector::isLeft(const Position& P0, const Position& P1,  const Position& P2) const {
683     return (P1.x() - P0.x()) * (P2.y() - P0.y()) - (P2.x() - P0.x()) * (P1.y() - P0.y());
684 }
685 
686 
687 void
append(const PositionVector & v,double sameThreshold)688 PositionVector::append(const PositionVector& v, double sameThreshold) {
689     if ((size() > 0) && (v.size() > 0) && (back().distanceTo(v[0]) < sameThreshold)) {
690         copy(v.begin() + 1, v.end(), back_inserter(*this));
691     } else {
692         copy(v.begin(), v.end(), back_inserter(*this));
693     }
694 }
695 
696 
697 PositionVector
getSubpart(double beginOffset,double endOffset) const698 PositionVector::getSubpart(double beginOffset, double endOffset) const {
699     PositionVector ret;
700     Position begPos = front();
701     if (beginOffset > POSITION_EPS) {
702         begPos = positionAtOffset(beginOffset);
703     }
704     Position endPos = back();
705     if (endOffset < length() - POSITION_EPS) {
706         endPos = positionAtOffset(endOffset);
707     }
708     ret.push_back(begPos);
709 
710     double seen = 0;
711     const_iterator i = begin();
712     // skip previous segments
713     while ((i + 1) != end()
714             &&
715             seen + (*i).distanceTo(*(i + 1)) < beginOffset) {
716         seen += (*i).distanceTo(*(i + 1));
717         i++;
718     }
719     // append segments in between
720     while ((i + 1) != end()
721             &&
722             seen + (*i).distanceTo(*(i + 1)) < endOffset) {
723 
724         ret.push_back_noDoublePos(*(i + 1));
725         seen += (*i).distanceTo(*(i + 1));
726         i++;
727     }
728     // append end
729     ret.push_back_noDoublePos(endPos);
730     if (ret.size() == 1) {
731         ret.push_back(endPos);
732     }
733     return ret;
734 }
735 
736 
737 PositionVector
getSubpart2D(double beginOffset,double endOffset) const738 PositionVector::getSubpart2D(double beginOffset, double endOffset) const {
739     if (size() == 0) {
740         return PositionVector();
741     }
742     PositionVector ret;
743     Position begPos = front();
744     if (beginOffset > POSITION_EPS) {
745         begPos = positionAtOffset2D(beginOffset);
746     }
747     Position endPos = back();
748     if (endOffset < length2D() - POSITION_EPS) {
749         endPos = positionAtOffset2D(endOffset);
750     }
751     ret.push_back(begPos);
752 
753     double seen = 0;
754     const_iterator i = begin();
755     // skip previous segments
756     while ((i + 1) != end()
757             &&
758             seen + (*i).distanceTo2D(*(i + 1)) < beginOffset) {
759         seen += (*i).distanceTo2D(*(i + 1));
760         i++;
761     }
762     // append segments in between
763     while ((i + 1) != end()
764             &&
765             seen + (*i).distanceTo2D(*(i + 1)) < endOffset) {
766 
767         ret.push_back_noDoublePos(*(i + 1));
768         seen += (*i).distanceTo2D(*(i + 1));
769         i++;
770     }
771     // append end
772     ret.push_back_noDoublePos(endPos);
773     if (ret.size() == 1) {
774         ret.push_back(endPos);
775     }
776     return ret;
777 }
778 
779 
780 PositionVector
getSubpartByIndex(int beginIndex,int count) const781 PositionVector::getSubpartByIndex(int beginIndex, int count) const {
782     if (size() == 0) {
783         return PositionVector();
784     }
785     if (beginIndex < 0) {
786         beginIndex += (int)size();
787     }
788     assert(count >= 0);
789     assert(beginIndex < (int)size());
790     assert(beginIndex + count <= (int)size());
791     PositionVector result;
792     for (int i = beginIndex; i < beginIndex + count; ++i) {
793         result.push_back((*this)[i]);
794     }
795     return result;
796 }
797 
798 
799 double
beginEndAngle() const800 PositionVector::beginEndAngle() const {
801     if (size() == 0) {
802         return INVALID_DOUBLE;
803     }
804     return front().angleTo2D(back());
805 }
806 
807 
808 double
nearest_offset_to_point2D(const Position & p,bool perpendicular) const809 PositionVector::nearest_offset_to_point2D(const Position& p, bool perpendicular) const {
810     if (size() == 0) {
811         return INVALID_DOUBLE;
812     }
813     double minDist = std::numeric_limits<double>::max();
814     double nearestPos = GeomHelper::INVALID_OFFSET;
815     double seen = 0;
816     for (const_iterator i = begin(); i != end() - 1; i++) {
817         const double pos =
818             GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, perpendicular);
819         const double dist = pos == GeomHelper::INVALID_OFFSET ? minDist : p.distanceTo2D(positionAtOffset2D(*i, *(i + 1), pos));
820         if (dist < minDist) {
821             nearestPos = pos + seen;
822             minDist = dist;
823         }
824         if (perpendicular && i != begin() && pos == GeomHelper::INVALID_OFFSET) {
825             // even if perpendicular is set we still need to check the distance to the inner points
826             const double cornerDist = p.distanceTo2D(*i);
827             if (cornerDist < minDist) {
828                 const double pos1 =
829                     GeomHelper::nearest_offset_on_line_to_point2D(*(i - 1), *i, p, false);
830                 const double pos2 =
831                     GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, false);
832                 if (pos1 == (*(i - 1)).distanceTo2D(*i) && pos2 == 0.) {
833                     nearestPos = seen;
834                     minDist = cornerDist;
835                 }
836             }
837         }
838         seen += (*i).distanceTo2D(*(i + 1));
839     }
840     return nearestPos;
841 }
842 
843 
844 double
nearest_offset_to_point25D(const Position & p,bool perpendicular) const845 PositionVector::nearest_offset_to_point25D(const Position& p, bool perpendicular) const {
846     if (size() == 0) {
847         return INVALID_DOUBLE;
848     }
849     double minDist = std::numeric_limits<double>::max();
850     double nearestPos = GeomHelper::INVALID_OFFSET;
851     double seen = 0;
852     for (const_iterator i = begin(); i != end() - 1; i++) {
853         const double pos =
854             GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, perpendicular);
855         const double dist = pos == GeomHelper::INVALID_OFFSET ? minDist : p.distanceTo2D(positionAtOffset2D(*i, *(i + 1), pos));
856         if (dist < minDist) {
857             const double pos25D = pos * (*i).distanceTo(*(i + 1)) / (*i).distanceTo2D(*(i + 1));
858             nearestPos = pos25D + seen;
859             minDist = dist;
860         }
861         if (perpendicular && i != begin() && pos == GeomHelper::INVALID_OFFSET) {
862             // even if perpendicular is set we still need to check the distance to the inner points
863             const double cornerDist = p.distanceTo2D(*i);
864             if (cornerDist < minDist) {
865                 const double pos1 =
866                     GeomHelper::nearest_offset_on_line_to_point2D(*(i - 1), *i, p, false);
867                 const double pos2 =
868                     GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, false);
869                 if (pos1 == (*(i - 1)).distanceTo2D(*i) && pos2 == 0.) {
870                     nearestPos = seen;
871                     minDist = cornerDist;
872                 }
873             }
874         }
875         seen += (*i).distanceTo(*(i + 1));
876     }
877     return nearestPos;
878 }
879 
880 
881 Position
transformToVectorCoordinates(const Position & p,bool extend) const882 PositionVector::transformToVectorCoordinates(const Position& p, bool extend) const {
883     if (size() == 0) {
884         return Position::INVALID;
885     }
886     // @toDo this duplicates most of the code in nearest_offset_to_point2D. It should be refactored
887     if (extend) {
888         PositionVector extended = *this;
889         const double dist = 2 * distance2D(p);
890         extended.extrapolate(dist);
891         return extended.transformToVectorCoordinates(p) - Position(dist, 0);
892     }
893     double minDist = std::numeric_limits<double>::max();
894     double nearestPos = -1;
895     double seen = 0;
896     int sign = 1;
897     for (const_iterator i = begin(); i != end() - 1; i++) {
898         const double pos =
899             GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, true);
900         const double dist = pos < 0 ? minDist : p.distanceTo2D(positionAtOffset(*i, *(i + 1), pos));
901         if (dist < minDist) {
902             nearestPos = pos + seen;
903             minDist = dist;
904             sign = isLeft(*i, *(i + 1), p) >= 0 ? -1 : 1;
905         }
906         if (i != begin() && pos == GeomHelper::INVALID_OFFSET) {
907             // even if perpendicular is set we still need to check the distance to the inner points
908             const double cornerDist = p.distanceTo2D(*i);
909             if (cornerDist < minDist) {
910                 const double pos1 =
911                     GeomHelper::nearest_offset_on_line_to_point2D(*(i - 1), *i, p, false);
912                 const double pos2 =
913                     GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, false);
914                 if (pos1 == (*(i - 1)).distanceTo2D(*i) && pos2 == 0.) {
915                     nearestPos = seen;
916                     minDist = cornerDist;
917                     sign = isLeft(*(i - 1), *i, p) >= 0 ? -1 : 1;
918                 }
919             }
920         }
921         seen += (*i).distanceTo2D(*(i + 1));
922     }
923     if (nearestPos != -1) {
924         return Position(nearestPos, sign * minDist);
925     } else {
926         return Position::INVALID;
927     }
928 }
929 
930 
931 int
indexOfClosest(const Position & p) const932 PositionVector::indexOfClosest(const Position& p) const {
933     if (size() == 0) {
934         return -1;
935     }
936     double minDist = std::numeric_limits<double>::max();
937     double dist;
938     int closest = 0;
939     for (int i = 0; i < (int)size(); i++) {
940         dist = p.distanceTo((*this)[i]);
941         if (dist < minDist) {
942             closest = i;
943             minDist = dist;
944         }
945     }
946     return closest;
947 }
948 
949 
950 int
insertAtClosest(const Position & p)951 PositionVector::insertAtClosest(const Position& p) {
952     if (size() == 0) {
953         return -1;
954     }
955     double minDist = std::numeric_limits<double>::max();
956     int insertionIndex = 1;
957     for (int i = 0; i < (int)size() - 1; i++) {
958         const double length = GeomHelper::nearest_offset_on_line_to_point2D((*this)[i], (*this)[i + 1], p, false);
959         const Position& outIntersection = PositionVector::positionAtOffset2D((*this)[i], (*this)[i + 1], length);
960         const double dist = p.distanceTo2D(outIntersection);
961         if (dist < minDist) {
962             insertionIndex = i + 1;
963             minDist = dist;
964         }
965     }
966     insert(begin() + insertionIndex, p);
967     return insertionIndex;
968 }
969 
970 
971 int
removeClosest(const Position & p)972 PositionVector::removeClosest(const Position& p) {
973     if (size() == 0) {
974         return -1;
975     }
976     double minDist = std::numeric_limits<double>::max();
977     int removalIndex = 0;
978     for (int i = 0; i < (int)size(); i++) {
979         const double dist = p.distanceTo2D((*this)[i]);
980         if (dist < minDist) {
981             removalIndex = i;
982             minDist = dist;
983         }
984     }
985     erase(begin() + removalIndex);
986     return removalIndex;
987 }
988 
989 
990 std::vector<double>
intersectsAtLengths2D(const PositionVector & other) const991 PositionVector::intersectsAtLengths2D(const PositionVector& other) const {
992     std::vector<double> ret;
993     if (other.size() == 0) {
994         return ret;
995     }
996     for (const_iterator i = other.begin(); i != other.end() - 1; i++) {
997         std::vector<double> atSegment = intersectsAtLengths2D(*i, *(i + 1));
998         copy(atSegment.begin(), atSegment.end(), back_inserter(ret));
999     }
1000     return ret;
1001 }
1002 
1003 
1004 std::vector<double>
intersectsAtLengths2D(const Position & lp1,const Position & lp2) const1005 PositionVector::intersectsAtLengths2D(const Position& lp1, const Position& lp2) const {
1006     std::vector<double> ret;
1007     if (size() == 0) {
1008         return ret;
1009     }
1010     double pos = 0;
1011     for (const_iterator i = begin(); i != end() - 1; i++) {
1012         const Position& p1 = *i;
1013         const Position& p2 = *(i + 1);
1014         double x, y, m;
1015         if (intersects(p1, p2, lp1, lp2, 0., &x, &y, &m)) {
1016             ret.push_back(Position(x, y).distanceTo2D(p1) + pos);
1017         }
1018         pos += p1.distanceTo2D(p2);
1019     }
1020     return ret;
1021 }
1022 
1023 
1024 void
extrapolate(const double val,const bool onlyFirst,const bool onlyLast)1025 PositionVector::extrapolate(const double val, const bool onlyFirst, const bool onlyLast) {
1026     if (size() > 0) {
1027         Position& p1 = (*this)[0];
1028         Position& p2 = (*this)[1];
1029         const Position offset = (p2 - p1) * (val / p1.distanceTo(p2));
1030         if (!onlyLast) {
1031             p1.sub(offset);
1032         }
1033         if (!onlyFirst) {
1034             if (size() == 2) {
1035                 p2.add(offset);
1036             } else {
1037                 const Position& e1 = (*this)[-2];
1038                 Position& e2 = (*this)[-1];
1039                 e2.sub((e1 - e2) * (val / e1.distanceTo(e2)));
1040             }
1041         }
1042     }
1043 }
1044 
1045 
1046 void
extrapolate2D(const double val,const bool onlyFirst)1047 PositionVector::extrapolate2D(const double val, const bool onlyFirst) {
1048     if (size() > 0) {
1049         Position& p1 = (*this)[0];
1050         Position& p2 = (*this)[1];
1051         if (p1.distanceTo2D(p2) > 0) {
1052             const Position offset = (p2 - p1) * (val / p1.distanceTo2D(p2));
1053             p1.sub(offset);
1054             if (!onlyFirst) {
1055                 if (size() == 2) {
1056                     p2.add(offset);
1057                 } else {
1058                     const Position& e1 = (*this)[-2];
1059                     Position& e2 = (*this)[-1];
1060                     e2.sub((e1 - e2) * (val / e1.distanceTo2D(e2)));
1061                 }
1062             }
1063         }
1064     }
1065 }
1066 
1067 
1068 PositionVector
reverse() const1069 PositionVector::reverse() const {
1070     PositionVector ret;
1071     for (const_reverse_iterator i = rbegin(); i != rend(); i++) {
1072         ret.push_back(*i);
1073     }
1074     return ret;
1075 }
1076 
1077 
1078 Position
sideOffset(const Position & beg,const Position & end,const double amount)1079 PositionVector::sideOffset(const Position& beg, const Position& end, const double amount) {
1080     const double scale = amount / beg.distanceTo2D(end);
1081     return Position((beg.y() - end.y()) * scale, (end.x() - beg.x()) * scale);
1082 }
1083 
1084 
1085 void
move2side(double amount)1086 PositionVector::move2side(double amount) {
1087     if (size() < 2) {
1088         return;
1089     }
1090     removeDoublePoints(POSITION_EPS, true);
1091     if (length2D() == 0) {
1092         return;
1093     }
1094     PositionVector shape;
1095     for (int i = 0; i < static_cast<int>(size()); i++) {
1096         if (i == 0) {
1097             const Position& from = (*this)[i];
1098             const Position& to = (*this)[i + 1];
1099             if (from != to) {
1100                 shape.push_back(from - sideOffset(from, to, amount));
1101             }
1102         } else if (i == static_cast<int>(size()) - 1) {
1103             const Position& from = (*this)[i - 1];
1104             const Position& to = (*this)[i];
1105             if (from != to) {
1106                 shape.push_back(to - sideOffset(from, to, amount));
1107             }
1108         } else {
1109             const Position& from = (*this)[i - 1];
1110             const Position& me = (*this)[i];
1111             const Position& to = (*this)[i + 1];
1112             PositionVector fromMe(from, me);
1113             fromMe.extrapolate2D(me.distanceTo2D(to));
1114             const double extrapolateDev = fromMe[1].distanceTo2D(to);
1115             if (fabs(extrapolateDev) < POSITION_EPS) {
1116                 // parallel case, just shift the middle point
1117                 shape.push_back(me - sideOffset(from, to, amount));
1118             } else if (fabs(extrapolateDev - 2 * me.distanceTo2D(to)) < POSITION_EPS) {
1119                 // counterparallel case, just shift the middle point
1120                 PositionVector fromMe(from, me);
1121                 fromMe.extrapolate2D(amount);
1122                 shape.push_back(fromMe[1]);
1123             } else {
1124                 Position offsets = sideOffset(from, me, amount);
1125                 Position offsets2 = sideOffset(me, to, amount);
1126                 PositionVector l1(from - offsets, me - offsets);
1127                 PositionVector l2(me - offsets2, to - offsets2);
1128                 Position meNew  = l1.intersectionPosition2D(l2[0], l2[1], 100);
1129                 if (meNew == Position::INVALID) {
1130                     throw InvalidArgument("no line intersection");
1131                 }
1132                 meNew = meNew + Position(0, 0, me.z());
1133                 shape.push_back(meNew);
1134             }
1135             // copy original z value
1136             shape.back().set(shape.back().x(), shape.back().y(), me.z());
1137         }
1138     }
1139     *this = shape;
1140 }
1141 
1142 
1143 void
move2side(std::vector<double> amount)1144 PositionVector::move2side(std::vector<double> amount) {
1145     if (size() < 2) {
1146         return;
1147     }
1148     if (length2D() == 0) {
1149         return;
1150     }
1151     if (size() != amount.size()) {
1152         throw InvalidArgument("Numer of offsets (" + toString(amount.size())
1153                               + ") does not match number of points (" + toString(size()) + ")");
1154     }
1155     PositionVector shape;
1156     for (int i = 0; i < static_cast<int>(size()); i++) {
1157         if (i == 0) {
1158             const Position& from = (*this)[i];
1159             const Position& to = (*this)[i + 1];
1160             if (from != to) {
1161                 shape.push_back(from - sideOffset(from, to, amount[i]));
1162             }
1163         } else if (i == static_cast<int>(size()) - 1) {
1164             const Position& from = (*this)[i - 1];
1165             const Position& to = (*this)[i];
1166             if (from != to) {
1167                 shape.push_back(to - sideOffset(from, to, amount[i]));
1168             }
1169         } else {
1170             const Position& from = (*this)[i - 1];
1171             const Position& me = (*this)[i];
1172             const Position& to = (*this)[i + 1];
1173             PositionVector fromMe(from, me);
1174             fromMe.extrapolate2D(me.distanceTo2D(to));
1175             const double extrapolateDev = fromMe[1].distanceTo2D(to);
1176             if (fabs(extrapolateDev) < POSITION_EPS) {
1177                 // parallel case, just shift the middle point
1178                 shape.push_back(me - sideOffset(from, to, amount[i]));
1179             } else if (fabs(extrapolateDev - 2 * me.distanceTo2D(to)) < POSITION_EPS) {
1180                 // counterparallel case, just shift the middle point
1181                 PositionVector fromMe(from, me);
1182                 fromMe.extrapolate2D(amount[i]);
1183                 shape.push_back(fromMe[1]);
1184             } else {
1185                 Position offsets = sideOffset(from, me, amount[i]);
1186                 Position offsets2 = sideOffset(me, to, amount[i]);
1187                 PositionVector l1(from - offsets, me - offsets);
1188                 PositionVector l2(me - offsets2, to - offsets2);
1189                 Position meNew  = l1.intersectionPosition2D(l2[0], l2[1], 100);
1190                 if (meNew == Position::INVALID) {
1191                     throw InvalidArgument("no line intersection");
1192                 }
1193                 meNew = meNew + Position(0, 0, me.z());
1194                 shape.push_back(meNew);
1195             }
1196             // copy original z value
1197             shape.back().set(shape.back().x(), shape.back().y(), me.z());
1198         }
1199     }
1200     *this = shape;
1201 }
1202 
1203 double
angleAt2D(int pos) const1204 PositionVector::angleAt2D(int pos) const {
1205     if ((pos + 1) < (int)size()) {
1206         return (*this)[pos].angleTo2D((*this)[pos + 1]);
1207     } else {
1208         return INVALID_DOUBLE;
1209     }
1210 }
1211 
1212 
1213 void
closePolygon()1214 PositionVector::closePolygon() {
1215     if ((size() != 0) && ((*this)[0] != back())) {
1216         push_back((*this)[0]);
1217     }
1218 }
1219 
1220 
1221 std::vector<double>
distances(const PositionVector & s,bool perpendicular) const1222 PositionVector::distances(const PositionVector& s, bool perpendicular) const {
1223     std::vector<double> ret;
1224     const_iterator i;
1225     for (i = begin(); i != end(); i++) {
1226         const double dist = s.distance2D(*i, perpendicular);
1227         if (dist != GeomHelper::INVALID_OFFSET) {
1228             ret.push_back(dist);
1229         }
1230     }
1231     for (i = s.begin(); i != s.end(); i++) {
1232         const double dist = distance2D(*i, perpendicular);
1233         if (dist != GeomHelper::INVALID_OFFSET) {
1234             ret.push_back(dist);
1235         }
1236     }
1237     return ret;
1238 }
1239 
1240 
1241 double
distance2D(const Position & p,bool perpendicular) const1242 PositionVector::distance2D(const Position& p, bool perpendicular) const {
1243     if (size() == 0) {
1244         return std::numeric_limits<double>::max();
1245     } else if (size() == 1) {
1246         return front().distanceTo(p);
1247     }
1248     const double nearestOffset = nearest_offset_to_point2D(p, perpendicular);
1249     if (nearestOffset == GeomHelper::INVALID_OFFSET) {
1250         return GeomHelper::INVALID_OFFSET;
1251     } else {
1252         return p.distanceTo2D(positionAtOffset2D(nearestOffset));
1253     }
1254 }
1255 
1256 
1257 void
push_back_noDoublePos(const Position & p)1258 PositionVector::push_back_noDoublePos(const Position& p) {
1259     if (size() == 0 || !p.almostSame(back())) {
1260         push_back(p);
1261     }
1262 }
1263 
1264 
1265 void
push_front_noDoublePos(const Position & p)1266 PositionVector::push_front_noDoublePos(const Position& p) {
1267     if (size() == 0 || !p.almostSame(front())) {
1268         insert(begin(), p);
1269     }
1270 }
1271 
1272 
1273 void
insert_noDoublePos(const std::vector<Position>::iterator & at,const Position & p)1274 PositionVector::insert_noDoublePos(const std::vector<Position>::iterator& at, const Position& p) {
1275     if (at == begin()) {
1276         push_front_noDoublePos(p);
1277     } else if (at == end()) {
1278         push_back_noDoublePos(p);
1279     } else {
1280         if (!p.almostSame(*at) && !p.almostSame(*(at - 1))) {
1281             insert(at, p);
1282         }
1283     }
1284 }
1285 
1286 
1287 bool
isClosed() const1288 PositionVector::isClosed() const {
1289     return (size() >= 2) && ((*this)[0] == back());
1290 }
1291 
1292 
1293 bool
isNAN() const1294 PositionVector::isNAN() const {
1295     // iterate over all positions and check if is NAN
1296     for (auto i = begin(); i != end(); i++) {
1297         if (i->isNAN()) {
1298             return true;
1299         }
1300     }
1301     // all ok, then return false
1302     return false;
1303 }
1304 
1305 
1306 void
removeDoublePoints(double minDist,bool assertLength)1307 PositionVector::removeDoublePoints(double minDist, bool assertLength) {
1308     if (size() > 1) {
1309         iterator last = begin();
1310         for (iterator i = begin() + 1; i != end() && (!assertLength || size() > 2);) {
1311             if (last->almostSame(*i, minDist)) {
1312                 i = erase(i);
1313             } else {
1314                 last = i;
1315                 ++i;
1316             }
1317         }
1318     }
1319 }
1320 
1321 
1322 bool
operator ==(const PositionVector & v2) const1323 PositionVector::operator==(const PositionVector& v2) const {
1324     return static_cast<vp>(*this) == static_cast<vp>(v2);
1325 }
1326 
1327 
1328 bool
operator !=(const PositionVector & v2) const1329 PositionVector::operator!=(const PositionVector& v2) const {
1330     return static_cast<vp>(*this) != static_cast<vp>(v2);
1331 }
1332 
1333 PositionVector
operator -(const PositionVector & v2) const1334 PositionVector::operator-(const PositionVector& v2) const {
1335     if (length() != v2.length()) {
1336         WRITE_ERROR("Trying to substract PositionVectors of different lengths.");
1337     }
1338     PositionVector pv;
1339     auto i1 = begin();
1340     auto i2 = v2.begin();
1341     while (i1 != end()) {
1342         pv.add(*i1 - *i2);
1343     }
1344     return pv;
1345 }
1346 
1347 PositionVector
operator +(const PositionVector & v2) const1348 PositionVector::operator+(const PositionVector& v2) const {
1349     if (length() != v2.length()) {
1350         WRITE_ERROR("Trying to substract PositionVectors of different lengths.");
1351     }
1352     PositionVector pv;
1353     auto i1 = begin();
1354     auto i2 = v2.begin();
1355     while (i1 != end()) {
1356         pv.add(*i1 + *i2);
1357     }
1358     return pv;
1359 }
1360 
1361 bool
hasElevation() const1362 PositionVector::hasElevation() const {
1363     if (size() < 2) {
1364         return false;
1365     }
1366     for (const_iterator i = begin(); i != end() - 1; i++) {
1367         if ((*i).z() != (*(i + 1)).z()) {
1368             return true;
1369         }
1370     }
1371     return false;
1372 }
1373 
1374 
1375 bool
intersects(const Position & p11,const Position & p12,const Position & p21,const Position & p22,const double withinDist,double * x,double * y,double * mu)1376 PositionVector::intersects(const Position& p11, const Position& p12, const Position& p21, const Position& p22, const double withinDist, double* x, double* y, double* mu) {
1377     const double eps = std::numeric_limits<double>::epsilon();
1378     const double denominator = (p22.y() - p21.y()) * (p12.x() - p11.x()) - (p22.x() - p21.x()) * (p12.y() - p11.y());
1379     const double numera = (p22.x() - p21.x()) * (p11.y() - p21.y()) - (p22.y() - p21.y()) * (p11.x() - p21.x());
1380     const double numerb = (p12.x() - p11.x()) * (p11.y() - p21.y()) - (p12.y() - p11.y()) * (p11.x() - p21.x());
1381     /* Are the lines coincident? */
1382     if (fabs(numera) < eps && fabs(numerb) < eps && fabs(denominator) < eps) {
1383         double a1;
1384         double a2;
1385         double a3;
1386         double a4;
1387         double a = -1e12;
1388         if (p11.x() != p12.x()) {
1389             a1 = p11.x() < p12.x() ? p11.x() : p12.x();
1390             a2 = p11.x() < p12.x() ? p12.x() : p11.x();
1391             a3 = p21.x() < p22.x() ? p21.x() : p22.x();
1392             a4 = p21.x() < p22.x() ? p22.x() : p21.x();
1393         } else {
1394             a1 = p11.y() < p12.y() ? p11.y() : p12.y();
1395             a2 = p11.y() < p12.y() ? p12.y() : p11.y();
1396             a3 = p21.y() < p22.y() ? p21.y() : p22.y();
1397             a4 = p21.y() < p22.y() ? p22.y() : p21.y();
1398         }
1399         if (a1 <= a3 && a3 <= a2) {
1400             if (a4 < a2) {
1401                 a = (a3 + a4) / 2;
1402             } else {
1403                 a = (a2 + a3) / 2;
1404             }
1405         }
1406         if (a3 <= a1 && a1 <= a4) {
1407             if (a2 < a4) {
1408                 a = (a1 + a2) / 2;
1409             } else {
1410                 a = (a1 + a4) / 2;
1411             }
1412         }
1413         if (a != -1e12) {
1414             if (x != nullptr) {
1415                 if (p11.x() != p12.x()) {
1416                     *mu = (a - p11.x()) / (p12.x() - p11.x());
1417                     *x = a;
1418                     *y = p11.y() + (*mu) * (p12.y() - p11.y());
1419                 } else {
1420                     *x = p11.x();
1421                     *y = a;
1422                     if (p12.y() == p11.y()) {
1423                         *mu = 0;
1424                     } else {
1425                         *mu = (a - p11.y()) / (p12.y() - p11.y());
1426                     }
1427                 }
1428             }
1429             return true;
1430         }
1431         return false;
1432     }
1433     /* Are the lines parallel */
1434     if (fabs(denominator) < eps) {
1435         return false;
1436     }
1437     /* Is the intersection along the segments */
1438     double mua = numera / denominator;
1439     /* reduce rounding errors for lines ending in the same point */
1440     if (fabs(p12.x() - p22.x()) < eps && fabs(p12.y() - p22.y()) < eps) {
1441         mua = 1.;
1442     } else {
1443         const double offseta = withinDist / p11.distanceTo2D(p12);
1444         const double offsetb = withinDist / p21.distanceTo2D(p22);
1445         const double mub = numerb / denominator;
1446         if (mua < -offseta || mua > 1 + offseta || mub < -offsetb || mub > 1 + offsetb) {
1447             return false;
1448         }
1449     }
1450     if (x != nullptr) {
1451         *x = p11.x() + mua * (p12.x() - p11.x());
1452         *y = p11.y() + mua * (p12.y() - p11.y());
1453         *mu = mua;
1454     }
1455     return true;
1456 }
1457 
1458 
1459 void
rotate2D(double angle)1460 PositionVector::rotate2D(double angle) {
1461     const double s = sin(angle);
1462     const double c = cos(angle);
1463     for (int i = 0; i < (int)size(); i++) {
1464         const double x = (*this)[i].x();
1465         const double y = (*this)[i].y();
1466         const double z = (*this)[i].z();
1467         const double xnew = x * c - y * s;
1468         const double ynew = x * s + y * c;
1469         (*this)[i].set(xnew, ynew, z);
1470     }
1471 }
1472 
1473 
1474 PositionVector
simplified() const1475 PositionVector::simplified() const {
1476     PositionVector result = *this;
1477     bool changed = true;
1478     while (changed && result.size() > 3) {
1479         changed = false;
1480         for (int i = 0; i < (int)result.size(); i++) {
1481             const Position& p1 = result[i];
1482             const Position& p2 = result[(i + 2) % result.size()];
1483             const int middleIndex = (i + 1) % result.size();
1484             const Position& p0 = result[middleIndex];
1485             // https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line#Line_defined_by_two_points
1486             const double triangleArea2 = fabs((p2.y() - p1.y()) * p0.x() - (p2.x() - p1.x()) * p0.y() + p2.x() * p1.y()  - p2.y() * p1.x());
1487             const double distIK = p1.distanceTo2D(p2);
1488             if (distIK > NUMERICAL_EPS && triangleArea2 / distIK < NUMERICAL_EPS) {
1489                 changed = true;
1490                 result.erase(result.begin() + middleIndex);
1491                 break;
1492             }
1493         }
1494     }
1495     return result;
1496 }
1497 
1498 
1499 PositionVector
getOrthogonal(const Position & p,double extend,bool before,double length) const1500 PositionVector::getOrthogonal(const Position& p, double extend, bool before, double length) const {
1501     PositionVector result;
1502     PositionVector tmp = *this;
1503     tmp.extrapolate2D(extend);
1504     const double baseOffset = tmp.nearest_offset_to_point2D(p);
1505     if (baseOffset == GeomHelper::INVALID_OFFSET || size() < 2) {
1506         // fail
1507         return result;
1508     }
1509     Position base = tmp.positionAtOffset2D(baseOffset);
1510     const int closestIndex = tmp.indexOfClosest(base);
1511     result.push_back(base);
1512     if (fabs(baseOffset - tmp.offsetAtIndex2D(closestIndex)) > NUMERICAL_EPS) {
1513         result.push_back(tmp[closestIndex]);
1514     } else if (before) {
1515         // take the segment before closestIndex if possible
1516         if (closestIndex > 0) {
1517             result.push_back(tmp[closestIndex - 1]);
1518         } else {
1519             result.push_back(tmp[1]);
1520         }
1521     } else {
1522         // take the segment after closestIndex if possible
1523         if (closestIndex < (int)size() - 1) {
1524             result.push_back(tmp[closestIndex + 1]);
1525         } else {
1526             result.push_back(tmp[-1]);
1527         }
1528     }
1529     result = result.getSubpart2D(0, length);
1530     // rotate around base
1531     result.add(base * -1);
1532     result.rotate2D(DEG2RAD(90));
1533     result.add(base);
1534     return result;
1535 }
1536 
1537 
1538 PositionVector
smoothedZFront(double dist) const1539 PositionVector::smoothedZFront(double dist) const {
1540     PositionVector result = *this;
1541     if (size() == 0) {
1542         return result;
1543     }
1544     const double z0 = (*this)[0].z();
1545     // the z-delta of the first segment
1546     const double dz = (*this)[1].z() - z0;
1547     // if the shape only has 2 points it is as smooth as possible already
1548     if (size() > 2 && dz != 0) {
1549         dist = MIN2(dist, length2D());
1550         // check wether we need to insert a new point at dist
1551         Position pDist = positionAtOffset2D(dist);
1552         int iLast = indexOfClosest(pDist);
1553         // prevent close spacing to reduce impact of rounding errors in z-axis
1554         if (pDist.distanceTo2D((*this)[iLast]) > POSITION_EPS * 20) {
1555             iLast = result.insertAtClosest(pDist);
1556         }
1557         double dist2 = result.offsetAtIndex2D(iLast);
1558         const double dz2 = result[iLast].z() - z0;
1559         double seen = 0;
1560         for (int i = 1; i < iLast; ++i) {
1561             seen += result[i].distanceTo2D(result[i - 1]);
1562             result[i].set(result[i].x(), result[i].y(), z0 + dz2 * seen / dist2);
1563         }
1564     }
1565     return result;
1566 
1567 }
1568 
1569 
1570 PositionVector
interpolateZ(double zStart,double zEnd) const1571 PositionVector::interpolateZ(double zStart, double zEnd) const {
1572     PositionVector result = *this;
1573     if (size() == 0) {
1574         return result;
1575     }
1576     result[0].setz(zStart);
1577     result[-1].setz(zEnd);
1578     const double dz = zEnd - zStart;
1579     const double length = length2D();
1580     double seen = 0;
1581     for (int i = 1; i < (int)size() - 1; ++i) {
1582         seen += result[i].distanceTo2D(result[i - 1]);
1583         result[i].setz(zStart + dz * seen / length);
1584     }
1585     return result;
1586 }
1587 
1588 
1589 PositionVector
resample(double maxLength) const1590 PositionVector::resample(double maxLength) const {
1591     PositionVector result;
1592     if (maxLength == 0) {
1593         return result;
1594     }
1595     const double length = length2D();
1596     if (length < POSITION_EPS) {
1597         return result;
1598     }
1599     maxLength = length / ceil(length / maxLength);
1600     for (double pos = 0; pos <= length; pos += maxLength) {
1601         result.push_back(positionAtOffset2D(pos));
1602     }
1603     return result;
1604 }
1605 
1606 
1607 double
offsetAtIndex2D(int index) const1608 PositionVector::offsetAtIndex2D(int index) const {
1609     if (index < 0 || index >= (int)size()) {
1610         return GeomHelper::INVALID_OFFSET;
1611     }
1612     double seen = 0;
1613     for (int i = 1; i <= index; ++i) {
1614         seen += (*this)[i].distanceTo2D((*this)[i - 1]);
1615     }
1616     return seen;
1617 }
1618 
1619 
1620 double
getMaxGrade(double & maxJump) const1621 PositionVector::getMaxGrade(double& maxJump) const {
1622     double result = 0;
1623     for (int i = 1; i < (int)size(); ++i) {
1624         const Position& p1 = (*this)[i - 1];
1625         const Position& p2 = (*this)[i];
1626         const double distZ = fabs(p1.z() - p2.z());
1627         const double dist2D = p1.distanceTo2D(p2);
1628         if (dist2D == 0) {
1629             maxJump = MAX2(maxJump, distZ);
1630         } else {
1631             result = MAX2(result, distZ / dist2D);
1632         }
1633     }
1634     return result;
1635 }
1636 
1637 
1638 PositionVector
bezier(int numPoints)1639 PositionVector::bezier(int numPoints) {
1640     // inspired by David F. Rogers
1641     assert(size() < 33);
1642     static const double fac[33] = {
1643         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,
1644         6227020800.0, 87178291200.0, 1307674368000.0, 20922789888000.0, 355687428096000.0, 6402373705728000.0,
1645         121645100408832000.0, 2432902008176640000.0, 51090942171709440000.0, 1124000727777607680000.0,
1646         25852016738884976640000.0, 620448401733239439360000.0, 15511210043330985984000000.0,
1647         403291461126605635584000000.0, 10888869450418352160768000000.0, 304888344611713860501504000000.0,
1648         8841761993739701954543616000000.0, 265252859812191058636308480000000.0,
1649         8222838654177922817725562880000000.0, 263130836933693530167218012160000000.0
1650     };
1651     PositionVector ret;
1652     const int npts = (int)size();
1653     // calculate the points on the Bezier curve
1654     const double step = (double) 1.0 / (numPoints - 1);
1655     double t = 0.;
1656     Position prev;
1657     for (int i1 = 0; i1 < numPoints; i1++) {
1658         if ((1.0 - t) < 5e-6) {
1659             t = 1.0;
1660         }
1661         double x = 0., y = 0., z = 0.;
1662         for (int i = 0; i < npts; i++) {
1663             const double ti = (i == 0) ? 1.0 : pow(t, i);
1664             const double tni = (npts == i + 1) ? 1.0 : pow(1 - t, npts - i - 1);
1665             const double basis = fac[npts - 1] / (fac[i] * fac[npts - 1 - i]) * ti * tni;
1666             x += basis * at(i).x();
1667             y += basis * at(i).y();
1668             z += basis * at(i).z();
1669         }
1670         t += step;
1671         Position current(x, y, z);
1672         if (prev != current && !ISNAN(x) && !ISNAN(y) && !ISNAN(z)) {
1673             ret.push_back(current);
1674         }
1675         prev = current;
1676     }
1677     return ret;
1678 }
1679 
1680 
1681 /****************************************************************************/
1682