16 #ifndef AStarLookupTable_h
17 #define AStarLookupTable_h
32 #define UNREACHABLE (std::numeric_limits<double>::max() / 1000.0)
57 template<
class E,
class V>
61 virtual double lowerBound(
const E* from,
const E* to,
double speed,
double speedFactor,
double fromEffort,
double toEffort)
const = 0;
68 template<
class E,
class V>
74 for (
int i = 0; i < size; i++) {
75 for (
int j = 0; j < size; j++) {
86 double lowerBound(
const E* from,
const E* to,
double ,
double speedFactor,
double ,
double )
const {
87 return myTable[from->getNumericalID()][to->getNumericalID()] / speedFactor;
95 std::vector<std::vector<double> >
myTable;
99 template<
class E,
class V>
104 std::map<std::string, int> numericID;
106 if (!e->isInternal()) {
113 std::ifstream strm(filename.c_str());
115 throw ProcessError(
"Could not load landmark-lookup-table from '" + filename +
"'.");
117 std::ofstream* ostrm =
nullptr;
118 if (!outfile.empty()) {
119 ostrm =
new std::ofstream(outfile.c_str());
120 if (!ostrm->good()) {
121 throw ProcessError(
"Could not open file '" + outfile +
"' for writing.");
125 int numLandMarks = 0;
126 while (std::getline(strm, line)) {
132 if (st.
size() == 1) {
133 const std::string lm = st.
get(0);
137 if (ostrm !=
nullptr) {
138 (*ostrm) << lm <<
"\n";
141 assert(st.
size() == 4);
142 const std::string lm = st.
get(0);
143 const std::string edge = st.
get(1);
145 WRITE_WARNING(
"Unknown or unordered edge '" + edge +
"' in landmark file.");
154 WRITE_WARNING(
"No landmarks in '" + filename +
"', falling back to standard A*.");
161 for (
int i = 0; i < (int)
myLandmarks.size(); ++i) {
164 const E* landmark =
nullptr;
166 for (
const E*
const edge : edges) {
167 if (edge->getID() == landmarkID) {
172 if (landmark ==
nullptr) {
173 WRITE_WARNING(
"Landmark '" + landmarkID +
"' does not exist in the network.");
176 if (router !=
nullptr) {
177 const std::string missing = outfile.empty() ? filename +
".missing" : outfile;
178 WRITE_WARNING(
"Not all network edges were found in the lookup table '" + filename +
"' for landmark '" + landmarkID +
"'. Saving missing values to '" + missing +
"'.");
179 if (ostrm ==
nullptr) {
180 ostrm =
new std::ofstream(missing.c_str());
181 if (!ostrm->good()) {
182 throw ProcessError(
"Could not open file '" + missing +
"' for writing.");
186 throw ProcessError(
"Not all network edges were found in the lookup table '" + filename +
"' for landmark '" + landmarkID +
"'.");
188 std::vector<const E*> routeLM(1, landmark);
189 const double lmCost = router->
recomputeCosts(routeLM, defaultVehicle, 0);
190 std::vector<const E*> route;
192 if (maxNumThreads > 0) {
193 if (threadPool.
size() == 0) {
196 router->
compute(landmark, landmark, defaultVehicle, 0, route);
198 while ((
int)threadPool.
size() < maxNumThreads) {
199 new WorkerThread(threadPool, router->
clone(), defaultVehicle);
202 std::vector<RoutingTask*> currentTasks;
204 const E* edge = edges[j];
205 if (landmark != edge) {
206 std::vector<const E*> routeE(1, edge);
207 const double sourceDestCost = lmCost + router->
recomputeCosts(routeE, defaultVehicle, 0);
209 if (edge->getPredecessors().size() > 0 && landmark->getSuccessors().size() > 0) {
210 currentTasks.push_back(
new RoutingTask(landmark, edge, sourceDestCost));
211 threadPool.
add(currentTasks.back());
214 if (landmark->getPredecessors().size() > 0 && edge->getSuccessors().size() > 0) {
215 currentTasks.push_back(
new RoutingTask(edge, landmark, sourceDestCost));
216 threadPool.
add(currentTasks.back());
223 const E* edge = edges[j];
224 double distFrom = -1;
226 if (landmark == edge) {
230 if (edge->getPredecessors().size() > 0 && landmark->getSuccessors().size() > 0) {
231 distFrom = currentTasks[taskIndex]->getCost();
232 delete currentTasks[taskIndex++];
234 if (landmark->getPredecessors().size() > 0 && edge->getSuccessors().size() > 0) {
235 distTo = currentTasks[taskIndex]->getCost();
236 delete currentTasks[taskIndex++];
241 (*ostrm) << landmarkID <<
" " << edge->getID() <<
" " << distFrom <<
" " << distTo <<
"\n";
243 currentTasks.clear();
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);
276 (*ostrm) << landmarkID <<
" " << edge->getID() <<
" " << distFrom <<
" " << distTo <<
"\n";
286 double lowerBound(
const E* from,
const E* to,
double speed,
double speedFactor,
double fromEffort,
double toEffort)
const {
287 double result = from->getDistanceTo(to) / speed;
288 #ifdef ASTAR_DEBUG_LOOKUPTABLE
289 if (from->getID() == ASTAR_DEBUG_LOOKUPTABLE_FROM) {
290 std::cout <<
" lowerBound to=" << to->getID() <<
" result1=" << result <<
"\n";
293 for (
int i = 0; i < (int)
myLandmarks.size(); ++i) {
297 if (fl >= 0 && tl >= 0) {
298 const double bound = (fl - tl - toEffort) / speedFactor;
299 #ifdef ASTAR_DEBUG_LOOKUPTABLE
300 if (from->getID() == ASTAR_DEBUG_LOOKUPTABLE_FROM && result < bound) {
301 std::cout <<
" landmarkTo=" <<
getLandmark(i) <<
" result2=" << bound
302 <<
" fl=" << fl <<
" tl=" << tl <<
"\n";
305 result =
MAX2(result, bound);
309 if (lt >= 0 && lf >= 0) {
310 const double bound = (lt - lf - fromEffort) / speedFactor;
311 #ifdef ASTAR_DEBUG_LOOKUPTABLE
312 if (from->getID() == ASTAR_DEBUG_LOOKUPTABLE_FROM && result < bound) {
313 std::cout <<
" landmarkFrom=" <<
getLandmark(i) <<
" result3=" << bound
314 <<
" lt=" << lt <<
" lf=" << lf <<
"\n";
317 result =
MAX2(result, bound);
319 if ((tl >= 0 && fl < 0)
320 || (lf >= 0 && lt < 0)) {
322 #ifdef ASTAR_DEBUG_UNREACHABLE
323 std::cout <<
" unreachable: from=" << from->getID() <<
" to=" << to->getID() <<
" landmark=" <<
getLandmark(i) <<
" "
324 << ((tl >= 0 && fl < 0) ?
" (toLandmark)" :
" (fromLandmark)")
325 <<
" fl=" << fl <<
" tl=" << tl <<
" lt=" << lt <<
" lf=" << lf
351 virtual ~WorkerThread() {
354 double compute(
const E* src,
const E* dest,
const double costOff) {
356 if (myRouter->compute(src, dest, myVehicle, 0, myRoute)) {
357 result =
MAX2(0.0, myRouter->recomputeCosts(myRoute, myVehicle, 0) + costOff);
365 std::vector<const E*> myRoute;
370 RoutingTask(
const E* src,
const E* dest,
const double costOff)
371 : mySrc(src), myDest(dest), myCost(-costOff) {}
373 myCost = ((WorkerThread*)context)->compute(mySrc, myDest, myCost);
379 const E*
const mySrc;
380 const E*
const myDest;
384 RoutingTask& operator=(
const RoutingTask&);
393 for (std::map<std::string, int>::const_iterator it =
myLandmarks.begin(); it !=
myLandmarks.end(); ++it) {
394 if (it->second == i) {