TransitLeastCostPathTree.java

Go to the documentation of this file.

/* *********************************************************************** *
 * project: org.matsim.*
 * TransitDijkstra.java
 *                                                                         *
 * *********************************************************************** *
 *                                                                         *
 * copyright       : (C) 2009 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.pt.router;

import java.util.ArrayList;
import java.util.HashMap;
import java.util.HashSet;
import java.util.LinkedList;
import java.util.List;
import java.util.Map;
import java.util.Set;

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.InitialNode;
import org.matsim.core.router.util.DijkstraNodeData;
import org.matsim.core.router.util.LeastCostPathCalculator.Path;
import org.matsim.core.router.util.TravelTime;
import org.matsim.core.utils.collections.PseudoRemovePriorityQueue;
import org.matsim.core.utils.collections.RouterPriorityQueue;
import org.matsim.pt.router.TransitRouterNetwork.TransitRouterNetworkLink;
import org.matsim.pt.router.TransitRouterNetwork.TransitRouterNetworkNode;
import org.matsim.pt.transitSchedule.api.TransitLine;
import org.matsim.pt.transitSchedule.api.TransitRoute;
import org.matsim.pt.transitSchedule.api.TransitStopFacility;
import org.matsim.vehicles.Vehicle;

public class TransitLeastCostPathTree {

    protected Network network;

    private final TransitTravelDisutility costFunction;

    private final TravelTime timeFunction;

    private final HashMap<Id<Node>, DijkstraNodeData> nodeData;
    private Person person = null;
    private Vehicle vehicle = null;
    private CustomDataManager customDataManager = new CustomDataManager();
    private Map<Node, InitialNode> fromNodes = null;

    private RouterPriorityQueue<Node> pendingNodes;

    public TransitLeastCostPathTree(final Network network, final TransitTravelDisutility costFunction,
                                    final TravelTime timeFunction,
                                    final Map<Node, InitialNode> fromNodes, final Person person) {
        this.network = network;
        this.costFunction = costFunction;
        this.timeFunction = timeFunction;

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

        //create tree
        this.resetNetworkVisited();
        this.person = person;
        this.customDataManager.reset();
        this.fromNodes = fromNodes;

        pendingNodes = (RouterPriorityQueue<Node>) createRouterPriorityQueue();
        for (Map.Entry<Node, InitialNode> entry : fromNodes.entrySet()) {
            DijkstraNodeData data = getData(entry.getKey());
            visitNode(entry.getKey(), data, pendingNodes, entry.getValue().initialTime, entry.getValue().initialCost, null);
        }

        // do the real work
        while (pendingNodes.size() > 0) {
            Node outNode = pendingNodes.poll();
            relaxNode(outNode, pendingNodes);
        }
    }

    public TransitLeastCostPathTree(final Network network, final TransitTravelDisutility costFunction,
                                    final TravelTime timeFunction,
                                    final Map<Node, InitialNode> fromNodes, final Map<Node, InitialNode> toNodes,
                                    final Person person) {
        this.network = network;
        this.costFunction = costFunction;
        this.timeFunction = timeFunction;

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

        //create tree
        this.resetNetworkVisited();
        this.person = person;
        this.customDataManager.reset();
        this.fromNodes = fromNodes;

        pendingNodes = (RouterPriorityQueue<Node>) createRouterPriorityQueue();
        for (Map.Entry<Node, InitialNode> entry : fromNodes.entrySet()) {
            DijkstraNodeData data = getData(entry.getKey());
            visitNode(entry.getKey(), data, pendingNodes, entry.getValue().initialTime, entry.getValue().initialCost, null);
        }

        expandNodeData(toNodes);
    }

    private int getIterationId() {
//      TODO could delete all the now unnecessary occurrences of the IterationID especially in DijkstraNodeData and
//      TODO replace it with a flag visited but that would interfere the useage of the MultiNodeDijkstra
        return 0;
    }

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

    private void expandNodeData(final Map<Node, InitialNode> toNodes) {
        Set<Node> endNodes = new HashSet<>(toNodes.keySet());
        double minCost = Double.POSITIVE_INFINITY;

        // do the real work
        while (endNodes.size() > 0) {
            Node outNode = pendingNodes.poll();

            if (outNode == null) {
                // seems we have no more nodes left, but not yet reached all endNodes...
                endNodes.clear();
            } else {
                DijkstraNodeData data = getData(outNode);
                boolean isEndNode = endNodes.remove(outNode);
                if (isEndNode) {
                    InitialNode initData = toNodes.get(outNode);
                    double cost = data.getCost() + initData.initialCost;
                    if (cost < minCost) {
                        minCost = cost;
                    }
                }
                if (data.getCost() > minCost) {
                    endNodes.clear(); // we can't get any better now
                } else {
                    relaxNode(outNode, pendingNodes);
                }
            }
        }
    }

    public InternalTransitPassengerRoute getTransitPassengerRoute(final Map<Node, InitialNode> toNodes) {
        //find the best node
        double minCost = Double.POSITIVE_INFINITY;
        Node minCostNode = null;
        for (Map.Entry<Node,InitialNode> e : toNodes.entrySet()) {
            Node currentNode = e.getKey();
            DijkstraNodeData r = this.nodeData.get(currentNode.getId());
            if (r == null) {
                expandNodeData(toNodes);
            }
            DijkstraNodeData data = getData(currentNode);
            InitialNode initData = e.getValue();
            double cost = data.getCost() + initData.initialCost;
            if (data.getCost() != 0.0 || fromNodes.containsKey(currentNode)) {
                if (cost < minCost) {
                    minCost = cost;
                    minCostNode = currentNode;
                }
            }
        }

        if (minCostNode == null) {
            return null;
        }

        // now construct route segments, which are required for TransitPassengerRoute
        List<RouteSegment> routeSegments = new ArrayList<>();

        TransitRouterNetworkLink link = (TransitRouterNetworkLink) getData(minCostNode).getPrevLink();
        TransitRouterNetworkLink downstreamLink = null;
        Node previousFromNode = minCostNode;
        double transferCost = 0.;

        while (link != null) {
            TransitRouterNetworkNode fromNode = link.fromNode;
            TransitRouterNetworkNode toNode = link.toNode;

            double travelTime = getData(toNode).getTime() - getData(fromNode).getTime();
            Id<TransitLine> transitLineId = null;
            Id<TransitRoute> routeId = null;

            boolean isTransferLeg = false;
            if (link.line==null) isTransferLeg = true;
            else {
                transitLineId = link.line.getId();
                routeId = link.route.getId();
            }

            if (downstreamLink==null && isTransferLeg) {
                // continuous transfers: see TransitRouterImplTest.testDoubleWalk.
                // Another possibility is that trip start with transfer itself, see TransitRouterImplTest.testDoubleWalkOnly.
                double tempTravelTime = 0.;
                TransitStopFacility toStop = null;

                if (routeSegments.size()==0) { // very first leg starts with transfer
                    toStop = toNode.stop.getStopFacility();
                } else {
                    RouteSegment routeSegment = routeSegments.remove(0);
                    travelTime = routeSegment.travelTime;
                    toStop = routeSegment.toStop;
                }

                routeSegments.add(0,
                        new RouteSegment(fromNode.stop.getStopFacility(),
                        toStop,
                        tempTravelTime+travelTime,
                        transitLineId,
                        routeId));
                //not sure, if transferCost will be included for every transfer or not. Currently, transfer cost will be accumulated for every transfer. Amit Sep'17
            } else if (downstreamLink == null // very first pt leg or first pt leg after transfer
                    || isTransferLeg ) {
                routeSegments.add(0, new RouteSegment( fromNode.stop.getStopFacility(),
                        toNode.stop.getStopFacility(),
                        travelTime,
                        transitLineId,
                        routeId
                ));
            } else if (downstreamLink.line.getId() == link.line.getId() && downstreamLink.route.getId() == link.route.getId() ){
                //same route --> update the top routeSegment
                RouteSegment routeSegment = routeSegments.remove(0);
                routeSegments.add(0, new RouteSegment(fromNode.stop.getStopFacility(),
                        routeSegment.toStop,
                        routeSegment.travelTime+travelTime,
                        transitLineId,
                        routeId));
            }

            if (isTransferLeg) {
                // transfer cost
                if ( ! (this.costFunction instanceof  TransitRouterNetworkTravelTimeAndDisutility) ) {
                    throw new RuntimeException("TransitTravelDisutility is not instance of "+TransitRouterNetworkTravelTimeAndDisutility.class.getSimpleName()
                    +". An acc ");
                }

                transferCost += ((TransitRouterNetworkTravelTimeAndDisutility) this.costFunction).defaultTransferCost(link,
                        Double.NEGATIVE_INFINITY,null,null);

                downstreamLink = null;
            } else {
                downstreamLink = link;
            }

            previousFromNode = fromNode;
            link = (TransitRouterNetworkLink) getData(fromNode).getPrevLink();
        }

        DijkstraNodeData startNodeData = getData(previousFromNode);
        DijkstraNodeData toNodeData = getData(minCostNode);

        double cost = toNodeData.getCost() - startNodeData.getCost()
                + this.fromNodes.get(previousFromNode).initialCost
                + toNodes.get(minCostNode).initialCost
                + transferCost;

        // if there is no connection found, getPath(...) return a path with nothing in it, however, I (and AN) think that it should throw null. Amit Sep'17
        if (routeSegments.size()==0) return null;
        else return new InternalTransitPassengerRoute(cost, routeSegments);
    }

    public Path getPath(final Map<Node, InitialNode> toNodes) {

        //find the best node
        double minCost = Double.POSITIVE_INFINITY;
        Node minCostNode = null;
        for (Map.Entry<Node, InitialNode> e : toNodes.entrySet()) {
            Node currentNode = e.getKey();
            DijkstraNodeData r = this.nodeData.get(currentNode.getId());
            if (r == null) {
                expandNodeData(toNodes);
            }
            DijkstraNodeData data = getData(currentNode);
            InitialNode initData = e.getValue();
            double cost = data.getCost() + initData.initialCost;
            if (data.getCost() != 0.0 || fromNodes.containsKey(currentNode)) {
                if (cost < minCost) {
                    minCost = cost;
                    minCostNode = currentNode;
                }
            }
        }

        if (minCostNode == null) {
            return null;
        }

        // now construct the path
        List<Node> nodes = new LinkedList<>();
        List<Link> links = new LinkedList<>();

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

        DijkstraNodeData startNodeData = getData(nodes.get(0));
        DijkstraNodeData toNodeData = getData(minCostNode);

        return new Path(nodes, links, toNodeData.getTime() - startNodeData.getTime(),
                toNodeData.getCost() - startNodeData.getCost());
    }

    /*package*/ RouterPriorityQueue<? extends Node> createRouterPriorityQueue() {
        return new PseudoRemovePriorityQueue<>(500);
    }

    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 relaxNode(final Node outNode, final RouterPriorityQueue<Node> pendingNodes) {

        DijkstraNodeData outData = getData(outNode);
        double currTime = outData.getTime();
        double currCost = outData.getCost();
        for (Link l : outNode.getOutLinks().values()) {
            relaxNodeLogic(l, pendingNodes, currTime, currCost);
        }
    }

    /*package*/ void relaxNodeLogic(final Link l, final RouterPriorityQueue<Node> pendingNodes,
            final double currTime, final double currCost) {
        addToPendingNodes(l, l.getToNode(), pendingNodes, currTime, currCost);
    }

    protected boolean addToPendingNodes(final Link l, final Node n,
            final RouterPriorityQueue<Node> pendingNodes, final double currTime,
            final double currCost) {

        this.customDataManager.initForLink(l);
        double travelTime = this.timeFunction.getLinkTravelTime(l, currTime, this.person, this.vehicle);
        double travelCost = this.costFunction.getLinkTravelDisutility(l, currTime, this.person, this.vehicle, this.customDataManager);
        DijkstraNodeData data = getData(n);
        double nCost = data.getCost();
        if (!data.isVisited(getIterationId())) {
            visitNode(n, data, pendingNodes, currTime + travelTime, currCost + travelCost, l);
            this.customDataManager.storeTmpData();
            return true;
        }
        double totalCost = currCost + travelCost;
        if (totalCost < nCost) {
            revisitNode(n, data, pendingNodes, currTime + travelTime, totalCost, l);
            this.customDataManager.storeTmpData();
            return true;
        }

        return false;
    }

    void revisitNode(final Node n, final DijkstraNodeData data,
            final RouterPriorityQueue<Node> pendingNodes, final double time, final double cost,
            final Link outLink) {
        pendingNodes.remove(n);

        data.visit(outLink, cost, time, getIterationId());
        pendingNodes.add(n, getPriority(data));
    }

    private 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 = new DijkstraNodeData();
            this.nodeData.put(n.getId(), r);
        }
        return r;
    }

}