Breadth-first pathfinder

This code has been declared by its author to be Public Domain code.

Breadth-first pathfinder by Andres

2008

Here's an easy to use pathfinder, made it as portable as possible. Uses only 4 functions. Supports 2D and 3D maps (map's depth).
The pathmap is filled with flags like:
PATH_TOP, PATH_BOTTOM, PATH_LEFT, PATH_RIGHT, PATH_CEIL, PATH_FLOOR, PATH_ALL

As you can see it doesn't support just 1/0 maps, but also can handle walls (hope you understand what i mean :)

The path returned by FindPath() is a string (something like "11444422222222223331123") where 1 - north, 2 - south, 3 - west, 4 - east, 5 - up, 6 - down.

Functions:

CreatePathmap:TPathmap(width:Long, height:Long, depth:Long = 1)
PathmapSlot(this:TPathmap, flags:Int, x:Long, y:Long, z:Long = 0) ' Used to fill pathmap with information
FindPath:String(this:TPathmap, x1:Long, y1:Long, z1:Long, x2:Long, y2:Long, z2:Long) ' finds the path in a pathmap from position x1, y1, z1 to x2, y2, z2
FreePathmap(this:TPathmap)

Example:

SuperStrict

Graphics(800, 600)
SeedRnd(MilliSecs())

Global map_width:Int = 80
Global map_height:Int = 60

Include "pathfind.bmx"

' Create level
Global map:Int[map_width, map_height]
For Local y:Int = 0 To map_height -1
	For Local x:Int = 0 To map_width - 1
		If Rand(0, 5) < 2 Then map[x, y] = PATH_ALL
	Next
Next

' Create pathmap
Global pathmap:TPathmap = CreatePathmap:TPathmap(map_width, map_height)
Global path:String = ""

' Copy level to pathmap
For Local y:Long = 0 To map_height -1
	For Local x:Long = 0 To map_width - 1
		PathmapSlot(pathmap, map[x, y], x, y)
	Next
Next

While Not KeyHit(KEY_ESCAPE)
	Cls()
	
	' Draw level
	For Local y:Int = 0 To map_height -1
		For Local x:Int = 0 To map_width - 1
			If CheckFlag(map[x, y], PATH_ALL)
				SetColor(255, 0, 0)
			Else
				SetColor(255, 255, 255)
			EndIf
			DrawRect(x * 10, y * 10, 10, 10)
		Next
	Next
	
	' 1 - north, 2 - south, 3 - west, 4 - east, 5 - up, 6 - down
	path = FindPath(pathmap, map_width / 2, map_height / 2, 0, MouseX() / 10.0, MouseY() / 10.0, 0)
	
	' Draw the PATH
	If Len(path) > 0
		SetColor(0, 0, 255)
		Local x:Long = map_width / 2, y:Long = map_height / 2
		For Local i:Int = 1 To Len(path)
			Select Mid(path, i, 1)
				Case "1"
					y :- 1
				Case "2"
					y :+ 1
				Case "3"
					x :- 1
				Case "4"
					x :+ 1
			End Select
			DrawOval(x * 10, y * 10, 10, 10)
		Next
	EndIf
	
	Flip()
Wend

FreePathmap(pathmap)

Const FLAG1:Int = $01
Const FLAG2:Int = $02
Const FLAG4:Int = $04
Const FLAG8:Int = $08
Const FLAG16:Int = $10
Const FLAG32:Int = $20
Const FLAG64:Int = $40
Const FLAG128:Int = $80

Function CheckFlag:Int(flags:Int, flag:Int)
	If flags & flag Then Return True Else Return False
End Function

Const PATH_TOP:Int = FLAG1
Const PATH_BOTTOM:Int = FLAG2
Const PATH_LEFT:Int = FLAG4
Const PATH_RIGHT:Int = FLAG8
Const PATH_CEIL:Int = FLAG16
Const PATH_FLOOR:Int = FLAG32
Const PATH_ALL:Int = FLAG64

' List used for path find
Global pathmap_path_list:TList = CreateList()

' Pathmap type
Type TPathmap
	Field width:Long, height:Long, depth:Long
	
	Field map:TBank
End Type

Type TPath
	Field x:Long, y:Long, z:Long
	Field path:String
End Type

' Create a new pathmap
Function CreatePathmap:TPathmap(width:Long, height:Long, depth:Long = 1)
	Local this:TPathmap = New TPathmap
		this.width = width
		this.height = height
		this.depth = depth
		
		this.map = CreateBank(this.width * this.height * this.depth)
		
	Return this
End Function

' Insert flags to pathmap slot
Function PathmapSlot(this:TPathmap, flags:Int, x:Long, y:Long, z:Long = 0)
	Local offset:Long = (z * this.width * this.height) + (y * this.width) + x
	PokeByte(this.map, offset, flags)
End Function

