50 #define UNREACHABLE (std::numeric_limits<double>::max() / 1000.0)
77 template<
class E,
class V,
class BASE>
91 bool operator()(
const typename BASE::EdgeInfo* nod1,
const typename BASE::EdgeInfo* nod2)
const {
92 if (nod1->heuristicEffort == nod2->heuristicEffort) {
93 return nod1->edge->getNumericalID() > nod2->edge->getNumericalID();
95 return nod1->heuristicEffort > nod2->heuristicEffort;
100 AStarRouter(
const std::vector<E*>& edges,
bool unbuildIsWarning,
typename BASE::Operation operation,
const std::shared_ptr<const LookupTable> lookup = 0) :
101 BASE(
"AStarRouter", unbuildIsWarning, operation),
104 for (
const E*
const edge : edges) {
105 myEdgeInfos.push_back(
typename BASE::EdgeInfo(edge));
110 AStarRouter(
const std::vector<typename BASE::EdgeInfo>& edgeInfos,
bool unbuildIsWarning,
typename BASE::Operation operation,
const std::shared_ptr<const LookupTable> lookup = 0) :
111 BASE(
"AStarRouter", unbuildIsWarning, operation),
114 for (
const auto& edgeInfo : edgeInfos) {
115 myEdgeInfos.push_back(
typename BASE::EdgeInfo(edgeInfo.edge));
133 for (
auto& edgeInfo :
myFound) {
141 virtual bool compute(
const E* from,
const E* to,
const V*
const vehicle,
142 SUMOTime msTime, std::vector<const E*>& into,
bool silent =
false) {
143 assert(from != 0 && to != 0);
145 if (this->isProhibited(from, vehicle)) {
147 this->myErrorMsgHandler->inform(
"Vehicle '" +
Named::getIDSecure(vehicle) +
"' is not allowed on source edge '" + from->getID() +
"'.");
151 if (this->isProhibited(to, vehicle)) {
153 this->myErrorMsgHandler->inform(
"Vehicle '" +
Named::getIDSecure(vehicle) +
"' is not allowed on destination edge '" + to->getID() +
"'.");
159 #ifdef ASTAR_DEBUG_QUERY
160 std::cout <<
"DEBUG: starting search for '" <<
Named::getIDSecure(vehicle) <<
"' speed: " <<
MIN2(vehicle->getMaxSpeed(),
myMaxSpeed * vehicle->getChosenSpeedFactor()) <<
" time: " <<
STEPS2TIME(msTime) <<
"\n";
163 if (this->myBulkMode) {
164 const auto& toInfo =
myEdgeInfos[to->getNumericalID()];
165 if (toInfo.visited) {
173 auto*
const fromInfo = &(
myEdgeInfos[from->getNumericalID()]);
174 fromInfo->effort = 0.;
175 fromInfo->heuristicEffort = 0.;
176 fromInfo->prev =
nullptr;
183 const double speed = vehicle ==
nullptr ?
myMaxSpeed :
MIN2(vehicle->getMaxSpeed(),
myMaxSpeed * vehicle->getChosenSpeedFactor());
188 const E*
const minEdge = minimumInfo->edge;
192 this->endQuery(num_visited);
193 #ifdef ASTAR_DEBUG_QUERY_PERF
194 std::cout <<
"visited " +
toString(num_visited) +
" edges (final path length=" +
toString(into.size())
195 +
" time=" +
toString(recomputeCosts(into, vehicle, msTime))
196 +
" edges=" +
toString(into) +
")\n";
198 #ifdef ASTAR_DEBUG_VISITED
202 dev <<
"edge:" << i.edge->getID() <<
"\n";
211 myFound.push_back(minimumInfo);
212 minimumInfo->visited =
true;
213 #ifdef ASTAR_DEBUG_QUERY
214 std::cout <<
"DEBUG: hit=" << minEdge->getID()
215 <<
" TT=" << minimumInfo->effort
216 <<
" EF=" << this->getEffort(minEdge, vehicle, minimumInfo->leaveTime)
217 <<
" HT=" << minimumInfo->heuristicEffort
218 <<
" Q(TT,HT,Edge)=";
220 std::cout << (*it)->effort <<
"," << (*it)->heuristicEffort <<
"," << (*it)->edge->getID() <<
" ";
224 const double effortDelta = this->getEffort(minEdge, vehicle, minimumInfo->leaveTime);
225 const double leaveTime = minimumInfo->leaveTime + this->
getTravelTime(minEdge, vehicle, minimumInfo->leaveTime, effortDelta);
228 const double heuristic_remaining = (
myLookupTable ==
nullptr ? minEdge->getDistanceTo(to) / speed :
229 myLookupTable->lowerBound(minEdge, to, speed, vehicle->getChosenSpeedFactor(),
230 minEdge->getMinimumTravelTime(
nullptr), to->getMinimumTravelTime(
nullptr)));
234 const double heuristicEffort = minimumInfo->effort + effortDelta + heuristic_remaining;
236 for (
const std::pair<const E*, const E*>& follower : minEdge->getViaSuccessors(vClass)) {
237 auto*
const followerInfo = &(
myEdgeInfos[follower.first->getNumericalID()]);
239 if (this->isProhibited(follower.first, vehicle)) {
242 double effort = minimumInfo->effort + effortDelta;
243 double time = leaveTime;
244 this->updateViaEdgeCost(follower.second, vehicle, time, effort, length);
245 const double oldEffort = followerInfo->effort;
246 if ((!followerInfo->visited || mayRevisit) && effort < oldEffort) {
247 followerInfo->effort = effort;
249 followerInfo->heuristicEffort =
MIN2(heuristicEffort, followerInfo->heuristicEffort);
250 followerInfo->leaveTime = time;
251 followerInfo->prev = minimumInfo;
252 #ifdef ASTAR_DEBUG_QUERY_FOLLOWERS
253 std::cout <<
" follower=" << followerInfo->edge->getID()
254 <<
" OEF=" << (oldEffort == std::numeric_limits<double>::max() ?
"inf" :
toString(oldEffort))
255 <<
" TT=" << effort <<
" HR=" << heuristic_remaining <<
" HT=" << followerInfo->heuristicEffort <<
"\n";
257 if (oldEffort == std::numeric_limits<double>::max()) {
273 this->endQuery(num_visited);
274 #ifdef ASTAR_DEBUG_QUERY_PERF
275 std::cout <<
"visited " +
toString(num_visited) +
" edges (unsuccessful path length: " +
toString(into.size()) +
")\n";
278 this->myErrorMsgHandler->inform(
"No connection between edge '" + from->getID() +
"' and edge '" + to->getID() +
"' found.");
285 void buildPathFrom(
const typename BASE::EdgeInfo* rbegin, std::vector<const E*>& edges) {
286 std::vector<const E*> tmp;
287 while (rbegin != 0) {
288 tmp.push_back(rbegin->edge);
289 rbegin = rbegin->prev;
291 std::copy(tmp.rbegin(), tmp.rend(), std::back_inserter(edges));
301 std::vector<typename BASE::EdgeInfo*>
myFound;