Node2EdgeRouter.h

Go to the documentation of this file.

/****************************************************************************/
// Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
// Copyright (C) 2012-2024 German Aerospace Center (DLR) and others.
// This program and the accompanying materials are made available under the
// terms of the Eclipse Public License 2.0 which is available at
// https://www.eclipse.org/legal/epl-2.0/
// This Source Code may also be made available under the following Secondary
// Licenses when the conditions for such availability set forth in the Eclipse
// Public License 2.0 are satisfied: GNU General Public License, version 2
// or later which is available at
// https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
// SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
/****************************************************************************/
// Simple extension of edge-based AStarRouter, adding a) a new method computeNode2Edge,
// which computes a shortest path starting at a given node and ending at a given edge,
// and b) a new method computeNode2Edges, which computes shortest paths, each starting
// at the given node and ending at one of the given edges
/****************************************************************************/
#pragma once
#include <config.h>
#include <unordered_set>
#include <utils/common/StdDefs.h>
#include "AStarRouter.h"
 
// ===========================================================================
// class definitions
// ===========================================================================
template<class E, class N, class V>
class Node2EdgeRouter : public AStarRouter<E, V> {
public:
    typedef AbstractLookupTable<E, V> LookupTable;
 
    typename SUMOAbstractRouter<E, V>::EdgeInfo* edgeInfo(const E* const edge) {
        return &(this->myEdgeInfos[edge->getNumericalID()]);
    }
    const typename SUMOAbstractRouter<E, V>::EdgeInfo* edgeInfo(const E* const edge) const {
        return &(this->myEdgeInfos[edge->getNumericalID()]);
    }
 
    Node2EdgeRouter(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) :
        AStarRouter<E, V>(edges, unbuildIsWarning, operation, lookup, havePermissions, haveRestrictions) {
    }
 
    Node2EdgeRouter(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) :
        AStarRouter<E, V>(edgeInfos, unbuildIsWarning, operation, lookup, havePermissions, haveRestrictions) {
    }
 
    virtual ~Node2EdgeRouter() {
        WRITE_MESSAGE("The following stats for AStarRouter (which might also be empty) belong to Node2EdgeRouter, a derived class:");
    }
 
    virtual SUMOAbstractRouter<E, V>* clone() {
        return new Node2EdgeRouter<E, N, V>(this->myEdgeInfos, this->myErrorMsgHandler == MsgHandler::getWarningInstance(), this->myOperation, this->myLookupTable,
                                            this->myHavePermissions, this->myHaveRestrictions);
    }
 
    virtual void reset(const V* const vehicle) {
        UNUSED_PARAMETER(vehicle);
        for (auto& edgeInfo : this->myFrontierList) {
            edgeInfo->reset();
        }
        this->myFrontierList.clear();
        for (auto& edgeInfo : this->myFound) {
            edgeInfo->reset();
        }
        this->myFound.clear();
    }
 
