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++) {
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>
105 std::map<std::string, int> numericID;
107 if (!e->isInternal()) {
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);
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);
146 WRITE_WARNING(
"Unknown or unordered edge '" + edge +
"' in landmark file.");
155 WRITE_WARNING(
"No landmarks in '" + filename +
"', falling back to standard A*.");
162 for (
int i = 0; i < (int)
myLandmarks.size(); ++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;
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());
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++];
242 (*ostrm) << landmarkID <<
" " << edge->getID() <<
" " << distFrom <<
" " << distTo <<
"\n";
244 currentTasks.clear();
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);
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) {
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);
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) {