Hi,
I've been making an A* Pathfinder Algolithm and I'm having a bit of trouble because it's no finding the shortest path. Take a look at the code below:
sync on
mapwidth=9
mapheight=7
dim nodeW(mapwidth,mapheight)
for y=1 to mapheight
for x=1 to mapwidth
read mapdata
if mapdata=1 then nodeW(x,y)=1
if mapdata=2 then startx=x : starty=y
if mapdata=3 then endx=x : endy=y
next y
next x
dim Path(mapwidth,mapheight)
dim NodeOC(mapwidth,mapheight)
dim Nodestack(mapwidth,mapheight)
dim Openlistx(mapwidth*mapheight)
dim Openlisty(mapwidth*mapheight)
dim Oparentx(mapwidth*mapheight)
dim Oparenty(mapwidth*mapheight)
dim ONodeG(mapwidth*mapheight)
dim ONodeH(mapwidth*mapheight)
dim ONodeF(mapwidth*mapheight)
dim Cparentx(mapwidth*mapheight)
dim Cparenty(mapwidth*mapheight)
dim CNodeG(mapwidth*mapheight)
dim CNodeH(mapwidth*mapheight)
dim CNodeF(mapwidth*mapheight)
dim Closelistx(mapwidth*mapheight)
dim Closelisty(mapwidth*mapheight)
Opens=0
Closed=0
gosub AStarPathFinder
ink 155,0
box 40,40,(mapwidth+1)*40,(mapheight+1)*40
set text size 10
set text font "courier new"
sync
for x=1 to mapwidth
for y=1 to mapheight
if nodeW(x,y)=1
ink 155,0
box x*40+1,y*40+1,x*40+39,y*40+39
else
ink 0,0
box x*40+1,y*40+1,x*40+39,y*40+39
if x=startx and y=starty
ink rgb(0,255,0),0
box x*40+1,y*40+1,x*40+39,y*40+39
endif
if x=endx and y=endy
ink rgb(255,0,0),0
box x*40+1,y*40+1,x*40+39,y*40+39
endif
if Path(x,y)=1
ink rgb(255,0,0),0
box x*40+10,y*40+10,x*40+30,y*40+30
endif
Stack=NodeStack(x,y)
if NodeOC(x,y)=1
ink rgb(255,255,0),0
linebox(x*40+2,y*40+2,x*40+38,y*40+38)
linebox(x*40+1,y*40+1,x*40+39,y*40+39)
G=ONodeG(Stack)
H=ONodeH(Stack)
F=ONodeF(Stack)
ink rgb(0,100,255),0
text x*40+5,y*40+30,str$(G)
text x*40+27,y*40+30,str$(H)
text x*40+15,y*40+3,str$(F)
if OParentx(Stack)>0 and OParentx(Stack)>0
circle x*40+20,y*40+20,2
px=OParentx(Stack)
py=OParenty(Stack)
angle#=wrapvalue( wrapvalue( 360-atanfull( (px*40+20) - (x*40+20) , (py*40+20) - (y*40+20) ) )-270 )
x2#=cos(angle#)*12
y2#=sin(angle#)*12
x2#=x2#+(x*40+20)
y2#=y2#+(y*40+20)
line x*40+20,y*40+20,x2#,y2#
endif
endif
if NodeOC(x,y)=2
ink rgb(255,0,255),0
linebox(x*40+2,y*40+2,x*40+38,y*40+38)
linebox(x*40+1,y*40+1,x*40+39,y*40+39)
G=CNodeG(Stack)
H=CNodeH(Stack)
F=CNodeF(Stack)
ink rgb(0,100,255),0
text x*40+5,y*40+30,str$(G)
text x*40+27,y*40+30,str$(H)
text x*40+15,y*40+3,str$(F)
if CParentx(Stack)>0 and CParentx(Stack)>0
circle x*40+20,y*40+20,2
px=CParentx(Stack)
py=CParenty(Stack)
angle#=wrapvalue( wrapvalue( 360-atanfull( (px*40+20) - (x*40+20) , (py*40+20) - (y*40+20) ) )-270 )
x2#=cos(angle#)*12
y2#=sin(angle#)*12
x2#=x2#+(x*40+20)
y2#=y2#+(y*40+20)
line x*40+20,y*40+20,x2#,y2#
endif
endif
endif
next y
next x
end
AStarPathFinder:
Opens=Opens+1
NodeOC(startx,starty)=1
NodeStack(startx,starty)=Opens
Openlistx(Opens)=startx
Openlisty(Opens)=starty
ONodeG(Opens)=0
ONodeH(Opens)=ManhattanHeuristic(startx,starty,endx,endy)*10
ONodeF(Opens)=ONodeG(Opens)+ONodeH(Opens)
OParentx(Opens)=0
OParenty(Opens)=0
NoPath=0
finish=0
endloop=0
Node=1
Nodex=startx
Nodey=starty
repeat
Closed=Closed+1
NodeOC(nodex,nodey)=2
NodeStack(nodex,nodey)=Closed
Closelistx(Closed)=nodex
Closelisty(Closed)=nodey
CNodeG(Closed)=ONodeG(Node)
CNodeH(Closed)=ONodeH(Node)
CNodeF(Closed)=ONodeF(Node)
CParentx(Closed)=OParentx(Node)
CParenty(Closed)=OParenty(Node)
ONodeG(Node)=-1
ONodeH(Node)=-1
ONodeF(Node)=-1
OParentx(Node)=-1
OParenty(Node)=-1
if nodex=endx and nodey=endy then finish=1
x=nodex : y=nodey-1
if NodeW(x,y)=0
if NodeOC(x,y)=0
NodeOC(x,y)=1
Opens=Opens+1
NodeStack(x,y)=Opens
Openlistx(Opens)=x
Openlisty(Opens)=y
ONodeG(Opens)=10
ONodeH(Opens)=ManhattanHeuristic(x,y,endx,endy)*10
ONodeF(Opens)=ONodeG(Opens)+ONodeH(Opens)
OParentx(Opens)=nodex
OParenty(Opens)=nodey
else
if NodeOC(x,y)=1
Stack=NodeStack(x,y)
if CNodeG(Closed)+10<ONodeG(Stack)
ONodeG(Stack)=CNodeG(Closed)+10
ONodeF(Stack)=ONodeG(Stack)+ONodeH(Stack)
OParentx(Stack)=nodex
OParenty(Stack)=nodey
endif
endif
endif
endif
x=nodex+1 : y=nodey-1
if NodeW(x,y)=0 and NodeW(nodex+1,nodey)=0 and NodeW(nodex,nodey-1)=0
if NodeOC(x,y)=0
NodeOC(x,y)=1
Opens=Opens+1
NodeStack(x,y)=Opens
Openlistx(Opens)=x
Openlisty(Opens)=y
ONodeG(Opens)=14
ONodeH(Opens)=ManhattanHeuristic(x,y,endx,endy)*10
ONodeF(Opens)=ONodeG(Opens)+ONodeH(Opens)
OParentx(Opens)=nodex
OParenty(Opens)=nodey
else
if NodeOC(x,y)=1
Stack=NodeStack(x,y)
if CNodeG(Closed)+14<ONodeG(Stack)
ONodeG(Stack)=CNodeG(Closed)+14
ONodeF(Stack)=ONodeG(Stack)+ONodeH(Stack)
OParentx(Stack)=nodex
OParenty(Stack)=nodey
endif
endif
endif
endif
x=nodex+1 : y=nodey
if NodeW(x,y)=0
if NodeOC(x,y)=0
NodeOC(x,y)=1
Opens=Opens+1
NodeStack(x,y)=Opens
Openlistx(Opens)=x
Openlisty(Opens)=y
ONodeG(Opens)=10
ONodeH(Opens)=ManhattanHeuristic(x,y,endx,endy)*10
ONodeF(Opens)=ONodeG(Opens)+ONodeH(Opens)
OParentx(Opens)=nodex
OParenty(Opens)=nodey
else
if NodeOC(x,y)=1
Stack=NodeStack(x,y)
if CNodeG(Closed)+10<ONodeG(Stack)
ONodeG(Stack)=CNodeG(Closed)+10
ONodeF(Stack)=ONodeG(Stack)+ONodeH(Stack)
OParentx(Stack)=nodex
OParenty(Stack)=nodey
endif
endif
endif
endif
x=nodex+1 : y=nodey+1
if NodeW(x,y)=0 and NodeW(nodex+1,nodey)=0 and NodeW(nodex,nodey+1)=0
if NodeOC(x,y)=0
NodeOC(x,y)=1
Opens=Opens+1
NodeStack(x,y)=Opens
Openlistx(Opens)=x
Openlisty(Opens)=y
ONodeG(Opens)=14
ONodeH(Opens)=ManhattanHeuristic(x,y,endx,endy)*10
ONodeF(Opens)=ONodeG(Opens)+ONodeH(Opens)
OParentx(Opens)=nodex
OParenty(Opens)=nodey
else
if NodeOC(x,y)=1
Stack=NodeStack(x,y)
if CNodeG(Closed)+14<ONodeG(Stack)
ONodeG(Stack)=CNodeG(Closed)+14
ONodeF(Stack)=ONodeG(Stack)+ONodeH(Stack)
OParentx(Stack)=nodex
OParenty(Stack)=nodey
endif
endif
endif
endif
x=nodex : y=nodey+1
if NodeW(x,y)=0
if NodeOC(x,y)=0
NodeOC(x,y)=1
Opens=Opens+1
NodeStack(x,y)=Opens
Openlistx(Opens)=x
Openlisty(Opens)=y
ONodeG(Opens)=10
ONodeH(Opens)=ManhattanHeuristic(x,y,endx,endy)*10
ONodeF(Opens)=ONodeG(Opens)+ONodeH(Opens)
OParentx(Opens)=nodex
OParenty(Opens)=nodey
else
if NodeOC(x,y)=1
Stack=NodeStack(x,y)
if CNodeG(Closed)+10<ONodeG(Stack)
ONodeG(Stack)=CNodeG(Closed)+10
ONodeF(Stack)=ONodeG(Stack)+ONodeH(Stack)
OParentx(Stack)=nodex
OParenty(Stack)=nodey
endif
endif
endif
endif
x=nodex-1 : y=nodey+1
if NodeW(x,y)=0 and NodeW(nodex-1,nodey)=0 and NodeW(nodex,nodey+1)=0
if NodeOC(x,y)=0
NodeOC(x,y)=1
Opens=Opens+1
NodeStack(x,y)=Opens
Openlistx(Opens)=x
Openlisty(Opens)=y
ONodeG(Opens)=14
ONodeH(Opens)=ManhattanHeuristic(x,y,endx,endy)*10
ONodeF(Opens)=ONodeG(Opens)+ONodeH(Opens)
OParentx(Opens)=nodex
OParenty(Opens)=nodey
else
if NodeOC(x,y)=1
Stack=NodeStack(x,y)
if CNodeG(Closed)+14<ONodeG(Stack)
ONodeG(Stack)=CNodeG(Closed)+14
ONodeF(Stack)=ONodeG(Stack)+ONodeH(Stack)
OParentx(Stack)=nodex
OParenty(Stack)=nodey
endif
endif
endif
endif
x=nodex-1 : y=nodey
if NodeW(x,y)=0
if NodeOC(x,y)=0
NodeOC(x,y)=1
Opens=Opens+1
NodeStack(x,y)=Opens
Openlistx(Opens)=x
Openlisty(Opens)=y
ONodeG(Opens)=10
ONodeH(Opens)=ManhattanHeuristic(x,y,endx,endy)*10
ONodeF(Opens)=ONodeG(Opens)+ONodeH(Opens)
OParentx(Opens)=nodex
OParenty(Opens)=nodey
else
if NodeOC(x,y)=1
Stack=NodeStack(x,y)
if CNodeG(Closed)+10<ONodeG(Stack)
ONodeG(Stack)=CNodeG(Closed)+10
ONodeF(Stack)=ONodeG(Stack)+ONodeH(Stack)
OParentx(Stack)=nodex
OParenty(Stack)=nodey
endif
endif
endif
endif
x=nodex-1 : y=nodey-1
if NodeW(x,y)=0 and NodeW(nodex-1,nodey)=0 and NodeW(nodex,nodey-1)=0
if NodeOC(x,y)=0
NodeOC(x,y)=1
Opens=Opens+1
NodeStack(x,y)=Opens
Openlistx(Opens)=x
Openlisty(Opens)=y
ONodeG(Opens)=14
ONodeH(Opens)=ManhattanHeuristic(x,y,endx,endy)*10
ONodeF(Opens)=ONodeG(Opens)+ONodeH(Opens)
OParentx(Opens)=nodex
OParenty(Opens)=nodey
else
if NodeOC(x,y)=1
Stack=NodeStack(x,y)
if CNodeG(Closed)+14<ONodeG(Stack)
ONodeG(Stack)=CNodeG(Closed)+14
ONodeF(Stack)=ONodeG(Stack)+ONodeH(Stack)
OParentx(Stack)=nodex
OParenty(Stack)=nodey
endif
endif
endif
endif
Node=FindlowestOpen(Opens)
Nodex=Openlistx(Node)
Nodey=Openlisty(Node)
if Node=0 and finish=0 then noPath=1
until finish=1 or NoPath=1
if Nopath=0
nodex=endx
nodey=endy
Stack=NodeStack(nodex,nodey)
nodex=CParentx(Stack)
nodey=CParenty(Stack)
repeat
path(nodex,nodey)=1
Stack=NodeStack(nodex,nodey)
nodex=CParentx(Stack)
nodey=CParenty(Stack)
until nodex=startx and nodey=starty
else
cls
Print "No Path!!!!!"
suspend for key
endif
return
end
function linebox(x1,y1,x2,y2)
line x1,y1,x1,y2
line x1,y2,x2,y2
line x2,y2,x2,y1
line x2,y1,x1,y1
endfunction
function ManhattanHeuristic(startx,starty,endx,endy)
returnv=abs(startx-endx)+abs(starty-endy)
endfunction returnv
function FindlowestOpen(Opens)
cOpen=0
cF=1000000
for i=1 to Opens
if ONodeF(i)>-1
if ONodeF(i)<cF
cOpen=i
cF=ONodeF(i)
endif
endif
next i
endfunction cOpen
rem start
data 1,1,1,1,1,1,1,1,1
data 1,2,1,0,0,0,1,3,1
data 1,0,1,0,0,0,1,0,1
data 1,0,1,0,0,0,1,0,1
data 1,0,1,0,0,0,1,0,1
data 1,0,0,0,0,0,0,0,1
data 1,1,1,1,1,1,1,1,1
remend
data 1,1,1,1,1,1,1,1,1
data 1,0,0,0,0,0,1,3,1
data 1,0,0,0,0,0,1,0,1
data 1,0,0,0,0,0,1,0,1
data 1,0,0,0,0,0,1,0,1
data 1,2,0,0,0,0,0,0,1
data 1,1,1,1,1,1,1,1,1
There are two sets of data statements at the bottom, to view the other one, just turn
rem start
into
remstart
When the path is displayed, the arrow in the middle indicates the parent of that node, the number at the bottom left is the cost of moving from its parent node to it, the number at the bottom right is the heuristic and the number at the top is the two added together. If a node is in a yellow box, then it is on the open list, if it is in a purple box, then its is on the closed list. The green node is the start and the red one is the end.
Any help would greatly appreciated.
[EDIT] Found the problem - When looking at a node and finding out the cost of moving to that node, I wasn't adding the cost of moving to the previous node to the cost of moving to that node. Because of that, when trying to shorten the path it was only analyzing the cost of moving from node to node, and not the cost of moving from the start all the way to that node.
I know that this might not make sense to those of you who haven't made an A* Pathfinder, but the important thing is that it works.
What are the chemical formulae of:
Sodium Nitrate, Gallium, Manganese and Einsteinium
