SUMO - Simulation of Urban MObility
|
00001 /****************************************************************************/ 00010 // } 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 00029 #ifdef _MSC_VER 00030 #include <windows_config.h> 00031 #else 00032 #include <config.h> 00033 #endif 00034 00035 #include "Position.h" 00036 #include "Line.h" 00037 #include "GeomHelper.h" 00038 #include <utils/common/ToString.h> 00039 #include <cassert> 00040 00041 #ifdef CHECK_MEMORY_LEAKS 00042 #include <foreign/nvwa/debug_new.h> 00043 #endif // CHECK_MEMORY_LEAKS 00044 00045 00046 // =========================================================================== 00047 // member method definitions 00048 // =========================================================================== 00049 00050 Line::Line() {} 00051 00052 00053 Line::Line(const Position& p1, const Position& p2) 00054 : myP1(p1), myP2(p2) {} 00055 00056 00057 Line::~Line() {} 00058 00059 00060 void 00061 Line::extrapolateBy(SUMOReal length) { 00062 SUMOReal factor = length / myP1.distanceTo(myP2); 00063 Position offset = (myP2 - myP1) * factor; 00064 myP1.sub(offset); 00065 myP2.add(offset); 00066 } 00067 00068 00069 void 00070 Line::extrapolateFirstBy(SUMOReal length) { 00071 myP1 = GeomHelper::extrapolate_first(myP1, myP2, length); 00072 } 00073 00074 00075 void 00076 Line::extrapolateSecondBy(SUMOReal length) { 00077 myP2 = GeomHelper::extrapolate_second(myP1, myP2, length); 00078 } 00079 00080 const Position& 00081 Line::p1() const { 00082 return myP1; 00083 } 00084 00085 00086 const Position& 00087 Line::p2() const { 00088 return myP2; 00089 } 00090 00091 00092 Position 00093 Line::getPositionAtDistance(SUMOReal offset) const { 00094 const SUMOReal length = myP1.distanceTo(myP2); 00095 if (length == 0) { 00096 if (offset != 0) { 00097 throw InvalidArgument("Invalid offset " + toString(offset) + " for Line with length " + toString(length));; 00098 } 00099 return myP1; 00100 } 00101 return myP1 + (myP2 - myP1) * (offset / length); 00102 } 00103 00104 00105 Position 00106 Line::getPositionAtDistance2D(SUMOReal offset) const { 00107 const SUMOReal length = myP1.distanceTo2D(myP2); 00108 if (length == 0) { 00109 if (offset != 0) { 00110 throw InvalidArgument("Invalid offset " + toString(offset) + " for Line with length " + toString(length));; 00111 } 00112 return myP1; 00113 } 00114 return myP1 + (myP2 - myP1) * (offset / length); 00115 } 00116 00117 00118 void 00119 Line::move2side(SUMOReal amount) { 00120 std::pair<SUMOReal, SUMOReal> p = GeomHelper::getNormal90D_CW(myP1, myP2, amount); 00121 myP1.add(p.first, p.second); 00122 myP2.add(p.first, p.second); 00123 } 00124 00125 00126 std::vector<SUMOReal> 00127 Line::intersectsAtLengths2D(const PositionVector& v) { 00128 PositionVector p = v.intersectionPoints2D(*this); 00129 std::vector<SUMOReal> ret; 00130 for (size_t i = 0; i < p.size(); i++) { 00131 ret.push_back(myP1.distanceTo2D(p[int(i)])); 00132 } 00133 return ret; 00134 } 00135 00136 00137 SUMOReal 00138 Line::atan2Angle() const { 00139 return atan2(myP1.x() - myP2.x(), myP1.y() - myP2.y()); 00140 } 00141 00142 00143 SUMOReal 00144 Line::atan2DegreeAngle() const { 00145 return (SUMOReal) atan2(myP1.x() - myP2.x(), myP1.y() - myP2.y()) * (SUMOReal) 180.0 / (SUMOReal) PI; 00146 } 00147 00148 00149 SUMOReal 00150 Line::atan2PositiveAngle() const { 00151 SUMOReal angle = atan2Angle(); 00152 if (angle < 0) { 00153 angle = (SUMOReal) PI * (SUMOReal) 2.0 + angle; 00154 } 00155 return angle; 00156 } 00157 00158 Position 00159 Line::intersectsAt(const Line& l) const { 00160 return GeomHelper::intersection_position2D(myP1, myP2, l.myP1, l.myP2); 00161 } 00162 00163 00164 bool 00165 Line::intersects(const Line& l) const { 00166 return GeomHelper::intersects(myP1, myP2, l.myP1, l.myP2); 00167 } 00168 00169 00170 SUMOReal 00171 Line::length2D() const { 00172 return myP1.distanceTo2D(myP2); 00173 } 00174 00175 00176 SUMOReal 00177 Line::length() const { 00178 return myP1.distanceTo(myP2); 00179 } 00180 00181 00182 void 00183 Line::add(SUMOReal x, SUMOReal y) { 00184 myP1.add(x, y); 00185 myP2.add(x, y); 00186 } 00187 00188 00189 void 00190 Line::add(const Position& p) { 00191 myP1.add(p.x(), p.y(), p.z()); 00192 myP2.add(p.x(), p.y(), p.z()); 00193 } 00194 00195 00196 void 00197 Line::sub(SUMOReal x, SUMOReal y) { 00198 myP1.sub(x, y); 00199 myP2.sub(x, y); 00200 } 00201 00202 00203 00204 Line& 00205 Line::reverse() { 00206 Position tmp(myP1); 00207 myP1 = myP2; 00208 myP2 = tmp; 00209 return *this; 00210 } 00211 00212 00213 SUMOReal 00214 Line::intersectsAtLength2D(const Line& v) { 00215 Position pos = 00216 GeomHelper::intersection_position2D(myP1, myP2, v.myP1, v.myP2); 00217 return GeomHelper::nearest_position_on_line_to_point2D(myP1, myP2, pos); 00218 } 00219 00220 00221 void 00222 Line::rotateAtP1(SUMOReal rot) { 00223 Position p = myP2; 00224 p.sub(myP1); 00225 p.reshiftRotate(0, 0, rot); 00226 p.add(myP1); 00227 myP2 = p; 00228 } 00229 00230 00231 /****************************************************************************/