Звездный алгоритм | тупик

Question

Алгоритм A Star, над которым я работал, блокирует меня (входит в бесконечный цикл), но я не могу понять, как он блокируется (я знаю, где, но не знаю почему).

Главный:

Node nodes[][];
private GridMap gridMap;
AStar astar;

//...//

nodes = new Node[rows][columns];
gridMap = new GridMap(rows, columns, cellSize, nodes);
astar = new AStar();

//...//

public void drawPath()
    {
        List<Node> bestPath = astar.findBestPath(gridMap.getNode(1, 1), gridMap.getNode(15,15));

        g2d.setColor(Color.YELLOW);

        for(int i = 0; i < bestPath.size(); i++)
        {
            g2d.drawRect(bestPath.get(i).getX() * gridMap.getCellSize(), bestPath.get(i).getY() * gridMap.getCellSize(), 5, 5);
        }
    }

Астар класс:

public class AStar
{
    public AStar()
    {

    }

    public List<Node> findBestPath(Node start, Node goal)
    {
        List<Node> openList = new ArrayList<Node>();
        List<Node> closedList = new ArrayList<Node>();

        Node current = start;
        Node testNode;

        int l;
        int i;

        List<Node> connectedNodes = new ArrayList<Node>();
        double travelCost = 1.0;

        double g;
        double h;
        double f;

        current.g = 0;
        current.h = h(current, goal, travelCost);
        current.f = current.g + current.f;

        while(current != goal)
        {
            connectedNodes = current.getAdjancies();

            l = connectedNodes.size();

            for(i = 0; i < l; i++)
            {
                testNode = connectedNodes.get(i);

                if(testNode == current || testNode.isWalkable() == true) 
                    continue;

                g = current.g + travelCost;
                h = h(testNode, goal, travelCost);
                f = g + h;

                if(isOpen(testNode, openList) || isClosed(testNode, closedList))
                {
                    if(testNode.f > f)
                    {
                        testNode.f = f;
                        testNode.g = g;
                        testNode.h = h;
                        testNode.setParent(current);
                    }
                }
                //Goes into infinite loop here
                else {
                    testNode.f = f;
                    testNode.g = g;
                    testNode.h = h;
                    testNode.setParent(current);
                    openList.add(testNode);
                }
            }

            closedList.add(current);

            if(openList.isEmpty())
            {
                return null;
            }
        }

        return buildPath(goal, start);
    }

    public List<Node> buildPath(Node destinationNode, Node startNode)
    {
        List<Node> path = new ArrayList<Node>();
        Node node = destinationNode;

        path.add(node);
        while(node != startNode)
        {
            node = node.getParent();
        }

        return path;
    }

    public boolean isOpen(Node node, List<Node> openNodes)
    {
        int l = openNodes.size();
        for(int i = 0; i < l; i++)
        {
            if(openNodes.get(i) == node)
            {
                return true;
            }
        }

        return false;
    }

    public boolean isClosed(Node node, List<Node> closedNodes)
    {
        int l = closedNodes.size();

        for(int i = 0; i < l; i++)
        {
            if(closedNodes.get(i) == node)
            {
                return true;
            }
        }

        return false;
    }

    //Heuristic
    public double h(Node current, Node goal, double cost)
    {
        return Math.abs(current.getX() - goal.getX()) * cost + Math.abs(current.getY() - goal.getY()) * cost;
    }

Класс GridMap:

/* --------------------- GRID Codes ------------------------
 * 0 - Can walk on
 * 1 - Can NOT walk on
 * 2 - Player spawn
 * 3 - NPC spawn
 */

import java.awt.Point;
import java.util.LinkedList;
import java.util.List;
import java.util.Random;

public class GridMap 
{
    private int rows;
    private int columns;
    private int cellSize;

    //walkable and unwalkable will store grid locations that allow for quick
    //retrieval of whether a grid is walkable or not
    //player stores the player spawn location
    //npc stores the NPC spawn location
    private List<Point> walkable = new LinkedList<Point>();
    private List<Point> unwalkable = new LinkedList<Point>();
    private Point player = new Point();
    private Point npc = new Point();
    private Random rand = new Random();
    private Node nodes[][];

