-
Notifications
You must be signed in to change notification settings - Fork 1.4k
/
DijkstraRouter.h
269 lines (255 loc) · 13.2 KB
/
DijkstraRouter.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
/****************************************************************************/
// Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
// Copyright (C) 2001-2024 German Aerospace Center (DLR) and others.
// This program and the accompanying materials are made available under the
// terms of the Eclipse Public License 2.0 which is available at
// https://www.eclipse.org/legal/epl-2.0/
// This Source Code may also be made available under the following Secondary
// Licenses when the conditions for such availability set forth in the Eclipse
// Public License 2.0 are satisfied: GNU General Public License, version 2
// or later which is available at
// https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
// SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
/****************************************************************************/
/// @file DijkstraRouter.h
/// @author Daniel Krajzewicz
/// @author Jakob Erdmann
/// @author Michael Behrisch
/// @date Mon, 25 July 2005
///
// Dijkstra shortest path algorithm using travel time or other values
/****************************************************************************/
#pragma once
#include <config.h>
#include <cassert>
#include <string>
#include <functional>
#include <vector>
#include <set>
#include <limits>
#include <algorithm>
#include <iterator>
#include <utils/common/ToString.h>
#include <utils/common/MsgHandler.h>
#include <utils/common/StdDefs.h>
#include "EffortCalculator.h"
#include "SUMOAbstractRouter.h"
//#define DijkstraRouter_DEBUG_QUERY
//#define DijkstraRouter_DEBUG_QUERY_VISITED
//#define DijkstraRouter_DEBUG_QUERY_PERF
//#define DijkstraRouter_DEBUG_BULKMODE
#define DijkstraRouter_DEBUG_QUERY_VISITED_OUT std::cerr
// ===========================================================================
// class definitions
// ===========================================================================
/**
* @class DijkstraRouter
* @brief Computes the shortest path through a network using the Dijkstra algorithm.
*
* The template parameters are:
* @param E The edge class to use (MSEdge/ROEdge)
* @param V The vehicle class to use (MSVehicle/ROVehicle)
*
* The router is edge-based. It must know the number of edges for internal reasons
* and whether a missing connection between two given edges (unbuild route) shall
* be reported as an error or as a warning.
*
*/
template<class E, class V>
class DijkstraRouter : public SUMOAbstractRouter<E, V> {
public:
/**
* @class EdgeInfoByEffortComparator
* Class to compare (and so sort) nodes by their effort
*/
class EdgeInfoByEffortComparator {
public:
/// Comparing method
bool operator()(const typename SUMOAbstractRouter<E, V>::EdgeInfo* nod1, const typename SUMOAbstractRouter<E, V>::EdgeInfo* nod2) const {
if (nod1->effort == nod2->effort) {
return nod1->edge->getNumericalID() > nod2->edge->getNumericalID();
}
return nod1->effort > nod2->effort;
}
};
/// Constructor
DijkstraRouter(const std::vector<E*>& edges, bool unbuildIsWarning, typename SUMOAbstractRouter<E, V>::Operation effortOperation,
typename SUMOAbstractRouter<E, V>::Operation ttOperation = nullptr, bool silent = false, EffortCalculator* calc = nullptr,
const bool havePermissions = false, const bool haveRestrictions = false) :
SUMOAbstractRouter<E, V>("DijkstraRouter", unbuildIsWarning, effortOperation, ttOperation, havePermissions, haveRestrictions),
mySilent(silent), myExternalEffort(calc) {
for (typename std::vector<E*>::const_iterator i = edges.begin(); i != edges.end(); ++i) {
this->myEdgeInfos.push_back(typename SUMOAbstractRouter<E, V>::EdgeInfo(*i));
}
}
/// Destructor
virtual ~DijkstraRouter() { }
virtual SUMOAbstractRouter<E, V>* clone() {
auto clone = new DijkstraRouter<E, V>(this->myEdgeInfos, this->myErrorMsgHandler == MsgHandler::getWarningInstance(),
this->myOperation, this->myTTOperation, mySilent, myExternalEffort,
this->myHavePermissions, this->myHaveRestrictions);
clone->setAutoBulkMode(this->myAutoBulkMode);
return clone;
}
/** @brief Builds the route between the given edges using the minimum effort at the given time
The definition of the effort depends on the wished routing scheme */
bool compute(const E* from, const E* to, const V* const vehicle,
SUMOTime msTime, std::vector<const E*>& into, bool silent = false) {
assert(from != nullptr && (vehicle == nullptr || to != nullptr));
// check whether from and to can be used
if (this->myEdgeInfos[from->getNumericalID()].prohibited || this->isProhibited(from, vehicle)) {
if (!silent) {
this->myErrorMsgHandler->inform("Vehicle '" + Named::getIDSecure(vehicle) + "' is not allowed on source edge '" + from->getID() + "'.");
}
return false;
}
if (to != nullptr && (this->myEdgeInfos[to->getNumericalID()].prohibited || this->isProhibited(to, vehicle))) {
if (!silent) {
this->myErrorMsgHandler->inform("Vehicle '" + Named::getIDSecure(vehicle) + "' is not allowed on destination edge '" + to->getID() + "'.");
}
return false;
}
double length = 0.; // dummy for the via edge cost update
this->startQuery();
#ifdef DijkstraRouter_DEBUG_QUERY
std::cout << "DEBUG: starting search for '" << Named::getIDSecure(vehicle) << "' time: " << STEPS2TIME(msTime) << "\n";
#endif
const SUMOVehicleClass vClass = vehicle == nullptr ? SVC_IGNORING : vehicle->getVClass();
const bool ignoreTransient = vehicle == nullptr ? false : vehicle->ignoreTransientPermissions();
std::tuple<const E*, const V*, SUMOTime> query = std::make_tuple(from, vehicle, msTime);
if ((this->myBulkMode || (this->myAutoBulkMode && query == myLastQuery)) && !this->myAmClean) {
#ifdef DijkstraRouter_DEBUG_BULKMODE
if (query != myLastQuery) {
std::cout << " invalid bulk mode. myLastQuery="
<< std::get<0>(myLastQuery)->getID() << ","
<< std::get<1>(myLastQuery)->getID() << ","
<< time2string(std::get<2>(myLastQuery))
<< " query="
<< std::get<0>(query)->getID() << ","
<< std::get<1>(query)->getID() << ","
<< time2string(std::get<2>(query))
<< "\n";
}
#endif
const auto& toInfo = this->myEdgeInfos[to->getNumericalID()];
if (toInfo.visited) {
this->buildPathFrom(&toInfo, into);
this->endQuery(1);
return true;
}
} else {
this->init(from->getNumericalID(), msTime);
if (myExternalEffort != nullptr) {
myExternalEffort->setInitialState(from->getNumericalID());
}
this->myAmClean = false;
}
myLastQuery = query;
// loop
int num_visited = 0;
#ifdef DijkstraRouter_DEBUG_QUERY_VISITED
DijkstraRouter_DEBUG_QUERY_VISITED_OUT << "<edgeData>\n <interval begin=\"" << time2string(msTime) << "\" end=\"" << toString(msTime + 1000) << "\">\n";
#endif
while (!this->myFrontierList.empty()) {
num_visited += 1;
// use the node with the minimal length
auto* const minimumInfo = this->myFrontierList.front();
const E* const minEdge = minimumInfo->edge;
#ifdef DijkstraRouter_DEBUG_QUERY
std::cout << "DEBUG: hit=" << minEdge->getID()
<< " TT=" << minimumInfo->effort
<< " EF=" << this->getEffort(minEdge, vehicle, minimumInfo->leaveTime)
<< " Leave: " << minimumInfo->leaveTime << " Q: ";
for (const auto& edgeInfo : this->myFrontierList) {
std::cout << "\n " << edgeInfo->effort << ", " << edgeInfo->edge->getID();
}
std::cout << "\n";
#endif
#ifdef DijkstraRouter_DEBUG_QUERY_VISITED
DijkstraRouter_DEBUG_QUERY_VISITED_OUT << " <edge id=\"" << minEdge->getID() << "\" index=\"" << num_visited << "\" cost=\"" << minimumInfo->effort << "\" time=\"" << minimumInfo->leaveTime << "\"/>\n";
#endif
// check whether the destination node was already reached
if (minEdge == to) {
//propagate last external effort state to destination edge
if (myExternalEffort != nullptr) {
myExternalEffort->update(minEdge->getNumericalID(), minimumInfo->prev->edge->getNumericalID(), minEdge->getLength());
}
this->buildPathFrom(minimumInfo, into);
this->endQuery(num_visited);
#ifdef DijkstraRouter_DEBUG_QUERY_PERF
const double cost = this->recomputeCosts(into, vehicle, msTime);
std::cout << "visited " + toString(num_visited) + " edges (final path length=" + toString(into.size()) + " cost=" << cost << " edges=" + toString(into) + ")\n";
#endif
#ifdef DijkstraRouter_DEBUG_QUERY_VISITED
DijkstraRouter_DEBUG_QUERY_VISITED_OUT << " </interval>\n</edgeData>\n";
#endif
return true;
}
std::pop_heap(this->myFrontierList.begin(), this->myFrontierList.end(), myComparator);
this->myFrontierList.pop_back();
this->myFound.push_back(minimumInfo);
minimumInfo->visited = true;
const double effortDelta = this->getEffort(minEdge, vehicle, minimumInfo->leaveTime);
const double leaveTime = minimumInfo->leaveTime + this->getTravelTime(minEdge, vehicle, minimumInfo->leaveTime, effortDelta);
if (myExternalEffort != nullptr) {
myExternalEffort->update(minEdge->getNumericalID(), minimumInfo->prev->edge->getNumericalID(), minEdge->getLength());
}
// check all ways from the node with the minimal length
for (const std::pair<const E*, const E*>& follower : minEdge->getViaSuccessors(vClass, ignoreTransient)) {
auto& followerInfo = this->myEdgeInfos[follower.first->getNumericalID()];
// check whether it can be used
if (followerInfo.prohibited || this->isProhibited(follower.first, vehicle)) {
continue;
}
double effort = minimumInfo->effort + effortDelta;
double time = leaveTime;
this->updateViaEdgeCost(follower.second, vehicle, time, effort, length);
assert(effort >= minimumInfo->effort);
assert(time >= minimumInfo->leaveTime);
const double oldEffort = followerInfo.effort;
if (!followerInfo.visited && effort < oldEffort) {
followerInfo.effort = effort;
followerInfo.leaveTime = time;
followerInfo.prev = minimumInfo;
if (oldEffort == std::numeric_limits<double>::max()) {
this->myFrontierList.push_back(&followerInfo);
std::push_heap(this->myFrontierList.begin(), this->myFrontierList.end(), myComparator);
} else {
std::push_heap(this->myFrontierList.begin(),
std::find(this->myFrontierList.begin(), this->myFrontierList.end(), &followerInfo) + 1,
myComparator);
}
}
}
}
this->endQuery(num_visited);
#ifdef DijkstraRouter_DEBUG_QUERY_PERF
std::cout << "visited " + toString(num_visited) + " edges (unsuccessful path length: " + toString(into.size()) + ")\n";
#endif
if (to != nullptr && !mySilent && !silent) {
this->myErrorMsgHandler->informf(TL("No connection between edge '%' and edge '%' found."), from->getID(), to->getID());
}
#ifdef DijkstraRouter_DEBUG_QUERY_VISITED
DijkstraRouter_DEBUG_QUERY_VISITED_OUT << " </interval>\n</edgeData>\n";
#endif
return false;
}
private:
DijkstraRouter(const std::vector<typename SUMOAbstractRouter<E, V>::EdgeInfo>& edgeInfos, bool unbuildIsWarning,
typename SUMOAbstractRouter<E, V>::Operation effortOperation, typename SUMOAbstractRouter<E, V>::Operation ttOperation, bool silent, EffortCalculator* calc,
const bool havePermissions, const bool haveRestrictions) :
SUMOAbstractRouter<E, V>("DijkstraRouter", unbuildIsWarning, effortOperation, ttOperation, havePermissions, haveRestrictions),
mySilent(silent),
myExternalEffort(calc) {
for (const auto& edgeInfo : edgeInfos) {
this->myEdgeInfos.push_back(typename SUMOAbstractRouter<E, V>::EdgeInfo(edgeInfo.edge));
}
}
private:
/// @brief whether to suppress warning/error if no route was found
bool mySilent;
/// cache of the last query to enable automated bulk routing
std::tuple<const E*, const V*, SUMOTime> myLastQuery;
EffortCalculator* const myExternalEffort;
EdgeInfoByEffortComparator myComparator;
};