CHRouter.h

Go to the documentation of this file.

/****************************************************************************/
// Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.org/sumo
// Copyright (C) 2001-2018 German Aerospace Center (DLR) and others.
// This program and the accompanying materials
// are made available under the terms of the Eclipse Public License v2.0
// which accompanies this distribution, and is available at
// http://www.eclipse.org/legal/epl-v20.html
// SPDX-License-Identifier: EPL-2.0
/****************************************************************************/
// Shortest Path search using a Contraction Hierarchy
/****************************************************************************/
#ifndef CHRouter_h
#define CHRouter_h


// ===========================================================================
// included modules
// ===========================================================================
#include <config.h>

#include <string>
#include <functional>
#include <vector>
#include <set>
#include <limits>
#include <algorithm>
#include <iterator>
#include <deque>
#include <utils/common/SysUtils.h>
#include <utils/common/MsgHandler.h>
#include <utils/common/StdDefs.h>
#include <utils/vehicle/SUMOAbstractRouter.h>
#include "CHBuilder.h"

//#define CHRouter_DEBUG_QUERY
//#define CHRouter_DEBUG_QUERY_PERF

// ===========================================================================
// class definitions
// ===========================================================================
template<class E, class V, class BASE>
class CHRouter: public BASE {

public:
    typedef double(* Operation)(const E* const, const V* const, double);

    class EdgeInfo {
    public:
        EdgeInfo(const E* e) :
            edge(e),
            traveltime(std::numeric_limits<double>::max()),
            prev(0),
            visited(false) {
        }

        const E* edge;

        double traveltime;

        EdgeInfo* prev;

        bool visited;

        inline void reset() {
            traveltime = std::numeric_limits<double>::max();
            visited = false;
        }
    };


    typedef std::pair<const EdgeInfo*, const EdgeInfo*> Meeting;

    class Unidirectional {
    public:
        Unidirectional(const std::vector<E*>& edges, bool forward):
            myAmForward(forward),
            myVehicle(0) {
            for (typename std::vector<E*>::const_iterator i = edges.begin(); i != edges.end(); ++i) {
                myEdgeInfos.push_back(EdgeInfo(*i));
            }
        }

        inline bool found(const E* edge) const {
            return myFound.count(edge) > 0;
        }

        inline EdgeInfo* getEdgeInfo(const E* const edge) {
            return &(myEdgeInfos[edge->getNumericalID()]);
        }

        inline const EdgeInfo* getEdgeInfo(const E* const edge) const {
            return &(myEdgeInfos[edge->getNumericalID()]);
        }

        class EdgeInfoByTTComparator {
        public:
            bool operator()(const EdgeInfo* nod1, const EdgeInfo* nod2) const {
                if (nod1->traveltime == nod2->traveltime) {
                    return nod1->edge->getNumericalID() > nod2->edge->getNumericalID();
                }
                return nod1->traveltime > nod2->traveltime;
            }
        };


        void init(const E* const start, const V* const vehicle) {
            assert(vehicle != 0);
            // all EdgeInfos touched in the previous query are either in myFrontier or myFound: clean those up
            for (typename std::vector<EdgeInfo*>::iterator i = myFrontier.begin(); i != myFrontier.end(); i++) {
                (*i)->reset();
            }
            myFrontier.clear();
            for (typename std::set<const E*>::const_iterator i = myFound.begin(); i != myFound.end(); i++) {
                getEdgeInfo(*i)->reset();
            }
            myFound.clear();
            myVehicle = vehicle;
            EdgeInfo* startInfo = getEdgeInfo(start);
            startInfo->traveltime = 0;
            startInfo->prev = 0;
            myFrontier.push_back(startInfo);
        }


