A* / Dijkstra's algorithm simple implementation (Pascal)

Question

I'm trying to implement A* path finding algorithm (now it's Dijkstra's algorithm i.e without heuristic) using this article Link. But I can't figure out what's wrong in my code (it finds incorrect path).

instead of the empty begin ... end; it should be this step:

If it is on the open list already, check to see if this path to that square is better, using G cost as the measure. A lower G cost means that this is a better path. If so, change the parent of the square to the current square, and recalculate the G and F scores of the square.

but I think it is not important because there is no diagonal movement.

uses
    crt;

const
    MAXX = 20;
    MAXY = 25;

type
    TArr = array [0..MAXY, 0..MAXX] of integer;
    
    TCell = record
        x: integer;
        y: integer;
    end;
    
    TListCell = record
        x: integer;
        y: integer;
        G: integer;
        parent: TCell;
    end;
    
    TListArr = array [1..10000] of TListCell;
    
    TList = record
        arr: TListArr;
        len: integer;
    end;

var
    i, j, minind, ind, c: integer;
    start, finish: TCell;
    current: TListCell;
    field: TArr;
    opened, closed: TList;

procedure ShowField;
var
    i, j: integer;
begin
    textcolor(15);
    for i := 0 to MAXX do
    begin
        for j := 0 to MAXY do
        begin
            case field[j, i] of
                99: textcolor(8);  // not walkable
                71: textcolor(14); // walkable
                11: textcolor(10); // start
                21: textcolor(12); // finish
                15: textcolor(2);  // path
                14: textcolor(5);
                16: textcolor(6);
            end;
            write(field[j, i], ' ');
        end;
        writeln;
    end;
    textcolor(15);
end; 


procedure AddClosed(a: TListCell);
begin
    closed.arr[closed.len + 1] := a;
    inc(closed.len);
end;


procedure AddOpened(x, y, G: integer);
begin
    opened.arr[opened.len + 1].x := x;
    opened.arr[opened.len + 1].y := y;
    opened.arr[opened.len + 1].G := G;
    inc(opened.len);
end;

procedure DelOpened(n: integer);
var
    i: integer;
begin
    AddClosed(opened.arr[n]);
    for i := n to opened.len - 1 do
        opened.arr[i] := opened.arr[i + 1];
    dec(opened.len);
end;


procedure SetParent(var a: TListCell; parx, pary: integer);
begin
    a.parent.x := parx;
    a.parent.y := pary;
end;


function GetMin(var a: TList): integer;
var
    i, min, mini: integer;
begin
    min := MaxInt;
    mini := 0;
    for i := 1 to a.len do
        if a.arr[i].G < min then
        begin
            min := a.arr[i].G;
            mini := i;
        end;
    
    GetMin := mini;
end;


function FindCell(a: TList; x, y: integer): integer;
var
    i: integer;
begin
    FindCell := 0;
    for i := 1 to a.len do
        if (a.arr[i].x = x) and (a.arr[i].y = y) then
        begin
            FindCell := i;
            break;
        end;
end;


procedure ProcessNeighbourCell(x, y: integer);
begin
    if (field[current.x + x, current.y + y] <> 99) then    // if walkable
        if (FindCell(closed, current.x + x, current.y + y) <= 0) then // and not visited before
            if (FindCell(opened, current.x + x, current.y + y) <= 0) then // and not added to list already
            begin
                AddOpened(current.x + x, current.y + y, current.G + 10);
                SetParent(opened.arr[opened.len], current.x, current.y);
                //  field[opened.arr[opened.len].x, opened.arr[opened.len].y]:=16;
            end
                else
            begin
                
            end;
end;


begin
    randomize;
    for i := 0 to MAXX do
        for j := 0 to MAXY do
            field[j, i] := 99;
    
    for i := 1 to MAXX - 1 do
        for j := 1 to MAXY - 1 do
            if random(5) mod 5 = 0 then
                field[j, i] := 99
            else field[j, i] := 71;
  
    // start and finish positions coordinates
    start.x := 5;
    start.y := 3;
    finish.x := 19;
    finish.y := 16;
    field[start.x, start.y] := 11;
    field[finish.x, finish.y] := 21;
    
    ShowField;
    
    writeln;
    
    opened.len := 0;
    closed.len := 0;
    AddOpened(start.x, start.y, 0);
    SetParent(opened.arr[opened.len], -1, -1);
    current.x := start.x;
    current.y := start.y;
    
    repeat
        minind := GetMin(opened);
        current.x := opened.arr[minind].x;
        current.y := opened.arr[minind].y;
        current.G := opened.arr[minind].G; 
        DelOpened(minind); 
        
        ProcessNeighbourCell(1, 0);  // look at the cell to the right
        ProcessNeighbourCell(-1, 0); // look at the cell to the left
        ProcessNeighbourCell(0, 1);  // look at the cell above
        ProcessNeighbourCell(0, -1); // look at the cell below
      
        if (FindCell(opened, finish.x, finish.y) > 0) then
            break;
    until opened.len = 0;
    
    // count and mark path
    c := 0;
    while ((current.x <> start.x) or (current.y <> start.y)) do
    begin
        field[current.x, current.y] := 15;
        ind := FindCell(closed, current.x, current.y);
        current.x := closed.arr[ind].parent.x;
        current.y := closed.arr[ind].parent.y;
        inc(c);
    end;
    
    
    ShowField;
    writeln(c);
    readln;
end.

Edit Feb 1 '12: updated code, also fixed path marking (there should be or instead and), looks like it works now :)

Answer 1

You should rewrite the program to use a loop instead of cut-and-paste to visit each neighbor. If you do that you will avoid bugs like the following:

if (field[current.x, current.y - 1] <> 99) then
    if (FindCell(closed, current.x, current.y - 1) <= 0) then
        if (FindCell(opened, current.x + 1, current.y) <= 0) then

(See the inconsistent current.x + 1, current.y in the last line.)

---

With respect to the loop, I was thinking of something like this (pseudo-Python):

neighbor_offsets = [(0, 1), (0, -1), (1, 0), (-1, 0)]
for offset in neighbor_offsets:
    neighbor = current + offset
    if is_walkable(neighbor) and not is_visited(neighbor):
        # Open 'neighbor' with 'current' as parent:
        open(neighbor, current)

        # Perhaps check if the goal is reached:
        if neighbor == finish:
            goal_reached = True
            break

If you don't write a loop but just refactor to

ProcessCell(x+1, y); 
ProcessCell(x-1, y); 
ProcessCell(x, y-1); 
ProcessCell(x, y-1);

then that's a great improvement too.

Answer 2

Youre posting quite a lot of code, have you tried narrow it down where it fails?

Have you compared your code with the pseudocode on wikipedia?

Also remember that dijkstra is just A* with a heuristic of 0.

Edit:

The article you linked (which I now realize is the very same I used to learn the A*, funny) contains illustrated steps. I would suggest that you recreate that map/grid and run your implementation on it. Then step through the images:

1. Are the eight initial neighbors added to the open list? Do they have the correct parent?
2. Is the correct open node picked as next to be scanned according to the heuristic?
3. Is the list of closed nodes correct?
4. And so on...