Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2012-2025 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 AFRouter.h
15 : /// @author Ruediger Ebendt
16 : /// @date 01.12.2023
17 : ///
18 : // Realizes an arc flag routing algorithm (Hilger et al.) in its multi-level variant
19 : // (also called "stripped SHARC" by Delling et al.)
20 : /****************************************************************************/
21 : #pragma once
22 : #include <config.h>
23 :
24 : #include <cassert>
25 : #include <string>
26 : #include <functional>
27 : #include <vector>
28 : #include <set>
29 : #include <limits>
30 : #include <algorithm>
31 : #include <iterator>
32 : #include <map>
33 : #include <iostream>
34 : #include <memory>
35 : #include <stdexcept>
36 : #include <cstddef>
37 : #include <utils/common/MsgHandler.h>
38 : #include <utils/common/StringTokenizer.h>
39 : #include <utils/common/StringUtils.h>
40 : #include <utils/common/StdDefs.h>
41 : #include <utils/common/ToString.h>
42 : #include <utils/iodevices/OutputDevice.h>
43 : #include "AStarLookupTable.h"
44 : #include "SUMOAbstractRouter.h"
45 : #include "KDTreePartition.h"
46 : #include "FlippedEdge.h"
47 : #include "AFInfo.h"
48 : #include "AFBuilder.h"
49 :
50 : #define UNREACHABLE (std::numeric_limits<double>::max() / 1000.0)
51 : #define AFRO_WRITE_QGIS_FILTERS
52 :
53 : //#define AFRO_DEBUG_LEVEL_0
54 : //#define AFRO_DEBUG_LEVEL_1
55 : //#define AFRO_DEBUG_LEVEL_2
56 : //#define AFRO_DEBUG_LEVEL_3
57 :
58 : #ifdef AFRO_DEBUG_LEVEL_3
59 : #define AFRO_DEBUG_LEVEL_2
60 : #endif
61 :
62 : #ifdef AFRO_DEBUG_LEVEL_2
63 : #define AFRO_DEBUG_LEVEL_1
64 : #endif
65 :
66 : #ifdef AFRO_DEBUG_LEVEL_1
67 : #define AFRO_DEBUG_LEVEL_0
68 : #endif
69 :
70 : // ===========================================================================
71 : // class definitions
72 : // ===========================================================================
73 : /**
74 : * @class AFRouter
75 : * Computes the shortest path through a network with an arc flag routing
76 : * algorithm (Hilger et al.) in its multi-level variant (also called
77 : * "stripped SHARC" by Delling et al.)
78 : *
79 : * The template parameters are:
80 : * @param E The edge class to use (MSEdge/ROEdge)
81 : * @param N The node class to use (MSJunction/RONode)
82 : * @param V The vehicle class to use (MSVehicle/ROVehicle)
83 : *
84 : * The router is edge-based
85 : * It must know the number of edges for internal reasons
86 : * and whether a missing connection between two given edges (unbuild route) shall
87 : * be reported as an error or as a warning
88 : *
89 : */
90 : template<class E, class N, class V, class M>
91 : class AFRouter : public SUMOAbstractRouter<E, V> {
92 : public:
93 : typedef AbstractLookupTable<E, V> LookupTable;
94 : typedef typename KDTreePartition<E, N, V>::Cell Cell;
95 : typedef typename AFInfo<E>::FlagInfo FlagInfo;
96 : typedef AbstractLookupTable<FlippedEdge<E, N, V>, V> FlippedLookupTable;
97 :
98 : /** @brief Returns the edge information for the passed edge
99 : * @param[in] edge The edge
100 : * @note Non-const version
101 : * @return The edge information
102 : */
103 : typename SUMOAbstractRouter<E, V>::EdgeInfo* edgeInfo(const E* const edge) {
104 : return &(this->myEdgeInfos[edge->getNumericalID()]);
105 : }
106 :
107 : /** @brief Returns the edge information for the passed edge
108 : * @param[in] edge The edge
109 : * @note Const version
110 : * @return The edge information
111 : */
112 : const typename SUMOAbstractRouter<E, V>::EdgeInfo* edgeInfo(const E* const edge) const {
113 : return &(this->myEdgeInfos[edge->getNumericalID()]);
114 : }
115 :
116 : /**
117 : * @class EdgeInfoComparator
118 : *
119 : * Class to compare (and so sort) nodes by their effort
120 : */
121 : class EdgeInfoComparator {
122 : public:
123 : /** @brief Comparing method
124 : * @param[in] edgeInfo1 First edge information
125 : * @param[in] edgeInfo2 Second edge information
126 : * @return true iff heuristic effort of first edge information is greater than that of the second
127 : * @note In case of ties: true iff first edge information's numerical id is greater than that of the second
128 : */
129 :
130 : bool operator()(const typename SUMOAbstractRouter<E, V>::EdgeInfo* edgeInfo1, const typename SUMOAbstractRouter<E, V>::EdgeInfo* edgeInfo2) const {
131 0 : if (edgeInfo1->heuristicEffort == edgeInfo2->heuristicEffort) {
132 0 : return edgeInfo1->edge->getNumericalID() > edgeInfo2->edge->getNumericalID();
133 : }
134 0 : return edgeInfo1->heuristicEffort > edgeInfo2->heuristicEffort;
135 : }
136 : };
137 :
138 : /** @brief Constructor
139 : * @param[in] edges The edges
140 : * @param[in] partition A partition of the router's network wrt a k-d tree subdivision scheme
141 : * @param[in] unbuildIsWarning The flag indicating whether network unbuilds should issue warnings or errors
142 : * @param[in] operation The operation for a forward graph
143 : * @param[in] flippedOperation The operation for a backward graph with flipped edges
144 : * @param[in] weightPeriod The validity duration of one weight interval
145 : * @param[in] lookup The lookup table for a forward graph
146 : * @param[in] flippedLookup The lookup table for a backward graph with flipped edges
147 : * @param[in] havePermissions The boolean flag indicating whether edge permissions need to be considered or not
148 : * @param[in] haveRestrictions The boolean flag indicating whether edge restrictions need to be considered or not
149 : */
150 0 : AFRouter(const std::vector<E*>& edges,
151 : const KDTreePartition<E, N, V>* partition,
152 : bool unbuildIsWarning,
153 : typename SUMOAbstractRouter<E, V>::Operation operation, typename SUMOAbstractRouter<FlippedEdge<E, N, V>, V>::Operation flippedOperation,
154 : SUMOTime weightPeriod, const std::shared_ptr<const LookupTable> lookup = nullptr,
155 : const std::shared_ptr<const FlippedLookupTable> flippedLookup = nullptr,
156 : const bool havePermissions = false, const bool haveRestrictions = false) :
157 : SUMOAbstractRouter<E, V>("arcFlagRouter", unbuildIsWarning, operation, nullptr, havePermissions, haveRestrictions),
158 0 : myFlagInfos(nullptr),
159 0 : myPartition(partition),
160 : myLookupTable(lookup),
161 0 : myMaxSpeed(NUMERICAL_EPS),
162 0 : myWeightPeriod(weightPeriod),
163 0 : myValidUntil(0),
164 0 : myBuilder(new AFBuilder<E, N, V, M>(myPartition->getNumberOfLevels(), edges, unbuildIsWarning,
165 : flippedOperation, flippedLookup, havePermissions, haveRestrictions)),
166 0 : myType("arcFlagRouter"),
167 0 : myQueryVisits(0),
168 0 : myNumQueries(0),
169 0 : myQueryStartTime(0),
170 0 : myQueryTimeSum(0),
171 : #ifdef AFRO_DEBUG_LEVEL_2
172 : myFlagContextStartTime(0),
173 : myFlagContextTimeSum(0),
174 : #endif
175 0 : myLastSettledEdgeCell(nullptr),
176 0 : myTargetEdgeCellLevel0(nullptr) {
177 0 : for (const E* const edge : edges) {
178 0 : this->myEdgeInfos.push_back(typename SUMOAbstractRouter<E, V>::EdgeInfo(edge));
179 0 : myMaxSpeed = MAX2(myMaxSpeed, edge->getSpeedLimit() * MAX2(1.0, edge->getLengthGeometryFactor()));
180 : }
181 0 : }
182 :
183 : /** @brief "Normal" cloning constructor for uninitialized or time-dependent instances
184 : * @param[in] edges The edges
185 : * @param[in] partition A partition of the router's network wrt a k-d tree subdivision scheme
186 : * @param[in] unbuildIsWarning The flag indicating whether network unbuilds should issue warnings or errors
187 : * @param[in] operation The operation for a forward graph
188 : * @param[in] flippedOperation The operation for a backward graph with flipped edges
189 : * @param[in] weightPeriod The validity duration of one weight interval
190 : * @param[in] lookup The lookup table for a forward graph
191 : * @param[in] flippedLookup The lookup table for a backward graph with flipped edges
192 : * @param[in] havePermissions The boolean flag indicating whether edge permissions need to be considered
193 : * @param[in] haveRestrictions The boolean flag indicating whether edge restrictions need to be considered
194 : */
195 0 : AFRouter(const std::vector<typename SUMOAbstractRouter<E, V>::EdgeInfo>& edgeInfos,
196 : const std::vector<E*>& edges,
197 : const KDTreePartition<E, N, V>* partition,
198 : bool unbuildIsWarning,
199 : typename SUMOAbstractRouter<E, V>::Operation operation,
200 : typename SUMOAbstractRouter<FlippedEdge<E, N, V>, V>::Operation flippedOperation,
201 : SUMOTime weightPeriod, const std::shared_ptr<const LookupTable> lookup = nullptr,
202 : const std::shared_ptr<const FlippedLookupTable> flippedLookup = nullptr,
203 : const bool havePermissions = false, const bool haveRestrictions = false) :
204 : SUMOAbstractRouter<E, V>("arcFlagRouter", unbuildIsWarning, operation, nullptr, havePermissions, haveRestrictions),
205 0 : myFlagInfos(nullptr),
206 0 : myPartition(partition),
207 : myLookupTable(lookup),
208 0 : myMaxSpeed(NUMERICAL_EPS),
209 0 : myWeightPeriod(weightPeriod),
210 0 : myValidUntil(0),
211 0 : myBuilder(new AFBuilder<E, N, V, M>(myPartition->getNumberOfLevels(), edges, unbuildIsWarning,
212 : flippedOperation, flippedLookup, havePermissions, haveRestrictions)),
213 0 : myType("arcFlagRouter"),
214 0 : myQueryVisits(0),
215 0 : myNumQueries(0),
216 0 : myQueryStartTime(0),
217 0 : myQueryTimeSum(0),
218 : #ifdef AFRO_DEBUG_LEVEL_2
219 : myFlagContextStartTime(0),
220 : myFlagContextTimeSum(0),
221 : #endif
222 0 : myLastSettledEdgeCell(nullptr),
223 0 : myTargetEdgeCellLevel0(nullptr) {
224 0 : for (const auto& edgeInfo : edgeInfos) {
225 0 : this->myEdgeInfos.push_back(typename SUMOAbstractRouter<E, V>::EdgeInfo(edgeInfo.edge));
226 0 : myMaxSpeed = MAX2(myMaxSpeed, edgeInfo.edge->getSpeedLimit() * edgeInfo.edge->getLengthGeometryFactor());
227 : }
228 0 : }
229 :
230 : /** @brief Special cloning constructor, only for time-independent instances which never rebuild arc infos
231 : * @param[in] edgeInfos The vector of edge information
232 : * @param[in] partition A partition of the router's network wrt a k-d tree subdivision scheme
233 : * @param[in] unbuildIsWarning The flag indicating whether network unbuilds should issue warnings or errors
234 : * @param[in] operation The operation for a forward graph
235 : * @param[in] flagInfos The vector of arc flag information
236 : * @param[in] lookup The lookup table for a forward graph
237 : * @param[in] havePermissions The boolean flag indicating whether edge permissions need to be considered
238 : * @param[in] haveRestrictions The boolean flag indicating whether edge restrictions need to be considered
239 : */
240 0 : AFRouter(const std::vector<typename SUMOAbstractRouter<E, V>::EdgeInfo>& edgeInfos,
241 : const KDTreePartition<E, N, V>* partition,
242 : bool unbuildIsWarning, typename SUMOAbstractRouter<E, V>::Operation operation,
243 : std::vector<FlagInfo*>* flagInfos,
244 : const std::shared_ptr<const LookupTable> lookup = nullptr,
245 : const bool havePermissions = false, const bool haveRestrictions = false) :
246 : SUMOAbstractRouter<E, V>("arcFlagRouterClone", unbuildIsWarning, operation, nullptr, havePermissions, haveRestrictions),
247 0 : myFlagInfos(flagInfos),
248 0 : myPartition(partition),
249 : myLookupTable(lookup),
250 0 : myMaxSpeed(NUMERICAL_EPS),
251 0 : myWeightPeriod(SUMOTime_MAX),
252 0 : myValidUntil(SUMOTime_MAX),
253 0 : myBuilder(nullptr),
254 0 : myType("arcFlagRouterClone"),
255 0 : myQueryVisits(0),
256 0 : myNumQueries(0),
257 0 : myQueryStartTime(0),
258 0 : myQueryTimeSum(0),
259 : #ifdef AFRO_DEBUG_LEVEL_2
260 : myFlagContextStartTime(0),
261 : myFlagContextTimeSum(0),
262 : #endif
263 0 : myLastSettledEdgeCell(nullptr),
264 0 : myTargetEdgeCellLevel0(nullptr) {
265 0 : for (const auto& edgeInfo : edgeInfos) {
266 0 : this->myEdgeInfos.push_back(typename SUMOAbstractRouter<E, V>::EdgeInfo(edgeInfo.edge));
267 0 : myMaxSpeed = MAX2(myMaxSpeed, edgeInfo.edge->getSpeedLimit() * edgeInfo.edge->getLengthGeometryFactor());
268 : }
269 0 : }
270 :
271 : /// @brief Destructor
272 0 : virtual ~AFRouter() {
273 0 : delete myBuilder;
274 0 : }
275 :
276 : /// @brief Cloning method
277 0 : virtual SUMOAbstractRouter<E, V>* clone() {
278 : // I am either a clone myself, or I am already initialized and time-independent
279 : // (i.e., I have been created with a maximum weight period)
280 0 : if (myWeightPeriod == SUMOTime_MAX && myFlagInfos != nullptr) {
281 : // we only need the arc infos once:
282 0 : return new AFRouter(this->myEdgeInfos, myPartition, this->myErrorMsgHandler == MsgHandler::getWarningInstance(),
283 0 : this->myOperation, myFlagInfos, myLookupTable, this->myHavePermissions, this->myHaveRestrictions);
284 : }
285 : // I am not a clone: I am either uninitialized, or initialized but time-dependent:
286 : // create another such guy (also flagged as a non-clone)
287 0 : return new AFRouter(this->myEdgeInfos, myBuilder->getEdges(), myPartition,
288 0 : this->myErrorMsgHandler == MsgHandler::getWarningInstance(),
289 0 : this->myOperation, myBuilder->getArcFlagBuild()->getFlippedOperation(),
290 0 : myWeightPeriod, myLookupTable, myBuilder->getArcFlagBuild()->getFlippedLookup(),
291 0 : this->myHavePermissions, this->myHaveRestrictions);
292 : }
293 :
294 : /** @brief Converts a partition level number to a SHARC level number
295 : * @param[in] partitionLevel The partition level
296 : * @param[in] numberOfPartitionLevels The number of partition levels
297 : * @return The SHARC level number
298 : */
299 : static int partitionLevel2SHARCLevel(int partitionLevel, int numberOfPartitionLevels) {
300 : // heads up: partition levels must start at zero, with zero being an illegal argument
301 : // (since it would corresponds to level L = V, which is not a valid SHARC level)
302 : if (partitionLevel <= 0) {
303 : throw std::invalid_argument("partitionLevel2SHARCLevel: given partition level is zero (0) or below. This does not correspond to a valid SHARC level. Partition levels valid for conversion to SHARC levels go from one to number of partition levels minus one.");
304 : }
305 : // heads up: partition levels must start at zero
306 : if (partitionLevel > numberOfPartitionLevels - 1) {
307 : throw std::invalid_argument("partitionLevel2SHARCLevel: given partition level exceeds the number of partition levels minus one. Most likely you did not start the partition level numbering at zero (0), which is required here.");
308 : }
309 : return (numberOfPartitionLevels - 1) - partitionLevel;
310 : }
311 :
312 : /** @brief Converts a SHARC level number to a partition level number
313 : * @param[in] sHARCLevel The SHARC level
314 : * @param[in] numberOfPartitionLevels The number of partition levels
315 : * @return The partition level number
316 : */
317 0 : static int sHARCLevel2PartitionLevel(int sHARCLevel, int numberOfPartitionLevels) {
318 0 : int numberOfSHARCLevels = numberOfPartitionLevels - 1;
319 0 : if (sHARCLevel < 0) {
320 0 : throw std::invalid_argument("sHARCLevel2PartitionLevel: given SHARC level is negative.");
321 : }
322 : // heads up: SHARC levels must start at zero (0),
323 : // and end at number of partition levels minus two
324 0 : if (sHARCLevel > numberOfSHARCLevels - 1) {
325 0 : throw std::invalid_argument("sHARCLevel2PartitionLevel: given SHARC level exceeds the number of SHARC levels minus one. Most likely you did not start the SHARC level numbering at zero (0), which is required here.");
326 : }
327 0 : return numberOfSHARCLevels - sHARCLevel;
328 : }
329 :
330 : /** @brief Returns the arc flag of the edge in flagInfo wrt flagContext
331 : * @param[in] flagInfo The arc flag information
332 : * @param[in] flagContext The flag context tuple
333 : * @return The flag indicating whether the arc flag is set or not, wrt given arc flag information and context
334 : */
335 0 : static bool flag(const FlagInfo* flagInfo, const std::tuple<int, int, bool> flagContext) {
336 : assert(flagInfo);
337 : return flagInfo->arcFlags.empty() ? true : /* play it safe */
338 0 : (flagInfo->arcFlags)[std::get<0>(flagContext) /* assumed to be the SHARC level */ * 2
339 0 : + std::get<1>(flagContext) /* assumed to be the cell index */];
340 :
341 : }
342 :
343 : /** @brief Returns the arc flags of the passed edge
344 : * @param[in] edge The edge
345 : * @return The arc flags of the given edge
346 : */
347 : std::vector<bool>& flags(const E* edge);
348 :
349 : /** @brief Trigger arc flags rebuild
350 : * @param[in] The vehicle
351 : */
352 0 : virtual void reset(const V* const vehicle) {
353 0 : if (myValidUntil == 0) {
354 0 : myValidUntil = myWeightPeriod;
355 : }
356 : assert(myBuilder);
357 : #ifdef AFRO_DEBUG_LEVEL_0
358 : long long int firstCallStart = 0;
359 : long long int firstCallTime = 0;
360 : firstCallStart = SysUtils::getCurrentMillis();
361 : std::cout << "Calling arc flag router for the first time during current weight period (arc flags build). This might take a while... " << std::endl;
362 : #endif
363 0 : myFlagInfos = &(myBuilder->build(myValidUntil - myWeightPeriod, vehicle));
364 : #ifdef AFRO_DEBUG_LEVEL_0
365 : firstCallTime = (SysUtils::getCurrentMillis() - firstCallStart);
366 : std::cout << "Time spent for arc flags build: " << elapsedMs2string(firstCallTime) << std::endl;
367 : #endif
368 0 : }
369 :
370 : /** @brief Initialize the arc flag router
371 : * param[in] edgeID The edge id(entifier)
372 : * param[in] msTime The start time of the routes in milliseconds
373 : */
374 : void init(const int edgeID, const SUMOTime msTime);
375 : /** @brief Returns the flag context for a route query from given settled edge to the target edge
376 : * @param[in] settledEdge The settled edge
377 : * @param[in] targetEdge The target edge
378 : */
379 : std::tuple<int, int, bool> flagContext(const E* settledEdge, const E* targetEdge);
380 : /// @brief Kept for runtime comparisons
381 : std::tuple<int, int, bool> flagContextNaive(const E* settledEdge, const E* targetEdge);
382 :
383 : /** @brief Builds the route between the given edges using the minimum travel time
384 : * param[in] from The from-/start/source/head edge
385 : * param[in] to The to-/end/target/tail edge
386 : * param[in] vehicle The vehicle
387 : * param[in] msTime The start time of the routes in milliseconds
388 : * param[out] into The vector of edges, into which the solution route is written
389 : * @param[in] silent The boolean flag indicating whether the method stays silent or puts out messages
390 : */
391 0 : bool compute(const E* from, const E* to, const V* const vehicle,
392 : SUMOTime msTime, std::vector<const E*>& into, bool silent = false) {
393 : assert(from != nullptr && to != nullptr);
394 : // check whether from and to can be used
395 0 : if (this->myEdgeInfos[from->getNumericalID()].prohibited || this->isProhibited(from, vehicle)) {
396 0 : if (!silent) {
397 0 : this->myErrorMsgHandler->inform("Vehicle '" + Named::getIDSecure(vehicle) + "' is not allowed on source edge '" + from->getID() + "'.");
398 : }
399 0 : return false;
400 : }
401 0 : if (this->myEdgeInfos[to->getNumericalID()].prohibited || this->isProhibited(to, vehicle)) {
402 0 : if (!silent) {
403 0 : this->myErrorMsgHandler->inform("Vehicle '" + Named::getIDSecure(vehicle) + "' is not allowed on destination edge '" + to->getID() + "'.");
404 : }
405 0 : return false;
406 : }
407 :
408 0 : if (msTime >= myValidUntil) {
409 : assert(myBuilder != nullptr); // only time independent clones do not have a builder
410 0 : while (msTime >= myValidUntil) {
411 0 : myValidUntil += myWeightPeriod;
412 : }
413 0 : reset(vehicle);
414 : }
415 : // rewind routing start time to building time (this can only be a gross approximation
416 : // of time-dependent routing)
417 0 : msTime = myValidUntil - myWeightPeriod;
418 :
419 0 : double length = 0.; // dummy for the via edge cost update
420 0 : this->startQuery();
421 0 : const SUMOVehicleClass vClass = vehicle == 0 ? SVC_IGNORING : vehicle->getVClass();
422 0 : this->init(from->getNumericalID(), msTime);
423 0 : this->myAmClean = false;
424 : // loop
425 : int num_visited = 0;
426 : #ifdef AFRO_DEBUG_LEVEL_1
427 : int numberOfFollowers = 0;
428 : int numberOfAvoidedFollowers = 0;
429 : int numberOfEmptyFlagVectors = 0;
430 : #endif
431 0 : const bool mayRevisit = myLookupTable != nullptr && !myLookupTable->consistent();
432 0 : const double speed = vehicle == nullptr ? myMaxSpeed : MIN2(vehicle->getMaxSpeed(), myMaxSpeed * vehicle->getChosenSpeedFactor());
433 :
434 0 : while (!this->myFrontierList.empty()) {
435 0 : num_visited += 1;
436 : // use the edge with the minimal length
437 0 : auto* const minimumInfo = this->myFrontierList.front();
438 0 : const E* const minEdge = minimumInfo->edge;
439 : // check whether the destination edge was already reached
440 0 : if (minEdge == to) {
441 0 : this->buildPathFrom(minimumInfo, into);
442 0 : this->endQuery(num_visited);
443 : #ifdef AFRO_DEBUG_LEVEL_1
444 : std::cout << "Found to, to->getID(): " << to->getID() << std::endl;
445 : std::cout << static_cast<double>(numberOfFollowers - numberOfAvoidedFollowers) / static_cast<double>(num_visited)
446 : << " followers considered (out of " << static_cast<double>(numberOfFollowers) / static_cast<double>(num_visited) << ") on average." << std::endl;
447 : std::cout << static_cast<double>(numberOfFollowers - numberOfAvoidedFollowers)
448 : << " followers considered (out of " << static_cast<double>(numberOfFollowers) << ")." << std::endl;
449 : std::cout << numberOfEmptyFlagVectors << " out of " << numberOfFollowers << " flag vectors of followers were unassigned (i.e., empty)." << std::endl;
450 : std::cout << "num_visited: " << num_visited << std::endl;
451 : #endif
452 0 : return true;
453 : }
454 0 : std::pop_heap(this->myFrontierList.begin(), this->myFrontierList.end(), myComparator);
455 : this->myFrontierList.pop_back();
456 0 : this->myFound.push_back(minimumInfo);
457 0 : minimumInfo->visited = true;
458 :
459 0 : const double effortDelta = this->getEffort(minEdge, vehicle, minimumInfo->leaveTime);
460 0 : const double leaveTime = minimumInfo->leaveTime + this->getTravelTime(minEdge, vehicle, minimumInfo->leaveTime, effortDelta);
461 :
462 : // admissible A* heuristic: straight line distance at maximum speed
463 : // this is calculated from the end of minEdge so it possibly includes via efforts to the followers
464 : double heuristic_remaining = 0.;
465 0 : double heuristicEffort = minimumInfo->effort + effortDelta + heuristic_remaining;
466 : // check all ways from the edge with the minimal length
467 0 : for (const std::pair<const E*, const E*>& follower : minEdge->getViaSuccessors(vClass)) {
468 0 : auto& followerInfo = this->myEdgeInfos[follower.first->getNumericalID()];
469 0 : const FlagInfo* followerFlagInfo = (*myFlagInfos)[follower.first->getNumericalID()];
470 : // check whether it can be used
471 0 : if (followerInfo.prohibited || this->isProhibited(follower.first, vehicle)) {
472 0 : continue;
473 : }
474 : #ifdef AFRO_DEBUG_LEVEL_1
475 : numberOfFollowers++;
476 : if (followerFlagInfo->arcFlags.empty()) {
477 : numberOfEmptyFlagVectors++;
478 : }
479 : #endif
480 : #ifdef AFRO_DEBUG_LEVEL_2
481 : myFlagContextStartTime = SysUtils::getCurrentMillis();
482 : #endif
483 0 : std::tuple<int, int, bool> flagContext = this->flagContext(follower.first, to);
484 : //std::tuple<int, int, bool> flagContext = this->flagContextNaive(follower.first, to);
485 : #ifdef AFRO_DEBUG_LEVEL_2
486 : myFlagContextTimeSum += (SysUtils::getCurrentMillis() - myFlagContextStartTime);
487 : #endif
488 0 : if (!flag(followerFlagInfo, flagContext)) {
489 : #ifdef AFRO_DEBUG_LEVEL_1
490 : numberOfAvoidedFollowers++;
491 : #endif
492 0 : continue;
493 : }
494 :
495 : // admissible A* heuristic: straight line distance at maximum speed
496 : // this is calculated from the end of minEdge so it possibly includes via efforts to the followers
497 0 : if (heuristic_remaining == 0 && std::get<0>(flagContext) == 0 && std::get<2>(flagContext)) {
498 : // arrived at the target cell at level 0? use heuristic
499 : heuristic_remaining =
500 0 : (myLookupTable == nullptr ? minEdge->getDistanceTo(to) / speed :
501 0 : myLookupTable->lowerBound(minEdge, to, speed, vehicle->getChosenSpeedFactor(),
502 : minEdge->getMinimumTravelTime(nullptr), to->getMinimumTravelTime(nullptr)));
503 0 : if (heuristic_remaining == UNREACHABLE) {
504 : break; // -> skip remaining followers, continue with next min heap element
505 : }
506 0 : heuristicEffort += heuristic_remaining;
507 : }
508 :
509 0 : double effort = minimumInfo->effort + effortDelta;
510 0 : double time = leaveTime;
511 0 : this->updateViaEdgeCost(follower.second, vehicle, time, effort, length);
512 0 : const double oldEffort = followerInfo.effort;
513 0 : if ((!followerInfo.visited || mayRevisit) && effort < oldEffort) {
514 0 : followerInfo.effort = effort;
515 : // if we use the effort including the via effort below we would count the via twice as shown by the ticket676 test
516 : // but we should never get below the real effort, see #12463
517 0 : followerInfo.heuristicEffort = MAX2(MIN2(heuristicEffort, followerInfo.heuristicEffort), effort);
518 0 : followerInfo.leaveTime = time;
519 0 : followerInfo.prev = minimumInfo;
520 0 : if (oldEffort == std::numeric_limits<double>::max()) {
521 0 : this->myFrontierList.push_back(&followerInfo);
522 : std::push_heap(this->myFrontierList.begin(), this->myFrontierList.end(), myComparator);
523 : } else {
524 0 : auto fi = std::find(this->myFrontierList.begin(), this->myFrontierList.end(), &followerInfo);
525 0 : if (fi == this->myFrontierList.end()) {
526 : assert(mayRevisit);
527 0 : this->myFrontierList.push_back(&followerInfo);
528 : std::push_heap(this->myFrontierList.begin(), this->myFrontierList.end(), myComparator);
529 : } else {
530 : std::push_heap(this->myFrontierList.begin(), fi + 1, myComparator);
531 : }
532 : }
533 : }
534 : } // for followers
535 : }
536 0 : this->endQuery(num_visited);
537 : #ifdef AFRO_DEBUG_LEVEL_1
538 : std::cout << "Queue ran empty, no solution." << std::endl;
539 : std::cout << static_cast<double>(numberOfFollowers - numberOfAvoidedFollowers) / static_cast<double>(num_visited)
540 : << " followers considered (out of " << static_cast<double>(numberOfFollowers) / static_cast<double>(num_visited) << ") on average." << std::endl;
541 : std::cout << static_cast<double>(numberOfFollowers - numberOfAvoidedFollowers)
542 : << " followers considered (out of " << static_cast<double>(numberOfFollowers) << ")." << std::endl;
543 : std::cout << numberOfEmptyFlagVectors << " out of " << numberOfFollowers << " flag vectors of followers were unassigned (i.e., empty)." << std::endl;
544 : std::cout << "num_visited: " << num_visited << std::endl;
545 : #endif
546 0 : if (!silent) {
547 0 : this->myErrorMsgHandler->informf("No connection between edge '%' and edge '%' found.", from->getID(), to->getID());
548 : }
549 : return false;
550 : }
551 :
552 : /// @brief Start timer for query time sum
553 : void startQuery();
554 : /// @brief Stop timer for query time sum
555 : void endQuery(int visits);
556 : /// @brief Report query time statistics
557 : void reportStatistics();
558 : /// @brief Reset query time statistics
559 : void resetStatistics();
560 : /// @brief Bulk mode is not supported
561 0 : virtual void setBulkMode(const bool mode) {
562 : UNUSED_PARAMETER(mode);
563 0 : throw std::runtime_error("Bulk mode is not supported by the arc flag router.");
564 : }
565 :
566 : protected:
567 : /// @brief Edge infos containing the associated edge and its arc flags
568 : std::vector<FlagInfo*>* myFlagInfos;
569 : /// @brief The partition
570 : const KDTreePartition<E, N, V>* myPartition;
571 : /// @brief The comparator for edge information
572 : EdgeInfoComparator myComparator;
573 : /// @brief The lookup table for travel time heuristics
574 : const std::shared_ptr<const LookupTable> myLookupTable;
575 : /// @brief The maximum speed in the network
576 : double myMaxSpeed;
577 : /// @brief The validity duration of one weight interval
578 : const SUMOTime myWeightPeriod;
579 : /// @brief The validity duration of the current flag infos (exclusive)
580 : SUMOTime myValidUntil;
581 : /// @brief The builder
582 : AFBuilder<E, N, V, M>* myBuilder;
583 : /// @brief The type of this router
584 : /// @note The one in SUMOAbstractRouter is private, required for more flexible performance logging (see below)
585 : const std::string myType;
586 : /// @brief Counters for performance logging
587 : /// @note The ones in SUMOAbstractRouter are private - introduced to reset stats / recalculate before destruction
588 : long long int myQueryVisits;
589 : long long int myNumQueries;
590 : /// @brief The time spent querying in milliseconds
591 : /// @note The ones in SUMOAbstractRouter are private - introduced to reset stats / recalculate before destruction
592 : long long int myQueryStartTime;
593 : long long int myQueryTimeSum;
594 : #ifdef AFRO_DEBUG_LEVEL_2
595 : /// @brief The time spent for flagContext in milliseconds
596 : long long int myFlagContextStartTime;
597 : long long int myFlagContextTimeSum;
598 : #endif
599 : private:
600 : /// @brief The cell of the last settled edge
601 : const Cell* myLastSettledEdgeCell;
602 : /// @brief The last flag context
603 : std::tuple<int, int, bool> myLastFlagContext;
604 : /// @brief The cell of the target edge at SHARC level 0
605 : const Cell* myTargetEdgeCellLevel0;
606 : };
607 :
608 : // ===========================================================================
609 : // method definitions
610 : // ===========================================================================
611 :
612 : template<class E, class N, class V, class M>
613 : std::vector<bool>& AFRouter<E, N, V, M>::flags(const E* edge) {
614 : assert(edge);
615 : if (!myFlagInfos) {
616 : throw std::runtime_error("flag infos not initialized, call compute() at least once before calling flags().");
617 : }
618 : return ((*myFlagInfos)[edge->getNumericalID()])->arcFlags;
619 : }
620 :
621 : template<class E, class N, class V, class M>
622 0 : void AFRouter<E, N, V, M>::init(const int edgeID, const SUMOTime msTime) {
623 : // all EdgeInfos touched in the previous query are either in myFrontierList or myFound: clean those up
624 0 : myTargetEdgeCellLevel0 = nullptr;
625 0 : for (auto& edgeInfo : this->myFrontierList) {
626 0 : edgeInfo->reset();
627 : }
628 : this->myFrontierList.clear();
629 0 : for (auto& edgeInfo : this->myFound) {
630 0 : edgeInfo->reset();
631 : }
632 : this->myFound.clear();
633 0 : if (edgeID > -1) {
634 : // add begin node
635 0 : auto& fromInfo = this->myEdgeInfos[edgeID];
636 0 : fromInfo.heuristicEffort = 0.;
637 0 : fromInfo.effort = 0.;
638 0 : fromInfo.leaveTime = STEPS2TIME(msTime);
639 0 : fromInfo.prev = nullptr;
640 0 : this->myFrontierList.push_back(&fromInfo);
641 : }
642 0 : }
643 :
644 : template<class E, class N, class V, class M>
645 : std::tuple<int, int, bool> AFRouter<E, N, V, M>::flagContextNaive(const E* settledEdge, const E* targetEdge) {
646 : assert(settledEdge != nullptr && targetEdge != nullptr);
647 : int sHARCLevel;
648 : for (sHARCLevel = 0; sHARCLevel < myPartition->getNumberOfLevels() - 1; sHARCLevel++) {
649 : int partitionLevel = sHARCLevel2PartitionLevel(sHARCLevel, myPartition->getNumberOfLevels());
650 : const std::vector<const Cell*>& levelCells = myPartition->getCellsAtLevel(partitionLevel);
651 : typename std::vector<const Cell*>::const_iterator first = levelCells.begin();
652 : typename std::vector<const Cell*>::const_iterator last = levelCells.end();
653 : typename std::vector<const Cell*>::const_iterator iter;
654 : const Cell* settledEdgeCell = nullptr;
655 : const Cell* targetEdgeCell = nullptr;
656 : // go through the cells of the level
657 : for (iter = first; iter != last; iter++) {
658 : // myPartition is assumed to partition a non-reversed (forward) graph
659 : if (!settledEdgeCell && (*iter)->contains(settledEdge->getFromJunction())) {
660 : settledEdgeCell = *iter;
661 : }
662 : if (!targetEdgeCell && (*iter)->contains(targetEdge->getFromJunction())) {
663 : targetEdgeCell = *iter;
664 : }
665 : if (settledEdgeCell && targetEdgeCell) {
666 : break;
667 : }
668 : }
669 : assert(settledEdgeCell && targetEdgeCell); // we should find both edges on each level
670 : if (settledEdgeCell->getSupercell() == targetEdgeCell->getSupercell()) {
671 : return std::make_tuple(sHARCLevel, targetEdgeCell->isLeftOrLowerCell() ? 0 : 1,
672 : settledEdgeCell == targetEdgeCell);
673 : }
674 : }
675 : // we should never arrive here
676 : throw std::runtime_error("flagContext: relevant level could not be determined.");
677 : }
678 :
679 : template<class E, class N, class V, class M>
680 0 : std::tuple<int, int, bool> AFRouter<E, N, V, M>::flagContext(const E* settledEdge, const E* targetEdge) {
681 : assert(settledEdge != nullptr && targetEdge != nullptr);
682 : int sHARCLevel = 0; // lowest level with smallest cells
683 : const Cell* settledEdgeCell = nullptr;
684 : const Cell* targetEdgeCell = nullptr;
685 0 : if (myLastSettledEdgeCell
686 0 : && myLastSettledEdgeCell->contains(settledEdge->getFromJunction())) {
687 : // exploit the partial locality of Dijkstra's algorithm: settled edge is still
688 : // in the same cell as the last one? Then we can simply return the
689 : // last flagContext tuple again.
690 0 : return myLastFlagContext;
691 : }
692 0 : int numberOfPartitionLevels = myPartition->getNumberOfLevels();
693 0 : if (numberOfPartitionLevels <= 4) { // small number of bottom cells -> go through them, no use of k-d tree
694 0 : int partitionLevel = sHARCLevel2PartitionLevel(sHARCLevel, myPartition->getNumberOfLevels());
695 0 : const std::vector<const Cell*>& levelCells = myPartition->getCellsAtLevel(partitionLevel);
696 : typename std::vector<const Cell*>::const_iterator first = levelCells.begin();
697 : typename std::vector<const Cell*>::const_iterator last = levelCells.end();
698 : typename std::vector<const Cell*>::const_iterator iter;
699 : // go through the cells of the level
700 0 : for (iter = first; iter != last; iter++) {
701 : // myPartition is assumed to partition a non-reversed (forward) graph
702 : if (!settledEdgeCell
703 0 : && (*iter)->contains(settledEdge->getFromJunction())) {
704 : settledEdgeCell = *iter;
705 : }
706 0 : if (!targetEdgeCell && myTargetEdgeCellLevel0) {
707 : targetEdgeCell = myTargetEdgeCellLevel0;
708 : } else if (!targetEdgeCell
709 0 : && (*iter)->contains(targetEdge->getFromJunction())) {
710 0 : myTargetEdgeCellLevel0 = *iter;
711 : targetEdgeCell = myTargetEdgeCellLevel0;
712 : }
713 0 : if (settledEdgeCell && targetEdgeCell) {
714 : assert(myTargetEdgeCellLevel0);
715 : break;
716 : }
717 : }
718 : } else { // larger number of bottom cells -> use a k-d tree
719 0 : settledEdgeCell = myPartition->searchNode(settledEdge->getFromJunction());
720 0 : if (!targetEdgeCell && myTargetEdgeCellLevel0) {
721 : // search only once per query
722 : targetEdgeCell = myTargetEdgeCellLevel0;
723 : } else if (!targetEdgeCell) {
724 0 : myTargetEdgeCellLevel0 = myPartition->searchNode(targetEdge->getFromJunction());
725 : targetEdgeCell = myTargetEdgeCellLevel0; // myTargetEdgeCellLevel0 is reset in init()
726 : }
727 : }
728 : assert(settledEdgeCell && targetEdgeCell); // we should find both edges on each level
729 0 : while (settledEdgeCell->getSupercell() != targetEdgeCell->getSupercell()) {
730 : settledEdgeCell = settledEdgeCell->getSupercell();
731 : targetEdgeCell = targetEdgeCell->getSupercell();
732 0 : sHARCLevel++;
733 : }
734 0 : myLastSettledEdgeCell = settledEdgeCell;
735 0 : std::tuple<int, int, bool> flagContext = std::make_tuple(sHARCLevel, targetEdgeCell->isLeftOrLowerCell() ? 0 : 1,
736 0 : settledEdgeCell == targetEdgeCell);
737 : myLastFlagContext = flagContext;
738 : return flagContext;
739 : }
740 :
741 : template<class E, class N, class V, class M>
742 0 : void AFRouter<E, N, V, M>::startQuery() {
743 0 : myNumQueries++;
744 0 : myQueryStartTime = SysUtils::getCurrentMillis();
745 : SUMOAbstractRouter<E, V>::startQuery();
746 0 : }
747 :
748 : template<class E, class N, class V, class M>
749 0 : void AFRouter<E, N, V, M>::endQuery(int visits) {
750 0 : myQueryVisits += visits;
751 0 : myQueryTimeSum += (SysUtils::getCurrentMillis() - myQueryStartTime);
752 : SUMOAbstractRouter<E, V>::endQuery(visits);
753 0 : }
754 :
755 : template<class E, class N, class V, class M>
756 : void AFRouter<E, N, V, M>::reportStatistics() {
757 : if (myNumQueries > 0) {
758 : WRITE_MESSAGE(myType + " answered " + toString(myNumQueries) + " queries and explored " + toString((double)myQueryVisits / (double)myNumQueries) + " edges on average.");
759 : WRITE_MESSAGE(myType + " spent " + elapsedMs2string(myQueryTimeSum) + " answering queries (" + toString((double)myQueryTimeSum / (double)myNumQueries) + " ms on average).");
760 : #ifdef AFRO_DEBUG_LEVEL_2
761 : WRITE_MESSAGE("flagContext spent " + elapsedMs2string(myFlagContextTimeSum) + " (" + toString((double)myFlagContextTimeSum / (double)myNumQueries) + " ms on average).");
762 : #endif
763 : }
764 : }
765 :
766 : template<class E, class N, class V, class M>
767 : void AFRouter<E, N, V, M>::resetStatistics() {
768 : myNumQueries = 0;
769 : myQueryVisits = 0;
770 : myQueryTimeSum = 0;
771 : myQueryStartTime = 0;
772 : }
|
