SUMO - Simulation of Urban MObility
AStarRouter.h
Go to the documentation of this file.
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 
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Friends Defines