SUMO - Simulation of Urban MObility
|
00001 /****************************************************************************/ 00010 // Some geometrical helpers 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 <cmath> 00035 #include <limits> 00036 #include <algorithm> 00037 #include <iostream> 00038 #include <utils/common/StdDefs.h> 00039 #include <utils/common/ToString.h> 00040 #include <utils/geom/Line.h> 00041 #include "Boundary.h" 00042 #include "GeomHelper.h" 00043 00044 #ifdef CHECK_MEMORY_LEAKS 00045 #include <foreign/nvwa/debug_new.h> 00046 #endif // CHECK_MEMORY_LEAKS 00047 00048 00049 // =========================================================================== 00050 // method definitions 00051 // =========================================================================== 00052 bool 00053 GeomHelper::intersects(const SUMOReal x1, const SUMOReal y1, 00054 const SUMOReal x2, const SUMOReal y2, 00055 const SUMOReal x3, const SUMOReal y3, 00056 const SUMOReal x4, const SUMOReal y4, 00057 SUMOReal* x, SUMOReal* y, SUMOReal* mu) { 00058 const SUMOReal eps = std::numeric_limits<SUMOReal>::epsilon(); 00059 const double denominator = (y4-y3) * (x2-x1) - (x4-x3) * (y2-y1); 00060 const double numera = (x4-x3) * (y1-y3) - (y4-y3) * (x1-x3); 00061 const double numerb = (x2-x1) * (y1-y3) - (y2-y1) * (x1-x3); 00062 /* Are the lines coincident? */ 00063 if (fabs(numera) < eps && fabs(numerb) < eps && fabs(denominator) < eps) { 00064 SUMOReal a1; 00065 SUMOReal a2; 00066 SUMOReal a3; 00067 SUMOReal a4; 00068 SUMOReal a = -1e12; 00069 if (x1 != x2) { 00070 a1 = x1 < x2 ? x1 : x2; 00071 a2 = x1 < x2 ? x2 : x1; 00072 a3 = x3 < x4 ? x3 : x4; 00073 a4 = x3 < x4 ? x4 : x3; 00074 } else { 00075 a1 = y1 < y2 ? y1 : y2; 00076 a2 = y1 < y2 ? y2 : y1; 00077 a3 = y3 < y4 ? y3 : y4; 00078 a4 = y3 < y4 ? y4 : y3; 00079 } 00080 if (a1 <= a3 && a3 <= a2) { 00081 if (a4 < a2) { 00082 a = (a3+a4) / 2; 00083 } else { 00084 a = (a2+a3) / 2; 00085 } 00086 } 00087 if (a3 <= a1 && a1 <= a4) { 00088 if (a2 < a4) { 00089 a = (a1+a2) / 2; 00090 } else { 00091 a = (a1+a4) / 2; 00092 } 00093 } 00094 if (a != -1e12) { 00095 if (x != 0) { 00096 if (x1 != x2) { 00097 *mu = (a - x1) / (x2 - x1); 00098 *x = a; 00099 *y = y1 + (*mu) * (y2 - y1); 00100 } else { 00101 *x = x1; 00102 *y = a; 00103 *mu = (a - y1) / (y2 - y1); 00104 } 00105 } 00106 return true; 00107 } 00108 return false; 00109 } 00110 /* Are the lines parallel */ 00111 if (fabs(denominator) < eps) { 00112 return false; 00113 } 00114 /* Is the intersection along the segments */ 00115 const double mua = numera / denominator; 00116 const double mub = numerb / denominator; 00117 if (mua < 0 || mua > 1 || mub < 0 || mub > 1) { 00118 return false; 00119 } 00120 if (x != 0) { 00121 *x = x1 + mua * (x2 - x1); 00122 *y = y1 + mua * (y2 - y1); 00123 *mu = mua; 00124 } 00125 return true; 00126 } 00127 00128 00129 00130 00131 bool 00132 GeomHelper::intersects(const Position& p11, const Position& p12, 00133 const Position& p21, const Position& p22) { 00134 return intersects(p11.x(), p11.y(), p12.x(), p12.y(), 00135 p21.x(), p21.y(), p22.x(), p22.y(), 0, 0, 0); 00136 } 00137 00138 00139 Position 00140 GeomHelper::intersection_position2D(const Position& p11, 00141 const Position& p12, 00142 const Position& p21, 00143 const Position& p22) { 00144 SUMOReal x, y, m; 00145 if (intersects(p11.x(), p11.y(), p12.x(), p12.y(), 00146 p21.x(), p21.y(), p22.x(), p22.y(), &x, &y, &m)) { 00147 // @todo calculate better "average" z value 00148 return Position(x, y, p11.z() + m * (p12.z() - p11.z())); 00149 } 00150 return Position(-1, -1); 00151 } 00152 00153 00154 00155 /* 00156 Return the angle between two vectors on a plane 00157 The angle is from vector 1 to vector 2, positive anticlockwise 00158 The result is between -pi -> pi 00159 */ 00160 SUMOReal 00161 GeomHelper::Angle2D(SUMOReal x1, SUMOReal y1, SUMOReal x2, SUMOReal y2) { 00162 SUMOReal dtheta = atan2(y2, x2) - atan2(y1, x1); 00163 while (dtheta > (SUMOReal) PI) { 00164 dtheta -= (SUMOReal)(2.0 * PI); 00165 } 00166 while (dtheta < (SUMOReal) - PI) { 00167 dtheta += (SUMOReal)(2.0 * PI); 00168 } 00169 return dtheta; 00170 } 00171 00172 00173 Position 00174 GeomHelper::interpolate(const Position& p1, 00175 const Position& p2, SUMOReal length) { 00176 const SUMOReal factor = length / p1.distanceTo(p2); 00177 return p1 + (p2 - p1) * factor; 00178 } 00179 00180 00181 Position 00182 GeomHelper::extrapolate_first(const Position& p1, 00183 const Position& p2, SUMOReal length) { 00184 const SUMOReal factor = length / p1.distanceTo(p2); 00185 return p1 - (p2 - p1) * factor; 00186 } 00187 00188 00189 Position 00190 GeomHelper::extrapolate_second(const Position& p1, 00191 const Position& p2, SUMOReal length) { 00192 const SUMOReal factor = length / p1.distanceTo(p2); 00193 return p2 - (p1 - p2) * factor; 00194 } 00195 00196 00197 SUMOReal 00198 GeomHelper::nearest_position_on_line_to_point2D(const Position& LineStart, 00199 const Position& LineEnd, 00200 const Position& Point, bool perpendicular) { 00201 const SUMOReal lineLength2D = LineStart.distanceTo2D(LineEnd); 00202 if (lineLength2D == 0.0f) { 00203 return 0.0f; 00204 } else { 00205 // scalar product equals length of orthogonal projection times length of vector being projected onto 00206 // dividing the scalar product by the square of the distance gives the relative position 00207 const SUMOReal u = (((Point.x() - LineStart.x()) * (LineEnd.x() - LineStart.x())) + 00208 ((Point.y() - LineStart.y()) * (LineEnd.y() - LineStart.y())) 00209 ) / (lineLength2D * lineLength2D); 00210 if (u < 0.0f || u > 1.0f) { // closest point does not fall within the line segment 00211 if (perpendicular) { 00212 return -1; 00213 } 00214 if (u < 0.0f) { 00215 return 0.0f; 00216 } 00217 return lineLength2D; 00218 } 00219 return u * lineLength2D; 00220 } 00221 } 00222 00223 00224 SUMOReal 00225 GeomHelper::distancePointLine(const Position& point, 00226 const Position& lineStart, 00227 const Position& lineEnd) { 00228 const SUMOReal lineLengthSquared = lineStart.distanceSquaredTo(lineEnd); 00229 if (lineLengthSquared == 0.0f) { 00230 return point.distanceTo(lineStart); 00231 } else { 00232 // scalar product equals length of orthogonal projection times length of vector being projected onto 00233 // dividing the scalar product by the square of the distance gives the relative position 00234 SUMOReal u = (((point.x() - lineStart.x()) * (lineEnd.x() - lineStart.x())) + 00235 ((point.y() - lineStart.y()) * (lineEnd.y() - lineStart.y())) 00236 ) / lineLengthSquared; 00237 if (u < 0.0f || u > 1.0f) { 00238 return -1; // closest point does not fall within the line segment 00239 } 00240 Position intersection( 00241 lineStart.x() + u * (lineEnd.x() - lineStart.x()), 00242 lineStart.y() + u * (lineEnd.y() - lineStart.y())); 00243 return point.distanceTo(intersection); 00244 } 00245 } 00246 00247 00248 SUMOReal 00249 GeomHelper::closestDistancePointLine(const Position& point, 00250 const Position& lineStart, 00251 const Position& lineEnd, 00252 Position& outIntersection) { 00253 const SUMOReal length = nearest_position_on_line_to_point2D(lineStart, lineEnd, point, false); 00254 outIntersection.set(Line(lineStart, lineEnd).getPositionAtDistance(length)); 00255 return point.distanceTo2D(outIntersection); 00256 } 00257 00258 00259 00260 Position 00261 GeomHelper::transfer_to_side(Position& p, 00262 const Position& lineBeg, 00263 const Position& lineEnd, 00264 SUMOReal amount) { 00265 const SUMOReal dx = lineBeg.x() - lineEnd.x(); 00266 const SUMOReal dy = lineBeg.y() - lineEnd.y(); 00267 const SUMOReal length = sqrt(dx * dx + dy * dy); 00268 if (length > 0) { 00269 p.add(dy * amount / length, -dx * amount / length); 00270 } 00271 return p; 00272 } 00273 00274 00275 00276 Position 00277 GeomHelper::crossPoint(const Boundary& b, const PositionVector& v) { 00278 if (v.intersects(Position(b.xmin(), b.ymin()), Position(b.xmin(), b.ymax()))) { 00279 return v.intersectsAtPoint( 00280 Position(b.xmin(), b.ymin()), 00281 Position(b.xmin(), b.ymax())); 00282 } 00283 if (v.intersects(Position(b.xmax(), b.ymin()), Position(b.xmax(), b.ymax()))) { 00284 return v.intersectsAtPoint( 00285 Position(b.xmax(), b.ymin()), 00286 Position(b.xmax(), b.ymax())); 00287 } 00288 if (v.intersects(Position(b.xmin(), b.ymin()), Position(b.xmax(), b.ymin()))) { 00289 return v.intersectsAtPoint( 00290 Position(b.xmin(), b.ymin()), 00291 Position(b.xmax(), b.ymin())); 00292 } 00293 if (v.intersects(Position(b.xmin(), b.ymax()), Position(b.xmax(), b.ymax()))) { 00294 return v.intersectsAtPoint( 00295 Position(b.xmin(), b.ymax()), 00296 Position(b.xmax(), b.ymax())); 00297 } 00298 throw 1; 00299 } 00300 00301 std::pair<SUMOReal, SUMOReal> 00302 GeomHelper::getNormal90D_CW(const Position& beg, 00303 const Position& end, 00304 SUMOReal wanted_offset) { 00305 return getNormal90D_CW(beg, end, beg.distanceTo2D(end), wanted_offset); 00306 } 00307 00308 00309 std::pair<SUMOReal, SUMOReal> 00310 GeomHelper::getNormal90D_CW(const Position& beg, 00311 const Position& end, 00312 SUMOReal length, SUMOReal wanted_offset) { 00313 if (beg == end) { 00314 throw InvalidArgument("same points at " + toString(beg)); 00315 } 00316 return std::pair<SUMOReal, SUMOReal> 00317 ((beg.y() - end.y()) * wanted_offset / length, (end.x() - beg.x()) * wanted_offset / length); 00318 } 00319 00320 00321 SUMOReal 00322 GeomHelper::getCCWAngleDiff(SUMOReal angle1, SUMOReal angle2) { 00323 SUMOReal v = angle2 - angle1; 00324 if (v < 0) { 00325 v = 360 + v; 00326 } 00327 return v; 00328 } 00329 00330 00331 SUMOReal 00332 GeomHelper::getCWAngleDiff(SUMOReal angle1, SUMOReal angle2) { 00333 SUMOReal v = angle1 - angle2; 00334 if (v < 0) { 00335 v = 360 + v; 00336 } 00337 return v; 00338 } 00339 00340 00341 SUMOReal 00342 GeomHelper::getMinAngleDiff(SUMOReal angle1, SUMOReal angle2) { 00343 return MIN2(getCWAngleDiff(angle1, angle2), getCCWAngleDiff(angle1, angle2)); 00344 } 00345 00346 00347 SUMOReal 00348 GeomHelper::getMaxAngleDiff(SUMOReal angle1, SUMOReal angle2) { 00349 return MAX2(getCWAngleDiff(angle1, angle2), getCCWAngleDiff(angle1, angle2)); 00350 } 00351 00352 00353 00354 /****************************************************************************/ 00355