A Star Pathfinding

Monkey Forums/Monkey Code/A Star Pathfinding

Pakz	(Posted 2015) [#1]

Here is the A* pathfinding algorithm.

The code makes a map and creates and draws a path every second. (SetUpdateRate(1))

Here a screenshot :

Here a video of it running at SetUpdateRate(60) on a Intel N2820 laptop.

Here the code :

'A Star (A*) Pathfinding by R.b.v.Etten(Pakz)
'
' 
'
Import mojo

Const mapwidth:Int=40
Const mapheight:Int=30
Const tilewidth:Int=16
Const tileheight:Int=16
Global sx:Int 'path start x
Global sy:Int 'path start y
Global ex:Int 'path end x
Global ey:Int 'path end y
Global map:Int[mapwidth][]
Global olmap:Int[mapwidth][]
Global clmap:Int[mapwidth][]
Class openlist
    Field x:Int
    Field y:Int
    Field f:Int
    Field g:Int
    Field h:Int
    Field px:Int
    Field py:Int
    Method New(_x=0,_y=0,_f=0,_g=0,_h=0,_px=0,_py=0)
        x=_x
        y=_y
        f=_f
        g=_g
        h=_h
        px=_px
        py=_py
    End Method
End Class
Class closedlist
    Field x:Int
    Field y:Int
    Field px:Int
    Field py:Int 
    Method New(_x,_y,_px,_py)
        x = _x
        y = _y
        px = _px
        py = _py
    End Method
End Class
Class pathnode
    Field x:Int
    Field y:Int
    Method New(_x,_y)
        x = _x
        y = _y
    End Method
End Class

Global ol:List<openlist> = New List<openlist>
Global cl:List<closedlist> = New List<closedlist>
Global path:List<pathnode> = New List<pathnode>

Class MyGame Extends App

    Method OnCreate()
        SetUpdateRate(1)
        For Local i=0 Until mapwidth
            map[i] = New Int[mapheight]
            olmap[i] = New Int[mapheight]
            clmap[i] = New Int[mapheight]
        Next
        makemap
    End Method
    Method OnUpdate()        
        sx = Rnd(mapwidth)
        sy = Rnd(mapheight)
        ex = Rnd(mapwidth)
        ey = Rnd(mapheight)
        findpath
    End Method
    Method OnRender()
        Cls 0,0,0 
        SetColor 255,255,255
        drawmap        
        drawpath
        SetColor 255,255,255
        DrawText "a Star Pathfinding example.",0,0
    End Method
End Class

Function findpath:Bool()
    If sx = ex And sy = ey Then Return False
    For Local y=0 Until mapheight
    For Local x=0 Until mapwidth
        olmap[x][y] = 0
        clmap[x][y] = 0
    Next
    Next
    ol.Clear
    cl.Clear
    path.Clear
    ol.AddFirst(New openlist(sx,sy))
    Local tx:Int
    Local ty:Int
    Local tf:Int
    Local tg:Int
    Local th:Int
    Local tpx:Int
    Local tpy:Int
    Local newx:Int
    Local newy:Int
    Local lowestf:Int
    olmap[sx][sy] = 1
    Repeat
        If ol.IsEmpty() = True Then Return False
        lowestf = 100000
        For Local i:=Eachin ol
            If i.f < lowestf
                lowestf = i.f
                tx = i.x
                ty = i.y
                tf = i.f
                tg = i.g
                th = i.h
                tpx = i.px
                tpy = i.py
            End If
        Next
        If tx = ex And ty = ey
            cl.AddLast(New closedlist(tx,ty,tpx,tpy))
            findpathback
            Return True
        Else
            removefromopenlist(tx,ty)
            olmap[tx][ty] = 0
            clmap[tx][ty] = 1
            cl.AddLast(New closedlist(tx,ty,tpx,tpy))
            For Local y=-1 To 1
            For Local x=-1 To 1
                newx = tx+x
                newy = ty+y
                If newx>=0 And newy>=0 And newx<mapwidth And newy<mapheight
                If olmap[newx][newy] = 0
                If clmap[newx][newy] = 0
                    olmap[newx][newy] = 1
                    Local gg = map[newx][newy]+1
                    Local hh = distance(newx,newy,ex,ey)
                    Local ff = gg+hh
                    ol.AddLast(New openlist(newx,newy,ff,gg,hh,tx,ty))
                End If
                End If
                End If
            Next
            Next
        End If
    Forever
    Return False
End Function

Function drawpath:Void()
    Local cnt:Int=1
    For Local i:=Eachin path
        SetColor 255,255,0
        DrawOval i.x*tilewidth,i.y*tileheight,4,4
        SetColor 255,255,255
        DrawText cnt,i.x*tilewidth,i.y*tileheight
        cnt+=1
    Next
End Function

Function findpathback:Bool()
    Local x=ex
    Local y=ey
    path.AddFirst(New pathnode(x,y))
    Repeat
        For Local i:=Eachin cl
            If i.x = x And i.y = y
                x = i.px
                y = i.py
                path.AddFirst(New pathnode(x,y))
            End If
        Next
        If x = sx And y = sy Then Return True
    Forever    
End Function

Function removefromopenlist:Void(x1:Int,y1:Int)
    For Local i:=Eachin ol
        If i.x = x1 And i.y = y1
            ol.Remove i
            Exit
        End If
    Next
End Function

Function drawmap:Void()
    For Local y=0 Until mapheight
    For Local x=0 Until mapwidth
        SetColor 0,map[x][y]*10,0
        DrawRect x*tilewidth,y*tileheight,tilewidth,tileheight
    Next
    Next
End Function

Function makemap:Void()
    For Local y=0 Until mapheight
    For Local x=0 Until mapwidth
        map[x][y] = 0
    Next
    Next
    Local lowest = 0
    While lowest < 13
        Local x1 = Rnd(mapwidth)
        Local y1 = Rnd(mapheight)
        Local radius = Rnd(3,6)
        For Local y2=-radius To radius
        For Local x2=-radius To radius
            If ((x2*x2)+(y2*y2)) <= radius*radius+radius*0.8
                Local x3 = x1+x2
                Local y3 = y1+y2
                If x3>=0 And y3>=0 And x3<mapwidth And y3<mapheight
                    map[x3][y3]=map[x3][y3]+1
                    If map[x3][y3] > lowest Then lowest = map[x3][y3]
                End If
            End If
        Next
        Next
    Wend
End Function

Function distance:Int(x1:Int,y1:Int,x2:Int,y2:Int)
    Return Abs(x2-x1)+Abs(y2-y1)
End Function

Function Main()
    New MyGame()
End Function

Pakz	(Posted 2015) [#2]

Here is the A* pathfinding algorithm.

The code makes a map and creates and draws a path every second. (SetUpdateRate(1))

Here a screenshot :

Here a video of it running at SetUpdateRate(60) on a Intel N2820 laptop:

Here the code :

'A Star (A*) Pathfinding by R.b.v.Etten(Pakz)
'
' 
'
Import mojo

Const mapwidth:Int=40
Const mapheight:Int=30
Const tilewidth:Int=16
Const tileheight:Int=16
Global sx:Int 'path start x
Global sy:Int 'path start y
Global ex:Int 'path end x
Global ey:Int 'path end y
Global map:Int[mapwidth][]
Global olmap:Int[mapwidth][]
Global clmap:Int[mapwidth][]
Class openlist
    Field x:Int
    Field y:Int
    Field f:Int
    Field g:Int
    Field h:Int
    Field px:Int
    Field py:Int
    Method New(_x=0,_y=0,_f=0,_g=0,_h=0,_px=0,_py=0)
        x=_x
        y=_y
        f=_f
        g=_g
        h=_h
        px=_px
        py=_py
    End Method
End Class
Class closedlist
    Field x:Int
    Field y:Int
    Field px:Int
    Field py:Int 
    Method New(_x,_y,_px,_py)
        x = _x
        y = _y
        px = _px
        py = _py
    End Method
End Class
Class pathnode
    Field x:Int
    Field y:Int
    Method New(_x,_y)
        x = _x
        y = _y
    End Method
End Class

Global ol:List<openlist> = New List<openlist>
Global cl:List<closedlist> = New List<closedlist>
Global path:List<pathnode> = New List<pathnode>

Class MyGame Extends App

    Method OnCreate()
        SetUpdateRate(1)
        For Local i=0 Until mapwidth
            map[i] = New Int[mapheight]
            olmap[i] = New Int[mapheight]
            clmap[i] = New Int[mapheight]
        Next
        makemap
    End Method
    Method OnUpdate()        
        sx = Rnd(mapwidth)
        sy = Rnd(mapheight)
        ex = Rnd(mapwidth)
        ey = Rnd(mapheight)
        findpath
    End Method
    Method OnRender()
        Cls 0,0,0 
        SetColor 255,255,255
        drawmap        
        drawpath
        SetColor 255,255,255
        DrawText "a Star Pathfinding example.",0,0
    End Method
End Class

Function findpath:Bool()
    If sx = ex And sy = ey Then Return False
    For Local y=0 Until mapheight
    For Local x=0 Until mapwidth
        olmap[x][y] = 0
        clmap[x][y] = 0
    Next
    Next
    ol.Clear
    cl.Clear
    path.Clear
    ol.AddFirst(New openlist(sx,sy))
    Local tx:Int
    Local ty:Int
    Local tf:Int
    Local tg:Int
    Local th:Int
    Local tpx:Int
    Local tpy:Int
    Local newx:Int
    Local newy:Int
    Local lowestf:Int
    olmap[sx][sy] = 1
    Repeat
        If ol.IsEmpty() = True Then Return False
        lowestf = 100000
        For Local i:=Eachin ol
            If i.f < lowestf
                lowestf = i.f
                tx = i.x
                ty = i.y
                tf = i.f
                tg = i.g
                th = i.h
                tpx = i.px
                tpy = i.py
            End If
        Next
        If tx = ex And ty = ey
            cl.AddLast(New closedlist(tx,ty,tpx,tpy))
            findpathback
            Return True
        Else
            removefromopenlist(tx,ty)
            olmap[tx][ty] = 0
            clmap[tx][ty] = 1
            cl.AddLast(New closedlist(tx,ty,tpx,tpy))
            For Local y=-1 To 1
            For Local x=-1 To 1
                newx = tx+x
                newy = ty+y
                If newx>=0 And newy>=0 And newx<mapwidth And newy<mapheight
                If olmap[newx][newy] = 0
                If clmap[newx][newy] = 0
                    olmap[newx][newy] = 1
                    Local gg = map[newx][newy]+1
                    Local hh = distance(newx,newy,ex,ey)
                    Local ff = gg+hh
                    ol.AddLast(New openlist(newx,newy,ff,gg,hh,tx,ty))
                End If
                End If
                End If
            Next
            Next
        End If
    Forever
    Return False
End Function

Function drawpath:Void()
    Local cnt:Int=1
    For Local i:=Eachin path
        SetColor 255,255,0
        DrawOval i.x*tilewidth,i.y*tileheight,4,4
        SetColor 255,255,255
        DrawText cnt,i.x*tilewidth,i.y*tileheight
        cnt+=1
    Next
End Function

Function findpathback:Bool()
    Local x=ex
    Local y=ey
    path.AddFirst(New pathnode(x,y))
    Repeat
        For Local i:=Eachin cl
            If i.x = x And i.y = y
                x = i.px
                y = i.py
                path.AddFirst(New pathnode(x,y))
            End If
        Next
        If x = sx And y = sy Then Return True
    Forever    
End Function

Function removefromopenlist:Void(x1:Int,y1:Int)
    For Local i:=Eachin ol
        If i.x = x1 And i.y = y1
            ol.Remove i
            Exit
        End If
    Next
End Function

Function drawmap:Void()
    For Local y=0 Until mapheight
    For Local x=0 Until mapwidth
        SetColor 0,map[x][y]*10,0
        DrawRect x*tilewidth,y*tileheight,tilewidth,tileheight
    Next
    Next
End Function

Function makemap:Void()
    For Local y=0 Until mapheight
    For Local x=0 Until mapwidth
        map[x][y] = 0
    Next
    Next
    Local lowest = 0
    While lowest < 13
        Local x1 = Rnd(mapwidth)
        Local y1 = Rnd(mapheight)
        Local radius = Rnd(3,6)
        For Local y2=-radius To radius
        For Local x2=-radius To radius
            If ((x2*x2)+(y2*y2)) <= radius*radius+radius*0.8
                Local x3 = x1+x2
                Local y3 = y1+y2
                If x3>=0 And y3>=0 And x3<mapwidth And y3<mapheight
                    map[x3][y3]=map[x3][y3]+1
                    If map[x3][y3] > lowest Then lowest = map[x3][y3]
                End If
            End If
        Next
        Next
    Wend
End Function

Function distance:Int(x1:Int,y1:Int,x2:Int,y2:Int)
    Return Abs(x2-x1)+Abs(y2-y1)
End Function

Function Main()
    New MyGame()
End Function

CGV	(Posted 2015) [#3]

Nice work.
Is it possible to switch off diagonals?
In some types of games you only want horizontal or vertical moves.

Pakz	(Posted 2015) [#4]

Here is a modified version of the code with horizontal and vertical moves.

'A Star (A*) Pathfinding by R.b.v.Etten(Pakz)
'
' horizontal/vertical movement version (4 directions)
'
Import mojo

Const mapwidth:Int=40
Const mapheight:Int=30
Const tilewidth:Int=16
Const tileheight:Int=16
Global sx:Int 'path start x
Global sy:Int 'path start y
Global ex:Int 'path end x
Global ey:Int 'path end y
Global map:Int[mapwidth][]
Global olmap:Int[mapwidth][]
Global clmap:Int[mapwidth][]
Class openlist
    Field x:Int
    Field y:Int
    Field f:Int
    Field g:Int
    Field h:Int
    Field px:Int
    Field py:Int
    Method New(_x=0,_y=0,_f=0,_g=0,_h=0,_px=0,_py=0)
        x=_x
        y=_y
        f=_f
        g=_g
        h=_h
        px=_px
        py=_py
    End Method
End Class
Class closedlist
    Field x:Int
    Field y:Int
    Field px:Int
    Field py:Int 
    Method New(_x,_y,_px,_py)
        x = _x
        y = _y
        px = _px
        py = _py
    End Method
End Class
Class pathnode
    Field x:Int
    Field y:Int
    Method New(_x,_y)
        x = _x
        y = _y
    End Method
End Class

Global ol:List<openlist> = New List<openlist>
Global cl:List<closedlist> = New List<closedlist>
Global path:List<pathnode> = New List<pathnode>

Class MyGame Extends App

    Method OnCreate()
        SetUpdateRate(1)
        For Local i=0 Until mapwidth
            map[i] = New Int[mapheight]
            olmap[i] = New Int[mapheight]
            clmap[i] = New Int[mapheight]
        Next
        makemap
    End Method
    Method OnUpdate()        
        sx = Rnd(mapwidth)
        sy = Rnd(mapheight)
        ex = Rnd(mapwidth)
        ey = Rnd(mapheight)
        findpath
    End Method
    Method OnRender()
        Cls 0,0,0 
        SetColor 255,255,255
        drawmap        
        drawpath
        SetColor 255,255,255
        DrawText "a Star Pathfinding example.",0,0
    End Method
End Class

Function findpath:Bool()
    If sx = ex And sy = ey Then Return False
    For Local y=0 Until mapheight
    For Local x=0 Until mapwidth
        olmap[x][y] = 0
        clmap[x][y] = 0
    Next
    Next
    ol.Clear
    cl.Clear
    path.Clear
    ol.AddFirst(New openlist(sx,sy))
    Local tx:Int
    Local ty:Int
    Local tf:Int
    Local tg:Int
    Local th:Int
    Local tpx:Int
    Local tpy:Int
    Local newx:Int
    Local newy:Int
    Local lowestf:Int
    olmap[sx][sy] = 1
    Repeat
        If ol.IsEmpty() = True Then Return False
        lowestf = 100000
        For Local i:=Eachin ol
            If i.f < lowestf
                lowestf = i.f
                tx = i.x
                ty = i.y
                tf = i.f
                tg = i.g
                th = i.h
                tpx = i.px
                tpy = i.py
            End If
        Next
        If tx = ex And ty = ey
            cl.AddLast(New closedlist(tx,ty,tpx,tpy))
            findpathback
            Return True
        Else
            removefromopenlist(tx,ty)
            olmap[tx][ty] = 0
            clmap[tx][ty] = 1
            cl.AddLast(New closedlist(tx,ty,tpx,tpy))
            For Local y=-1 To 1
            For Local x=-1 To 1
                newx = tx+x
                newy = ty+y
                If newx>=0 And newy>=0 And newx<mapwidth And newy<mapheight
                If olmap[newx][newy] = 0
                If clmap[newx][newy] = 0
                    olmap[newx][newy] = 1
                    Local gg = map[newx][newy]+1
                    Local hh = distance(newx,newy,ex,ey)
                    Local ff = gg+hh
                    ol.AddLast(New openlist(newx,newy,ff,gg,hh,tx,ty))
                End If
                End If
                End If
            Next
            Next
        End If
    Forever
    Return False
End Function

Function drawpath:Void()
    Local cnt:Int=1
    For Local i:=Eachin path
        SetColor 255,255,0
        DrawOval i.x*tilewidth,i.y*tileheight,4,4
        SetColor 255,255,255
        DrawText cnt,i.x*tilewidth,i.y*tileheight
        cnt+=1
    Next
End Function

Function findpathback:Bool()
    Local x=ex
    Local y=ey
    path.AddFirst(New pathnode(x,y))
    Repeat
        For Local i:=Eachin cl
            If i.x = x And i.y = y
                x = i.px
                y = i.py
                path.AddFirst(New pathnode(x,y))
            End If
        Next
        If x = sx And y = sy Then Return True
    Forever    
End Function

Function removefromopenlist:Void(x1:Int,y1:Int)
    For Local i:=Eachin ol
        If i.x = x1 And i.y = y1
            ol.Remove i
            Exit
        End If
    Next
End Function

Function drawmap:Void()
    For Local y=0 Until mapheight
    For Local x=0 Until mapwidth
        SetColor 0,map[x][y]*10,0
        DrawRect x*tilewidth,y*tileheight,tilewidth,tileheight
    Next
    Next
End Function

Function makemap:Void()
	Local diag:Int[]=[-1,0 ,1,0 ,0,-1 ,0,1]
    For Local y=0 Until mapheight
    For Local x=0 Until mapwidth
        map[x][y] = 0
    Next
    Next
    Local lowest = 0
    While lowest < 13
        Local x1 = Rnd(mapwidth)
        Local y1 = Rnd(mapheight)
        Local radius = Rnd(3,6)		
		For Local i=0 Until 7 Step 2
			Local x2 = diag[i]
			Local y2 = diag[i+1]
            If ((x2*x2)+(y2*y2)) <= radius*radius+radius*0.8
                Local x3 = x1+x2
                Local y3 = y1+y2
                If x3>=0 And y3>=0 And x3<mapwidth And y3<mapheight
                    map[x3][y3]=map[x3][y3]+1
                    If map[x3][y3] > lowest Then lowest = map[x3][y3]
                End If
            End If
            Next
    Wend
End Function

Function distance:Int(x1:Int,y1:Int,x2:Int,y2:Int)
    Return Abs(x2-x1)+Abs(y2-y1)
End Function

Function Main()
    New MyGame()
End Function

[VOLOFORCE]

(Posted May) [#5]

That's great. In case the programmer is still available why is there:

Local gg = map[newx][newy]+1

Could you explain why you add 1?

Pakz	(Posted May) [#6]

gg is the go to cost.

It was a time ago that I long ago worked with it but I thought I converted it from the book I had.

You could try removing the +1 and see if it works then. The terrain cost is added in that line. I think that the +1 is needed though.

[VOLOFORCE]

(Posted May) [#7]

Thanks. I changed the number a bit but the result was always the same. It's a pretty neat implementation of the algorithm, I must say. I have ported it also to C#.

Is it possible to modify the code so that the terrain cost can be modifed by the direction you enter it, e.g. for height levels, coming from height 2 to 4 (more costs) or coming from 4 to 2 (less costs).

Pakz	(Posted May) [#8]

Is it possible to modify the code so that the terrain cost can be modifed by the direction you enter it, e.g. for height levels, coming from height 2 to 4 (more costs) or coming from 4 to 2 (less costs).

I am not quite understanding what you are writing here. The map value (each cell) has it's own height value. It reads that value and then adds this to the list. It then calculates the lowest value in the code and choses this as the next step.
On the map you assign each grid value with a height value. On the image in this thread the lighter colors are higher height values and the darker colors are lower step values. If the image was one color then all map values would be have the same cost value.

Edit: The way it works is that it floods the map (think water) and it flows into the lowest area first. It also bases it next flow direction on the distance of the current position and the target position. Think of map() of a heightmap with each cell having a height.

[VOLOFORCE]

(Posted May) [#9]

I am just stating that e.g. heights work different from terrain type. With heights it depends on the direction you enter the square, with terrain it needn't matter, as e.g.street would be cost 1, mud would be 3, rock would be 5, but regardless of the direction you enter the square. So I suppose the first case is included, while the second one would be done by simply adding the cost.

[3][5][1]
So entering from 3 to 5 would cost less than from 1 to 5?

Pakz	(Posted May) [#10]

The height of the square you enter gets added. It does not matter what the height is on the tile you are on. So from 3 to 5 costs 5. And from 1 to 5 costs 5.

[VOLOFORCE]

(Posted May) [#11]

Yes, that's how I thought it works. So I will have to modify it.

[VOLOFORCE]

(Posted May) [#12]

For Local y=-1 To 1
For Local x=-1 To 1
newx = tx+x
newy = ty+y
If newx>=0 And newy>=0 And newx<mapwidth And newy<mapheight
If olmap[newx][newy] = 0
If clmap[newx][newy] = 0
olmap[newx][newy] = 1
Local gg = map[newx][newy]+1
Local hh = distance(newx,newy,ex,ey)
Local ff = gg+hh
ol.AddLast(New openlist(newx,newy,ff,gg,hh,tx,ty))
End If
End If
End If

So, Pakz, do you think it will work when I compare the height of tx/ty with that of newx and newx...

so kinda like

if height[tx][ty] < height[newx][newy] then gg = gg + 5

I have the feeling that that would be too easy :)

Pakz	(Posted May) [#13]

You can always make special cases with which to add the gg cost. I am still not too sure what it is for but I understand a little bit better what you mean now.