Как внедрить алгоритм Дейкстры для игры змейка на java?

Question

[MaksymZymyn]

https://github.com/MaksymZymyn/SnakeBattle/

Пытаюсь написать алгоритм Дейкстры для игры змейка на java. Для реализации алгоритма я создал взвешенный граф. Так как змейка может быть настолько длинной, что может разрезать граф на несколько несвязанных между собой графов, то вершины храню в матрице. Но моя змейка все равно, сколько я не пробовал, идет на шаг вверх, потом на шаг вправо до тех пор, пока не ударится в стену, вместо того, чтобы искать яблоко. Не могу найти ошибку. Вот основной метод:

@Override
    public String get(Board board) {
        this.board = board;

        // Если игра окончена, вернуть направление RIGHT
        if (board.isGameOver()) {
            return Direction.RIGHT.toString();
        }

        // Получить координаты головы змейки и координаты яблока
        Point head = board.getHead();
        Point target = board.getApples().get(0);

        // Создать граф на основе текущего состояния поля
        Vertex[][] vertexMatrix = Dijkstra.createGraph(board.getBoardState(), board.getBarriers());
        System.out.println("Vertex Matrix: ");
        Dijkstra.printMatrix(vertexMatrix);

        // Найти вершину графа, соответствующую текущей позиции головы змейки
        Vertex sourceVertex = vertexMatrix[head.getY()][head.getX()];
        System.out.println("Source Vertex: ");
        System.out.println(sourceVertex);

        // Запустить алгоритм Дейкстры для поиска кратчайшего пути до яблока
        Dijkstra.dijkstra(sourceVertex);

        // Найти вершину графа, соответствующую позиции яблока
        Vertex targetVertex = vertexMatrix[target.getY()][target.getX()];
        System.out.println("Apple: ");
        System.out.println(targetVertex);

        // Получить кратчайший путь до яблока
        List<Vertex> shortestPath = Dijkstra.getShortestPathTo(targetVertex);
        System.out.println("Path: ");
        System.out.println(shortestPath);

        // Найти следующую позицию для движения змейки вдоль кратчайшего пути
        Point nextPosition = Dijkstra.getNextPositionOnPath(head, shortestPath);
        System.out.println("Next Position: ");
        System.out.println(nextPosition);

        // Определить направление движения змейки от текущей позиции до следующей
        System.out.println(Dijkstra.determineDirection(head, nextPosition));
        return Dijkstra.determineDirection(head, nextPosition);
    }
Алгоритм Дейкстры:
package com.codenjoy.dojo.snake.client;

import com.codenjoy.dojo.services.Direction;
import com.codenjoy.dojo.services.Point;
import com.codenjoy.dojo.services.PointImpl;

import java.util.*;

public class Dijkstra {

    static Point getNextPositionOnPath(Point current, List<Vertex> shortestPath) {
        if (shortestPath == null || shortestPath.size() <= 1) {
            return current;
        }
        Vertex nextVertex = shortestPath.get(0);
        return new PointImpl(nextVertex.getY(), nextVertex.getX());
    }

    static String determineDirection(Point current, Point next) {
        int dx = next.getX() - current.getX();
        int dy = next.getY() - current.getY();

        if (dy < 0) {
            return Direction.UP.toString();
        } else if (dy > 0) {
            return Direction.DOWN.toString();
        } else if (dx < 0) {
            return Direction.LEFT.toString();
        } else {
            return Direction.RIGHT.toString();
        }
    }

    public static void printMatrix(Vertex[][] matrix){
        for (int i = 0; i < matrix.length; i++){
            for (int j = 0; j < matrix[0].length; j++){
                System.out.print(matrix[i][j] + " ");
            }
            System.out.println();
        }
    }

