SUMO - Simulation of Urban MObility
|
00001 /****************************************************************************/ 00007 // A* Algorithm using euclidean distance heuristic. 00008 // Based on DijkstraRouterTT. For routing by effort a novel heuristic would be needed. 00009 /****************************************************************************/ 00010 // SUMO, Simulation of Urban MObility; see http://sumo.sourceforge.net/ 00011 // Copyright (C) 2001-2012 DLR (http://www.dlr.de/) and contributors 00012 /****************************************************************************/ 00013 // 00014 // This file is part of SUMO. 00015 // SUMO is free software: you can redistribute it and/or modify 00016 // it under the terms of the GNU General Public License as published by 00017 // the Free Software Foundation, either version 3 of the License, or 00018 // (at your option) any later version. 00019 // 00020 /****************************************************************************/ 00021 #ifndef AStarRouterTT_h 00022 #define AStarRouterTT_h 00023 00024 00025 // =========================================================================== 00026 // included modules 00027 // =========================================================================== 00028 #ifdef _MSC_VER 00029 #include <windows_config.h> 00030 #else 00031 #include <config.h> 00032 #endif 00033 00034 #include <string> 00035 #include <functional> 00036 #include <vector> 00037 #include <set> 00038 #include <limits> 00039 #include <algorithm> 00040 #include <iterator> 00041 #include <utils/common/MsgHandler.h> 00042 #include <utils/common/StdDefs.h> 00043 #include <utils/common/ToString.h> 00044 #include "SUMOAbstractRouter.h" 00045 00046 00047 // =========================================================================== 00048 // class definitions 00049 // =========================================================================== 00065 template<class E, class V, class PF> 00066 class AStarRouterTTBase : public SUMOAbstractRouter<E, V>, public PF { 00067 using SUMOAbstractRouter<E, V>::startQuery; 00068 using SUMOAbstractRouter<E, V>::endQuery; 00069 00070 public: 00072 AStarRouterTTBase(size_t noE, bool unbuildIsWarning): 00073 SUMOAbstractRouter<E, V>("AStarRouter"), 00074 myErrorMsgHandler(unbuildIsWarning ? MsgHandler::getWarningInstance() : MsgHandler::getErrorInstance()) 00075 { 00076 for (size_t i = 0; i < noE; i++) { 00077 myEdgeInfos.push_back(EdgeInfo(i)); 00078 } 00079 } 00080 00082 virtual ~AStarRouterTTBase() {} 00083 00089 class EdgeInfo { 00090 public: 00092 EdgeInfo(size_t id) : 00093 edge(E::dictionary(id)), 00094 traveltime(std::numeric_limits<SUMOReal>::max()), 00095 heuristicTime(std::numeric_limits<SUMOReal>::max()), 00096 prev(0), 00097 visited(false) 00098 {} 00099 00101 const E* edge; 00102 00104 SUMOReal traveltime; 00105 00107 SUMOReal heuristicTime; 00108 00110 EdgeInfo* prev; 00111 00113 bool visited; 00114 00115 inline void reset() { 00116 // heuristicTime is set before adding to the frontier, thus no reset is needed 00117 traveltime = std::numeric_limits<SUMOReal>::max(); 00118 visited = false; 00119 } 00120 00121 }; 00122 00127 class EdgeInfoComparator { 00128 public: 00130 bool operator()(const EdgeInfo* nod1, const EdgeInfo* nod2) const { 00131 if (nod1->heuristicTime == nod2->heuristicTime) { 00132 return nod1->edge->getNumericalID() > nod2->edge->getNumericalID(); 00133 } 00134 return nod1->heuristicTime > nod2->heuristicTime; 00135 } 00136 }; 00137 00138 virtual SUMOReal getEffort(const E* const e, const V* const v, SUMOReal t) const = 0; 00139 00140 00141 void init() { 00142 // all EdgeInfos touched in the previous query are either in myFrontierList or myFound: clean those up 00143 for (typename std::vector<EdgeInfo*>::iterator i = myFrontierList.begin(); i != myFrontierList.end(); i++) { 00144 (*i)->reset(); 00145 } 00146 myFrontierList.clear(); 00147 for (typename std::vector<EdgeInfo*>::iterator i = myFound.begin(); i != myFound.end(); i++) { 00148 (*i)->reset(); 00149 } 00150 myFound.clear(); 00151 } 00152 00153 00155 virtual void compute(const E* from, const E* to, const V* const vehicle, 00156 SUMOTime msTime, std::vector<const E*> &into) { 00157 assert(from != 0 && to != 0); 00158 startQuery(); 00159 const SUMOReal time = STEPS2TIME(msTime); 00160 init(); 00161 // add begin node 00162 EdgeInfo* const fromInfo = &(myEdgeInfos[from->getNumericalID()]); 00163 fromInfo->traveltime = 0; 00164 fromInfo->prev = 0; 00165 myFrontierList.push_back(fromInfo); 00166 // loop 00167 int num_visited = 0; 00168 while (!myFrontierList.empty()) { 00169 num_visited += 1; 00170 // use the node with the minimal length 00171 EdgeInfo* const minimumInfo = myFrontierList.front(); 00172 const E* const minEdge = minimumInfo->edge; 00173 pop_heap(myFrontierList.begin(), myFrontierList.end(), myComparator); 00174 myFrontierList.pop_back(); 00175 myFound.push_back(minimumInfo); 00176 // check whether the destination node was already reached 00177 if (minEdge == to) { 00178 buildPathFrom(minimumInfo, into); 00179 endQuery(num_visited); 00180 // DEBUG 00181 //std::cout << "visited " + toString(num_visited) + " edges (final path length: " + toString(into.size()) + ")\n"; 00182 return; 00183 } 00184 minimumInfo->visited = true; 00185 const SUMOReal traveltime = minimumInfo->traveltime + getEffort(minEdge, vehicle, time + minimumInfo->traveltime); 00186 // check all ways from the node with the minimal length 00187 unsigned int i = 0; 00188 const unsigned int length_size = minEdge->getNoFollowing(); 00189 for (i = 0; i < length_size; i++) { 00190 const E* const follower = minEdge->getFollower(i); 00191 EdgeInfo* const followerInfo = &(myEdgeInfos[follower->getNumericalID()]); 00192 // check whether it can be used 00193 if (PF::operator()(follower, vehicle)) { 00194 continue; 00195 } 00196 const SUMOReal oldEffort = followerInfo->traveltime; 00197 if (!followerInfo->visited && traveltime < oldEffort) { 00198 // admissible A* heuristic: straight line distance at maximum speed 00199 const SUMOReal heuristic_remaining = minEdge->getDistanceTo(to) / vehicle->getMaxSpeed(); 00200 followerInfo->traveltime = traveltime; 00201 followerInfo->heuristicTime = traveltime + heuristic_remaining; 00202 followerInfo->prev = minimumInfo; 00203 if (oldEffort == std::numeric_limits<SUMOReal>::max()) { 00204 myFrontierList.push_back(followerInfo); 00205 push_heap(myFrontierList.begin(), myFrontierList.end(), myComparator); 00206 } else { 00207 push_heap(myFrontierList.begin(), 00208 find(myFrontierList.begin(), myFrontierList.end(), followerInfo) + 1, 00209 myComparator); 00210 } 00211 } 00212 } 00213 } 00214 endQuery(num_visited); 00215 myErrorMsgHandler->inform("No connection between '" + from->getID() + "' and '" + to->getID() + "' found."); 00216 } 00217 00218 00219 SUMOReal recomputeCosts(const std::vector<const E*> &edges, const V* const v, SUMOTime msTime) const { 00220 const SUMOReal time = STEPS2TIME(msTime); 00221 SUMOReal costs = 0; 00222 for (typename std::vector<const E*>::const_iterator i = edges.begin(); i != edges.end(); ++i) { 00223 if (PF::operator()(*i, v)) { 00224 return -1; 00225 } 00226 costs += getEffort(*i, v, time + costs); 00227 } 00228 return costs; 00229 } 00230 00231 public: 00233 void buildPathFrom(EdgeInfo* rbegin, std::vector<const E*> &edges) { 00234 std::deque<const E*> tmp; 00235 while (rbegin != 0) { 00236 tmp.push_front((E*) rbegin->edge); // !!! 00237 rbegin = rbegin->prev; 00238 } 00239 std::copy(tmp.begin(), tmp.end(), std::back_inserter(edges)); 00240 } 00241 00242 protected: 00244 std::vector<EdgeInfo> myEdgeInfos; 00245 00247 std::vector<EdgeInfo*> myFrontierList; 00249 std::vector<EdgeInfo*> myFound; 00250 00251 EdgeInfoComparator myComparator; 00252 00254 MsgHandler* const myErrorMsgHandler; 00255 00256 }; 00257 00258 00259 template<class E, class V, class PF, class EC> 00260 class AStarRouterTT_ByProxi : public AStarRouterTTBase<E, V, PF> { 00261 public: 00263 typedef SUMOReal(EC::* Operation)(const E* const, const V* const, SUMOReal) const; 00264 00265 AStarRouterTT_ByProxi(size_t noE, bool unbuildIsWarningOnly, EC* receiver, Operation operation) 00266 : AStarRouterTTBase<E, V, PF>(noE, unbuildIsWarningOnly), 00267 myReceiver(receiver), myOperation(operation) {} 00268 00269 inline SUMOReal getEffort(const E* const e, const V* const v, SUMOReal t) const { 00270 return (myReceiver->*myOperation)(e, v, t); 00271 } 00272 00273 private: 00275 EC* myReceiver; 00276 00278 Operation myOperation; 00279 00280 00281 }; 00282 00283 00284 template<class E, class V, class PF> 00285 class AStarRouterTT_Direct : public AStarRouterTTBase<E, V, PF> { 00286 public: 00288 typedef SUMOReal(E::* Operation)(const V* const, SUMOReal) const; 00289 00290 AStarRouterTT_Direct(size_t noE, bool unbuildIsWarningOnly, Operation operation) 00291 : AStarRouterTTBase<E, V, PF>(noE, unbuildIsWarningOnly), myOperation(operation) {} 00292 00293 inline SUMOReal getEffort(const E* const e, const V* const v, SUMOReal t) const { 00294 return (e->*myOperation)(v, t); 00295 } 00296 00297 private: 00298 Operation myOperation; 00299 00300 }; 00301 00302 00303 #endif 00304 00305 /****************************************************************************/ 00306