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