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