1
\$\begingroup\$

This post elaborates on NBA*: Very efficient bidirectional heuristic search algorithm in Java.

I have made the following changes:

  1. Added an explicit type for representing digraph paths: DirectedGraphPath.
  2. If the target node is unreachable from the source node, a TargetUnreachableException is thrown instead of returning a sentinel value representing a nonexistent path.
  3. HeapEntry removed from AbstractPathfinder and moved into the package net.coderodde.graph.pathfinding.support, where it is declared as package-private.
  4. ZeroHeuristicFunction is removed and a lambda is used instead.
  5. HeuristicFunction is annotated as @FunctionalInterface.
  6. The source and target nodes in the demonstration are chosen to be as far from each other as feasible.
  7. Minor improvements in the Demo.
  8. An optimality related bug fixed.

NBAStarPathfinder.java

package net.coderodde.graph.pathfinding.support;

import java.util.Arrays;
import java.util.HashMap;
import java.util.HashSet;
import java.util.Map;
import java.util.Objects;
import java.util.PriorityQueue;
import java.util.Set;
import net.coderodde.graph.DirectedGraph;
import net.coderodde.graph.DirectedGraphWeightFunction;
import net.coderodde.graph.pathfinding.AbstractPathfinder;
import net.coderodde.graph.pathfinding.DirectedGraphPath;
import net.coderodde.graph.pathfinding.HeuristicFunction;
import net.coderodde.graph.pathfinding.TargetUnreachableException;

/**
 * This pathfinding algorithm is due to Wim Pijls and Henk Post in "Yet another
 * bidirectional algorithm for shortest paths." 15 June 2009.
 * <p>
 * <b>This class is not thread-safe.</b> If you need it in different threads,
 * make sure each thread has its own object of this class.
 *
 * @author Rodion "rodde" Efremov
 * @version 1.61 (Oct 13, 2016)
 */
public final class NBAStarPathfinder extends AbstractPathfinder {

    private final HeuristicFunction heuristicFunction;
    private final PriorityQueue<HeapEntry> OPENA = new PriorityQueue<>();
    private final PriorityQueue<HeapEntry> OPENB = new PriorityQueue<>();
    private final Map<Integer, Integer> PARENTSA = new HashMap<>();
    private final Map<Integer, Integer> PARENTSB = new HashMap<>();
    private final Map<Integer, Double> DISTANCEA = new HashMap<>();
    private final Map<Integer, Double> DISTANCEB = new HashMap<>();
    private final Set<Integer> CLOSED = new HashSet<>();

    private double fA;
    private double fB;
    private double bestPathLength;
    private Integer touchNode;
    private Integer sourceNode;
    private Integer targetNode;

    public NBAStarPathfinder(DirectedGraph graph,
            DirectedGraphWeightFunction weightFunction,
            HeuristicFunction heuristicFunction) {
        super(graph, weightFunction);
        this.heuristicFunction
                = Objects.requireNonNull(heuristicFunction,
                        "The input heuristic function is null.");
    }

    @Override
    public DirectedGraphPath search(int sourceNode, int targetNode) {
        if (sourceNode == targetNode) {
            return new DirectedGraphPath(Arrays.asList(sourceNode));
        }

        init(sourceNode, targetNode);

        while (!OPENA.isEmpty() && !OPENB.isEmpty()) {
            if (OPENA.size() < OPENB.size()) {
                expandInForwardDirection();
            } else {
                expandInBackwardDirection();
            }
        }

        if (touchNode == null) {
            throw new TargetUnreachableException(graph, 
                                                 sourceNode,
                                                 targetNode);
        }

        return tracebackPath(touchNode, PARENTSA, PARENTSB);
    }

