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-2016 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 #ifdef CHECK_MEMORY_LEAKS
52 #include <foreign/nvwa/debug_new.h>
53 #endif // CHECK_MEMORY_LEAKS
54 
55 // ===========================================================================
56 // method definitions
57 // ===========================================================================
58 
60 
61 
62 PositionVector::PositionVector(const std::vector<Position>& v) {
63  std::copy(v.begin(), v.end(), std::back_inserter(*this));
64 }
65 
66 
67 PositionVector::PositionVector(const std::vector<Position>::const_iterator beg, const std::vector<Position>::const_iterator end) {
68  std::copy(beg, end, std::back_inserter(*this));
69 }
70 
71 
73  push_back(p1);
74  push_back(p2);
75 }
76 
77 
79 
80 
81 bool
82 PositionVector::around(const Position& p, SUMOReal offset) const {
83  if (offset != 0) {
84  PositionVector tmp(*this);
85  tmp.scaleAbsolute(offset);
86  return tmp.around(p);
87  }
88  SUMOReal angle = 0;
89  for (const_iterator i = begin(); i != end() - 1; i++) {
90  Position p1(
91  (*i).x() - p.x(),
92  (*i).y() - p.y());
93  Position p2(
94  (*(i + 1)).x() - p.x(),
95  (*(i + 1)).y() - p.y());
96  angle += GeomHelper::angle2D(p1, p2);
97  }
98  Position p1(
99  (*(end() - 1)).x() - p.x(),
100  (*(end() - 1)).y() - p.y());
101  Position p2(
102  (*(begin())).x() - p.x(),
103  (*(begin())).y() - p.y());
104  angle += GeomHelper::angle2D(p1, p2);
105  return (!(fabs(angle) < M_PI));
106 }
107 
108 
109 bool
111  for (const_iterator i = begin(); i != end() - 1; i++) {
112  if (poly.around(*i, offset)) {
113  return true;
114  }
115  }
116  return false;
117 }
118 
119 
120 bool
121 PositionVector::intersects(const Position& p1, const Position& p2) const {
122  if (size() < 2) {
123  return false;
124  }
125  for (const_iterator i = begin(); i != end() - 1; i++) {
126  if (intersects(*i, *(i + 1), p1, p2)) {
127  return true;
128  }
129  }
130  return false;
131 }
132 
133 
134 bool
136  if (size() < 2) {
137  return false;
138  }
139  for (const_iterator i = begin(); i != end() - 1; i++) {
140  if (v1.intersects(*i, *(i + 1))) {
141  return true;
142  }
143  }
144  return false;
145 }
146 
147 
148 Position
149 PositionVector::intersectionPosition2D(const Position& p1, const Position& p2, const SUMOReal withinDist) const {
150  for (const_iterator i = begin(); i != end() - 1; i++) {
151  SUMOReal x, y, m;
152  if (intersects(*i, *(i + 1), p1, p2, withinDist, &x, &y, &m)) {
153  return Position(x, y);
154  }
155  }
156  return Position::INVALID;
157 }
158 
159 
160 Position
162  for (const_iterator i = begin(); i != end() - 1; i++) {
163  if (v1.intersects(*i, *(i + 1))) {
164  return v1.intersectionPosition2D(*i, *(i + 1));
165  }
166  }
167  return Position::INVALID;
168 }
169 
170 
171 const Position&
172 PositionVector::operator[](int index) const {
173  if (index >= 0) {
174  return at(index);
175  } else {
176  return at((int)size() + index);
177  }
178 }
179 
180 
181 Position&
183  if (index >= 0) {
184  return at(index);
185  } else {
186  return at((int)size() + index);
187  }
188 }
189 
190 
191 Position
193  const_iterator i = begin();
194  SUMOReal seenLength = 0;
195  do {
196  const SUMOReal nextLength = (*i).distanceTo(*(i + 1));
197  if (seenLength + nextLength > pos) {
198  return positionAtOffset(*i, *(i + 1), pos - seenLength, lateralOffset);
199  }
200  seenLength += nextLength;
201  } while (++i != end() - 1);
202  return back();
203 }
204 
205 
206 Position
208  const_iterator i = begin();
209  SUMOReal seenLength = 0;
210  do {
211  const SUMOReal nextLength = (*i).distanceTo2D(*(i + 1));
212  if (seenLength + nextLength > pos) {
213  return positionAtOffset2D(*i, *(i + 1), pos - seenLength, lateralOffset);
214  }
215  seenLength += nextLength;
216  } while (++i != end() - 1);
217  return back();
218 }
219 
220 
221 SUMOReal
223  if (pos < 0) {
224  pos += length();
225  }
226  const_iterator i = begin();
227  SUMOReal seenLength = 0;
228  do {
229  const Position& p1 = *i;
230  const Position& p2 = *(i + 1);
231  const SUMOReal nextLength = p1.distanceTo(p2);
232  if (seenLength + nextLength > pos) {
233  return p1.angleTo2D(p2);
234  }
235  seenLength += nextLength;
236  } while (++i != end() - 1);
237  const Position& p1 = (*this)[-2];
238  const Position& p2 = back();
239  return p1.angleTo2D(p2);
240 }
241 
242 
243 SUMOReal
246 }
247 
248 
249 SUMOReal
251  const_iterator i = begin();
252  SUMOReal seenLength = 0;
253  do {
254  const Position& p1 = *i;
255  const Position& p2 = *(i + 1);
256  const SUMOReal nextLength = p1.distanceTo(p2);
257  if (seenLength + nextLength > pos) {
258  return RAD2DEG(atan2(p2.z() - p1.z(), p1.distanceTo2D(p2)));
259  }
260  seenLength += nextLength;
261  } while (++i != end() - 1);
262  const Position& p1 = (*this)[-2];
263  const Position& p2 = back();
264  return RAD2DEG(atan2(p2.z() - p1.z(), p1.distanceTo2D(p2)));
265 }
266 
267 
268 Position
269 PositionVector::positionAtOffset(const Position& p1, const Position& p2, SUMOReal pos, SUMOReal lateralOffset) {
270  const SUMOReal dist = p1.distanceTo(p2);
271  if (pos < 0 || dist < pos) {
272  return Position::INVALID;
273  }
274  if (lateralOffset != 0) {
275  const Position offset = sideOffset(p1, p2, -lateralOffset); // move in the same direction as Position::move2side
276  if (pos == 0.) {
277  return p1 + offset;
278  }
279  return p1 + (p2 - p1) * (pos / dist) + offset;
280  }
281  if (pos == 0.) {
282  return p1;
283  }
284  return p1 + (p2 - p1) * (pos / dist);
285 }
286 
287 
288 Position
289 PositionVector::positionAtOffset2D(const Position& p1, const Position& p2, SUMOReal pos, SUMOReal lateralOffset) {
290  const SUMOReal dist = p1.distanceTo2D(p2);
291  if (pos < 0 || dist < pos) {
292  return Position::INVALID;
293  }
294  if (lateralOffset != 0) {
295  const Position offset = sideOffset(p1, p2, -lateralOffset); // move in the same direction as Position::move2side
296  if (pos == 0.) {
297  return p1 + offset;
298  }
299  return p1 + (p2 - p1) * (pos / dist) + offset;
300  }
301  if (pos == 0.) {
302  return p1;
303  }
304  return p1 + (p2 - p1) * (pos / dist);
305 }
306 
307 
308 Boundary
310  Boundary ret;
311  for (const_iterator i = begin(); i != end(); i++) {
312  ret.add(*i);
313  }
314  return ret;
315 }
316 
317 
318 Position
320  SUMOReal x = 0;
321  SUMOReal y = 0;
322  SUMOReal z = 0;
323  for (const_iterator i = begin(); i != end(); i++) {
324  x += (*i).x();
325  y += (*i).y();
326  z += (*i).z();
327  }
328  return Position(x / (SUMOReal) size(), y / (SUMOReal) size(), z / (SUMOReal)size());
329 }
330 
331 
332 Position
334  PositionVector tmp = *this;
335  if (!isClosed()) { // make sure its closed
336  tmp.push_back(tmp[0]);
337  }
338  const int endIndex = (int)tmp.size() - 1;
339  SUMOReal div = 0; // 6 * area including sign
340  SUMOReal x = 0;
341  SUMOReal y = 0;
342  if (tmp.area() != 0) { // numerical instability ?
343  // http://en.wikipedia.org/wiki/Polygon
344  for (int i = 0; i < endIndex; i++) {
345  const SUMOReal z = tmp[i].x() * tmp[i + 1].y() - tmp[i + 1].x() * tmp[i].y();
346  div += z; // area formula
347  x += (tmp[i].x() + tmp[i + 1].x()) * z;
348  y += (tmp[i].y() + tmp[i + 1].y()) * z;
349  }
350  div *= 3; // 6 / 2, the 2 comes from the area formula
351  return Position(x / div, y / div);
352  } else {
353  // compute by decomposing into line segments
354  // http://en.wikipedia.org/wiki/Centroid#By_geometric_decomposition
355  SUMOReal lengthSum = 0;
356  for (int i = 0; i < endIndex; i++) {
357  SUMOReal length = tmp[i].distanceTo(tmp[i + 1]);
358  x += (tmp[i].x() + tmp[i + 1].x()) * length / 2;
359  y += (tmp[i].y() + tmp[i + 1].y()) * length / 2;
360  lengthSum += length;
361  }
362  if (lengthSum == 0) {
363  // it is probably only one point
364  return tmp[0];
365  }
366  return Position(x / lengthSum, y / lengthSum);
367  }
368 }
369 
370 
371 void
373  Position centroid = getCentroid();
374  for (int i = 0; i < static_cast<int>(size()); i++) {
375  (*this)[i] = centroid + (((*this)[i] - centroid) * factor);
376  }
377 }
378 
379 
380 void
382  Position centroid = getCentroid();
383  for (int i = 0; i < static_cast<int>(size()); i++) {
384  (*this)[i] = centroid + (((*this)[i] - centroid) + offset);
385  }
386 }
387 
388 
389 Position
391  if (size() == 1) {
392  return (*this)[0];
393  }
394  return positionAtOffset(SUMOReal((length() / 2.)));
395 }
396 
397 
398 SUMOReal
400  SUMOReal len = 0;
401  for (const_iterator i = begin(); i != end() - 1; i++) {
402  len += (*i).distanceTo(*(i + 1));
403  }
404  return len;
405 }
406 
407 
408 SUMOReal
410  SUMOReal len = 0;
411  for (const_iterator i = begin(); i != end() - 1; i++) {
412  len += (*i).distanceTo2D(*(i + 1));
413  }
414  return len;
415 }
416 
417 
418 SUMOReal
420  if (size() < 3) {
421  return 0;
422  }
423  SUMOReal area = 0;
424  PositionVector tmp = *this;
425  if (!isClosed()) { // make sure its closed
426  tmp.push_back(tmp[0]);
427  }
428  const int endIndex = (int)tmp.size() - 1;
429  // http://en.wikipedia.org/wiki/Polygon
430  for (int i = 0; i < endIndex; i++) {
431  area += tmp[i].x() * tmp[i + 1].y() - tmp[i + 1].x() * tmp[i].y();
432  }
433  if (area < 0) { // we whether we had cw or ccw order
434  area *= -1;
435  }
436  return area / 2;
437 }
438 
439 
440 bool
442  for (const_iterator i = begin(); i != end() - 1; i++) {
443  if (poly.around(*i, offset)) {
444  return true;
445  }
446  }
447  return false;
448 }
449 
450 
451 bool
452 PositionVector::crosses(const Position& p1, const Position& p2) const {
453  return intersects(p1, p2);
454 }
455 
456 
457 std::pair<PositionVector, PositionVector>
459  if (size() < 2) {
460  throw InvalidArgument("Vector to short for splitting");
461  }
462  if (where <= POSITION_EPS || where >= length() - POSITION_EPS) {
463  WRITE_WARNING("Splitting vector close to end (pos: " + toString(where) + ", length: " + toString(length()) + ")");
464  }
465  PositionVector first, second;
466  first.push_back((*this)[0]);
467  SUMOReal seen = 0;
468  const_iterator it = begin() + 1;
469  SUMOReal next = first.back().distanceTo(*it);
470  // see how many points we can add to first
471  while (where >= seen + next + POSITION_EPS) {
472  seen += next;
473  first.push_back(*it);
474  it++;
475  next = first.back().distanceTo(*it);
476  }
477  if (fabs(where - (seen + next)) > POSITION_EPS || it == end() - 1) {
478  // we need to insert a new point because 'where' is not close to an
479  // existing point or it is to close to the endpoint
480  const Position p = positionAtOffset(first.back(), *it, where - seen);
481  first.push_back(p);
482  second.push_back(p);
483  } else {
484  first.push_back(*it);
485  }
486  // add the remaining points to second
487  for (; it != end(); it++) {
488  second.push_back(*it);
489  }
490  assert(first.size() >= 2);
491  assert(second.size() >= 2);
492  assert(first.back() == second.front());
493  assert(fabs(first.length() + second.length() - length()) < 2 * POSITION_EPS);
494  return std::pair<PositionVector, PositionVector>(first, second);
495 }
496 
497 
498 std::ostream&
499 operator<<(std::ostream& os, const PositionVector& geom) {
500  for (PositionVector::const_iterator i = geom.begin(); i != geom.end(); i++) {
501  if (i != geom.begin()) {
502  os << " ";
503  }
504  os << (*i);
505  }
506  return os;
507 }
508 
509 
510 void
512  std::sort(begin(), end(), as_poly_cw_sorter());
513 }
514 
515 
516 void
518  for (int i = 0; i < static_cast<int>(size()); i++) {
519  (*this)[i].add(xoff, yoff, zoff);
520  }
521 }
522 
523 
524 void
526  add(offset.x(), offset.y(), offset.z());
527 }
528 
529 
530 void
532  for (int i = 0; i < static_cast<int>(size()); i++) {
533  (*this)[i].mul(1, -1);
534  }
535 }
536 
537 
539 
540 
541 int
543  return atan2(p1.x(), p1.y()) < atan2(p2.x(), p2.y());
544 }
545 
546 
547 void
549  std::sort(begin(), end(), increasing_x_y_sorter());
550 }
551 
552 
554 
555 
556 int
558  const Position& p2) const {
559  if (p1.x() != p2.x()) {
560  return p1.x() < p2.x();
561  }
562  return p1.y() < p2.y();
563 }
564 
565 
566 SUMOReal
567 PositionVector::isLeft(const Position& P0, const Position& P1, const Position& P2) const {
568  return (P1.x() - P0.x()) * (P2.y() - P0.y()) - (P2.x() - P0.x()) * (P1.y() - P0.y());
569 }
570 
571 
574  PositionVector ret = *this;
575  ret.sortAsPolyCWByAngle();
576  return simpleHull_2D(ret);
577 }
578 
579 
580 void
582  if (size() > 0 && v.size() > 0 && back().distanceTo(v[0]) < sameThreshold) {
583  copy(v.begin() + 1, v.end(), back_inserter(*this));
584  } else {
585  copy(v.begin(), v.end(), back_inserter(*this));
586  }
587 }
588 
589 
591 PositionVector::getSubpart(SUMOReal beginOffset, SUMOReal endOffset) const {
592  PositionVector ret;
593  Position begPos = front();
594  if (beginOffset > POSITION_EPS) {
595  begPos = positionAtOffset(beginOffset);
596  }
597  Position endPos = back();
598  if (endOffset < length() - POSITION_EPS) {
599  endPos = positionAtOffset(endOffset);
600  }
601  ret.push_back(begPos);
602 
603  SUMOReal seen = 0;
604  const_iterator i = begin();
605  // skip previous segments
606  while ((i + 1) != end()
607  &&
608  seen + (*i).distanceTo(*(i + 1)) < beginOffset) {
609  seen += (*i).distanceTo(*(i + 1));
610  i++;
611  }
612  // append segments in between
613  while ((i + 1) != end()
614  &&
615  seen + (*i).distanceTo(*(i + 1)) < endOffset) {
616 
617  ret.push_back_noDoublePos(*(i + 1));
618  seen += (*i).distanceTo(*(i + 1));
619  i++;
620  }
621  // append end
622  ret.push_back_noDoublePos(endPos);
623  return ret;
624 }
625 
626 
628 PositionVector::getSubpart2D(SUMOReal beginOffset, SUMOReal endOffset) const {
629  PositionVector ret;
630  Position begPos = front();
631  if (beginOffset > POSITION_EPS) {
632  begPos = positionAtOffset2D(beginOffset);
633  }
634  Position endPos = back();
635  if (endOffset < length2D() - POSITION_EPS) {
636  endPos = positionAtOffset2D(endOffset);
637  }
638  ret.push_back(begPos);
639 
640  SUMOReal seen = 0;
641  const_iterator i = begin();
642  // skip previous segments
643  while ((i + 1) != end()
644  &&
645  seen + (*i).distanceTo2D(*(i + 1)) < beginOffset) {
646  seen += (*i).distanceTo2D(*(i + 1));
647  i++;
648  }
649  // append segments in between
650  while ((i + 1) != end()
651  &&
652  seen + (*i).distanceTo2D(*(i + 1)) < endOffset) {
653 
654  ret.push_back_noDoublePos(*(i + 1));
655  seen += (*i).distanceTo2D(*(i + 1));
656  i++;
657  }
658  // append end
659  ret.push_back_noDoublePos(endPos);
660  return ret;
661 }
662 
663 
665 PositionVector::getSubpartByIndex(int beginIndex, int count) const {
666  if (beginIndex < 0) {
667  beginIndex += (int)size();
668  }
669  assert(count >= 0);
670  assert(beginIndex < (int)size());
671  assert(beginIndex + count <= (int)size());
672  PositionVector result;
673  for (int i = beginIndex; i < beginIndex + count; ++i) {
674  result.push_back((*this)[i]);
675  }
676  return result;
677 }
678 
679 
680 SUMOReal
682  return front().angleTo2D(back());
683 }
684 
685 
686 SUMOReal
687 PositionVector::nearest_offset_to_point2D(const Position& p, bool perpendicular) const {
690  SUMOReal seen = 0;
691  for (const_iterator i = begin(); i != end() - 1; i++) {
692  const SUMOReal pos =
693  GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, perpendicular);
694  const SUMOReal dist = pos == GeomHelper::INVALID_OFFSET ? minDist : p.distanceTo2D(positionAtOffset2D(*i, *(i + 1), pos));
695  if (dist < minDist) {
696  nearestPos = pos + seen;
697  minDist = dist;
698  }
699  if (perpendicular && i != begin() && pos == GeomHelper::INVALID_OFFSET) {
700  // even if perpendicular is set we still need to check the distance to the inner points
701  const SUMOReal cornerDist = p.distanceTo2D(*i);
702  if (cornerDist < minDist) {
703  const SUMOReal pos1 =
704  GeomHelper::nearest_offset_on_line_to_point2D(*(i - 1), *i, p, false);
705  const SUMOReal pos2 =
706  GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, false);
707  if (pos1 == (*(i - 1)).distanceTo2D(*i) && pos2 == 0.) {
708  nearestPos = seen;
709  minDist = cornerDist;
710  }
711  }
712  }
713  seen += (*i).distanceTo2D(*(i + 1));
714  }
715  return nearestPos;
716 }
717 
718 
719 Position
721  // @toDo this duplicates most of the code in nearest_offset_to_point2D. It should be refactored
722  if (extend) {
723  PositionVector extended = *this;
724  const SUMOReal dist = 2 * distance2D(p);
725  extended.extrapolate(dist);
726  return extended.transformToVectorCoordinates(p) - Position(dist, 0);
727  }
729  SUMOReal nearestPos = -1;
730  SUMOReal seen = 0;
731  int sign = 1;
732  for (const_iterator i = begin(); i != end() - 1; i++) {
733  const SUMOReal pos =
734  GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, true);
735  const SUMOReal dist = pos < 0 ? minDist : p.distanceTo2D(positionAtOffset(*i, *(i + 1), pos));
736  if (dist < minDist) {
737  nearestPos = pos + seen;
738  minDist = dist;
739  sign = isLeft(*i, *(i + 1), p) >= 0 ? -1 : 1;
740  }
741  if (i != begin() && pos == GeomHelper::INVALID_OFFSET) {
742  // even if perpendicular is set we still need to check the distance to the inner points
743  const SUMOReal cornerDist = p.distanceTo2D(*i);
744  if (cornerDist < minDist) {
745  const SUMOReal pos1 =
746  GeomHelper::nearest_offset_on_line_to_point2D(*(i - 1), *i, p, false);
747  const SUMOReal pos2 =
748  GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, false);
749  if (pos1 == (*(i - 1)).distanceTo2D(*i) && pos2 == 0.) {
750  nearestPos = seen;
751  minDist = cornerDist;
752  sign = isLeft(*(i - 1), *i, p) >= 0 ? -1 : 1;
753  }
754  }
755  }
756  seen += (*i).distanceTo2D(*(i + 1));
757  }
758  if (nearestPos != -1) {
759  return Position(nearestPos, sign * minDist);
760  } else {
761  return Position::INVALID;
762  }
763 }
764 
765 
766 int
768  assert(size() > 0);
770  SUMOReal dist;
771  int closest = 0;
772  for (int i = 0; i < (int)size(); i++) {
773  dist = p.distanceTo((*this)[i]);
774  if (dist < minDist) {
775  closest = i;
776  minDist = dist;
777  }
778  }
779  return closest;
780 }
781 
782 
783 int
786  int insertionIndex = 1;
787  for (int i = 0; i < (int)size() - 1; i++) {
788  const SUMOReal length = GeomHelper::nearest_offset_on_line_to_point2D((*this)[i], (*this)[i + 1], p, false);
789  const Position& outIntersection = PositionVector::positionAtOffset2D((*this)[i], (*this)[i + 1], length);
790  const SUMOReal dist = p.distanceTo2D(outIntersection);
791  if (dist < minDist) {
792  insertionIndex = i + 1;
793  minDist = dist;
794  }
795  }
796  insert(begin() + insertionIndex, p);
797  return insertionIndex;
798 }
799 
800 
801 int
803  if (size() == 0) {
804  return -1;
805  }
807  int removalIndex = 0;
808  for (int i = 0; i < (int)size(); i++) {
809  const SUMOReal dist = p.distanceTo2D((*this)[i]);
810  if (dist < minDist) {
811  removalIndex = i;
812  minDist = dist;
813  }
814  }
815  erase(begin() + removalIndex);
816  return removalIndex;
817 }
818 
819 
820 std::vector<SUMOReal>
822  std::vector<SUMOReal> ret;
823  for (const_iterator i = other.begin(); i != other.end() - 1; i++) {
824  std::vector<SUMOReal> atSegment = intersectsAtLengths2D(*i, *(i + 1));
825  copy(atSegment.begin(), atSegment.end(), back_inserter(ret));
826  }
827  return ret;
828 }
829 
830 
831 std::vector<SUMOReal>
833  std::vector<SUMOReal> ret;
834  SUMOReal pos = 0;
835  for (const_iterator i = begin(); i != end() - 1; i++) {
836  const Position& p1 = *i;
837  const Position& p2 = *(i + 1);
838  SUMOReal x, y, m;
839  if (intersects(p1, p2, lp1, lp2, 0., &x, &y, &m)) {
840  ret.push_back(Position(x, y).distanceTo2D(p1) + pos);
841  }
842  pos += p1.distanceTo2D(p2);
843  }
844  return ret;
845 }
846 
847 
848 void
849 PositionVector::extrapolate(const SUMOReal val, const bool onlyFirst) {
850  assert(size() > 1);
851  Position& p1 = (*this)[0];
852  Position& p2 = (*this)[1];
853  const Position offset = (p2 - p1) * (val / p1.distanceTo(p2));
854  p1.sub(offset);
855  if (!onlyFirst) {
856  if (size() == 2) {
857  p2.add(offset);
858  } else {
859  const Position& e1 = (*this)[-2];
860  Position& e2 = (*this)[-1];
861  e2.sub((e1 - e2) * (val / e1.distanceTo(e2)));
862  }
863  }
864 }
865 
866 
867 void
868 PositionVector::extrapolate2D(const SUMOReal val, const bool onlyFirst) {
869  assert(size() > 1);
870  Position& p1 = (*this)[0];
871  Position& p2 = (*this)[1];
872  const Position offset = (p2 - p1) * (val / p1.distanceTo2D(p2));
873  p1.sub(offset);
874  if (!onlyFirst) {
875  if (size() == 2) {
876  p2.add(offset);
877  } else {
878  const Position& e1 = (*this)[-2];
879  Position& e2 = (*this)[-1];
880  e2.sub((e1 - e2) * (val / e1.distanceTo2D(e2)));
881  }
882  }
883 }
884 
885 
888  PositionVector ret;
889  for (const_reverse_iterator i = rbegin(); i != rend(); i++) {
890  ret.push_back(*i);
891  }
892  return ret;
893 }
894 
895 
896 Position
897 PositionVector::sideOffset(const Position& beg, const Position& end, const SUMOReal amount) {
898  const SUMOReal scale = amount / beg.distanceTo2D(end);
899  return Position((beg.y() - end.y()) * scale, (end.x() - beg.x()) * scale);
900 }
901 
902 
903 void
905  if (size() < 2) {
906  return;
907  }
908  if (length2D() == 0) {
909  return;
910  }
911  PositionVector shape;
912  for (int i = 0; i < static_cast<int>(size()); i++) {
913  if (i == 0) {
914  const Position& from = (*this)[i];
915  const Position& to = (*this)[i + 1];
916  shape.push_back(from - sideOffset(from, to, amount));
917  } else if (i == static_cast<int>(size()) - 1) {
918  const Position& from = (*this)[i - 1];
919  const Position& to = (*this)[i];
920  shape.push_back(to - sideOffset(from, to, amount));
921  } else {
922  const Position& from = (*this)[i - 1];
923  const Position& me = (*this)[i];
924  const Position& to = (*this)[i + 1];
925  PositionVector fromMe(from, me);
926  fromMe.extrapolate2D(me.distanceTo2D(to));
927  const SUMOReal extrapolateDev = fromMe[1].distanceTo2D(to);
928  if (fabs(extrapolateDev) < POSITION_EPS) {
929  // parallel case, just shift the middle point
930  shape.push_back(me - sideOffset(from, to, amount));
931  } else if (fabs(extrapolateDev - 2 * me.distanceTo2D(to)) < POSITION_EPS) {
932  // counterparallel case, just shift the middle point
933  PositionVector fromMe(from, me);
934  fromMe.extrapolate2D(amount);
935  shape.push_back(fromMe[1]);
936  } else {
937  Position offsets = sideOffset(from, me, amount);
938  Position offsets2 = sideOffset(me, to, amount);
939  PositionVector l1(from - offsets, me - offsets);
940  PositionVector l2(me - offsets2, to - offsets2);
941  shape.push_back(l1.intersectionPosition2D(l2[0], l2[1], 100));
942  if (shape.back() == Position::INVALID) {
943  throw InvalidArgument("no line intersection");
944  }
945  }
946  // copy original z value
947  shape.back().set(shape.back().x(), shape.back().y(), me.z());
948  }
949  }
950  *this = shape;
951 }
952 
953 
954 SUMOReal
956  assert((int)size() > pos + 1);
957  return (*this)[pos].angleTo2D((*this)[pos + 1]);
958 }
959 
960 
961 void
963  if (size() == 0 || (*this)[0] == back()) {
964  return;
965  }
966  push_back((*this)[0]);
967 }
968 
969 
970 std::vector<SUMOReal>
971 PositionVector::distances(const PositionVector& s, bool perpendicular) const {
972  std::vector<SUMOReal> ret;
973  const_iterator i;
974  for (i = begin(); i != end(); i++) {
975  const SUMOReal dist = s.distance2D(*i, perpendicular);
976  if (dist != GeomHelper::INVALID_OFFSET) {
977  ret.push_back(dist);
978  }
979  }
980  for (i = s.begin(); i != s.end(); i++) {
981  const SUMOReal dist = distance2D(*i, perpendicular);
982  if (dist != GeomHelper::INVALID_OFFSET) {
983  ret.push_back(dist);
984  }
985  }
986  return ret;
987 }
988 
989 
990 SUMOReal
991 PositionVector::distance2D(const Position& p, bool perpendicular) const {
992  if (size() == 0) {
994  } else if (size() == 1) {
995  return front().distanceTo(p);
996  }
997  const SUMOReal nearestOffset = nearest_offset_to_point2D(p, perpendicular);
998  if (nearestOffset == GeomHelper::INVALID_OFFSET) {
1000  } else {
1001  return p.distanceTo2D(positionAtOffset2D(nearestOffset));
1002  }
1003 }
1004 
1005 
1006 void
1008  if (size() == 0 || !p.almostSame(back())) {
1009  push_back(p);
1010  }
1011 }
1012 
1013 
1014 void
1016  if (size() == 0 || !p.almostSame(front())) {
1017  insert(begin(), p);
1018  }
1019 }
1020 
1021 
1022 bool
1024  return size() >= 2 && (*this)[0] == back();
1025 }
1026 
1027 
1028 void
1029 PositionVector::removeDoublePoints(SUMOReal minDist, bool assertLength) {
1030  if (size() > 1) {
1031  iterator last = begin();
1032  for (iterator i = begin() + 1; i != end() && (!assertLength || size() > 2);) {
1033  if (last->almostSame(*i, minDist)) {
1034  i = erase(i);
1035  } else {
1036  last = i;
1037  ++i;
1038  }
1039  }
1040  }
1041 }
1042 
1043 
1044 bool
1046  if (size() == v2.size()) {
1047  for (int i = 0; i < (int)size(); i++) {
1048  if ((*this)[i] != v2[i]) {
1049  return false;
1050  }
1051  }
1052  return true;
1053  } else {
1054  return false;
1055  }
1056 }
1057 
1058 
1059 bool
1061  if (size() > 2) {
1062  return false;
1063  }
1064  for (const_iterator i = begin(); i != end() - 1; i++) {
1065  if ((*i).z() != (*(i + 1)).z()) {
1066  return true;
1067  }
1068  }
1069  return false;
1070 }
1071 
1072 
1073 bool
1074 PositionVector::intersects(const Position& p11, const Position& p12, const Position& p21, const Position& p22, const SUMOReal withinDist, SUMOReal* x, SUMOReal* y, SUMOReal* mu) {
1075  const SUMOReal eps = std::numeric_limits<SUMOReal>::epsilon();
1076  const double denominator = (p22.y() - p21.y()) * (p12.x() - p11.x()) - (p22.x() - p21.x()) * (p12.y() - p11.y());
1077  const double numera = (p22.x() - p21.x()) * (p11.y() - p21.y()) - (p22.y() - p21.y()) * (p11.x() - p21.x());
1078  const double numerb = (p12.x() - p11.x()) * (p11.y() - p21.y()) - (p12.y() - p11.y()) * (p11.x() - p21.x());
1079  /* Are the lines coincident? */
1080  if (fabs(numera) < eps && fabs(numerb) < eps && fabs(denominator) < eps) {
1081  SUMOReal a1;
1082  SUMOReal a2;
1083  SUMOReal a3;
1084  SUMOReal a4;
1085  SUMOReal a = -1e12;
1086  if (p11.x() != p12.x()) {
1087  a1 = p11.x() < p12.x() ? p11.x() : p12.x();
1088  a2 = p11.x() < p12.x() ? p12.x() : p11.x();
1089  a3 = p21.x() < p22.x() ? p21.x() : p22.x();
1090  a4 = p21.x() < p22.x() ? p22.x() : p21.x();
1091  } else {
1092  a1 = p11.y() < p12.y() ? p11.y() : p12.y();
1093  a2 = p11.y() < p12.y() ? p12.y() : p11.y();
1094  a3 = p21.y() < p22.y() ? p21.y() : p22.y();
1095  a4 = p21.y() < p22.y() ? p22.y() : p21.y();
1096  }
1097  if (a1 <= a3 && a3 <= a2) {
1098  if (a4 < a2) {
1099  a = (a3 + a4) / 2;
1100  } else {
1101  a = (a2 + a3) / 2;
1102  }
1103  }
1104  if (a3 <= a1 && a1 <= a4) {
1105  if (a2 < a4) {
1106  a = (a1 + a2) / 2;
1107  } else {
1108  a = (a1 + a4) / 2;
1109  }
1110  }
1111  if (a != -1e12) {
1112  if (x != 0) {
1113  if (p11.x() != p12.x()) {
1114  *mu = (a - p11.x()) / (p12.x() - p11.x());
1115  *x = a;
1116  *y = p11.y() + (*mu) * (p12.y() - p11.y());
1117  } else {
1118  *x = p11.x();
1119  *y = a;
1120  if (p12.y() == p11.y()) {
1121  *mu = 0;
1122  } else {
1123  *mu = (a - p11.y()) / (p12.y() - p11.y());
1124  }
1125  }
1126  }
1127  return true;
1128  }
1129  return false;
1130  }
1131  /* Are the lines parallel */
1132  if (fabs(denominator) < eps) {
1133  return false;
1134  }
1135  /* Is the intersection along the segments */
1136  double mua = numera / denominator;
1137  /* reduce rounding errors for lines ending in the same point */
1138  if (fabs(p12.x() - p22.x()) < eps && fabs(p12.y() - p22.y()) < eps) {
1139  mua = 1.;
1140  } else {
1141  const double offseta = withinDist / p11.distanceTo2D(p12);
1142  const double offsetb = withinDist / p21.distanceTo2D(p22);
1143  const double mub = numerb / denominator;
1144  if (mua < -offseta || mua > 1 + offseta || mub < -offsetb || mub > 1 + offsetb) {
1145  return false;
1146  }
1147  }
1148  if (x != 0) {
1149  *x = p11.x() + mua * (p12.x() - p11.x());
1150  *y = p11.y() + mua * (p12.y() - p11.y());
1151  *mu = mua;
1152  }
1153  return true;
1154 }
1155 
1156 
1157 void
1159  const SUMOReal s = sin(angle);
1160  const SUMOReal c = cos(angle);
1161  for (int i = 0; i < (int)size(); i++) {
1162  const SUMOReal x = (*this)[i].x();
1163  const SUMOReal y = (*this)[i].y();
1164  const SUMOReal z = (*this)[i].z();
1165  const SUMOReal xnew = x * c - y * s;
1166  const SUMOReal ynew = x * s + y * c;
1167  (*this)[i].set(xnew, ynew, z);
1168  }
1169 }
1170 
1171 
1174  PositionVector result = *this;
1175  bool changed = true;
1176  while (changed && result.size() > 3) {
1177  changed = false;
1178  for (int i = 0; i < (int)result.size(); i++) {
1179  const Position& p1 = result[i];
1180  const Position& p2 = result[(i + 2) % result.size()];
1181  const int middleIndex = (i + 1) % result.size();
1182  const Position& p0 = result[middleIndex];
1183  // https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line#Line_defined_by_two_points
1184  const SUMOReal triangleArea2 = fabs((p2.y() - p1.y()) * p0.x() - (p2.x() - p1.x()) * p0.y() + p2.x() * p1.y() - p2.y() * p1.x());
1185  const SUMOReal distIK = p1.distanceTo2D(p2);
1186  if (distIK > NUMERICAL_EPS && triangleArea2 / distIK < NUMERICAL_EPS) {
1187  changed = true;
1188  result.erase(result.begin() + middleIndex);
1189  break;
1190  }
1191  }
1192  }
1193  return result;
1194 }
1195 
1196 
1198 PositionVector::getOrthogonal(const Position& p, SUMOReal extend, SUMOReal& distToClosest) const {
1199  PositionVector result;
1200  PositionVector tmp = *this;
1201  tmp.extrapolate2D(extend);
1202  const SUMOReal baseOffset = tmp.nearest_offset_to_point2D(p);
1203  if (baseOffset == GeomHelper::INVALID_OFFSET) {
1204  // fail
1205  return result;
1206  }
1207  Position base = tmp.positionAtOffset2D(baseOffset);
1208  distToClosest = tmp[tmp.indexOfClosest(base)].distanceTo2D(base);
1209  if (p.distanceTo2D(base) > NUMERICAL_EPS) {
1210  result.push_back(p);
1211  result.push_back(base);
1212  }
1213  return result;
1214 }
1215 
1216 /****************************************************************************/
1217 
PositionVector getOrthogonal(const Position &p, SUMOReal extend, SUMOReal &distToClosest) const
return orthogonal through p (extending this vector if necessary)
void sub(SUMOReal dx, SUMOReal dy)
Substracts the given position from this one.
Definition: Position.h:139
static SUMOReal 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:94
bool overlapsWith(const AbstractPoly &poly, SUMOReal offset=0) const
Returns the information whether the given polygon overlaps with this.
clase for increasing Sorter
static Position sideOffset(const Position &beg, const Position &end, const SUMOReal amount)
get a side position of position vector using a offset
int operator()(const Position &p1, const Position &p2) const
comparing operation
PositionVector getSubpart2D(SUMOReal beginOffset, SUMOReal endOffset) const
get subpart of a position vector in two dimensions (Z is ignored)
SUMOReal angleTo2D(const Position &other) const
returns the angle in the plane of the vector pointing from here to the other position ...
Definition: Position.h:243
void sortAsPolyCWByAngle()
short as polygon CV by angle
void add(const Position &pos)
Adds the given position to this one.
Definition: Position.h:119
int indexOfClosest(const Position &p) const
index of the closest position to p
SUMOReal distanceTo(const Position &p2) const
returns the euclidean distance in 3 dimension
Definition: Position.h:221
SUMOReal slopeDegreeAtOffset(SUMOReal pos) const
Returns the slope at the given length.
SUMOReal length() const
Returns the length.
#define M_PI
Definition: angles.h:37
friend std::ostream & operator<<(std::ostream &os, const PositionVector &geom)
Position positionAtOffset(SUMOReal pos, SUMOReal lateralOffset=0) const
Returns the position at the given length.
void scaleRelative(SUMOReal factor)
enlarges/shrinks the polygon by a factor based at the centroid
Position getCentroid() const
Returns the centroid (closes the polygon if unclosed)
std::pair< PositionVector, PositionVector > splitAt(SUMOReal where) const
Returns the two lists made when this list vector is splitted at the given point.
PositionVector reverse() const
reverse position vector
#define RAD2DEG(x)
Definition: GeomHelper.h:46
bool hasElevation() const
return whether two positions differ in z-coordinate
std::vector< SUMOReal > distances(const PositionVector &s, bool perpendicular=false) const
distances of all my points to s and all of s points to myself
A class that stores a 2D geometrical boundary.
Definition: Boundary.h:48
SUMOReal rotationAtOffset(SUMOReal pos) const
Returns the rotation at the given length.
#define WRITE_WARNING(msg)
Definition: MsgHandler.h:200
Position getLineCenter() const
get line center
SUMOReal nearest_offset_to_point2D(const Position &p, bool perpendicular=true) const
return the nearest offest to point 2D
SUMOReal beginEndAngle() const
returns the angle in radians of the line connecting the first and the last position ...
SUMOReal distanceTo2D(const Position &p2) const
returns the euclidean distance in the x-y-plane
Definition: Position.h:232
void rotate2D(SUMOReal angle)
~PositionVector()
Destructor.
void extrapolate2D(const SUMOReal 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
#define max(a, b)
Definition: polyfonts.c:65
SUMOReal angleAt2D(int pos) const
get angle in certain position of position vector
static SUMOReal legacyDegree(const SUMOReal angle, const bool positive=false)
Definition: GeomHelper.cpp:204
bool operator==(const PositionVector &v2) const
comparing operation
SUMOReal z() const
Returns the z-position.
Definition: Position.h:73
A point in 2D or 3D with translation and scaling methods.
Definition: Position.h:46
A list of positions.
void add(SUMOReal xoff, SUMOReal yoff, SUMOReal zoff)
Position getPolygonCenter() const
Returns the arithmetic of all corner points.
int operator()(const Position &p1, const Position &p2) const
comparing operation for sort
SUMOReal rotationDegreeAtOffset(SUMOReal pos) const
Returns the rotation at the given length.
PositionVector getSubpart(SUMOReal beginOffset, SUMOReal endOffset) const
get subpart of a position vector
SUMOReal x() const
Returns the x-position.
Definition: Position.h:63
#define POSITION_EPS
Definition: config.h:187
SUMOReal length2D() const
Returns the length.
const Position & operator[](int index) const
returns the constat position at the given index !!! exceptions?
SUMOReal 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) ...
int insertAtClosest(const Position &p)
inserts p between the two closest positions and returns the insertion index
std::string toString(const T &t, std::streamsize accuracy=OUTPUT_ACCURACY)
Definition: ToString.h:55
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
std::vector< SUMOReal > intersectsAtLengths2D(const PositionVector &other) const
For all intersections between this vector and other, return the 2D-length of the subvector from this ...
virtual bool around(const Position &p, SUMOReal offset=0) const =0
PositionVector simplified() const
return the same shape with intermediate colinear points removed
PositionVector()
Constructor. Creates an empty position vector.
PositionVector simpleHull_2D(const PositionVector &V)
#define sign(a)
Definition: polyfonts.c:68
void removeDoublePoints(SUMOReal minDist=POSITION_EPS, bool assertLength=false)
Removes positions if too near.
SUMOReal area() const
Returns the area (0 for non-closed)
void add(SUMOReal x, SUMOReal y, SUMOReal z=0)
Makes the boundary include the given coordinate.
Definition: Boundary.cpp:90
bool almostSame(const Position &p2, SUMOReal maxDiv=POSITION_EPS) const
Definition: Position.h:215
bool isClosed() const
check if PositionVector is closed
void scaleAbsolute(SUMOReal offset)
enlarges/shrinks the polygon by an absolute offset based at the centroid
bool around(const Position &p, SUMOReal offset=0) const
Returns the information whether the position vector describes a polygon lying around the given point...
static SUMOReal nearest_offset_on_line_to_point2D(const Position &lineStart, const Position &lineEnd, const Position &p, bool perpendicular=true)
Definition: GeomHelper.cpp:100
Position positionAtOffset2D(SUMOReal pos, SUMOReal lateralOffset=0) const
Returns the position at the given length.
PositionVector convexHull() const
void move2side(SUMOReal amount)
move position vector to side using certain ammount
#define SUMOReal
Definition: config.h:213
Boundary getBoxBoundary() const
Returns a boundary enclosing this list of lines.
#define NUMERICAL_EPS
Definition: config.h:160
void push_back_noDoublePos(const Position &p)
insert in back a non double position
bool crosses(const Position &p1, const Position &p2) const
Position intersectionPosition2D(const Position &p1, const Position &p2, const SUMOReal withinDist=0.) const
Returns the position of the intersection.
bool partialWithin(const AbstractPoly &poly, SUMOReal offset=0) const
Returns the information whether this polygon lies partially within the given polygon.
SUMOReal y() const
Returns the y-position.
Definition: Position.h:68
void closePolygon()
ensures that the last position equals the first
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.
SUMOReal isLeft(const Position &P0, const Position &P1, const Position &P2) const
get left
void append(const PositionVector &v, SUMOReal sameThreshold=2.0)
void extrapolate(const SUMOReal val, const bool onlyFirst=false)
extrapolate position vector
static const SUMOReal INVALID_OFFSET
a value to signify offsets outside the range of [0, Line.length()]
Definition: GeomHelper.h:59
static const Position INVALID
Definition: Position.h:261