Astar pathfinding in a grid

pom.xml

@@ -24,6 +24,20 @@
     </build>
     <modules>
         <module>clock</module>
+        <module>simple-pathfinding</module>
     </modules>
 
+    <dependencies>
+        <dependency>
+            <groupId>org.slf4j</groupId>
+            <artifactId>slf4j-api</artifactId>
+            <version>RELEASE</version>
+        </dependency>
+
+        <dependency>
+            <groupId>org.slf4j</groupId>
+            <artifactId>slf4j-simple</artifactId>
+            <version>RELEASE</version>
+        </dependency>
+    </dependencies>
 </project>

simple-pathfinding/pom.xml

@@ -0,0 +1,15 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
+         xmlns="http://maven.apache.org/POM/4.0.0"
+         xsi:schemaLocation="http://maven.apache.org/POM/4.0.0 http://maven.apache.org/xsd/maven-4.0.0.xsd">
+    <parent>
+        <artifactId>swingapps</artifactId>
+        <groupId>fr.uha.gasser</groupId>
+        <version>1.0-SNAPSHOT</version>
+    </parent>
+    <modelVersion>4.0.0</modelVersion>
+
+    <artifactId>simple-pathfinding</artifactId>
+
+
+</project>

simple-pathfinding/src/main/java/fr/uha/gasser/pathfinding/algorithm/AStarSearch.java

@@ -0,0 +1,91 @@
+package fr.uha.gasser.pathfinding.algorithm;
+
+import java.awt.*;
+import java.util.*;
+import java.util.List;
+
+class AStarSearch {
+
+    private final Node start;
+    private final Node goal;
+
+    AStarSearch(Node start, Node goal) {
+        this.start = start;
+        this.goal = goal;
+    }
+
+
+    private Node search() {
+        PriorityQueue<Node> frontier = new PriorityQueue<>();
+        Set<Node> explored = new HashSet<>();
+        frontier.add(start);
+
+        while (!frontier.isEmpty()) {
+            Node currentNode = frontier.poll();
+            explored.add(currentNode);
+
+            if (currentNode.getLoc().equals(goal.getLoc())) {
+                return currentNode;
+            }
+
+            for (Node neighbor : getNeighbors(currentNode)) {
+                // Ignore the neighbor which is already evaluated.
+                if (explored.contains(neighbor)) continue;
+                if (!frontier.contains(neighbor)) {
+                    neighbor.setParent(currentNode);
+                    neighbor.cost++;
+                    neighbor.hValue = neighbor.cost + neighbor.loc.distance(goal.loc);
+                    frontier.add(neighbor);
+                }
+            }
+        }
+
+        return null;
+    }
+
+    List<Node> shortestPath() {
+        Node currentNode = search();
+        LinkedList<Node> path = new LinkedList<>();
+        while (currentNode.getParent() != null) {
+            path.addFirst(currentNode);
+            currentNode = currentNode.getParent();
+        }
+
+        return path;
+    }
+
+    private List<Node> getNeighbors(Node current) {
+        LinkedList<Node> neighbors = new LinkedList<>();
+        for (Direction direction : Direction.values()) {
+            neighbors.add(getNeighbor(current, direction));
+        }
+
+        return neighbors;
+    }
+
+    private Node getNeighbor(Node current, Direction direction) {
+        Node node = new Node(current);
+        node.setLoc(new Point(current.loc.x + direction.x, current.loc.y + direction.y));
+        return node;
+    }
+
+    private enum Direction {
+        UP(0, 1),
+        DOWN(0, -1),
+        LEFT(-1, 0),
+        LEFT_UP(-1, 1),
+        LEFT_DOWN(-1, -1),
+        RIGHT(1, 0),
+        RIGHT_UP(1, 1),
+        RIGHT_DOWN(1, -1);
+
+        public final int x;
+        public final int y;
+
+        Direction(int x, int y) {
+            this.x = x;
+            this.y = y;
+        }
+    }
+
+}

