使用java写个路径规划算法

路径规划算法是一个非常广泛的领域，有很多不同的算法可以使用。以下是使用Java编写路径规划算法的一般步骤：

1. 定义数据结构：定义节点、边、图等数据结构。
2. 读取地图数据：从文件中读取地图数据，将其转换为图数据结构。
3. 实现算法：实现A*、Dijkstra或其他路径规划算法。
4. 输出结果：将路径输出到文件或控制台上。

下面是一个简单的A*算法实现，用于在一个网格地图上找到一条从起点到终点的路径：

import java.util.*;

public class AStar {
    private int[][] map;
    private int startRow, startCol, endRow, endCol;

    public AStar(int[][] map, int startRow, int startCol, int endRow, int endCol) {
        this.map = map;
        this.startRow = startRow;
        this.startCol = startCol;
        this.endRow = endRow;
        this.endCol = endCol;
    }

    public List<int[]> findPath() {
        List<int[]> path = new ArrayList<>();

        PriorityQueue<Node> openList = new PriorityQueue<>();
        Set<Node> closedList = new HashSet<>();

        Node startNode = new Node(startRow, startCol, null, 0, heuristic(startRow, startCol));
        openList.add(startNode);

        while (!openList.isEmpty()) {
            Node currentNode = openList.poll();

            if (currentNode.row == endRow && currentNode.col == endCol) {
                // Found path
                while (currentNode != null) {
                    path.add(0, new int[]{currentNode.row, currentNode.col});
                    currentNode = currentNode.parent;
                }
                break;
            }

            closedList.add(currentNode);

            for (int[] neighbor : getNeighbors(currentNode.row, currentNode.col)) {
                int neighborRow = neighbor[0];
                int neighborCol = neighbor[1];

                Node neighborNode = new Node(neighborRow, neighborCol, currentNode,
                                             currentNode.g + 1, heuristic(neighborRow, neighborCol));

                if (closedList.contains(neighborNode)) {
                    continue;
                }

                if (openList.contains(neighborNode)) {
                    Node existingNode = openList.stream()
                                               .filter(node -> node.equals(neighborNode))
                                               .findFirst()
                                               .get();
                    if (existingNode.g > neighborNode.g) {
                        existingNode.g = neighborNode.g;
                        existingNode.parent = currentNode;
                    }
                } else {
                    openList.add(neighborNode);
                }
            }
        }

        return path;
    }

    private List<int[]> getNeighbors(int row, int col) {
        List<int[]> neighbors = new ArrayList<>();

        if (row > 0 && map[row-1][col] == 0) {
            neighbors.add(new int[]{row-1, col});
        }

        if (row < map.length-1 && map[row+1][col] == 0) {
            neighbors.add(new int[]{row+1, col});
        }

        if (col > 0 && map[row][col-1] == 0) {
            neighbors.add(new int[]{row, col-1});
        }

        if (col < map[0].length-1 && map[row][col+1] == 0) {
            neighbors.add(new int[]{row, col+1});
        }

        return neighbors;
    }

    private int heuristic(int row, int col) {
        return Math.abs(row - endRow) + Math.abs(col - endCol);
    }

    private static class Node implements Comparable<Node> {
        public int row, col;
        public Node parent;
        public int g, h;

        public Node(int row, int col, Node parent, int g, int h) {
            this.row = row;
            this.col = col;
            this.parent = parent;
            this.g = g;
            this.h = h;
        }

        public int f() {
            return g + h;
        }

        @Override
        public boolean equals(Object obj) {
            if (obj instanceof Node) {
                Node other = (Node) obj;
                return row == other.row && col == other.col;
            }
            return false;
        }

        @Override
        public int compareTo(Node other) {
            return Integer.compare(f(), other.f());
        }
    }
}

使用示例：

public static void main(String[] args) {
    int[][] map = {
        {0, 0, 0, 0, 0, 0},
        {0, 1, 0, 1, 1, 0},
        {0, 1, 0, 0, 0, 0},
        {0, 1, 1, 0, 1, 0},
        {0, 0, 0, 0, 1, 0},
        {0, 0, 0, 0, 0, 0}
    };

    AStar astar = new AStar(map, 1, 1, 4, 4);
    List<int[]> path = astar.findPath();

    for (int[] node : path) {
        System.out.println(Arrays.toString(node));
    }
}