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.sourceforge.net/
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) < 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 (size_t i = 0; i < 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 (size_t i = 0; i < size(); i++) {
477  (*this)[i].add(xoff, yoff, zoff);
478  }
479 }
480 
481 
482 void
484  for (size_t i = 0; i < 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)[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)[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)[size() - 1], (*this)[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  seen += (*i).distanceTo2D(*(i + 1));
786  }
787  return nearestPos;
788 }
789 
790 
791 int
793  assert(size() > 0);
795  SUMOReal dist;
796  int closest = 0;
797  for (int i = 0; i < (int)size(); i++) {
798  dist = p.distanceTo((*this)[i]);
799  if (dist < minDist) {
800  closest = i;
801  minDist = dist;
802  }
803  }
804  return closest;
805 }
806 
807 
808 int
810  Position outIntersection = Position();
812  SUMOReal dist;
813  int insertionIndex = 1;
814  for (int i = 0; i < (int)size() - 1; i++) {
815  dist = GeomHelper::closestDistancePointLine(p, (*this)[i], (*this)[i + 1], outIntersection);
816  if (dist < minDist) {
817  insertionIndex = i + 1;
818  minDist = dist;
819  }
820  }
821  insertAt(insertionIndex, p);
822  return insertionIndex;
823 }
824 
825 
826 SUMOReal
828  if (size() == 1) {
829  return front().distanceTo(p);
830  }
831  Position outIntersection;
833  for (const_iterator i = begin(); i != end() - 1; i++) {
834  minDist = MIN2(minDist, GeomHelper::closestDistancePointLine(
835  p, *i, *(i + 1), outIntersection));
836  }
837  return minDist;
838 }
839 
840 
841 std::vector<SUMOReal>
843  std::vector<SUMOReal> ret;
844  for (const_iterator i = other.begin(); i != other.end() - 1; i++) {
845  std::vector<SUMOReal> atSegment = intersectsAtLengths2D(Line(*i, *(i + 1)));
846  copy(atSegment.begin(), atSegment.end(), back_inserter(ret));
847  }
848  return ret;
849 }
850 
851 
852 std::vector<SUMOReal>
854  std::vector<SUMOReal> ret;
855  SUMOReal pos = 0;
856  for (const_iterator i = begin(); i != end() - 1; i++) {
857  Line l((*i), *(i + 1));
858  if (GeomHelper::intersects(l.p1(), l.p2(), line.p1(), line.p2())) {
859  Position p =
860  GeomHelper::intersection_position2D(l.p1(), l.p2(), line.p1(), line.p2());
861  SUMOReal atLength = p.distanceTo2D(l.p1());
862  ret.push_back(atLength + pos);
863  }
864  pos += l.length2D();
865  }
866  return ret;
867 }
868 
869 
870 void
872  assert(size() > 1);
873  Position nb =
874  GeomHelper::extrapolate_first((*this)[0], (*this)[1], val);
875  Position ne =
877  (*this)[size() - 2], (*this)[size() - 1], val);
878  erase(begin());
879  push_front(nb);
880  erase(end() - 1);
881  push_back(ne);
882 }
883 
884 
887  PositionVector ret;
888  for (const_reverse_iterator i = rbegin(); i != rend(); i++) {
889  ret.push_back(*i);
890  }
891  return ret;
892 }
893 
894 
895 void
897  if (size() < 2) {
898  return;
899  }
900  PositionVector shape;
901  for (size_t i = 0; i < size(); i++) {
902  if (i == 0) {
903  Position from = (*this)[i];
904  Position to = (*this)[i + 1];
905  std::pair<SUMOReal, SUMOReal> offsets =
906  GeomHelper::getNormal90D_CW(from, to, amount);
907  shape.push_back(Position(from.x() - offsets.first,
908  from.y() - offsets.second, from.z()));
909  } else if (i == size() - 1) {
910  Position from = (*this)[i - 1];
911  Position to = (*this)[i];
912  std::pair<SUMOReal, SUMOReal> offsets =
913  GeomHelper::getNormal90D_CW(from, to, amount);
914  shape.push_back(Position(to.x() - offsets.first,
915  to.y() - offsets.second, to.z()));
916  } else {
917  Position from = (*this)[i - 1];
918  Position me = (*this)[i];
919  Position to = (*this)[i + 1];
920  const double sinAngle = sin(GeomHelper::Angle2D(from.x() - me.x(), from.y() - me.y(),
921  me.x() - to.x(), me.y() - to.y()) / 2);
922  const double maxDev = 2 * (from.distanceTo2D(me) + me.distanceTo2D(to)) * sinAngle;
923  if (fabs(maxDev) < POSITION_EPS) {
924  // parallel case, just shift the middle point
925  std::pair<SUMOReal, SUMOReal> off =
926  GeomHelper::getNormal90D_CW(from, to, amount);
927  shape.push_back(Position(me.x() - off.first, me.y() - off.second, me.z()));
928  continue;
929  }
930  std::pair<SUMOReal, SUMOReal> offsets =
931  GeomHelper::getNormal90D_CW(from, me, amount);
932  std::pair<SUMOReal, SUMOReal> offsets2 =
933  GeomHelper::getNormal90D_CW(me, to, amount);
934  Line l1(
935  Position(from.x() - offsets.first, from.y() - offsets.second),
936  Position(me.x() - offsets.first, me.y() - offsets.second));
937  l1.extrapolateBy(100);
938  Line l2(
939  Position(me.x() - offsets2.first, me.y() - offsets2.second),
940  Position(to.x() - offsets2.first, to.y() - offsets2.second));
941  l2.extrapolateBy(100);
942  if (l1.intersects(l2)) {
943  shape.push_back(l1.intersectsAt(l2));
944  } else {
945  throw InvalidArgument("no line intersection");
946  }
947  }
948  }
949 
950  /*
951  ContType newCont;
952  std::pair<SUMOReal, SUMOReal> p;
953  Position newPos;
954  // first point
955  newPos = (*(begin()));
956  p = GeomHelper::getNormal90D_CW(*(begin()), *(begin()+1), amount);
957  newPos.add(p.first, p.second);
958  newCont.push_back(newPos);
959  // middle points
960  for(const_iterator i=begin()+1; i!=end()-1; i++) {
961  std::pair<SUMOReal, SUMOReal> oldp = p;
962  newPos = *i;
963  newPos.add(p.first, p.second);
964  newCont.push_back(newPos);
965  p = GeomHelper::getNormal90D_CW(*i, *(i+1), amount);
966  // Position newPos(*i);
967  // newPos.add((p.first+oldp.first)/2.0, (p.second+oldp.second)/2.0);
968  // newCont.push_back(newPos);
969  }
970  // last point
971  newPos = (*(end()-1));
972  newPos.add(p.first, p.second);
973  newCont.push_back(newPos);
974  myCont = newCont;
975  */
976  *this = shape;
977 }
978 
979 
980 Line
981 PositionVector::lineAt(size_t pos) const {
982  assert(size() > pos + 1);
983  return Line((*this)[pos], (*this)[pos + 1]);
984 }
985 
986 
987 Line
989  return lineAt(0);
990 }
991 
992 
993 Line
995  return lineAt(size() - 2);
996 }
997 
998 
999 void
1001  if ((*this)[0] == back()) {
1002  return;
1003  }
1004  push_back((*this)[0]);
1005 }
1006 
1007 
1008 std::vector<SUMOReal>
1010  std::vector<SUMOReal> ret;
1011  const_iterator i;
1012  for (i = begin(); i != end(); i++) {
1013  ret.push_back(s.distance(*i));
1014  }
1015  for (i = s.begin(); i != s.end(); i++) {
1016  ret.push_back(distance(*i));
1017  }
1018  return ret;
1019 }
1020 
1021 
1022 void
1023 PositionVector::insertAt(int index, const Position& p) {
1024  if (index >= 0) {
1025  insert(begin() + index, p);
1026  } else {
1027  insert(end() + index, p);
1028  }
1029 }
1030 
1031 
1032 void
1033 PositionVector::replaceAt(int index, const Position& p) {
1034  assert(index < static_cast<int>(size()));
1035  assert(index + size() >= 0);
1036  if (index >= 0) {
1037  (*this)[index] = p;
1038  } else {
1039  (*this)[index + size()] = p;
1040  }
1041 }
1042 
1043 
1044 void
1046  if (size() == 0 || !p.almostSame(back())) {
1047  push_back(p);
1048  }
1049 }
1050 
1051 
1052 void
1054  if (size() == 0 || !p.almostSame(front())) {
1055  insert(begin(), p);
1056  }
1057 }
1058 
1059 
1060 bool
1062  return size() >= 2 && (*this)[0] == back();
1063 }
1064 
1065 
1066 void
1067 PositionVector::removeDoublePoints(SUMOReal minDist, bool assertLength) {
1068  if (size() > 1) {
1069  iterator last = begin();
1070  for (iterator i = begin() + 1; i != end() && (!assertLength || size() > 2);) {
1071  if (last->almostSame(*i, minDist)) {
1072  i = erase(i);
1073  } else {
1074  last = i;
1075  ++i;
1076  }
1077  }
1078  }
1079 }
1080 
1081 
1082 void
1084  if (size() > 2) {
1085  Position& last = front();
1086  for (iterator i = begin() + 1; i != end() - 1;) {
1087  if (GeomHelper::distancePointLine(*i, last, *(i + 1)) < 0.001) {
1088  i = erase(i);
1089  } else {
1090  last = *i;
1091  ++i;
1092  }
1093  }
1094  }
1095 }
1096 
1097 
1098 bool
1100  if (size() == v2.size()) {
1101  for (int i = 0; i < (int)size(); i++) {
1102  if ((*this)[i] != v2[i]) {
1103  return false;
1104  }
1105  }
1106  return true;
1107  } else {
1108  return false;
1109  }
1110 }
1111 
1112 
1113 
1114 /****************************************************************************/
1115