SUMO - Simulation of Urban MObility
PositionVector.cpp
Go to the documentation of this file.
1 /****************************************************************************/
2 // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
3 // Copyright (C) 2001-2018 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 /****************************************************************************/
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>
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 // ===========================================================================
47 
48 // ===========================================================================
49 // method definitions
50 // ===========================================================================
51 
53 
54 
55 PositionVector::PositionVector(const std::vector<Position>& v) {
56  std::copy(v.begin(), v.end(), std::back_inserter(*this));
57 }
58 
59 
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 
66  push_back(p1);
67  push_back(p2);
68 }
69 
70 
72 
73 
74 bool
75 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
106 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
129 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)) {
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
157 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
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
185 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
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&
208 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&
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
246 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
268 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
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
313 }
314 
315 
316 double
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
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
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
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
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
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
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
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
467  if (size() == 1) {
468  return (*this)[0];
469  } else {
470  return positionAtOffset(double((length() / 2.)));
471  }
472 }
473 
474 
475 double
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
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
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
524 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
538 PositionVector::crosses(const Position& p1, const Position& p2) const {
539  return intersects(p1, p2);
540 }
541 
542 
543 std::pair<PositionVector, PositionVector>
544 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&
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
604  std::sort(begin(), end(), as_poly_cw_sorter());
605 }
606 
607 
608 void
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
618  add(offset.x(), offset.y(), offset.z());
619 }
620 
621 
622 void
624  for (int i = 0; i < (int)size(); i++) {
625  (*this)[i].mul(1, -1);
626  }
627 }
628 
629 
631 
632 
633 int
635  return atan2(p1.x(), p1.y()) < atan2(p2.x(), p2.y());
636 }
637 
638 
639 void
641  std::sort(begin(), end(), increasing_x_y_sorter());
642 }
643 
644 
646 
647 
648 int
650  if (p1.x() != p2.x()) {
651  return p1.x() < p2.x();
652  }
653  return p1.y() < p2.y();
654 }
655 
656 
657 double
658 PositionVector::isLeft(const Position& P0, const Position& P1, const Position& P2) const {
659  return (P1.x() - P0.x()) * (P2.y() - P0.y()) - (P2.x() - P0.x()) * (P1.y() - P0.y());
660 }
661 
662 
663 void
664 PositionVector::append(const PositionVector& v, double sameThreshold) {
665  if ((size() > 0) && (v.size() > 0) && (back().distanceTo(v[0]) < sameThreshold)) {
666  copy(v.begin() + 1, v.end(), back_inserter(*this));
667  } else {
668  copy(v.begin(), v.end(), back_inserter(*this));
669  }
670 }
671 
672 
674 PositionVector::getSubpart(double beginOffset, double endOffset) const {
675  PositionVector ret;
676  Position begPos = front();
677  if (beginOffset > POSITION_EPS) {
678  begPos = positionAtOffset(beginOffset);
679  }
680  Position endPos = back();
681  if (endOffset < length() - POSITION_EPS) {
682  endPos = positionAtOffset(endOffset);
683  }
684  ret.push_back(begPos);
685 
686  double seen = 0;
687  const_iterator i = begin();
688  // skip previous segments
689  while ((i + 1) != end()
690  &&
691  seen + (*i).distanceTo(*(i + 1)) < beginOffset) {
692  seen += (*i).distanceTo(*(i + 1));
693  i++;
694  }
695  // append segments in between
696  while ((i + 1) != end()
697  &&
698  seen + (*i).distanceTo(*(i + 1)) < endOffset) {
699 
700  ret.push_back_noDoublePos(*(i + 1));
701  seen += (*i).distanceTo(*(i + 1));
702  i++;
703  }
704  // append end
705  ret.push_back_noDoublePos(endPos);
706  if (ret.size() == 1) {
707  ret.push_back(endPos);
708  }
709  return ret;
710 }
711 
712 
714 PositionVector::getSubpart2D(double beginOffset, double endOffset) const {
715  if (size() == 0) {
716  return PositionVector();
717  }
718  PositionVector ret;
719  Position begPos = front();
720  if (beginOffset > POSITION_EPS) {
721  begPos = positionAtOffset2D(beginOffset);
722  }
723  Position endPos = back();
724  if (endOffset < length2D() - POSITION_EPS) {
725  endPos = positionAtOffset2D(endOffset);
726  }
727  ret.push_back(begPos);
728 
729  double seen = 0;
730  const_iterator i = begin();
731  // skip previous segments
732  while ((i + 1) != end()
733  &&
734  seen + (*i).distanceTo2D(*(i + 1)) < beginOffset) {
735  seen += (*i).distanceTo2D(*(i + 1));
736  i++;
737  }
738  // append segments in between
739  while ((i + 1) != end()
740  &&
741  seen + (*i).distanceTo2D(*(i + 1)) < endOffset) {
742 
743  ret.push_back_noDoublePos(*(i + 1));
744  seen += (*i).distanceTo2D(*(i + 1));
745  i++;
746  }
747  // append end
748  ret.push_back_noDoublePos(endPos);
749  if (ret.size() == 1) {
750  ret.push_back(endPos);
751  }
752  return ret;
753 }
754 
755 
757 PositionVector::getSubpartByIndex(int beginIndex, int count) const {
758  if (size() == 0) {
759  return PositionVector();
760  }
761  if (beginIndex < 0) {
762  beginIndex += (int)size();
763  }
764  assert(count >= 0);
765  assert(beginIndex < (int)size());
766  assert(beginIndex + count <= (int)size());
767  PositionVector result;
768  for (int i = beginIndex; i < beginIndex + count; ++i) {
769  result.push_back((*this)[i]);
770  }
771  return result;
772 }
773 
774 
775 double
777  if (size() == 0) {
778  return INVALID_DOUBLE;
779  }
780  return front().angleTo2D(back());
781 }
782 
783 
784 double
785 PositionVector::nearest_offset_to_point2D(const Position& p, bool perpendicular) const {
786  if (size() == 0) {
787  return INVALID_DOUBLE;
788  }
789  double minDist = std::numeric_limits<double>::max();
790  double nearestPos = GeomHelper::INVALID_OFFSET;
791  double seen = 0;
792  for (const_iterator i = begin(); i != end() - 1; i++) {
793  const double pos =
794  GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, perpendicular);
795  const double dist = pos == GeomHelper::INVALID_OFFSET ? minDist : p.distanceTo2D(positionAtOffset2D(*i, *(i + 1), pos));
796  if (dist < minDist) {
797  nearestPos = pos + seen;
798  minDist = dist;
799  }
800  if (perpendicular && i != begin() && pos == GeomHelper::INVALID_OFFSET) {
801  // even if perpendicular is set we still need to check the distance to the inner points
802  const double cornerDist = p.distanceTo2D(*i);
803  if (cornerDist < minDist) {
804  const double pos1 =
805  GeomHelper::nearest_offset_on_line_to_point2D(*(i - 1), *i, p, false);
806  const double pos2 =
807  GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, false);
808  if (pos1 == (*(i - 1)).distanceTo2D(*i) && pos2 == 0.) {
809  nearestPos = seen;
810  minDist = cornerDist;
811  }
812  }
813  }
814  seen += (*i).distanceTo2D(*(i + 1));
815  }
816  return nearestPos;
817 }
818 
819 
820 Position
822  if (size() == 0) {
823  return Position::INVALID;
824  }
825  // @toDo this duplicates most of the code in nearest_offset_to_point2D. It should be refactored
826  if (extend) {
827  PositionVector extended = *this;
828  const double dist = 2 * distance2D(p);
829  extended.extrapolate(dist);
830  return extended.transformToVectorCoordinates(p) - Position(dist, 0);
831  }
832  double minDist = std::numeric_limits<double>::max();
833  double nearestPos = -1;
834  double seen = 0;
835  int sign = 1;
836  for (const_iterator i = begin(); i != end() - 1; i++) {
837  const double pos =
838  GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, true);
839  const double dist = pos < 0 ? minDist : p.distanceTo2D(positionAtOffset(*i, *(i + 1), pos));
840  if (dist < minDist) {
841  nearestPos = pos + seen;
842  minDist = dist;
843  sign = isLeft(*i, *(i + 1), p) >= 0 ? -1 : 1;
844  }
845  if (i != begin() && pos == GeomHelper::INVALID_OFFSET) {
846  // even if perpendicular is set we still need to check the distance to the inner points
847  const double cornerDist = p.distanceTo2D(*i);
848  if (cornerDist < minDist) {
849  const double pos1 =
850  GeomHelper::nearest_offset_on_line_to_point2D(*(i - 1), *i, p, false);
851  const double pos2 =
852  GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, false);
853  if (pos1 == (*(i - 1)).distanceTo2D(*i) && pos2 == 0.) {
854  nearestPos = seen;
855  minDist = cornerDist;
856  sign = isLeft(*(i - 1), *i, p) >= 0 ? -1 : 1;
857  }
858  }
859  }
860  seen += (*i).distanceTo2D(*(i + 1));
861  }
862  if (nearestPos != -1) {
863  return Position(nearestPos, sign * minDist);
864  } else {
865  return Position::INVALID;
866  }
867 }
868 
869 
870 int
872  if (size() == 0) {
873  return -1;
874  }
875  double minDist = std::numeric_limits<double>::max();
876  double dist;
877  int closest = 0;
878  for (int i = 0; i < (int)size(); i++) {
879  dist = p.distanceTo((*this)[i]);
880  if (dist < minDist) {
881  closest = i;
882  minDist = dist;
883  }
884  }
885  return closest;
886 }
887 
888 
889 int
891  if (size() == 0) {
892  return -1;
893  }
894  double minDist = std::numeric_limits<double>::max();
895  int insertionIndex = 1;
896  for (int i = 0; i < (int)size() - 1; i++) {
897  const double length = GeomHelper::nearest_offset_on_line_to_point2D((*this)[i], (*this)[i + 1], p, false);
898  const Position& outIntersection = PositionVector::positionAtOffset2D((*this)[i], (*this)[i + 1], length);
899  const double dist = p.distanceTo2D(outIntersection);
900  if (dist < minDist) {
901  insertionIndex = i + 1;
902  minDist = dist;
903  }
904  }
905  insert(begin() + insertionIndex, p);
906  return insertionIndex;
907 }
908 
909 
910 int
912  if (size() == 0) {
913  return -1;
914  }
915  double minDist = std::numeric_limits<double>::max();
916  int removalIndex = 0;
917  for (int i = 0; i < (int)size(); i++) {
918  const double dist = p.distanceTo2D((*this)[i]);
919  if (dist < minDist) {
920  removalIndex = i;
921  minDist = dist;
922  }
923  }
924  erase(begin() + removalIndex);
925  return removalIndex;
926 }
927 
928 
929 std::vector<double>
931  std::vector<double> ret;
932  if (other.size() == 0) {
933  return ret;
934  }
935  for (const_iterator i = other.begin(); i != other.end() - 1; i++) {
936  std::vector<double> atSegment = intersectsAtLengths2D(*i, *(i + 1));
937  copy(atSegment.begin(), atSegment.end(), back_inserter(ret));
938  }
939  return ret;
940 }
941 
942 
943 std::vector<double>
945  std::vector<double> ret;
946  if (size() == 0) {
947  return ret;
948  }
949  double pos = 0;
950  for (const_iterator i = begin(); i != end() - 1; i++) {
951  const Position& p1 = *i;
952  const Position& p2 = *(i + 1);
953  double x, y, m;
954  if (intersects(p1, p2, lp1, lp2, 0., &x, &y, &m)) {
955  ret.push_back(Position(x, y).distanceTo2D(p1) + pos);
956  }
957  pos += p1.distanceTo2D(p2);
958  }
959  return ret;
960 }
961 
962 
963 void
964 PositionVector::extrapolate(const double val, const bool onlyFirst, const bool onlyLast) {
965  if (size() > 0) {
966  Position& p1 = (*this)[0];
967  Position& p2 = (*this)[1];
968  const Position offset = (p2 - p1) * (val / p1.distanceTo(p2));
969  if (!onlyLast) {
970  p1.sub(offset);
971  }
972  if (!onlyFirst) {
973  if (size() == 2) {
974  p2.add(offset);
975  } else {
976  const Position& e1 = (*this)[-2];
977  Position& e2 = (*this)[-1];
978  e2.sub((e1 - e2) * (val / e1.distanceTo(e2)));
979  }
980  }
981  }
982 }
983 
984 
985 void
986 PositionVector::extrapolate2D(const double val, const bool onlyFirst) {
987  if (size() > 0) {
988  Position& p1 = (*this)[0];
989  Position& p2 = (*this)[1];
990  if (p1.distanceTo2D(p2) > 0) {
991  const Position offset = (p2 - p1) * (val / p1.distanceTo2D(p2));
992  p1.sub(offset);
993  if (!onlyFirst) {
994  if (size() == 2) {
995  p2.add(offset);
996  } else {
997  const Position& e1 = (*this)[-2];
998  Position& e2 = (*this)[-1];
999  e2.sub((e1 - e2) * (val / e1.distanceTo2D(e2)));
1000  }
1001  }
1002  }
1003  }
1004 }
1005 
1006 
1009  PositionVector ret;
1010  for (const_reverse_iterator i = rbegin(); i != rend(); i++) {
1011  ret.push_back(*i);
1012  }
1013  return ret;
1014 }
1015 
1016 
1017 Position
1018 PositionVector::sideOffset(const Position& beg, const Position& end, const double amount) {
1019  const double scale = amount / beg.distanceTo2D(end);
1020  return Position((beg.y() - end.y()) * scale, (end.x() - beg.x()) * scale);
1021 }
1022 
1023 
1024 void
1026  if (size() < 2) {
1027  return;
1028  }
1030  if (length2D() == 0) {
1031  return;
1032  }
1033  PositionVector shape;
1034  for (int i = 0; i < static_cast<int>(size()); i++) {
1035  if (i == 0) {
1036  const Position& from = (*this)[i];
1037  const Position& to = (*this)[i + 1];
1038  if (from != to) {
1039  shape.push_back(from - sideOffset(from, to, amount));
1040  }
1041  } else if (i == static_cast<int>(size()) - 1) {
1042  const Position& from = (*this)[i - 1];
1043  const Position& to = (*this)[i];
1044  if (from != to) {
1045  shape.push_back(to - sideOffset(from, to, amount));
1046  }
1047  } else {
1048  const Position& from = (*this)[i - 1];
1049  const Position& me = (*this)[i];
1050  const Position& to = (*this)[i + 1];
1051  PositionVector fromMe(from, me);
1052  fromMe.extrapolate2D(me.distanceTo2D(to));
1053  const double extrapolateDev = fromMe[1].distanceTo2D(to);
1054  if (fabs(extrapolateDev) < POSITION_EPS) {
1055  // parallel case, just shift the middle point
1056  shape.push_back(me - sideOffset(from, to, amount));
1057  } else if (fabs(extrapolateDev - 2 * me.distanceTo2D(to)) < POSITION_EPS) {
1058  // counterparallel case, just shift the middle point
1059  PositionVector fromMe(from, me);
1060  fromMe.extrapolate2D(amount);
1061  shape.push_back(fromMe[1]);
1062  } else {
1063  Position offsets = sideOffset(from, me, amount);
1064  Position offsets2 = sideOffset(me, to, amount);
1065  PositionVector l1(from - offsets, me - offsets);
1066  PositionVector l2(me - offsets2, to - offsets2);
1067  Position meNew = l1.intersectionPosition2D(l2[0], l2[1], 100);
1068  if (meNew == Position::INVALID) {
1069  throw InvalidArgument("no line intersection");
1070  }
1071  meNew = meNew + Position(0, 0, me.z());
1072  shape.push_back(meNew);
1073  }
1074  // copy original z value
1075  shape.back().set(shape.back().x(), shape.back().y(), me.z());
1076  }
1077  }
1078  *this = shape;
1079 }
1080 
1081 
1082 void
1083 PositionVector::move2side(std::vector<double> amount) {
1084  if (size() < 2) {
1085  return;
1086  }
1087  if (length2D() == 0) {
1088  return;
1089  }
1090  if (size() != amount.size()) {
1091  throw InvalidArgument("Numer of offsets (" + toString(amount.size())
1092  + ") does not match number of points (" + toString(size()) + ")");
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[i]));
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[i]));
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[i]));
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[i]);
1122  shape.push_back(fromMe[1]);
1123  } else {
1124  Position offsets = sideOffset(from, me, amount[i]);
1125  Position offsets2 = sideOffset(me, to, amount[i]);
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 double
1144  if ((pos + 1) < (int)size()) {
1145  return (*this)[pos].angleTo2D((*this)[pos + 1]);
1146  } else {
1147  return INVALID_DOUBLE;
1148  }
1149 }
1150 
1151 
1152 void
1154  if ((size() != 0) && ((*this)[0] != back())) {
1155  push_back((*this)[0]);
1156  }
1157 }
1158 
1159 
1160 std::vector<double>
1161 PositionVector::distances(const PositionVector& s, bool perpendicular) const {
1162  std::vector<double> ret;
1163  const_iterator i;
1164  for (i = begin(); i != end(); i++) {
1165  const double dist = s.distance2D(*i, perpendicular);
1166  if (dist != GeomHelper::INVALID_OFFSET) {
1167  ret.push_back(dist);
1168  }
1169  }
1170  for (i = s.begin(); i != s.end(); i++) {
1171  const double dist = distance2D(*i, perpendicular);
1172  if (dist != GeomHelper::INVALID_OFFSET) {
1173  ret.push_back(dist);
1174  }
1175  }
1176  return ret;
1177 }
1178 
1179 
1180 double
1181 PositionVector::distance2D(const Position& p, bool perpendicular) const {
1182  if (size() == 0) {
1183  return std::numeric_limits<double>::max();
1184  } else if (size() == 1) {
1185  return front().distanceTo(p);
1186  }
1187  const double nearestOffset = nearest_offset_to_point2D(p, perpendicular);
1188  if (nearestOffset == GeomHelper::INVALID_OFFSET) {
1190  } else {
1191  return p.distanceTo2D(positionAtOffset2D(nearestOffset));
1192  }
1193 }
1194 
1195 
1196 void
1198  if (size() == 0 || !p.almostSame(back())) {
1199  push_back(p);
1200  }
1201 }
1202 
1203 
1204 void
1206  if (size() == 0 || !p.almostSame(front())) {
1207  insert(begin(), p);
1208  }
1209 }
1210 
1211 
1212 void
1213 PositionVector::insert_noDoublePos(const std::vector<Position>::iterator& at, const Position& p) {
1214  if (at == begin()) {
1216  } else if (at == end()) {
1218  } else {
1219  if (!p.almostSame(*at) && !p.almostSame(*(at - 1))) {
1220  insert(at, p);
1221  }
1222  }
1223 }
1224 
1225 
1226 bool
1228  return (size() >= 2) && ((*this)[0] == back());
1229 }
1230 
1231 
1232 bool
1234  // iterate over all positions and check if is NAN
1235  for (auto i = begin(); i != end(); i++) {
1236  if (i->isNAN()) {
1237  return true;
1238  }
1239  }
1240  // all ok, then return false
1241  return false;
1242 }
1243 
1244 
1245 void
1246 PositionVector::removeDoublePoints(double minDist, bool assertLength) {
1247  if (size() > 1) {
1248  iterator last = begin();
1249  for (iterator i = begin() + 1; i != end() && (!assertLength || size() > 2);) {
1250  if (last->almostSame(*i, minDist)) {
1251  i = erase(i);
1252  } else {
1253  last = i;
1254  ++i;
1255  }
1256  }
1257  }
1258 }
1259 
1260 
1261 bool
1263  return static_cast<vp>(*this) == static_cast<vp>(v2);
1264 }
1265 
1266 
1267 bool
1269  return static_cast<vp>(*this) != static_cast<vp>(v2);
1270 }
1271 
1274  if (length() != v2.length()) {
1275  WRITE_ERROR("Trying to substract PositionVectors of different lengths.");
1276  }
1277  PositionVector pv;
1278  auto i1 = begin();
1279  auto i2 = v2.begin();
1280  while (i1 != end()) {
1281  pv.add(*i1 - *i2);
1282  }
1283  return pv;
1284 }
1285 
1288  if (length() != v2.length()) {
1289  WRITE_ERROR("Trying to substract PositionVectors of different lengths.");
1290  }
1291  PositionVector pv;
1292  auto i1 = begin();
1293  auto i2 = v2.begin();
1294  while (i1 != end()) {
1295  pv.add(*i1 + *i2);
1296  }
1297  return pv;
1298 }
1299 
1300 bool
1302  if (size() < 2) {
1303  return false;
1304  }
1305  for (const_iterator i = begin(); i != end() - 1; i++) {
1306  if ((*i).z() != (*(i + 1)).z()) {
1307  return true;
1308  }
1309  }
1310  return false;
1311 }
1312 
1313 
1314 bool
1315 PositionVector::intersects(const Position& p11, const Position& p12, const Position& p21, const Position& p22, const double withinDist, double* x, double* y, double* mu) {
1316  const double eps = std::numeric_limits<double>::epsilon();
1317  const double denominator = (p22.y() - p21.y()) * (p12.x() - p11.x()) - (p22.x() - p21.x()) * (p12.y() - p11.y());
1318  const double numera = (p22.x() - p21.x()) * (p11.y() - p21.y()) - (p22.y() - p21.y()) * (p11.x() - p21.x());
1319  const double numerb = (p12.x() - p11.x()) * (p11.y() - p21.y()) - (p12.y() - p11.y()) * (p11.x() - p21.x());
1320  /* Are the lines coincident? */
1321  if (fabs(numera) < eps && fabs(numerb) < eps && fabs(denominator) < eps) {
1322  double a1;
1323  double a2;
1324  double a3;
1325  double a4;
1326  double a = -1e12;
1327  if (p11.x() != p12.x()) {
1328  a1 = p11.x() < p12.x() ? p11.x() : p12.x();
1329  a2 = p11.x() < p12.x() ? p12.x() : p11.x();
1330  a3 = p21.x() < p22.x() ? p21.x() : p22.x();
1331  a4 = p21.x() < p22.x() ? p22.x() : p21.x();
1332  } else {
1333  a1 = p11.y() < p12.y() ? p11.y() : p12.y();
1334  a2 = p11.y() < p12.y() ? p12.y() : p11.y();
1335  a3 = p21.y() < p22.y() ? p21.y() : p22.y();
1336  a4 = p21.y() < p22.y() ? p22.y() : p21.y();
1337  }
1338  if (a1 <= a3 && a3 <= a2) {
1339  if (a4 < a2) {
1340  a = (a3 + a4) / 2;
1341  } else {
1342  a = (a2 + a3) / 2;
1343  }
1344  }
1345  if (a3 <= a1 && a1 <= a4) {
1346  if (a2 < a4) {
1347  a = (a1 + a2) / 2;
1348  } else {
1349  a = (a1 + a4) / 2;
1350  }
1351  }
1352  if (a != -1e12) {
1353  if (x != nullptr) {
1354  if (p11.x() != p12.x()) {
1355  *mu = (a - p11.x()) / (p12.x() - p11.x());
1356  *x = a;
1357  *y = p11.y() + (*mu) * (p12.y() - p11.y());
1358  } else {
1359  *x = p11.x();
1360  *y = a;
1361  if (p12.y() == p11.y()) {
1362  *mu = 0;
1363  } else {
1364  *mu = (a - p11.y()) / (p12.y() - p11.y());
1365  }
1366  }
1367  }
1368  return true;
1369  }
1370  return false;
1371  }
1372  /* Are the lines parallel */
1373  if (fabs(denominator) < eps) {
1374  return false;
1375  }
1376  /* Is the intersection along the segments */
1377  double mua = numera / denominator;
1378  /* reduce rounding errors for lines ending in the same point */
1379  if (fabs(p12.x() - p22.x()) < eps && fabs(p12.y() - p22.y()) < eps) {
1380  mua = 1.;
1381  } else {
1382  const double offseta = withinDist / p11.distanceTo2D(p12);
1383  const double offsetb = withinDist / p21.distanceTo2D(p22);
1384  const double mub = numerb / denominator;
1385  if (mua < -offseta || mua > 1 + offseta || mub < -offsetb || mub > 1 + offsetb) {
1386  return false;
1387  }
1388  }
1389  if (x != nullptr) {
1390  *x = p11.x() + mua * (p12.x() - p11.x());
1391  *y = p11.y() + mua * (p12.y() - p11.y());
1392  *mu = mua;
1393  }
1394  return true;
1395 }
1396 
1397 
1398 void
1400  const double s = sin(angle);
1401  const double c = cos(angle);
1402  for (int i = 0; i < (int)size(); i++) {
1403  const double x = (*this)[i].x();
1404  const double y = (*this)[i].y();
1405  const double z = (*this)[i].z();
1406  const double xnew = x * c - y * s;
1407  const double ynew = x * s + y * c;
1408  (*this)[i].set(xnew, ynew, z);
1409  }
1410 }
1411 
1412 
1415  PositionVector result = *this;
1416  bool changed = true;
1417  while (changed && result.size() > 3) {
1418  changed = false;
1419  for (int i = 0; i < (int)result.size(); i++) {
1420  const Position& p1 = result[i];
1421  const Position& p2 = result[(i + 2) % result.size()];
1422  const int middleIndex = (i + 1) % result.size();
1423  const Position& p0 = result[middleIndex];
1424  // https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line#Line_defined_by_two_points
1425  const double triangleArea2 = fabs((p2.y() - p1.y()) * p0.x() - (p2.x() - p1.x()) * p0.y() + p2.x() * p1.y() - p2.y() * p1.x());
1426  const double distIK = p1.distanceTo2D(p2);
1427  if (distIK > NUMERICAL_EPS && triangleArea2 / distIK < NUMERICAL_EPS) {
1428  changed = true;
1429  result.erase(result.begin() + middleIndex);
1430  break;
1431  }
1432  }
1433  }
1434  return result;
1435 }
1436 
1437 
1439 PositionVector::getOrthogonal(const Position& p, double extend, bool before, double length) const {
1440  PositionVector result;
1441  PositionVector tmp = *this;
1442  tmp.extrapolate2D(extend);
1443  const double baseOffset = tmp.nearest_offset_to_point2D(p);
1444  if (baseOffset == GeomHelper::INVALID_OFFSET || size() < 2) {
1445  // fail
1446  return result;
1447  }
1448  Position base = tmp.positionAtOffset2D(baseOffset);
1449  const int closestIndex = tmp.indexOfClosest(base);
1450  result.push_back(base);
1451  if (fabs(baseOffset - tmp.offsetAtIndex2D(closestIndex)) > NUMERICAL_EPS) {
1452  result.push_back(tmp[closestIndex]);
1453  } else if (before) {
1454  // take the segment before closestIndex if possible
1455  if (closestIndex > 0) {
1456  result.push_back(tmp[closestIndex - 1]);
1457  } else {
1458  result.push_back(tmp[1]);
1459  }
1460  } else {
1461  // take the segment after closestIndex if possible
1462  if (closestIndex < (int)size() - 1) {
1463  result.push_back(tmp[closestIndex + 1]);
1464  } else {
1465  result.push_back(tmp[-1]);
1466  }
1467  }
1468  result = result.getSubpart2D(0, length);
1469  // rotate around base
1470  result.add(base * -1);
1471  result.rotate2D(DEG2RAD(90));
1472  result.add(base);
1473  return result;
1474 }
1475 
1476 
1479  PositionVector result = *this;
1480  if (size() == 0) {
1481  return result;
1482  }
1483  const double z0 = (*this)[0].z();
1484  // the z-delta of the first segment
1485  const double dz = (*this)[1].z() - z0;
1486  // if the shape only has 2 points it is as smooth as possible already
1487  if (size() > 2 && dz != 0) {
1488  dist = MIN2(dist, length2D());
1489  // check wether we need to insert a new point at dist
1490  Position pDist = positionAtOffset2D(dist);
1491  int iLast = indexOfClosest(pDist);
1492  // prevent close spacing to reduce impact of rounding errors in z-axis
1493  if (pDist.distanceTo2D((*this)[iLast]) > POSITION_EPS * 20) {
1494  iLast = result.insertAtClosest(pDist);
1495  }
1496  double dist2 = result.offsetAtIndex2D(iLast);
1497  const double dz2 = result[iLast].z() - z0;
1498  double seen = 0;
1499  for (int i = 1; i < iLast; ++i) {
1500  seen += result[i].distanceTo2D(result[i - 1]);
1501  result[i].set(result[i].x(), result[i].y(), z0 + dz2 * seen / dist2);
1502  }
1503  }
1504  return result;
1505 
1506 }
1507 
1508 
1510 PositionVector::interpolateZ(double zStart, double zEnd) const {
1511  PositionVector result = *this;
1512  if (size() == 0) {
1513  return result;
1514  }
1515  result[0].setz(zStart);
1516  result[-1].setz(zEnd);
1517  const double dz = zEnd - zStart;
1518  const double length = length2D();
1519  double seen = 0;
1520  for (int i = 1; i < (int)size() - 1; ++i) {
1521  seen += result[i].distanceTo2D(result[i - 1]);
1522  result[i].setz(zStart + dz * seen / length);
1523  }
1524  return result;
1525 }
1526 
1528 PositionVector::resample(double maxLength) const {
1529  PositionVector result;
1530  if (maxLength == 0) {
1531  return result;
1532  }
1533  const double length = length2D();
1534  if (length < POSITION_EPS) {
1535  return result;
1536  }
1537  maxLength = length / ceil(length / maxLength);
1538  for (double pos = 0; pos <= length; pos += maxLength) {
1539  result.push_back(positionAtOffset2D(pos));
1540  }
1541  return result;
1542 }
1543 
1544 double
1546  if (index < 0 || index >= (int)size()) {
1548  }
1549  double seen = 0;
1550  for (int i = 1; i <= index; ++i) {
1551  seen += (*this)[i].distanceTo2D((*this)[i - 1]);
1552  }
1553  return seen;
1554 }
1555 
1556 
1557 double
1558 PositionVector::getMaxGrade(double& maxJump) const {
1559  double result = 0;
1560  for (int i = 1; i < (int)size(); ++i) {
1561  const Position& p1 = (*this)[i - 1];
1562  const Position& p2 = (*this)[i];
1563  const double distZ = fabs(p1.z() - p2.z());
1564  const double dist2D = p1.distanceTo2D(p2);
1565  if (dist2D == 0) {
1566  maxJump = MAX2(maxJump, distZ);
1567  } else {
1568  result = MAX2(result, distZ / dist2D);
1569  }
1570  }
1571  return result;
1572 }
1573 
1574 /****************************************************************************/
1575 
PositionVector operator+(const PositionVector &v2) const
adds two vectors (requires vectors of the same length)
bool around(const Position &p, double offset=0) const
Returns the information whether the position vector describes a polygon lying around the given point...
static const PositionVector EMPTY
empty Vector
clase for increasing Sorter
double rotationDegreeAtOffset(double pos) const
Returns the rotation at the given length.
virtual bool partialWithin(const AbstractPoly &poly, double offset=0) const =0
Returns whether the AbstractPoly is partially within the given polygon.
int operator()(const Position &p1, const Position &p2) const
comparing operation
bool almostSame(const Position &p2, double maxDiv=POSITION_EPS) const
check if two position is almost the sme as other
Definition: Position.h:229
double length2D() const
Returns the length.
void append(const PositionVector &v, double sameThreshold=2.0)
PositionVector getOrthogonal(const Position &p, double extend, bool before, double length=1.0) const
return orthogonal through p (extending this vector if necessary)
double z() const
Returns the z-position.
Definition: Position.h:67
void sortAsPolyCWByAngle()
short as polygon CV by angle
double distance2D(const Position &p, bool perpendicular=false) const
closest 2D-distance to point p (or -1 if perpendicular is true and the point is beyond this vector) ...
void add(const Position &pos)
Adds the given position to this one.
Definition: Position.h:127
double getMaxGrade(double &maxJump) const
int indexOfClosest(const Position &p) const
index of the closest position to p
double distanceTo2D(const Position &p2) const
returns the euclidean distance in the x-y-plane
Definition: Position.h:244
friend std::ostream & operator<<(std::ostream &os, const PositionVector &geom)
Position positionAtOffset2D(double pos, double lateralOffset=0) const
Returns the position at the given length.
PositionVector resample(double maxLength) const
resample shape with the given number of points (equal spacing)
Position intersectionPosition2D(const Position &p1, const Position &p2, const double withinDist=0.) const
Returns the position of the intersection.
std::pair< PositionVector, PositionVector > splitAt(double where, bool use2D=false) const
Returns the two lists made when this list vector is splitted at the given point.
double y() const
Returns the y-position.
Definition: Position.h:62
double x() const
Returns the x-position.
Definition: Position.h:57
virtual bool crosses(const Position &p1, const Position &p2) const =0
Returns whether the AbstractPoly crosses the given line.
double angleTo2D(const Position &other) const
returns the angle in the plane of the vector pointing from here to the other position ...
Definition: Position.h:254
Position getCentroid() const
Returns the centroid (closes the polygon if unclosed)
T MAX2(T a, T b)
Definition: StdDefs.h:76
bool partialWithin(const AbstractPoly &poly, double offset=0) const
Returns the information whether this polygon lies partially within the given polygon.
PositionVector reverse() const
reverse position vector
double rotationAtOffset(double pos) const
Returns the rotation at the given length.
PositionVector interpolateZ(double zStart, double zEnd) const
returned vector that varies z smoothly over its length
#define RAD2DEG(x)
Definition: GeomHelper.h:39
void insert_noDoublePos(const std::vector< Position >::iterator &at, const Position &p)
insert in front a non double position
std::vector< double > distances(const PositionVector &s, bool perpendicular=false) const
distances of all my points to s and all of s points to myself
bool hasElevation() const
return whether two positions differ in z-coordinate
bool operator!=(const PositionVector &v2) const
comparing operation
A class that stores a 2D geometrical boundary.
Definition: Boundary.h:42
#define WRITE_WARNING(msg)
Definition: MsgHandler.h:241
Position getLineCenter() const
get line center
static double legacyDegree(const double angle, const bool positive=false)
Definition: GeomHelper.cpp:203
double area() const
Returns the area (0 for non-closed)
~PositionVector()
Destructor.
void extrapolate2D(const double val, const bool onlyFirst=false)
extrapolate position vector in two dimensions (Z is ignored)
void push_front_noDoublePos(const Position &p)
insert in front a non double position
void removeDoublePoints(double minDist=POSITION_EPS, bool assertLength=false)
Removes positions if too near.
static double nearest_offset_on_line_to_point2D(const Position &lineStart, const Position &lineEnd, const Position &p, bool perpendicular=true)
Definition: GeomHelper.cpp:89
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
Definition: ToString.h:49
void scaleAbsolute(double offset)
enlarges/shrinks the polygon by an absolute offset based at the centroid
bool operator==(const PositionVector &v2) const
comparing operation
double beginEndAngle() const
returns the angle in radians of the line connecting the first and the last position ...
A point in 2D or 3D with translation and scaling methods.
Definition: Position.h:39
std::vector< Position > vp
vector of position
A list of positions.
Position getPolygonCenter() const
Returns the arithmetic of all corner points.
int operator()(const Position &p1, const Position &p2) const
comparing operation for sort
virtual bool around(const Position &p, double offset=0) const =0
Returns whether the AbstractPoly the given coordinate.
static double angle2D(const Position &p1, const Position &p2)
Returns the angle between two vectors on a plane The angle is from vector 1 to vector 2...
Definition: GeomHelper.cpp:83
T MIN2(T a, T b)
Definition: StdDefs.h:70
#define POSITION_EPS
Definition: config.h:172
std::vector< double > intersectsAtLengths2D(const PositionVector &other) const
For all intersections between this vector and other, return the 2D-length of the subvector from this ...
const Position & operator[](int index) const
returns the constat position at the given index !!! exceptions?
int insertAtClosest(const Position &p)
inserts p between the two closest positions and returns the insertion index
PositionVector getSubpart(double beginOffset, double endOffset) const
get subpart of a position vector
bool overlapsWith(const AbstractPoly &poly, double offset=0) const
Returns the information whether the given polygon overlaps with this.
#define DEG2RAD(x)
Definition: GeomHelper.h:38
PositionVector smoothedZFront(double dist=std::numeric_limits< double >::max()) const
returned vector that is smoothed at the front (within dist)
static const double INVALID_OFFSET
a value to signify offsets outside the range of [0, Line.length()]
Definition: GeomHelper.h:52
PositionVector getSubpartByIndex(int beginIndex, int count) const
get subpart of a position vector using index and a cout
void sortByIncreasingXY()
shory by increasing X-Y Psitions
void move2side(double amount)
move position vector to side using certain ammount
PositionVector operator-(const PositionVector &v2) const
substracts two vectors (requires vectors of the same length)
PositionVector simplified() const
return the same shape with intermediate colinear points removed
double slopeDegreeAtOffset(double pos) const
Returns the slope at the given length.
PositionVector()
Constructor. Creates an empty position vector.
#define WRITE_ERROR(msg)
Definition: MsgHandler.h:247
double isLeft(const Position &P0, const Position &P1, const Position &P2) const
get left
PositionVector getSubpart2D(double beginOffset, double endOffset) const
get subpart of a position vector in two dimensions (Z is ignored)
void rotate2D(double angle)
void extrapolate(const double val, const bool onlyFirst=false, const bool onlyLast=false)
extrapolate position vector
double length() const
Returns the length.
#define M_PI
Definition: odrSpiral.cpp:40
bool isClosed() const
check if PositionVector is closed
static Position sideOffset(const Position &beg, const Position &end, const double amount)
get a side position of position vector using a offset
void scaleRelative(double factor)
enlarges/shrinks the polygon by a factor based at the centroid
bool isNAN() const
check if PositionVector is NAN
double angleAt2D(int pos) const
get angle in certain position of position vector
double offsetAtIndex2D(int index) const
return the offset at the given index
double distanceTo(const Position &p2) const
returns the euclidean distance in 3 dimension
Definition: Position.h:234
Boundary getBoxBoundary() const
Returns a boundary enclosing this list of lines.
#define NUMERICAL_EPS
Definition: config.h:148
void push_back_noDoublePos(const Position &p)
insert in back a non double position
const double INVALID_DOUBLE
Definition: StdDefs.h:62
bool crosses(const Position &p1, const Position &p2) const
Returns whether the AbstractPoly crosses the given line.
double getOverlapWith(const PositionVector &poly, double zThreshold) const
Returns the maximum overlaps between this and the given polygon (when not separated by at least zThre...
void add(double x, double y, double z=0)
Makes the boundary include the given coordinate.
Definition: Boundary.cpp:79
void add(double xoff, double yoff, double zoff)
double nearest_offset_to_point2D(const Position &p, bool perpendicular=true) const
return the nearest offest to point 2D
void closePolygon()
ensures that the last position equals the first
Position positionAtOffset(double pos, double lateralOffset=0) const
Returns the position at the given length.
int removeClosest(const Position &p)
removes the point closest to p and return the removal index
Position transformToVectorCoordinates(const Position &p, bool extend=false) const
return position p within the length-wise coordinate system defined by this position vector...
bool intersects(const Position &p1, const Position &p2) const
Returns the information whether this list of points interesects the given line.
static const Position INVALID
used to indicate that a position is valid
Definition: Position.h:285
void sub(double dx, double dy)
Substracts the given position from this one.
Definition: Position.h:147