AStarEuclidean.java

Go to the documentation of this file.

/* *********************************************************************** *
 * project: org.matsim.*
 * AStarEuclidean.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 org.apache.logging.log4j.LogManager;
import org.apache.logging.log4j.Logger;
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.core.router.util.*;
import org.matsim.core.utils.collections.RouterPriorityQueue;
import org.matsim.core.utils.geometry.CoordUtils;

public class AStarEuclidean extends Dijkstra {
    private static final Logger log = LogManager.getLogger( AStarEuclidean.class ) ;

    protected final double overdoFactor;

    private double minTravelCostPerLength;

    AStarEuclidean(final Network network, final PreProcessEuclidean preProcessData,
            final TravelTime timeFunction) {
        this(network, preProcessData, timeFunction, 1);
    }

    AStarEuclidean(final Network network, final PreProcessEuclidean preProcessData,
            final TravelTime timeFunction, final double overdoFactor) {
        this(network, preProcessData, preProcessData.getCostFunction(),
                timeFunction, overdoFactor);
    }

    AStarEuclidean(final Network network,
            final PreProcessEuclidean preProcessData,
            final TravelDisutility costFunction, final TravelTime timeFunction, final double overdoFactor) {
        super(network, costFunction, timeFunction, preProcessData);

        setMinTravelCostPerLength(preProcessData.getMinTravelCostPerLength());

        this.overdoFactor = overdoFactor;
    }

    @Override
    protected void initFromNode(final Node fromNode, final Node toNode, final double startTime, final RouterPriorityQueue<Node> pendingNodes) {
        AStarNodeData data = getData(fromNode);
        visitNode(fromNode, data, pendingNodes, startTime, 0, null);
        data.setExpectedRemainingCost(estimateRemainingTravelCost(fromNode, toNode));
    }

    @Override
    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 AStarNodeData data = getData(n);
        if (!data.isVisited(getIterationId())) {
            double remainingTravelCost = estimateRemainingTravelCost(n, toNode);
            visitNode(n, data, pendingNodes, currTime + travelTime, currCost + travelCost, remainingTravelCost, 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.
            
            if ( totalCost==0. ) {
                log.warn( "finding totalCost=" + totalCost + "; this will often (or always?) lead to a null " +
                                  "pointer exception later.  In my own case, it was related to a network " +
                                  "having freespeed infinity at places.  linkId=" + l.getId()+ ". kai, jan'18") ;
            }
            
            if (data.getPrevLink().getId().compareTo(l.getId()) > 0) {
                revisitNode(n, data, pendingNodes, currTime + travelTime, totalCost, l);
                return true;
            }
        }
        return false;
    }

    private void visitNode(final Node n, final AStarNodeData data, final RouterPriorityQueue<Node> pendingNodes, 
            final double time, final double cost, final double expectedRemainingCost, final Link outLink) {
        data.setExpectedRemainingCost(expectedRemainingCost);
        super.visitNode(n, data, pendingNodes, time, cost, outLink);
    }
    
    public double getOverdoFactor() {
        return this.overdoFactor;
    }

    protected double estimateRemainingTravelCost(final Node fromNode, final Node toNode) {
        double dist = CoordUtils.calcEuclideanDistance(fromNode.getCoord(), toNode.getCoord()) * getMinTravelCostPerLength();
        return dist * this.overdoFactor;
    }

    @Override
    protected AStarNodeData getData(final Node n) {
        return (AStarNodeData) super.getData(n);
    }

    @Override
    protected AStarNodeData createNodeData() {
        return new AStarNodeData();
    }
    
    /*package*/ void setMinTravelCostPerLength(final double minTravelCostPerLength) {
        this.minTravelCostPerLength = minTravelCostPerLength;
    }

    public final double getMinTravelCostPerLength() {
        return this.minTravelCostPerLength;
    }

    @Override
    protected double getPriority(final DijkstraNodeData data) {
        return ((AStarNodeData) data).getExpectedCost();
    }
}