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 "RandHelper.h"
27 : #include "SUMOVehicleClass.h"
28 : #include "MsgHandler.h"
29 :
30 : #define JUNCTION_TAZ_MISSING_HELP "\nSet option '--junction-taz' or load a TAZ-file"
31 :
32 : // ===========================================================================
33 : // class declarations
34 : // ===========================================================================
35 :
36 : // ===========================================================================
37 : // class definitions
38 : // ===========================================================================
39 : /**
40 : * @class MapMatcher
41 : * @brief Provides utility functions for matching locations to edges (during route parsing)
42 : */
43 :
44 : template<class E, class L, class N>
45 : class MapMatcher {
46 : protected:
47 95318 : MapMatcher(bool matchJunctions, bool matchTAZ, double matchDistance, MsgHandler* errorOutput):
48 95318 : myMapMatchTAZ(matchTAZ),
49 95318 : myLaneTree(nullptr),
50 95318 : myMapMatchJunctions(matchJunctions),
51 95318 : myMapMatchingDistance(matchDistance),
52 95318 : myErrorOutput(errorOutput) {}
53 :
54 : virtual ~MapMatcher() {
55 177 : delete myLaneTree;
56 92561 : }
57 :
58 831 : void parseGeoEdges(const PositionVector& positions, bool geo, SUMOVehicleClass vClass,
59 : std::vector<const E*>& into, const std::string& rid, bool isFrom, bool& ok, bool forceEdge = false) {
60 831 : if (geo && !GeoConvHelper::getFinal().usingGeoProjection()) {
61 0 : WRITE_ERROR(TL("Cannot convert geo-positions because the network has no geo-reference"));
62 0 : ok = false;
63 0 : return;
64 : }
65 1684 : for (Position pos : positions) {
66 853 : Position orig = pos;
67 853 : if (geo) {
68 156 : GeoConvHelper::getFinal().x2cartesian_const(pos);
69 : }
70 853 : double dist = MIN2(10.0, myMapMatchingDistance);
71 853 : const L* best = getClosestLane(pos, vClass, dist);
72 853 : while (best == nullptr && dist < myMapMatchingDistance) {
73 0 : dist = MIN2(dist * 2, myMapMatchingDistance);
74 0 : best = getClosestLane(pos, vClass, dist);
75 : }
76 853 : if (best == nullptr) {
77 36 : myErrorOutput->inform("No edge found near position " + toString(orig, geo ? gPrecisionGeo : gPrecision) + " within the route " + rid + ".");
78 18 : ok = false;
79 : } else {
80 835 : const E* bestEdge = &best->getEdge();
81 854 : while (bestEdge->isInternal()) {
82 19 : bestEdge = bestEdge->getSuccessors().front();
83 : }
84 835 : if ((myMapMatchJunctions || myMapMatchTAZ) && !forceEdge) {
85 442 : if (myMapMatchTAZ) {
86 69 : bestEdge = getTaz(pos, bestEdge, isFrom);
87 : } else {
88 373 : bestEdge = getJunctionTaz(pos, bestEdge, vClass, isFrom);
89 : }
90 442 : if (bestEdge != nullptr) {
91 427 : into.push_back(bestEdge);
92 : } else {
93 15 : ok = false;
94 : }
95 : } else {
96 : // handle multiple via locations on the same edge without loops
97 393 : if (positions.size() == 1 || into.empty() || into.back() != bestEdge) {
98 371 : into.push_back(bestEdge);
99 : }
100 : }
101 : }
102 : }
103 : }
104 :
105 :
106 : /// @brief find closest lane within distance for the given position or nullptr
107 853 : const L* getClosestLane(const Position& pos, SUMOVehicleClass vClass, double distance = -1.) {
108 853 : NamedRTree* t = getLaneTree();
109 853 : Boundary b;
110 853 : b.add(pos);
111 853 : b.grow(distance < 0 ? myMapMatchingDistance : distance);
112 853 : const float cmin[2] = {(float) b.xmin(), (float) b.ymin()};
113 853 : const float cmax[2] = {(float) b.xmax(), (float) b.ymax()};
114 : std::set<const Named*> lanes;
115 : Named::StoringVisitor sv(lanes);
116 853 : t->Search(cmin, cmax, sv);
117 : // use closest
118 : double minDist = std::numeric_limits<double>::max();
119 : const L* best = nullptr;
120 2943 : for (const Named* o : lanes) {
121 : const L* cand = static_cast<const L*>(o);
122 2090 : if (!cand->allowsVehicleClass(vClass)) {
123 104 : continue;
124 : }
125 1986 : double dist = cand->getShape().distance2D(pos);
126 1986 : if (dist < minDist) {
127 : minDist = dist;
128 : best = cand;
129 : }
130 : }
131 853 : return best;
132 853 : }
133 :
134 : /// @brief find closest junction taz given the closest edge
135 373 : const E* getJunctionTaz(const Position& pos, const E* closestEdge, SUMOVehicleClass vClass, bool isFrom) {
136 373 : if (closestEdge == nullptr) {
137 : return nullptr;
138 : } else {
139 : const N* fromJunction = closestEdge->getFromJunction();
140 : const N* toJunction = closestEdge->getToJunction();
141 373 : const bool fromCloser = (fromJunction->getPosition().distanceSquaredTo2D(pos) <
142 245 : toJunction->getPosition().distanceSquaredTo2D(pos));
143 373 : const E* fromSource = retrieveEdge(fromJunction->getID() + "-source");
144 373 : const E* fromSink = retrieveEdge(fromJunction->getID() + "-sink");
145 373 : const E* toSource = retrieveEdge(toJunction->getID() + "-source");
146 373 : const E* toSink = retrieveEdge(toJunction->getID() + "-sink");
147 373 : if (fromSource == nullptr || fromSink == nullptr) {
148 0 : myErrorOutput->inform("Junction-taz '" + fromJunction->getID() + "' not found when mapping position " + toString(pos) + "." + JUNCTION_TAZ_MISSING_HELP);
149 0 : return nullptr;
150 : }
151 373 : if (toSource == nullptr || toSink == nullptr) {
152 0 : myErrorOutput->inform("Junction-taz '" + toJunction->getID() + "' not found when mapping position " + toString(pos) + "." + JUNCTION_TAZ_MISSING_HELP);
153 0 : return nullptr;
154 : }
155 373 : const bool fromPossible = isFrom ? fromSource->getSuccessors(vClass).size() > 0 : fromSink->getPredecessors().size() > 0;
156 373 : const bool toPossible = isFrom ? toSource->getSuccessors(vClass).size() > 0 : toSink->getPredecessors().size() > 0;
157 : //std::cout << "getJunctionTaz pos=" << pos << " isFrom=" << isFrom << " closest=" << closestEdge->getID() << " fromPossible=" << fromPossible << " toPossible=" << toPossible << "\n";
158 373 : if (fromCloser && fromPossible) {
159 : // return closest if possible
160 185 : return isFrom ? fromSource : fromSink;
161 208 : } else if (!fromCloser && toPossible) {
162 : // return closest if possible
163 395 : return isFrom ? toSource : toSink;
164 : } else {
165 : // return possible
166 7 : if (fromPossible) {
167 0 : return isFrom ? fromSource : fromSink;
168 : } else {
169 14 : return isFrom ? toSource : toSink;
170 : }
171 : }
172 : }
173 : }
174 :
175 : /// @brief find closest junction taz given the closest edge
176 69 : const E* getTaz(const Position& pos, const E* closestEdge, bool isFrom) {
177 69 : if (closestEdge == nullptr) {
178 : return nullptr;
179 : } else {
180 : std::vector<const E*> cands;
181 69 : if (isFrom) {
182 144 : for (const E* e : closestEdge->getPredecessors()) {
183 106 : if (e->isTazConnector()) {
184 75 : cands.push_back(e);
185 : }
186 : }
187 : } else {
188 89 : for (const E* e : closestEdge->getSuccessors()) {
189 58 : if (e->isTazConnector()) {
190 27 : cands.push_back(e);
191 : }
192 : }
193 : }
194 69 : if (cands.size() == 0) {
195 60 : myErrorOutput->inform("Taz for edge '" + closestEdge->getID() + "' not found when mapping position " + toString(pos) + "." + JUNCTION_TAZ_MISSING_HELP);
196 15 : return nullptr;
197 : }
198 54 : if (cands.size() > 1) {
199 16 : return cands[RandHelper::rand((int)cands.size(), getRNG())];
200 : } else {
201 38 : return cands.front();
202 : }
203 69 : }
204 : }
205 :
206 16 : virtual SumoRNG* getRNG() {
207 16 : return nullptr;
208 : }
209 :
210 : virtual void initLaneTree(NamedRTree* tree) = 0;
211 :
212 : virtual E* retrieveEdge(const std::string& id) = 0;
213 :
214 : bool myMapMatchTAZ;
215 :
216 : private:
217 : /// @brief initialize lane-RTree
218 853 : NamedRTree* getLaneTree() {
219 853 : if (myLaneTree == nullptr) {
220 192 : myLaneTree = new NamedRTree();
221 192 : initLaneTree(myLaneTree);
222 : }
223 853 : return myLaneTree;
224 : }
225 :
226 : /// @brief RTree for finding lanes
227 : NamedRTree* myLaneTree;
228 : bool myMapMatchJunctions;
229 : double myMapMatchingDistance;
230 : MsgHandler* myErrorOutput;
231 :
232 :
233 : private:
234 : /// @brief Invalidated copy constructor
235 : MapMatcher(const MapMatcher& s) = delete;
236 :
237 : /// @brief Invalidated assignment operator
238 : MapMatcher& operator=(const MapMatcher& s) = delete;
239 : };
|
