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-2014 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  PositionVector tmp(*this);
96  tmp.scaleAbsolute(offset);
97  return tmp.around(p);
98  }
99  SUMOReal angle = 0;
100  for (const_iterator i = begin(); i != end() - 1; i++) {
101  Position p1(
102  (*i).x() - p.x(),
103  (*i).y() - p.y());
104  Position p2(
105  (*(i + 1)).x() - p.x(),
106  (*(i + 1)).y() - p.y());
107  angle += GeomHelper::Angle2D(p1.x(), p1.y(), p2.x(), p2.y());
108  }
109  Position p1(
110  (*(end() - 1)).x() - p.x(),
111  (*(end() - 1)).y() - p.y());
112  Position p2(
113  (*(begin())).x() - p.x(),
114  (*(begin())).y() - p.y());
115  angle += GeomHelper::Angle2D(p1.x(), p1.y(), p2.x(), p2.y());
116  return (!(fabs(angle) < M_PI));
117 }
118 
119 
120 bool
122  for (const_iterator i = begin(); i != end() - 1; i++) {
123  if (poly.around(*i, offset)) {
124  return true;
125  }
126  }
127  return false;
128 }
129 
130 
131 bool
132 PositionVector::intersects(const Position& p1, const Position& p2) const {
133  if (size() < 2) {
134  return false;
135  }
136  for (const_iterator i = begin(); i != end() - 1; i++) {
137  if (GeomHelper::intersects(*i, *(i + 1), p1, p2)) {
138  return true;
139  }
140  }
141  //return GeomHelper::intersects(*(end()-1), *(begin()), p1, p2);
142  return false;
143 }
144 
145 
146 bool
148  if (size() < 2) {
149  return false;
150  }
151  for (const_iterator i = begin(); i != end() - 1; i++) {
152  if (v1.intersects(*i, *(i + 1))) {
153  return true;
154  }
155  }
156  //return v1.intersects(*(end()-1), *(begin()));
157  return false;
158 }
159 
160 
161 Position
163  const Position& p2) const {
164  for (const_iterator i = begin(); i != end() - 1; i++) {
165  if (GeomHelper::intersects(*i, *(i + 1), p1, p2)) {
166  return GeomHelper::intersection_position2D(*i, *(i + 1), p1, p2);
167  }
168  }
169  return Position(-1, -1);
170 }
171 
172 
173 Position
175  for (const_iterator i = begin(); i != end() - 1; i++) {
176  if (v1.intersects(*i, *(i + 1))) {
177  return v1.intersectsAtPoint(*i, *(i + 1));
178  }
179  }
180  /*
181  if(v1.intersects(*(end()-1), *(begin()))) {
182  return v1.intersectsAtPoint(*(end()-1), *(begin()));
183  }
184  */
185  return Position(-1, -1);
186 }
187 
188 
189 const Position&
190 PositionVector::operator[](int index) const {
191  if (index >= 0) {
192  return at(index);
193  } else {
194  return at((int)size() + index);
195  }
196 }
197 
198 
199 Position&
201  if (index >= 0) {
202  return at(index);
203  } else {
204  return at((int)size() + index);
205  }
206 }
207 
208 
209 Position
211  const_iterator i = begin();
212  SUMOReal seenLength = 0;
213  do {
214  const SUMOReal nextLength = (*i).distanceTo(*(i + 1));
215  if (seenLength + nextLength > pos) {
216  return positionAtOffset(*i, *(i + 1), pos - seenLength, lateralOffset);
217  }
218  seenLength += nextLength;
219  } while (++i != end() - 1);
220  return back();
221 }
222 
223 
224 Position
226  const_iterator i = begin();
227  SUMOReal seenLength = 0;
228  do {
229  const SUMOReal nextLength = (*i).distanceTo2D(*(i + 1));
230  if (seenLength + nextLength > pos) {
231  return positionAtOffset2D(*i, *(i + 1), pos - seenLength, lateralOffset);
232  }
233  seenLength += nextLength;
234  } while (++i != end() - 1);
235  return back();
236 }
237 
238 
239 SUMOReal
241  if (pos < 0) {
242  pos += length();
243  }
244  const_iterator i = begin();
245  SUMOReal seenLength = 0;
246  do {
247  SUMOReal nextLength = (*i).distanceTo(*(i + 1));
248  if (seenLength + nextLength > pos) {
249  Line l(*i, *(i + 1));
250  return l.atan2DegreeAngle();
251  }
252  seenLength += nextLength;
253  } while (++i != end() - 1);
254  Line l(*(end() - 2), *(end() - 1));
255  return l.atan2DegreeAngle();
256 }
257 
258 SUMOReal
260  const_iterator i = begin();
261  SUMOReal seenLength = 0;
262  do {
263  SUMOReal nextLength = (*i).distanceTo(*(i + 1));
264  if (seenLength + nextLength > pos) {
265  Line l(*i, *(i + 1));
266  return l.atan2DegreeSlope();
267  }
268  seenLength += nextLength;
269  } while (++i != end() - 1);
270  Line l(*(end() - 2), *(end() - 1));
271  return l.atan2DegreeSlope();
272 }
273 
274 Position
276  const Position& p2,
277  SUMOReal pos, SUMOReal lateralOffset) {
278  const SUMOReal dist = p1.distanceTo(p2);
279  if (dist < pos) {
280  return Position(-1, -1);
281  }
282  if (lateralOffset != 0) {
283  Line l(p1, p2);
284  l.move2side(-lateralOffset); // move in the same direction as Position::move2side
285  return l.getPositionAtDistance(pos);
286  }
287  return p1 + (p2 - p1) * (pos / dist);
288 }
289 
290 
291 Position
293  const Position& p2,
294  SUMOReal pos, SUMOReal lateralOffset) {
295  const SUMOReal dist = p1.distanceTo2D(p2);
296  if (dist < pos) {
297  return Position(-1, -1);
298  }
299  if (lateralOffset != 0) {
300  Line l(p1, p2);
301  l.move2side(-lateralOffset); // move in the same direction as Position::move2side
302  return l.getPositionAtDistance2D(pos);
303  }
304  return p1 + (p2 - p1) * (pos / dist);
305 }
306 
307 
308 Boundary
310  Boundary ret;
311  for (const_iterator i = begin(); i != end(); i++) {
312  ret.add(*i);
313  }
314  return ret;
315 }
316 
317 
318 Position
320  SUMOReal x = 0;
321  SUMOReal y = 0;
322  for (const_iterator i = begin(); i != end(); i++) {
323  x += (*i).x();
324  y += (*i).y();
325  }
326  return Position(x / (SUMOReal) size(), y / (SUMOReal) size());
327 }
328 
329 
330 Position
332  PositionVector tmp = *this;
333  if (!isClosed()) { // make sure its closed
334  tmp.push_back(tmp[0]);
335  }
336  const int endIndex = (int)tmp.size() - 1;
337  SUMOReal div = 0; // 6 * area including sign
338  SUMOReal x = 0;
339  SUMOReal y = 0;
340  if (tmp.area() != 0) { // numerical instability ?
341  // http://en.wikipedia.org/wiki/Polygon
342  for (int i = 0; i < endIndex; i++) {
343  const SUMOReal z = tmp[i].x() * tmp[i + 1].y() - tmp[i + 1].x() * tmp[i].y();
344  div += z; // area formula
345  x += (tmp[i].x() + tmp[i + 1].x()) * z;
346  y += (tmp[i].y() + tmp[i + 1].y()) * z;
347  }
348  div *= 3; // 6 / 2, the 2 comes from the area formula
349  return Position(x / div, y / div);
350  } else {
351  // compute by decomposing into line segments
352  // http://en.wikipedia.org/wiki/Centroid#By_geometric_decomposition
353  SUMOReal lengthSum = 0;
354  for (int i = 0; i < endIndex; i++) {
355  SUMOReal length = tmp[i].distanceTo(tmp[i + 1]);
356  x += (tmp[i].x() + tmp[i + 1].x()) * length / 2;
357  y += (tmp[i].y() + tmp[i + 1].y()) * length / 2;
358  lengthSum += length;
359  }
360  if (lengthSum == 0) {
361  // it is probably only one point
362  return tmp[0];
363  }
364  return Position(x / lengthSum, y / lengthSum);
365  }
366 }
367 
368 
369 void
371  Position centroid = getCentroid();
372  for (int i = 0; i < static_cast<int>(size()); i++) {
373  (*this)[i] = centroid + (((*this)[i] - centroid) * factor);
374  }
375 }
376 
377 
378 void
380  Position centroid = getCentroid();
381  for (int i = 0; i < static_cast<int>(size()); i++) {
382  (*this)[i] = centroid + (((*this)[i] - centroid) + offset);
383  }
384 }
385 
386 
387 Position
389  if (size() == 1) {
390  return (*this)[0];
391  }
392  return positionAtOffset(SUMOReal((length() / 2.)));
393 }
394 
395 
396 SUMOReal
398  SUMOReal len = 0;
399  for (const_iterator i = begin(); i != end() - 1; i++) {
400  len += (*i).distanceTo(*(i + 1));
401  }
402  return len;
403 }
404 
405 SUMOReal
407  SUMOReal len = 0;
408  for (const_iterator i = begin(); i != end() - 1; i++) {
409  len += (*i).distanceTo2D(*(i + 1));
410  }
411  return len;
412 }
413 
414 
415 SUMOReal
417  SUMOReal area = 0;
418  PositionVector tmp = *this;
419  if (!isClosed()) { // make sure its closed
420  tmp.push_back(tmp[0]);
421  }
422  const int endIndex = (int)tmp.size() - 1;
423  // http://en.wikipedia.org/wiki/Polygon
424  for (int i = 0; i < endIndex; i++) {
425  area += tmp[i].x() * tmp[i + 1].y() - tmp[i + 1].x() * tmp[i].y();
426  }
427  if (area < 0) { // we whether we had cw or ccw order
428  area *= -1;
429  }
430  return area / 2;
431 }
432 
433 
434 bool
436  for (const_iterator i = begin(); i != end() - 1; i++) {
437  if (poly.around(*i, offset)) {
438  return true;
439  }
440  }
441  return false;
442 }
443 
444 
445 bool
446 PositionVector::crosses(const Position& p1, const Position& p2) const {
447  return intersects(p1, p2);
448 }
449 
450 
451 std::pair<PositionVector, PositionVector>
453  if (size() < 2) {
454  throw InvalidArgument("Vector to short for splitting");
455  }
456  if (where <= POSITION_EPS || where >= length() - POSITION_EPS) {
457  WRITE_WARNING("Splitting vector close to end (pos: " + toString(where) + ", length: " + toString(length()) + ")");
458  }
459  PositionVector first, second;
460  first.push_back((*this)[0]);
461  SUMOReal seen = 0;
462  const_iterator it = begin() + 1;
463  SUMOReal next = first.back().distanceTo(*it);
464  // see how many points we can add to first
465  while (where >= seen + next + POSITION_EPS) {
466  seen += next;
467  first.push_back(*it);
468  it++;
469  next = first.back().distanceTo(*it);
470  }
471  if (fabs(where - (seen + next)) > POSITION_EPS || it == end() - 1) {
472  // we need to insert a new point because 'where' is not close to an
473  // existing point or it is to close to the endpoint
474  Line tmpL(first.back(), *it);
475  Position p = tmpL.getPositionAtDistance(where - seen);
476  first.push_back(p);
477  second.push_back(p);
478  } else {
479  first.push_back(*it);
480  }
481  // add the remaining points to second
482  for (; it != end(); it++) {
483  second.push_back(*it);
484  }
485  assert(first.size() >= 2);
486  assert(second.size() >= 2);
487  assert(first.back() == second.front());
488  assert(fabs(first.length() + second.length() - length()) < 2 * POSITION_EPS);
489  return std::pair<PositionVector, PositionVector>(first, second);
490 }
491 
492 
493 std::ostream&
494 operator<<(std::ostream& os, const PositionVector& geom) {
495  for (PositionVector::const_iterator i = geom.begin(); i != geom.end(); i++) {
496  if (i != geom.begin()) {
497  os << " ";
498  }
499  os << (*i);
500  }
501  return os;
502 }
503 
504 
505 void
507  std::sort(begin(), end(), as_poly_cw_sorter());
508 }
509 
510 
511 void
513  for (int i = 0; i < static_cast<int>(size()); i++) {
514  (*this)[i].add(xoff, yoff, zoff);
515  }
516 }
517 
518 
519 void
521  for (int i = 0; i < static_cast<int>(size()); i++) {
522  (*this)[i].reshiftRotate(xoff, yoff, rot);
523  }
524 }
525 
526 
527 int
529  const Position& p2) const {
530  return atan2(p1.x(), p1.y()) < atan2(p2.x(), p2.y());
531 }
532 
533 
534 
535 void
537  std::sort(begin(), end(), increasing_x_y_sorter());
538 }
539 
540 
541 
543 
544 
545 
546 int
548  const Position& p2) const {
549  if (p1.x() != p2.x()) {
550  return p1.x() < p2.x();
551  }
552  return p1.y() < p2.y();
553 }
554 
555 
556 
557 SUMOReal
559  const Position& P2) const {
560  return (P1.x() - P0.x()) * (P2.y() - P0.y()) - (P2.x() - P0.x()) * (P1.y() - P0.y());
561 }
562 
563 
566  PositionVector ret = *this;
567  ret.sortAsPolyCWByAngle();
568  return simpleHull_2D(ret);
569 }
570 
571 
574  PositionVector ret;
575  for (const_iterator i = begin(); i != end() - 1; i++) {
576  if (GeomHelper::intersects(*i, *(i + 1), line.p1(), line.p2())) {
578  *i, *(i + 1), line.p1(), line.p2()));
579  }
580  }
581  return ret;
582 }
583 
584 
585 int
587  if (back().distanceTo(v[0]) < 2) { // !!! heuristic
588  copy(v.begin() + 1, v.end(), back_inserter(*this));
589  return 1;
590  }
591  //
592  Line l1((*this)[static_cast<int>(size()) - 2], back());
593  l1.extrapolateBy(100);
594  Line l2(v[0], v[1]);
595  l2.extrapolateBy(100);
596  if (l1.intersects(l2) && l1.intersectsAtLength2D(l2) < l1.length2D() - 100) { // !!! heuristic
597  Position p = l1.intersectsAt(l2);
598  (*this)[static_cast<int>(size()) - 1] = p;
599  copy(v.begin() + 1, v.end(), back_inserter(*this));
600  return 2;
601  } else {
602  copy(v.begin(), v.end(), back_inserter(*this));
603  return 3;
604  }
605 }
606 
607 
608 void
610  if (back().distanceTo(v[0]) < 2) {
611  copy(v.begin() + 1, v.end(), back_inserter(*this));
612  } else {
613  copy(v.begin(), v.end(), back_inserter(*this));
614  }
615 }
616 
617 
619 PositionVector::getSubpart(SUMOReal beginOffset, SUMOReal endOffset) const {
620  PositionVector ret;
621  Position begPos = front();
622  if (beginOffset > POSITION_EPS) {
623  begPos = positionAtOffset(beginOffset);
624  }
625  Position endPos = back();
626  if (endOffset < length() - POSITION_EPS) {
627  endPos = positionAtOffset(endOffset);
628  }
629  ret.push_back(begPos);
630 
631  SUMOReal seen = 0;
632  const_iterator i = begin();
633  // skip previous segments
634  while ((i + 1) != end()
635  &&
636  seen + (*i).distanceTo(*(i + 1)) < beginOffset) {
637  seen += (*i).distanceTo(*(i + 1));
638  i++;
639  }
640  // append segments in between
641  while ((i + 1) != end()
642  &&
643  seen + (*i).distanceTo(*(i + 1)) < endOffset) {
644 
645  ret.push_back_noDoublePos(*(i + 1));
646  /*
647  if(ret.at(-1)!=*(i+1)) {
648  ret.push_back(*(i+1));
649  }
650  */
651  seen += (*i).distanceTo(*(i + 1));
652  i++;
653  }
654  // append end
655  ret.push_back_noDoublePos(endPos);
656  return ret;
657 }
658 
659 
661 PositionVector::getSubpart2D(SUMOReal beginOffset, SUMOReal endOffset) const {
662  PositionVector ret;
663  Position begPos = front();
664  if (beginOffset > POSITION_EPS) {
665  begPos = positionAtOffset2D(beginOffset);
666  }
667  Position endPos = back();
668  if (endOffset < length() - POSITION_EPS) {
669  endPos = positionAtOffset2D(endOffset);
670  }
671  ret.push_back(begPos);
672 
673  SUMOReal seen = 0;
674  const_iterator i = begin();
675  // skip previous segments
676  while ((i + 1) != end()
677  &&
678  seen + (*i).distanceTo2D(*(i + 1)) < beginOffset) {
679  seen += (*i).distanceTo2D(*(i + 1));
680  i++;
681  }
682  // append segments in between
683  while ((i + 1) != end()
684  &&
685  seen + (*i).distanceTo2D(*(i + 1)) < endOffset) {
686 
687  ret.push_back_noDoublePos(*(i + 1));
688  /*
689  if(ret.at(-1)!=*(i+1)) {
690  ret.push_back(*(i+1));
691  }
692  */
693  seen += (*i).distanceTo2D(*(i + 1));
694  i++;
695  }
696  // append end
697  ret.push_back_noDoublePos(endPos);
698  return ret;
699 }
700 
701 
702 void
704  // find minimum distance (from the begin)
705  size_t pos = 0;
706  SUMOReal dist = 1000000;
707  size_t currPos = 0;
709  GeomHelper::extrapolate_first(*(begin()), *(begin() + 1), 100),
710  *(begin() + 1));
711 // assert(currDist>=0);
712  if (currDist >= 0 && currDist < dist) {
713  dist = currDist;
714  pos = currPos;
715  }
716 
717  for (iterator i = begin(); i != end() - 1; i++, currPos++) {
718  SUMOReal currDist = GeomHelper::distancePointLine(p, *i, *(i + 1));
719  if (currDist >= 0 && currDist < dist) {
720  dist = currDist;
721  pos = currPos;
722  }
723  }
724  // remove leading items
725  for (size_t j = 0; j < pos; j++) {
726  erase(begin());
727  }
728  // replace first item by the new position
730  (*this)[0], (*this)[1], p);
731  if (lpos == -1) {
732  return;
733  }
734  Position np = positionAtOffset(lpos);
735  if (np != *(begin())) {
736  erase(begin());
737  if (np != *(begin())) {
738  insert(begin(), p);
739  assert(size() > 1);
740  assert(*(begin()) != *(end() - 1));
741  }
742  }
743 }
744 
745 
747 PositionVector::getSubpartByIndex(int beginIndex, int count) const {
748  if (beginIndex < 0) {
749  beginIndex += (int)size();
750  }
751  assert(count > 0);
752  assert(beginIndex < (int)size());
753  assert(beginIndex + count <= (int)size());
754  PositionVector result;
755  for (int i = beginIndex; i < beginIndex + count; ++i) {
756  result.push_back((*this)[i]);
757  }
758  return result;
759 }
760 
761 
762 void
764  // find minimum distance (from the end)
765  size_t pos = 0;
766  SUMOReal dist = 1000000;
767  size_t currPos = 0;
769  *(end() - 1),
770  GeomHelper::extrapolate_second(*(end() - 2), *(end() - 1), 100));
771 // assert(currDist>=0);
772  if (currDist >= 0 && currDist < dist) {
773  dist = currDist;
774  pos = currPos;
775  }
776 
777  for (reverse_iterator i = rbegin(); i != rend() - 1; i++, currPos++) {
778  SUMOReal currDist = GeomHelper::distancePointLine(p, *(i), *(i + 1));
779  if (currDist >= 0 && currDist < dist) {
780  dist = currDist;
781  pos = currPos;
782  }
783  }
784  // remove trailing items
785  for (size_t j = 0; j < pos; j++) {
786  erase(end() - 1);
787  }
788  // replace last item by the new position
789  SUMOReal lpos =
791  (*this)[static_cast<int>(size()) - 1], (*this)[static_cast<int>(size()) - 2], p);
792  if (lpos == -1) {
793  return;
794  }
796  length() - lpos);
797  if (np != *(end() - 1)) {
798  erase(end() - 1);
799  if (np != *(end() - 1)) {
800  push_back(np);
801  assert(size() > 1);
802  assert(*(begin()) != *(end() - 1));
803  }
804  }
805 }
806 
807 
808 SUMOReal
810  Line tmp(front(), back());
811  return tmp.atan2Angle();
812 }
813 
814 
815 void
817  if (i >= 0) {
818  erase(begin() + i);
819  } else {
820  erase(end() + i);
821  }
822 }
823 
824 
825 SUMOReal
826 PositionVector::nearest_offset_to_point2D(const Position& p, bool perpendicular) const {
828  SUMOReal nearestPos = -1;
829  SUMOReal seen = 0;
830  for (const_iterator i = begin(); i != end() - 1; i++) {
831  const SUMOReal pos =
832  GeomHelper::nearest_offset_on_line_to_point2D(*i, *(i + 1), p, perpendicular);
833  const SUMOReal dist = pos < 0 ? minDist : p.distanceTo2D(Line(*i, *(i + 1)).getPositionAtDistance(pos));
834  if (dist < minDist) {
835  nearestPos = pos + seen;
836  minDist = dist;
837  }
838  if (perpendicular && i != begin()) {
839  // even if perpendicular is set we still need to check the distance to the inner points
840  const SUMOReal cornerDist = p.distanceTo2D(*i);
841  if (cornerDist < minDist) {
842  nearestPos = seen;
843  minDist = cornerDist;
844  }
845  }
846  seen += (*i).distanceTo2D(*(i + 1));
847  }
848  return nearestPos;
849 }
850 
851 
852 int
854  assert(size() > 0);
856  SUMOReal dist;
857  int closest = 0;
858  for (int i = 0; i < (int)size(); i++) {
859  dist = p.distanceTo((*this)[i]);
860  if (dist < minDist) {
861  closest = i;
862  minDist = dist;
863  }
864  }
865  return closest;
866 }
867 
868 
869 int
871  Position outIntersection = Position();
873  SUMOReal dist;
874  int insertionIndex = 1;
875  for (int i = 0; i < (int)size() - 1; i++) {
876  dist = GeomHelper::closestDistancePointLine(p, (*this)[i], (*this)[i + 1], outIntersection);
877  if (dist < minDist) {
878  insertionIndex = i + 1;
879  minDist = dist;
880  }
881  }
882  insertAt(insertionIndex, p);
883  return insertionIndex;
884 }
885 
886 
887 SUMOReal
889  if (size() == 1) {
890  return front().distanceTo(p);
891  }
892  Position outIntersection;
894  for (const_iterator i = begin(); i != end() - 1; i++) {
895  minDist = MIN2(minDist, GeomHelper::closestDistancePointLine(
896  p, *i, *(i + 1), outIntersection));
897  }
898  return minDist;
899 }
900 
901 
902 std::vector<SUMOReal>
904  std::vector<SUMOReal> ret;
905  for (const_iterator i = other.begin(); i != other.end() - 1; i++) {
906  std::vector<SUMOReal> atSegment = intersectsAtLengths2D(Line(*i, *(i + 1)));
907  copy(atSegment.begin(), atSegment.end(), back_inserter(ret));
908  }
909  return ret;
910 }
911 
912 
913 std::vector<SUMOReal>
915  std::vector<SUMOReal> ret;
916  SUMOReal pos = 0;
917  for (const_iterator i = begin(); i != end() - 1; i++) {
918  Line l((*i), *(i + 1));
919  if (GeomHelper::intersects(l.p1(), l.p2(), line.p1(), line.p2())) {
920  Position p =
921  GeomHelper::intersection_position2D(l.p1(), l.p2(), line.p1(), line.p2());
922  SUMOReal atLength = p.distanceTo2D(l.p1());
923  ret.push_back(atLength + pos);
924  }
925  pos += l.length2D();
926  }
927  return ret;
928 }
929 
930 
931 void
933  assert(size() > 1);
934  Position nb =
935  GeomHelper::extrapolate_first((*this)[0], (*this)[1], val);
936  Position ne =
938  (*this)[static_cast<int>(size()) - 2], (*this)[static_cast<int>(size()) - 1], val);
939  erase(begin());
940  push_front(nb);
941  erase(end() - 1);
942  push_back(ne);
943 }
944 
945 
948  PositionVector ret;
949  for (const_reverse_iterator i = rbegin(); i != rend(); i++) {
950  ret.push_back(*i);
951  }
952  return ret;
953 }
954 
955 
956 void
958  if (size() < 2) {
959  return;
960  }
961  PositionVector shape;
962  for (int i = 0; i < static_cast<int>(size()); i++) {
963  if (i == 0) {
964  Position from = (*this)[i];
965  Position to = (*this)[i + 1];
966  std::pair<SUMOReal, SUMOReal> offsets =
967  GeomHelper::getNormal90D_CW(from, to, amount);
968  shape.push_back(Position(from.x() - offsets.first,
969  from.y() - offsets.second, from.z()));
970  } else if (i == static_cast<int>(size()) - 1) {
971  Position from = (*this)[i - 1];
972  Position to = (*this)[i];
973  std::pair<SUMOReal, SUMOReal> offsets =
974  GeomHelper::getNormal90D_CW(from, to, amount);
975  shape.push_back(Position(to.x() - offsets.first,
976  to.y() - offsets.second, to.z()));
977  } else {
978  Position from = (*this)[i - 1];
979  Position me = (*this)[i];
980  Position to = (*this)[i + 1];
981  const double sinAngle = sin(GeomHelper::Angle2D(from.x() - me.x(), from.y() - me.y(),
982  me.x() - to.x(), me.y() - to.y()) / 2);
983  const double maxDev = 2 * (from.distanceTo2D(me) + me.distanceTo2D(to)) * sinAngle;
984  if (fabs(maxDev) < POSITION_EPS) {
985  // parallel case, just shift the middle point
986  std::pair<SUMOReal, SUMOReal> off =
987  GeomHelper::getNormal90D_CW(from, to, amount);
988  shape.push_back(Position(me.x() - off.first, me.y() - off.second, me.z()));
989  continue;
990  }
991  std::pair<SUMOReal, SUMOReal> offsets =
992  GeomHelper::getNormal90D_CW(from, me, amount);
993  std::pair<SUMOReal, SUMOReal> offsets2 =
994  GeomHelper::getNormal90D_CW(me, to, amount);
995  Line l1(
996  Position(from.x() - offsets.first, from.y() - offsets.second),
997  Position(me.x() - offsets.first, me.y() - offsets.second));
998  l1.extrapolateBy(100);
999  Line l2(
1000  Position(me.x() - offsets2.first, me.y() - offsets2.second),
1001  Position(to.x() - offsets2.first, to.y() - offsets2.second));
1002  l2.extrapolateBy(100);
1003  if (l1.intersects(l2)) {
1004  shape.push_back(l1.intersectsAt(l2));
1005  } else {
1006  throw InvalidArgument("no line intersection");
1007  }
1008  }
1009  }
1010 
1011  /*
1012  ContType newCont;
1013  std::pair<SUMOReal, SUMOReal> p;
1014  Position newPos;
1015  // first point
1016  newPos = (*(begin()));
1017  p = GeomHelper::getNormal90D_CW(*(begin()), *(begin()+1), amount);
1018  newPos.add(p.first, p.second);
1019  newCont.push_back(newPos);
1020  // middle points
1021  for(const_iterator i=begin()+1; i!=end()-1; i++) {
1022  std::pair<SUMOReal, SUMOReal> oldp = p;
1023  newPos = *i;
1024  newPos.add(p.first, p.second);
1025  newCont.push_back(newPos);
1026  p = GeomHelper::getNormal90D_CW(*i, *(i+1), amount);
1027  // Position newPos(*i);
1028  // newPos.add((p.first+oldp.first)/2.0, (p.second+oldp.second)/2.0);
1029  // newCont.push_back(newPos);
1030  }
1031  // last point
1032  newPos = (*(end()-1));
1033  newPos.add(p.first, p.second);
1034  newCont.push_back(newPos);
1035  myCont = newCont;
1036  */
1037  *this = shape;
1038 }
1039 
1040 
1041 Line
1042 PositionVector::lineAt(int pos) const {
1043  assert((int)size() > pos + 1);
1044  return Line((*this)[pos], (*this)[pos + 1]);
1045 }
1046 
1047 
1048 Line
1050  return lineAt(0);
1051 }
1052 
1053 
1054 Line
1056  return lineAt((int)size() - 2);
1057 }
1058 
1059 
1060 void
1062  if ((*this)[0] == back()) {
1063  return;
1064  }
1065  push_back((*this)[0]);
1066 }
1067 
1068 
1069 std::vector<SUMOReal>
1071  std::vector<SUMOReal> ret;
1072  const_iterator i;
1073  for (i = begin(); i != end(); i++) {
1074  ret.push_back(s.distance(*i));
1075  }
1076  for (i = s.begin(); i != s.end(); i++) {
1077  ret.push_back(distance(*i));
1078  }
1079  return ret;
1080 }
1081 
1082 
1083 void
1084 PositionVector::insertAt(int index, const Position& p) {
1085  if (index >= 0) {
1086  insert(begin() + index, p);
1087  } else {
1088  insert(end() + index, p);
1089  }
1090 }
1091 
1092 
1093 void
1094 PositionVector::replaceAt(int index, const Position& p) {
1095  assert(index < static_cast<int>(size()));
1096  assert(index + static_cast<int>(size()) >= 0);
1097  if (index >= 0) {
1098  (*this)[index] = p;
1099  } else {
1100  (*this)[index + static_cast<int>(size())] = p;
1101  }
1102 }
1103 
1104 
1105 void
1107  if (size() == 0 || !p.almostSame(back())) {
1108  push_back(p);
1109  }
1110 }
1111 
1112 
1113 void
1115  if (size() == 0 || !p.almostSame(front())) {
1116  insert(begin(), p);
1117  }
1118 }
1119 
1120 
1121 bool
1123  return size() >= 2 && (*this)[0] == back();
1124 }
1125 
1126 
1127 void
1128 PositionVector::removeDoublePoints(SUMOReal minDist, bool assertLength) {
1129  if (size() > 1) {
1130  iterator last = begin();
1131  for (iterator i = begin() + 1; i != end() && (!assertLength || size() > 2);) {
1132  if (last->almostSame(*i, minDist)) {
1133  i = erase(i);
1134  } else {
1135  last = i;
1136  ++i;
1137  }
1138  }
1139  }
1140 }
1141 
1142 
1143 void
1145  if (size() > 2) {
1146  Position& last = front();
1147  for (iterator i = begin() + 1; i != end() - 1;) {
1148  if (GeomHelper::distancePointLine(*i, last, *(i + 1)) < 0.001) {
1149  i = erase(i);
1150  } else {
1151  last = *i;
1152  ++i;
1153  }
1154  }
1155  }
1156 }
1157 
1158 
1159 bool
1161  if (size() == v2.size()) {
1162  for (int i = 0; i < (int)size(); i++) {
1163  if ((*this)[i] != v2[i]) {
1164  return false;
1165  }
1166  }
1167  return true;
1168  } else {
1169  return false;
1170  }
1171 }
1172 
1173 
1174 
1175 /****************************************************************************/
1176