    private void expandInForwardDirection() {
        Integer currentNode = OPENA.remove().getNode();

        if (CLOSED.contains(currentNode)) {
            return;
        }

        CLOSED.add(currentNode);

        if (DISTANCEA.get(currentNode) +
                heuristicFunction.estimateDistanceBetween(currentNode,
                                                          targetNode)
                >= bestPathLength
                ||
                DISTANCEA.get(currentNode) +
                fB - 
                heuristicFunction.estimateDistanceBetween(currentNode,
                                                          sourceNode)
                >= bestPathLength) {
            // Reject the 'currentNode'.
        } else {
            // Stabilize the 'currentNode'.
            for (Integer childNode : graph.getChildrenOf(currentNode)) {
                if (CLOSED.contains(childNode)) {
                    continue;
                }

                double tentativeDistance
                        = DISTANCEA.get(currentNode)
                        + weightFunction.get(currentNode, childNode);

                if (!DISTANCEA.containsKey(childNode)
                        || 
                        DISTANCEA.get(childNode) > tentativeDistance) {
                    DISTANCEA.put(childNode, tentativeDistance);
                    PARENTSA.put(childNode, currentNode);
                    HeapEntry e
                            = new HeapEntry(
                                    childNode,
                                    tentativeDistance
                                    + heuristicFunction
                                    .estimateDistanceBetween(childNode,
                                                             targetNode));
                    OPENA.add(e);

                    if (DISTANCEB.containsKey(childNode)) {
                        double pathLength = tentativeDistance
                                + DISTANCEB.get(childNode);

                        if (bestPathLength > pathLength) {
                            bestPathLength = pathLength;
                            touchNode = childNode;
                        }
                    }
                }
            }
        }

        if (!OPENA.isEmpty()) {
            fA = OPENA.peek().getDistance();
        }
    }

    private void expandInBackwardDirection() {
        Integer currentNode = OPENB.remove().getNode();

        if (CLOSED.contains(currentNode)) {
            return;
        }

        CLOSED.add(currentNode);

        if (DISTANCEB.get(currentNode) +
                heuristicFunction.estimateDistanceBetween(currentNode,
                                                          sourceNode)
                >= bestPathLength
                || 
                DISTANCEB.get(currentNode) +
                fA -
                heuristicFunction.estimateDistanceBetween(currentNode, 
                                                          targetNode)
                >= bestPathLength) {
            // Reject the node 'currentNode'.
        } else {
            for (Integer parentNode : graph.getParentsOf(currentNode)) {
                if (CLOSED.contains(parentNode)) {
                    continue;
                }

                double tentativeDistance
                        = DISTANCEB.get(currentNode)
                        + weightFunction.get(parentNode, currentNode);

                if (!DISTANCEB.containsKey(parentNode)
                        ||
                        DISTANCEB.get(parentNode) > tentativeDistance) {
                    DISTANCEB.put(parentNode, tentativeDistance);
                    PARENTSB.put(parentNode, currentNode);
                    HeapEntry e
                            = new HeapEntry(parentNode,
                                    tentativeDistance
                                    + heuristicFunction
                                    .estimateDistanceBetween(parentNode,
                                                             sourceNode));
                    OPENB.add(e);

                    if (DISTANCEA.containsKey(parentNode)) {
                        double pathLength = tentativeDistance
                                + DISTANCEA.get(parentNode);

                        if (bestPathLength > pathLength) {
                            bestPathLength = pathLength;
                            touchNode = parentNode;
                        }
                    }
                }
            }
        }

        if (!OPENB.isEmpty()) {
            fB = OPENB.peek().getDistance();
        }
    }

    private void init(Integer sourceNode, Integer targetNode) {
        OPENA.clear();
        OPENB.clear();
        PARENTSA.clear();
        PARENTSB.clear();
        DISTANCEA.clear();
        DISTANCEB.clear();
        CLOSED.clear();

        double totalDistance
                = heuristicFunction.estimateDistanceBetween(sourceNode,
                                                            targetNode);

        fA = totalDistance;
        fB = totalDistance;
        bestPathLength = Double.MAX_VALUE;
        touchNode = null;
        this.sourceNode = sourceNode;
        this.targetNode = targetNode;

        OPENA.add(new HeapEntry(sourceNode, fA));
        OPENB.add(new HeapEntry(targetNode, fB));
        PARENTSA.put(sourceNode, null);
        PARENTSB.put(targetNode, null);
        DISTANCEA.put(sourceNode, 0.0);
        DISTANCEB.put(targetNode, 0.0);
    }
}

AStarPathfinder.java

package net.coderodde.graph.pathfinding.support;

