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);
87 double lowerBound(
const E* from,
const E* to,
double ,
double speedFactor,
double ,
double )
const {
88 return myTable[from->getNumericalID()][to->getNumericalID()] / speedFactor;
96 std::vector<std::vector<double> >
myTable;
100 template<
class E,
class V>
104 myFirstNonInternal = -1;
105 std::map<std::string, int> numericID;
107 if (!e->isInternal()) {
108 if (myFirstNonInternal == -1) {
109 myFirstNonInternal = e->getNumericalID();
111 numericID[e->getID()] = e->getNumericalID() - myFirstNonInternal;
114 std::ifstream strm(filename.c_str());
116 throw ProcessError(
"Could not load landmark-lookup-table from '" + filename +
"'.");
118 std::ofstream* ostrm =
nullptr;
119 if (!outfile.empty()) {
120 ostrm =
new std::ofstream(outfile.c_str());
121 if (!ostrm->good()) {
122 throw ProcessError(
"Could not open file '" + outfile +
"' for writing.");
126 int numLandMarks = 0;
127 while (std::getline(strm, line)) {
133 if (st.
size() == 1) {
134 const std::string lm = st.
get(0);
135 myLandmarks[lm] = numLandMarks++;
136 myFromLandmarkDists.push_back(std::vector<double>(0));
137 myToLandmarkDists.push_back(std::vector<double>(0));
138 if (ostrm !=
nullptr) {
139 (*ostrm) << lm <<
"\n";
142 assert(st.
size() == 4);
143 const std::string lm = st.
get(0);
144 const std::string edge = st.
get(1);
145 if (numericID[edge] != (
int)myFromLandmarkDists[myLandmarks[lm]].size()) {
146 WRITE_WARNING(
"Unknown or unordered edge '" + edge +
"' in landmark file.");
150 myFromLandmarkDists[myLandmarks[lm]].push_back(distFrom);
151 myToLandmarkDists[myLandmarks[lm]].push_back(distTo);
154 if (myLandmarks.empty()) {
155 WRITE_WARNING(
"No landmarks in '" + filename +
"', falling back to standard A*.");
162 for (
int i = 0; i < (int)myLandmarks.size(); ++i) {
163 if ((
int)myFromLandmarkDists[i].size() != (int)edges.size() - myFirstNonInternal) {
164 const std::string landmarkID = getLandmark(i);
165 const E* landmark =
nullptr;
167 for (
const E*
const edge : edges) {
168 if (edge->getID() == landmarkID) {
173 if (landmark ==
nullptr) {
174 WRITE_WARNING(
"Landmark '" + landmarkID +
"' does not exist in the network.");
177 if (router !=
nullptr) {
178 const std::string missing = outfile.empty() ? filename +
".missing" : outfile;
179 WRITE_WARNING(
"Not all network edges were found in the lookup table '" + filename +
"' for landmark '" + landmarkID +
"'. Saving missing values to '" + missing +
"'.");
180 if (ostrm ==
nullptr) {
181 ostrm =
new std::ofstream(missing.c_str());
182 if (!ostrm->good()) {
183 throw ProcessError(
"Could not open file '" + missing +
"' for writing.");
187 throw ProcessError(
"Not all network edges were found in the lookup table '" + filename +
"' for landmark '" + landmarkID +
"'.");
189 std::vector<const E*> routeLM(1, landmark);
190 const double lmCost = router->
recomputeCosts(routeLM, defaultVehicle, 0);
191 std::vector<const E*> route;
193 if (maxNumThreads > 0) {
194 if (threadPool.
size() == 0) {
197 router->
compute(landmark, landmark, defaultVehicle, 0, route);
199 while ((
int)threadPool.
size() < maxNumThreads) {
200 new WorkerThread(threadPool, router->
clone(), defaultVehicle);
203 std::vector<RoutingTask*> currentTasks;
204 for (
int j = (
int)myFromLandmarkDists[i].size() + myFirstNonInternal; j < (int)edges.size(); ++j) {
205 const E* edge = edges[j];
206 if (landmark != edge) {
207 std::vector<const E*> routeE(1, edge);
208 const double sourceDestCost = lmCost + router->
recomputeCosts(routeE, defaultVehicle, 0);
210 if (edge->getPredecessors().size() > 0 && landmark->getSuccessors().size() > 0) {
211 currentTasks.push_back(
new RoutingTask(landmark, edge, sourceDestCost));
212 threadPool.
add(currentTasks.back());
215 if (landmark->getPredecessors().size() > 0 && edge->getSuccessors().size() > 0) {
216 currentTasks.push_back(
new RoutingTask(edge, landmark, sourceDestCost));
217 threadPool.
add(currentTasks.back());
223 for (
int j = (
int)myFromLandmarkDists[i].size() + myFirstNonInternal; j < (int)edges.size(); ++j) {
224 const E* edge = edges[j];
225 double distFrom = -1;
227 if (landmark == edge) {
231 if (edge->getPredecessors().size() > 0 && landmark->getSuccessors().size() > 0) {
232 distFrom = currentTasks[taskIndex]->getCost();
233 delete currentTasks[taskIndex++];
235 if (landmark->getPredecessors().size() > 0 && edge->getSuccessors().size() > 0) {
236 distTo = currentTasks[taskIndex]->getCost();
237 delete currentTasks[taskIndex++];
240 myFromLandmarkDists[i].push_back(distFrom);
241 myToLandmarkDists[i].push_back(distTo);
242 (*ostrm) << landmarkID <<
" " << edge->getID() <<
" " << distFrom <<
" " << distTo <<
"\n";
244 currentTasks.clear();
250 for (
int j = (
int)myFromLandmarkDists[i].size() + myFirstNonInternal; j < (int)edges.size(); ++j) {
251 const E* edge = edges[j];
252 double distFrom = -1.;
254 if (landmark == edge) {
258 std::vector<const E*> routeE(1, edge);
259 const double sourceDestCost = lmCost + router->
recomputeCosts(routeE, defaultVehicle, 0);
261 if (edge->getPredecessors().size() > 0 && landmark->getSuccessors().size() > 0) {
262 if (router->
compute(landmark, edge, defaultVehicle, 0, route)) {
263 distFrom =
MAX2(0.0, router->
recomputeCosts(route, defaultVehicle, 0) - sourceDestCost);
268 if (landmark->getPredecessors().size() > 0 && edge->getSuccessors().size() > 0) {
269 if (router->
compute(edge, landmark, defaultVehicle, 0, route)) {
270 distTo =
MAX2(0.0, router->
recomputeCosts(route, defaultVehicle, 0) - sourceDestCost);
275 myFromLandmarkDists[i].push_back(distFrom);
276 myToLandmarkDists[i].push_back(distTo);
277 (*ostrm) << landmarkID <<
" " << edge->getID() <<
" " << distFrom <<
" " << distTo <<
"\n";
287 double lowerBound(
const E* from,
const E* to,
double speed,
double speedFactor,
double fromEffort,
double toEffort)
const {
288 double result = from->getDistanceTo(to) / speed;
289 #ifdef ASTAR_DEBUG_LOOKUPTABLE 290 if (from->getID() == ASTAR_DEBUG_LOOKUPTABLE_FROM) {
291 std::cout <<
" lowerBound to=" << to->getID() <<
" result1=" << result <<
"\n";
294 for (
int i = 0; i < (int)myLandmarks.size(); ++i) {
296 const double fl = myToLandmarkDists[i][from->getNumericalID() - myFirstNonInternal];
297 const double tl = myToLandmarkDists[i][to->getNumericalID() - myFirstNonInternal];
298 if (fl >= 0 && tl >= 0) {
299 const double bound = (fl - tl - toEffort) / speedFactor;
300 #ifdef ASTAR_DEBUG_LOOKUPTABLE 301 if (from->getID() == ASTAR_DEBUG_LOOKUPTABLE_FROM && result < bound) {
302 std::cout <<
" landmarkTo=" << getLandmark(i) <<
" result2=" << bound
303 <<
" fl=" << fl <<
" tl=" << tl <<
"\n";
306 result =
MAX2(result, bound);
308 const double lt = myFromLandmarkDists[i][to->getNumericalID() - myFirstNonInternal];
309 const double lf = myFromLandmarkDists[i][from->getNumericalID() - myFirstNonInternal];
310 if (lt >= 0 && lf >= 0) {
311 const double bound = (lt - lf - fromEffort) / speedFactor;
312 #ifdef ASTAR_DEBUG_LOOKUPTABLE 313 if (from->getID() == ASTAR_DEBUG_LOOKUPTABLE_FROM && result < bound) {
314 std::cout <<
" landmarkFrom=" << getLandmark(i) <<
" result3=" << bound
315 <<
" lt=" << lt <<
" lf=" << lf <<
"\n";
318 result =
MAX2(result, bound);
320 if ((tl >= 0 && fl < 0)
321 || (lf >= 0 && lt < 0)) {
323 #ifdef ASTAR_DEBUG_UNREACHABLE 324 std::cout <<
" unreachable: from=" << from->getID() <<
" to=" << to->getID() <<
" landmark=" << getLandmark(i) <<
" " 325 << ((tl >= 0 && fl < 0) ?
" (toLandmark)" :
" (fromLandmark)")
326 <<
" fl=" << fl <<
" tl=" << tl <<
" lt=" << lt <<
" lf=" << lf
352 virtual ~WorkerThread() {
355 double compute(
const E* src,
const E* dest,
const double costOff) {
357 if (myRouter->compute(src, dest, myVehicle, 0, myRoute)) {
358 result =
MAX2(0.0, myRouter->recomputeCosts(myRoute, myVehicle, 0) + costOff);
366 std::vector<const E*> myRoute;
371 RoutingTask(
const E* src,
const E* dest,
const double costOff)
372 : mySrc(src), myDest(dest), myCost(-costOff) {}
374 myCost = ((WorkerThread*)context)->compute(mySrc, myDest, myCost);
380 const E*
const mySrc;
381 const E*
const myDest;
385 RoutingTask& operator=(
const RoutingTask&);
394 for (std::map<std::string, int>::const_iterator it = myLandmarks.begin(); it != myLandmarks.end(); ++it) {
395 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) ...
std::string get(int pos) const
returns the item at the given position
#define UNUSED_PARAMETER(x)
#define WRITE_WARNING(msg)
virtual ~LandmarkLookupTable()
int size() const
returns the number of existing substrings
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 ...
double recomputeCosts(const std::vector< const E *> &edges, const V *const v, SUMOTime msTime, double *lengthp=nullptr) const
FullLookupTable(const std::string &filename, const int size)
virtual bool compute(const E *from, const E *to, const V *const vehicle, SUMOTime msTime, std::vector< const E *> &into, bool silent=false)=0
Builds the route between the given edges using the minimum effort at the given time The definition of...
std::map< std::string, int > myLandmarks
virtual ~FullLookupTable()
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.
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...