SUMO - Simulation of Urban MObility
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
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-sim.org/
13 // Copyright (C) 2001-2013 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 "Line.h"
49 #include "Helper_ConvexHull.h"
50 #include "Boundary.h"
51 
52 #ifdef CHECK_MEMORY_LEAKS
53 #include <foreign/nvwa/debug_new.h>
54 #endif // CHECK_MEMORY_LEAKS
55 
56 
57 // ===========================================================================
58 // method definitions
59 // ===========================================================================
61 
62 
63 PositionVector::PositionVector(const std::vector<Position>& v) {
64  std::copy(v.begin(), v.end(), std::back_inserter(*this));
65 }
66 
67 
69 
70 
71 // ------------ Adding items to the container
72 void
74  copy(p.begin(), p.end(), back_inserter(*this));
75 }
76 
77 
78 void
80  insert(begin(), p);
81 }
82 
83 
86  Position first = front();
87  erase(begin());
88  return first;
89 }
90 
91 
92 bool
93 PositionVector::around(const Position& p, SUMOReal offset) const {
94  if (offset != 0) {
95  //throw 1; // !!! not yet implemented
96  }
97  SUMOReal angle = 0;
98  for (const_iterator i = begin(); i != end() - 1; i++) {
99  Position p1(
100  (*i).x() - p.x(),
101  (*i).y() - p.y());
102  Position p2(
103  (*(i + 1)).x() - p.x(),
104  (*(i + 1)).y() - p.y());
105  angle += GeomHelper::Angle2D(p1.x(), p1.y(), p2.x(), p2.y());
106  }
107  Position p1(
108  (*(end() - 1)).x() - p.x(),
109  (*(end() - 1)).y() - p.y());
110  Position p2(
111  (*(begin())).x() - p.x(),
112  (*(begin())).y() - p.y());
113  angle += GeomHelper::Angle2D(p1.x(), p1.y(), p2.x(), p2.y());
114  return (!(fabs(angle) < M_PI));
115 }
116 
117 
118 bool
120  for (const_iterator i = begin(); i != end() - 1; i++) {
121  if (poly.around(*i, offset)) {
122  return true;
123  }
124  }
125  return false;
126 }
127 
128 
129 bool
130 PositionVector::intersects(const Position& p1, const Position& p2) const {
131  if (size() < 2) {
132  return false;
133  }
134  for (const_iterator i = begin(); i != end() - 1; i++) {
135  if (GeomHelper::intersects(*i, *(i + 1), p1, p2)) {
136  return true;
137  }
138  }
139  //return GeomHelper::intersects(*(end()-1), *(begin()), p1, p2);
140  return false;
141 }
142 
143 
144 bool
146  if (size() < 2) {
147  return false;
148  }
149  for (const_iterator i = begin(); i != end() - 1; i++) {
150  if (v1.intersects(*i, *(i + 1))) {
151  return true;
152  }
153  }
154  //return v1.intersects(*(end()-1), *(begin()));
155  return false;
156 }
157 
158 
159 Position
161  const Position& p2) const {
162  for (const_iterator i = begin(); i != end() - 1; i++) {
163  if (GeomHelper::intersects(*i, *(i + 1), p1, p2)) {
164  return GeomHelper::intersection_position2D(*i, *(i + 1), p1, p2);
165  }
166  }
167  return Position(-1, -1);
168 }
169 
170 
171 Position
173  for (const_iterator i = begin(); i != end() - 1; i++) {
174  if (v1.intersects(*i, *(i + 1))) {
175  return v1.intersectsAtPoint(*i, *(i + 1));
176  }
177  }
178  /*
179  if(v1.intersects(*(end()-1), *(begin()))) {
180  return v1.intersectsAtPoint(*(end()-1), *(begin()));
181  }
182  */
183  return Position(-1, -1);
184 }
185 
186 
187 const Position&
188 PositionVector::operator[](int index) const {
189  if (index >= 0) {
190  return at(index);
191  } else {
192  return at(size() + index);
193  }
194 }
195 
196 
197 Position&
199  if (index >= 0) {
200  return at(index);
201  } else {
202  return at(size() + index);
203  }
204 }
205 
206 
207 Position
209  const_iterator i = begin();
210  SUMOReal seenLength = 0;
211  do {
212  const SUMOReal nextLength = (*i).distanceTo(*(i + 1));
213  if (seenLength + nextLength > pos) {
214  return positionAtOffset(*i, *(i + 1), pos - seenLength);
215  }
216  seenLength += nextLength;
217  } while (++i != end() - 1);
218  return back();
219 }
220 
221 
222 Position
224  const_iterator i = begin();
225  SUMOReal seenLength = 0;
226  do {
227  const SUMOReal nextLength = (*i).distanceTo2D(*(i + 1));
228  if (seenLength + nextLength > pos) {
229  return positionAtOffset2D(*i, *(i + 1), pos - seenLength);
230  }
231  seenLength += nextLength;
232  } while (++i != end() - 1);
233  return back();
234 }
235 
236 
237 SUMOReal
239  const_iterator i = begin();
240  SUMOReal seenLength = 0;
241  do {
242  SUMOReal nextLength = (*i).distanceTo(*(i + 1));
243  if (seenLength + nextLength > pos) {
244  Line l(*i, *(i + 1));
245  return l.atan2DegreeAngle();
246  }
247  seenLength += nextLength;
248  } while (++i != end() - 1);
249  Line l(*(end() - 2), *(end() - 1));
250  return l.atan2DegreeAngle();
251 }
252 
253 SUMOReal
255  const_iterator i = begin();
256  SUMOReal seenLength = 0;
257  do {
258  SUMOReal nextLength = (*i).distanceTo(*(i + 1));
259  if (seenLength + nextLength > pos) {
260  Line l(*i, *(i + 1));
261  return l.atan2DegreeSlope();
262  }
263  seenLength += nextLength;
264  } while (++i != end() - 1);
265  Line l(*(end() - 2), *(end() - 1));
266  return l.atan2DegreeSlope();
267 }
268 
269 Position
271  const Position& p2,
272  SUMOReal pos) {
273  const SUMOReal dist = p1.distanceTo(p2);
274  if (dist < pos) {
275  return Position(-1, -1);
276  }
277  return p1 + (p2 - p1) * (pos / dist);
278 }
279 
280 
281 Position
283  const Position& p2,
284  SUMOReal pos) {
285  const SUMOReal dist = p1.distanceTo2D(p2);
286  if (dist < pos) {
287  return Position(-1, -1);
288  }
289  return p1 + (p2 - p1) * (pos / dist);
290 }
291 
292 
293 Boundary
295  Boundary ret;
296  for (const_iterator i = begin(); i != end(); i++) {
297  ret.add(*i);
298  }
299  return ret;
300 }
301 
302 
303 Position
305  SUMOReal x = 0;
306  SUMOReal y = 0;
307  for (const_iterator i = begin(); i != end(); i++) {
308  x += (*i).x();
309  y += (*i).y();
310  }
311  return Position(x / (SUMOReal) size(), y / (SUMOReal) size());
312 }
313 
314 
315 Position
317  PositionVector tmp = *this;
318  if (!isClosed()) { // make sure its closed
319  tmp.push_back(tmp[0]);
320  }
321  const int endIndex = (int)tmp.size() - 1;
322  SUMOReal div = 0; // 6 * area including sign
323  SUMOReal x = 0;
324  SUMOReal y = 0;
325  if (tmp.area() != 0) { // numerical instability ?
326  // http://en.wikipedia.org/wiki/Polygon
327  for (int i = 0; i < endIndex; i++) {
328  const SUMOReal z = tmp[i].x() * tmp[i + 1].y() - tmp[i + 1].x() * tmp[i].y();
329  div += z; // area formula
330  x += (tmp[i].x() + tmp[i + 1].x()) * z;
331  y += (tmp[i].y() + tmp[i + 1].y()) * z;
332  }
333  div *= 3; // 6 / 2, the 2 comes from the area formula
334  return Position(x / div, y / div);
335  } else {
336  // compute by decomposing into line segments
337  // http://en.wikipedia.org/wiki/Centroid#By_geometric_decomposition
338  SUMOReal lengthSum = 0;
339  for (int i = 0; i < endIndex; i++) {
340  SUMOReal length = tmp[i].distanceTo(tmp[i + 1]);
341  x += (tmp[i].x() + tmp[i + 1].x()) * length / 2;
342  y += (tmp[i].y() + tmp[i + 1].y()) * length / 2;
343  lengthSum += length;
344  }
345  return Position(x / lengthSum, y / lengthSum);
346  }
347 }
348 
349 
350 void
352  Position centroid = getCentroid();
353  for (int i = 0; i < static_cast<int>(size()); i++) {
354  (*this)[i] = centroid + (((*this)[i] - centroid) * factor);
355  }
356 }
357 
358 
359 Position
361  if (size() == 1) {
362  return (*this)[0];
363  }
364  return positionAtOffset(SUMOReal((length() / 2.)));
365 }
366 
367 
368 SUMOReal
370  SUMOReal len = 0;
371  for (const_iterator i = begin(); i != end() - 1; i++) {
372  len += (*i).distanceTo(*(i + 1));
373  }
374  return len;
375 }
376 
377 
378 SUMOReal
380  SUMOReal area = 0;
381  PositionVector tmp = *this;
382  if (!isClosed()) { // make sure its closed
383  tmp.push_back(tmp[0]);
384  }
385  const int endIndex = (int)tmp.size() - 1;
386  // http://en.wikipedia.org/wiki/Polygon
387  for (int i = 0; i < endIndex; i++) {
388  area += tmp[i].x() * tmp[i + 1].y() - tmp[i + 1].x() * tmp[i].y();
389  }
390  if (area < 0) { // we whether we had cw or ccw order
391  area *= -1;
392  }
393  return area / 2;
394 }
395 
396 
397 bool
399  for (const_iterator i = begin(); i != end() - 1; i++) {
400  if (poly.around(*i, offset)) {
401  return true;
402  }
403  }
404  return false;
405 }
406 
407 
408 bool
409 PositionVector::crosses(const Position& p1, const Position& p2) const {
410  return intersects(p1, p2);
411 }
412 
413 
414 std::pair<PositionVector, PositionVector>
416  if (size() < 2) {
417  throw InvalidArgument("Vector to short for splitting");
418  }
419  if (where <= POSITION_EPS || where >= length() - POSITION_EPS) {
420  WRITE_WARNING("Splitting vector close to end (pos: " + toString(where) + ", length: " + toString(length()) + ")");
421  }
422  PositionVector first, second;
423  first.push_back((*this)[0]);
424  SUMOReal seen = 0;
425  const_iterator it = begin() + 1;
426  SUMOReal next = first.back().distanceTo(*it);
427  // see how many points we can add to first
428  while (where >= seen + next + POSITION_EPS) {
429  seen += next;
430  first.push_back(*it);
431  it++;
432  next = first.back().distanceTo(*it);
433  }
434  if (fabs(where - (seen + next)) > POSITION_EPS || it == end() - 1) {
435  // we need to insert a new point because 'where' is not close to an
436  // existing point or it is to close to the endpoint
437  Line tmpL(first.back(), *it);
438  Position p = tmpL.getPositionAtDistance(where - seen);
439  first.push_back(p);
440  second.push_back(p);
441  } else {
442  first.push_back(*it);
443  }
444  // add the remaining points to second
445  for (; it != end(); it++) {
446  second.push_back(*it);
447  }
448  assert(first.size() >= 2);
449  assert(second.size() >= 2);
450  assert(first.back() == second.front());
451  assert(fabs(first.length() + second.length() - length()) < 2 * POSITION_EPS);
452  return std::pair<PositionVector, PositionVector>(first, second);
453 }
454 
455 
456 std::ostream&
457 operator<<(std::ostream& os, const PositionVector& geom) {
458  for (PositionVector::const_iterator i = geom.begin(); i != geom.end(); i++) {
459  if (i != geom.begin()) {
460  os << " ";
461  }
462  os << (*i);
463  }
464  return os;
465 }
466 
467 
468 void
470  std::sort(begin(), end(), as_poly_cw_sorter());
471 }
472 
473 
474 void
476  for (int i = 0; i < static_cast<int>(size()); i++) {
477  (*this)[i].add(xoff, yoff, zoff);
478  }
479 }
480 
481 
482 void
484  for (int i = 0; i < static_cast<int>(size()); i++) {
485  (*this)[i].reshiftRotate(xoff, yoff, rot);
486  }
487 }
488 
489 
490 int
492  const Position& p2) const {
493  return atan2(p1.x(), p1.y()) < atan2(p2.x(), p2.y());
494 }
495 
496 
497 
498 void
500  std::sort(begin(), end(), increasing_x_y_sorter());
501 }
502 
503 
504 
506 
507 
508 
509 int
511  const Position& p2) const {
512  if (p1.x() != p2.x()) {
513  return p1.x() < p2.x();
514  }
515  return p1.y() < p2.y();
516 }
517 
518 
519 
520 SUMOReal
522  const Position& P2) const {
523  return (P1.x() - P0.x()) * (P2.y() - P0.y()) - (P2.x() - P0.x()) * (P1.y() - P0.y());
524 }
525 
526 
529  PositionVector ret = *this;
530  ret.sortAsPolyCWByAngle();
531  return simpleHull_2D(ret);
532 }
533 
534 
537  PositionVector ret;
538  for (const_iterator i = begin(); i != end() - 1; i++) {
539  if (GeomHelper::intersects(*i, *(i + 1), line.p1(), line.p2())) {
541  *i, *(i + 1), line.p1(), line.p2()));
542  }
543  }
544  return ret;
545 }
546 
547 
548 int
550  if (back().distanceTo(v[0]) < 2) { // !!! heuristic
551  copy(v.begin() + 1, v.end(), back_inserter(*this));
552  return 1;
553  }
554  //
555  Line l1((*this)[static_cast<int>(size()) - 2], back());
556  l1.extrapolateBy(100);
557  Line l2(v[0], v[1]);
558  l2.extrapolateBy(100);
559  if (l1.intersects(l2) && l1.intersectsAtLength2D(l2) < l1.length2D() - 100) { // !!! heuristic
560  Position p = l1.intersectsAt(l2);
561  (*this)[static_cast<int>(size()) - 1] = p;
562  copy(v.begin() + 1, v.end(), back_inserter(*this));
563  return 2;
564  } else {
565  copy(v.begin(), v.end(), back_inserter(*this));
566  return 3;
567  }
568 }
569 
570 
571 void
573  if (back().distanceTo(v[0]) < 2) {
574  copy(v.begin() + 1, v.end(), back_inserter(*this));
575  } else {
576  copy(v.begin(), v.end(), back_inserter(*this));
577  }
578 }
579 
580 
582 PositionVector::getSubpart(SUMOReal beginOffset, SUMOReal endOffset) const {
583  PositionVector ret;
584  Position begPos = front();
585  if (beginOffset > POSITION_EPS) {
586  begPos = positionAtOffset(beginOffset);
587  }
588  Position endPos = back();
589  if (endOffset < length() - POSITION_EPS) {
590  endPos = positionAtOffset(endOffset);
591  }
592  ret.push_back(begPos);
593 
594  SUMOReal seen = 0;
595  const_iterator i = begin();
596  // skip previous segments
597  while ((i + 1) != end()
598  &&
599  seen + (*i).distanceTo(*(i + 1)) < beginOffset) {
600  seen += (*i).distanceTo(*(i + 1));
601  i++;
602  }
603  // append segments in between
604  while ((i + 1) != end()
605  &&
606  seen + (*i).distanceTo(*(i + 1)) < endOffset) {
607 
608  ret.push_back_noDoublePos(*(i + 1));
609  /*
610  if(ret.at(-1)!=*(i+1)) {
611  ret.push_back(*(i+1));
612  }
613  */
614  seen += (*i).distanceTo(*(i + 1));
615  i++;
616  }
617  // append end
618  ret.push_back_noDoublePos(endPos);
619  return ret;
620 }
621 
622 
624 PositionVector::getSubpart2D(SUMOReal beginOffset, SUMOReal endOffset) const {
625  PositionVector ret;
626  Position begPos = front();
627  if (beginOffset > POSITION_EPS) {
628  begPos = positionAtOffset2D(beginOffset);
629  }
630  Position endPos = back();
631  if (endOffset < length() - POSITION_EPS) {
632  endPos = positionAtOffset2D(endOffset);
633  }
634  ret.push_back(begPos);
635 
636  SUMOReal seen = 0;
637  const_iterator i = begin();
638  // skip previous segments
639  while ((i + 1) != end()
640  &&
641  seen + (*i).distanceTo2D(*(i + 1)) < beginOffset) {
642  seen += (*i).distanceTo2D(*(i + 1));
643  i++;
644  }
645  // append segments in between
646  while ((i + 1) != end()
647  &&
648  seen + (*i).distanceTo2D(*(i + 1)) < endOffset) {
649 
650  ret.push_back_noDoublePos(*(i + 1));
651  /*
652  if(ret.at(-1)!=*(i+1)) {
653  ret.push_back(*(i+1));
654  }
655  */
656  seen += (*i).distanceTo2D(*(i + 1));
657  i++;
658  }
659  // append end
660  ret.push_back_noDoublePos(endPos);
661  return ret;
662 }
663 
664 
665 void
667  // find minimum distance (from the begin)
668  size_t pos = 0;
669  SUMOReal dist = 1000000;
670  size_t currPos = 0;
672  GeomHelper::extrapolate_first(*(begin()), *(begin() + 1), 100),
673  *(begin() + 1));
674 // assert(currDist>=0);
675  if (currDist >= 0 && currDist < dist) {
676  dist = currDist;
677  pos = currPos;
678  }
679 
680  for (iterator i = begin(); i != end() - 1; i++, currPos++) {
681  SUMOReal currDist = GeomHelper::distancePointLine(p, *i, *(i + 1));
682  if (currDist >= 0 && currDist < dist) {
683  dist = currDist;
684  pos = currPos;
685  }
686  }
687  // remove leading items
688  for (size_t j = 0; j < pos; j++) {
689  erase(begin());
690  }
691  // replace first item by the new position
693  (*this)[0], (*this)[1], p);
694  if (lpos == -1) {
695  return;
696  }
697  Position np = positionAtOffset(lpos);
698  if (np != *(begin())) {
699  erase(begin());
700  if (np != *(begin())) {
701  insert(begin(), p);
702  assert(size() > 1);
703  assert(*(begin()) != *(end() - 1));
704  }
705  }
706 }
707 
708 
709 void
711  // find minimum distance (from the end)
712  size_t pos = 0;
713  SUMOReal dist = 1000000;
714  size_t currPos = 0;
716  *(end() - 1),
717  GeomHelper::extrapolate_second(*(end() - 2), *(end() - 1), 100));
718 // assert(currDist>=0);
719  if (currDist >= 0 && currDist < dist) {
720  dist = currDist;
721  pos = currPos;
722  }
723 
724  for (reverse_iterator i = rbegin(); i != rend() - 1; i++, currPos++) {
725  SUMOReal currDist = GeomHelper::distancePointLine(p, *(i), *(i + 1));
726  if (currDist >= 0 && currDist < dist) {
727  dist = currDist;
728  pos = currPos;
729  }
730  }
731  // remove trailing items
732  for (size_t j = 0; j < pos; j++) {
733  erase(end() - 1);
734  }
735  // replace last item by the new position
736  SUMOReal lpos =
738  (*this)[static_cast<int>(size()) - 1], (*this)[static_cast<int>(size()) - 2], p);
739  if (lpos == -1) {
740  return;
741  }
743  length() - lpos);
744  if (np != *(end() - 1)) {
745  erase(end() - 1);
746  if (np != *(end() - 1)) {
747  push_back(np);
748  assert(size() > 1);
749  assert(*(begin()) != *(end() - 1));
750  }
751  }
752 }
753 
754 
755 SUMOReal
757  Line tmp(front(), back());
758  return tmp.atan2Angle();
759 }
760 
761 
762 void
764  if (i >= 0) {
765  erase(begin() + i);
766  } else {
767  erase(end() + i);
768  }
769 }
770 
771 
772 SUMOReal
773 PositionVector::nearest_offset_to_point2D(const Position& p, bool perpendicular) const {
775  SUMOReal nearestPos = -1;
776  SUMOReal seen = 0;
777  for (const_iterator i = begin(); i != end() - 1; i++) {
778  const SUMOReal pos =
779  GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, perpendicular);
780  const SUMOReal dist = pos < 0 ? minDist : p.distanceTo2D(Line(*i, *(i + 1)).getPositionAtDistance(pos));
781  if (dist < minDist) {
782  nearestPos = pos + seen;
783  minDist = dist;
784  }
785  if (perpendicular && i != begin()) {
786  // even if perpendicular is set we still need to check the distance to the inner points
787  const SUMOReal cornerDist = p.distanceTo2D(*i);
788  if (cornerDist < minDist) {
789  nearestPos = seen;
790  minDist = cornerDist;
791  }
792  }
793  seen += (*i).distanceTo2D(*(i + 1));
794  }
795  return nearestPos;
796 }
797 
798 
799 int
801  assert(size() > 0);
803  SUMOReal dist;
804  int closest = 0;
805  for (int i = 0; i < (int)size(); i++) {
806  dist = p.distanceTo((*this)[i]);
807  if (dist < minDist) {
808  closest = i;
809  minDist = dist;
810  }
811  }
812  return closest;
813 }
814 
815 
816 int
818  Position outIntersection = Position();
820  SUMOReal dist;
821  int insertionIndex = 1;
822  for (int i = 0; i < (int)size() - 1; i++) {
823  dist = GeomHelper::closestDistancePointLine(p, (*this)[i], (*this)[i + 1], outIntersection);
824  if (dist < minDist) {
825  insertionIndex = i + 1;
826  minDist = dist;
827  }
828  }
829  insertAt(insertionIndex, p);
830  return insertionIndex;
831 }
832 
833 
834 SUMOReal
836  if (size() == 1) {
837  return front().distanceTo(p);
838  }
839  Position outIntersection;
841  for (const_iterator i = begin(); i != end() - 1; i++) {
842  minDist = MIN2(minDist, GeomHelper::closestDistancePointLine(
843  p, *i, *(i + 1), outIntersection));
844  }
845  return minDist;
846 }
847 
848 
849 std::vector<SUMOReal>
851  std::vector<SUMOReal> ret;
852  for (const_iterator i = other.begin(); i != other.end() - 1; i++) {
853  std::vector<SUMOReal> atSegment = intersectsAtLengths2D(Line(*i, *(i + 1)));
854  copy(atSegment.begin(), atSegment.end(), back_inserter(ret));
855  }
856  return ret;
857 }
858 
859 
860 std::vector<SUMOReal>
862  std::vector<SUMOReal> ret;
863  SUMOReal pos = 0;
864  for (const_iterator i = begin(); i != end() - 1; i++) {
865  Line l((*i), *(i + 1));
866  if (GeomHelper::intersects(l.p1(), l.p2(), line.p1(), line.p2())) {
867  Position p =
868  GeomHelper::intersection_position2D(l.p1(), l.p2(), line.p1(), line.p2());
869  SUMOReal atLength = p.distanceTo2D(l.p1());
870  ret.push_back(atLength + pos);
871  }
872  pos += l.length2D();
873  }
874  return ret;
875 }
876 
877 
878 void
880  assert(size() > 1);
881  Position nb =
882  GeomHelper::extrapolate_first((*this)[0], (*this)[1], val);
883  Position ne =
885  (*this)[static_cast<int>(size()) - 2], (*this)[static_cast<int>(size()) - 1], val);
886  erase(begin());
887  push_front(nb);
888  erase(end() - 1);
889  push_back(ne);
890 }
891 
892 
895  PositionVector ret;
896  for (const_reverse_iterator i = rbegin(); i != rend(); i++) {
897  ret.push_back(*i);
898  }
899  return ret;
900 }
901 
902 
903 void
905  if (size() < 2) {
906  return;
907  }
908  PositionVector shape;
909  for (int i = 0; i < static_cast<int>(size()); i++) {
910  if (i == 0) {
911  Position from = (*this)[i];
912  Position to = (*this)[i + 1];
913  std::pair<SUMOReal, SUMOReal> offsets =
914  GeomHelper::getNormal90D_CW(from, to, amount);
915  shape.push_back(Position(from.x() - offsets.first,
916  from.y() - offsets.second, from.z()));
917  } else if (i == static_cast<int>(size()) - 1) {
918  Position from = (*this)[i - 1];
919  Position to = (*this)[i];
920  std::pair<SUMOReal, SUMOReal> offsets =
921  GeomHelper::getNormal90D_CW(from, to, amount);
922  shape.push_back(Position(to.x() - offsets.first,
923  to.y() - offsets.second, to.z()));
924  } else {
925  Position from = (*this)[i - 1];
926  Position me = (*this)[i];
927  Position to = (*this)[i + 1];
928  const double sinAngle = sin(GeomHelper::Angle2D(from.x() - me.x(), from.y() - me.y(),
929  me.x() - to.x(), me.y() - to.y()) / 2);
930  const double maxDev = 2 * (from.distanceTo2D(me) + me.distanceTo2D(to)) * sinAngle;
931  if (fabs(maxDev) < POSITION_EPS) {
932  // parallel case, just shift the middle point
933  std::pair<SUMOReal, SUMOReal> off =
934  GeomHelper::getNormal90D_CW(from, to, amount);
935  shape.push_back(Position(me.x() - off.first, me.y() - off.second, me.z()));
936  continue;
937  }
938  std::pair<SUMOReal, SUMOReal> offsets =
939  GeomHelper::getNormal90D_CW(from, me, amount);
940  std::pair<SUMOReal, SUMOReal> offsets2 =
941  GeomHelper::getNormal90D_CW(me, to, amount);
942  Line l1(
943  Position(from.x() - offsets.first, from.y() - offsets.second),
944  Position(me.x() - offsets.first, me.y() - offsets.second));
945  l1.extrapolateBy(100);
946  Line l2(
947  Position(me.x() - offsets2.first, me.y() - offsets2.second),
948  Position(to.x() - offsets2.first, to.y() - offsets2.second));
949  l2.extrapolateBy(100);
950  if (l1.intersects(l2)) {
951  shape.push_back(l1.intersectsAt(l2));
952  } else {
953  throw InvalidArgument("no line intersection");
954  }
955  }
956  }
957 
958  /*
959  ContType newCont;
960  std::pair<SUMOReal, SUMOReal> p;
961  Position newPos;
962  // first point
963  newPos = (*(begin()));
964  p = GeomHelper::getNormal90D_CW(*(begin()), *(begin()+1), amount);
965  newPos.add(p.first, p.second);
966  newCont.push_back(newPos);
967  // middle points
968  for(const_iterator i=begin()+1; i!=end()-1; i++) {
969  std::pair<SUMOReal, SUMOReal> oldp = p;
970  newPos = *i;
971  newPos.add(p.first, p.second);
972  newCont.push_back(newPos);
973  p = GeomHelper::getNormal90D_CW(*i, *(i+1), amount);
974  // Position newPos(*i);
975  // newPos.add((p.first+oldp.first)/2.0, (p.second+oldp.second)/2.0);
976  // newCont.push_back(newPos);
977  }
978  // last point
979  newPos = (*(end()-1));
980  newPos.add(p.first, p.second);
981  newCont.push_back(newPos);
982  myCont = newCont;
983  */
984  *this = shape;
985 }
986 
987 
988 Line
989 PositionVector::lineAt(int pos) const {
990  assert((int)size() > pos + 1);
991  return Line((*this)[pos], (*this)[pos + 1]);
992 }
993 
994 
995 Line
997  return lineAt(0);
998 }
999 
1000 
1001 Line
1003  return lineAt((int)size() - 2);
1004 }
1005 
1006 
1007 void
1009  if ((*this)[0] == back()) {
1010  return;
1011  }
1012  push_back((*this)[0]);
1013 }
1014 
1015 
1016 std::vector<SUMOReal>
1018  std::vector<SUMOReal> ret;
1019  const_iterator i;
1020  for (i = begin(); i != end(); i++) {
1021  ret.push_back(s.distance(*i));
1022  }
1023  for (i = s.begin(); i != s.end(); i++) {
1024  ret.push_back(distance(*i));
1025  }
1026  return ret;
1027 }
1028 
1029 
1030 void
1031 PositionVector::insertAt(int index, const Position& p) {
1032  if (index >= 0) {
1033  insert(begin() + index, p);
1034  } else {
1035  insert(end() + index, p);
1036  }
1037 }
1038 
1039 
1040 void
1041 PositionVector::replaceAt(int index, const Position& p) {
1042  assert(index < static_cast<int>(size()));
1043  assert(index + static_cast<int>(size()) >= 0);
1044  if (index >= 0) {
1045  (*this)[index] = p;
1046  } else {
1047  (*this)[index + static_cast<int>(size())] = p;
1048  }
1049 }
1050 
1051 
1052 void
1054  if (size() == 0 || !p.almostSame(back())) {
1055  push_back(p);
1056  }
1057 }
1058 
1059 
1060 void
1062  if (size() == 0 || !p.almostSame(front())) {
1063  insert(begin(), p);
1064  }
1065 }
1066 
1067 
1068 bool
1070  return size() >= 2 && (*this)[0] == back();
1071 }
1072 
1073 
1074 void
1075 PositionVector::removeDoublePoints(SUMOReal minDist, bool assertLength) {
1076  if (size() > 1) {
1077  iterator last = begin();
1078  for (iterator i = begin() + 1; i != end() && (!assertLength || size() > 2);) {
1079  if (last->almostSame(*i, minDist)) {
1080  i = erase(i);
1081  } else {
1082  last = i;
1083  ++i;
1084  }
1085  }
1086  }
1087 }
1088 
1089 
1090 void
1092  if (size() > 2) {
1093  Position& last = front();
1094  for (iterator i = begin() + 1; i != end() - 1;) {
1095  if (GeomHelper::distancePointLine(*i, last, *(i + 1)) < 0.001) {
1096  i = erase(i);
1097  } else {
1098  last = *i;
1099  ++i;
1100  }
1101  }
1102  }
1103 }
1104 
1105 
1106 bool
1108  if (size() == v2.size()) {
1109  for (int i = 0; i < (int)size(); i++) {
1110  if ((*this)[i] != v2[i]) {
1111  return false;
1112  }
1113  }
1114  return true;
1115  } else {
1116  return false;
1117  }
1118 }
1119 
1120 
1121 
1122 /****************************************************************************/
1123 
SUMOReal length2D() const
Definition: Line.cpp:177
SUMOReal atan2DegreeAngle() const
Definition: Line.cpp:143
static std::pair< SUMOReal, SUMOReal > getNormal90D_CW(const Position &beg, const Position &end, SUMOReal length, SUMOReal wanted_offset)
Definition: GeomHelper.cpp:310
const Position & p2() const
Definition: Line.cpp:86
static SUMOReal Angle2D(SUMOReal x1, SUMOReal y1, SUMOReal x2, SUMOReal y2)
Definition: GeomHelper.cpp:161
void pruneFromBeginAt(const Position &p)
static Position intersection_position2D(const Position &p11, const Position &p12, const Position &p21, const Position &p22)
returns the intersection point of the (infinite) lines p11,p12 and p21,p22. If the given lines are pa...
Definition: GeomHelper.cpp:140
SUMOReal nearest_offset_to_point2D(const Position &p, bool perpendicular=true) const
PositionVector getSubpart2D(SUMOReal beginOffset, SUMOReal endOffset) const
Position positionAtOffset(SUMOReal pos) const
Returns the position at the given length.
void sortAsPolyCWByAngle()
void replaceAt(int index, const Position &by)
SUMOReal intersectsAtLength2D(const Line &v)
returns distance between myP1 and intersection or -1 if line segments do not intersect ...
Definition: Line.cpp:220
void insertAt(int index, const Position &p)
#define M_PI
Definition: angles.h:37
std::vector< SUMOReal > distances(const PositionVector &s) const
SUMOReal atan2DegreeSlope() const
Definition: Line.cpp:159
Line getEndLine() const
Position getCentroid() const
Returns the centroid (closes the polygon if unclosed)
bool intersects(const Position &p1, const Position &p2) const
bool partialWithin(const AbstractPoly &poly, SUMOReal offset=0) const
Returns the information whether this polygon lies partially within the given polygon.
static Position extrapolate_second(const Position &p1, const Position &p2, SUMOReal length)
Definition: GeomHelper.cpp:190
SUMOReal distanceTo(const Position &p2) const
returns the euclidean distance in 3 dimension
Definition: Position.h:208
void eraseAt(int i)
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:202
static Position extrapolate_first(const Position &p1, const Position &p2, SUMOReal length)
Definition: GeomHelper.cpp:182
SUMOReal beginEndAngle() const
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
A class that stores a 2D geometrical boundary.
Definition: Boundary.h:48
#define WRITE_WARNING(msg)
Definition: MsgHandler.h:196
PositionVector reverse() const
SUMOReal slopeDegreeAtOffset(SUMOReal pos) const
Returns the slope at the given length.
PositionVector convexHull() const
~PositionVector()
Destructor.
void scaleSize(SUMOReal factor)
enlarges/shrinks the polygon based at the centroid
static SUMOReal nearest_offset_on_line_to_point2D(const Position &l1, const Position &l2, const Position &p, bool perpendicular=true)
Definition: GeomHelper.cpp:198
Line lineAt(int pos) const
static bool intersects(const Position &p11, const Position &p12, const Position &p21, const Position &p22)
return whether the line segments defined by Line p11,p12 and Line p21,p22 intersect ...
Definition: GeomHelper.cpp:132
void push_front_noDoublePos(const Position &p)
const Position & p1() const
Definition: Line.cpp:80
#define max(a, b)
Definition: polyfonts.c:61
void reshiftRotate(SUMOReal xoff, SUMOReal yoff, SUMOReal rot)
A point in 2D or 3D with translation and scaling methods.
Definition: Position.h:46
Position pop_front()
Removes and returns the position at the fron of the list.
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
void push_front(const Position &p)
Puts the given position at the front of the list.
static SUMOReal distancePointLine(const Position &point, const Position &lineStart, const Position &lineEnd)
Definition: GeomHelper.cpp:225
SUMOReal z() const
Returns the z-position.
Definition: Position.h:73
SUMOReal distance(const Position &p) const
Definition: Line.h:51
T MIN2(T a, T b)
Definition: StdDefs.h:57
#define POSITION_EPS
Definition: config.h:192
int insertAtClosest(const Position &p)
std::string toString(const T &t, std::streamsize accuracy=OUTPUT_ACCURACY)
Definition: ToString.h:51
Position intersectsAtPoint(const Position &p1, const Position &p2) const
SUMOReal atan2Angle() const
Definition: Line.cpp:137
bool intersects(const Line &l) const
Definition: Line.cpp:171
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
void extrapolate(SUMOReal val)
PositionVector()
Constructor.
void extrapolateBy(SUMOReal length)
Definition: Line.cpp:60
SUMOReal length() const
Returns the length.
SUMOReal rotationDegreeAtOffset(SUMOReal pos) const
Returns the rotation at the given length.
void push_back(const PositionVector &p)
Appends all positions from the given vector.
void add(SUMOReal x, SUMOReal y)
Makes the boundary include the given coordinate.
Definition: Boundary.cpp:76
PositionVector simpleHull_2D(const PositionVector &V)
void removeDoublePoints(SUMOReal minDist=POSITION_EPS, bool assertLength=false)
Removes positions if too near.
PositionVector intersectionPoints2D(const Line &line) const
int appendWithCrossingPoint(const PositionVector &v)
Position positionAtOffset2D(SUMOReal pos) const
Returns the position at the given length.
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...
Line getBegLine() const
void pruneFromEndAt(const Position &p)
Position getLineCenter() const
SUMOReal distanceTo2D(const Position &p2) const
returns the euclidean distance in the x-y-plane
Definition: Position.h:219
void move2side(SUMOReal amount)
#define SUMOReal
Definition: config.h:221
void push_back_noDoublePos(const Position &p)
static SUMOReal closestDistancePointLine(const Position &point, const Position &lineStart, const Position &lineEnd, Position &outIntersection)
Definition: GeomHelper.cpp:249
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::ostream & operator<<(std::ostream &os, const MTRand &mtrand)
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.
void append(const PositionVector &v)
bool crosses(const Position &p1, const Position &p2) const
Position intersectsAt(const Line &l) const
Definition: Line.cpp:165
PositionVector getSubpart(SUMOReal beginOffset, SUMOReal endOffset) const
SUMOReal isLeft(const Position &P0, const Position &P1, const Position &P2) const