46 #define UNREACHABLE (std::numeric_limits<double>::max() / 1000.0)
75 template<
class E,
class V>
91 return nod1->
edge->getNumericalID() > nod2->
edge->getNumericalID();
99 const bool havePermissions =
false,
const bool haveRestrictions =
false) :
100 SUMOAbstractRouter<E, V>(
"AStarRouter", unbuildIsWarning, operation, nullptr, havePermissions, haveRestrictions),
103 for (
const E*
const edge : edges) {
110 const bool havePermissions =
false,
const bool haveRestrictions =
false) :
111 SUMOAbstractRouter<E, V>(
"AStarRouter", unbuildIsWarning, operation, nullptr, havePermissions, haveRestrictions),
114 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);
146 if (
myEdgeInfos[from->getNumericalID()].prohibited || this->isProhibited(from, vehicle)) {
152 if (
myEdgeInfos[to->getNumericalID()].prohibited || this->isProhibited(to, vehicle)) {
160 #ifdef ASTAR_DEBUG_QUERY
161 if (ASTAR_DEBUG_COND) {
162 std::cout <<
"DEBUG: starting search for '" <<
Named::getIDSecure(vehicle) <<
"' speed: " <<
MIN2(vehicle->getMaxSpeed(),
myMaxSpeed * vehicle->getChosenSpeedFactor()) <<
" time: " <<
STEPS2TIME(msTime) <<
"\n";
168 const auto& toInfo =
myEdgeInfos[to->getNumericalID()];
169 if (toInfo.visited) {
177 auto*
const fromInfo = &(
myEdgeInfos[from->getNumericalID()]);
178 fromInfo->effort = 0.;
179 fromInfo->heuristicEffort = 0.;
180 fromInfo->prev =
nullptr;
187 const double speed = vehicle ==
nullptr ?
myMaxSpeed :
MIN2(vehicle->getMaxSpeed(),
myMaxSpeed * vehicle->getChosenSpeedFactor());
192 const E*
const minEdge = minimumInfo->edge;
197 #ifdef ASTAR_DEBUG_QUERY_PERF
198 if (ASTAR_DEBUG_COND) {
199 std::cout <<
"visited " +
toString(num_visited) +
" edges (final path length=" +
toString(into.size())
201 +
" edges=" +
toString(into) +
")\n";
204 #ifdef ASTAR_DEBUG_VISITED
205 if (ASTAR_DEBUG_COND) {
209 dev <<
"edge:" << i.edge->getID() <<
"\n";
219 myFound.push_back(minimumInfo);
220 minimumInfo->visited =
true;
221 #ifdef ASTAR_DEBUG_QUERY
222 if (ASTAR_DEBUG_COND) {
223 std::cout <<
"DEBUG: hit=" << minEdge->getID()
224 <<
" TT=" << minimumInfo->effort
225 <<
" EF=" << this->
getEffort(minEdge, vehicle, minimumInfo->leaveTime)
226 <<
" HT=" << minimumInfo->heuristicEffort
227 <<
" Q(TT,HT,Edge)=";
229 std::cout << edgeInfo->effort <<
"," << edgeInfo->heuristicEffort <<
"," << edgeInfo->edge->getID() <<
" ";
234 const double effortDelta = this->
getEffort(minEdge, vehicle, minimumInfo->leaveTime);
235 const double leaveTime = minimumInfo->leaveTime + this->
getTravelTime(minEdge, vehicle, minimumInfo->leaveTime, effortDelta);
238 const double heuristic_remaining = (
myLookupTable ==
nullptr ? minEdge->getDistanceTo(to) / speed :
239 myLookupTable->lowerBound(minEdge, to, speed, vehicle->getChosenSpeedFactor(),
240 minEdge->getMinimumTravelTime(
nullptr), to->getMinimumTravelTime(
nullptr)));
244 const double heuristicEffort = minimumInfo->effort + effortDelta + heuristic_remaining;
246 for (
const std::pair<const E*, const E*>& follower : minEdge->getViaSuccessors(vClass)) {
247 auto*
const followerInfo = &(
myEdgeInfos[follower.first->getNumericalID()]);
249 if (followerInfo->prohibited || this->isProhibited(follower.first, vehicle)) {
252 double effort = minimumInfo->effort + effortDelta;
253 double time = leaveTime;
255 const double oldEffort = followerInfo->effort;
256 if ((!followerInfo->visited || mayRevisit) && effort < oldEffort) {
257 followerInfo->effort = effort;
259 followerInfo->heuristicEffort =
MIN2(heuristicEffort, followerInfo->heuristicEffort);
260 followerInfo->leaveTime = time;
261 followerInfo->prev = minimumInfo;
262 #ifdef ASTAR_DEBUG_QUERY_FOLLOWERS
263 if (ASTAR_DEBUG_COND) {
264 std::cout <<
" follower=" << followerInfo->edge->getID()
265 <<
" OEF=" << (oldEffort == std::numeric_limits<double>::max() ?
"inf" :
toString(oldEffort))
266 <<
" TT=" << effort <<
" HR=" << heuristic_remaining <<
" HT=" << followerInfo->heuristicEffort <<
"\n";
269 if (oldEffort == std::numeric_limits<double>::max()) {
286 #ifdef ASTAR_DEBUG_QUERY_PERF
287 if (ASTAR_DEBUG_COND) {
288 std::cout <<
"visited " +
toString(num_visited) +
" edges (unsuccessful path length: " +
toString(into.size()) +
")\n";
300 myEdgeInfos[edge->getNumericalID()].prohibited =
false;
302 for (E*
const edge : toProhibit) {
303 myEdgeInfos[edge->getNumericalID()].prohibited =
true;
305 this->myProhibited = toProhibit;
311 std::vector<const E*> tmp;
312 while (rbegin != 0) {
313 tmp.push_back(rbegin->
edge);
314 rbegin = rbegin->
prev;
316 std::copy(tmp.rbegin(), tmp.rend(), std::back_inserter(edges));
321 std::vector<typename SUMOAbstractRouter<E, V>::EdgeInfo>
myEdgeInfos;
326 std::vector<typename SUMOAbstractRouter<E, V>::EdgeInfo*>
myFound;
SUMOVehicleClass
Definition of vehicle classes to differ between different lane usage and authority types.
@ SVC_IGNORING
vehicles ignoring classes
std::string toString(const T &t, std::streamsize accuracy=gPrecision)
bool operator()(const typename SUMOAbstractRouter< E, V >::EdgeInfo *nod1, const typename SUMOAbstractRouter< E, V >::EdgeInfo *nod2) const
Comparing method.
Computes the shortest path through a network using the A* algorithm.
double myMaxSpeed
maximum speed in the network
std::vector< typename SUMOAbstractRouter< E, V >::EdgeInfo * > myFrontierList
A container for reusage of the min edge heap.
void buildPathFrom(const typename SUMOAbstractRouter< E, V >::EdgeInfo *rbegin, std::vector< const E * > &edges)
Builds the path from marked edges.
virtual SUMOAbstractRouter< E, V > * clone()
void prohibit(const std::vector< E * > &toProhibit)
EdgeInfoComparator myComparator
FullLookupTable< E, V > FLT
virtual ~AStarRouter()
Destructor.
std::vector< typename SUMOAbstractRouter< E, V >::EdgeInfo * > myFound
list of visited Edges (for resetting)
bool compute(const E *from, const E *to, const V *const vehicle, SUMOTime msTime, std::vector< const E * > &into, bool silent=false)
Builds the route between the given edges using the minimum travel time.
AStarRouter(const std::vector< E * > &edges, bool unbuildIsWarning, typename SUMOAbstractRouter< E, V >::Operation operation, const std::shared_ptr< const LookupTable > lookup=nullptr, const bool havePermissions=false, const bool haveRestrictions=false)
Constructor.
std::vector< typename SUMOAbstractRouter< E, V >::EdgeInfo > myEdgeInfos
The container of edge information.
AStarRouter(const std::vector< typename SUMOAbstractRouter< E, V >::EdgeInfo > &edgeInfos, bool unbuildIsWarning, typename SUMOAbstractRouter< E, V >::Operation operation, const std::shared_ptr< const LookupTable > lookup=nullptr, const bool havePermissions=false, const bool haveRestrictions=false)
LandmarkLookupTable< E, V > LMLT
AbstractLookupTable< E, V > LookupTable
const std::shared_ptr< const LookupTable > myLookupTable
the lookup table for travel time heuristics
Computes the shortest path through a network using the A* algorithm.
virtual void inform(std::string msg, bool addType=true)
adds a new error to the list
static MsgHandler * getWarningInstance()
Returns the instance to add warnings to.
void informf(const std::string &format, T value, Targs... Fargs)
adds a new formatted message
static std::string getIDSecure(const T *obj, const std::string &fallBack="NULL")
get an identifier for Named-like object which may be Null
Static storage of an output device and its base (abstract) implementation.
void close()
Closes the device and removes it from the dictionary.
static OutputDevice & getDevice(const std::string &name)
Returns the described OutputDevice.
const E *const edge
The current edge.
const EdgeInfo * prev
The previous edge.
double heuristicEffort
Estimated effort to reach the edge (effort + lower bound on remaining effort)
MsgHandler *const myErrorMsgHandler
the handler for routing errors
std::vector< E * > myProhibited
const bool myHavePermissions
whether edge permissions need to be considered
bool myBulkMode
whether we are currently operating several route queries in a bulk
double recomputeCosts(const std::vector< const E * > &edges, const V *const v, SUMOTime msTime, double *lengthp=nullptr) const
Operation myOperation
The object's operation to perform.
double getTravelTime(const E *const e, const V *const v, const double t, const double effort) const
double getEffort(const E *const e, const V *const v, double t) const
void updateViaEdgeCost(const E *viaEdge, const V *const v, double &time, double &effort, double &length) const
const bool myHaveRestrictions
whether edge restrictions need to be considered
void endQuery(int visits)