    private int GRID[][] =
    {
        {1,1,0,0,0,1,0,0,1,1,0,0,0,1,0,0},
        {1,0,0,0,0,3,0,0,0,0,0,0,0,1,0,0},
        {1,0,0,0,0,0,1,0,1,0,0,0,0,0,0,0},
        {0,1,1,0,1,0,0,0,1,0,0,0,0,1,0,0},
        {0,0,0,0,0,0,0,1,0,1,0,0,0,0,0,0},
        {0,0,0,1,0,1,0,0,0,0,1,0,0,0,0,0},
        {0,0,0,1,0,0,1,0,1,0,0,0,0,1,0,0},
        {0,0,1,0,0,1,0,0,1,0,1,0,0,1,0,0},
        {0,0,0,0,0,0,1,0,0,1,0,0,1,0,0,0},
        {0,0,0,1,0,0,0,0,0,0,0,0,0,1,0,0},
        {0,0,1,0,1,0,0,0,0,1,0,0,0,0,0,0},
        {0,0,1,0,0,0,0,1,0,0,1,0,0,1,0,0},
        {0,0,0,1,0,0,0,0,0,0,0,1,0,0,0,0},
        {0,0,1,0,0,0,0,0,0,0,0,0,0,1,0,0},
        {1,0,0,0,0,0,0,1,0,1,0,0,0,0,0,0},
        {1,1,0,2,0,1,1,1,0,1,0,0,0,1,0,0}
    };

    public GridMap(int rows, int columns, int cellSize, Node nodes[][])
    {
        this.rows = rows;
        this.columns = columns;
        this.cellSize = cellSize;
        this.nodes = nodes;

        InitGRID();
    }

    private void InitGRID()
    {
        boolean isWalkable = false;

        for(int y = 0; y < rows; y++)
        {
            for(int x = 0; x < columns; x++)
            {
                if(this.getGridValue(x, y) == 0)
                {
                    walkable.add(new Point(y, x));
                    isWalkable = true;
                }

                if(this.getGridValue(x, y) == 1)
                {
                    unwalkable.add(new Point(y, x));
                    isWalkable = false;
                }

                if(this.getGridValue(x, y) == 2)
                {
                    player.setLocation(y, x);
                    isWalkable = true;
                }

                if(this.getGridValue(x, y) == 3)
                {
                    npc.setLocation(y, x);
                    isWalkable = true;
                }

                nodes[y][x] = new Node(new Point(y, x));
                nodes[y][x].setWalkable(isWalkable);
            }
        }
    }

    public Node[][] getNodes()
    {
        return nodes;
    }

    public Node getNode(int x, int y)
    {
        return nodes[x][y];
    }

    public Point getPlayerSpawnLocation()
    {
        return player;
    }

    public Point getNPCSpawnLocation()
    {
        return npc;
    }

    public Point getRandomWalkableLocation()
    {
        int tmp = rand.nextInt(walkable.size());
        return walkable.get(tmp);
    }

    public int getRows()
    {
        return rows;
    }

    public int getColumns()
    {
        return columns;
    }

    public int getCellSize()
    {
        return cellSize;
    }

    public int getGridValue(int x, int y)
    {
        return GRID[x][y];
    }
}

Класс узла:

public class Node
{
    private int x;
    private int y;
    private boolean isWalkable = false;
    private boolean start = false;
    private boolean end = false;
    public double g = 1.0;
    public double h;
    public double f;

    private Node parent;
    private ArrayList<Node> adjancies = new ArrayList<Node>();
    private Point p;

    public Node(Point p)
    {
        this.p = p;
        this.x = p.x;
        this.y = p.y;
    } 

    public boolean isWalkable()
    {
        return isWalkable;
    }

    public void setWalkable(boolean isWalkable)
    {
        this.isWalkable = isWalkable;
    }

    public Point getPoint()
    {
        return p;
    }

    public int getX()
    {
        return x;
    }

    public int getY()
    {
        return y;
    }

    public boolean isStart()
    {
        return start;
    }

    public void setStart(boolean start)
    {
        this.start = start;
    }

    public boolean isEnd()
    {
        return end;
    }

    public void setEnd(boolean end)
    {
        this.end = end;
    }

    public ArrayList<Node> getAdjancies()
    {
        calculateAdjancies(this.y, this.x);
        return adjancies;
    }

    public void calculateAdjancies(int y, int x)
    {
        int top = y - 1;
        int bottom = y + 1;
        int left = x - 1;
        int right = x + 1;

        int columns = 16;
        int rows = 16;

        if(right > 0 && right < columns)
        {
            adjancies.add(new Node(new Point(right, y)));
        }

        if(left > 0 && left < columns)
        {
            adjancies.add(new Node(new Point(left, y)));
        }

        if(top > 0 && top < rows)
        {
            adjancies.add(new Node(new Point(x, top)));
        }

        if(bottom > 0 && bottom < rows)
        {
            adjancies.add(new Node(new Point(x, bottom)));
        }
    }

    public Node getParent()
    {
        return parent;
    }

    public void setParent(Node parent)
    {
        this.parent = parent;
    }
}