    bool computeNode2Edges(const N* fromNode, const std::unordered_set<const E*>& toEdges, const V* const vehicle,
                           SUMOTime msTime, bool silent = false) {
        assert(fromNode != nullptr);
        // check whether fromNode and to can be used
        bool found = false;
        bool allVisited = true; // we try to disprove this
        for (const E* from : fromNode->getOutgoing()) {
            if (from->isInternal()) {
                continue;
            }
            if (this->myEdgeInfos[from->getNumericalID()].prohibited || this->isProhibited(from, vehicle)) {
                if (!silent) {
                    this->myErrorMsgHandler->inform("Vehicle '" + Named::getIDSecure(vehicle) + "' is not allowed on source edge '" + from->getID() + "'.");
                }
            } else {
                found = true;
                break;
            }
        }
        if (!found) {
            return false;
        }
        const std::vector<const E*>& fromEdges = fromNode->getOutgoing();
 
        if (fromEdges.empty() || toEdges.empty()) { // nothing to do here
            return false;
        }
 
        double length = 0.; // dummy for the via edge cost update
        this->startQuery();
#ifdef ASTAR_DEBUG_QUERY
        if (ASTAR_DEBUG_COND) {
            std::cout << "DEBUG: starting search for '" << Named::getIDSecure(vehicle) << "' speed: " << MIN2(vehicle->getMaxSpeed(), myMaxSpeed * vehicle->getChosenSpeedFactor()) << " time: " << STEPS2TIME(msTime) << "\n";
        }
#endif
 
        const SUMOVehicleClass vClass = vehicle == 0 ? SVC_IGNORING : vehicle->getVClass();
        init(fromEdges, vehicle, msTime);
        this->myAmClean = false;
        // loop
        int num_visited = 0;
        while (!this->myFrontierList.empty()) {
            num_visited += 1;
            // use the edge with the minimal length
            auto* const minimumInfo = this->myFrontierList.front();
            const E* const minEdge = minimumInfo->edge;
            // check whether the destination edge was already reached
            if (toEdges.count(minEdge)) {
                for (const E* to : toEdges) {
                    const auto& toInfo = this->myEdgeInfos[to->getNumericalID()];
                    if (!toInfo.visited) {
                        allVisited = false;
                        break;
                    }
                }
                if (allVisited) {
                    this->endQuery(num_visited);
                    return true;
                }
            }
            std::pop_heap(this->myFrontierList.begin(), this->myFrontierList.end(), this->myComparator);
            this->myFrontierList.pop_back();
            this->myFound.push_back(minimumInfo);
            minimumInfo->visited = true;
            const double effortDelta = this->getEffort(minEdge, vehicle, minimumInfo->leaveTime);
            const double leaveTime = minimumInfo->leaveTime + this->getTravelTime(minEdge, vehicle, minimumInfo->leaveTime, effortDelta);
 
            const double heuristic_remaining = 0;
            if (heuristic_remaining == UNREACHABLE) {
                continue;
            }
            const double heuristicEffort = minimumInfo->effort + effortDelta + heuristic_remaining;
            // check all ways from the edge with the minimal length
            for (const std::pair<const E*, const E*>& follower : minEdge->getViaSuccessors(vClass)) {
                auto& followerInfo = this->myEdgeInfos[follower.first->getNumericalID()];
                // check whether it can be used
                if (followerInfo.prohibited || this->isProhibited(follower.first, vehicle)) {
                    continue;
                }
                double effort = minimumInfo->effort + effortDelta;
                double time = leaveTime;
                this->updateViaEdgeCost(follower.second, vehicle, time, effort, length);
                const double oldEffort = followerInfo.effort;
                if ((!followerInfo.visited) && effort < oldEffort) {
                    followerInfo.effort = effort;
                    // if we use the effort including the via effort below we would count the via twice as shown by the ticket676 test
                    // but we should never get below the real effort, see #12463
                    followerInfo.heuristicEffort = MAX2(MIN2(heuristicEffort, followerInfo.heuristicEffort), effort);
                    followerInfo.leaveTime = time;
                    followerInfo.prev = minimumInfo;
                    if (oldEffort == std::numeric_limits<double>::max()) {
                        this->myFrontierList.push_back(&followerInfo);
                        std::push_heap(this->myFrontierList.begin(), this->myFrontierList.end(), this->myComparator);
                    } else {
                        auto fi = std::find(this->myFrontierList.begin(), this->myFrontierList.end(), &followerInfo);
                        assert(fi != this->myFrontierList.end());
                        std::push_heap(this->myFrontierList.begin(), fi + 1, this->myComparator);
                    }
                }
            }
        }
        this->endQuery(num_visited);
        for (const E* to : toEdges) {
            const auto& toInfo = this->myEdgeInfos[to->getNumericalID()];
            if (toInfo.visited) {
                if (!silent) {
                    this->myErrorMsgHandler->informf("Only some connections between node '%' and the given edges were found.", fromNode->getID());
                }
                return true; // at least one connection could be found
            }
        }
        if (!silent) {
            this->myErrorMsgHandler->informf("No connections between node '%' and the given edges were found.", fromNode->getID());
        }
        return false;
    }
 
