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