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 SUMOReal
379  SUMOReal len = 0;
380  for (const_iterator i = begin(); i != end() - 1; i++) {
381  len += (*i).distanceTo2D(*(i + 1));
382  }
383  return len;
384 }
385 
386 
387 SUMOReal
389  SUMOReal area = 0;
390  PositionVector tmp = *this;
391  if (!isClosed()) { // make sure its closed
392  tmp.push_back(tmp[0]);
393  }
394  const int endIndex = (int)tmp.size() - 1;
395  // http://en.wikipedia.org/wiki/Polygon
396  for (int i = 0; i < endIndex; i++) {
397  area += tmp[i].x() * tmp[i + 1].y() - tmp[i + 1].x() * tmp[i].y();
398  }
399  if (area < 0) { // we whether we had cw or ccw order
400  area *= -1;
401  }
402  return area / 2;
403 }
404 
405 
406 bool
408  for (const_iterator i = begin(); i != end() - 1; i++) {
409  if (poly.around(*i, offset)) {
410  return true;
411  }
412  }
413  return false;
414 }
415 
416 
417 bool
418 PositionVector::crosses(const Position& p1, const Position& p2) const {
419  return intersects(p1, p2);
420 }
421 
422 
423 std::pair<PositionVector, PositionVector>
425  if (size() < 2) {
426  throw InvalidArgument("Vector to short for splitting");
427  }
428  if (where <= POSITION_EPS || where >= length() - POSITION_EPS) {
429  WRITE_WARNING("Splitting vector close to end (pos: " + toString(where) + ", length: " + toString(length()) + ")");
430  }
431  PositionVector first, second;
432  first.push_back((*this)[0]);
433  SUMOReal seen = 0;
434  const_iterator it = begin() + 1;
435  SUMOReal next = first.back().distanceTo(*it);
436  // see how many points we can add to first
437  while (where >= seen + next + POSITION_EPS) {
438  seen += next;
439  first.push_back(*it);
440  it++;
441  next = first.back().distanceTo(*it);
442  }
443  if (fabs(where - (seen + next)) > POSITION_EPS || it == end() - 1) {
444  // we need to insert a new point because 'where' is not close to an
445  // existing point or it is to close to the endpoint
446  Line tmpL(first.back(), *it);
447  Position p = tmpL.getPositionAtDistance(where - seen);
448  first.push_back(p);
449  second.push_back(p);
450  } else {
451  first.push_back(*it);
452  }
453  // add the remaining points to second
454  for (; it != end(); it++) {
455  second.push_back(*it);
456  }
457  assert(first.size() >= 2);
458  assert(second.size() >= 2);
459  assert(first.back() == second.front());
460  assert(fabs(first.length() + second.length() - length()) < 2 * POSITION_EPS);
461  return std::pair<PositionVector, PositionVector>(first, second);
462 }
463 
464 
465 std::ostream&
466 operator<<(std::ostream& os, const PositionVector& geom) {
467  for (PositionVector::const_iterator i = geom.begin(); i != geom.end(); i++) {
468  if (i != geom.begin()) {
469  os << " ";
470  }
471  os << (*i);
472  }
473  return os;
474 }
475 
476 
477 void
479  std::sort(begin(), end(), as_poly_cw_sorter());
480 }
481 
482 
483 void
485  for (int i = 0; i < static_cast<int>(size()); i++) {
486  (*this)[i].add(xoff, yoff, zoff);
487  }
488 }
489 
490 
491 void
493  for (int i = 0; i < static_cast<int>(size()); i++) {
494  (*this)[i].reshiftRotate(xoff, yoff, rot);
495  }
496 }
497 
498 
499 int
501  const Position& p2) const {
502  return atan2(p1.x(), p1.y()) < atan2(p2.x(), p2.y());
503 }
504 
505 
506 
507 void
509  std::sort(begin(), end(), increasing_x_y_sorter());
510 }
511 
512 
513 
515 
516 
517 
518 int
520  const Position& p2) const {
521  if (p1.x() != p2.x()) {
522  return p1.x() < p2.x();
523  }
524  return p1.y() < p2.y();
525 }
526 
527 
528 
529 SUMOReal
531  const Position& P2) const {
532  return (P1.x() - P0.x()) * (P2.y() - P0.y()) - (P2.x() - P0.x()) * (P1.y() - P0.y());
533 }
534 
535 
538  PositionVector ret = *this;
539  ret.sortAsPolyCWByAngle();
540  return simpleHull_2D(ret);
541 }
542 
543 
546  PositionVector ret;
547  for (const_iterator i = begin(); i != end() - 1; i++) {
548  if (GeomHelper::intersects(*i, *(i + 1), line.p1(), line.p2())) {
550  *i, *(i + 1), line.p1(), line.p2()));
551  }
552  }
553  return ret;
554 }
555 
556 
557 int
559  if (back().distanceTo(v[0]) < 2) { // !!! heuristic
560  copy(v.begin() + 1, v.end(), back_inserter(*this));
561  return 1;
562  }
563  //
564  Line l1((*this)[static_cast<int>(size()) - 2], back());
565  l1.extrapolateBy(100);
566  Line l2(v[0], v[1]);
567  l2.extrapolateBy(100);
568  if (l1.intersects(l2) && l1.intersectsAtLength2D(l2) < l1.length2D() - 100) { // !!! heuristic
569  Position p = l1.intersectsAt(l2);
570  (*this)[static_cast<int>(size()) - 1] = p;
571  copy(v.begin() + 1, v.end(), back_inserter(*this));
572  return 2;
573  } else {
574  copy(v.begin(), v.end(), back_inserter(*this));
575  return 3;
576  }
577 }
578 
579 
580 void
582  if (back().distanceTo(v[0]) < 2) {
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  /*
619  if(ret.at(-1)!=*(i+1)) {
620  ret.push_back(*(i+1));
621  }
622  */
623  seen += (*i).distanceTo(*(i + 1));
624  i++;
625  }
626  // append end
627  ret.push_back_noDoublePos(endPos);
628  return ret;
629 }
630 
631 
633 PositionVector::getSubpart2D(SUMOReal beginOffset, SUMOReal endOffset) const {
634  PositionVector ret;
635  Position begPos = front();
636  if (beginOffset > POSITION_EPS) {
637  begPos = positionAtOffset2D(beginOffset);
638  }
639  Position endPos = back();
640  if (endOffset < length() - POSITION_EPS) {
641  endPos = positionAtOffset2D(endOffset);
642  }
643  ret.push_back(begPos);
644 
645  SUMOReal seen = 0;
646  const_iterator i = begin();
647  // skip previous segments
648  while ((i + 1) != end()
649  &&
650  seen + (*i).distanceTo2D(*(i + 1)) < beginOffset) {
651  seen += (*i).distanceTo2D(*(i + 1));
652  i++;
653  }
654  // append segments in between
655  while ((i + 1) != end()
656  &&
657  seen + (*i).distanceTo2D(*(i + 1)) < endOffset) {
658 
659  ret.push_back_noDoublePos(*(i + 1));
660  /*
661  if(ret.at(-1)!=*(i+1)) {
662  ret.push_back(*(i+1));
663  }
664  */
665  seen += (*i).distanceTo2D(*(i + 1));
666  i++;
667  }
668  // append end
669  ret.push_back_noDoublePos(endPos);
670  return ret;
671 }
672 
673 
674 void
676  // find minimum distance (from the begin)
677  size_t pos = 0;
678  SUMOReal dist = 1000000;
679  size_t currPos = 0;
681  GeomHelper::extrapolate_first(*(begin()), *(begin() + 1), 100),
682  *(begin() + 1));
683 // assert(currDist>=0);
684  if (currDist >= 0 && currDist < dist) {
685  dist = currDist;
686  pos = currPos;
687  }
688 
689  for (iterator i = begin(); i != end() - 1; i++, currPos++) {
690  SUMOReal currDist = GeomHelper::distancePointLine(p, *i, *(i + 1));
691  if (currDist >= 0 && currDist < dist) {
692  dist = currDist;
693  pos = currPos;
694  }
695  }
696  // remove leading items
697  for (size_t j = 0; j < pos; j++) {
698  erase(begin());
699  }
700  // replace first item by the new position
702  (*this)[0], (*this)[1], p);
703  if (lpos == -1) {
704  return;
705  }
706  Position np = positionAtOffset(lpos);
707  if (np != *(begin())) {
708  erase(begin());
709  if (np != *(begin())) {
710  insert(begin(), p);
711  assert(size() > 1);
712  assert(*(begin()) != *(end() - 1));
713  }
714  }
715 }
716 
717 
718 void
720  // find minimum distance (from the end)
721  size_t pos = 0;
722  SUMOReal dist = 1000000;
723  size_t currPos = 0;
725  *(end() - 1),
726  GeomHelper::extrapolate_second(*(end() - 2), *(end() - 1), 100));
727 // assert(currDist>=0);
728  if (currDist >= 0 && currDist < dist) {
729  dist = currDist;
730  pos = currPos;
731  }
732 
733  for (reverse_iterator i = rbegin(); i != rend() - 1; i++, currPos++) {
734  SUMOReal currDist = GeomHelper::distancePointLine(p, *(i), *(i + 1));
735  if (currDist >= 0 && currDist < dist) {
736  dist = currDist;
737  pos = currPos;
738  }
739  }
740  // remove trailing items
741  for (size_t j = 0; j < pos; j++) {
742  erase(end() - 1);
743  }
744  // replace last item by the new position
745  SUMOReal lpos =
747  (*this)[static_cast<int>(size()) - 1], (*this)[static_cast<int>(size()) - 2], p);
748  if (lpos == -1) {
749  return;
750  }
752  length() - lpos);
753  if (np != *(end() - 1)) {
754  erase(end() - 1);
755  if (np != *(end() - 1)) {
756  push_back(np);
757  assert(size() > 1);
758  assert(*(begin()) != *(end() - 1));
759  }
760  }
761 }
762 
763 
764 SUMOReal
766  Line tmp(front(), back());
767  return tmp.atan2Angle();
768 }
769 
770 
771 void
773  if (i >= 0) {
774  erase(begin() + i);
775  } else {
776  erase(end() + i);
777  }
778 }
779 
780 
781 SUMOReal
782 PositionVector::nearest_offset_to_point2D(const Position& p, bool perpendicular) const {
784  SUMOReal nearestPos = -1;
785  SUMOReal seen = 0;
786  for (const_iterator i = begin(); i != end() - 1; i++) {
787  const SUMOReal pos =
788  GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, perpendicular);
789  const SUMOReal dist = pos < 0 ? minDist : p.distanceTo2D(Line(*i, *(i + 1)).getPositionAtDistance(pos));
790  if (dist < minDist) {
791  nearestPos = pos + seen;
792  minDist = dist;
793  }
794  if (perpendicular && i != begin()) {
795  // even if perpendicular is set we still need to check the distance to the inner points
796  const SUMOReal cornerDist = p.distanceTo2D(*i);
797  if (cornerDist < minDist) {
798  nearestPos = seen;
799  minDist = cornerDist;
800  }
801  }
802  seen += (*i).distanceTo2D(*(i + 1));
803  }
804  return nearestPos;
805 }
806 
807 
808 int
810  assert(size() > 0);
812  SUMOReal dist;
813  int closest = 0;
814  for (int i = 0; i < (int)size(); i++) {
815  dist = p.distanceTo((*this)[i]);
816  if (dist < minDist) {
817  closest = i;
818  minDist = dist;
819  }
820  }
821  return closest;
822 }
823 
824 
825 int
827  Position outIntersection = Position();
829  SUMOReal dist;
830  int insertionIndex = 1;
831  for (int i = 0; i < (int)size() - 1; i++) {
832  dist = GeomHelper::closestDistancePointLine(p, (*this)[i], (*this)[i + 1], outIntersection);
833  if (dist < minDist) {
834  insertionIndex = i + 1;
835  minDist = dist;
836  }
837  }
838  insertAt(insertionIndex, p);
839  return insertionIndex;
840 }
841 
842 
843 SUMOReal
845  if (size() == 1) {
846  return front().distanceTo(p);
847  }
848  Position outIntersection;
850  for (const_iterator i = begin(); i != end() - 1; i++) {
851  minDist = MIN2(minDist, GeomHelper::closestDistancePointLine(
852  p, *i, *(i + 1), outIntersection));
853  }
854  return minDist;
855 }
856 
857 
858 std::vector<SUMOReal>
860  std::vector<SUMOReal> ret;
861  for (const_iterator i = other.begin(); i != other.end() - 1; i++) {
862  std::vector<SUMOReal> atSegment = intersectsAtLengths2D(Line(*i, *(i + 1)));
863  copy(atSegment.begin(), atSegment.end(), back_inserter(ret));
864  }
865  return ret;
866 }
867 
868 
869 std::vector<SUMOReal>
871  std::vector<SUMOReal> ret;
872  SUMOReal pos = 0;
873  for (const_iterator i = begin(); i != end() - 1; i++) {
874  Line l((*i), *(i + 1));
875  if (GeomHelper::intersects(l.p1(), l.p2(), line.p1(), line.p2())) {
876  Position p =
877  GeomHelper::intersection_position2D(l.p1(), l.p2(), line.p1(), line.p2());
878  SUMOReal atLength = p.distanceTo2D(l.p1());
879  ret.push_back(atLength + pos);
880  }
881  pos += l.length2D();
882  }
883  return ret;
884 }
885 
886 
887 void
889  assert(size() > 1);
890  Position nb =
891  GeomHelper::extrapolate_first((*this)[0], (*this)[1], val);
892  Position ne =
894  (*this)[static_cast<int>(size()) - 2], (*this)[static_cast<int>(size()) - 1], val);
895  erase(begin());
896  push_front(nb);
897  erase(end() - 1);
898  push_back(ne);
899 }
900 
901 
904  PositionVector ret;
905  for (const_reverse_iterator i = rbegin(); i != rend(); i++) {
906  ret.push_back(*i);
907  }
908  return ret;
909 }
910 
911 
912 void
914  if (size() < 2) {
915  return;
916  }
917  PositionVector shape;
918  for (int i = 0; i < static_cast<int>(size()); i++) {
919  if (i == 0) {
920  Position from = (*this)[i];
921  Position to = (*this)[i + 1];
922  std::pair<SUMOReal, SUMOReal> offsets =
923  GeomHelper::getNormal90D_CW(from, to, amount);
924  shape.push_back(Position(from.x() - offsets.first,
925  from.y() - offsets.second, from.z()));
926  } else if (i == static_cast<int>(size()) - 1) {
927  Position from = (*this)[i - 1];
928  Position to = (*this)[i];
929  std::pair<SUMOReal, SUMOReal> offsets =
930  GeomHelper::getNormal90D_CW(from, to, amount);
931  shape.push_back(Position(to.x() - offsets.first,
932  to.y() - offsets.second, to.z()));
933  } else {
934  Position from = (*this)[i - 1];
935  Position me = (*this)[i];
936  Position to = (*this)[i + 1];
937  const double sinAngle = sin(GeomHelper::Angle2D(from.x() - me.x(), from.y() - me.y(),
938  me.x() - to.x(), me.y() - to.y()) / 2);
939  const double maxDev = 2 * (from.distanceTo2D(me) + me.distanceTo2D(to)) * sinAngle;
940  if (fabs(maxDev) < POSITION_EPS) {
941  // parallel case, just shift the middle point
942  std::pair<SUMOReal, SUMOReal> off =
943  GeomHelper::getNormal90D_CW(from, to, amount);
944  shape.push_back(Position(me.x() - off.first, me.y() - off.second, me.z()));
945  continue;
946  }
947  std::pair<SUMOReal, SUMOReal> offsets =
948  GeomHelper::getNormal90D_CW(from, me, amount);
949  std::pair<SUMOReal, SUMOReal> offsets2 =
950  GeomHelper::getNormal90D_CW(me, to, amount);
951  Line l1(
952  Position(from.x() - offsets.first, from.y() - offsets.second),
953  Position(me.x() - offsets.first, me.y() - offsets.second));
954  l1.extrapolateBy(100);
955  Line l2(
956  Position(me.x() - offsets2.first, me.y() - offsets2.second),
957  Position(to.x() - offsets2.first, to.y() - offsets2.second));
958  l2.extrapolateBy(100);
959  if (l1.intersects(l2)) {
960  shape.push_back(l1.intersectsAt(l2));
961  } else {
962  throw InvalidArgument("no line intersection");
963  }
964  }
965  }
966 
967  /*
968  ContType newCont;
969  std::pair<SUMOReal, SUMOReal> p;
970  Position newPos;
971  // first point
972  newPos = (*(begin()));
973  p = GeomHelper::getNormal90D_CW(*(begin()), *(begin()+1), amount);
974  newPos.add(p.first, p.second);
975  newCont.push_back(newPos);
976  // middle points
977  for(const_iterator i=begin()+1; i!=end()-1; i++) {
978  std::pair<SUMOReal, SUMOReal> oldp = p;
979  newPos = *i;
980  newPos.add(p.first, p.second);
981  newCont.push_back(newPos);
982  p = GeomHelper::getNormal90D_CW(*i, *(i+1), amount);
983  // Position newPos(*i);
984  // newPos.add((p.first+oldp.first)/2.0, (p.second+oldp.second)/2.0);
985  // newCont.push_back(newPos);
986  }
987  // last point
988  newPos = (*(end()-1));
989  newPos.add(p.first, p.second);
990  newCont.push_back(newPos);
991  myCont = newCont;
992  */
993  *this = shape;
994 }
995 
996 
997 Line
998 PositionVector::lineAt(int pos) const {
999  assert((int)size() > pos + 1);
1000  return Line((*this)[pos], (*this)[pos + 1]);
1001 }
1002 
1003 
1004 Line
1006  return lineAt(0);
1007 }
1008 
1009 
1010 Line
1012  return lineAt((int)size() - 2);
1013 }
1014 
1015 
1016 void
1018  if ((*this)[0] == back()) {
1019  return;
1020  }
1021  push_back((*this)[0]);
1022 }
1023 
1024 
1025 std::vector<SUMOReal>
1027  std::vector<SUMOReal> ret;
1028  const_iterator i;
1029  for (i = begin(); i != end(); i++) {
1030  ret.push_back(s.distance(*i));
1031  }
1032  for (i = s.begin(); i != s.end(); i++) {
1033  ret.push_back(distance(*i));
1034  }
1035  return ret;
1036 }
1037 
1038 
1039 void
1040 PositionVector::insertAt(int index, const Position& p) {
1041  if (index >= 0) {
1042  insert(begin() + index, p);
1043  } else {
1044  insert(end() + index, p);
1045  }
1046 }
1047 
1048 
1049 void
1050 PositionVector::replaceAt(int index, const Position& p) {
1051  assert(index < static_cast<int>(size()));
1052  assert(index + static_cast<int>(size()) >= 0);
1053  if (index >= 0) {
1054  (*this)[index] = p;
1055  } else {
1056  (*this)[index + static_cast<int>(size())] = p;
1057  }
1058 }
1059 
1060 
1061 void
1063  if (size() == 0 || !p.almostSame(back())) {
1064  push_back(p);
1065  }
1066 }
1067 
1068 
1069 void
1071  if (size() == 0 || !p.almostSame(front())) {
1072  insert(begin(), p);
1073  }
1074 }
1075 
1076 
1077 bool
1079  return size() >= 2 && (*this)[0] == back();
1080 }
1081 
1082 
1083 void
1084 PositionVector::removeDoublePoints(SUMOReal minDist, bool assertLength) {
1085  if (size() > 1) {
1086  iterator last = begin();
1087  for (iterator i = begin() + 1; i != end() && (!assertLength || size() > 2);) {
1088  if (last->almostSame(*i, minDist)) {
1089  i = erase(i);
1090  } else {
1091  last = i;
1092  ++i;
1093  }
1094  }
1095  }
1096 }
1097 
1098 
1099 void
1101  if (size() > 2) {
1102  Position& last = front();
1103  for (iterator i = begin() + 1; i != end() - 1;) {
1104  if (GeomHelper::distancePointLine(*i, last, *(i + 1)) < 0.001) {
1105  i = erase(i);
1106  } else {
1107  last = *i;
1108  ++i;
1109  }
1110  }
1111  }
1112 }
1113 
1114 
1115 bool
1117  if (size() == v2.size()) {
1118  for (int i = 0; i < (int)size(); i++) {
1119  if ((*this)[i] != v2[i]) {
1120  return false;
1121  }
1122  }
1123  return true;
1124  } else {
1125  return false;
1126  }
1127 }
1128 
1129 
1130 
1131 /****************************************************************************/
1132 
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:359
const Position & p2() const
Definition: Line.cpp:86
static SUMOReal Angle2D(SUMOReal x1, SUMOReal y1, SUMOReal x2, SUMOReal y2)
Definition: GeomHelper.cpp:210
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:189
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:239
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:231
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.
SUMOReal length2D() const
Returns the length.
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:247
Line lineAt(int pos) const
static bool intersects(const Position &p11, const Position &p12, const Position &p21, const Position &p22)
return whether given lines intersect
Definition: GeomHelper.cpp:131
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:274
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:186
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:215
void push_back_noDoublePos(const Position &p)
static SUMOReal closestDistancePointLine(const Position &point, const Position &lineStart, const Position &lineEnd, Position &outIntersection)
Definition: GeomHelper.cpp:298
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