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