How can I debug my code for the A star algorithm? - a-star

Question

I have been trying to program different A star algorithms I found online and though they make sense, every implementation I have programmed failed.
This is my Free Pascal code:
function getHeuristic(currentXY, targetXY: array of word): word;
begin
getHeuristic:=abs(currentXY[0]-targetXY[0])+abs(currentXY[1]-targetXY[1]);
end;
function getPath(startingNodeXY, targetNodeXY: array of word; grid: wordArray3; out pathToControlledCharPtr: word; worldObjIndex: word): wordArray2;
var
openList, closedList: array of array of word; { x/y/g/h/parent x/parent y, total }
qXYGH: array[0..5] of word; { x/y/g/h/parent x/parent y }
gridXCnt, gridYCnt: longInt;
maxF, q, openListCnt, closedListCnt, parentClosedListCnt, getPathCnt, adjSquNewGScore: word;
openListIndexCnt, closedListIndexCnt, qIndexCnt, successorIndexCnt: byte;
getMaxF, successorOnClosedList, successorOnOpenList, pathFound: boolean;
begin
{ Add the starting square (or node) to the open list. }
setLength(openList, 6, length(openList)+1);
openList[0, 0]:=startingNodeXY[0];
openList[1, 0]:=startingNodeXY[1];
setLength(closedList, 6, 0);
{ Repeat the following: }
{ D) Stop when you: }
{ Fail to find the target square, and the open list is empty. In this case, there is no path. }
pathFound:=false;
{ writeLn('h1'); }
while length(openList[0])>0 do
begin
{ A) Look for the lowest F cost square on the open list. We refer to this as the current square. }
maxF:=0;
q:=0;
getMaxF:=true;
for openListCnt:=0 to length(openList[0])-1 do
begin
//writeLn(formatVal('open list xy {} {}, cnt {}, list max index {}', [openList[0, openListCnt], openList[1, openListCnt], openListCnt, length(openList[0])-1]));
{ readLnPromptX; }
if (getMaxF=true) or (maxF>openList[2, openListCnt]+openList[3, openListCnt]) then
begin
getMaxF:=false;
maxF:=openList[2, openListCnt]+openList[3, openListCnt];
q:=openListCnt;
end;
end;
for qIndexCnt:=0 to length(qXYGH)-1 do
qXYGH[qIndexCnt]:=openList[qIndexCnt, q];
{ B). Switch it to the closed list. }
setLength(closedList, length(closedList), length(closedList[0])+1);
for closedListIndexCnt:=0 to length(closedList)-1 do
closedList[closedListIndexCnt, length(closedList[0])-1]:=qXYGH[closedListIndexCnt];
{ Remove current square from open list }
if q<length(openList[0])-1 then
begin
for openListCnt:=q to length(openList[0])-2 do
begin
for openListIndexCnt:=0 to length(openList)-1 do
openList[openListIndexCnt, openListCnt]:=openList[openListIndexCnt, openListCnt+1];
end;
end;
setLength(openList, length(openList), length(openList[0])-1);
//writeLn(formatVal('q[x] {}, q[y] {}, startingNodeXY x {}, startingNodeXY y {}, targetNodeXY x {}, targetNodeXY y {}', [qXYGH[0], qXYGH[1], startingNodeXY[0], startingNodeXY[1], targetNodeXY[0], targetNodeXY[1]]));
{ readLnPromptX; }
{ D) Stop when you: }
{ Add the target square to the closed list, in which case the path has been found, or }
if (qXYGH[0]=targetNodeXY[0]) and (qXYGH[1]=targetNodeXY[1]) then
begin
pathFound:=true;
break;
end;
{ C) For each of the 8 squares adjacent to this current square … }
for gridXCnt:=qXYGH[0]-1 to qXYGH[0]+1 do
begin
for gridYCnt:=qXYGH[1]-1 to qXYGH[1]+1 do
begin
{ Adjacent square cannot be the current square }
if (gridXCnt<>qXYGH[0]) or (gridYCnt<>qXYGH[1]) then
begin
//writeLn(formatVal('gridXCnt {} gridYCnt {} qXYGH[0] {} qXYGH[1] {}', [gridXCnt, gridYCnt, qXYGH[0], qXYGH[1]]));
{ readLnPromptX; }
{ Check if successor is on closed list }
successorOnClosedList:=false;
if length(closedList[0])>0 then
begin
for closedListCnt:=0 to length(closedList[0])-1 do
begin
if (closedList[0, closedListCnt]=gridXCnt) and (closedList[1, closedListCnt]=gridYCnt) then
begin
successorOnClosedList:=true;
break;
end;
end;
end;
{ If it is not walkable or if it is on the closed list, ignore it. Otherwise do the following. }
if (gridXCnt>=0) and (gridXCnt<=length(grid[3])-1) and (gridYCnt>=0) and (gridYCnt<=length(grid[3, 0])-1) and (grid[3, gridXCnt, gridYCnt]=0) and (successorOnClosedList=false) then
begin
{ If it isn’t on the open list, add it to the open list. Make the current square the parent of this square. Record the F, G, and H costs of the square. }
successorOnOpenList:=false;
if length(openList[0])>0 then
begin
for openListCnt:=0 to length(openList[0])-1 do
begin
if (openList[0, openListCnt]=gridXCnt) and (openList[1, openListCnt]=gridYCnt) then
begin
successorOnOpenList:=true;
break;
end;
end;
end;
if successorOnOpenList=false then
begin
setLength(openList, length(openList), length(openList[0])+1);
openList[0, length(openList[0])-1]:=gridXCnt;
openList[1, length(openList[0])-1]:=gridYCnt;
openList[4, length(openList[0])-1]:=qXYGH[0];
openList[5, length(openList[0])-1]:=qXYGH[1];
if (openList[0, length(openList[0])-1]=qXYGH[0]) or (openList[1, length(openList[0])-1]=qXYGH[1]) then
begin
openList[2, length(openList[0])-1]:=openList[2, length(openList[0])-1]+10;
end
else
begin
openList[2, length(openList[0])-1]:=openList[2, length(openList[0])-1]+14;
end;
openList[3, length(openList[0])-1]:=getHeuristic([openList[0, length(openList[0])-1], openList[1, length(openList[0])-1]], [targetNodeXY[0], targetNodeXY[1]]);
end
else
begin
{ 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 (check to see if the G score for the adjacent square is lower if we use the current square to get there (adjacent square
new G score = current square G score + 10 (if adjacent squre is vertical or horizontal to current square) or +14 (if it is diagonal); if result is lower than adjacent square current G score then this path is better). 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. If you are keeping your open list sorted by F score, you may need to resort the list to account for the
change. }
adjSquNewGScore:=openList[2, openListCnt];
if (openList[0, openListCnt]=qXYGH[0]) or (openList[1, openListCnt]=qXYGH[1]) then
begin
adjSquNewGScore:=adjSquNewGScore+10;
end
else
begin
adjSquNewGScore:=adjSquNewGScore+14;
end;
if adjSquNewGScore<openList[2, openListCnt] then
begin
openList[4, openListCnt]:=qXYGH[0];
openList[5, openListCnt]:=qXYGH[1];
openList[2, openListCnt]:=adjSquNewGScore;
end;
end;
end;
end;
end;
end;
end;
{ writeLn('h2'); }
{ writeLn(pathFound); }
{ readLnHalt; }
if pathFound=true then
begin
{ Save the path. Working backwards from the target square, go from each square to its parent square until you reach the starting square. That is your path. }
closedListCnt:=length(closedList[0])-1;
setLength(getPath, 2, 0);
{ While starting node has not been added to path }
while (length(getPath[0])=0) or (getPath[0, length(getPath[0])-1]<>startingNodeXY[0]) or (getPath[1, length(getPath[0])-1]<>startingNodeXY[1]) do
begin
{ Add node from closed list to path }
setLength(getPath, 2, length(getPath[0])+1);
getPath[0, length(getPath[0])-1]:=closedList[0, closedListCnt];
getPath[1, length(getPath[0])-1]:=closedList[1, closedListCnt];
{ Find next node on closed list with coord matching parent coord of current closed list node }
for parentClosedListCnt:=length(closedList[0])-1 downto 0 do
if (closedList[0, parentClosedListCnt]=closedList[4, closedListCnt]) and (closedList[1, parentClosedListCnt]=closedList[5, closedListCnt]) then break;
closedListCnt:=parentClosedListCnt;
{ if (closedList[0, closedListCnt]=0) and (closedList[1, closedListCnt]=0) then break; }
end;
pathToControlledCharPtr:=length(getPath[0])-1;
end;
end;
The steps I'm following are:
Add the starting square (or node) to the open list.
Repeat the following:
A) Look for the lowest F cost square on the open list. We refer to this as the current square.
B). Switch it to the closed list.
C) For each of the 8 squares adjacent to this current square …
If it is not walkable or if it is on the closed list, ignore it. Otherwise do the following.
If it isn’t on the open list, add it to the open list. Make the current square the parent of this square. Record the F, G, and H costs of the square.
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. If you are keeping your open list sorted by F score, you may need to resort the list to account for the change.
D) Stop when you:
Add the target square to the closed list, in which case the path has been found, or
Fail to find the target square, and the open list is empty. In this case, there is no path.
Save the path. Working backwards from the target square, go from each square to its parent square until you reach the starting square. That is your path.