    bool computeNode2Edge(const N* fromNode, const E* to, const V* const vehicle,
                          SUMOTime msTime, std::vector<const E*>& into, bool silent = false) {
        assert(fromNode != nullptr && to != nullptr);
        // check whether fromNode and to can be used
        bool found = false;
        for (const E* from : fromNode->getOutgoing()) {
            if (from->isInternal()) {
                continue;
            }
            if (this->myEdgeInfos[from->getNumericalID()].prohibited || this->isProhibited(from, vehicle)) {
                if (!silent) {
                    this->myErrorMsgHandler->inform("Vehicle '" + Named::getIDSecure(vehicle) + "' is not allowed on source edge '" + from->getID() + "'.");
                }
            } else {
                found = true;
                break;
            }
        }
        if (!found) {
            return false;
        }
        const std::vector<const E*>& fromEdges = fromNode->getOutgoing();
 
        if (fromEdges.empty()) { // nothing to do here
            return false;
        }
        if (this->myEdgeInfos[to->getNumericalID()].prohibited || this->isProhibited(to, vehicle)) {
            if (!silent) {
                this->myErrorMsgHandler->inform("Vehicle '" + Named::getIDSecure(vehicle) + "' is not allowed on destination edge '" + to->getID() + "'.");
            }
            return false;
        }
        double length = 0.; // dummy for the via edge cost update
        this->startQuery();
 
        const SUMOVehicleClass vClass = vehicle == 0 ? SVC_IGNORING : vehicle->getVClass();
        init(fromEdges, vehicle, msTime);
        this->myAmClean = false;
        // loop
        int num_visited = 0;
        const bool mayRevisit = this->myLookupTable != nullptr && !this->myLookupTable->consistent();
        const double speed = vehicle == nullptr ? this->myMaxSpeed : MIN2(vehicle->getMaxSpeed(), this->myMaxSpeed * vehicle->getChosenSpeedFactor());
        while (!this->myFrontierList.empty()) {
            num_visited += 1;
            // use the edge with the minimal length
            auto* const minimumInfo = this->myFrontierList.front();
            const E* const minEdge = minimumInfo->edge;
            // check whether the destination edge was already reached
            if (minEdge == to) {
                this->buildPathFrom(minimumInfo, into);
                this->endQuery(num_visited);
                return true;
            }
            std::pop_heap(this->myFrontierList.begin(), this->myFrontierList.end(), this->myComparator);
            this->myFrontierList.pop_back();
            this->myFound.push_back(minimumInfo);
            minimumInfo->visited = true;
            const double effortDelta = this->getEffort(minEdge, vehicle, minimumInfo->leaveTime);
            const double leaveTime = minimumInfo->leaveTime + this->getTravelTime(minEdge, vehicle, minimumInfo->leaveTime, effortDelta);
 
            // admissible A* heuristic: straight line distance at maximum speed
            // this is calculated from the end of minEdge so it possibly includes via efforts to the followers
            const double heuristic_remaining = (this->myLookupTable == nullptr ? minEdge->getDistanceTo(to) / speed :
                                                this->myLookupTable->lowerBound(minEdge, to, speed, vehicle->getChosenSpeedFactor(),
                                                        minEdge->getMinimumTravelTime(nullptr), to->getMinimumTravelTime(nullptr)));
            //const double heuristic_remaining = 0.;
            if (heuristic_remaining == UNREACHABLE) {
                continue;
            }
            const double heuristicEffort = minimumInfo->effort + effortDelta + heuristic_remaining;
            // check all ways from the edge with the minimal length
            for (const std::pair<const E*, const E*>& follower : minEdge->getViaSuccessors(vClass)) {
                auto& followerInfo = this->myEdgeInfos[follower.first->getNumericalID()];
                // check whether it can be used
                if (followerInfo.prohibited || this->isProhibited(follower.first, vehicle)) {
                    continue;
                }
                double effort = minimumInfo->effort + effortDelta;
                double time = leaveTime;
                this->updateViaEdgeCost(follower.second, vehicle, time, effort, length);
                const double oldEffort = followerInfo.effort;
                if ((!followerInfo.visited || mayRevisit) && effort < oldEffort) {
                    followerInfo.effort = effort;
                    // if we use the effort including the via effort below we would count the via twice as shown by the ticket676 test
                    // but we should never get below the real effort, see #12463
                    followerInfo.heuristicEffort = MAX2(MIN2(heuristicEffort, followerInfo.heuristicEffort), effort);
                    followerInfo.leaveTime = time;
                    followerInfo.prev = minimumInfo;
                    if (oldEffort == std::numeric_limits<double>::max()) {
                        this->myFrontierList.push_back(&followerInfo);
                        std::push_heap(this->myFrontierList.begin(), this->myFrontierList.end(), this->myComparator);
                    } else {
                        auto fi = std::find(this->myFrontierList.begin(), this->myFrontierList.end(), &followerInfo);
                        if (fi == this->myFrontierList.end()) {
                            assert(mayRevisit);
                            this->myFrontierList.push_back(&followerInfo);
                            std::push_heap(this->myFrontierList.begin(), this->myFrontierList.end(), this->myComparator);
                        } else {
                            std::push_heap(this->myFrontierList.begin(), fi + 1, this->myComparator);
                        }
                    }
                }
            }
        }
        this->endQuery(num_visited);
        if (!silent) {
            this->myErrorMsgHandler->informf("No connection between node '%' and edge '%' found.", fromNode->getID(), to->getID());
        }
        return false;
    }
 