import java.util.HashMap;
import java.util.HashSet;
import java.util.Map;
import java.util.Objects;
import java.util.PriorityQueue;
import java.util.Set;
import net.coderodde.graph.DirectedGraph;
import net.coderodde.graph.DirectedGraphWeightFunction;
import net.coderodde.graph.pathfinding.AbstractPathfinder;
import net.coderodde.graph.pathfinding.DirectedGraphPath;
import net.coderodde.graph.pathfinding.HeuristicFunction;
import net.coderodde.graph.pathfinding.TargetUnreachableException;

public final class AStarPathfinder extends AbstractPathfinder {

    private final HeuristicFunction heuristicFunction;
    private final PriorityQueue<HeapEntry> OPEN = new PriorityQueue<>();
    private final Set<Integer> CLOSED = new HashSet<>();
    private final Map<Integer, Double> DISTANCE = new HashMap<>();
    private final Map<Integer, Integer> PARENTS = new HashMap<>();

    public AStarPathfinder(DirectedGraph graph,
                           DirectedGraphWeightFunction weightFunction,
                           HeuristicFunction heuristicFunction) {
        super(graph, weightFunction);
        this.heuristicFunction = 
                Objects.requireNonNull(heuristicFunction,
                                       "The input heuristic function is null.");
    }

    @Override
    public DirectedGraphPath search(int sourceNodeId, int targetNodeId) {
        init(sourceNodeId);

        while (!OPEN.isEmpty()) {
            Integer currentNodeId = OPEN.remove().getNode();

            if (currentNodeId.equals(targetNodeId)) {
                return tracebackPath(currentNodeId, PARENTS);
            }

            if (CLOSED.contains(currentNodeId)) {
                continue;
            }

            CLOSED.add(currentNodeId);

            for (Integer childNodeId : graph.getChildrenOf(currentNodeId)) {
                if (CLOSED.contains(childNodeId)) {
                    continue;
                }

                double tentativeDistance = 
                        DISTANCE.get(currentNodeId) +
                        weightFunction.get(currentNodeId, childNodeId);

                if (!DISTANCE.containsKey(childNodeId)
                        || DISTANCE.get(childNodeId) > tentativeDistance) {
                    DISTANCE.put(childNodeId, tentativeDistance);
                    PARENTS.put(childNodeId, currentNodeId);
                    OPEN.add(
                        new HeapEntry(
                            childNodeId, 
                            tentativeDistance +
                            heuristicFunction
                                    .estimateDistanceBetween(childNodeId, 
                                                             targetNodeId)));
                }
            }
        }

        throw new TargetUnreachableException(graph, sourceNodeId, targetNodeId);
    }

    private void init(int sourceNodeId) {
        OPEN.clear();
        CLOSED.clear();
        PARENTS.clear();
        DISTANCE.clear();

        OPEN.add(new HeapEntry(sourceNodeId, 0.0));
        PARENTS.put(sourceNodeId, null);
        DISTANCE.put(sourceNodeId, 0.0);
    }
}

DijkstraPathfinder.java

package net.coderodde.graph.pathfinding.support;

import net.coderodde.graph.DirectedGraph;
import net.coderodde.graph.DirectedGraphWeightFunction;
import net.coderodde.graph.pathfinding.AbstractPathfinder;
import net.coderodde.graph.pathfinding.DirectedGraphPath;

public final class DijkstraPathfinder extends AbstractPathfinder {

    private final AStarPathfinder finderImplementation;

    public DijkstraPathfinder(DirectedGraph graph,
                              DirectedGraphWeightFunction weightFunction) {
        this.finderImplementation = 
                new AStarPathfinder(graph, 
                                    weightFunction,
                                    (a, b) -> { return 0.0; });
    }

    @Override
    public DirectedGraphPath search(int sourceNodeId, int targetNodeId) {
        return finderImplementation.search(sourceNodeId, targetNodeId);
    }
}

HeapEntry.java

package net.coderodde.graph.pathfinding.support;

/**
 * This class implements an entry for {@link java.util.PriorityQueue}.
 *
 * @author Rodion "rodde" Efremov
 * @version 1.6 (Oct 13, 2016)
 */
final class HeapEntry implements Comparable<HeapEntry> {

    private final int nodeId;
    private final double distance; // The priority key.

    public HeapEntry(int nodeId, double distance) {
        this.nodeId = nodeId;
        this.distance = distance;
    }

    public int getNode() {
        return nodeId;
    }

    public double getDistance() {
        return distance;
    }

    @Override
    public int compareTo(HeapEntry o) {
        return Double.compare(distance, o.distance);
    }
}