simple-pathfinding/src/main/java/fr/uha/gasser/pathfinding/algorithm/AStarWorker.java

@@ -0,0 +1,35 @@
+package fr.uha.gasser.pathfinding.algorithm;
+
+import fr.uha.gasser.pathfinding.ui.Grid;
+
+import javax.swing.*;
+import java.awt.*;
+import java.util.List;
+
+public class AStarWorker extends SwingWorker<List<Node>, List<Node>> {
+    private final Node start;
+    private final Node goal;
+    private final Grid target;
+    private List<Node> path;
+
+    public AStarWorker(Node start, Node goal, Grid target) {
+        this.start = start;
+        this.goal = goal;
+        this.target = target;
+    }
+
+    @Override
+    protected List<Node> doInBackground() throws Exception {
+        AStarSearch aStar = new AStarSearch(start, goal);
+        path = aStar.shortestPath();
+        return path;
+    }
+
+    @Override
+    protected void done() {
+        if (this.isCancelled()) return;
+        path.forEach(node -> {
+            if (! node.loc.equals(goal.loc)) target.setTile(node.loc, Color.BLUE);
+        });
+    }
+}

simple-pathfinding/src/main/java/fr/uha/gasser/pathfinding/algorithm/Node.java

@@ -0,0 +1,46 @@
+package fr.uha.gasser.pathfinding.algorithm;
+
+import java.awt.*;
+
+public class Node implements Comparable<Node> {
+    private Node parent;
+    Point loc;
+    int cost;
+    double hValue; // heuristic value
+
+    public Node(Point loc) {
+        this.loc = loc;
+    }
+
+    Node(Node other) {
+        this.parent = other.parent;
+        this.loc = other.loc;
+        this.cost = other.cost;
+        this.hValue = other.hValue;
+    }
+
+    void setParent(Node parent) {
+        this.parent = parent;
+    }
+
+    public void sethValue(double hValue) {
+        this.hValue = hValue;
+    }
+
+    void setLoc(Point point) {
+        this.loc = point;
+    }
+
+    public Point getLoc() {
+        return loc;
+    }
+
+    Node getParent() {
+        return parent;
+    }
+
+    @Override
+    public int compareTo(Node other) {
+        return Double.compare(this.hValue, other.hValue);
+    }
+}

simple-pathfinding/src/main/java/fr/uha/gasser/pathfinding/ui/App.java

