49 #define UNREACHABLE (std::numeric_limits<double>::max() / 1000.0)
76 template<
class E,
class V>
92 return nod1->
edge->getNumericalID() > nod2->
edge->getNumericalID();
100 const bool havePermissions =
false,
const bool haveRestrictions =
false) :
101 SUMOAbstractRouter<E, V>(
"AStarRouter", unbuildIsWarning, operation, nullptr, havePermissions, haveRestrictions),
104 for (
const E*
const edge : edges) {
111 const bool havePermissions =
false,
const bool haveRestrictions =
false) :
112 SUMOAbstractRouter<E, V>(
"AStarRouter", unbuildIsWarning, operation, nullptr, havePermissions, haveRestrictions),
115 for (
const auto& edgeInfo : edgeInfos) {
134 for (
auto& edgeInfo :
myFound) {
142 bool compute(
const E* from,
const E* to,
const V*
const vehicle,
143 SUMOTime msTime, std::vector<const E*>& into,
bool silent =
false) {
144 assert(from !=
nullptr && to !=
nullptr);
160 #ifdef ASTAR_DEBUG_QUERY
161 std::cout <<
"DEBUG: starting search for '" <<
Named::getIDSecure(vehicle) <<
"' speed: " <<
MIN2(vehicle->getMaxSpeed(),
myMaxSpeed * vehicle->getChosenSpeedFactor()) <<
" time: " <<
STEPS2TIME(msTime) <<
"\n";
165 const auto& toInfo =
myEdgeInfos[to->getNumericalID()];
166 if (toInfo.visited) {
174 auto*
const fromInfo = &(
myEdgeInfos[from->getNumericalID()]);
175 fromInfo->effort = 0.;
176 fromInfo->heuristicEffort = 0.;
177 fromInfo->prev =
nullptr;
184 const double speed = vehicle ==
nullptr ?
myMaxSpeed :
MIN2(vehicle->getMaxSpeed(),
myMaxSpeed * vehicle->getChosenSpeedFactor());
189 const E*
const minEdge = minimumInfo->edge;
194 #ifdef ASTAR_DEBUG_QUERY_PERF
195 std::cout <<
"visited " +
toString(num_visited) +
" edges (final path length=" +
toString(into.size())
197 +
" edges=" +
toString(into) +
")\n";
199 #ifdef ASTAR_DEBUG_VISITED
203 dev <<
"edge:" << i.edge->getID() <<
"\n";
212 myFound.push_back(minimumInfo);
213 minimumInfo->visited =
true;
214 #ifdef ASTAR_DEBUG_QUERY
215 std::cout <<
"DEBUG: hit=" << minEdge->getID()
216 <<
" TT=" << minimumInfo->effort
217 <<
" EF=" << this->
getEffort(minEdge, vehicle, minimumInfo->leaveTime)
218 <<
" HT=" << minimumInfo->heuristicEffort
219 <<
" Q(TT,HT,Edge)=";
221 std::cout << (*it)->effort <<
"," << (*it)->heuristicEffort <<
"," << (*it)->edge->getID() <<
" ";
225 const double effortDelta = this->
getEffort(minEdge, vehicle, minimumInfo->leaveTime);
226 const double leaveTime = minimumInfo->leaveTime + this->
getTravelTime(minEdge, vehicle, minimumInfo->leaveTime, effortDelta);
229 const double heuristic_remaining = (
myLookupTable ==
nullptr ? minEdge->getDistanceTo(to) / speed :
230 myLookupTable->lowerBound(minEdge, to, speed, vehicle->getChosenSpeedFactor(),
231 minEdge->getMinimumTravelTime(
nullptr), to->getMinimumTravelTime(
nullptr)));
235 const double heuristicEffort = minimumInfo->effort + effortDelta + heuristic_remaining;
237 for (
const std::pair<const E*, const E*>& follower : minEdge->getViaSuccessors(vClass)) {
238 auto*
const followerInfo = &(
myEdgeInfos[follower.first->getNumericalID()]);
240 if (followerInfo->prohibited || this->isProhibited(follower.first, vehicle)) {
243 double effort = minimumInfo->effort + effortDelta;
244 double time = leaveTime;
246 const double oldEffort = followerInfo->effort;
247 if ((!followerInfo->visited || mayRevisit) && effort < oldEffort) {
248 followerInfo->effort = effort;
250 followerInfo->heuristicEffort =
MIN2(heuristicEffort, followerInfo->heuristicEffort);
251 followerInfo->leaveTime = time;
252 followerInfo->prev = minimumInfo;
253 #ifdef ASTAR_DEBUG_QUERY_FOLLOWERS
254 std::cout <<
" follower=" << followerInfo->edge->getID()
255 <<
" OEF=" << (oldEffort == std::numeric_limits<double>::max() ?
"inf" :
toString(oldEffort))
256 <<
" TT=" << effort <<
" HR=" << heuristic_remaining <<
" HT=" << followerInfo->heuristicEffort <<
"\n";
258 if (oldEffort == std::numeric_limits<double>::max()) {
275 #ifdef ASTAR_DEBUG_QUERY_PERF
276 std::cout <<
"visited " +
toString(num_visited) +
" edges (unsuccessful path length: " +
toString(into.size()) +
")\n";
287 myEdgeInfos[edge->getNumericalID()].prohibited =
false;
289 for (E*
const edge : toProhibit) {
290 myEdgeInfos[edge->getNumericalID()].prohibited =
true;
292 this->myProhibited = toProhibit;
298 std::vector<const E*> tmp;
299 while (rbegin != 0) {
300 tmp.push_back(rbegin->
edge);
301 rbegin = rbegin->
prev;
303 std::copy(tmp.rbegin(), tmp.rend(), std::back_inserter(edges));
308 std::vector<typename SUMOAbstractRouter<E, V>::EdgeInfo>
myEdgeInfos;
313 std::vector<typename SUMOAbstractRouter<E, V>::EdgeInfo*>
myFound;