' Search for a path within pathmap; 1 - north, 2 - south, 3 - west, 4 - east, 5 - up, 6 - down
Function FindPath:String(this:TPathmap, x1:Long, y1:Long, z1:Long, x2:Long, y2:Long, z2:Long)
	Local offset:Long, offset_top:Long, offset_bottom:Long, offset_left:Long, offset_right:Long, offset_ceil:Long, offset_floor:Long
	
	Local temp_bank:TBank = CreateBank(BankSize(this.map))
	CopyBank(this.map, 0, temp_bank, 0, BankSize(this.map))
	
	ClearList(pathmap_path_list)
	Local that:TPath = New TPath
		that.x = x1
		that.y = y1
		that.z = z1
		that.path = ""
		ListAddFirst(pathmap_path_list, that)
		ListAddFirst(pathmap_path_list, that)
	
	For that:TPath = EachIn pathmap_path_list
		offset = (that.z * this.width * this.height) + (that.y * this.width) + that.x
		
		' TOP
		If that.y > 0
			offset_top = (that.z * this.width * this.height) + ((that.y - 1) * this.width) + that.x
			If Not CheckFlag(PeekByte(temp_bank, offset), PATH_TOP) And Not CheckFlag(PeekByte(temp_bank, offset_top), PATH_BOTTOM) And Not CheckFlag(PeekByte(temp_bank, offset_top), PATH_ALL)
				Local pth:TPath = New TPath
					pth.x = that.x
					pth.y = that.y - 1
					pth.z = that.z
					pth.path = that.path + "1"
					ListAddLast(pathmap_path_list, pth)
					PokeByte(temp_bank, offset_top, PATH_ALL)
			EndIf
		EndIf
		' LEFT
		If that.x > 0
			offset_left = (that.z * this.width * this.height) + (that.y * this.width) + that.x - 1
			If Not CheckFlag(PeekByte(temp_bank, offset), PATH_LEFT) And Not CheckFlag(PeekByte(temp_bank, offset_left), PATH_RIGHT) And Not CheckFlag(PeekByte(temp_bank, offset_left), PATH_ALL)
				Local pth:TPath = New TPath
					pth.x = that.x - 1
					pth.y = that.y
					pth.z = that.z
					pth.path = that.path + "3"
					ListAddLast(pathmap_path_list, pth)
					PokeByte(temp_bank, offset_left, PATH_ALL)
			EndIf
		EndIf
		' BOTTOM
		If that.y + 1 < this.height
			offset_bottom = (that.z * this.width * this.height) + ((that.y + 1) * this.width) + that.x
			If Not CheckFlag(PeekByte(temp_bank, offset), PATH_BOTTOM) And Not CheckFlag(PeekByte(temp_bank, offset_bottom), PATH_TOP) And Not CheckFlag(PeekByte(temp_bank, offset_bottom), PATH_ALL)
				Local pth:TPath = New TPath
					pth.x = that.x
					pth.y = that.y + 1
					pth.z = that.z
					pth.path = that.path + "2"
					ListAddLast(pathmap_path_list, pth)
					PokeByte(temp_bank, offset_bottom, PATH_ALL)
			EndIf
		EndIf
		' RIGHT
		If that.x + 1 < this.width
			offset_right = (that.z * this.width * this.height) + (that.y * this.width) + that.x + 1
			If Not CheckFlag(PeekByte(temp_bank, offset), PATH_RIGHT) And Not CheckFlag(PeekByte(temp_bank, offset_right), PATH_LEFT) And Not CheckFlag(PeekByte(temp_bank, offset_right), PATH_ALL)
				Local pth:TPath = New TPath
					pth.x = that.x + 1
					pth.y = that.y
					pth.z = that.z
					pth.path = that.path + "4"
					ListAddLast(pathmap_path_list, pth)
					PokeByte(temp_bank, offset_right, PATH_ALL)
			EndIf
		EndIf
		
		' If the pathmap has 3 dimensions
		If this.depth > 1
			If that.z > 0
				offset_ceil = ((that.z - 1) * this.width * this.height) + (that.y * this.width) + that.x
				If Not CheckFlag(PeekByte(temp_bank, offset), PATH_CEIL) And Not CheckFlag(PeekByte(temp_bank, offset_ceil), PATH_FLOOR) And Not CheckFlag(PeekByte(temp_bank, offset_ceil), PATH_ALL)
					Local pth:TPath = New TPath
						pth.x = that.x
						pth.y = that.y - 1
						pth.z = that.z
						pth.path = that.path + "5"
						ListAddLast(pathmap_path_list, pth)
						PokeByte(temp_bank, offset_ceil, PATH_ALL)
				EndIf
			EndIf
			If that.z + 1< this.depth
				offset_floor = ((that.z + 1) * this.width * this.height) + (that.y * this.width) + that.x
				If Not CheckFlag(PeekByte(temp_bank, offset), PATH_FLOOR) And Not CheckFlag(PeekByte(temp_bank, offset_floor), PATH_CEIL) And Not CheckFlag(PeekByte(temp_bank, offset_floor), PATH_ALL)
					Local pth:TPath = New TPath
						pth.x = that.x
						pth.y = that.y + 1
						pth.z = that.z
						pth.path = that.path + "6"
						ListAddLast(pathmap_path_list, pth)
						PokeByte(temp_bank, offset_floor, PATH_ALL)
				EndIf
			EndIf
		EndIf
		
		' CORRECT PATH FOUND
		If that.x = x2 And that.y = y2 And that.z = z2
			Local path:String = that.path
			ClearList(pathmap_path_list)
			temp_bank = Null
			Return path
		EndIf
		
		' Return false if there is no solution
		If ListIsEmpty(pathmap_path_list) Then
			temp_bank = Null
			Return "[ERROR]"
		EndIf
				
		' Remove the parent
		ListRemove(pathmap_path_list, that)
		that = Null
	Next
	Return "FAILED"
End Function

' Remove pathmap from memory
Function FreePathmap(this:TPathmap)
	this.map = Null
End Function

Comments

markcw

2008

That was cool, Andres.

Code Archives Forum