SUMO - Simulation of Urban MObility
|
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