EuclideanHeuristicFunction.java

package net.coderodde.graph.pathfinding.support;

import java.util.Objects;
import net.coderodde.graph.pathfinding.DirectedGraphNodeCoordinates;
import net.coderodde.graph.pathfinding.HeuristicFunction;

/**
 * This class implements a heuristic function that returns the Euclidean
 * distance between two given nodes.
 * 
 * @author Rodion "rodde" Efremov
 * @version 1.6 (Oct 6, 2016)
 */
public class EuclideanHeuristicFunction implements HeuristicFunction {

    private final DirectedGraphNodeCoordinates coordinates;

    public EuclideanHeuristicFunction(DirectedGraphNodeCoordinates coordinates) {
        this.coordinates =
                Objects.requireNonNull(coordinates,
                                       "The input coordinate map is null.");
    }

    /**
     * {@inheritDoc }
     */
    @Override
    public double estimateDistanceBetween(int nodeId1, int nodeId2) {
        return coordinates.get(nodeId1).distance(coordinates.get(nodeId2));
    }
}

HeuristicFunction.java

package net.coderodde.graph.pathfinding;

/**
 * This interface defines the API for heuristic functions used in pathfinding.
 * 
 * @author Rodion "rodde" Efremov
 * @version 1.6 (Oct 6, 2016)
 */
@FunctionalInterface
public interface HeuristicFunction {

    /**
     * Provides an optimistic (underestimated) distance between {@code nodeId1}
     * and {@code nodeId2} using a specific distance metric.
     * 
     * @param nodeId1 the first node.
     * @param nodeId2 the second node.
     * @return a shortest path estimate between the two input nodes.
     */
    public double estimateDistanceBetween(int nodeId1, int nodeId2);
}

TargetUnreachableException.java

package net.coderodde.graph.pathfinding;

import net.coderodde.graph.DirectedGraph;

public class TargetUnreachableException extends RuntimeException {

    private final DirectedGraph graph;
    private final Integer sourceNode;
    private final Integer targetNode;

    public TargetUnreachableException(DirectedGraph graph,
                                      Integer sourceNode,
                                      Integer targetNode) {
        this.graph = graph;
        this.sourceNode = sourceNode;
        this.targetNode = targetNode;
    }

    public DirectedGraph getGraph() {
        return graph;
    }

    public int getSourceNode() {
        return sourceNode;
    }

    public int getTargetNode() {
        return targetNode;
    }

    @Override
    public String toString() {
        return "'" + targetNode + "' is not reachable from '" 
                   + sourceNode + "'.";
    }
}

DirectedGraphPath.java

package net.coderodde.graph.pathfinding;

import java.util.ArrayList;
import java.util.List;
import net.coderodde.graph.DirectedGraphWeightFunction;

/**
 * This class implements a type for representing paths in directed graphs.
 * 
 * @author Rodion "rodde" Efremov
 * @version 1.6 (Oct 16, 2016)
 */
public final class DirectedGraphPath {

    /**
     * The actual list of nodes on a path.
     */
    private final List<Integer> path;

    public DirectedGraphPath(List<Integer> path) {
        checkNotEmpty(path);
        this.path = new ArrayList<>(path);
    }

    public int getNode(int index) {
        return path.get(index);
    }

    public int getNumberOfNodes() {
        return path.size();
    }

    public int getNumberOfEdges() {
        return path.size() - 1;
    }

    public double getCost(DirectedGraphWeightFunction weightFunction) {
        double cost = 0.0;

        for (int i = 0; i < path.size() - 1; ++i) {
            cost += weightFunction.get(path.get(i), path.get(i + 1));
        }

        return cost;
    }

    @Override
    public boolean equals(Object o) {
        if (this == o) {
            return true;
        }

        if (o == null || !o.getClass().equals(getClass())) {
            return false;
        }

        return path.equals(((DirectedGraphPath) o).path);
    }

    @Override
    public String toString() {
        StringBuilder sb = new StringBuilder("[");
        String separator = "";

        for (Integer node : path) {
            sb.append(separator).append(node);
            separator = ", ";
        }

        return sb.append(']').toString();
    }

    private void checkNotEmpty(List<Integer> path) {
        if (path.isEmpty()) {
            throw new IllegalArgumentException(
                    "The input path is not allowed to be empty.");
        }
    }
}

