Javaのリハビリ幅優先探索

Main.java

import java.io.BufferedReader;
import java.io.IOException;
import java.io.InputStream;
import java.io.InputStreamReader;
import java.util.LinkedList;
import java.util.StringTokenizer;

public class Main {

    public static class Position {
        public int x;
        public int y;
        public int pathLength = 0;

        public Position(int y, int x) {
            this.y = y;
            this.x = x;
        }

        public boolean isEqual(Position other) {
            return (this.y == other.y) && (this.x == other.x);
        }
    }

    static Position[] vectors = new Position[] { new Position(1, 0),
            new Position(-1, 0), new Position(0, 1), new Position(0, -1) };

    static int searchGoal(Position start, Position goal, char[][] maze) {
        LinkedList<Position> queue = new LinkedList<Position>();
        boolean[][] visited = new boolean[maze.length][maze[0].length];

        queue.add(start);
        while (!queue.isEmpty()) {
            Position p = queue.poll();

            if (p.isEqual(goal)) {
                return p.pathLength;
            }

            for (Position vector : vectors) {
                Position next = new Position(p.y + vector.y, p.x + vector.x);
                if (0 <= next.y && next.y < maze.length && 0 <= next.x
                        && next.x < maze[0].length
                        && maze[next.y][next.x] == '.') {
                    if (visited[next.y][next.x])
                        continue;

                    visited[next.y][next.x] = true;
                    next.pathLength = p.pathLength + 1;
                    queue.add(next);
                }
            }

        }

        return -1;
    }

    public static void main(String[] args) throws IOException {
        Scanner scanner = new Scanner(System.in);
        int y, x;
        int Y = scanner.nextInt();
        int X = scanner.nextInt();

        y = scanner.nextInt() - 1;
        x = scanner.nextInt() - 1;
        Position start = new Position(y, x);

        y = scanner.nextInt() - 1;
        x = scanner.nextInt() - 1;
        Position goal = new Position(y, x);

        char[][] maze = new char[Y][X];

        for (int i = 0; i < Y; i++) {
            char[] in = scanner.next().toCharArray();
            for (int j = 0; j < X; j++) {
                maze[i][j] = in[j];

            }
        }

        System.out.println(searchGoal(start, goal, maze));

    }

    public static class Scanner {

        private BufferedReader br;
        private StringTokenizer tok;

        public Scanner(InputStream is) throws IOException {
            br = new BufferedReader(new InputStreamReader(is));
            getLine();
        }

        private void getLine() throws IOException {
            while (tok == null || !tok.hasMoreTokens()) {
                tok = new StringTokenizer(br.readLine());
            }
        }

        private boolean hasNext() {
            return tok.hasMoreTokens();
        }

        public String next() throws IOException {
            if (hasNext()) {
                return tok.nextToken();
            } else {
                getLine();
                return tok.nextToken();
            }
        }

        public int nextInt() throws IOException {
            if (hasNext()) {
                return Integer.parseInt(tok.nextToken());
            } else {
                getLine();
                return Integer.parseInt(tok.nextToken());
            }
        }

        public long nextLong() throws IOException {
            if (hasNext()) {
                return Long.parseLong(tok.nextToken());
            } else {
                getLine();
                return Long.parseLong(tok.nextToken());
            }
        }

        public double nextDouble() throws IOException {
            if (hasNext()) {
                return Double.parseDouble(tok.nextToken());
            } else {
                getLine();
                return Double.parseDouble(tok.nextToken());
            }
        }
    }
}