SUMO - Simulation of Urban MObility
PositionVector.cpp
Go to the documentation of this file.
00001 /****************************************************************************/
00010 // A list of positions
00011 /****************************************************************************/
00012 // SUMO, Simulation of Urban MObility; see http://sumo.sourceforge.net/
00013 // Copyright (C) 2001-2012 DLR (http://www.dlr.de/) and contributors
00014 /****************************************************************************/
00015 //
00016 //   This file is part of SUMO.
00017 //   SUMO is free software: you can redistribute it and/or modify
00018 //   it under the terms of the GNU General Public License as published by
00019 //   the Free Software Foundation, either version 3 of the License, or
00020 //   (at your option) any later version.
00021 //
00022 /****************************************************************************/
00023 
00024 
00025 // ===========================================================================
00026 // included modules
00027 // ===========================================================================
00028 #ifdef _MSC_VER
00029 #include <windows_config.h>
00030 #else
00031 #include <config.h>
00032 #endif
00033 
00034 #include <queue>
00035 #include <cmath>
00036 #include <iostream>
00037 #include <algorithm>
00038 #include <cassert>
00039 #include <iterator>
00040 #include <limits>
00041 #include <utils/common/StdDefs.h>
00042 #include <utils/common/MsgHandler.h>
00043 #include <utils/common/ToString.h>
00044 #include "AbstractPoly.h"
00045 #include "Position.h"
00046 #include "PositionVector.h"
00047 #include "GeomHelper.h"
00048 #include "Line.h"
00049 #include "Helper_ConvexHull.h"
00050 #include "Boundary.h"
00051 
00052 #ifdef CHECK_MEMORY_LEAKS
00053 #include <foreign/nvwa/debug_new.h>
00054 #endif // CHECK_MEMORY_LEAKS
00055 
00056 
00057 // ===========================================================================
00058 // method definitions
00059 // ===========================================================================
00060 PositionVector::PositionVector() {}
00061 
00062 
00063 PositionVector::PositionVector(const std::vector<Position> &v) {
00064     std::copy(v.begin(), v.end(), std::back_inserter(myCont));
00065 }
00066 
00067 
00068 PositionVector::~PositionVector() {}
00069 
00070 
00071 // ------------ Adding items to the container
00072 void
00073 PositionVector::push_back(const Position& p) {
00074     myCont.push_back(p);
00075 }
00076 
00077 
00078 void
00079 PositionVector::push_back(const PositionVector& p) {
00080     copy(p.myCont.begin(), p.myCont.end(), back_inserter(myCont));
00081 }
00082 
00083 
00084 void
00085 PositionVector::push_front(const Position& p) {
00086     myCont.insert(myCont.begin(), p);
00087 }
00088 
00089 
00090 bool
00091 PositionVector::around(const Position& p, SUMOReal offset) const {
00092     if (offset != 0) {
00093         //throw 1; // !!! not yet implemented
00094     }
00095     SUMOReal angle = 0;
00096     for (ContType::const_iterator i = myCont.begin(); i != myCont.end() - 1; i++) {
00097         Position p1(
00098             (*i).x() - p.x(),
00099             (*i).y() - p.y());
00100         Position p2(
00101             (*(i + 1)).x() - p.x(),
00102             (*(i + 1)).y() - p.y());
00103         angle += GeomHelper::Angle2D(p1.x(), p1.y(), p2.x(), p2.y());
00104     }
00105     Position p1(
00106         (*(myCont.end() - 1)).x() - p.x(),
00107         (*(myCont.end() - 1)).y() - p.y());
00108     Position p2(
00109         (*(myCont.begin())).x() - p.x(),
00110         (*(myCont.begin())).y() - p.y());
00111     angle += GeomHelper::Angle2D(p1.x(), p1.y(), p2.x(), p2.y());
00112     return (!(fabs(angle) < PI));
00113 }
00114 
00115 
00116 bool
00117 PositionVector::overlapsWith(const AbstractPoly& poly, SUMOReal offset) const {
00118     for (ContType::const_iterator i = myCont.begin(); i != myCont.end() - 1; i++) {
00119         if (poly.around(*i, offset)) {
00120             return true;
00121         }
00122     }
00123     return false;
00124 }
00125 
00126 
00127 bool
00128 PositionVector::intersects(const Position& p1, const Position& p2) const {
00129     if (size() < 2) {
00130         return false;
00131     }
00132     for (ContType::const_iterator i = myCont.begin(); i != myCont.end() - 1; i++) {
00133         if (GeomHelper::intersects(*i, *(i + 1), p1, p2)) {
00134             return true;
00135         }
00136     }
00137     //return GeomHelper::intersects(*(myCont.end()-1), *(myCont.begin()), p1, p2);
00138     return false;
00139 }
00140 
00141 
00142 bool
00143 PositionVector::intersects(const PositionVector& v1) const {
00144     if (size() < 2) {
00145         return false;
00146     }
00147     for (ContType::const_iterator i = myCont.begin(); i != myCont.end() - 1; i++) {
00148         if (v1.intersects(*i, *(i + 1))) {
00149             return true;
00150         }
00151     }
00152     //return v1.intersects(*(myCont.end()-1), *(myCont.begin()));
00153     return false;
00154 }
00155 
00156 
00157 Position
00158 PositionVector::intersectsAtPoint(const Position& p1,
00159                                   const Position& p2) const {
00160     for (ContType::const_iterator i = myCont.begin(); i != myCont.end() - 1; i++) {
00161         if (GeomHelper::intersects(*i, *(i + 1), p1, p2)) {
00162             return GeomHelper::intersection_position2D(*i, *(i + 1), p1, p2);
00163         }
00164     }
00165     return Position(-1, -1);
00166 }
00167 
00168 
00169 Position
00170 PositionVector::intersectsAtPoint(const PositionVector& v1) const {
00171     for (ContType::const_iterator i = myCont.begin(); i != myCont.end() - 1; i++) {
00172         if (v1.intersects(*i, *(i + 1))) {
00173             return v1.intersectsAtPoint(*i, *(i + 1));
00174         }
00175     }
00176     /*
00177     if(v1.intersects(*(myCont.end()-1), *(myCont.begin()))) {
00178         return v1.intersectsAtPoint(*(myCont.end()-1), *(myCont.begin()));
00179     }
00180     */
00181     return Position(-1, -1);
00182 }
00183 
00184 
00185 void
00186 PositionVector::clear() {
00187     myCont.clear();
00188 }
00189 
00190 
00191 const Position&
00192 PositionVector::operator[](int index) const {
00193     if (index >= 0) {
00194         return myCont[index];
00195     } else {
00196         return myCont[myCont.size() + index];
00197     }
00198 }
00199 
00200 
00201 Position&
00202 PositionVector::operator[](int index) {
00203     if (index >= 0) {
00204         return myCont[index];
00205     } else {
00206         return myCont[myCont.size() + index];
00207     }
00208 }
00209 
00210 
00211 size_t
00212 PositionVector::size() const {
00213     return myCont.size();
00214 }
00215 
00216 
00217 Position
00218 PositionVector::positionAtLengthPosition(SUMOReal pos) const {
00219     ContType::const_iterator i = myCont.begin();
00220     SUMOReal seenLength = 0;
00221     do {
00222         const SUMOReal nextLength = (*i).distanceTo(*(i + 1));
00223         if (seenLength + nextLength > pos) {
00224             return positionAtLengthPosition(*i, *(i + 1), pos - seenLength);
00225         }
00226         seenLength += nextLength;
00227     } while (++i != myCont.end() - 1);
00228     return myCont.back();
00229 }
00230 
00231 
00232 Position
00233 PositionVector::positionAtLengthPosition2D(SUMOReal pos) const {
00234     ContType::const_iterator i = myCont.begin();
00235     SUMOReal seenLength = 0;
00236     do {
00237         const SUMOReal nextLength = (*i).distanceTo2D(*(i + 1));
00238         if (seenLength + nextLength > pos) {
00239             return positionAtLengthPosition2D(*i, *(i + 1), pos - seenLength);
00240         }
00241         seenLength += nextLength;
00242     } while (++i != myCont.end() - 1);
00243     return myCont.back();
00244 }
00245 
00246 
00247 SUMOReal
00248 PositionVector::rotationDegreeAtLengthPosition(SUMOReal pos) const {
00249     ContType::const_iterator i = myCont.begin();
00250     SUMOReal seenLength = 0;
00251     do {
00252         SUMOReal nextLength = (*i).distanceTo(*(i + 1));
00253         if (seenLength + nextLength > pos) {
00254             Line l(*i, *(i + 1));
00255             return l.atan2DegreeAngle();
00256         }
00257         seenLength += nextLength;
00258     } while (++i != myCont.end() - 1);
00259     Line l(*(myCont.end() - 2), *(myCont.end() - 1));
00260     return l.atan2DegreeAngle();
00261 }
00262 
00263 
00264 Position
00265 PositionVector::positionAtLengthPosition(const Position& p1,
00266         const Position& p2,
00267         SUMOReal pos) {
00268     const SUMOReal dist = p1.distanceTo(p2);
00269     if (dist < pos) {
00270         return Position(-1, -1);
00271     }
00272     return p1 + (p2 - p1) * (pos / dist);
00273 }
00274 
00275 
00276 Position
00277 PositionVector::positionAtLengthPosition2D(const Position& p1,
00278         const Position& p2,
00279         SUMOReal pos) {
00280     const SUMOReal dist = p1.distanceTo2D(p2);
00281     if (dist < pos) {
00282         return Position(-1, -1);
00283     }
00284     return p1 + (p2 - p1) * (pos / dist);
00285 }
00286 
00287 
00288 Boundary
00289 PositionVector::getBoxBoundary() const {
00290     Boundary ret;
00291     for (ContType::const_iterator i = myCont.begin(); i != myCont.end(); i++) {
00292         ret.add(*i);
00293     }
00294     return ret;
00295 }
00296 
00297 
00298 Position
00299 PositionVector::getPolygonCenter() const {
00300     SUMOReal x = 0;
00301     SUMOReal y = 0;
00302     for (ContType::const_iterator i = myCont.begin(); i != myCont.end(); i++) {
00303         x += (*i).x();
00304         y += (*i).y();
00305     }
00306     return Position(x / (SUMOReal) myCont.size(), y / (SUMOReal) myCont.size());
00307 }
00308 
00309 
00310 Position
00311 PositionVector::getCentroid() const {
00312     PositionVector tmp = *this;
00313     if (!isClosed()) { // make sure its closed
00314         tmp.push_back(tmp[0]);
00315     }
00316     const int endIndex = (int)tmp.size() - 1;
00317     SUMOReal div = 0; // 6 * area including sign
00318     SUMOReal x = 0;
00319     SUMOReal y = 0;
00320     if (tmp.area() != 0) { // numerical instability ?
00321         // http://en.wikipedia.org/wiki/Polygon
00322         for (int i = 0; i < endIndex; i++) {
00323             const SUMOReal z = tmp[i].x() * tmp[i + 1].y() - tmp[i + 1].x() * tmp[i].y();
00324             div += z; // area formula
00325             x += (tmp[i].x() + tmp[i + 1].x()) * z;
00326             y += (tmp[i].y() + tmp[i + 1].y()) * z;
00327         }
00328         div *= 3; //  6 / 2, the 2 comes from the area formula
00329         return Position(x / div, y / div);
00330     } else {
00331         // compute by decomposing into line segments
00332         // http://en.wikipedia.org/wiki/Centroid#By_geometric_decomposition
00333         SUMOReal lengthSum = 0;
00334         for (int i = 0; i < endIndex; i++) {
00335             SUMOReal length = tmp[i].distanceTo(tmp[i + 1]);
00336             x += (tmp[i].x() + tmp[i + 1].x()) * length / 2;
00337             y += (tmp[i].y() + tmp[i + 1].y()) * length / 2;
00338             lengthSum += length;
00339         }
00340         return Position(x / lengthSum, y / lengthSum);
00341     }
00342 }
00343 
00344 
00345 void
00346 PositionVector::scaleSize(SUMOReal factor) {
00347     Position centroid = getCentroid();
00348     for (size_t i = 0; i < size(); i++) {
00349         myCont[i] = centroid + ((myCont[i] - centroid) * factor);
00350     }
00351 }
00352 
00353 
00354 Position
00355 PositionVector::getLineCenter() const {
00356     if (myCont.size() == 1) {
00357         return myCont[0];
00358     }
00359     return positionAtLengthPosition(SUMOReal((length() / 2.)));
00360 }
00361 
00362 
00363 SUMOReal
00364 PositionVector::length() const {
00365     SUMOReal len = 0;
00366     for (ContType::const_iterator i = myCont.begin(); i != myCont.end() - 1; i++) {
00367         len += (*i).distanceTo(*(i + 1));
00368     }
00369     return len;
00370 }
00371 
00372 
00373 SUMOReal
00374 PositionVector::area() const {
00375     SUMOReal area = 0;
00376     PositionVector tmp = *this;
00377     if (!isClosed()) { // make sure its closed
00378         tmp.push_back(tmp[0]);
00379     }
00380     const int endIndex = (int)tmp.size() - 1;
00381     // http://en.wikipedia.org/wiki/Polygon
00382     for (int i = 0; i < endIndex; i++) {
00383         area += tmp[i].x() * tmp[i + 1].y() - tmp[i + 1].x() * tmp[i].y();
00384     }
00385     if (area < 0) { // we whether we had cw or ccw order
00386         area *= -1;
00387     }
00388     return area / 2;
00389 }
00390 
00391 
00392 bool
00393 PositionVector::partialWithin(const AbstractPoly& poly, SUMOReal offset) const {
00394     for (ContType::const_iterator i = myCont.begin(); i != myCont.end() - 1; i++) {
00395         if (poly.around(*i, offset)) {
00396             return true;
00397         }
00398     }
00399     return false;
00400 }
00401 
00402 
00403 
00404 bool
00405 PositionVector::crosses(const Position& p1, const Position& p2) const {
00406     return intersects(p1, p2);
00407 }
00408 
00409 
00410 
00411 const Position&
00412 PositionVector::getBegin() const {
00413     return myCont[0];
00414 }
00415 
00416 
00417 const Position&
00418 PositionVector::getEnd() const {
00419     return myCont.back();
00420 }
00421 
00422 
00423 std::pair<PositionVector, PositionVector>
00424 PositionVector::splitAt(SUMOReal where) const {
00425     if (size() < 2) {
00426         throw InvalidArgument("Vector to short for splitting");
00427     }
00428     if (where <= POSITION_EPS || where >= length() - POSITION_EPS) {
00429         WRITE_WARNING("Splitting vector close to end (pos: " + toString(where) + ", length: " + toString(length()) + ")");
00430     }
00431     PositionVector first, second;
00432     first.push_back(myCont[0]);
00433     SUMOReal seen = 0;
00434     ContType::const_iterator it = myCont.begin() + 1;
00435     SUMOReal next = first.getEnd().distanceTo(*it);
00436     // see how many points we can add to first
00437     while (where >= seen + next + POSITION_EPS) {
00438         seen += next;
00439         first.push_back(*it);
00440         it++;
00441         next = first.getEnd().distanceTo(*it);
00442     }
00443     if (fabs(where - (seen + next)) > POSITION_EPS || it == myCont.end() - 1) {
00444         // we need to insert a new point because 'where' is not close to an
00445         // existing point or it is to close to the endpoint
00446         Line tmpL(first.getEnd(), *it);
00447         Position p = tmpL.getPositionAtDistance(where - seen);
00448         first.push_back(p);
00449         second.push_back(p);
00450     } else {
00451         first.push_back(*it);
00452     }
00453     // add the remaining points to second
00454     for (; it != myCont.end(); it++) {
00455         second.push_back(*it);
00456     }
00457     assert(first.size() >= 2);
00458     assert(second.size() >= 2);
00459     assert(first.getEnd() == second.getBegin());
00460     assert(fabs(first.length() + second.length() - length()) < 2 * POSITION_EPS);
00461     return std::pair<PositionVector, PositionVector>(first, second);
00462 }
00463 
00464 
00465 std::ostream&
00466 operator<<(std::ostream& os, const PositionVector& geom) {
00467     for (PositionVector::ContType::const_iterator i = geom.myCont.begin(); i != geom.myCont.end(); i++) {
00468         if (i != geom.myCont.begin()) {
00469             os << " ";
00470         }
00471         os << (*i);
00472     }
00473     return os;
00474 }
00475 
00476 
00477 void
00478 PositionVector::sortAsPolyCWByAngle() {
00479     std::sort(myCont.begin(), myCont.end(), as_poly_cw_sorter());
00480 }
00481 
00482 
00483 void
00484 PositionVector::add(SUMOReal xoff, SUMOReal yoff, SUMOReal zoff) {
00485     for (size_t i = 0; i < size(); i++) {
00486         myCont[i].add(xoff, yoff, zoff);
00487     }
00488 }
00489 
00490 
00491 void
00492 PositionVector::reshiftRotate(SUMOReal xoff, SUMOReal yoff, SUMOReal rot) {
00493     for (size_t i = 0; i < size(); i++) {
00494         myCont[i].reshiftRotate(xoff, yoff, rot);
00495     }
00496 }
00497 
00498 
00499 int
00500 PositionVector::as_poly_cw_sorter::operator()(const Position& p1,
00501         const Position& p2) const {
00502     return atan2(p1.x(), p1.y()) < atan2(p2.x(), p2.y());
00503 }
00504 
00505 
00506 
00507 void
00508 PositionVector::sortByIncreasingXY() {
00509     std::sort(myCont.begin(), myCont.end(), increasing_x_y_sorter());
00510 }
00511 
00512 
00513 
00514 PositionVector::increasing_x_y_sorter::increasing_x_y_sorter() {}
00515 
00516 
00517 
00518 int
00519 PositionVector::increasing_x_y_sorter::operator()(const Position& p1,
00520         const Position& p2) const {
00521     if (p1.x() != p2.x()) {
00522         return p1.x() < p2.x();
00523     }
00524     return p1.y() < p2.y();
00525 }
00526 
00527 
00528 
00529 SUMOReal
00530 PositionVector::isLeft(const Position& P0, const Position& P1,
00531                        const Position& P2) const {
00532     return (P1.x() - P0.x()) * (P2.y() - P0.y()) - (P2.x() - P0.x()) * (P1.y() - P0.y());
00533 }
00534 
00535 
00536 PositionVector
00537 PositionVector::convexHull() const {
00538     PositionVector ret = *this;
00539     ret.sortAsPolyCWByAngle();
00540     return simpleHull_2D(ret);
00541 }
00542 
00543 
00544 void
00545 PositionVector::set(size_t pos, const Position& p) {
00546     myCont[pos] = p;
00547 }
00548 
00549 
00550 PositionVector 
00551 PositionVector::intersectionPoints2D(const Line& line) const {
00552     PositionVector ret;
00553     for (ContType::const_iterator i = myCont.begin(); i != myCont.end() - 1; i++) {
00554         if (GeomHelper::intersects(*i, *(i + 1), line.p1(), line.p2())) {
00555             ret.push_back_noDoublePos(GeomHelper::intersection_position2D(
00556                         *i, *(i + 1), line.p1(), line.p2()));
00557         }
00558     }
00559     return ret;
00560 }
00561 
00562 
00563 int
00564 PositionVector::appendWithCrossingPoint(const PositionVector& v) {
00565     if (myCont.back().distanceTo(v.myCont[0]) < 2) { // !!! heuristic
00566         copy(v.myCont.begin() + 1, v.myCont.end(), back_inserter(myCont));
00567         return 1;
00568     }
00569     //
00570     Line l1(myCont[myCont.size() - 2], myCont.back());
00571     l1.extrapolateBy(100);
00572     Line l2(v.myCont[0], v.myCont[1]);
00573     l2.extrapolateBy(100);
00574     if (l1.intersects(l2) && l1.intersectsAtLength2D(l2) < l1.length2D() - 100) { // !!! heuristic
00575         Position p = l1.intersectsAt(l2);
00576         myCont[myCont.size() - 1] = p;
00577         copy(v.myCont.begin() + 1, v.myCont.end(), back_inserter(myCont));
00578         return 2;
00579     } else {
00580         copy(v.myCont.begin(), v.myCont.end(), back_inserter(myCont));
00581         return 3;
00582     }
00583 }
00584 
00585 
00586 PositionVector
00587 PositionVector::getSubpart(SUMOReal begin, SUMOReal end) const {
00588     PositionVector ret;
00589     Position begPos = myCont.front();
00590     if (begin > POSITION_EPS) {
00591         begPos = positionAtLengthPosition(begin);
00592     }
00593     Position endPos = myCont.back();
00594     if (end < length() - POSITION_EPS) {
00595         endPos = positionAtLengthPosition(end);
00596     }
00597     ret.push_back(begPos);
00598 
00599     SUMOReal seen = 0;
00600     ContType::const_iterator i = myCont.begin();
00601     // skip previous segments
00602     while ((i + 1) != myCont.end()
00603             &&
00604             seen + (*i).distanceTo(*(i + 1)) < begin) {
00605         seen += (*i).distanceTo(*(i + 1));
00606         i++;
00607     }
00608     // append segments in between
00609     while ((i + 1) != myCont.end()
00610             &&
00611             seen + (*i).distanceTo(*(i + 1)) < end) {
00612 
00613         ret.push_back_noDoublePos(*(i + 1));
00614         /*
00615         if(ret.at(-1)!=*(i+1)) {
00616             ret.push_back(*(i+1));
00617         }
00618         */
00619         seen += (*i).distanceTo(*(i + 1));
00620         i++;
00621     }
00622     // append end
00623     ret.push_back_noDoublePos(endPos);
00624     return ret;
00625 }
00626 
00627 
00628 PositionVector
00629 PositionVector::getSubpart2D(SUMOReal begin, SUMOReal end) const {
00630     PositionVector ret;
00631     Position begPos = myCont.front();
00632     if (begin > POSITION_EPS) {
00633         begPos = positionAtLengthPosition2D(begin);
00634     }
00635     Position endPos = myCont.back();
00636     if (end < length() - POSITION_EPS) {
00637         endPos = positionAtLengthPosition2D(end);
00638     }
00639     ret.push_back(begPos);
00640 
00641     SUMOReal seen = 0;
00642     ContType::const_iterator i = myCont.begin();
00643     // skip previous segments
00644     while ((i + 1) != myCont.end()
00645             &&
00646             seen + (*i).distanceTo2D(*(i + 1)) < begin) {
00647         seen += (*i).distanceTo2D(*(i + 1));
00648         i++;
00649     }
00650     // append segments in between
00651     while ((i + 1) != myCont.end()
00652             &&
00653             seen + (*i).distanceTo2D(*(i + 1)) < end) {
00654 
00655         ret.push_back_noDoublePos(*(i + 1));
00656         /*
00657         if(ret.at(-1)!=*(i+1)) {
00658             ret.push_back(*(i+1));
00659         }
00660         */
00661         seen += (*i).distanceTo2D(*(i + 1));
00662         i++;
00663     }
00664     // append end
00665     ret.push_back_noDoublePos(endPos);
00666     return ret;
00667 }
00668 
00669 
00670 void
00671 PositionVector::pruneFromBeginAt(const Position& p) {
00672     // find minimum distance (from the begin)
00673     size_t pos = 0;
00674     SUMOReal dist = 1000000;
00675     size_t currPos = 0;
00676     SUMOReal currDist = GeomHelper::distancePointLine(p,
00677                         GeomHelper::extrapolate_first(*(myCont.begin()), *(myCont.begin() + 1), 100),
00678                         *(myCont.begin() + 1));
00679 //    assert(currDist>=0);
00680     if (currDist >= 0 && currDist < dist) {
00681         dist = currDist;
00682         pos = currPos;
00683     }
00684 
00685     for (ContType::iterator i = myCont.begin(); i != myCont.end() - 1; i++, currPos++) {
00686         SUMOReal currDist = GeomHelper::distancePointLine(p, *i, *(i + 1));
00687         if (currDist >= 0 && currDist < dist) {
00688             dist = currDist;
00689             pos = currPos;
00690         }
00691     }
00692     // remove leading items
00693     for (size_t j = 0; j < pos; j++) {
00694         myCont.erase(myCont.begin());
00695     }
00696     // replace first item by the new position
00697     SUMOReal lpos = GeomHelper::nearest_position_on_line_to_point2D(
00698                         myCont[0], myCont[1], p);
00699     if (lpos == -1) {
00700         return;
00701     }
00702     Position np = positionAtLengthPosition(lpos);
00703     if (np != *(myCont.begin())) {
00704         myCont.erase(myCont.begin());
00705         if (np != *(myCont.begin())) {
00706             myCont.insert(myCont.begin(), p);
00707             assert(myCont.size() > 1);
00708             assert(*(myCont.begin()) != *(myCont.end() - 1));
00709         }
00710     }
00711 }
00712 
00713 
00714 void
00715 PositionVector::pruneFromEndAt(const Position& p) {
00716     // find minimum distance (from the end)
00717     size_t pos = 0;
00718     SUMOReal dist = 1000000;
00719     size_t currPos = 0;
00720     SUMOReal currDist = GeomHelper::distancePointLine(p,
00721                         *(myCont.end() - 1),
00722                         GeomHelper::extrapolate_second(*(myCont.end() - 2), *(myCont.end() - 1), 100));
00723 //    assert(currDist>=0);
00724     if (currDist >= 0 && currDist < dist) {
00725         dist = currDist;
00726         pos = currPos;
00727     }
00728 
00729     for (ContType::reverse_iterator i = myCont.rbegin(); i != myCont.rend() - 1; i++, currPos++) {
00730         SUMOReal currDist = GeomHelper::distancePointLine(p, *(i), *(i + 1));
00731         if (currDist >= 0 && currDist < dist) {
00732             dist = currDist;
00733             pos = currPos;
00734         }
00735     }
00736     // remove trailing items
00737     for (size_t j = 0; j < pos; j++) {
00738         myCont.erase(myCont.end() - 1);
00739     }
00740     // replace last item by the new position
00741     SUMOReal lpos =
00742         GeomHelper::nearest_position_on_line_to_point2D(
00743             myCont[myCont.size() - 1], myCont[myCont.size() - 2], p);
00744     if (lpos == -1) {
00745         return;
00746     }
00747     Position np = positionAtLengthPosition(
00748                       length() - lpos);
00749     if (np != *(myCont.end() - 1)) {
00750         myCont.erase(myCont.end() - 1);
00751         if (np != *(myCont.end() - 1)) {
00752             myCont.push_back(np);
00753             assert(myCont.size() > 1);
00754             assert(*(myCont.begin()) != *(myCont.end() - 1));
00755         }
00756     }
00757 }
00758 
00759 
00760 SUMOReal
00761 PositionVector::beginEndAngle() const {
00762     Line tmp(getBegin(), getEnd());
00763     return tmp.atan2Angle();
00764 }
00765 
00766 
00767 void
00768 PositionVector::eraseAt(int i) {
00769     if (i >= 0) {
00770         myCont.erase(myCont.begin() + i);
00771     } else {
00772         myCont.erase(myCont.end() + i);
00773     }
00774 }
00775 
00776 
00777 SUMOReal
00778 PositionVector::nearest_position_on_line_to_point2D(const Position& p, bool perpendicular) const {
00779     SUMOReal shortestDist = -1;
00780     SUMOReal nearestPos = -1;
00781     SUMOReal seen = 0;
00782     for (ContType::const_iterator i = myCont.begin(); i != myCont.end() - 1; i++) {
00783         const SUMOReal pos =
00784             GeomHelper::nearest_position_on_line_to_point2D(*i, *(i + 1), p, perpendicular);
00785         const SUMOReal dist =
00786             pos < 0 ? -1 : p.distanceTo2D(positionAtLengthPosition2D(pos + seen));
00787         //
00788         if (dist >= 0 && (shortestDist < 0 || shortestDist > dist)) {
00789             nearestPos = pos + seen;
00790             shortestDist = dist;
00791         }
00792         seen += (*i).distanceTo2D(*(i + 1));
00793         //
00794     }
00795     return nearestPos;
00796 }
00797 
00798 
00799 int
00800 PositionVector::indexOfClosest(const Position& p) const {
00801     assert(size() > 0);
00802     SUMOReal minDist = std::numeric_limits<SUMOReal>::max();
00803     SUMOReal dist;
00804     int closest = 0;
00805     for (int i = 1; i < (int)size(); i++) {
00806         dist = p.distanceTo(myCont[i]);
00807         if (dist < minDist) {
00808             closest = i;
00809             minDist = dist;
00810         }
00811     }
00812     return closest;
00813 }
00814 
00815 
00816 void
00817 PositionVector::insertAtClosest(const Position& p) {
00818     Position outIntersection = Position();
00819     SUMOReal minDist = std::numeric_limits<SUMOReal>::max();
00820     SUMOReal dist;
00821     int insertionIndex = 1;
00822     for (int i = 1; i < (int)size() - 1; i++) {
00823         dist = GeomHelper::closestDistancePointLine(p, myCont[i], myCont[i + 1], outIntersection);
00824         if (dist < minDist) {
00825             insertionIndex = i + 1;
00826             minDist = dist;
00827         }
00828     }
00829     insertAt(insertionIndex, p);
00830 }
00831 
00832 
00833 SUMOReal
00834 PositionVector::distance(const Position& p) const {
00835     Position outIntersection = Position();
00836     SUMOReal minDist = std::numeric_limits<double>::max();
00837     for (ContType::const_iterator i = myCont.begin(); i != myCont.end() - 1; i++) {
00838         minDist = MIN2(minDist, GeomHelper::closestDistancePointLine(
00839                            p, *i, *(i + 1), outIntersection));
00840     }
00841     return minDist;
00842 }
00843 
00844 
00845 std::vector<SUMOReal>
00846 PositionVector::intersectsAtLengths2D(const PositionVector& other) const {
00847     std::vector<SUMOReal> ret;
00848     for (ContType::const_iterator i = other.myCont.begin(); i != other.myCont.end() - 1; i++) {
00849         std::vector<SUMOReal> atSegment = intersectsAtLengths2D(Line(*i, *(i+1)));
00850         copy(atSegment.begin(), atSegment.end(), back_inserter(ret));
00851     }
00852     return ret;
00853 }
00854 
00855 
00856 std::vector<SUMOReal>
00857 PositionVector::intersectsAtLengths2D(const Line& line) const {
00858     std::vector<SUMOReal> ret;
00859     SUMOReal pos = 0;
00860     for (ContType::const_iterator i = myCont.begin(); i != myCont.end() - 1; i++) {
00861         Line l((*i), *(i + 1));
00862         if (GeomHelper::intersects(l.p1(), l.p2(), line.p1(), line.p2())) {
00863             Position p =
00864                 GeomHelper::intersection_position2D(l.p1(), l.p2(), line.p1(), line.p2());
00865             SUMOReal atLength = p.distanceTo2D(l.p1());
00866             ret.push_back(atLength + pos);
00867         }
00868         pos += l.length2D();
00869     }
00870     return ret;
00871 }
00872 
00873 
00874 void
00875 PositionVector::extrapolate(SUMOReal val) {
00876     assert(myCont.size() > 1);
00877     Position nb =
00878         GeomHelper::extrapolate_first(myCont[0], myCont[1], val);
00879     Position ne =
00880         GeomHelper::extrapolate_second(
00881             myCont[myCont.size() - 2], myCont[myCont.size() - 1], val);
00882     myCont.erase(myCont.begin());
00883     push_front(nb);
00884     myCont.erase(myCont.end() - 1);
00885     push_back(ne);
00886 }
00887 
00888 
00889 PositionVector
00890 PositionVector::reverse() const {
00891     PositionVector ret;
00892     for (ContType::const_reverse_iterator i = myCont.rbegin(); i != myCont.rend(); i++) {
00893         ret.push_back(*i);
00894     }
00895     return ret;
00896 }
00897 
00898 
00899 void
00900 PositionVector::move2side(SUMOReal amount) {
00901     if (myCont.size() < 2) {
00902         return;
00903     }
00904     PositionVector shape;
00905     for (size_t i = 0; i < myCont.size(); i++) {
00906         if (i == 0) {
00907             Position from = myCont[i];
00908             Position to = myCont[i + 1];
00909             std::pair<SUMOReal, SUMOReal> offsets =
00910                 GeomHelper::getNormal90D_CW(from, to, amount);
00911             shape.push_back(Position(from.x() - offsets.first,
00912                                      from.y() - offsets.second, from.z()));
00913         } else if (i == myCont.size() - 1) {
00914             Position from = myCont[i - 1];
00915             Position to = myCont[i];
00916             std::pair<SUMOReal, SUMOReal> offsets =
00917                 GeomHelper::getNormal90D_CW(from, to, amount);
00918             shape.push_back(Position(to.x() - offsets.first,
00919                                      to.y() - offsets.second, to.z()));
00920         } else {
00921             Position from = myCont[i - 1];
00922             Position me = myCont[i];
00923             Position to = myCont[i + 1];
00924             const double sinAngle = sin(GeomHelper::Angle2D(from.x() - me.x(), from.y() - me.y(),
00925                                         me.x() - to.x(), me.y() - to.y()) / 2);
00926             const double maxDev = 2 * (from.distanceTo2D(me) + me.distanceTo2D(to)) * sinAngle;
00927             if (fabs(maxDev) < POSITION_EPS) {
00928                 // parallel case, just shift the middle point
00929                 std::pair<SUMOReal, SUMOReal> off =
00930                     GeomHelper::getNormal90D_CW(from, to, amount);
00931                 shape.push_back(Position(me.x() - off.first, me.y() - off.second, me.z()));
00932                 continue;
00933             }
00934             std::pair<SUMOReal, SUMOReal> offsets =
00935                 GeomHelper::getNormal90D_CW(from, me, amount);
00936             std::pair<SUMOReal, SUMOReal> offsets2 =
00937                 GeomHelper::getNormal90D_CW(me, to, amount);
00938             Line l1(
00939                 Position(from.x() - offsets.first, from.y() - offsets.second),
00940                 Position(me.x() - offsets.first, me.y() - offsets.second));
00941             l1.extrapolateBy(100);
00942             Line l2(
00943                 Position(me.x() - offsets2.first, me.y() - offsets2.second),
00944                 Position(to.x() - offsets2.first, to.y() - offsets2.second));
00945             l2.extrapolateBy(100);
00946             if (l1.intersects(l2)) {
00947                 shape.push_back(l1.intersectsAt(l2));
00948             } else {
00949                 throw InvalidArgument("no line intersection");
00950             }
00951         }
00952     }
00953 
00954     /*
00955     ContType newCont;
00956     std::pair<SUMOReal, SUMOReal> p;
00957     Position newPos;
00958     // first point
00959     newPos = (*(myCont.begin()));
00960     p = GeomHelper::getNormal90D_CW(*(myCont.begin()), *(myCont.begin()+1), amount);
00961     newPos.add(p.first, p.second);
00962     newCont.push_back(newPos);
00963     // middle points
00964     for(ContType::const_iterator i=myCont.begin()+1; i!=myCont.end()-1; i++) {
00965         std::pair<SUMOReal, SUMOReal> oldp = p;
00966         newPos = *i;
00967         newPos.add(p.first, p.second);
00968         newCont.push_back(newPos);
00969         p = GeomHelper::getNormal90D_CW(*i, *(i+1), amount);
00970     //        Position newPos(*i);
00971     //        newPos.add((p.first+oldp.first)/2.0, (p.second+oldp.second)/2.0);
00972     //        newCont.push_back(newPos);
00973     }
00974     // last point
00975     newPos = (*(myCont.end()-1));
00976     newPos.add(p.first, p.second);
00977     newCont.push_back(newPos);
00978     myCont = newCont;
00979     */
00980     myCont = shape.myCont;
00981 }
00982 
00983 
00984 Line
00985 PositionVector::lineAt(size_t pos) const {
00986     assert(myCont.size() > pos + 1);
00987     return Line(myCont[pos], myCont[pos + 1]);
00988 }
00989 
00990 
00991 Line
00992 PositionVector::getBegLine() const {
00993     return lineAt(0);
00994 }
00995 
00996 
00997 Line
00998 PositionVector::getEndLine() const {
00999     return lineAt(myCont.size() - 2);
01000 }
01001 
01002 
01003 void
01004 PositionVector::closePolygon() {
01005     if (myCont[0] == myCont.back()) {
01006         return;
01007     }
01008     push_back(myCont[0]);
01009 }
01010 
01011 
01012 std::vector<SUMOReal>
01013 PositionVector::distances(const PositionVector& s) const {
01014     std::vector<SUMOReal> ret;
01015     ContType::const_iterator i;
01016     for (i = myCont.begin(); i != myCont.end(); i++) {
01017         ret.push_back(s.distance(*i));
01018     }
01019     for (i = s.myCont.begin(); i != s.myCont.end(); i++) {
01020         ret.push_back(distance(*i));
01021     }
01022     return ret;
01023 }
01024 
01025 
01026 Position
01027 PositionVector::pop_back() {
01028     Position last = myCont.back();
01029     myCont.erase(myCont.end() - 1);
01030     return last;
01031 }
01032 
01033 
01034 Position
01035 PositionVector::pop_front() {
01036     Position first = myCont.front();
01037     myCont.erase(myCont.begin());
01038     return first;
01039 }
01040 
01041 
01042 void
01043 PositionVector::insertAt(int index, const Position& p) {
01044     if (index >= 0) {
01045         myCont.insert(myCont.begin() + index, p);
01046     } else {
01047         myCont.insert(myCont.end() + index, p);
01048     }
01049 }
01050 
01051 
01052 void
01053 PositionVector::push_back_noDoublePos(const Position& p) {
01054     if (size() == 0 || !p.almostSame(myCont.back())) {
01055         myCont.push_back(p);
01056     }
01057 }
01058 
01059 
01060 void
01061 PositionVector::push_front_noDoublePos(const Position& p) {
01062     if (size() == 0 || !p.almostSame(myCont.front())) {
01063         myCont.insert(myCont.begin(), p);
01064     }
01065 }
01066 
01067 
01068 void
01069 PositionVector::replaceAt(size_t index, const Position& by) {
01070     assert(size() > index);
01071     myCont[index] = by;
01072 }
01073 
01074 
01075 bool
01076 PositionVector::isClosed() const {
01077     return myCont.size() >= 2 && myCont[0] == myCont.back();
01078 }
01079 
01080 
01081 void
01082 PositionVector::removeDoublePoints() {
01083     if (myCont.size() > 1) {
01084         ContType::iterator last = myCont.begin();
01085         for (ContType::iterator i = myCont.begin() + 1; i != myCont.end();) {
01086             if (last->almostSame(*i)) {
01087                 i = myCont.erase(i);
01088             } else {
01089                 last = i;
01090                 ++i;
01091             }
01092         }
01093     }
01094 }
01095 
01096 
01097 void
01098 PositionVector::removeColinearPoints() {
01099     if (myCont.size() > 2) {
01100         Position& last = myCont.front();
01101         for (ContType::iterator i = myCont.begin() + 1; i != myCont.end() - 1;) {
01102             if (GeomHelper::distancePointLine(*i, last, *(i + 1)) < 0.001) {
01103                 i = myCont.erase(i);
01104             } else {
01105                 last = *i;
01106                 ++i;
01107             }
01108         }
01109     }
01110 }
01111 
01112 
01113 bool
01114 PositionVector::operator==(const PositionVector& v2) const {
01115     if (size() == v2.size()) {
01116         for (int i = 0; i < (int)size(); i++) {
01117             if ((*this)[i] != v2[i]) {
01118                 return false;
01119             }
01120         }
01121         return true;
01122     } else {
01123         return false;
01124     }
01125 }
01126 
01127 
01128 
01129 /****************************************************************************/
01130 
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Friends Defines