SUMO - Simulation of Urban MObility
PositionVector.cpp
Go to the documentation of this file.
1 /****************************************************************************/
10 // A list of positions
11 /****************************************************************************/
12 // SUMO, Simulation of Urban MObility; see http://sumo.dlr.de/
13 // Copyright (C) 2001-2017 DLR (http://www.dlr.de/) and contributors
14 /****************************************************************************/
15 //
16 // This file is part of SUMO.
17 // SUMO is free software: you can redistribute it and/or modify
18 // it under the terms of the GNU General Public License as published by
19 // the Free Software Foundation, either version 3 of the License, or
20 // (at your option) any later version.
21 //
22 /****************************************************************************/
23 
24 
25 // ===========================================================================
26 // included modules
27 // ===========================================================================
28 #ifdef _MSC_VER
29 #include <windows_config.h>
30 #else
31 #include <config.h>
32 #endif
33 
34 #include <queue>
35 #include <cmath>
36 #include <iostream>
37 #include <algorithm>
38 #include <cassert>
39 #include <iterator>
40 #include <limits>
41 #include <utils/common/StdDefs.h>
43 #include <utils/common/ToString.h>
44 #include "AbstractPoly.h"
45 #include "Position.h"
46 #include "PositionVector.h"
47 #include "GeomHelper.h"
48 #include "Helper_ConvexHull.h"
49 #include "Boundary.h"
50 
51 // ===========================================================================
52 // method definitions
53 // ===========================================================================
54 
56 
57 
58 PositionVector::PositionVector(const std::vector<Position>& v) {
59  std::copy(v.begin(), v.end(), std::back_inserter(*this));
60 }
61 
62 
63 PositionVector::PositionVector(const std::vector<Position>::const_iterator beg, const std::vector<Position>::const_iterator end) {
64  std::copy(beg, end, std::back_inserter(*this));
65 }
66 
67 
69  push_back(p1);
70  push_back(p2);
71 }
72 
73 
75 
76 
77 bool
78 PositionVector::around(const Position& p, double offset) const {
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  for (const_iterator i = begin(); i != end() - 1; i++) {
108  if (poly.around(*i, offset)) {
109  return true;
110  }
111  }
112  return false;
113 }
114 
115 
116 double
117 PositionVector::getOverlapWith(const PositionVector& poly, double zThreshold) const {
118  double result = 0;
119  // this points within poly
120  for (const_iterator i = begin(); i != end() - 1; i++) {
121  if (poly.around(*i)) {
122  Position closest = poly.positionAtOffset2D(poly.nearest_offset_to_point2D(*i));
123  if (fabs(closest.z() - (*i).z()) < zThreshold) {
124  result = MAX2(result, poly.distance2D(*i));
125  }
126  }
127  }
128  // polys points within this
129  for (const_iterator i = poly.begin(); i != poly.end() - 1; i++) {
130  if (around(*i)) {
132  if (fabs(closest.z() - (*i).z()) < zThreshold) {
133  result = MAX2(result, distance2D(*i));
134  }
135  }
136  }
137  return result;
138 }
139 
140 
141 bool
142 PositionVector::intersects(const Position& p1, const Position& p2) const {
143  if (size() < 2) {
144  return false;
145  }
146  for (const_iterator i = begin(); i != end() - 1; i++) {
147  if (intersects(*i, *(i + 1), p1, p2)) {
148  return true;
149  }
150  }
151  return false;
152 }
153 
154 
155 bool
157  if (size() < 2) {
158  return false;
159  }
160  for (const_iterator i = begin(); i != end() - 1; i++) {
161  if (v1.intersects(*i, *(i + 1))) {
162  return true;
163  }
164  }
165  return false;
166 }
167 
168 
169 Position
170 PositionVector::intersectionPosition2D(const Position& p1, const Position& p2, const double withinDist) const {
171  for (const_iterator i = begin(); i != end() - 1; i++) {
172  double x, y, m;
173  if (intersects(*i, *(i + 1), p1, p2, withinDist, &x, &y, &m)) {
174  return Position(x, y);
175  }
176  }
177  return Position::INVALID;
178 }
179 
180 
181 Position
183  for (const_iterator i = begin(); i != end() - 1; i++) {
184  if (v1.intersects(*i, *(i + 1))) {
185  return v1.intersectionPosition2D(*i, *(i + 1));
186  }
187  }
188  return Position::INVALID;
189 }
190 
191 
192 const Position&
193 PositionVector::operator[](int index) const {
194  if (index >= 0) {
195  return at(index);
196  } else {
197  return at((int)size() + index);
198  }
199 }
200 
201 
202 Position&
204  if (index >= 0) {
205  return at(index);
206  } else {
207  return at((int)size() + index);
208  }
209 }
210 
211 
212 Position
213 PositionVector::positionAtOffset(double pos, double lateralOffset) const {
214  assert(size() != 0);
215  const_iterator i = begin();
216  double seenLength = 0;
217  do {
218  const double nextLength = (*i).distanceTo(*(i + 1));
219  if (seenLength + nextLength > pos) {
220  return positionAtOffset(*i, *(i + 1), pos - seenLength, lateralOffset);
221  }
222  seenLength += nextLength;
223  } while (++i != end() - 1);
224  if (lateralOffset == 0 || size() < 2) {
225  return back();
226  } else {
227  return positionAtOffset(*(end() - 2), *(end() - 1), (*(end() - 2)).distanceTo(*(end() - 1)), lateralOffset);
228  }
229 }
230 
231 
232 Position
233 PositionVector::positionAtOffset2D(double pos, double lateralOffset) const {
234  const_iterator i = begin();
235  double seenLength = 0;
236  do {
237  const double nextLength = (*i).distanceTo2D(*(i + 1));
238  if (seenLength + nextLength > pos) {
239  return positionAtOffset2D(*i, *(i + 1), pos - seenLength, lateralOffset);
240  }
241  seenLength += nextLength;
242  } while (++i != end() - 1);
243  return back();
244 }
245 
246 
247 double
249  if (pos < 0) {
250  pos += length();
251  }
252  const_iterator i = begin();
253  double seenLength = 0;
254  do {
255  const Position& p1 = *i;
256  const Position& p2 = *(i + 1);
257  const double nextLength = p1.distanceTo(p2);
258  if (seenLength + nextLength > pos) {
259  return p1.angleTo2D(p2);
260  }
261  seenLength += nextLength;
262  } while (++i != end() - 1);
263  const Position& p1 = (*this)[-2];
264  const Position& p2 = back();
265  return p1.angleTo2D(p2);
266 }
267 
268 
269 double
272 }
273 
274 
275 double
277  const_iterator i = begin();
278  double seenLength = 0;
279  do {
280  const Position& p1 = *i;
281  const Position& p2 = *(i + 1);
282  const double nextLength = p1.distanceTo(p2);
283  if (seenLength + nextLength > pos) {
284  return RAD2DEG(atan2(p2.z() - p1.z(), p1.distanceTo2D(p2)));
285  }
286  seenLength += nextLength;
287  } while (++i != end() - 1);
288  const Position& p1 = (*this)[-2];
289  const Position& p2 = back();
290  return RAD2DEG(atan2(p2.z() - p1.z(), p1.distanceTo2D(p2)));
291 }
292 
293 
294 Position
295 PositionVector::positionAtOffset(const Position& p1, const Position& p2, double pos, double lateralOffset) {
296  const double dist = p1.distanceTo(p2);
297  if (pos < 0 || dist < pos) {
298  return Position::INVALID;
299  }
300  if (lateralOffset != 0) {
301  const Position offset = sideOffset(p1, p2, -lateralOffset); // move in the same direction as Position::move2side
302  if (pos == 0.) {
303  return p1 + offset;
304  }
305  return p1 + (p2 - p1) * (pos / dist) + offset;
306  }
307  if (pos == 0.) {
308  return p1;
309  }
310  return p1 + (p2 - p1) * (pos / dist);
311 }
312 
313 
314 Position
315 PositionVector::positionAtOffset2D(const Position& p1, const Position& p2, double pos, double lateralOffset) {
316  const double dist = p1.distanceTo2D(p2);
317  if (pos < 0 || dist < pos) {
318  return Position::INVALID;
319  }
320  if (lateralOffset != 0) {
321  const Position offset = sideOffset(p1, p2, -lateralOffset); // move in the same direction as Position::move2side
322  if (pos == 0.) {
323  return p1 + offset;
324  }
325  return p1 + (p2 - p1) * (pos / dist) + offset;
326  }
327  if (pos == 0.) {
328  return p1;
329  }
330  return p1 + (p2 - p1) * (pos / dist);
331 }
332 
333 
334 Boundary
336  Boundary ret;
337  for (const_iterator i = begin(); i != end(); i++) {
338  ret.add(*i);
339  }
340  return ret;
341 }
342 
343 
344 Position
346  double x = 0;
347  double y = 0;
348  double z = 0;
349  for (const_iterator i = begin(); i != end(); i++) {
350  x += (*i).x();
351  y += (*i).y();
352  z += (*i).z();
353  }
354  return Position(x / (double) size(), y / (double) size(), z / (double)size());
355 }
356 
357 
358 Position
360  PositionVector tmp = *this;
361  if (!isClosed()) { // make sure its closed
362  tmp.push_back(tmp[0]);
363  }
364  const int endIndex = (int)tmp.size() - 1;
365  double div = 0; // 6 * area including sign
366  double x = 0;
367  double y = 0;
368  if (tmp.area() != 0) { // numerical instability ?
369  // http://en.wikipedia.org/wiki/Polygon
370  for (int i = 0; i < endIndex; i++) {
371  const double z = tmp[i].x() * tmp[i + 1].y() - tmp[i + 1].x() * tmp[i].y();
372  div += z; // area formula
373  x += (tmp[i].x() + tmp[i + 1].x()) * z;
374  y += (tmp[i].y() + tmp[i + 1].y()) * z;
375  }
376  div *= 3; // 6 / 2, the 2 comes from the area formula
377  return Position(x / div, y / div);
378  } else {
379  // compute by decomposing into line segments
380  // http://en.wikipedia.org/wiki/Centroid#By_geometric_decomposition
381  double lengthSum = 0;
382  for (int i = 0; i < endIndex; i++) {
383  double length = tmp[i].distanceTo(tmp[i + 1]);
384  x += (tmp[i].x() + tmp[i + 1].x()) * length / 2;
385  y += (tmp[i].y() + tmp[i + 1].y()) * length / 2;
386  lengthSum += length;
387  }
388  if (lengthSum == 0) {
389  // it is probably only one point
390  return tmp[0];
391  }
392  return Position(x / lengthSum, y / lengthSum);
393  }
394 }
395 
396 
397 void
399  Position centroid = getCentroid();
400  for (int i = 0; i < static_cast<int>(size()); i++) {
401  (*this)[i] = centroid + (((*this)[i] - centroid) * factor);
402  }
403 }
404 
405 
406 void
408  Position centroid = getCentroid();
409  for (int i = 0; i < static_cast<int>(size()); i++) {
410  (*this)[i] = centroid + (((*this)[i] - centroid) + offset);
411  }
412 }
413 
414 
415 Position
417  if (size() == 1) {
418  return (*this)[0];
419  }
420  return positionAtOffset(double((length() / 2.)));
421 }
422 
423 
424 double
426  double len = 0;
427  for (const_iterator i = begin(); i != end() - 1; i++) {
428  len += (*i).distanceTo(*(i + 1));
429  }
430  return len;
431 }
432 
433 
434 double
436  double len = 0;
437  for (const_iterator i = begin(); i != end() - 1; i++) {
438  len += (*i).distanceTo2D(*(i + 1));
439  }
440  return len;
441 }
442 
443 
444 double
446  if (size() < 3) {
447  return 0;
448  }
449  double area = 0;
450  PositionVector tmp = *this;
451  if (!isClosed()) { // make sure its closed
452  tmp.push_back(tmp[0]);
453  }
454  const int endIndex = (int)tmp.size() - 1;
455  // http://en.wikipedia.org/wiki/Polygon
456  for (int i = 0; i < endIndex; i++) {
457  area += tmp[i].x() * tmp[i + 1].y() - tmp[i + 1].x() * tmp[i].y();
458  }
459  if (area < 0) { // we whether we had cw or ccw order
460  area *= -1;
461  }
462  return area / 2;
463 }
464 
465 
466 bool
467 PositionVector::partialWithin(const AbstractPoly& poly, double offset) const {
468  for (const_iterator i = begin(); i != end() - 1; i++) {
469  if (poly.around(*i, offset)) {
470  return true;
471  }
472  }
473  return false;
474 }
475 
476 
477 bool
478 PositionVector::crosses(const Position& p1, const Position& p2) const {
479  return intersects(p1, p2);
480 }
481 
482 
483 std::pair<PositionVector, PositionVector>
484 PositionVector::splitAt(double where) const {
485  if (size() < 2) {
486  throw InvalidArgument("Vector to short for splitting");
487  }
488  if (where <= POSITION_EPS || where >= length() - POSITION_EPS) {
489  WRITE_WARNING("Splitting vector close to end (pos: " + toString(where) + ", length: " + toString(length()) + ")");
490  }
491  PositionVector first, second;
492  first.push_back((*this)[0]);
493  double seen = 0;
494  const_iterator it = begin() + 1;
495  double next = first.back().distanceTo(*it);
496  // see how many points we can add to first
497  while (where >= seen + next + POSITION_EPS) {
498  seen += next;
499  first.push_back(*it);
500  it++;
501  next = first.back().distanceTo(*it);
502  }
503  if (fabs(where - (seen + next)) > POSITION_EPS || it == end() - 1) {
504  // we need to insert a new point because 'where' is not close to an
505  // existing point or it is to close to the endpoint
506  const Position p = positionAtOffset(first.back(), *it, where - seen);
507  first.push_back(p);
508  second.push_back(p);
509  } else {
510  first.push_back(*it);
511  }
512  // add the remaining points to second
513  for (; it != end(); it++) {
514  second.push_back(*it);
515  }
516  assert(first.size() >= 2);
517  assert(second.size() >= 2);
518  assert(first.back() == second.front());
519  assert(fabs(first.length() + second.length() - length()) < 2 * POSITION_EPS);
520  return std::pair<PositionVector, PositionVector>(first, second);
521 }
522 
523 
524 std::ostream&
525 operator<<(std::ostream& os, const PositionVector& geom) {
526  for (PositionVector::const_iterator i = geom.begin(); i != geom.end(); i++) {
527  if (i != geom.begin()) {
528  os << " ";
529  }
530  os << (*i);
531  }
532  return os;
533 }
534 
535 
536 void
538  std::sort(begin(), end(), as_poly_cw_sorter());
539 }
540 
541 
542 void
543 PositionVector::add(double xoff, double yoff, double zoff) {
544  for (int i = 0; i < static_cast<int>(size()); i++) {
545  (*this)[i].add(xoff, yoff, zoff);
546  }
547 }
548 
549 
550 void
552  add(offset.x(), offset.y(), offset.z());
553 }
554 
555 
556 void
558  for (int i = 0; i < static_cast<int>(size()); i++) {
559  (*this)[i].mul(1, -1);
560  }
561 }
562 
563 
565 
566 
567 int
569  return atan2(p1.x(), p1.y()) < atan2(p2.x(), p2.y());
570 }
571 
572 
573 void
575  std::sort(begin(), end(), increasing_x_y_sorter());
576 }
577 
578 
580 
581 
582 int
584  const Position& p2) const {
585  if (p1.x() != p2.x()) {
586  return p1.x() < p2.x();
587  }
588  return p1.y() < p2.y();
589 }
590 
591 
592 double
593 PositionVector::isLeft(const Position& P0, const Position& P1, const Position& P2) const {
594  return (P1.x() - P0.x()) * (P2.y() - P0.y()) - (P2.x() - P0.x()) * (P1.y() - P0.y());
595 }
596 
597 
600  PositionVector ret = *this;
601  ret.sortAsPolyCWByAngle();
602  return simpleHull_2D(ret);
603 }
604 
605 
606 void
607 PositionVector::append(const PositionVector& v, double sameThreshold) {
608  if (size() > 0 && v.size() > 0 && back().distanceTo(v[0]) < sameThreshold) {
609  copy(v.begin() + 1, v.end(), back_inserter(*this));
610  } else {
611  copy(v.begin(), v.end(), back_inserter(*this));
612  }
613 }
614 
615 
617 PositionVector::getSubpart(double beginOffset, double endOffset) const {
618  PositionVector ret;
619  Position begPos = front();
620  if (beginOffset > POSITION_EPS) {
621  begPos = positionAtOffset(beginOffset);
622  }
623  Position endPos = back();
624  if (endOffset < length() - POSITION_EPS) {
625  endPos = positionAtOffset(endOffset);
626  }
627  ret.push_back(begPos);
628 
629  double seen = 0;
630  const_iterator i = begin();
631  // skip previous segments
632  while ((i + 1) != end()
633  &&
634  seen + (*i).distanceTo(*(i + 1)) < beginOffset) {
635  seen += (*i).distanceTo(*(i + 1));
636  i++;
637  }
638  // append segments in between
639  while ((i + 1) != end()
640  &&
641  seen + (*i).distanceTo(*(i + 1)) < endOffset) {
642 
643  ret.push_back_noDoublePos(*(i + 1));
644  seen += (*i).distanceTo(*(i + 1));
645  i++;
646  }
647  // append end
648  ret.push_back_noDoublePos(endPos);
649  if (ret.size() == 1) {
650  ret.push_back(endPos);
651  }
652  return ret;
653 }
654 
655 
657 PositionVector::getSubpart2D(double beginOffset, double endOffset) const {
658  PositionVector ret;
659  Position begPos = front();
660  if (beginOffset > POSITION_EPS) {
661  begPos = positionAtOffset2D(beginOffset);
662  }
663  Position endPos = back();
664  if (endOffset < length2D() - POSITION_EPS) {
665  endPos = positionAtOffset2D(endOffset);
666  }
667  ret.push_back(begPos);
668 
669  double seen = 0;
670  const_iterator i = begin();
671  // skip previous segments
672  while ((i + 1) != end()
673  &&
674  seen + (*i).distanceTo2D(*(i + 1)) < beginOffset) {
675  seen += (*i).distanceTo2D(*(i + 1));
676  i++;
677  }
678  // append segments in between
679  while ((i + 1) != end()
680  &&
681  seen + (*i).distanceTo2D(*(i + 1)) < endOffset) {
682 
683  ret.push_back_noDoublePos(*(i + 1));
684  seen += (*i).distanceTo2D(*(i + 1));
685  i++;
686  }
687  // append end
688  ret.push_back_noDoublePos(endPos);
689  if (ret.size() == 1) {
690  ret.push_back(endPos);
691  }
692  return ret;
693 }
694 
695 
697 PositionVector::getSubpartByIndex(int beginIndex, int count) const {
698  if (beginIndex < 0) {
699  beginIndex += (int)size();
700  }
701  assert(count >= 0);
702  assert(beginIndex < (int)size());
703  assert(beginIndex + count <= (int)size());
704  PositionVector result;
705  for (int i = beginIndex; i < beginIndex + count; ++i) {
706  result.push_back((*this)[i]);
707  }
708  return result;
709 }
710 
711 
712 double
714  return front().angleTo2D(back());
715 }
716 
717 
718 double
719 PositionVector::nearest_offset_to_point2D(const Position& p, bool perpendicular) const {
720  double minDist = std::numeric_limits<double>::max();
721  double nearestPos = GeomHelper::INVALID_OFFSET;
722  double seen = 0;
723  for (const_iterator i = begin(); i != end() - 1; i++) {
724  const double pos =
725  GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, perpendicular);
726  const double dist = pos == GeomHelper::INVALID_OFFSET ? minDist : p.distanceTo2D(positionAtOffset2D(*i, *(i + 1), pos));
727  if (dist < minDist) {
728  nearestPos = pos + seen;
729  minDist = dist;
730  }
731  if (perpendicular && i != begin() && pos == GeomHelper::INVALID_OFFSET) {
732  // even if perpendicular is set we still need to check the distance to the inner points
733  const double cornerDist = p.distanceTo2D(*i);
734  if (cornerDist < minDist) {
735  const double pos1 =
736  GeomHelper::nearest_offset_on_line_to_point2D(*(i - 1), *i, p, false);
737  const double pos2 =
738  GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, false);
739  if (pos1 == (*(i - 1)).distanceTo2D(*i) && pos2 == 0.) {
740  nearestPos = seen;
741  minDist = cornerDist;
742  }
743  }
744  }
745  seen += (*i).distanceTo2D(*(i + 1));
746  }
747  return nearestPos;
748 }
749 
750 
751 Position
753  // @toDo this duplicates most of the code in nearest_offset_to_point2D. It should be refactored
754  if (extend) {
755  PositionVector extended = *this;
756  const double dist = 2 * distance2D(p);
757  extended.extrapolate(dist);
758  return extended.transformToVectorCoordinates(p) - Position(dist, 0);
759  }
760  double minDist = std::numeric_limits<double>::max();
761  double nearestPos = -1;
762  double seen = 0;
763  int sign = 1;
764  for (const_iterator i = begin(); i != end() - 1; i++) {
765  const double pos =
766  GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, true);
767  const double dist = pos < 0 ? minDist : p.distanceTo2D(positionAtOffset(*i, *(i + 1), pos));
768  if (dist < minDist) {
769  nearestPos = pos + seen;
770  minDist = dist;
771  sign = isLeft(*i, *(i + 1), p) >= 0 ? -1 : 1;
772  }
773  if (i != begin() && pos == GeomHelper::INVALID_OFFSET) {
774  // even if perpendicular is set we still need to check the distance to the inner points
775  const double cornerDist = p.distanceTo2D(*i);
776  if (cornerDist < minDist) {
777  const double pos1 =
778  GeomHelper::nearest_offset_on_line_to_point2D(*(i - 1), *i, p, false);
779  const double pos2 =
780  GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, false);
781  if (pos1 == (*(i - 1)).distanceTo2D(*i) && pos2 == 0.) {
782  nearestPos = seen;
783  minDist = cornerDist;
784  sign = isLeft(*(i - 1), *i, p) >= 0 ? -1 : 1;
785  }
786  }
787  }
788  seen += (*i).distanceTo2D(*(i + 1));
789  }
790  if (nearestPos != -1) {
791  return Position(nearestPos, sign * minDist);
792  } else {
793  return Position::INVALID;
794  }
795 }
796 
797 
798 int
800  assert(size() > 0);
801  double minDist = std::numeric_limits<double>::max();
802  double dist;
803  int closest = 0;
804  for (int i = 0; i < (int)size(); i++) {
805  dist = p.distanceTo((*this)[i]);
806  if (dist < minDist) {
807  closest = i;
808  minDist = dist;
809  }
810  }
811  return closest;
812 }
813 
814 
815 int
817  double minDist = std::numeric_limits<double>::max();
818  int insertionIndex = 1;
819  for (int i = 0; i < (int)size() - 1; i++) {
820  const double length = GeomHelper::nearest_offset_on_line_to_point2D((*this)[i], (*this)[i + 1], p, false);
821  const Position& outIntersection = PositionVector::positionAtOffset2D((*this)[i], (*this)[i + 1], length);
822  const double dist = p.distanceTo2D(outIntersection);
823  if (dist < minDist) {
824  insertionIndex = i + 1;
825  minDist = dist;
826  }
827  }
828  insert(begin() + insertionIndex, p);
829  return insertionIndex;
830 }
831 
832 
833 int
835  if (size() == 0) {
836  return -1;
837  }
838  double minDist = std::numeric_limits<double>::max();
839  int removalIndex = 0;
840  for (int i = 0; i < (int)size(); i++) {
841  const double dist = p.distanceTo2D((*this)[i]);
842  if (dist < minDist) {
843  removalIndex = i;
844  minDist = dist;
845  }
846  }
847  erase(begin() + removalIndex);
848  return removalIndex;
849 }
850 
851 
852 std::vector<double>
854  std::vector<double> ret;
855  for (const_iterator i = other.begin(); i != other.end() - 1; i++) {
856  std::vector<double> atSegment = intersectsAtLengths2D(*i, *(i + 1));
857  copy(atSegment.begin(), atSegment.end(), back_inserter(ret));
858  }
859  return ret;
860 }
861 
862 
863 std::vector<double>
865  std::vector<double> ret;
866  double pos = 0;
867  for (const_iterator i = begin(); i != end() - 1; i++) {
868  const Position& p1 = *i;
869  const Position& p2 = *(i + 1);
870  double x, y, m;
871  if (intersects(p1, p2, lp1, lp2, 0., &x, &y, &m)) {
872  ret.push_back(Position(x, y).distanceTo2D(p1) + pos);
873  }
874  pos += p1.distanceTo2D(p2);
875  }
876  return ret;
877 }
878 
879 
880 void
881 PositionVector::extrapolate(const double val, const bool onlyFirst, const bool onlyLast) {
882  assert(size() > 1);
883  Position& p1 = (*this)[0];
884  Position& p2 = (*this)[1];
885  const Position offset = (p2 - p1) * (val / p1.distanceTo(p2));
886  if (!onlyLast) {
887  p1.sub(offset);
888  }
889  if (!onlyFirst) {
890  if (size() == 2) {
891  p2.add(offset);
892  } else {
893  const Position& e1 = (*this)[-2];
894  Position& e2 = (*this)[-1];
895  e2.sub((e1 - e2) * (val / e1.distanceTo(e2)));
896  }
897  }
898 }
899 
900 
901 void
902 PositionVector::extrapolate2D(const double val, const bool onlyFirst) {
903  assert(size() > 1);
904  Position& p1 = (*this)[0];
905  Position& p2 = (*this)[1];
906  const Position offset = (p2 - p1) * (val / p1.distanceTo2D(p2));
907  p1.sub(offset);
908  if (!onlyFirst) {
909  if (size() == 2) {
910  p2.add(offset);
911  } else {
912  const Position& e1 = (*this)[-2];
913  Position& e2 = (*this)[-1];
914  e2.sub((e1 - e2) * (val / e1.distanceTo2D(e2)));
915  }
916  }
917 }
918 
919 
922  PositionVector ret;
923  for (const_reverse_iterator i = rbegin(); i != rend(); i++) {
924  ret.push_back(*i);
925  }
926  return ret;
927 }
928 
929 
930 Position
931 PositionVector::sideOffset(const Position& beg, const Position& end, const double amount) {
932  const double scale = amount / beg.distanceTo2D(end);
933  return Position((beg.y() - end.y()) * scale, (end.x() - beg.x()) * scale);
934 }
935 
936 
937 void
939  if (size() < 2) {
940  return;
941  }
942  if (length2D() == 0) {
943  return;
944  }
945  PositionVector shape;
946  for (int i = 0; i < static_cast<int>(size()); i++) {
947  if (i == 0) {
948  const Position& from = (*this)[i];
949  const Position& to = (*this)[i + 1];
950  shape.push_back(from - sideOffset(from, to, amount));
951  } else if (i == static_cast<int>(size()) - 1) {
952  const Position& from = (*this)[i - 1];
953  const Position& to = (*this)[i];
954  shape.push_back(to - sideOffset(from, to, amount));
955  } else {
956  const Position& from = (*this)[i - 1];
957  const Position& me = (*this)[i];
958  const Position& to = (*this)[i + 1];
959  PositionVector fromMe(from, me);
960  fromMe.extrapolate2D(me.distanceTo2D(to));
961  const double extrapolateDev = fromMe[1].distanceTo2D(to);
962  if (fabs(extrapolateDev) < POSITION_EPS) {
963  // parallel case, just shift the middle point
964  shape.push_back(me - sideOffset(from, to, amount));
965  } else if (fabs(extrapolateDev - 2 * me.distanceTo2D(to)) < POSITION_EPS) {
966  // counterparallel case, just shift the middle point
967  PositionVector fromMe(from, me);
968  fromMe.extrapolate2D(amount);
969  shape.push_back(fromMe[1]);
970  } else {
971  Position offsets = sideOffset(from, me, amount);
972  Position offsets2 = sideOffset(me, to, amount);
973  PositionVector l1(from - offsets, me - offsets);
974  PositionVector l2(me - offsets2, to - offsets2);
975  Position meNew = l1.intersectionPosition2D(l2[0], l2[1], 100);
976  if (meNew == Position::INVALID) {
977  throw InvalidArgument("no line intersection");
978  }
979  meNew = meNew + Position(0, 0, me.z());
980  shape.push_back(meNew);
981  }
982  // copy original z value
983  shape.back().set(shape.back().x(), shape.back().y(), me.z());
984  }
985  }
986  *this = shape;
987 }
988 
989 
990 double
992  assert((int)size() > pos + 1);
993  return (*this)[pos].angleTo2D((*this)[pos + 1]);
994 }
995 
996 
997 void
999  if (size() == 0 || (*this)[0] == back()) {
1000  return;
1001  }
1002  push_back((*this)[0]);
1003 }
1004 
1005 
1006 std::vector<double>
1007 PositionVector::distances(const PositionVector& s, bool perpendicular) const {
1008  std::vector<double> ret;
1009  const_iterator i;
1010  for (i = begin(); i != end(); i++) {
1011  const double dist = s.distance2D(*i, perpendicular);
1012  if (dist != GeomHelper::INVALID_OFFSET) {
1013  ret.push_back(dist);
1014  }
1015  }
1016  for (i = s.begin(); i != s.end(); i++) {
1017  const double dist = distance2D(*i, perpendicular);
1018  if (dist != GeomHelper::INVALID_OFFSET) {
1019  ret.push_back(dist);
1020  }
1021  }
1022  return ret;
1023 }
1024 
1025 
1026 double
1027 PositionVector::distance2D(const Position& p, bool perpendicular) const {
1028  if (size() == 0) {
1030  } else if (size() == 1) {
1031  return front().distanceTo(p);
1032  }
1033  const double nearestOffset = nearest_offset_to_point2D(p, perpendicular);
1034  if (nearestOffset == GeomHelper::INVALID_OFFSET) {
1036  } else {
1037  return p.distanceTo2D(positionAtOffset2D(nearestOffset));
1038  }
1039 }
1040 
1041 
1042 void
1044  if (size() == 0 || !p.almostSame(back())) {
1045  push_back(p);
1046  }
1047 }
1048 
1049 
1050 void
1052  if (size() == 0 || !p.almostSame(front())) {
1053  insert(begin(), p);
1054  }
1055 }
1056 
1057 
1058 bool
1060  return size() >= 2 && (*this)[0] == back();
1061 }
1062 
1063 
1064 void
1065 PositionVector::removeDoublePoints(double minDist, bool assertLength) {
1066  if (size() > 1) {
1067  iterator last = begin();
1068  for (iterator i = begin() + 1; i != end() && (!assertLength || size() > 2);) {
1069  if (last->almostSame(*i, minDist)) {
1070  i = erase(i);
1071  } else {
1072  last = i;
1073  ++i;
1074  }
1075  }
1076  }
1077 }
1078 
1079 
1080 bool
1082  if (size() == v2.size()) {
1083  for (int i = 0; i < (int)size(); i++) {
1084  if ((*this)[i] != v2[i]) {
1085  return false;
1086  }
1087  }
1088  return true;
1089  } else {
1090  return false;
1091  }
1092 }
1093 
1094 
1095 bool
1097  if (size() > 2) {
1098  return false;
1099  }
1100  for (const_iterator i = begin(); i != end() - 1; i++) {
1101  if ((*i).z() != (*(i + 1)).z()) {
1102  return true;
1103  }
1104  }
1105  return false;
1106 }
1107 
1108 
1109 bool
1110 PositionVector::intersects(const Position& p11, const Position& p12, const Position& p21, const Position& p22, const double withinDist, double* x, double* y, double* mu) {
1111  const double eps = std::numeric_limits<double>::epsilon();
1112  const double denominator = (p22.y() - p21.y()) * (p12.x() - p11.x()) - (p22.x() - p21.x()) * (p12.y() - p11.y());
1113  const double numera = (p22.x() - p21.x()) * (p11.y() - p21.y()) - (p22.y() - p21.y()) * (p11.x() - p21.x());
1114  const double numerb = (p12.x() - p11.x()) * (p11.y() - p21.y()) - (p12.y() - p11.y()) * (p11.x() - p21.x());
1115  /* Are the lines coincident? */
1116  if (fabs(numera) < eps && fabs(numerb) < eps && fabs(denominator) < eps) {
1117  double a1;
1118  double a2;
1119  double a3;
1120  double a4;
1121  double a = -1e12;
1122  if (p11.x() != p12.x()) {
1123  a1 = p11.x() < p12.x() ? p11.x() : p12.x();
1124  a2 = p11.x() < p12.x() ? p12.x() : p11.x();
1125  a3 = p21.x() < p22.x() ? p21.x() : p22.x();
1126  a4 = p21.x() < p22.x() ? p22.x() : p21.x();
1127  } else {
1128  a1 = p11.y() < p12.y() ? p11.y() : p12.y();
1129  a2 = p11.y() < p12.y() ? p12.y() : p11.y();
1130  a3 = p21.y() < p22.y() ? p21.y() : p22.y();
1131  a4 = p21.y() < p22.y() ? p22.y() : p21.y();
1132  }
1133  if (a1 <= a3 && a3 <= a2) {
1134  if (a4 < a2) {
1135  a = (a3 + a4) / 2;
1136  } else {
1137  a = (a2 + a3) / 2;
1138  }
1139  }
1140  if (a3 <= a1 && a1 <= a4) {
1141  if (a2 < a4) {
1142  a = (a1 + a2) / 2;
1143  } else {
1144  a = (a1 + a4) / 2;
1145  }
1146  }
1147  if (a != -1e12) {
1148  if (x != 0) {
1149  if (p11.x() != p12.x()) {
1150  *mu = (a - p11.x()) / (p12.x() - p11.x());
1151  *x = a;
1152  *y = p11.y() + (*mu) * (p12.y() - p11.y());
1153  } else {
1154  *x = p11.x();
1155  *y = a;
1156  if (p12.y() == p11.y()) {
1157  *mu = 0;
1158  } else {
1159  *mu = (a - p11.y()) / (p12.y() - p11.y());
1160  }
1161  }
1162  }
1163  return true;
1164  }
1165  return false;
1166  }
1167  /* Are the lines parallel */
1168  if (fabs(denominator) < eps) {
1169  return false;
1170  }
1171  /* Is the intersection along the segments */
1172  double mua = numera / denominator;
1173  /* reduce rounding errors for lines ending in the same point */
1174  if (fabs(p12.x() - p22.x()) < eps && fabs(p12.y() - p22.y()) < eps) {
1175  mua = 1.;
1176  } else {
1177  const double offseta = withinDist / p11.distanceTo2D(p12);
1178  const double offsetb = withinDist / p21.distanceTo2D(p22);
1179  const double mub = numerb / denominator;
1180  if (mua < -offseta || mua > 1 + offseta || mub < -offsetb || mub > 1 + offsetb) {
1181  return false;
1182  }
1183  }
1184  if (x != 0) {
1185  *x = p11.x() + mua * (p12.x() - p11.x());
1186  *y = p11.y() + mua * (p12.y() - p11.y());
1187  *mu = mua;
1188  }
1189  return true;
1190 }
1191 
1192 
1193 void
1195  const double s = sin(angle);
1196  const double c = cos(angle);
1197  for (int i = 0; i < (int)size(); i++) {
1198  const double x = (*this)[i].x();
1199  const double y = (*this)[i].y();
1200  const double z = (*this)[i].z();
1201  const double xnew = x * c - y * s;
1202  const double ynew = x * s + y * c;
1203  (*this)[i].set(xnew, ynew, z);
1204  }
1205 }
1206 
1207 
1210  PositionVector result = *this;
1211  bool changed = true;
1212  while (changed && result.size() > 3) {
1213  changed = false;
1214  for (int i = 0; i < (int)result.size(); i++) {
1215  const Position& p1 = result[i];
1216  const Position& p2 = result[(i + 2) % result.size()];
1217  const int middleIndex = (i + 1) % result.size();
1218  const Position& p0 = result[middleIndex];
1219  // https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line#Line_defined_by_two_points
1220  const double triangleArea2 = fabs((p2.y() - p1.y()) * p0.x() - (p2.x() - p1.x()) * p0.y() + p2.x() * p1.y() - p2.y() * p1.x());
1221  const double distIK = p1.distanceTo2D(p2);
1222  if (distIK > NUMERICAL_EPS && triangleArea2 / distIK < NUMERICAL_EPS) {
1223  changed = true;
1224  result.erase(result.begin() + middleIndex);
1225  break;
1226  }
1227  }
1228  }
1229  return result;
1230 }
1231 
1232 
1234 PositionVector::getOrthogonal(const Position& p, double extend, bool before, double length) const {
1235  PositionVector result;
1236  PositionVector tmp = *this;
1237  tmp.extrapolate2D(extend);
1238  const double baseOffset = tmp.nearest_offset_to_point2D(p);
1239  if (baseOffset == GeomHelper::INVALID_OFFSET || size() < 2) {
1240  // fail
1241  return result;
1242  }
1243  Position base = tmp.positionAtOffset2D(baseOffset);
1244  const int closestIndex = tmp.indexOfClosest(base);
1245  result.push_back(base);
1246  if (fabs(baseOffset - tmp.offsetAtIndex2D(closestIndex)) > NUMERICAL_EPS) {
1247  result.push_back(tmp[closestIndex]);
1248  } else if (before) {
1249  // take the segment before closestIndex if possible
1250  if (closestIndex > 0) {
1251  result.push_back(tmp[closestIndex - 1]);
1252  } else {
1253  result.push_back(tmp[1]);
1254  }
1255  } else {
1256  // take the segment after closestIndex if possible
1257  if (closestIndex < (int)size() - 1) {
1258  result.push_back(tmp[closestIndex + 1]);
1259  } else {
1260  result.push_back(tmp[-1]);
1261  }
1262  }
1263  result = result.getSubpart2D(0, length);
1264  // rotate around base
1265  result.add(base * -1);
1266  result.rotate2D(DEG2RAD(90));
1267  result.add(base);
1268  return result;
1269 }
1270 
1271 
1274  PositionVector result = *this;
1275  const double z0 = (*this)[0].z();
1276  // the z-delta of the first segment
1277  const double dz = (*this)[1].z() - z0;
1278  // if the shape only has 2 points it is as smooth as possible already
1279  if (size() > 2 && dz != 0) {
1280  dist = MIN2(dist, length());
1281  // check wether we need to insert a new point at dist
1282  Position pDist = positionAtOffset(dist);
1283  int iLast = indexOfClosest(pDist);
1284  // prevent close spacing to reduce impact of rounding errors in z-axis
1285  if (pDist.distanceTo2D((*this)[iLast]) > POSITION_EPS * 20) {
1286  iLast = result.insertAtClosest(pDist);
1287  }
1288  double dist2 = result.offsetAtIndex2D(iLast);
1289  const double dz2 = result[iLast].z() - z0;
1290  double seen = 0;
1291  for (int i = 1; i < iLast; ++i) {
1292  seen += result[i].distanceTo2D(result[i - 1]);
1293  result[i].set(result[i].x(), result[i].y(), z0 + dz2 * seen / dist2);
1294  }
1295  }
1296  return result;
1297 
1298 }
1299 
1300 
1301 double
1303  if (index < 0 || index >= (int)size()) {
1305  }
1306  double seen = 0;
1307  for (int i = 1; i <= index; ++i) {
1308  seen += (*this)[i].distanceTo2D((*this)[i - 1]);
1309  }
1310  return seen;
1311 }
1312 
1313 
1314 double
1316  double result = 0;
1317  for (int i = 1; i < (int)size(); ++i) {
1318  const Position& p1 = (*this)[i - 1];
1319  const Position& p2 = (*this)[i];
1320  result = MAX2(result, (double)fabs((p1.z() - p2.z()) / p1.distanceTo2D(p2)));
1321  }
1322  return result;
1323 }
1324 
1325 /****************************************************************************/
1326 
bool around(const Position &p, double offset=0) const
Returns the information whether the position vector describes a polygon lying around the given point...
clase for increasing Sorter
double rotationDegreeAtOffset(double pos) const
Returns the rotation at the given length.
int operator()(const Position &p1, const Position &p2) const
comparing operation
bool almostSame(const Position &p2, double maxDiv=POSITION_EPS) const
checki if two position is almost the sme as other
Definition: Position.h:235
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:73
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:133
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:250
#define M_PI
Definition: angles.h:37
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.
Position intersectionPosition2D(const Position &p1, const Position &p2, const double withinDist=0.) const
Returns the position of the intersection.
double y() const
Returns the y-position.
Definition: Position.h:68
double x() const
Returns the x-position.
Definition: Position.h:63
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:260
Position getCentroid() const
Returns the centroid (closes the polygon if unclosed)
T MAX2(T a, T b)
Definition: StdDefs.h:70
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.
#define RAD2DEG(x)
Definition: GeomHelper.h:46
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
std::pair< PositionVector, PositionVector > splitAt(double where) const
Returns the two lists made when this list vector is splitted at the given point.
A class that stores a 2D geometrical boundary.
Definition: Boundary.h:48
#define WRITE_WARNING(msg)
Definition: MsgHandler.h:200
Position getLineCenter() const
get line center
static double legacyDegree(const double angle, const bool positive=false)
Definition: GeomHelper.cpp:200
double area() const
Returns the area (0 for non-closed)
~PositionVector()
Destructor.
void extrapolate2D(const double val, const bool onlyFirst=false)
extrapolate position vector in two dimensions (Z is ignored)
void push_front_noDoublePos(const Position &p)
insert in front a non double position
void removeDoublePoints(double minDist=POSITION_EPS, bool assertLength=false)
Removes positions if too near.
#define max(a, b)
Definition: polyfonts.c:65
static double nearest_offset_on_line_to_point2D(const Position &lineStart, const Position &lineEnd, const Position &p, bool perpendicular=true)
Definition: GeomHelper.cpp:96
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
Definition: ToString.h:56
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:46
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
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:90
T MIN2(T a, T b)
Definition: StdDefs.h:64
#define POSITION_EPS
Definition: config.h:175
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:45
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:59
PositionVector getSubpartByIndex(int beginIndex, int count) const
get subpart of a position vector using index and a cout
void sortByIncreasingXY()
shory by increasing X-Y Psitions
void move2side(double amount)
move position vector to side using certain ammount
PositionVector 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.
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
PositionVector simpleHull_2D(const PositionVector &V)
#define sign(a)
Definition: polyfonts.c:68
double length() const
Returns the length.
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
double angleAt2D(int pos) const
get angle in certain position of position vector
PositionVector convexHull() const
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:240
Boundary getBoxBoundary() const
Returns a boundary enclosing this list of lines.
#define NUMERICAL_EPS
Definition: config.h:151
void push_back_noDoublePos(const Position &p)
insert in back a non double position
double getMaxGrade() const
return the maximum grade of all segments as a fraction of zRange/length2D
bool crosses(const Position &p1, const Position &p2) const
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:86
void add(double xoff, double yoff, double zoff)
double nearest_offset_to_point2D(const Position &p, bool perpendicular=true) const
return the nearest offest to point 2D
void closePolygon()
ensures that the last position equals the first
Position positionAtOffset(double pos, double lateralOffset=0) const
Returns the position at the given length.
int removeClosest(const Position &p)
removes the point closest to p and return the removal index
Position transformToVectorCoordinates(const Position &p, bool extend=false) const
return position p within the length-wise coordinate system defined by this position vector...
bool intersects(const Position &p1, const Position &p2) const
Returns the information whether this list of points interesects the given line.
static const Position INVALID
used to indicate that a position is valid
Definition: Position.h:278
void sub(double dx, double dy)
Substracts the given position from this one.
Definition: Position.h:153