Answer 1

Solved it!
This is the working code:
function getHeuristic(currentXY, targetXY: array of word): word;
begin
getHeuristic:=abs(currentXY[0]-targetXY[0])+abs(currentXY[1]-targetXY[1]);
end;
function getPath(startingNodeXY, targetNodeXY: array of word; grid: wordArray3; out pathToControlledCharPtr: word; worldObjIndex: word): wordArray2;
var
openList, closedList: array of array of word; { x/y/g/h/parent x/parent y, total }
qXYGH: array[0..5] of word; { x/y/g/h/parent x/parent y }
gridXCnt, gridYCnt: longInt;
maxF, q, openListCnt, closedListCnt, parentClosedListCnt, getPathCnt, adjSquNewGScore: word;
openListIndexCnt, closedListIndexCnt, qIndexCnt, successorIndexCnt: byte;
getMaxF, successorOnClosedList, successorOnOpenList, pathFound: boolean;
begin
{ Add the starting square (or node) to the open list. }
setLength(openList, 6, length(openList)+1);
openList[0, 0]:=startingNodeXY[0];
openList[1, 0]:=startingNodeXY[1];
setLength(closedList, 6, 0);
{ Repeat the following: }
{ D) Stop when you: }
{ Fail to find the target square, and the open list is empty. In this case, there is no path. }
pathFound:=false;
{ writeLn('h1'); }
while length(openList[0])>0 do
begin
{ A) Look for the lowest F cost square on the open list. We refer to this as the current square. }
maxF:=0;
q:=0;
getMaxF:=true;
for openListCnt:=0 to length(openList[0])-1 do
begin
//writeLn(formatVal('open list xy {} {}, cnt {}, list max index {}', [openList[0, openListCnt], openList[1, openListCnt], openListCnt, length(openList[0])-1]));
{ readLnPromptX; }
if (getMaxF=true) or (maxF>openList[2, openListCnt]+openList[3, openListCnt]) then
begin
getMaxF:=false;
maxF:=openList[2, openListCnt]+openList[3, openListCnt];
q:=openListCnt;
end;
end;
for qIndexCnt:=0 to length(qXYGH)-1 do
qXYGH[qIndexCnt]:=openList[qIndexCnt, q];
{ B). Switch it to the closed list. }
setLength(closedList, length(closedList), length(closedList[0])+1);
for closedListIndexCnt:=0 to length(closedList)-1 do
closedList[closedListIndexCnt, length(closedList[0])-1]:=qXYGH[closedListIndexCnt];
{ Remove current square from open list }
if q<length(openList[0])-1 then
begin
for openListCnt:=q to length(openList[0])-2 do
begin
for openListIndexCnt:=0 to length(openList)-1 do
openList[openListIndexCnt, openListCnt]:=openList[openListIndexCnt, openListCnt+1];
end;
end;
setLength(openList, length(openList), length(openList[0])-1);
//writeLn(formatVal('q[x] {}, q[y] {}, startingNodeXY x {}, startingNodeXY y {}, targetNodeXY x {}, targetNodeXY y {}', [qXYGH[0], qXYGH[1], startingNodeXY[0], startingNodeXY[1], targetNodeXY[0], targetNodeXY[1]]));
{ readLnPromptX; }
{ D) Stop when you: }
{ Add the target square to the closed list, in which case the path has been found, or }
if (qXYGH[0]=targetNodeXY[0]) and (qXYGH[1]=targetNodeXY[1]) then
begin
pathFound:=true;
break;
end;
{ C) For each of the 8 squares adjacent to this current square … }
for gridXCnt:=qXYGH[0]-1 to qXYGH[0]+1 do
begin
for gridYCnt:=qXYGH[1]-1 to qXYGH[1]+1 do
begin
{ Adjacent square cannot be the current square }
if (gridXCnt<>qXYGH[0]) or (gridYCnt<>qXYGH[1]) then
begin
//writeLn(formatVal('gridXCnt {} gridYCnt {} qXYGH[0] {} qXYGH[1] {}', [gridXCnt, gridYCnt, qXYGH[0], qXYGH[1]]));
{ readLnPromptX; }
{ Check if successor is on closed list }
successorOnClosedList:=false;
if length(closedList[0])>0 then
begin
for closedListCnt:=0 to length(closedList[0])-1 do
begin
if (closedList[0, closedListCnt]=gridXCnt) and (closedList[1, closedListCnt]=gridYCnt) then
begin
successorOnClosedList:=true;
break;
end;
end;
end;
{ If it is not walkable or if it is on the closed list, ignore it. Otherwise do the following. }
if (gridXCnt>=0) and (gridXCnt<=length(grid[3])-1) and (gridYCnt>=0) and (gridYCnt<=length(grid[3, 0])-1) and (grid[3, gridXCnt, gridYCnt]=0) and (successorOnClosedList=false) then
begin
{ If it isn’t on the open list, add it to the open list. Make the current square the parent of this square. Record the F, G, and H costs of the square. }
successorOnOpenList:=false;
if length(openList[0])>0 then
begin
for openListCnt:=0 to length(openList[0])-1 do
begin
if (openList[0, openListCnt]=gridXCnt) and (openList[1, openListCnt]=gridYCnt) then
begin
successorOnOpenList:=true;
break;
end;
end;
end;
if successorOnOpenList=false then
begin
setLength(openList, length(openList), length(openList[0])+1);
openList[0, length(openList[0])-1]:=gridXCnt;
openList[1, length(openList[0])-1]:=gridYCnt;
openList[4, length(openList[0])-1]:=qXYGH[0];
openList[5, length(openList[0])-1]:=qXYGH[1];
if (openList[0, length(openList[0])-1]=qXYGH[0]) or (openList[1, length(openList[0])-1]=qXYGH[1]) then
begin
openList[2, length(openList[0])-1]:=openList[2, length(openList[0])-1]+10;
end
else
begin
openList[2, length(openList[0])-1]:=openList[2, length(openList[0])-1]+14;
end;
openList[3, length(openList[0])-1]:=getHeuristic([openList[0, length(openList[0])-1], openList[1, length(openList[0])-1]], [targetNodeXY[0], targetNodeXY[1]]);
end
else
begin
{ 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 (check to see if the G score for the adjacent square is lower if we use the current square to get there (adjacent square
new G score = current square G score + 10 (if adjacent squre is vertical or horizontal to current square) or +14 (if it is diagonal); if result is lower than adjacent square current G score then this path is better). 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. If you are keeping your open list sorted by F score, you may need to resort the list to account for the
change. }
adjSquNewGScore:=openList[2, openListCnt];
if (openList[0, openListCnt]=qXYGH[0]) or (openList[1, openListCnt]=qXYGH[1]) then
begin
adjSquNewGScore:=adjSquNewGScore+10;
end
else
begin
adjSquNewGScore:=adjSquNewGScore+14;
end;
if adjSquNewGScore<openList[2, openListCnt] then
begin
openList[4, openListCnt]:=qXYGH[0];
openList[5, openListCnt]:=qXYGH[1];
openList[2, openListCnt]:=adjSquNewGScore;
end;
end;
end;
end;
end;
end;
end;
{ writeLn('h2'); }
{ writeLn(pathFound); }
{ readLnHalt; }
if pathFound=true then
begin
{ Save the path. Working backwards from the target square, go from each square to its parent square until you reach the starting square. That is your path. }
closedListCnt:=length(closedList[0])-1;
setLength(getPath, 2, 0);
{ While starting node has not been added to path }
while (length(getPath[0])=0) or (getPath[0, length(getPath[0])-1]<>startingNodeXY[0]) or (getPath[1, length(getPath[0])-1]<>startingNodeXY[1]) do
begin
{ Add node from closed list to path }
setLength(getPath, 2, length(getPath[0])+1);
getPath[0, length(getPath[0])-1]:=closedList[0, closedListCnt];
getPath[1, length(getPath[0])-1]:=closedList[1, closedListCnt];
//writeLn(formatVal('path found {} {}, start {} {}, target {} {}', [getPath[0, length(getPath[0])-1], getPath[1, length(getPath[0])-1], startingNodeXY[0], startingNodeXY[1], targetNodeXY[0], targetNodeXY[1]]));
{ readLnPromptX; }
{ Find next node on closed list with coord matching parent coord of current closed list node }
for parentClosedListCnt:=length(closedList[0])-1 downto 0 do
if (closedList[0, parentClosedListCnt]=closedList[4, closedListCnt]) and (closedList[1, parentClosedListCnt]=closedList[5, closedListCnt]) then break;
closedListCnt:=parentClosedListCnt;
{ if (closedList[0, closedListCnt]=0) and (closedList[1, closedListCnt]=0) then break; }
end;
pathToControlledCharPtr:=length(getPath[0])-1;
end;
end;