    void updateViaCostUpToEdge(const E* const prev, const E* const e, const V* const v, double& time, double& effort, double& length) const;
 
    double recomputeCostsNoLastEdge(const std::vector<const E*>& edges, const V* const v, SUMOTime msTime, double* lengthp = nullptr) const {
        const E* lastEdge = edges.back();
        double time = STEPS2TIME(msTime);
        double effort = 0.;
        double length = 0.;
        if (lengthp == nullptr) {
            lengthp = &length;
        } else {
            *lengthp = 0.;
        }
        const E* prev = nullptr;
        for (const E* const e : edges) {
            if (e == lastEdge) {
                break;
            }
            if (isProhibited(e, v)) {
                return -1;
            }
            updateViaCost(prev, e, v, time, effort, *lengthp);
            prev = e;
        }
        updateViaCostUpToEdge(prev, lastEdge, v, time, effort, *lengthp);
        return effort;
    }
 
    virtual void setBulkMode(const bool mode) {
        UNUSED_PARAMETER(mode);
        throw std::runtime_error("Bulk mode is not supported by the node-to-edge router.");
    }
 
protected:
    void init(std::vector<const E*> fromEdges, const V* const vehicle, const SUMOTime msTime);
};
 
// ===========================================================================
// method definitions
// ===========================================================================
 
template<class E, class N, class V>
void Node2EdgeRouter<E, N, V>::updateViaCostUpToEdge(const E* const prev, const E* const e, const V* const v, double& time, double& effort, double& length) const {
    assert(prev && e);
    for (const std::pair<const E*, const E*>& follower : prev->getViaSuccessors()) {
        if (follower.first == e) {
            updateViaEdgeCost(follower.second, v, time, effort, length);
            break;
        }
    }
}
 
template<class E, class N, class V>
void Node2EdgeRouter<E, N, V>::init(std::vector<const E*> fromEdges, const V* const vehicle, const SUMOTime msTime) {
    // all EdgeInfos touched in the previous query are either in myFrontierList or myFound: clean those up
    for (auto& edgeInfo : this->myFrontierList) {
        edgeInfo->reset();
    }
    this->myFrontierList.clear();
    for (auto& edgeInfo : this->myFound) {
        edgeInfo->reset();
    }
    this->myFound.clear();
    for (const E* from : fromEdges) {
        if (from->isInternal()) {
            continue;
        }
        int edgeID = from->getNumericalID();
        auto& fromInfo = this->myEdgeInfos[edgeID];
        if (fromInfo.prohibited || this->isProhibited(from, vehicle)) {
            continue;
        }
        fromInfo.effort = 0.;
        fromInfo.heuristicEffort = 0.;
        fromInfo.prev = nullptr;
        fromInfo.leaveTime = STEPS2TIME(msTime);
        this->myFrontierList.push_back(&fromInfo);
    }
}