DirectedGraphNodeCoordinates.java

package net.coderodde.graph.pathfinding;

import java.awt.geom.Point2D;
import java.util.HashMap;
import java.util.Map;

/**
 * This class allows mapping each graph node to its coordinates on a 
 * two-dimensional plane.
 * 
 * @author Rodion "rodde" Efremov
 * @version 1.6 (Oct 6, 2016)
 */
public class DirectedGraphNodeCoordinates {

    /**
     * Maps each node to its coordinates.
     */
    private final Map<Integer, Point2D.Double> map = new HashMap<>();

    /**
     * Associates the coordinates {@code point} to the node {@code nodeId}.
     * 
     * @param nodeId the node to map.
     * @param point  the coordinates to associate to the node.
     */
    public void put(int nodeId, Point2D.Double point) {
        map.put(nodeId, point);
    }

    /**
     * Return the point of the input node.
     * 
     * @param nodeId the node whose coordinates to return.
     * @return the coordinates.
     */
    public Point2D.Double get(int nodeId) {
        return map.get(nodeId);
    }
}

AbstractPathfinder.java

package net.coderodde.graph.pathfinding;

import java.util.ArrayList;
import java.util.Collections;
import java.util.List;
import java.util.Map;
import java.util.Objects;
import net.coderodde.graph.DirectedGraph;
import net.coderodde.graph.DirectedGraphWeightFunction;

/**
 * This abstract class defines some facilities shared by pathfinding algorithms
 * and API for using them.
 * 
 * @author Rodion "rodde" Efremov
 * @version 1.6 (Oct 6, 2016)
 */
public abstract class AbstractPathfinder {

    /**
     * The graph to search in.
     */
    protected final DirectedGraph graph;

    /**
     * The weight function to use.
     */
    protected final DirectedGraphWeightFunction weightFunction;

    protected AbstractPathfinder(DirectedGraph graph,
                                 DirectedGraphWeightFunction weightFunction) {
        this.graph = Objects.requireNonNull(graph, "The input graph is null.");
        this.weightFunction =
                Objects.requireNonNull(weightFunction,
                                       "The input weight function is null.");
    }

    protected AbstractPathfinder() {
        this.graph = null;
        this.weightFunction = null; // Compiler requires this initialization.
    }

    /**
     * Searches and returns a shortest path starting from the node 
     * {@code sourceNodeId} and leading to {@code targetNodeId}.
     * 
     * @param sourceNodeId the source node.
     * @param targetNodeId the target node.
     * @return a shortest path of nodes from source node to target node
     *         including the terminal nodes.
     */
    public abstract DirectedGraphPath search(int sourceNodeId,
                                             int targetNodeId);

    /**
     * Reconstructs a shortest path from the data structures maintained by a 
     * <b>bidirectional</b> pathfinding algorithm.
     * 
     * @param touchNodeId the node where the two search frontiers agree.
     * @param PARENTSA the parent map in the forward search direction.
     * @param PARENTSB the parent map in the backward search direction.
     * @return the shortest path object.
     */
    protected DirectedGraphPath tracebackPath(int touchNodeId, 
                                              Map<Integer, Integer> PARENTSA,
                                              Map<Integer, Integer> PARENTSB) {
        List<Integer> path = new ArrayList<>();
        Integer currentNodeId = touchNodeId;

        while (currentNodeId != null) {
            path.add(currentNodeId);
            currentNodeId = PARENTSA.get(currentNodeId);
        }

        Collections.<Integer>reverse(path);

        if (PARENTSB != null) {
            currentNodeId = PARENTSB.get(touchNodeId);

            while (currentNodeId != null) {
                path.add(currentNodeId);
                currentNodeId = PARENTSB.get(currentNodeId);
            }
        }

        return new DirectedGraphPath(path);
    }

    /**
     * Reconstructs a shortest path from the data structures maintained by a
     * unidirectional pathfinding algorithm.
     * 
     * @param targetNodeId the target node.
     * @param PARENTS      the parents map.
     * @return the shortest path object
     */
    protected DirectedGraphPath tracebackPath(int targetNodeId, 
                                              Map<Integer, Integer> PARENTS) {
        return tracebackPath(targetNodeId, PARENTS, null);
    }
}

