17 #ifndef AStarLookupTable_h 18 #define AStarLookupTable_h 33 #define UNREACHABLE (std::numeric_limits<double>::max() / 1000.0) 58 template<
class E,
class V>
62 virtual double lowerBound(
const E* from,
const E* to,
double speed,
double speedFactor,
double fromEffort,
double toEffort)
const = 0;
69 template<
class E,
class V>
75 for (
int i = 0; i < size; i++) {
76 for (
int j = 0; j < size; j++) {
79 myTable[i].push_back(val);
84 double lowerBound(
const E* from,
const E* to,
double ,
double speedFactor,
double ,
double )
const {
85 return myTable[from->getNumericalID()][to->getNumericalID()] / speedFactor;
93 std::vector<std::vector<double> >
myTable;
97 template<
class E,
class V>
101 myFirstNonInternal = -1;
102 std::map<std::string, int> numericID;
104 if (!e->isInternal()) {
105 if (myFirstNonInternal == -1) {
106 myFirstNonInternal = e->getNumericalID();
108 numericID[e->getID()] = e->getNumericalID() - myFirstNonInternal;
111 std::ifstream strm(filename.c_str());
113 throw ProcessError(
"Could not load landmark-lookup-table from '" + filename +
"'.");
115 std::ofstream* ostrm =
nullptr;
116 if (!outfile.empty()) {
117 ostrm =
new std::ofstream(outfile.c_str());
118 if (!ostrm->good()) {
119 throw ProcessError(
"Could not open file '" + outfile +
"' for writing.");
123 int numLandMarks = 0;
124 while (std::getline(strm, line)) {
130 if (st.
size() == 1) {
131 const std::string lm = st.
get(0);
132 myLandmarks[lm] = numLandMarks++;
133 myFromLandmarkDists.push_back(std::vector<double>(0));
134 myToLandmarkDists.push_back(std::vector<double>(0));
135 if (ostrm !=
nullptr) {
136 (*ostrm) << lm <<
"\n";
139 assert(st.
size() == 4);
140 const std::string lm = st.
get(0);
141 const std::string edge = st.
get(1);
142 if (numericID[edge] != (
int)myFromLandmarkDists[myLandmarks[lm]].size()) {
143 WRITE_WARNING(
"Unknown or unordered edge '" + edge +
"' in landmark file.");
147 myFromLandmarkDists[myLandmarks[lm]].push_back(distFrom);
148 myToLandmarkDists[myLandmarks[lm]].push_back(distTo);
151 if (myLandmarks.empty()) {
152 WRITE_WARNING(
"No landmarks in '" + filename +
"', falling back to standard A*.");
159 for (
int i = 0; i < (int)myLandmarks.size(); ++i) {
160 if ((
int)myFromLandmarkDists[i].size() != (int)edges.size() - myFirstNonInternal) {
161 const std::string landmarkID = getLandmark(i);
162 const E* landmark =
nullptr;
164 for (
const E*
const edge : edges) {
165 if (edge->getID() == landmarkID) {
170 if (landmark ==
nullptr) {
171 WRITE_WARNING(
"Landmark '" + landmarkID +
"' does not exist in the network.");
174 if (router !=
nullptr) {
175 const std::string missing = outfile.empty() ? filename +
".missing" : outfile;
176 WRITE_WARNING(
"Not all network edges were found in the lookup table '" + filename +
"' for landmark '" + landmarkID +
"'. Saving missing values to '" + missing +
"'.");
177 if (ostrm ==
nullptr) {
178 ostrm =
new std::ofstream(missing.c_str());
179 if (!ostrm->good()) {
180 throw ProcessError(
"Could not open file '" + missing +
"' for writing.");
184 throw ProcessError(
"Not all network edges were found in the lookup table '" + filename +
"' for landmark '" + landmarkID +
"'.");
186 std::vector<const E*> routeLM(1, landmark);
187 const double lmCost = router->
recomputeCosts(routeLM, defaultVehicle, 0);
188 std::vector<const E*> route;
190 if (maxNumThreads > 0) {
191 if (threadPool.
size() == 0) {
194 router->
compute(landmark, landmark, defaultVehicle, 0, route);
196 while ((
int)threadPool.
size() < maxNumThreads) {
197 new WorkerThread(threadPool, router->
clone(), defaultVehicle);
200 std::vector<RoutingTask*> currentTasks;
201 for (
int j = (
int)myFromLandmarkDists[i].size() + myFirstNonInternal; j < (int)edges.size(); ++j) {
202 const E* edge = edges[j];
203 if (landmark != edge) {
204 std::vector<const E*> routeE(1, edge);
205 const double sourceDestCost = lmCost + router->
recomputeCosts(routeE, defaultVehicle, 0);
207 if (edge->getPredecessors().size() > 0 && landmark->getSuccessors().size() > 0) {
208 currentTasks.push_back(
new RoutingTask(landmark, edge, sourceDestCost));
209 threadPool.
add(currentTasks.back());
212 if (landmark->getPredecessors().size() > 0 && edge->getSuccessors().size() > 0) {
213 currentTasks.push_back(
new RoutingTask(edge, landmark, sourceDestCost));
214 threadPool.
add(currentTasks.back());
220 for (
int j = (
int)myFromLandmarkDists[i].size() + myFirstNonInternal; j < (int)edges.size(); ++j) {
221 const E* edge = edges[j];
222 double distFrom = -1;
224 if (landmark == edge) {
228 if (edge->getPredecessors().size() > 0 && landmark->getSuccessors().size() > 0) {
229 distFrom = currentTasks[taskIndex]->getCost();
230 delete currentTasks[taskIndex++];
232 if (landmark->getPredecessors().size() > 0 && edge->getSuccessors().size() > 0) {
233 distTo = currentTasks[taskIndex]->getCost();
234 delete currentTasks[taskIndex++];
237 myFromLandmarkDists[i].push_back(distFrom);
238 myToLandmarkDists[i].push_back(distTo);
239 (*ostrm) << landmarkID <<
" " << edge->getID() <<
" " << distFrom <<
" " << distTo <<
"\n";
241 currentTasks.clear();
245 for (
int j = (
int)myFromLandmarkDists[i].size() + myFirstNonInternal; j < (int)edges.size(); ++j) {
246 const E* edge = edges[j];
247 double distFrom = -1.;
249 if (landmark == edge) {
253 std::vector<const E*> routeE(1, edge);
254 const double sourceDestCost = lmCost + router->
recomputeCosts(routeE, defaultVehicle, 0);
256 if (edge->getPredecessors().size() > 0 && landmark->getSuccessors().size() > 0) {
257 if (router->
compute(landmark, edge, defaultVehicle, 0, route)) {
258 distFrom =
MAX2(0.0, router->
recomputeCosts(route, defaultVehicle, 0) - sourceDestCost);
263 if (landmark->getPredecessors().size() > 0 && edge->getSuccessors().size() > 0) {
264 if (router->
compute(edge, landmark, defaultVehicle, 0, route)) {
265 distTo =
MAX2(0.0, router->
recomputeCosts(route, defaultVehicle, 0) - sourceDestCost);
270 myFromLandmarkDists[i].push_back(distFrom);
271 myToLandmarkDists[i].push_back(distTo);
272 (*ostrm) << landmarkID <<
" " << edge->getID() <<
" " << distFrom <<
" " << distTo <<
"\n";
279 double lowerBound(
const E* from,
const E* to,
double speed,
double speedFactor,
double fromEffort,
double toEffort)
const {
280 double result = from->getDistanceTo(to) / speed;
281 #ifdef ASTAR_DEBUG_LOOKUPTABLE 282 if (from->getID() == ASTAR_DEBUG_LOOKUPTABLE_FROM) {
283 std::cout <<
" lowerBound to=" << to->getID() <<
" result1=" << result <<
"\n";
286 for (
int i = 0; i < (int)myLandmarks.size(); ++i) {
288 const double fl = myToLandmarkDists[i][from->getNumericalID() - myFirstNonInternal];
289 const double tl = myToLandmarkDists[i][to->getNumericalID() - myFirstNonInternal];
290 if (fl >= 0 && tl >= 0) {
291 const double bound = (fl - tl - toEffort) / speedFactor;
292 #ifdef ASTAR_DEBUG_LOOKUPTABLE 293 if (from->getID() == ASTAR_DEBUG_LOOKUPTABLE_FROM && result < bound) {
294 std::cout <<
" landmarkTo=" << getLandmark(i) <<
" result2=" << bound
295 <<
" fl=" << fl <<
" tl=" << tl <<
"\n";
298 result =
MAX2(result, bound);
300 const double lt = myFromLandmarkDists[i][to->getNumericalID() - myFirstNonInternal];
301 const double lf = myFromLandmarkDists[i][from->getNumericalID() - myFirstNonInternal];
302 if (lt >= 0 && lf >= 0) {
303 const double bound = (lt - lf - fromEffort) / speedFactor;
304 #ifdef ASTAR_DEBUG_LOOKUPTABLE 305 if (from->getID() == ASTAR_DEBUG_LOOKUPTABLE_FROM && result < bound) {
306 std::cout <<
" landmarkFrom=" << getLandmark(i) <<
" result3=" << bound
307 <<
" lt=" << lt <<
" lf=" << lf <<
"\n";
310 result =
MAX2(result, bound);
312 if ((tl >= 0 && fl < 0)
313 || (lf >= 0 && lt < 0)) {
315 #ifdef ASTAR_DEBUG_UNREACHABLE 316 std::cout <<
" unreachable: from=" << from->getID() <<
" to=" << to->getID() <<
" landmark=" << getLandmark(i) <<
" " 317 << ((tl >= 0 && fl < 0) ?
" (toLandmark)" :
" (fromLandmark)")
318 <<
" fl=" << fl <<
" tl=" << tl <<
" lt=" << lt <<
" lf=" << lf
344 virtual ~WorkerThread() {
347 double compute(
const E* src,
const E* dest,
const double costOff) {
349 if (myRouter->compute(src, dest, myVehicle, 0, myRoute)) {
350 result =
MAX2(0.0, myRouter->recomputeCosts(myRoute, myVehicle, 0) + costOff);
358 std::vector<const E*> myRoute;
363 RoutingTask(
const E* src,
const E* dest,
const double costOff)
364 : mySrc(src), myDest(dest), myCost(-costOff) {}
366 myCost = ((WorkerThread*)context)->compute(mySrc, myDest, myCost);
372 const E*
const mySrc;
373 const E*
const myDest;
377 RoutingTask& operator=(
const RoutingTask&);
386 for (std::map<std::string, int>::const_iterator it = myLandmarks.begin(); it != myLandmarks.end(); ++it) {
387 if (it->second == i) {
void waitAll(const bool deleteFinished=true)
waits for all tasks to be finished
bool consistent() const
whether the heuristic ist consistent (found nodes are always visited on the shortest path the first t...
double lowerBound(const E *from, const E *to, double speed, double speedFactor, double fromEffort, double toEffort) const
provide a lower bound on the distance between from and to (excluding traveltime of both edges) ...
virtual bool compute(const E *from, const E *to, const V *const vehicle, SUMOTime msTime, std::vector< const E *> &into)=0
Builds the route between the given edges using the minimum effort at the given time The definition of...
std::string get(int pos) const
#define WRITE_WARNING(msg)
int size() const
Returns the number of threads in the pool.
std::vector< std::vector< double > > myToLandmarkDists
static double toDouble(const std::string &sData)
converts a string into the double value described by it by calling the char-type converter ...
FullLookupTable(const std::string &filename, const int size)
std::map< std::string, int > myLandmarks
double lowerBound(const E *from, const E *to, double, double speedFactor, double, double) const
provide a lower bound on the distance between from and to (excluding traveltime of both edges) ...
virtual bool consistent() const =0
whether the heuristic ist consistent (found nodes are always visited on the shortest path the first t...
void add(Task *const t, int index=-1)
Gives a number to the given task and assigns it to the worker with the given index. If the index is negative, assign to the next (round robin) one.
virtual double lowerBound(const E *from, const E *to, double speed, double speedFactor, double fromEffort, double toEffort) const =0
provide a lower bound on the distance between from and to (excluding traveltime of both edges) ...
A pool of worker threads which distributes the tasks and collects the results.
std::vector< std::vector< double > > myTable
std::string getLandmark(int i) const
Abstract superclass of a task to be run with an index to keep track of pending tasks.
double recomputeCosts(const std::vector< const E *> &edges, const V *const v, SUMOTime msTime) const
A thread repeatingly calculating incoming tasks.
Computes the shortest path through a network using the A* algorithm.
virtual SUMOAbstractRouter * clone()=0
LandmarkLookupTable(const std::string &filename, const std::vector< E *> &edges, SUMOAbstractRouter< E, V > *router, const V *defaultVehicle, const std::string &outfile, const int maxNumThreads)
std::vector< std::vector< double > > myFromLandmarkDists
bool consistent() const
whether the heuristic ist consistent (found nodes are always visited on the shortest path the first t...