Line data    Source code

       1             : /****************************************************************************/
       2             : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
       3             : // Copyright (C) 2001-2024 German Aerospace Center (DLR) and others.
       4             : // This program and the accompanying materials are made available under the
       5             : // terms of the Eclipse Public License 2.0 which is available at
       6             : // https://www.eclipse.org/legal/epl-2.0/
       7             : // This Source Code may also be made available under the following Secondary
       8             : // Licenses when the conditions for such availability set forth in the Eclipse
       9             : // Public License 2.0 are satisfied: GNU General Public License, version 2
      10             : // or later which is available at
      11             : // https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
      12             : // SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
      13             : /****************************************************************************/
      14             : /// @file    MapMatcher.h
      15             : /// @author  Jakob Erdmann
      16             : /// @date    Thu, 7 March 2024
      17             : ///
      18             : // Utility functions for matching locations to edges (during route parsing)
      19             : /****************************************************************************/
      20             : #pragma once
      21             : #include <config.h>
      22             : 
      23             : #include <utils/geom/GeoConvHelper.h>
      24             : #include <utils/vehicle/SUMOVTypeParameter.h>
      25             : #include "NamedRTree.h"
      26             : #include "SUMOVehicleClass.h"
      27             : #include "MsgHandler.h"
      28             : 
      29             : #define JUNCTION_TAZ_MISSING_HELP "\nSet option '--junction-taz' or load a TAZ-file"
      30             : 
      31             : // ===========================================================================
      32             : // class declarations
      33             : // ===========================================================================
      34             : 
      35             : // ===========================================================================
      36             : // class definitions
      37             : // ===========================================================================
      38             : /**
      39             :  * @class MapMatcher
      40             :  * @brief Provides utility functions for matching locations to edges (during route parsing)
      41             :  */
      42             : 
      43             : template<class E, class L, class N>
      44             : class MapMatcher {
      45             : protected:
      46       65895 :     MapMatcher(bool matchJunctions, double matchDistance, MsgHandler* errorOutput):
      47       65895 :         myLaneTree(nullptr),
      48       65895 :         myMapMatchJunctions(matchJunctions),
      49       65895 :         myMapMatchingDistance(matchDistance),
      50       65895 :         myErrorOutput(errorOutput) {}
      51             : 
      52             :     virtual ~MapMatcher() {
      53         101 :         delete myLaneTree;
      54       65550 :     }
      55             : 
      56         500 :     void parseGeoEdges(const PositionVector& positions, bool geo, SUMOVehicleClass vClass,
      57             :                        std::vector<const E*>& into, const std::string& rid, bool isFrom, bool& ok) {
      58         500 :         if (geo && !GeoConvHelper::getFinal().usingGeoProjection()) {
      59           0 :             WRITE_ERROR(TL("Cannot convert geo-positions because the network has no geo-reference"));
      60           0 :             return;
      61             :         }
      62        1022 :         for (Position pos : positions) {
      63         522 :             Position orig = pos;
      64         522 :             if (geo) {
      65         102 :                 GeoConvHelper::getFinal().x2cartesian_const(pos);
      66             :             }
      67         522 :             double dist = MIN2(10.0, myMapMatchingDistance);
      68         522 :             const E* best = getClosestEdge(pos, dist, vClass);
      69         522 :             while (best == nullptr && dist < myMapMatchingDistance) {
      70           0 :                 dist = MIN2(dist * 2, myMapMatchingDistance);
      71           0 :                 best = getClosestEdge(pos, dist, vClass);
      72             :             }
      73         522 :             if (best == nullptr) {
      74          22 :                 myErrorOutput->inform("No edge found near position " + toString(orig, geo ? gPrecisionGeo : gPrecision) + " within the route " + rid + ".");
      75          11 :                 ok = false;
      76             :             } else {
      77         511 :                 if (myMapMatchJunctions) {
      78         260 :                     best = getJunctionTaz(pos, best, vClass, isFrom);
      79         260 :                     if (best != nullptr) {
      80         260 :                         into.push_back(best);
      81             :                     }
      82             :                 } else {
      83             :                     // handle multiple via locations on the same edge without loops
      84         251 :                     if (positions.size() == 1 || into.empty() || into.back() != best) {
      85         229 :                         into.push_back(best);
      86             :                     }
      87             :                 }
      88             :             }
      89             :         }
      90             :     }
      91             : 
      92             : 
      93             :     /// @brief find closest edge within distance for the given position or nullptr
      94         522 :     const E* getClosestEdge(const Position& pos, double distance, SUMOVehicleClass vClass) {
      95         522 :         NamedRTree* t = getLaneTree();
      96         522 :         Boundary b;
      97         522 :         b.add(pos);
      98         522 :         b.grow(distance);
      99         522 :         const float cmin[2] = {(float) b.xmin(), (float) b.ymin()};
     100         522 :         const float cmax[2] = {(float) b.xmax(), (float) b.ymax()};
     101             :         std::set<const Named*> lanes;
     102             :         Named::StoringVisitor sv(lanes);
     103         522 :         t->Search(cmin, cmax, sv);
     104             :         // use closest
     105             :         double minDist = std::numeric_limits<double>::max();
     106             :         const L* best = nullptr;
     107        1697 :         for (const Named* o : lanes) {
     108             :             const L* cand = static_cast<const L*>(o);
     109        1175 :             if (!cand->allowsVehicleClass(vClass)) {
     110          68 :                 continue;
     111             :             }
     112        1107 :             double dist = cand->getShape().distance2D(pos);
     113        1107 :             if (dist < minDist) {
     114             :                 minDist = dist;
     115             :                 best = cand;
     116             :             }
     117             :         }
     118         522 :         if (best == nullptr) {
     119             :             return nullptr;
     120             :         } else {
     121             :             const E* bestEdge = &best->getEdge();
     122         515 :             while (bestEdge->isInternal()) {
     123           4 :                 bestEdge = bestEdge->getSuccessors().front();
     124             :             }
     125         511 :             return bestEdge;
     126             :         }
     127         522 :     }
     128             : 
     129             :     /// @brief find closest junction taz given the closest edge
     130         260 :     const E* getJunctionTaz(const Position& pos, const E* closestEdge, SUMOVehicleClass vClass, bool isFrom) {
     131         260 :         if (closestEdge == nullptr) {
     132             :             return nullptr;
     133             :         } else {
     134             :             const N* fromJunction = closestEdge->getFromJunction();
     135             :             const N* toJunction = closestEdge->getToJunction();
     136         260 :             const bool fromCloser = (fromJunction->getPosition().distanceSquaredTo2D(pos) <
     137         140 :                     toJunction->getPosition().distanceSquaredTo2D(pos));
     138         260 :             const E* fromSource = retrieveEdge(fromJunction->getID() + "-source");
     139         260 :             const E* fromSink = retrieveEdge(fromJunction->getID() + "-sink");
     140         260 :             const E* toSource = retrieveEdge(toJunction->getID() + "-source");
     141         260 :             const E* toSink = retrieveEdge(toJunction->getID() + "-sink");
     142         260 :             if (fromSource == nullptr || fromSink == nullptr) {
     143           0 :                 myErrorOutput->inform("Junction-taz '" + fromJunction->getID() + "' not found when mapping position " + toString(pos) + "." + JUNCTION_TAZ_MISSING_HELP);
     144           0 :                 return nullptr;
     145             :             }
     146         260 :             if (toSource == nullptr || toSink == nullptr) {
     147           0 :                 myErrorOutput->inform("Junction-taz '" + toJunction->getID() + "' not found when mapping position " + toString(pos) + "." + JUNCTION_TAZ_MISSING_HELP);
     148           0 :                 return nullptr;
     149             :             }
     150         260 :             const bool fromPossible = isFrom ? fromSource->getSuccessors(vClass).size() > 0 : fromSink->getPredecessors().size() > 0;
     151         260 :             const bool toPossible = isFrom ? toSource->getSuccessors(vClass).size() > 0 : toSink->getPredecessors().size() > 0;
     152             :             //std::cout << "getJunctionTaz pos=" << pos << " isFrom=" << isFrom << " closest=" << closestEdge->getID() << " fromPossible=" << fromPossible << " toPossible=" << toPossible << "\n";
     153         260 :             if (fromCloser && fromPossible) {
     154             :                 // return closest if possible
     155         141 :                 return isFrom ? fromSource : fromSink;
     156         139 :             } else if (!fromCloser && toPossible) {
     157             :                 // return closest if possible
     158         271 :                 return isFrom ? toSource : toSink;
     159             :             } else {
     160             :                 // return possible
     161           0 :                 if (fromPossible) {
     162           0 :                     return isFrom ? fromSource : fromSink;
     163             :                 } else {
     164           0 :                     return isFrom ? toSource : toSink;
     165             :                 }
     166             :             }
     167             :         }
     168             :     }
     169             : 
     170             : 
     171             :     virtual void initLaneTree(NamedRTree* tree) = 0;
     172             : 
     173             :     virtual E* retrieveEdge(const std::string& id) = 0;
     174             : 
     175             : private:
     176             :     /// @brief initialize lane-RTree
     177         522 :     NamedRTree* getLaneTree() {
     178         522 :         if (myLaneTree == nullptr) {
     179         106 :             myLaneTree = new NamedRTree();
     180         106 :             initLaneTree(myLaneTree);
     181             :         }
     182         522 :         return myLaneTree;
     183             :     }
     184             : 
     185             :     /// @brief RTree for finding lanes
     186             :     NamedRTree* myLaneTree;
     187             :     bool myMapMatchJunctions;
     188             :     double myMapMatchingDistance;
     189             :     MsgHandler* myErrorOutput;
     190             : 
     191             : 
     192             : private:
     193             :     /// @brief Invalidated copy constructor
     194             :     MapMatcher(const MapMatcher& s) = delete;
     195             : 
     196             :     /// @brief Invalidated assignment operator
     197             :     MapMatcher& operator=(const MapMatcher& s) = delete;
     198             : };