    public static Vertex[][] createGraph(int[][] board, List<Point> obstacles) {
        int numRows = board.length;
        int numCols = board[0].length;

        Vertex[][] vertexMatrix = new Vertex[numRows][numCols];

        // Создаем вершины для всех ячеек в матрице
        for (int i = 0; i < numRows; i++) {
            for (int j = 0; j < numCols; j++) {
                Vertex currentVertex = new Vertex(board[i][j]); // Создаем вершину с координатами ячейки
                vertexMatrix[i][j] = currentVertex;
            }
        }

        // Добавляем ребра для открытых путей
        for (int i = 0; i < numRows; i++) {
            for (int j = 0; j < numCols; j++) {
                if (board[i][j] == 1) { // Проверяем, является ли ячейка открытым путем

                    // Добавляем ребра с весом 1 к соседям
                    // Проверяем соседа сверху
                    if (i > 0 && board[i - 1][j] == 1 && !isObstacle(obstacles, i - 1, j)) {
                        vertexMatrix[i][j].neighbours.add(vertexMatrix[i - 1][j]);
                        vertexMatrix[i][j].edges.addAll(Arrays.asList(
                                new Edge(vertexMatrix[i - 1][j], 1)));
                        vertexMatrix[i - 1][j].neighbours.add(vertexMatrix[i][j]);
                        vertexMatrix[i - 1][j].edges.addAll(Arrays.asList(
                                new Edge(vertexMatrix[i][j], 1)));

                    }
                    // Проверяем соседа снизу
                    if (i < numRows - 1 && board[i + 1][j] == 1 && !isObstacle(obstacles, i + 1, j)) {
                        vertexMatrix[i][j].neighbours.add(vertexMatrix[i + 1][j]);
                        vertexMatrix[i][j].edges.addAll(Arrays.asList(
                                new Edge(vertexMatrix[i + 1][j], 1)));
                        vertexMatrix[i + 1][j].neighbours.add(vertexMatrix[i][j]);
                        vertexMatrix[i + 1][j].edges.addAll(Arrays.asList(
                                new Edge(vertexMatrix[i][j], 1)));
                    }
                    // Проверяем соседа слева
                    if (j > 0 && board[i][j - 1] == 1 && !isObstacle(obstacles, i, j - 1)) {
                        vertexMatrix[i][j].neighbours.add(vertexMatrix[i][j - 1]);
                        vertexMatrix[i][j].edges.addAll(Arrays.asList(
                                new Edge(vertexMatrix[i][j - 1], 1)));
                        vertexMatrix[i][j - 1].neighbours.add(vertexMatrix[i][j]);
                        vertexMatrix[i][j - 1].edges.addAll(Arrays.asList(
                                new Edge(vertexMatrix[i][j], 1)));
                    }
                    // Проверяем соседа справа
                    if (j < numCols - 1 && board[i][j + 1] == 1 && !isObstacle(obstacles, i, j + 1)) {
                        vertexMatrix[i][j].neighbours.add(vertexMatrix[i][j + 1]);
                        vertexMatrix[i][j].edges.addAll(Arrays.asList(
                                new Edge(vertexMatrix[i][j + 1], 1)));
                        vertexMatrix[i][j + 1].neighbours.add(vertexMatrix[i][j]);
                        vertexMatrix[i][j + 1].edges.addAll(Arrays.asList(
                                new Edge(vertexMatrix[i][j], 1)));
                    }
                }
            }
        }

        return vertexMatrix;
    }

    private static boolean isObstacle(List<Point> obstacles, int y, int x) {
        return obstacles.stream().anyMatch(p -> p.getY() == y && p.getX() == x);
    }

    public static void dijkstra(Vertex source) {
        source.minDistance = 0;
        PriorityQueue<Vertex> vertexQueue = new PriorityQueue<>();
        vertexQueue.offer(source);

        while (!vertexQueue.isEmpty()) {
            Vertex current = vertexQueue.poll();

            for (Edge adjEdge : current.edges) {
                Vertex neighbour = adjEdge.target();
                double edgeWeight = adjEdge.weight();
                double distanceThroughCurrent = current.minDistance + edgeWeight;

                if (distanceThroughCurrent < neighbour.minDistance) {
                    neighbour.minDistance = distanceThroughCurrent;
                    neighbour.previous = current;
                    // Добавляем соседа в очередь на обработку только если он еще не был посещен и доступен
                    if (!neighbour.isVisited()) {
                        vertexQueue.offer(neighbour);
                    }
                }
            }
            // После обработки всех соседей текущей вершины, помечаем текущую вершину как посещенную
            current.visited = true;
        }
    }

    public static List<Vertex> getShortestPathTo(Vertex target) {
        List<Vertex> path = new ArrayList<>();
        for (Vertex vertex = target; vertex != null; vertex = vertex.previous) {
            path.add(vertex);
        }
        Collections.reverse(path);
        return path;
    }
}