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->isProhibited(from, vehicle, STEPS2TIME(msTime))) {
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 : // technically, a temporary permission might be lifted by the time of arrival
402 0 : if (this->isProhibited(to, vehicle, STEPS2TIME(msTime))) {
403 0 : if (!silent) {
404 0 : this->myErrorMsgHandler->inform("Vehicle '" + Named::getIDSecure(vehicle) + "' is not allowed on destination edge '" + to->getID() + "'.");
405 : }
406 0 : return false;
407 : }
408 :
409 0 : if (msTime >= myValidUntil) {
410 : assert(myBuilder != nullptr); // only time independent clones do not have a builder
411 0 : while (msTime >= myValidUntil) {
412 0 : myValidUntil += myWeightPeriod;
413 : }
414 0 : reset(vehicle);
415 : }
416 : // rewind routing start time to building time (this can only be a gross approximation
417 : // of time-dependent routing)
418 0 : msTime = myValidUntil - myWeightPeriod;
419 :
420 0 : double length = 0.; // dummy for the via edge cost update
421 0 : this->startQuery();
422 0 : const SUMOVehicleClass vClass = vehicle == 0 ? SVC_IGNORING : vehicle->getVClass();
423 0 : this->init(from->getNumericalID(), msTime);
424 0 : this->myAmClean = false;
425 : // loop
426 : int num_visited = 0;
427 : #ifdef AFRO_DEBUG_LEVEL_1
428 : int numberOfFollowers = 0;
429 : int numberOfAvoidedFollowers = 0;
430 : int numberOfEmptyFlagVectors = 0;
431 : #endif
432 0 : const bool mayRevisit = myLookupTable != nullptr && !myLookupTable->consistent();
433 0 : const double speed = vehicle == nullptr ? myMaxSpeed : MIN2(vehicle->getMaxSpeed(), myMaxSpeed * vehicle->getChosenSpeedFactor());
434 :
435 0 : while (!this->myFrontierList.empty()) {
436 0 : num_visited += 1;
437 : // use the edge with the minimal length
438 0 : auto* const minimumInfo = this->myFrontierList.front();
439 0 : const E* const minEdge = minimumInfo->edge;
440 : // check whether the destination edge was already reached
441 0 : if (minEdge == to) {
442 0 : this->buildPathFrom(minimumInfo, into);
443 0 : this->endQuery(num_visited);
444 : #ifdef AFRO_DEBUG_LEVEL_1
445 : std::cout << "Found to, to->getID(): " << to->getID() << std::endl;
446 : std::cout << static_cast<double>(numberOfFollowers - numberOfAvoidedFollowers) / static_cast<double>(num_visited)
447 : << " followers considered (out of " << static_cast<double>(numberOfFollowers) / static_cast<double>(num_visited) << ") on average." << std::endl;
448 : std::cout << static_cast<double>(numberOfFollowers - numberOfAvoidedFollowers)
449 : << " followers considered (out of " << static_cast<double>(numberOfFollowers) << ")." << std::endl;
450 : std::cout << numberOfEmptyFlagVectors << " out of " << numberOfFollowers << " flag vectors of followers were unassigned (i.e., empty)." << std::endl;
451 : std::cout << "num_visited: " << num_visited << std::endl;
452 : #endif
453 0 : return true;
454 : }
455 0 : std::pop_heap(this->myFrontierList.begin(), this->myFrontierList.end(), myComparator);
456 : this->myFrontierList.pop_back();
457 0 : this->myFound.push_back(minimumInfo);
458 0 : minimumInfo->visited = true;
459 :
460 0 : const double effortDelta = this->getEffort(minEdge, vehicle, minimumInfo->leaveTime);
461 0 : const double leaveTime = minimumInfo->leaveTime + this->getTravelTime(minEdge, vehicle, minimumInfo->leaveTime, effortDelta);
462 :
463 : // admissible A* heuristic: straight line distance at maximum speed
464 : // this is calculated from the end of minEdge so it possibly includes via efforts to the followers
465 : double heuristic_remaining = 0.;
466 0 : double heuristicEffort = minimumInfo->effort + effortDelta + heuristic_remaining;
467 : // check all ways from the edge with the minimal length
468 0 : for (const std::pair<const E*, const E*>& follower : minEdge->getViaSuccessors(vClass)) {
469 0 : auto& followerInfo = this->myEdgeInfos[follower.first->getNumericalID()];
470 0 : const FlagInfo* followerFlagInfo = (*myFlagInfos)[follower.first->getNumericalID()];
471 : // check whether it can be used
472 0 : if (this->isProhibited(follower.first, vehicle, leaveTime)) {
473 0 : continue;
474 : }
475 : #ifdef AFRO_DEBUG_LEVEL_1
476 : numberOfFollowers++;
477 : if (followerFlagInfo->arcFlags.empty()) {
478 : numberOfEmptyFlagVectors++;
479 : }
480 : #endif
481 : #ifdef AFRO_DEBUG_LEVEL_2
482 : myFlagContextStartTime = SysUtils::getCurrentMillis();
483 : #endif
484 0 : std::tuple<int, int, bool> flagContext = this->flagContext(follower.first, to);
485 : //std::tuple<int, int, bool> flagContext = this->flagContextNaive(follower.first, to);
486 : #ifdef AFRO_DEBUG_LEVEL_2
487 : myFlagContextTimeSum += (SysUtils::getCurrentMillis() - myFlagContextStartTime);
488 : #endif
489 0 : if (!flag(followerFlagInfo, flagContext)) {
490 : #ifdef AFRO_DEBUG_LEVEL_1
491 : numberOfAvoidedFollowers++;
492 : #endif
493 0 : continue;
494 : }
495 :
496 : // admissible A* heuristic: straight line distance at maximum speed
497 : // this is calculated from the end of minEdge so it possibly includes via efforts to the followers
498 0 : if (heuristic_remaining == 0 && std::get<0>(flagContext) == 0 && std::get<2>(flagContext)) {
499 : // arrived at the target cell at level 0? use heuristic
500 : heuristic_remaining =
501 0 : (myLookupTable == nullptr ? minEdge->getDistanceTo(to) / speed :
502 0 : myLookupTable->lowerBound(minEdge, to, speed, vehicle->getChosenSpeedFactor(),
503 : minEdge->getMinimumTravelTime(nullptr), to->getMinimumTravelTime(nullptr)));
504 0 : if (heuristic_remaining == UNREACHABLE) {
505 : break; // -> skip remaining followers, continue with next min heap element
506 : }
507 0 : heuristicEffort += heuristic_remaining;
508 : }
509 :
510 0 : double effort = minimumInfo->effort + effortDelta;
511 0 : double time = leaveTime;
512 0 : this->updateViaEdgeCost(follower.second, vehicle, time, effort, length);
513 0 : const double oldEffort = followerInfo.effort;
514 0 : if ((!followerInfo.visited || mayRevisit) && effort < oldEffort) {
515 0 : followerInfo.effort = effort;
516 : // if we use the effort including the via effort below we would count the via twice as shown by the ticket676 test
517 : // but we should never get below the real effort, see #12463
518 0 : followerInfo.heuristicEffort = MAX2(MIN2(heuristicEffort, followerInfo.heuristicEffort), effort);
519 0 : followerInfo.leaveTime = time;
520 0 : followerInfo.prev = minimumInfo;
521 0 : if (oldEffort == std::numeric_limits<double>::max()) {
522 0 : this->myFrontierList.push_back(&followerInfo);
523 : std::push_heap(this->myFrontierList.begin(), this->myFrontierList.end(), myComparator);
524 : } else {
525 0 : auto fi = std::find(this->myFrontierList.begin(), this->myFrontierList.end(), &followerInfo);
526 0 : if (fi == this->myFrontierList.end()) {
527 : assert(mayRevisit);
528 0 : this->myFrontierList.push_back(&followerInfo);
529 : std::push_heap(this->myFrontierList.begin(), this->myFrontierList.end(), myComparator);
530 : } else {
531 : std::push_heap(this->myFrontierList.begin(), fi + 1, myComparator);
532 : }
533 : }
534 : }
535 : } // for followers
536 : }
537 0 : this->endQuery(num_visited);
538 : #ifdef AFRO_DEBUG_LEVEL_1
539 : std::cout << "Queue ran empty, no solution." << std::endl;
540 : std::cout << static_cast<double>(numberOfFollowers - numberOfAvoidedFollowers) / static_cast<double>(num_visited)
541 : << " followers considered (out of " << static_cast<double>(numberOfFollowers) / static_cast<double>(num_visited) << ") on average." << std::endl;
542 : std::cout << static_cast<double>(numberOfFollowers - numberOfAvoidedFollowers)
543 : << " followers considered (out of " << static_cast<double>(numberOfFollowers) << ")." << std::endl;
544 : std::cout << numberOfEmptyFlagVectors << " out of " << numberOfFollowers << " flag vectors of followers were unassigned (i.e., empty)." << std::endl;
545 : std::cout << "num_visited: " << num_visited << std::endl;
546 : #endif
547 0 : if (!silent) {
548 0 : this->myErrorMsgHandler->informf("No connection between edge '%' and edge '%' found.", from->getID(), to->getID());
549 : }
550 : return false;
551 : }
552 :
553 : /// @brief Start timer for query time sum
554 : void startQuery();
555 : /// @brief Stop timer for query time sum
556 : void endQuery(int visits);
557 : /// @brief Report query time statistics
558 : void reportStatistics();
559 : /// @brief Reset query time statistics
560 : void resetStatistics();
561 : /// @brief Bulk mode is not supported
562 0 : virtual void setBulkMode(const bool mode) {
563 : UNUSED_PARAMETER(mode);
564 0 : throw std::runtime_error("Bulk mode is not supported by the arc flag router.");
565 : }
566 :
567 : protected:
568 : /// @brief Edge infos containing the associated edge and its arc flags
569 : std::vector<FlagInfo*>* myFlagInfos;
570 : /// @brief The partition
571 : const KDTreePartition<E, N, V>* myPartition;
572 : /// @brief The comparator for edge information
573 : EdgeInfoComparator myComparator;
574 : /// @brief The lookup table for travel time heuristics
575 : const std::shared_ptr<const LookupTable> myLookupTable;
576 : /// @brief The maximum speed in the network
577 : double myMaxSpeed;
578 : /// @brief The validity duration of one weight interval
579 : const SUMOTime myWeightPeriod;
580 : /// @brief The validity duration of the current flag infos (exclusive)
581 : SUMOTime myValidUntil;
582 : /// @brief The builder
583 : AFBuilder<E, N, V, M>* myBuilder;
584 : /// @brief The type of this router
585 : /// @note The one in SUMOAbstractRouter is private, required for more flexible performance logging (see below)
586 : const std::string myType;
587 : /// @brief Counters for performance logging
588 : /// @note The ones in SUMOAbstractRouter are private - introduced to reset stats / recalculate before destruction
589 : long long int myQueryVisits;
590 : long long int myNumQueries;
591 : /// @brief The time spent querying in milliseconds
592 : /// @note The ones in SUMOAbstractRouter are private - introduced to reset stats / recalculate before destruction
593 : long long int myQueryStartTime;
594 : long long int myQueryTimeSum;
595 : #ifdef AFRO_DEBUG_LEVEL_2
596 : /// @brief The time spent for flagContext in milliseconds
597 : long long int myFlagContextStartTime;
598 : long long int myFlagContextTimeSum;
599 : #endif
600 : private:
601 : /// @brief The cell of the last settled edge
602 : const Cell* myLastSettledEdgeCell;
603 : /// @brief The last flag context
604 : std::tuple<int, int, bool> myLastFlagContext;
605 : /// @brief The cell of the target edge at SHARC level 0
606 : const Cell* myTargetEdgeCellLevel0;
607 : };
608 :
609 : // ===========================================================================
610 : // method definitions
611 : // ===========================================================================
612 :
613 : template<class E, class N, class V, class M>
614 : std::vector<bool>& AFRouter<E, N, V, M>::flags(const E* edge) {
615 : assert(edge);
616 : if (!myFlagInfos) {
617 : throw std::runtime_error("flag infos not initialized, call compute() at least once before calling flags().");
618 : }
619 : return ((*myFlagInfos)[edge->getNumericalID()])->arcFlags;
620 : }
621 :
622 : template<class E, class N, class V, class M>
623 0 : void AFRouter<E, N, V, M>::init(const int edgeID, const SUMOTime msTime) {
624 : // all EdgeInfos touched in the previous query are either in myFrontierList or myFound: clean those up
625 0 : myTargetEdgeCellLevel0 = nullptr;
626 0 : for (auto& edgeInfo : this->myFrontierList) {
627 0 : edgeInfo->reset();
628 : }
629 : this->myFrontierList.clear();
630 0 : for (auto& edgeInfo : this->myFound) {
631 0 : edgeInfo->reset();
632 : }
633 : this->myFound.clear();
634 0 : if (edgeID > -1) {
635 : // add begin node
636 0 : auto& fromInfo = this->myEdgeInfos[edgeID];
637 0 : fromInfo.heuristicEffort = 0.;
638 0 : fromInfo.effort = 0.;
639 0 : fromInfo.leaveTime = STEPS2TIME(msTime);
640 0 : fromInfo.prev = nullptr;
641 0 : this->myFrontierList.push_back(&fromInfo);
642 : }
643 0 : }
644 :
645 : template<class E, class N, class V, class M>
646 : std::tuple<int, int, bool> AFRouter<E, N, V, M>::flagContextNaive(const E* settledEdge, const E* targetEdge) {
647 : assert(settledEdge != nullptr && targetEdge != nullptr);
648 : int sHARCLevel;
649 : for (sHARCLevel = 0; sHARCLevel < myPartition->getNumberOfLevels() - 1; sHARCLevel++) {
650 : int partitionLevel = sHARCLevel2PartitionLevel(sHARCLevel, myPartition->getNumberOfLevels());
651 : const std::vector<const Cell*>& levelCells = myPartition->getCellsAtLevel(partitionLevel);
652 : typename std::vector<const Cell*>::const_iterator first = levelCells.begin();
653 : typename std::vector<const Cell*>::const_iterator last = levelCells.end();
654 : typename std::vector<const Cell*>::const_iterator iter;
655 : const Cell* settledEdgeCell = nullptr;
656 : const Cell* targetEdgeCell = nullptr;
657 : // go through the cells of the level
658 : for (iter = first; iter != last; iter++) {
659 : // myPartition is assumed to partition a non-reversed (forward) graph
660 : if (!settledEdgeCell && (*iter)->contains(settledEdge->getFromJunction())) {
661 : settledEdgeCell = *iter;
662 : }
663 : if (!targetEdgeCell && (*iter)->contains(targetEdge->getFromJunction())) {
664 : targetEdgeCell = *iter;
665 : }
666 : if (settledEdgeCell && targetEdgeCell) {
667 : break;
668 : }
669 : }
670 : assert(settledEdgeCell && targetEdgeCell); // we should find both edges on each level
671 : if (settledEdgeCell->getSupercell() == targetEdgeCell->getSupercell()) {
672 : return std::make_tuple(sHARCLevel, targetEdgeCell->isLeftOrLowerCell() ? 0 : 1,
673 : settledEdgeCell == targetEdgeCell);
674 : }
675 : }
676 : // we should never arrive here
677 : throw std::runtime_error("flagContext: relevant level could not be determined.");
678 : }
679 :
680 : template<class E, class N, class V, class M>
681 0 : std::tuple<int, int, bool> AFRouter<E, N, V, M>::flagContext(const E* settledEdge, const E* targetEdge) {
682 : assert(settledEdge != nullptr && targetEdge != nullptr);
683 : int sHARCLevel = 0; // lowest level with smallest cells
684 : const Cell* settledEdgeCell = nullptr;
685 : const Cell* targetEdgeCell = nullptr;
686 0 : if (myLastSettledEdgeCell
687 0 : && myLastSettledEdgeCell->contains(settledEdge->getFromJunction())) {
688 : // exploit the partial locality of Dijkstra's algorithm: settled edge is still
689 : // in the same cell as the last one? Then we can simply return the
690 : // last flagContext tuple again.
691 0 : return myLastFlagContext;
692 : }
693 0 : int numberOfPartitionLevels = myPartition->getNumberOfLevels();
694 0 : if (numberOfPartitionLevels <= 4) { // small number of bottom cells -> go through them, no use of k-d tree
695 0 : int partitionLevel = sHARCLevel2PartitionLevel(sHARCLevel, myPartition->getNumberOfLevels());
696 0 : const std::vector<const Cell*>& levelCells = myPartition->getCellsAtLevel(partitionLevel);
697 : typename std::vector<const Cell*>::const_iterator first = levelCells.begin();
698 : typename std::vector<const Cell*>::const_iterator last = levelCells.end();
699 : typename std::vector<const Cell*>::const_iterator iter;
700 : // go through the cells of the level
701 0 : for (iter = first; iter != last; iter++) {
702 : // myPartition is assumed to partition a non-reversed (forward) graph
703 : if (!settledEdgeCell
704 0 : && (*iter)->contains(settledEdge->getFromJunction())) {
705 : settledEdgeCell = *iter;
706 : }
707 0 : if (!targetEdgeCell && myTargetEdgeCellLevel0) {
708 : targetEdgeCell = myTargetEdgeCellLevel0;
709 : } else if (!targetEdgeCell
710 0 : && (*iter)->contains(targetEdge->getFromJunction())) {
711 0 : myTargetEdgeCellLevel0 = *iter;
712 : targetEdgeCell = myTargetEdgeCellLevel0;
713 : }
714 0 : if (settledEdgeCell && targetEdgeCell) {
715 : assert(myTargetEdgeCellLevel0);
716 : break;
717 : }
718 : }
719 : } else { // larger number of bottom cells -> use a k-d tree
720 0 : settledEdgeCell = myPartition->searchNode(settledEdge->getFromJunction());
721 0 : if (!targetEdgeCell && myTargetEdgeCellLevel0) {
722 : // search only once per query
723 : targetEdgeCell = myTargetEdgeCellLevel0;
724 : } else if (!targetEdgeCell) {
725 0 : myTargetEdgeCellLevel0 = myPartition->searchNode(targetEdge->getFromJunction());
726 : targetEdgeCell = myTargetEdgeCellLevel0; // myTargetEdgeCellLevel0 is reset in init()
727 : }
728 : }
729 : assert(settledEdgeCell && targetEdgeCell); // we should find both edges on each level
730 0 : while (settledEdgeCell->getSupercell() != targetEdgeCell->getSupercell()) {
731 : settledEdgeCell = settledEdgeCell->getSupercell();
732 : targetEdgeCell = targetEdgeCell->getSupercell();
733 0 : sHARCLevel++;
734 : }
735 0 : myLastSettledEdgeCell = settledEdgeCell;
736 0 : std::tuple<int, int, bool> flagContext = std::make_tuple(sHARCLevel, targetEdgeCell->isLeftOrLowerCell() ? 0 : 1,
737 0 : settledEdgeCell == targetEdgeCell);
738 : myLastFlagContext = flagContext;
739 : return flagContext;
740 : }
741 :
742 : template<class E, class N, class V, class M>
743 0 : void AFRouter<E, N, V, M>::startQuery() {
744 0 : myNumQueries++;
745 0 : myQueryStartTime = SysUtils::getCurrentMillis();
746 : SUMOAbstractRouter<E, V>::startQuery();
747 0 : }
748 :
749 : template<class E, class N, class V, class M>
750 0 : void AFRouter<E, N, V, M>::endQuery(int visits) {
751 0 : myQueryVisits += visits;
752 0 : myQueryTimeSum += (SysUtils::getCurrentMillis() - myQueryStartTime);
753 : SUMOAbstractRouter<E, V>::endQuery(visits);
754 0 : }
755 :
756 : template<class E, class N, class V, class M>
757 : void AFRouter<E, N, V, M>::reportStatistics() {
758 : if (myNumQueries > 0) {
759 : WRITE_MESSAGE(myType + " answered " + toString(myNumQueries) + " queries and explored " + toString((double)myQueryVisits / (double)myNumQueries) + " edges on average.");
760 : WRITE_MESSAGE(myType + " spent " + elapsedMs2string(myQueryTimeSum) + " answering queries (" + toString((double)myQueryTimeSum / (double)myNumQueries) + " ms on average).");
761 : #ifdef AFRO_DEBUG_LEVEL_2
762 : WRITE_MESSAGE("flagContext spent " + elapsedMs2string(myFlagContextTimeSum) + " (" + toString((double)myFlagContextTimeSum / (double)myNumQueries) + " ms on average).");
763 : #endif
764 : }
765 : }
766 :
767 : template<class E, class N, class V, class M>
768 : void AFRouter<E, N, V, M>::resetStatistics() {
769 : myNumQueries = 0;
770 : myQueryVisits = 0;
771 : myQueryTimeSum = 0;
772 : myQueryStartTime = 0;
773 : }
|