DirectedGraph.java

package net.coderodde.graph;

import java.util.Collections;
import java.util.HashMap;
import java.util.HashSet;
import java.util.Map;
import java.util.Set;

/**
 * This class implements a directed graph data structure via adjacency lists. 
 * This implementation represents each graph node as an unique integer.
 * 
 * @author Rodion "rodde" Efremov
 * @version 1.61 (Oct 13, 2016)
 */
public class DirectedGraph {

    /**
     * This map maps each directed graph node to the list of its child nodes.
     */
    private final Map<Integer, Set<Integer>> childMap  = new HashMap<>();

    /**
     * This map maps each directed graph node to the list of its parent nodes.
     */
    private final Map<Integer, Set<Integer>> parentMap = new HashMap<>();

    /**
     * Adds a new node represented by integer {@code nodeId} to this graph if
     * it is not yet present in it.
     * 
     * @param nodeId the node to add.
     */
    public void addNode(int nodeId) {
        childMap .putIfAbsent(nodeId, new HashSet<>());
        parentMap.putIfAbsent(nodeId, new HashSet<>());
    }

    /**
     * Creates a directed arc <tt>(tailNodeId, headNodeId)</tt> if it is not yet
     * present in the graph.
     * 
     * @param tailNodeId the tail node of the arc.
     * @param headNodeId the head node of the arc.
     */
    public void addArc(int tailNodeId, int headNodeId) {
        childMap .get(tailNodeId).add(headNodeId);
        parentMap.get(headNodeId).add(tailNodeId);
    }

    /**
     * Returns the view of all the nodes in this graph.
     * 
     * @return the set of all nodes.
     */
    public Set<Integer> getNodeSet() {
        return Collections.unmodifiableSet(childMap.keySet());
    }

    /**
     * Returns the set of all child nodes of the given node {@code nodeId}.
     * 
     * @param nodeId the node whose children to return.
     * @return the set of child nodes of {@code nodeId}.
     */
    public Set<Integer> getChildrenOf(int nodeId) {
        return Collections.<Integer>unmodifiableSet(childMap.get(nodeId));
    }

    /**
     * Returns the set of all parent nodes of the given node {@code nodeId}.
     * 
     * @param nodeId the node whose parents to return.
     * @return the set of parent nodes of {@code nodeId}.
     */
    public Set<Integer> getParentsOf(int nodeId) {
        return Collections.<Integer>unmodifiableSet(parentMap.get(nodeId));
    }
}

DirectedGraphWeightFunction.java

package net.coderodde.graph;

import java.util.HashMap;
import java.util.Map;

/**
 * This class maps directed arcs to their weights. An arc weight is not allowed
 * to be a <tt>NaN</tt> value or negative.
 * 
 * @author Rodion "rodde" Efremov
 * @vesion 1.6 (Oct 6, 2016)
 */
public class DirectedGraphWeightFunction {

    /**
     * Maps the arcs to the arc weights.
     */
    private final Map<Integer, Map<Integer, Double>> map = new HashMap<>();

    /**
     * Associates the weight {@code weight} with the arc 
     * <tt>(tailNodeId, headNodeId)</tt>.
     * 
     * @param tailNodeId the starting node of the arc.
     * @param headNodeId the ending node of the arc.
     * @param weight the arc weight.
     */
    public void put(int tailNodeId, int headNodeId, double weight) {
        checkWeight(weight);
        map.putIfAbsent(tailNodeId, new HashMap<>());
        map.get(tailNodeId).put(headNodeId, weight);
    }

    /**
     * Returns the weight of the given arc.
     * 
     * @param tailNodeId the starting node (tail node) of the arc.
     * @param headNodeId the ending node (head node) of the arc.
     * @return 
     */
    public double get(int tailNodeId, int headNodeId) {
        return map.get(tailNodeId).get(headNodeId);
    }

    private void checkWeight(double weight) {
        if (Double.isNaN(weight)) {
            throw new IllegalArgumentException("The input weight is NaN.");
        }

        if (weight < 0.0) {
            throw new IllegalArgumentException(
                    "The input weight is negative: " + weight + ".");
        }
    }
}

Demo.java

