spieleprogrammierer.de - Forum und Wiki zur Spieleprogrammierung und Spieleentwicklung

C#-Quelltext

/**
 * Implements A* Algotithm for pathfinding with manhattan metric
 */
public class AStar {

    /**
     * Finds the shortest path from startNode to targetNode
     * @param graph The graph the algorithm is applied on
     * @param startNode The start point
     * @param targetNode The target
     */
    public static Queue<GraphNode> shortestPath(Graph graph, GraphNode startNode, GraphNode targetNode) {
        PriorityQueue<OpenListEntry> openList = new PriorityQueue<>(graph.nodes.length, new OpenListEntryComparator());
        // For quickly accessing the f-values
        Map<GraphNode, Double> fMap = new HashMap<>();
        // Maps previous node to node
        Map<GraphNode, GraphNode> closedList = new HashMap<>();


        // Inserting the startNode
        openList.add(new OpenListEntry(startNode, 0, manhattanEstimateDistance(startNode, targetNode), null));
        fMap.put(startNode, manhattanEstimateDistance(startNode, targetNode));

        // Traversing all elements in openList
        for(int i = 0; i < graph.nodes.length; i++) {
            OpenListEntry olentry = openList.poll();

            for(GraphNode adjacent : olentry.node.getAdjacentNodes()) {
                OpenListEntry newEntry = new OpenListEntry(adjacent, olentry.g + manhattanEstimateDistance(adjacent, olentry.node), manhattanEstimateDistance(adjacent, targetNode), olentry.node);
                if(fMap.containsKey(adjacent)) {
                    if(newEntry.f() < fMap.get(adjacent)) {
                        // Removes object with same node
                        openList.remove(newEntry);
                    }
                }

                fMap.put(adjacent, newEntry.f());
                openList.add(newEntry);
            }

            // automatically removed from openList by calling poll()
            closedList.put(olentry.node, olentry.previous);


            // We surely have a path now
            if(olentry.node == targetNode) {
                break;
            }

        }

        LinkedList<GraphNode> path = new LinkedList<>();

        if(!closedList.containsKey(targetNode)) {
            return path;
        }

        GraphNode next = targetNode;
        while(next != null) {
            // The targetNode should be the last one then, so you start at the first one when polling from the queue
            path.addFirst(next);
            next = closedList.get(next);
        }

        if(path.getFirst() != startNode) {
            Log.w("A-STAR", "Unexpected behaviour in A-Star algorithm");
        }

        return path;
    }

    /**
     * Estimates the distance by manhattan distance. Also is the distance of 2 adjacent nodes.
     * @param node
     * @param target
     * @return
     */
    private static double manhattanEstimateDistance(GraphNode node, GraphNode target) {
        double xDiff = Math.abs(target.getX() - node.getX());
        double yDiff = Math.abs(target.getY() - node.getY());
        double zDiff = Math.abs(target.getZ() - node.getZ());

        return xDiff + yDiff + zDiff;
    }

    private static class OpenListEntry {
        public GraphNode node;
        // The distance that is estimated by the heuristic
        public double h;
        // The distance already gone from the startNode
        public double g;

        public GraphNode previous;

        /**
         *
         * @return The estimated total distance through that node
         */
        public double f() {
            return g+h;
        }

        public OpenListEntry(GraphNode node, double h, double g, GraphNode previous) {
            this.node = node;
            this.h = h;
            this.g = g;
        }


        @Override
        public boolean equals(Object o) {
            if(!(o instanceof OpenListEntry))
                return false;

            return node == ((OpenListEntry)o).node;
        }
    }


    private static class OpenListEntryComparator implements Comparator<OpenListEntry> {

        @Override
        public int compare(OpenListEntry lhs, OpenListEntry rhs) {
            double diff = (lhs.f() - rhs.f());

            // We can't directly return diff, because it is a double and maybe the decimals get lost by a direct cast
            return diff < 0 ? -1 : (diff > 0 ? 1 : 0);
        }
    }
}