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