SUMO - Simulation of Urban MObility
GeomHelper.cpp
Go to the documentation of this file.
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 
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Friends Defines