@@ -0,0 +1,91 @@
+package fr.uha.gasser.pathfinding.ui;
+
+import fr.uha.gasser.pathfinding.algorithm.AStarWorker;
+import fr.uha.gasser.pathfinding.algorithm.Node;
+import org.slf4j.Logger;
+import org.slf4j.LoggerFactory;
+
+import javax.swing.*;
+import java.awt.*;
+import java.awt.event.MouseAdapter;
+import java.awt.event.MouseEvent;
+
+public class App extends JFrame {
+
+    private static final int ROWS = 30;
+    private static final int COLS = 30;
+    private static final int CELL_SIZE = 16;
+    private static final Logger LOGGER = LoggerFactory.getLogger(App.class);
+
+    private final Grid grid;
+    private AStarWorker pathfinder;
+    private Node startNode;
+    private Node endNode;
+    private JPanel gridPane;
+
+    private App() throws HeadlessException {
+        super("Pathfinding");
+        grid = new Grid(ROWS, COLS, CELL_SIZE);
+        setupInterface();
+        pack();
+        setVisible(true);
+    }
+
+    private void setupInterface() {
+        gridPane = new JPanel();
+        gridPane.setLayout(new GridLayout(ROWS, COLS));
+        for (JLabel label : grid) {
+            gridPane.add(label);
+        }
+
+        gridPane.addMouseListener(new MouseAdapter() {
+            @Override
+            public void mousePressed(MouseEvent e) {
+                final JLabel at = (JLabel) gridPane.getComponentAt(e.getPoint());
+                final Point loc = grid.findLoc(at);
+                if (startNode == null) {
+                    grid.setTile(loc, Color.RED);
+                    startNode = new Node(loc);
+                } else if (endNode == null) {
+                    grid.setTile(loc, Color.GREEN);
+                    endNode = new Node(loc);
+                }
+            }
+        });
+
+        final JMenuBar menuBar = new JMenuBar();
+        final JMenuItem runItem = new JMenuItem("Run");
+        menuBar.add(runItem);
+        runItem.addActionListener(actionEvent -> doSearch());
+        final JMenuItem cancelItem = new JMenuItem("Cancel");
+        menuBar.add(cancelItem);
+        cancelItem.addActionListener(actionEvent -> doCancel());
+        final JMenuItem resetItem = new JMenuItem("Reset");
+        menuBar.add(resetItem);
+        resetItem.addActionListener(actionEvent -> doReset());
+
+        setJMenuBar(menuBar);
+        getContentPane().add(gridPane);
+    }
+
+    private void doReset() {
+        pathfinder = null;
+        startNode = null;
+        endNode = null;
+        grid.clear();
+    }
+
+    private void doCancel() {
+        if (pathfinder == null) return;
+        pathfinder.cancel(true);
+    }
+
+    private void doSearch() {
+        pathfinder = new AStarWorker(startNode, endNode, grid);
+        pathfinder.execute();
+    }
+
+    public static void main(String[] args) {
+        EventQueue.invokeLater(App::new);
+    }
+}

simple-pathfinding/src/main/java/fr/uha/gasser/pathfinding/ui/Grid.java

@@ -0,0 +1,81 @@
+package fr.uha.gasser.pathfinding.ui;
+
+import javax.swing.*;
+import javax.swing.border.Border;
+import java.awt.*;
+import java.awt.image.BufferedImage;
+import java.util.Iterator;
+
+public class Grid implements Iterable<JLabel> {
+
+    private final static Border lineBorder = BorderFactory.createLineBorder(Color.BLACK, 1);
+
+    private final JLabel[] cells;
+    private final Dimension size;
+    private int cellSize;
+
+    Grid(int rows, int cols, int cellSize) {
+        this.cellSize = cellSize;
+        this.size = new Dimension(rows, cols);
+        int size = rows * cols;
+        this.cells = new JLabel[size];
+
+        initGrid(size);
+    }
+
+    private void initGrid(int size) {
+        for (int i = 0; i < size; i++) {
+            final JLabel label = new JLabel();
+            // Force cell size
+            label.setPreferredSize(new Dimension(cellSize, cellSize));
+            label.setMinimumSize(label.getPreferredSize());
+            label.setMaximumSize(label.getPreferredSize());
+            label.setBorder(lineBorder);
+            label.setOpaque(true);
+            cells[i] = label;
+        }
+    }
+
+    public Point findLoc(JLabel cell) {
+        for (int i = 0; i < cells.length; i++) {
+            JLabel label = cells[i];
+            if (cell == label) return new Point(i % size.width, i / size.height);
+        }
+        // Not found
+        return new Point(-1, -1);
+    }
+
+    public void setTile(Point loc, Color color) {
+        setTile(loc.x + size.width*loc.y, color);
+    }
+
+    private void setTile(int index, Color color) {
+        final JLabel label = cells[index];
+        label.setBackground(color);
+        label.revalidate();
+    }
+
+    void clear() {
+        for (JLabel cell : cells) {
+            cell.setBackground(null);
+        }
+    }
+
+    @Override
+    public Iterator<JLabel> iterator() {
+        return new Iterator<>() {
+            private int pos = 0;
+
+            @Override
+            public boolean hasNext() {
+                return cells.length > pos;
+            }
+
+            @Override
+            public JLabel next() {
+                return cells[pos++];
+            }
+        };
+    }
+
+}