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 Position
266  const Position& p2,
267  SUMOReal pos) {
268  const SUMOReal dist = p1.distanceTo(p2);
269  if (dist < pos) {
270  return Position(-1, -1);
271  }
272  return p1 + (p2 - p1) * (pos / dist);
273 }
274 
275 
276 Position
278  const Position& p2,
279  SUMOReal pos) {
280  const SUMOReal dist = p1.distanceTo2D(p2);
281  if (dist < pos) {
282  return Position(-1, -1);
283  }
284  return p1 + (p2 - p1) * (pos / dist);
285 }
286 
287 
288 Boundary
290  Boundary ret;
291  for (ContType::const_iterator i = myCont.begin(); i != myCont.end(); i++) {
292  ret.add(*i);
293  }
294  return ret;
295 }
296 
297 
298 Position
300  SUMOReal x = 0;
301  SUMOReal y = 0;
302  for (ContType::const_iterator i = myCont.begin(); i != myCont.end(); i++) {
303  x += (*i).x();
304  y += (*i).y();
305  }
306  return Position(x / (SUMOReal) myCont.size(), y / (SUMOReal) myCont.size());
307 }
308 
309 
310 Position
312  PositionVector tmp = *this;
313  if (!isClosed()) { // make sure its closed
314  tmp.push_back(tmp[0]);
315  }
316  const int endIndex = (int)tmp.size() - 1;
317  SUMOReal div = 0; // 6 * area including sign
318  SUMOReal x = 0;
319  SUMOReal y = 0;
320  if (tmp.area() != 0) { // numerical instability ?
321  // http://en.wikipedia.org/wiki/Polygon
322  for (int i = 0; i < endIndex; i++) {
323  const SUMOReal z = tmp[i].x() * tmp[i + 1].y() - tmp[i + 1].x() * tmp[i].y();
324  div += z; // area formula
325  x += (tmp[i].x() + tmp[i + 1].x()) * z;
326  y += (tmp[i].y() + tmp[i + 1].y()) * z;
327  }
328  div *= 3; // 6 / 2, the 2 comes from the area formula
329  return Position(x / div, y / div);
330  } else {
331  // compute by decomposing into line segments
332  // http://en.wikipedia.org/wiki/Centroid#By_geometric_decomposition
333  SUMOReal lengthSum = 0;
334  for (int i = 0; i < endIndex; i++) {
335  SUMOReal length = tmp[i].distanceTo(tmp[i + 1]);
336  x += (tmp[i].x() + tmp[i + 1].x()) * length / 2;
337  y += (tmp[i].y() + tmp[i + 1].y()) * length / 2;
338  lengthSum += length;
339  }
340  return Position(x / lengthSum, y / lengthSum);
341  }
342 }
343 
344 
345 void
347  Position centroid = getCentroid();
348  for (size_t i = 0; i < size(); i++) {
349  myCont[i] = centroid + ((myCont[i] - centroid) * factor);
350  }
351 }
352 
353 
354 Position
356  if (myCont.size() == 1) {
357  return myCont[0];
358  }
359  return positionAtLengthPosition(SUMOReal((length() / 2.)));
360 }
361 
362 
363 SUMOReal
365  SUMOReal len = 0;
366  for (ContType::const_iterator i = myCont.begin(); i != myCont.end() - 1; i++) {
367  len += (*i).distanceTo(*(i + 1));
368  }
369  return len;
370 }
371 
372 
373 SUMOReal
375  SUMOReal area = 0;
376  PositionVector tmp = *this;
377  if (!isClosed()) { // make sure its closed
378  tmp.push_back(tmp[0]);
379  }
380  const int endIndex = (int)tmp.size() - 1;
381  // http://en.wikipedia.org/wiki/Polygon
382  for (int i = 0; i < endIndex; i++) {
383  area += tmp[i].x() * tmp[i + 1].y() - tmp[i + 1].x() * tmp[i].y();
384  }
385  if (area < 0) { // we whether we had cw or ccw order
386  area *= -1;
387  }
388  return area / 2;
389 }
390 
391 
392 bool
394  for (ContType::const_iterator i = myCont.begin(); i != myCont.end() - 1; i++) {
395  if (poly.around(*i, offset)) {
396  return true;
397  }
398  }
399  return false;
400 }
401 
402 
403 
404 bool
405 PositionVector::crosses(const Position& p1, const Position& p2) const {
406  return intersects(p1, p2);
407 }
408 
409 
410 
411 const Position&
413  return myCont[0];
414 }
415 
416 
417 const Position&
419  return myCont.back();
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(myCont[0]);
433  SUMOReal seen = 0;
434  ContType::const_iterator it = myCont.begin() + 1;
435  SUMOReal next = first.getEnd().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.getEnd().distanceTo(*it);
442  }
443  if (fabs(where - (seen + next)) > POSITION_EPS || it == myCont.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.getEnd(), *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 != myCont.end(); it++) {
455  second.push_back(*it);
456  }
457  assert(first.size() >= 2);
458  assert(second.size() >= 2);
459  assert(first.getEnd() == second.getBegin());
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::ContType::const_iterator i = geom.myCont.begin(); i != geom.myCont.end(); i++) {
468  if (i != geom.myCont.begin()) {
469  os << " ";
470  }
471  os << (*i);
472  }
473  return os;
474 }
475 
476 
477 void
479  std::sort(myCont.begin(), myCont.end(), as_poly_cw_sorter());
480 }
481 
482 
483 void
485  for (size_t i = 0; i < size(); i++) {
486  myCont[i].add(xoff, yoff, zoff);
487  }
488 }
489 
490 
491 void
493  for (size_t i = 0; i < size(); i++) {
494  myCont[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(myCont.begin(), myCont.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 
544 void
545 PositionVector::set(size_t pos, const Position& p) {
546  myCont[pos] = p;
547 }
548 
549 
552  PositionVector ret;
553  for (ContType::const_iterator i = myCont.begin(); i != myCont.end() - 1; i++) {
554  if (GeomHelper::intersects(*i, *(i + 1), line.p1(), line.p2())) {
556  *i, *(i + 1), line.p1(), line.p2()));
557  }
558  }
559  return ret;
560 }
561 
562 
563 int
565  if (myCont.back().distanceTo(v.myCont[0]) < 2) { // !!! heuristic
566  copy(v.myCont.begin() + 1, v.myCont.end(), back_inserter(myCont));
567  return 1;
568  }
569  //
570  Line l1(myCont[myCont.size() - 2], myCont.back());
571  l1.extrapolateBy(100);
572  Line l2(v.myCont[0], v.myCont[1]);
573  l2.extrapolateBy(100);
574  if (l1.intersects(l2) && l1.intersectsAtLength2D(l2) < l1.length2D() - 100) { // !!! heuristic
575  Position p = l1.intersectsAt(l2);
576  myCont[myCont.size() - 1] = p;
577  copy(v.myCont.begin() + 1, v.myCont.end(), back_inserter(myCont));
578  return 2;
579  } else {
580  copy(v.myCont.begin(), v.myCont.end(), back_inserter(myCont));
581  return 3;
582  }
583 }
584 
585 
588  PositionVector ret;
589  Position begPos = myCont.front();
590  if (begin > POSITION_EPS) {
591  begPos = positionAtLengthPosition(begin);
592  }
593  Position endPos = myCont.back();
594  if (end < length() - POSITION_EPS) {
595  endPos = positionAtLengthPosition(end);
596  }
597  ret.push_back(begPos);
598 
599  SUMOReal seen = 0;
600  ContType::const_iterator i = myCont.begin();
601  // skip previous segments
602  while ((i + 1) != myCont.end()
603  &&
604  seen + (*i).distanceTo(*(i + 1)) < begin) {
605  seen += (*i).distanceTo(*(i + 1));
606  i++;
607  }
608  // append segments in between
609  while ((i + 1) != myCont.end()
610  &&
611  seen + (*i).distanceTo(*(i + 1)) < end) {
612 
613  ret.push_back_noDoublePos(*(i + 1));
614  /*
615  if(ret.at(-1)!=*(i+1)) {
616  ret.push_back(*(i+1));
617  }
618  */
619  seen += (*i).distanceTo(*(i + 1));
620  i++;
621  }
622  // append end
623  ret.push_back_noDoublePos(endPos);
624  return ret;
625 }
626 
627 
630  PositionVector ret;
631  Position begPos = myCont.front();
632  if (begin > POSITION_EPS) {
633  begPos = positionAtLengthPosition2D(begin);
634  }
635  Position endPos = myCont.back();
636  if (end < length() - POSITION_EPS) {
637  endPos = positionAtLengthPosition2D(end);
638  }
639  ret.push_back(begPos);
640 
641  SUMOReal seen = 0;
642  ContType::const_iterator i = myCont.begin();
643  // skip previous segments
644  while ((i + 1) != myCont.end()
645  &&
646  seen + (*i).distanceTo2D(*(i + 1)) < begin) {
647  seen += (*i).distanceTo2D(*(i + 1));
648  i++;
649  }
650  // append segments in between
651  while ((i + 1) != myCont.end()
652  &&
653  seen + (*i).distanceTo2D(*(i + 1)) < end) {
654 
655  ret.push_back_noDoublePos(*(i + 1));
656  /*
657  if(ret.at(-1)!=*(i+1)) {
658  ret.push_back(*(i+1));
659  }
660  */
661  seen += (*i).distanceTo2D(*(i + 1));
662  i++;
663  }
664  // append end
665  ret.push_back_noDoublePos(endPos);
666  return ret;
667 }
668 
669 
670 void
672  // find minimum distance (from the begin)
673  size_t pos = 0;
674  SUMOReal dist = 1000000;
675  size_t currPos = 0;
677  GeomHelper::extrapolate_first(*(myCont.begin()), *(myCont.begin() + 1), 100),
678  *(myCont.begin() + 1));
679 // assert(currDist>=0);
680  if (currDist >= 0 && currDist < dist) {
681  dist = currDist;
682  pos = currPos;
683  }
684 
685  for (ContType::iterator i = myCont.begin(); i != myCont.end() - 1; i++, currPos++) {
686  SUMOReal currDist = GeomHelper::distancePointLine(p, *i, *(i + 1));
687  if (currDist >= 0 && currDist < dist) {
688  dist = currDist;
689  pos = currPos;
690  }
691  }
692  // remove leading items
693  for (size_t j = 0; j < pos; j++) {
694  myCont.erase(myCont.begin());
695  }
696  // replace first item by the new position
698  myCont[0], myCont[1], p);
699  if (lpos == -1) {
700  return;
701  }
703  if (np != *(myCont.begin())) {
704  myCont.erase(myCont.begin());
705  if (np != *(myCont.begin())) {
706  myCont.insert(myCont.begin(), p);
707  assert(myCont.size() > 1);
708  assert(*(myCont.begin()) != *(myCont.end() - 1));
709  }
710  }
711 }
712 
713 
714 void
716  // find minimum distance (from the end)
717  size_t pos = 0;
718  SUMOReal dist = 1000000;
719  size_t currPos = 0;
721  *(myCont.end() - 1),
722  GeomHelper::extrapolate_second(*(myCont.end() - 2), *(myCont.end() - 1), 100));
723 // assert(currDist>=0);
724  if (currDist >= 0 && currDist < dist) {
725  dist = currDist;
726  pos = currPos;
727  }
728 
729  for (ContType::reverse_iterator i = myCont.rbegin(); i != myCont.rend() - 1; i++, currPos++) {
730  SUMOReal currDist = GeomHelper::distancePointLine(p, *(i), *(i + 1));
731  if (currDist >= 0 && currDist < dist) {
732  dist = currDist;
733  pos = currPos;
734  }
735  }
736  // remove trailing items
737  for (size_t j = 0; j < pos; j++) {
738  myCont.erase(myCont.end() - 1);
739  }
740  // replace last item by the new position
741  SUMOReal lpos =
743  myCont[myCont.size() - 1], myCont[myCont.size() - 2], p);
744  if (lpos == -1) {
745  return;
746  }
748  length() - lpos);
749  if (np != *(myCont.end() - 1)) {
750  myCont.erase(myCont.end() - 1);
751  if (np != *(myCont.end() - 1)) {
752  myCont.push_back(np);
753  assert(myCont.size() > 1);
754  assert(*(myCont.begin()) != *(myCont.end() - 1));
755  }
756  }
757 }
758 
759 
760 SUMOReal
762  Line tmp(getBegin(), getEnd());
763  return tmp.atan2Angle();
764 }
765 
766 
767 void
769  if (i >= 0) {
770  myCont.erase(myCont.begin() + i);
771  } else {
772  myCont.erase(myCont.end() + i);
773  }
774 }
775 
776 
777 SUMOReal
779  SUMOReal shortestDist = -1;
780  SUMOReal nearestPos = -1;
781  SUMOReal seen = 0;
782  for (ContType::const_iterator i = myCont.begin(); i != myCont.end() - 1; i++) {
783  const SUMOReal pos =
784  GeomHelper::nearest_position_on_line_to_point2D(*i, *(i + 1), p, perpendicular);
785  const SUMOReal dist =
786  pos < 0 ? -1 : p.distanceTo2D(positionAtLengthPosition2D(pos + seen));
787  //
788  if (dist >= 0 && (shortestDist < 0 || shortestDist > dist)) {
789  nearestPos = pos + seen;
790  shortestDist = dist;
791  }
792  seen += (*i).distanceTo2D(*(i + 1));
793  //
794  }
795  return nearestPos;
796 }
797 
798 
799 int
801  assert(size() > 0);
803  SUMOReal dist;
804  int closest = 0;
805  for (int i = 1; i < (int)size(); i++) {
806  dist = p.distanceTo(myCont[i]);
807  if (dist < minDist) {
808  closest = i;
809  minDist = dist;
810  }
811  }
812  return closest;
813 }
814 
815 
816 void
818  Position outIntersection = Position();
820  SUMOReal dist;
821  int insertionIndex = 1;
822  for (int i = 1; i < (int)size() - 1; i++) {
823  dist = GeomHelper::closestDistancePointLine(p, myCont[i], myCont[i + 1], outIntersection);
824  if (dist < minDist) {
825  insertionIndex = i + 1;
826  minDist = dist;
827  }
828  }
829  insertAt(insertionIndex, p);
830 }
831 
832 
833 SUMOReal
835  Position outIntersection = Position();
837  for (ContType::const_iterator i = myCont.begin(); i != myCont.end() - 1; i++) {
838  minDist = MIN2(minDist, GeomHelper::closestDistancePointLine(
839  p, *i, *(i + 1), outIntersection));
840  }
841  return minDist;
842 }
843 
844 
845 std::vector<SUMOReal>
847  std::vector<SUMOReal> ret;
848  for (ContType::const_iterator i = other.myCont.begin(); i != other.myCont.end() - 1; i++) {
849  std::vector<SUMOReal> atSegment = intersectsAtLengths2D(Line(*i, *(i+1)));
850  copy(atSegment.begin(), atSegment.end(), back_inserter(ret));
851  }
852  return ret;
853 }
854 
855 
856 std::vector<SUMOReal>
858  std::vector<SUMOReal> ret;
859  SUMOReal pos = 0;
860  for (ContType::const_iterator i = myCont.begin(); i != myCont.end() - 1; i++) {
861  Line l((*i), *(i + 1));
862  if (GeomHelper::intersects(l.p1(), l.p2(), line.p1(), line.p2())) {
863  Position p =
864  GeomHelper::intersection_position2D(l.p1(), l.p2(), line.p1(), line.p2());
865  SUMOReal atLength = p.distanceTo2D(l.p1());
866  ret.push_back(atLength + pos);
867  }
868  pos += l.length2D();
869  }
870  return ret;
871 }
872 
873 
874 void
876  assert(myCont.size() > 1);
877  Position nb =
879  Position ne =
881  myCont[myCont.size() - 2], myCont[myCont.size() - 1], val);
882  myCont.erase(myCont.begin());
883  push_front(nb);
884  myCont.erase(myCont.end() - 1);
885  push_back(ne);
886 }
887 
888 
891  PositionVector ret;
892  for (ContType::const_reverse_iterator i = myCont.rbegin(); i != myCont.rend(); i++) {
893  ret.push_back(*i);
894  }
895  return ret;
896 }
897 
898 
899 void
901  if (myCont.size() < 2) {
902  return;
903  }
904  PositionVector shape;
905  for (size_t i = 0; i < myCont.size(); i++) {
906  if (i == 0) {
907  Position from = myCont[i];
908  Position to = myCont[i + 1];
909  std::pair<SUMOReal, SUMOReal> offsets =
910  GeomHelper::getNormal90D_CW(from, to, amount);
911  shape.push_back(Position(from.x() - offsets.first,
912  from.y() - offsets.second, from.z()));
913  } else if (i == myCont.size() - 1) {
914  Position from = myCont[i - 1];
915  Position to = myCont[i];
916  std::pair<SUMOReal, SUMOReal> offsets =
917  GeomHelper::getNormal90D_CW(from, to, amount);
918  shape.push_back(Position(to.x() - offsets.first,
919  to.y() - offsets.second, to.z()));
920  } else {
921  Position from = myCont[i - 1];
922  Position me = myCont[i];
923  Position to = myCont[i + 1];
924  const double sinAngle = sin(GeomHelper::Angle2D(from.x() - me.x(), from.y() - me.y(),
925  me.x() - to.x(), me.y() - to.y()) / 2);
926  const double maxDev = 2 * (from.distanceTo2D(me) + me.distanceTo2D(to)) * sinAngle;
927  if (fabs(maxDev) < POSITION_EPS) {
928  // parallel case, just shift the middle point
929  std::pair<SUMOReal, SUMOReal> off =
930  GeomHelper::getNormal90D_CW(from, to, amount);
931  shape.push_back(Position(me.x() - off.first, me.y() - off.second, me.z()));
932  continue;
933  }
934  std::pair<SUMOReal, SUMOReal> offsets =
935  GeomHelper::getNormal90D_CW(from, me, amount);
936  std::pair<SUMOReal, SUMOReal> offsets2 =
937  GeomHelper::getNormal90D_CW(me, to, amount);
938  Line l1(
939  Position(from.x() - offsets.first, from.y() - offsets.second),
940  Position(me.x() - offsets.first, me.y() - offsets.second));
941  l1.extrapolateBy(100);
942  Line l2(
943  Position(me.x() - offsets2.first, me.y() - offsets2.second),
944  Position(to.x() - offsets2.first, to.y() - offsets2.second));
945  l2.extrapolateBy(100);
946  if (l1.intersects(l2)) {
947  shape.push_back(l1.intersectsAt(l2));
948  } else {
949  throw InvalidArgument("no line intersection");
950  }
951  }
952  }
953 
954  /*
955  ContType newCont;
956  std::pair<SUMOReal, SUMOReal> p;
957  Position newPos;
958  // first point
959  newPos = (*(myCont.begin()));
960  p = GeomHelper::getNormal90D_CW(*(myCont.begin()), *(myCont.begin()+1), amount);
961  newPos.add(p.first, p.second);
962  newCont.push_back(newPos);
963  // middle points
964  for(ContType::const_iterator i=myCont.begin()+1; i!=myCont.end()-1; i++) {
965  std::pair<SUMOReal, SUMOReal> oldp = p;
966  newPos = *i;
967  newPos.add(p.first, p.second);
968  newCont.push_back(newPos);
969  p = GeomHelper::getNormal90D_CW(*i, *(i+1), amount);
970  // Position newPos(*i);
971  // newPos.add((p.first+oldp.first)/2.0, (p.second+oldp.second)/2.0);
972  // newCont.push_back(newPos);
973  }
974  // last point
975  newPos = (*(myCont.end()-1));
976  newPos.add(p.first, p.second);
977  newCont.push_back(newPos);
978  myCont = newCont;
979  */
980  myCont = shape.myCont;
981 }
982 
983 
984 Line
985 PositionVector::lineAt(size_t pos) const {
986  assert(myCont.size() > pos + 1);
987  return Line(myCont[pos], myCont[pos + 1]);
988 }
989 
990 
991 Line
993  return lineAt(0);
994 }
995 
996 
997 Line
999  return lineAt(myCont.size() - 2);
1000 }
1001 
1002 
1003 void
1005  if (myCont[0] == myCont.back()) {
1006  return;
1007  }
1008  push_back(myCont[0]);
1009 }
1010 
1011 
1012 std::vector<SUMOReal>
1014  std::vector<SUMOReal> ret;
1015  ContType::const_iterator i;
1016  for (i = myCont.begin(); i != myCont.end(); i++) {
1017  ret.push_back(s.distance(*i));
1018  }
1019  for (i = s.myCont.begin(); i != s.myCont.end(); i++) {
1020  ret.push_back(distance(*i));
1021  }
1022  return ret;
1023 }
1024 
1025 
1026 Position
1028  Position last = myCont.back();
1029  myCont.erase(myCont.end() - 1);
1030  return last;
1031 }
1032 
1033 
1034 Position
1036  Position first = myCont.front();
1037  myCont.erase(myCont.begin());
1038  return first;
1039 }
1040 
1041 
1042 void
1043 PositionVector::insertAt(int index, const Position& p) {
1044  if (index >= 0) {
1045  myCont.insert(myCont.begin() + index, p);
1046  } else {
1047  myCont.insert(myCont.end() + index, p);
1048  }
1049 }
1050 
1051 
1052 void
1054  if (size() == 0 || !p.almostSame(myCont.back())) {
1055  myCont.push_back(p);
1056  }
1057 }
1058 
1059 
1060 void
1062  if (size() == 0 || !p.almostSame(myCont.front())) {
1063  myCont.insert(myCont.begin(), p);
1064  }
1065 }
1066 
1067 
1068 void
1069 PositionVector::replaceAt(size_t index, const Position& by) {
1070  assert(size() > index);
1071  myCont[index] = by;
1072 }
1073 
1074 
1075 bool
1077  return myCont.size() >= 2 && myCont[0] == myCont.back();
1078 }
1079 
1080 
1081 void
1083  if (myCont.size() > 1) {
1084  ContType::iterator last = myCont.begin();
1085  for (ContType::iterator i = myCont.begin() + 1; i != myCont.end();) {
1086  if (last->almostSame(*i)) {
1087  i = myCont.erase(i);
1088  } else {
1089  last = i;
1090  ++i;
1091  }
1092  }
1093  }
1094 }
1095 
1096 
1097 void
1099  if (myCont.size() > 2) {
1100  Position& last = myCont.front();
1101  for (ContType::iterator i = myCont.begin() + 1; i != myCont.end() - 1;) {
1102  if (GeomHelper::distancePointLine(*i, last, *(i + 1)) < 0.001) {
1103  i = myCont.erase(i);
1104  } else {
1105  last = *i;
1106  ++i;
1107  }
1108  }
1109  }
1110 }
1111 
1112 
1113 bool
1115  if (size() == v2.size()) {
1116  for (int i = 0; i < (int)size(); i++) {
1117  if ((*this)[i] != v2[i]) {
1118  return false;
1119  }
1120  }
1121  return true;
1122  } else {
1123  return false;
1124  }
1125 }
1126 
1127 
1128 
1129 /****************************************************************************/
1130