        typedef std::vector<typename CHBuilder<E, V>::Connection> ConnectionVector;
        bool step(const std::vector<ConnectionVector>& uplinks, const Unidirectional& otherSearch, double& minTTSeen, Meeting& meeting) {
            // pop the node with the minimal length
            EdgeInfo* const minimumInfo = myFrontier.front();
            pop_heap(myFrontier.begin(), myFrontier.end(), myComparator);
            myFrontier.pop_back();
            // check for a meeting with the other search
            const E* const minEdge = minimumInfo->edge;
#ifdef CHRouter_DEBUG_QUERY
            std::cout << "DEBUG: " << (myAmForward ? "Forward" : "Backward") << " hit '" << minEdge->getID() << "' Q: ";
            for (typename std::vector<EdgeInfo*>::iterator it = myFrontier.begin(); it != myFrontier.end(); it++) {
                std::cout << (*it)->traveltime << "," << (*it)->edge->getID() << " ";
            }
            std::cout << "\n";
#endif
            if (otherSearch.found(minEdge)) {
                const EdgeInfo* const otherInfo = otherSearch.getEdgeInfo(minEdge);
                const double ttSeen = minimumInfo->traveltime + otherInfo->traveltime;
#ifdef CHRouter_DEBUG_QUERY
                std::cout << "DEBUG: " << (myAmForward ? "Forward" : "Backward") << "-Search hit other search at '" << minEdge->getID() << "', tt: " << ttSeen << " \n";
#endif
                if (ttSeen < minTTSeen) {
                    minTTSeen = ttSeen;
                    if (myAmForward) {
                        meeting.first = minimumInfo;
                        meeting.second = otherInfo;
                    } else {
                        meeting.first = otherInfo;
                        meeting.second = minimumInfo;
                    }
                }
            }
            // prepare next steps
            minimumInfo->visited = true;
            // XXX we only need to keep found elements if they have a higher rank than the lowest rank in the other search queue
            myFound.insert(minimumInfo->edge);
            const ConnectionVector& upward = uplinks[minEdge->getNumericalID()];
            for (typename ConnectionVector::const_iterator it = upward.begin(); it != upward.end(); it++) {
                EdgeInfo* upwardInfo = &myEdgeInfos[it->target];
                const double traveltime = minimumInfo->traveltime + it->cost;
                const SUMOVehicleClass svc = myVehicle->getVClass();
                // check whether it can be used
                if ((it->permissions & svc) != svc) {
                    continue;
                }
                const double oldTraveltime = upwardInfo->traveltime;
                if (!upwardInfo->visited && traveltime < oldTraveltime) {
                    upwardInfo->traveltime = traveltime;
                    upwardInfo->prev = minimumInfo;
                    if (oldTraveltime == std::numeric_limits<double>::max()) {
                        myFrontier.push_back(upwardInfo);
                        push_heap(myFrontier.begin(), myFrontier.end(), myComparator);
                    } else {
                        push_heap(myFrontier.begin(),
                                  find(myFrontier.begin(), myFrontier.end(), upwardInfo) + 1,
                                  myComparator);
                    }
                }
            }
            // @note: this effectively does a full dijkstra search.
            // the effort compared to the naive stopping criterion is thus
            // quadrupled. We could implement a better stopping criterion (Holte)
            // However since the search shall take place in a contracted graph
            // it probably does not matter
            return !myFrontier.empty() && myFrontier.front()->traveltime < minTTSeen;
        }

    private:
        bool myAmForward;
        std::vector<EdgeInfo*> myFrontier;
        std::set<const E*> myFound;
        std::vector<EdgeInfo> myEdgeInfos;

        EdgeInfoByTTComparator myComparator;

        const V* myVehicle;

    };

    CHRouter(const std::vector<E*>& edges, bool unbuildIsWarning, typename BASE::Operation operation,
             const SUMOVehicleClass svc,
             SUMOTime weightPeriod,
             bool validatePermissions):
        BASE("CHRouter", operation),
        myEdges(edges),
        myErrorMsgHandler(unbuildIsWarning ? MsgHandler::getWarningInstance() : MsgHandler::getErrorInstance()),
        myForwardSearch(edges, true),
        myBackwardSearch(edges, false),
        myHierarchyBuilder(new CHBuilder<E, V>(edges, unbuildIsWarning, svc, validatePermissions)),
        myHierarchy(0),
        myWeightPeriod(weightPeriod),
        myValidUntil(0),
        mySVC(svc) {
    }

    CHRouter(const std::vector<E*>& edges, bool unbuildIsWarning, typename BASE::Operation operation,
             const SUMOVehicleClass svc,
             SUMOTime weightPeriod,
             const typename CHBuilder<E, V>::Hierarchy* hierarchy) :
        BASE("CHRouterClone", operation),
        myEdges(edges),
        myErrorMsgHandler(unbuildIsWarning ? MsgHandler::getWarningInstance() : MsgHandler::getErrorInstance()),
        myForwardSearch(edges, true),
        myBackwardSearch(edges, false),
        myHierarchyBuilder(0),
        myHierarchy(hierarchy),
        myWeightPeriod(weightPeriod),
        myValidUntil(0),
        mySVC(svc) {
    }

    virtual ~CHRouter() {
        if (myHierarchyBuilder != 0) {
            delete myHierarchy;
            delete myHierarchyBuilder;
        }
    }


    virtual SUMOAbstractRouter<E, V>* clone() {
        WRITE_MESSAGE("Cloning Contraction Hierarchy for " + SumoVehicleClassStrings.getString(mySVC) + " and time " + time2string(myValidUntil) + ".");
        CHRouter<E, V, BASE>* clone = new CHRouter<E, V, BASE>(myEdges, myErrorMsgHandler == MsgHandler::getWarningInstance(), this->myOperation,
                mySVC, myWeightPeriod, myHierarchy);
        clone->myValidUntil = myValidUntil;
        return clone;
    }