import java.awt.geom.Point2D;
import java.util.ArrayList;
import java.util.List;
import java.util.Random;
import net.coderodde.graph.DirectedGraph;
import net.coderodde.graph.DirectedGraphWeightFunction;
import net.coderodde.graph.pathfinding.AbstractPathfinder;
import net.coderodde.graph.pathfinding.DirectedGraphNodeCoordinates;
import net.coderodde.graph.pathfinding.DirectedGraphPath;
import net.coderodde.graph.pathfinding.HeuristicFunction;
import net.coderodde.graph.pathfinding.support.AStarPathfinder;
import net.coderodde.graph.pathfinding.support.DijkstraPathfinder;
import net.coderodde.graph.pathfinding.support.EuclideanHeuristicFunction;
import net.coderodde.graph.pathfinding.support.NBAStarPathfinder;

/**
 * This class contains a demonstration program comparing performance of three
 * point-to-point shortest path algorithms:
 * <ol>
 *  <li>A*,</li>
 *  <li>Dijkstra's algorithm</li>
 *  <li>NBA*, New Bidirectional A*.</li>
 * </ol>
 * 
 * @author Rodion "rodde" Efremov
 * @version 1.61 (Oct 16, 2016)
 */
public class Demo {

    private static final int NODES = 100_000;
    private static final int ARCS =  500_000;
    private static final double PLANE_WIDTH = 1000.0;
    private static final double PLANE_HEIGHT = 1000.0;

    public static void main(String[] args) {
        long seed = System.nanoTime();
        Random random = new Random(seed);
        System.out.println("Seed = " + seed);

        long start = System.currentTimeMillis();
        DirectedGraph graph = getRandomGraph(NODES, ARCS, random);
        DirectedGraphNodeCoordinates coordinates = getCoordinates(graph,
                                                                  PLANE_WIDTH,
                                                                  PLANE_HEIGHT,
                                                                  random);
        DirectedGraphWeightFunction weightFunction = 
                getWeightFunction(graph, coordinates);

        Integer sourceNodeId = getSource(graph, coordinates);
        Integer targetNodeId = getTarget(graph, coordinates);

        long end = System.currentTimeMillis();

        System.out.println("Created the graph data structures in " +
                           (end - start) + " milliseconds.");

        System.out.println("Source: " + sourceNodeId);
        System.out.println("Target: " + targetNodeId);
        System.out.println();

        HeuristicFunction hf = new EuclideanHeuristicFunction(coordinates);

        AbstractPathfinder finder1 = new AStarPathfinder(graph,
                                                         weightFunction,
                                                         hf);

        AbstractPathfinder finder2 = new DijkstraPathfinder(graph,
                                                            weightFunction);

        AbstractPathfinder finder3 = new NBAStarPathfinder(graph, 
                                                           weightFunction,
                                                           hf);
        DirectedGraphPath path1 = benchmark(finder1,
                                            sourceNodeId, 
                                            targetNodeId);

        DirectedGraphPath path2 = benchmark(finder2, 
                                            sourceNodeId,
                                            targetNodeId);

        DirectedGraphPath path3 = benchmark(finder3, 
                                            sourceNodeId, 
                                            targetNodeId);

        boolean agreed = path1.equals(path2) && path1.equals(path3);

        if (agreed) {
            System.out.println("Algorithms agree: true");
        } else {
            System.out.println("Algorithms DISAGREED!");
            System.out.println("A* path length:       " 
                    + path1.getCost(weightFunction));
            System.out.println("Dijkstra path length: " 
                    + path2.getCost(weightFunction));
            System.out.println("NBA* path length:     " 
                    + path3.getCost(weightFunction));
        }
    }

    private static DirectedGraphPath benchmark(AbstractPathfinder pathfinder,
                                               int sourceNode, 
                                               int targetNode) {
        long start = System.currentTimeMillis();
        DirectedGraphPath path = pathfinder.search(sourceNode, targetNode);
        long end = System.currentTimeMillis();

        System.out.println(pathfinder.getClass().getSimpleName() + 
                           " in " + (end - start) + " milliseconds.");
        System.out.println(path);
        System.out.println();
        return path;
    }

    private static DirectedGraph getRandomGraph(int nodes, 
                                                int arcs, 
                                                Random random) {
        DirectedGraph graph = new DirectedGraph();

        for (int id = 0; id < nodes; ++id) {
            graph.addNode(id);
        }

        List<Integer> graphNodeList = new ArrayList<>(graph.getNodeSet());

        while (arcs-- > 0) {
            Integer tailNodeId = choose(graphNodeList, random);
            Integer headNodeId = choose(graphNodeList, random);
            graph.addArc(tailNodeId, headNodeId);
        }

        return graph;
    }

