Dijkstra.java

Go to the documentation of this file.

/* *********************************************************************** *
 * project: org.matsim.*
 * Dijkstra.java
 *                                                                         *
 * *********************************************************************** *
 *                                                                         *
 * copyright       : (C) 2007 by the members listed in the COPYING,        *
 *                   LICENSE and WARRANTY file.                            *
 * email           : info at matsim dot org                                *
 *                                                                         *
 * *********************************************************************** *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *   See also COPYING, LICENSE and WARRANTY file                           *
 *                                                                         *
 * *********************************************************************** */

package org.matsim.core.router;

import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import java.util.Set;

import org.apache.logging.log4j.LogManager;
import org.apache.logging.log4j.Logger;
import org.matsim.api.core.v01.Id;
import org.matsim.api.core.v01.network.Link;
import org.matsim.api.core.v01.network.Network;
import org.matsim.api.core.v01.network.Node;
import org.matsim.api.core.v01.population.Person;
import org.matsim.core.router.priorityqueue.WrappedBinaryMinHeap;
import org.matsim.core.router.util.DijkstraNodeData;
import org.matsim.core.router.util.LeastCostPathCalculator;
import org.matsim.core.router.util.PreProcessDijkstra;
import org.matsim.core.router.util.TravelDisutility;
import org.matsim.core.router.util.TravelTime;
import org.matsim.core.utils.collections.RouterPriorityQueue;
import org.matsim.vehicles.Vehicle;

 public class Dijkstra implements LeastCostPathCalculator {
    // yyyyyy I don't think that we should make this class publicly inheritable; as we know, will eventually lead
    // to problems.  kai, feb'18

    private final static Logger log = LogManager.getLogger(Dijkstra.class);

    protected Network network;

    protected final TravelDisutility costFunction;

    protected final TravelTime timeFunction;

    final HashMap<Id<Node>, DijkstraNodeData> nodeData;

    private int iterationID = Integer.MIN_VALUE + 1;

    private Node deadEndEntryNode;

    /*package*/ final boolean pruneDeadEnds;


    private final PreProcessDijkstra preProcessData;

    private RouterPriorityQueue<Node> heap = null;

    private String[] modeRestriction = null;
    
    /*package*/ Person person = null;
    /*package*/ Vehicle vehicle = null;

    // please use DijkstraFactory when you want to create an instance of this
    protected Dijkstra(final Network network, final TravelDisutility costFunction, final TravelTime timeFunction) {
        this(network, costFunction, timeFunction, null);
    }

    // please use DijkstraFactory when you want to create an instance of this
    protected Dijkstra(final Network network, final TravelDisutility costFunction, final TravelTime timeFunction,
            final PreProcessDijkstra preProcessData) {

        this.network = network;
        this.costFunction = costFunction;
        this.timeFunction = timeFunction;
        this.preProcessData = preProcessData;

        this.nodeData = new HashMap<>((int)(network.getNodes().size() * 1.1), 0.95f);

        if (preProcessData != null) {
            if (!preProcessData.containsData()) {
                this.pruneDeadEnds = false;
                log.warn("The preprocessing data provided to router class Dijkstra contains no data! Please execute its run(...) method first!");
                log.warn("Running without dead-end pruning.");
            } else {
                this.pruneDeadEnds = true;
            }
        } else {
            this.pruneDeadEnds = false;
        }
    }

    @Deprecated
    public void setModeRestriction(final Set<String> modeRestriction) {
        if (modeRestriction == null) {
            this.modeRestriction = null;
        } else {
            this.modeRestriction = modeRestriction.toArray(new String[modeRestriction.size()]);
        }
    }

    @Override
    public Path calcLeastCostPath(final Node fromNode, final Node toNode, final double startTime, final Person person2, final Vehicle vehicle2) {

        /*
         * Ensure that the given nodes are part of the network used by the router. Otherwise, the router would
         * route within the network of the nodes and NOT the one provided when it was created. Previously, this 
         * caused problems when sub-networks where used.
         * cdobler, jun'14
         */
        checkNodeBelongToNetwork(fromNode);
        checkNodeBelongToNetwork(toNode);
        
        augmentIterationId(); // this call makes the class not thread-safe
        this.person = person2;
        this.vehicle = vehicle2;

        if (this.pruneDeadEnds) {
            this.deadEndEntryNode = getPreProcessData(toNode).getDeadEndEntryNode();
        }

        RouterPriorityQueue<Node> pendingNodes = (RouterPriorityQueue<Node>) createRouterPriorityQueue();
        initFromNode(fromNode, toNode, startTime, pendingNodes);

        Node foundToNode = searchLogic(fromNode, toNode, pendingNodes, person2, vehicle2 );
        
        if (foundToNode == null) return null;
        else {
            DijkstraNodeData outData = getData(foundToNode);
            double arrivalTime = outData.getTime();
            
            // now construct and return the path
            return constructPath(fromNode, foundToNode, startTime, arrivalTime);            
        }
    }

    /*
     * Move this code to a separate method since it needs to be extended by the MultiNodeDijkstra.
     * cdobler, jun'14
     */
    /*package*/ void checkNodeBelongToNetwork(Node node) {
        if (this.network.getNodes().get(node.getId()) != node) {
            throw new IllegalArgumentException("The nodes passed as parameters are not part of the network stored by "+
                    getClass().getSimpleName() + ": the validity of the results cannot be guaranteed. Aborting!");
        }
    }
    
    /*package*/ RouterPriorityQueue<? extends Node> createRouterPriorityQueue() {
        /*
         * The (Wrapped)BinaryMinHeap replaces the PseudoRemovePriorityQueue as default
         * PriorityQueue. It offers a decreaseKey method and uses less memory. As a result,
         * the routing performance is increased by ~20%.
         * 
         * When an ArrayRoutingNetwork is used, an BinaryMinHeap can be used which uses
         * the getArrayIndex() method of the ArrayRoutingNetworkNodes which further reduces
         * the memory consumption and increases the performance by another ~10%.
         */
        /*
         * Create a WrappedBinaryMinHeap and add all nodes initially once in the same order as
         * they are in the network. Internally, the heap assigns all elements an index. By adding
         * all elements initially, we ensure that the indices are in the same order as the nodes
         * are in the network. In case the router finds two connections with the same costs,
         * the selected connection depends on the indices.
         * Moreover, re-use the heap instead of creating it from scratch for each calculated route.
         * According to findings from the FastDijkstra, this should be faster.
         * cdobler, sep'17
         */
        if (this.heap == null) {
            this.heap = new WrappedBinaryMinHeap<>(this.network.getNodes().size());
            for (Node node : this.network.getNodes().values()) this.heap.add(node, 0.);
        }
        this.heap.reset();          
        return this.heap;
//      return new WrappedBinaryMinHeap<>(this.network.getNodes().size());
//      return new PseudoRemovePriorityQueue<>(this.network.getNodes().size());
    }
    
    /*package*/ Node searchLogic( final Node fromNode, final Node toNode, final RouterPriorityQueue<Node> pendingNodes, Person person, Vehicle vehicle ) {
        // It is a bit overkill to pass Person and Vehicle.  However, since the method is package-private, I think that it is acceptable.  For
        // a more public method, presumably only an "info" string should be passed.  kai, oct'21

        boolean stillSearching = true;
        
        while (stillSearching) {
            Node outNode = pendingNodes.poll();

            if (outNode == null) {
                log.warn("No route was found from node " + fromNode.getId() + " to node " + toNode.getId() + ". " + createInfoMessage( person, vehicle ) + "Some possible reasons:" );
                log.warn("  * Network is not connected.  Run NetworkCleaner().") ;
                log.warn("  * Network for considered mode does not even exist.  Modes need to be entered for each link in network.xml.");
                log.warn("  * Network for considered mode is not connected to starting or ending point of route.  Setting insertingAccessEgressWalk to true may help.");
                log.warn("This will now return null, but it may fail later with a null pointer exception.");
                return null;
            }

            if (outNode == toNode) {
                stillSearching = false;
            } else {
                relaxNode(outNode, toNode, pendingNodes);
            }
        }
        return toNode;
    }
    static StringBuilder createInfoMessage( Person person, Vehicle vehicle ){
        StringBuilder strb = new StringBuilder();
        boolean flag = false ;
        if ( person != null ) {
            strb.append( person.getId() );
            flag = true ;
        }
        if ( vehicle !=null ) {
            strb.append( vehicle.getId() );
            flag = true;
        }
        if ( flag ) {
            strb.append( ". " );
        }
        return strb;
    }

    protected Path constructPath(Node fromNode, Node toNode, double startTime, double arrivalTime) {
        List<Node> nodes = new ArrayList<>();
        List<Link> links = new ArrayList<>();

        nodes.add(0, toNode);
        Link tmpLink = getData(toNode).getPrevLink();
        if (tmpLink != null) {
            while (tmpLink.getFromNode() != fromNode) {
                links.add(0, tmpLink);
                nodes.add(0, tmpLink.getFromNode());
                tmpLink = getData(tmpLink.getFromNode()).getPrevLink();
            }
            links.add(0, tmpLink);
            nodes.add(0, tmpLink.getFromNode());
            
        }

        DijkstraNodeData toNodeData = getData(toNode);
        Path path = new Path(nodes, links, arrivalTime - startTime, toNodeData.getCost());
        return path;
    }
    
    /*package*/ void initFromNode(final Node fromNode, final Node toNode, final double startTime,
            final RouterPriorityQueue<Node> pendingNodes) {
        DijkstraNodeData data = getData(fromNode);
        visitNode(fromNode, data, pendingNodes, startTime, 0, null);
    }

    protected void relaxNode(final Node outNode, final Node toNode, final RouterPriorityQueue<Node> pendingNodes) {

        DijkstraNodeData outData = getData(outNode);
        double currTime = outData.getTime();
        double currCost = outData.getCost();
        if (this.pruneDeadEnds) {
            PreProcessDijkstra.DeadEndData ddOutData = getPreProcessData(outNode);

            for (Link l : outNode.getOutLinks().values()) {
                relaxNodeLogic(l, pendingNodes, currTime, currCost, toNode, ddOutData);
            }
        } else { // this.pruneDeadEnds == false
            for (Link l : outNode.getOutLinks().values()) {
                relaxNodeLogic(l, pendingNodes, currTime, currCost, toNode, null);
            }               
        }
    }
    
    /*package*/ void relaxNodeLogic(final Link l, final RouterPriorityQueue<Node> pendingNodes, final double currTime, 
            final double currCost, final Node toNode, final PreProcessDijkstra.DeadEndData ddOutData) {
        if (this.pruneDeadEnds) {
            if (canPassLink(l)) {
                Node n = l.getToNode();
                PreProcessDijkstra.DeadEndData ddData = getPreProcessData(n);

                /* IF the current node n is not in a dead end
                 * OR it is in the same dead end as the fromNode
                 * OR it is in the same dead end as the toNode
                 * THEN we add the current node to the pending nodes */
                if ((ddData.getDeadEndEntryNode() == null)
                        || (ddOutData.getDeadEndEntryNode() != null)
                        || ((this.deadEndEntryNode != null)
                                && (this.deadEndEntryNode.getId() == ddData.getDeadEndEntryNode().getId()))) {
                    
                    addToPendingNodes(l, n, pendingNodes, currTime, currCost, toNode);
                }
            }
        } else {
            if (canPassLink(l)) {
                addToPendingNodes(l, l.getToNode(), pendingNodes, currTime, currCost, toNode);
            }
        }
    }
    
    protected boolean addToPendingNodes(final Link l, final Node n,
            final RouterPriorityQueue<Node> pendingNodes, final double currTime,
            final double currCost, final Node toNode) {
        
        final double travelTime = this.timeFunction.getLinkTravelTime(l, currTime, this.person, this.vehicle);
        final double travelCost = this.costFunction.getLinkTravelDisutility(l, currTime, this.person, this.vehicle);
        final DijkstraNodeData data = getData(n);
        if (!data.isVisited(getIterationId())) {
            visitNode(n, data, pendingNodes, currTime + travelTime, currCost + travelCost, l);
            return true;
        }
                
        final double nCost = data.getCost();
        final double totalCost = currCost + travelCost;
        if (totalCost < nCost) {
            revisitNode(n, data, pendingNodes, currTime + travelTime, totalCost, l);
            return true;
        } else if (totalCost == nCost) {
            // Special case: a node can be reached from two links with exactly the same costs.
            // Decide based on the linkId which one to take... just have to common criteria to be deterministic.
            Link prevLink = data.getPrevLink();
            if (prevLink != null && prevLink.getId().compareTo(l.getId()) > 0) {
                revisitNode(n, data, pendingNodes, currTime + travelTime, totalCost, l);
                return true;
            }
        }
        return false;
    }

    protected boolean canPassLink(final Link link) {
        if (this.modeRestriction == null) {
            return true;
        }
        for (String mode : this.modeRestriction) {
            if (link.getAllowedModes().contains(mode)) {
                return true;
            }
        }
        return false;
    }

    protected void revisitNode(final Node n, final DijkstraNodeData data, final RouterPriorityQueue<Node> pendingNodes, 
            final double time, final double cost, final Link outLink) {
        data.visit(outLink, cost, time, getIterationId());
        pendingNodes.decreaseKey(n, getPriority(data));
    }

    protected void visitNode(final Node n, final DijkstraNodeData data, final RouterPriorityQueue<Node> pendingNodes,
            final double time, final double cost, final Link outLink) {
        data.visit(outLink, cost, time, getIterationId());
        pendingNodes.add(n, getPriority(data));
    }

    protected void augmentIterationId() {
        if (getIterationId() == Integer.MAX_VALUE) {
            this.iterationID = Integer.MIN_VALUE + 1;
            resetNetworkVisited();
        } else {
            this.iterationID++;
        }
    }

    protected int getIterationId() {
        return this.iterationID;
    }

    private void resetNetworkVisited() {
        for (Node node : this.network.getNodes().values()) {
            DijkstraNodeData data = getData(node);
            data.resetVisited();
        }
    }

    protected double getPriority(final DijkstraNodeData data) {
        return data.getCost();
    }

    protected DijkstraNodeData getData(final Node n) {
        DijkstraNodeData r = this.nodeData.get(n.getId());
        if (null == r) {
            r = createNodeData();
            this.nodeData.put(n.getId(), r);
        }
        return r;
    }

    protected DijkstraNodeData createNodeData() {
        return new DijkstraNodeData();
    }
    
    protected PreProcessDijkstra.DeadEndData getPreProcessData(final Node n) {
        return this.preProcessData.getNodeData(n);
    }

    protected final Person getPerson() {
        return this.person;
    }
    
    protected final Vehicle getVehicle() {
        return this.vehicle;
    }
}