    virtual bool compute(const E* from, const E* to, const V* const vehicle,
                         SUMOTime msTime, std::vector<const E*>& into) {
        assert(from != 0 && to != 0);
        // assert(myHierarchyBuilder.mySPTree->validatePermissions() || vehicle->getVClass() == mySVC || mySVC == SVC_IGNORING);
        // do we need to rebuild the hierarchy?
        if (msTime >= myValidUntil) {
            while (msTime >= myValidUntil) {
                myValidUntil += myWeightPeriod;
            }
            buildContractionHierarchy(myValidUntil - myWeightPeriod, vehicle);
        }
        // ready for routing
        this->startQuery();
        myForwardSearch.init(from, vehicle);
        myBackwardSearch.init(to, vehicle);
        double minTTSeen = std::numeric_limits<double>::max();
        Meeting meeting(static_cast<EdgeInfo*>(0), static_cast<EdgeInfo*>(0));
        bool continueForward = true;
        bool continueBackward = true;
        int num_visited_fw = 0;
        int num_visited_bw = 0;
        bool result = true;
        while (continueForward || continueBackward) {
            if (continueForward) {
                continueForward = myForwardSearch.step(myHierarchy->forwardUplinks, myBackwardSearch, minTTSeen, meeting);
                num_visited_fw += 1;
            }
            if (continueBackward) {
                continueBackward = myBackwardSearch.step(myHierarchy->backwardUplinks, myForwardSearch, minTTSeen, meeting);
                num_visited_bw += 1;
            }
        }
        if (minTTSeen < std::numeric_limits<double>::max()) {
            buildPathFromMeeting(meeting, into);
        } else {
            myErrorMsgHandler->inform("No connection between edge '" + from->getID() + "' and edge '" + to->getID() + "' found.");
            result = false;
        }
#ifdef CHRouter_DEBUG_QUERY_PERF
        std::cout << "visited " << num_visited_fw + num_visited_bw << " edges (" << num_visited_fw << "," << num_visited_bw << ") ,final path length: " + toString(into.size()) + ")\n";
#endif
        this->endQuery(num_visited_bw + num_visited_fw);
        return result;
    }


    void buildPathFromMeeting(Meeting meeting, std::vector<const E*>& into) const {
        std::deque<const E*> tmp;
        const EdgeInfo* backtrack = meeting.first;
        while (backtrack != 0) {
            tmp.push_front((E*) backtrack->edge);  // !!!
            backtrack = backtrack->prev;
        }
        backtrack = meeting.second->prev; // don't use central edge twice
        while (backtrack != 0) {
            tmp.push_back((E*) backtrack->edge);  // !!!
            backtrack = backtrack->prev;
        }
        // expand shortcuts
        const E* prev = 0;
        while (!tmp.empty()) {
            const E* cur = tmp.front();
            tmp.pop_front();
            if (prev == 0) {
                into.push_back(cur);
                prev = cur;
            } else {
                const E* via = getVia(prev, cur);
                if (via == 0) {
                    into.push_back(cur);
                    prev = cur;
                } else {
                    tmp.push_front(cur);
                    tmp.push_front(via);
                }
            }
        }
    }

    void buildContractionHierarchy(SUMOTime time, const V* const vehicle) {
        if (myHierarchyBuilder != 0) {
            delete myHierarchy;
            myHierarchy = myHierarchyBuilder->buildContractionHierarchy(time, vehicle, this);
        }
        // declare new validUntil (prevent overflow)
        if (myWeightPeriod < std::numeric_limits<int>::max()) {
            myValidUntil = time + myWeightPeriod;
        } else {
            myValidUntil = myWeightPeriod;
        }
    }

private:
    // retrieve the via edge for a shortcut
    const E* getVia(const E* forwardFrom, const E* forwardTo) const {
        typename CHBuilder<E, V>::ConstEdgePair forward(forwardFrom, forwardTo);
        typename CHBuilder<E, V>::ShortcutVia::const_iterator it = myHierarchy->shortcuts.find(forward);
        if (it != myHierarchy->shortcuts.end()) {
            return it->second;
        } else {
            return 0;
        }
    }


private:
    const std::vector<E*>& myEdges;

    MsgHandler* const myErrorMsgHandler;

    Unidirectional myForwardSearch;
    Unidirectional myBackwardSearch;

    CHBuilder<E, V>* myHierarchyBuilder;
    const typename CHBuilder<E, V>::Hierarchy* myHierarchy;

    const SUMOTime myWeightPeriod;

    SUMOTime myValidUntil;

    const SUMOVehicleClass mySVC;
};


#endif

/****************************************************************************/