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