    private static DirectedGraphNodeCoordinates 
        getCoordinates(DirectedGraph graph, 
                       double planeWidth,
                       double planeHeight,
                       Random random) {
        DirectedGraphNodeCoordinates coordinates =
                new DirectedGraphNodeCoordinates();

        for (Integer nodeId : graph.getNodeSet()) {
            coordinates.put(nodeId, 
                            randomPoint(planeWidth, planeHeight, random));
        }

        return coordinates;
    }

    private static DirectedGraphWeightFunction 
        getWeightFunction(DirectedGraph graph,
                          DirectedGraphNodeCoordinates coordinates) {
        DirectedGraphWeightFunction weightFunction = 
                new DirectedGraphWeightFunction();

        for (Integer nodeId : graph.getNodeSet()) {
            Point2D.Double p1 = coordinates.get(nodeId);

            for (Integer childNodeId : graph.getChildrenOf(nodeId)) {
                Point2D.Double p2 = coordinates.get(childNodeId);
                double distance = p1.distance(p2);
                weightFunction.put(nodeId, childNodeId, 1.2 * distance);
            }
        }

        return weightFunction;
    }

    private static Point2D.Double randomPoint(double width,
                                              double height,
                                              Random random) {
        return new Point2D.Double(width * random.nextDouble(),
                                  height * random.nextDouble());
    }

    private static <T> T choose(List<T> list, Random random) {
        return list.get(random.nextInt(list.size()));
    }

    private static Integer
         getClosestTo(DirectedGraph graph,
                      DirectedGraphNodeCoordinates coordinates,
                      Point2D.Double point) {
        double bestDistance = Double.POSITIVE_INFINITY;
        Integer bestNode = null;

        for (Integer node : graph.getNodeSet()) {
            Point2D.Double nodePoint = coordinates.get(node);

            if (bestDistance > nodePoint.distance(point)) {
                bestDistance = nodePoint.distance(point);
                bestNode = node;
            }
        }

        return bestNode;
    }

    private static Integer getSource(DirectedGraph graph,
                                     DirectedGraphNodeCoordinates coordinates) {
        return getClosestTo(graph, coordinates, new Point2D.Double());
    }

    private static Integer getTarget(DirectedGraph graph,
                                     DirectedGraphNodeCoordinates coordinates) {
        return getClosestTo(graph, 
                            coordinates,
                            new Point2D.Double(PLANE_WIDTH, PLANE_HEIGHT));
    }
}

Performance figures

A typical run of the demonstration might output something like this:

Seed = 380420829228515
Created the graph data structures in 6350 milliseconds.
Source: 58350
Target: 45998

AStarPathfinder in 996 milliseconds.
[58350, 69183, 24066, 12240, 79684, 33326, 53655, 74615, 97690, 28336, 45998]

DijkstraPathfinder in 5025 milliseconds.
[58350, 69183, 24066, 12240, 79684, 33326, 53655, 74615, 97690, 28336, 45998]

NBAStarPathfinder in 29 milliseconds.
[58350, 69183, 24066, 12240, 79684, 33326, 53655, 74615, 97690, 28336, 45998]

Algorithms agree: true

Critique request

I would like to hear anything you can tell me, especially:

  • API design
  • Modularity
  • Naming
  • Coding conventions
  • Efficiency
  • Javadoc
\$\endgroup\$
1
\$\begingroup\$

NBAStarPathfinder:

  • What is the difference between the As and Bs? Names should be more descriptive.
  • fB needs a new name. Unless it's referring to Facebook, which I doubt very much.
  • expandIn___Direction: A method should do one thing and do it well. Break this up. (You may even find some duplicate code in the process.)
  • // Stabilize the 'currentNode'. - rather than that being a comment, make it a method with such a name.

AbstractPathFinder:

  • Will this even work if the default constructor is called? If not, can we get rid of it?

DirectedGraphWeightFunction:

  • checkWeight is checking what? That it's valid? This should be renamed accordingly.

That's just at a quick glance for you.

\$\endgroup\$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.