19 #ifndef AStarLookupTable_h 20 #define AStarLookupTable_h 39 #define UNREACHABLE (std::numeric_limits<double>::max() / 1000.0) 64 template<
class E,
class V>
68 virtual double lowerBound(
const E* from,
const E* to,
double speed,
double speedFactor,
double fromEffort,
double toEffort)
const = 0;
75 template<
class E,
class V>
81 for (
int i = 0; i < size; i++) {
82 for (
int j = 0; j < size; j++) {
85 myTable[i].push_back(val);
90 double lowerBound(
const E* from,
const E* to,
double ,
double speedFactor,
double ,
double )
const {
91 return myTable[from->getNumericalID()][to->getNumericalID()] / speedFactor;
99 std::vector<std::vector<double> >
myTable;
103 template<
class E,
class V>
107 myFirstNonInternal = -1;
108 std::map<std::string, int> numericID;
110 if (!e->isInternal()) {
111 if (myFirstNonInternal == -1) {
112 myFirstNonInternal = e->getNumericalID();
114 numericID[e->getID()] = e->getNumericalID() - myFirstNonInternal;
117 std::ifstream strm(filename.c_str());
119 throw ProcessError(
"Could not load landmark-lookup-table from '" + filename +
"'.");
121 std::ofstream* ostrm = 0;
122 if (!outfile.empty()) {
123 ostrm =
new std::ofstream(outfile.c_str());
124 if (!ostrm->good()) {
125 throw ProcessError(
"Could not open file '" + outfile +
"' for writing.");
129 int numLandMarks = 0;
130 while (std::getline(strm, line)) {
136 if (st.
size() == 1) {
137 const std::string lm = st.
get(0);
138 myLandmarks[lm] = numLandMarks++;
139 myFromLandmarkDists.push_back(std::vector<double>(0));
140 myToLandmarkDists.push_back(std::vector<double>(0));
142 (*ostrm) << lm <<
"\n";
145 assert(st.
size() == 4);
146 const std::string lm = st.
get(0);
147 const std::string edge = st.
get(1);
148 if (numericID[edge] != (
int)myFromLandmarkDists[myLandmarks[lm]].size()) {
149 WRITE_WARNING(
"Unknown or unordered edge '" + edge +
"' in landmark file.");
153 myFromLandmarkDists[myLandmarks[lm]].push_back(distFrom);
154 myToLandmarkDists[myLandmarks[lm]].push_back(distTo);
157 if (myLandmarks.empty()) {
158 WRITE_WARNING(
"No landmarks in '" + filename +
"', falling back to standard A*.");
164 for (
int i = 0; i < (int)myLandmarks.size(); ++i) {
165 if ((
int)myFromLandmarkDists[i].size() != (int)edges.size() - myFirstNonInternal) {
166 const std::string landmarkID = getLandmark(i);
167 const E* landmark = 0;
169 for (
typename std::vector<E*>::const_iterator it_e = edges.begin(); it_e != edges.end(); ++it_e) {
170 if ((*it_e)->getID() == landmarkID) {
175 WRITE_WARNING(
"Landmark '" + landmarkID +
"' does not exist in the network.");
179 const std::string missing = filename +
".missing";
180 WRITE_WARNING(
"Not all network edges were found in the lookup table '" + filename +
"' for landmark '" + landmarkID +
"'. Saving missing values to '" + missing +
"'.");
182 ostrm =
new std::ofstream(missing.c_str());
183 if (!ostrm->good()) {
184 throw ProcessError(
"Could not open file '" + missing +
"' for writing.");
188 throw ProcessError(
"Not all network edges were found in the lookup table '" + filename +
"' for landmark '" + landmarkID +
"'.");
190 std::vector<const E*> routeLM(1, landmark);
191 const double lmCost = router->
recomputeCosts(routeLM, defaultVehicle, 0);
192 std::vector<const E*> route;
194 if (maxNumThreads > 0) {
195 if (threadPool.
size() == 0) {
198 router->
compute(landmark, landmark, defaultVehicle, 0, route);
200 while ((
int)threadPool.
size() < maxNumThreads) {
201 new WorkerThread(threadPool, router->
clone(), defaultVehicle);
204 std::vector<RoutingTask*> currentTasks;
205 for (
int j = (
int)myFromLandmarkDists[i].size() + myFirstNonInternal; j < (int)edges.size(); ++j) {
206 const E* edge = edges[j];
207 if (landmark != edge) {
208 std::vector<const E*> routeE(1, edge);
209 const double sourceDestCost = lmCost + router->
recomputeCosts(routeE, defaultVehicle, 0);
211 if (edge->getPredecessors().size() > 0 && landmark->getSuccessors().size() > 0) {
212 currentTasks.push_back(
new RoutingTask(landmark, edge, sourceDestCost));
213 threadPool.
add(currentTasks.back());
216 if (landmark->getPredecessors().size() > 0 && edge->getSuccessors().size() > 0) {
217 currentTasks.push_back(
new RoutingTask(edge, landmark, sourceDestCost));
218 threadPool.
add(currentTasks.back());
224 for (
int j = (
int)myFromLandmarkDists[i].size() + myFirstNonInternal; j < (int)edges.size(); ++j) {
225 const E* edge = edges[j];
226 double distFrom = -1;
228 if (landmark == edge) {
232 if (edge->getPredecessors().size() > 0 && landmark->getSuccessors().size() > 0) {
233 distFrom = currentTasks[taskIndex]->getCost();
234 delete currentTasks[taskIndex++];
236 if (landmark->getPredecessors().size() > 0 && edge->getSuccessors().size() > 0) {
237 distTo = currentTasks[taskIndex]->getCost();
238 delete currentTasks[taskIndex++];
241 myFromLandmarkDists[i].push_back(distFrom);
242 myToLandmarkDists[i].push_back(distTo);
243 (*ostrm) << landmarkID <<
" " << edge->getID() <<
" " << distFrom <<
" " << distTo <<
"\n";
245 currentTasks.clear();
249 for (
int j = (
int)myFromLandmarkDists[i].size() + myFirstNonInternal; j < (int)edges.size(); ++j) {
250 const E* edge = edges[j];
251 double distFrom = -1;
253 if (landmark == edge) {
257 std::vector<const E*> routeE(1, edge);
258 const double sourceDestCost = lmCost + router->
recomputeCosts(routeE, defaultVehicle, 0);
260 if (edge->getPredecessors().size() > 0 && landmark->getSuccessors().size() > 0) {
261 if (router->
compute(landmark, edge, defaultVehicle, 0, route)) {
262 distFrom =
MAX2(0.0, router->
recomputeCosts(route, defaultVehicle, 0) - sourceDestCost);
267 if (landmark->getPredecessors().size() > 0 && edge->getSuccessors().size() > 0) {
268 if (router->
compute(edge, landmark, defaultVehicle, 0, route)) {
269 distTo =
MAX2(0.0, router->
recomputeCosts(route, defaultVehicle, 0) - sourceDestCost);
274 myFromLandmarkDists[i].push_back(distFrom);
275 myToLandmarkDists[i].push_back(distTo);
276 (*ostrm) << landmarkID <<
" " << edge->getID() <<
" " << distFrom <<
" " << distTo <<
"\n";
283 double lowerBound(
const E* from,
const E* to,
double speed,
double speedFactor,
double fromEffort,
double toEffort)
const {
284 double result = from->getDistanceTo(to) / speed;
285 #ifdef ASTAR_DEBUG_LOOKUPTABLE 286 if (from->getID() == ASTAR_DEBUG_LOOKUPTABLE_FROM) {
287 std::cout <<
" lowerBound to=" << to->getID() <<
" result1=" << result <<
"\n";
290 for (
int i = 0; i < (int)myLandmarks.size(); ++i) {
292 const double fl = myToLandmarkDists[i][from->getNumericalID() - myFirstNonInternal];
293 const double tl = myToLandmarkDists[i][to->getNumericalID() - myFirstNonInternal];
294 if (fl >= 0 && tl >= 0) {
295 const double bound = (fl - tl - toEffort) / speedFactor;
296 #ifdef ASTAR_DEBUG_LOOKUPTABLE 297 if (from->getID() == ASTAR_DEBUG_LOOKUPTABLE_FROM && result < bound) {
298 std::cout <<
" landmarkTo=" << getLandmark(i) <<
" result2=" << bound
299 <<
" fl=" << fl <<
" tl=" << tl <<
"\n";
302 result =
MAX2(result, bound);
304 const double lt = myFromLandmarkDists[i][to->getNumericalID() - myFirstNonInternal];
305 const double lf = myFromLandmarkDists[i][from->getNumericalID() - myFirstNonInternal];
306 if (lt >= 0 && lf >= 0) {
307 const double bound = (lt - lf - fromEffort) / speedFactor;
308 #ifdef ASTAR_DEBUG_LOOKUPTABLE 309 if (from->getID() == ASTAR_DEBUG_LOOKUPTABLE_FROM && result < bound) {
310 std::cout <<
" landmarkFrom=" << getLandmark(i) <<
" result3=" << bound
311 <<
" lt=" << lt <<
" lf=" << lf <<
"\n";
314 result =
MAX2(result, bound);
316 if ((tl >= 0 && fl < 0)
317 || (lf >= 0 && lt < 0)) {
319 #ifdef ASTAR_DEBUG_UNREACHABLE 320 std::cout <<
" unreachable: from=" << from->getID() <<
" to=" << to->getID() <<
" landmark=" << getLandmark(i) <<
" " 321 << ((tl >= 0 && fl < 0) ?
" (toLandmark)" :
" (fromLandmark)")
322 <<
" fl=" << fl <<
" tl=" << tl <<
" lt=" << lt <<
" lf=" << lf
348 virtual ~WorkerThread() {
351 double compute(
const E* src,
const E* dest,
const double costOff) {
353 if (myRouter->compute(src, dest, myVehicle, 0, myRoute)) {
354 result =
MAX2(0.0, myRouter->recomputeCosts(myRoute, myVehicle, 0) + costOff);
362 std::vector<const E*> myRoute;
367 RoutingTask(
const E* src,
const E* dest,
const double costOff)
368 : mySrc(src), myDest(dest), myCost(-costOff) {}
370 myCost = ((WorkerThread*)context)->compute(mySrc, myDest, myCost);
376 const E*
const mySrc;
377 const E*
const myDest;
381 RoutingTask& operator=(
const RoutingTask&);
390 for (std::map<std::string, int>::const_iterator it = myLandmarks.begin(); it != myLandmarks.end(); ++it) {
391 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 double recomputeCosts(const std::vector< const E *> &edges, const V *const v, SUMOTime msTime) const =0
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
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
static double _2double(const E *const data)
converts a char-type array into the double value described by it
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...