My First PathFinding Program

Blitz3D Forums/Blitz3D Beginners Area/My First PathFinding Program

The forum is locked. These are the original blitzbasic.com forums, kept so they can still be read. No new topics, replies or edits can be made here. To join in, head over to the Blitz3D+ forums.

Xyle
 #1
It took me 3 weeks and about 100 hours to get here, but I feel like I'm close. Please post some comments to help me optimize or correct this.

there are some collision issues that stop player movement, but I'm tired.

Edit 10-26-06: Heres the version 4 with collision between character, scenary, and structures turned off
http://www.hilbily-works.com/Downloads1/PathFinding_TestV4.zip

You can download the exe file here http://www.hilbily-works.com/Downloads1/PathFinding_TestV3.zip

Thanks!!
Pathfinding testV3.bb is the main program
Globals.bb is an include for the Testv3.bb and the PathfindingV9.bb
PathfindingV9.bb is the main code for the pathfinding


This is the global.bb
;Global Entities
Global Ground
Global Player
Global Cam
Global CamPiv
Global Lite
Dim Trees(50)
Global House1
Global House2


;Collision Type
Global Type_Ground = 1
Global Type_Char = 2
Global Type_Scenery = 3
Global Type_Structure = 4
	
Global SelEnt
Global SelName$
Global Move$
Global BegX#
Global BegZ#
Global DestX#
Global DestZ#
Global Cursor

Global EntX
Global EntY
Global EntZ

Global FPS
Global frame_count
Global fps_timeout
Global TarX
Global TarZ


here is the PathfindingV9.bb code

Include "Globals.bb"

Type Cell
	Field ID
	Field X
	Field Z
	Field FVal
	Field GVal
	Field HVal
	Field Walkable
	Field Parent
	Field ListNum ;0 = Not listed, 1 = Open, 2 = Closed
End Type

Global GridX = 200
Global GridZ = 200
Global TotCells = (GridX+1) * (GridZ+1)
Dim Grid.Cell(TotCells) ;The Custom Type and Array was exampled by Stevie G, Awesome!
Global StartCell ;Stores the number of the Current Cell
Dim OpenList(1000) ;used to store cells that need to be checked
Dim ClosedList(1000) ;Used to store the path
Dim AdjCell(9) ;Used to store adjacent cell info
Global MoveCell

Dim Marker(1000)


;----------------------------------------------------------------------------------
Function CreateGrid(X,Z)
	;X and Z are map dimensions passed in, It will create a 2d grid containing (X+1 * Z+1) number
	;of cells. The +1 is for adding the 0 column and 0 row. I love MS Excel and VB, hehehe.
	A = 1
	CellX = (GridX/2) * -1
	CellZ = (GridZ/2)

	While A < TotCells+1
		If CellX < (GridX/2)+1
			Grid(A) = New Cell
			Grid(A)\ID = A
			Grid(A)\X = CellX
			Grid(A)\Z = CellZ
			Grid(A)\FVal = 0
			Grid(A)\GVal = 0
			Grid(A)\HVal = 0
			
			SelEnt = LinePick(CellX, 200, CellZ, 0, -1000, 0)
			If SelEnt > 0
				SelName$ = EntityName(PickedEntity())
				If SelName$ = "Ground"
					Grid(A)\Walkable = 1
				Else
					Grid(A)\Walkable = 0	
				EndIf
			EndIf
			
			
			Grid(A)\Parent = 0
			CellX = CellX + 1
			A = A + 1
		Else
			CellX = (GridX/2) * -1
			CellZ = CellZ - 1
		End If
	Wend
						
End Function
;----------------------------------------------------------------------------------

;----------------------------------------------------------------------------------
Function FindPath(StartX,StartZ,EndX,EndZ)
	
	;Clears any previously drawn path
	ClearPath()

	;Dim these variables to clear them
	Dim OpenList(1000) ;used to store cells that need to be checked
	Dim ClosedList(1000) ;Used to store the path
	
;1 - Move Char to nearest cell coords, if not there already
	A = 1
	While A < TotCells+1
		If Grid(A)\X < StartX + 1 And Grid(A)\X > StartX -1
			If Grid(A)\Z < StartZ + 1 And Grid(A)\Z > StartZ -1
				;The Character is within Grid(A)'s coords, Make this Cell, the Start Cell
				StartCell = A
				Grid(A)\Parent = A
				Grid(A)\ListNum = 0
				Grid(A)\FVal = 0
				Grid(A)\GVal = 0
				Grid(A)\HVal = 0
				Exit
			End If
		End If
		A = A + 1
	Wend
	
	;Add Starting Square to OpenList
	A = 1
	If Grid(StartCell)\ListNum = 0
		While A < TotCells
			If OpenList(A) = 0
				OpenList(A) = Grid(StartCell)\ID
				Grid(StartCell)\ListNum = 1 ;0 = Not listed, 1 = Open, 2 = Closed
				Exit
			EndIf
			A = A + 1
		Wend
	EndIf
		
	OListMT = 0
	TarFound = 0
	F = 0
	
Repeat
;2 Put Starting Cell in closed list, this is where the player is at before the movement begins
	;If the Target cell is added to the closed list then the path is complete
	


	TempCell = 0
	;Find the Cell with the lowest FCost in the Open list And Make it the New Start Cell
	;if there are no longer any cells on open list, then there is no path
	If F > 0 Then
		A = 1
		TempCell = OpenList(A)
		While OpenList(A) <> 0 ;Cycle through each item in open list looking for Grid ID with lowest Fval
			B = 1
			While OpenList(B) <> 0
				If Grid(TempCell)\FVal <= Grid(OpenList(B))\FVal
					StartCell = TempCell
				Else
					TempCell = OpenList(B)
					StartCell = TempCell
				EndIf
				B = B + 1
			Wend
			A = A + 1
		Wend
	EndIf
	
	
	;take current cell off open list
	A = 1
	While OpenList(A) <> 0
		If OpenList(A) = StartCell
			Grid(OpenList(A))\ListNum = 0
			OpenList(A) = 0
			Exit
		EndIf
		A = A + 1
	Wend
	
	;add current cell to closed list
	A = 1
	While A < TotCells
		If ClosedList(A) = 0
			ClosedList(A) = StartCell
			Grid(ClosedList(A))\ListNum = 2
			Exit
		EndIf
		A = A + 1
	Wend
	
	If Grid(StartCell)\X = EndX And Grid(StartCell)\Z = EndZ 
		;The Target cell has been found
		;Leave the loop
		TarFound = 1
	EndIf
	
;3 - Check each adjacent cell, Fill in each adjacent cells Fcost, Gcost, Hcost values
	;and parent values
	;Later, I may change the walkable to just check the adjacent cells instead of checking
	;the whole map at startup, I will try it and see what kind of slow down happens at runtime
	
	;Iterate through cells using current cell as a starting point to find which cells
	;are adjacent, we can find the left and right cells by subtracting 1 from StartCell for the cell
	;to the left and adding 1 to StartCell for the cell to the right
	AdjCell(4) = StartCell - 1
	Grid(AdjCell(4))\Parent = StartCell
	AdjCell(5) = StartCell + 1
	Grid(AdjCell(5))\Parent = StartCell
	
	A = StartCell-1
	While A > 0
		If Grid(A)\X = Grid(StartCell)\X - 1
			If Grid(A)\Z = Grid(StartCell)\Z + 1
				;This is the Adjacent Cell Diagonal, up, Left of Start Cell
				AdjCell(1) = A
				Grid(A)\Parent = StartCell
			EndIf
		EndIf
		If Grid(A)\X = Grid(StartCell)\X
			If Grid(A)\Z = Grid(StartCell)\Z + 1
				;This is the Adjacent cell above the Start Cell
				AdjCell(2) = A
				Grid(A)\Parent = StartCell
			EndIf
		EndIf
		If Grid(A)\X = Grid(StartCell)\X + 1
			If Grid(A)\Z = Grid(StartCell)\Z + 1
				;This is the adjacent cell to diagonal up right of Start Cell
				AdjCell(3) = A
				Grid(A)\Parent = StartCell
			End If
		End If
	
		A = A - 1
	Wend
	
	A = StartCell + 1
	While A < TotCells+1
		If Grid(A)\X = Grid(StartCell)\X -1
			If Grid(A)\Z = Grid(StartCell)\Z -1
				;This is the adjacent cell to diagonal Down Left of Start Cell
				AdjCell(6) = A
				Grid(A)\Parent = StartCell
			End If
		End If
		If Grid(A)\X = Grid(StartCell)\X
			If Grid(A)\Z = Grid(StartCell)\Z -1
				;This is the Adjacent cell Below the Start Cell
				AdjCell(7) = A
				Grid(A)\Parent = StartCell
			EndIf
		EndIf
		If Grid(A)\X = Grid(StartCell)\X +1
			If Grid(A)\Z = Grid(StartCell)\Z -1
				;This is the Adjacent Cell Diagonal, down, right of Start Cell
				AdjCell(8) = A
				Grid(A)\Parent = StartCell
			EndIf
		EndIf	
		A = A + 1
	Wend
	
	;Fill in GVal, HVal and FVal values and Create AdjCell() array
	;ABS Enlightenment goes to Kevin8084
	A = 1
	While A < 9
		If Grid(AdjCell(A))\X <> Grid(StartCell)\X And Grid(AdjCell(A))\Z <> Grid(StartCell)\Z ;Its Diagonal
			Grid(AdjCell(A))\GVal = 14*(Abs(Grid(AdjCell(A))\Z - Grid(StartCell)\Z))
		Else ;Its not Diagonal
			If Grid(AdjCell(A))\Z = Grid(StartCell)\Z And Grid(AdjCell(A))\X <> Grid(StartCell)\X
				Grid(AdjCell(A))\GVal = 10*(Abs(Grid(AdjCell(A))\X - Grid(StartCell)\X))
			Else
				Grid(AdjCell(A))\GVal = 10*(Abs(Grid(AdjCell(A))\Z - Grid(StartCell)\Z))
			EndIf	
		EndIf	
		Grid(AdjCell(A))\HVal = 10*(Abs(Grid(AdjCell(A))\X - EndX)) + 10*(Abs(Grid(AdjCell(A))\Z - EndZ))
		Grid(AdjCell(A))\FVal = Grid(AdjCell(A))\Gval + Grid(AdjCell(A))\HVal
		A = A + 1
	Wend
	
	;Add all walkable Adjacent Cells to the open list, if not on the closed list
	A = 1
	B = 1
	While A < 9
		If Grid(AdjCell(A))\Walkable = 1 And Grid(AdjCell(A))\ListNum = 0
			B = 1
			While B < TotCells
				If OpenList(B) = 0
					OpenList(B) = Grid(AdjCell(A))\ID
					Grid(AdjCell(A))\ListNum = 1 ;0 = Not listed, 1 = Open, 2 = Closed
					Exit
				EndIf
				B = B + 1
			Wend
		EndIf
		A = A + 1
	Wend

	F = F + 1
	
Until TarFound = 1 Or F > 1000
	
	;Clear Openlist values, else it will interfere with a future pathfind
	A = 1
	While OpenList(A) <> 0
		Grid(OpenList(A))\FVal = 0
		Grid(OpenList(A))\GVal = 0
		Grid(OpenList(A))\HVal = 0
		Grid(OpenList(A))\ListNum = 0
		A = A + 1
	Wend
	
	;Clear Closedlist values, else it will interfere with a future pathfind
	A = 1
	While ClosedList(A) <> 0
		Grid(ClosedList(A))\FVal = 0
		Grid(ClosedList(A))\GVal = 0
		Grid(ClosedList(A))\HVal = 0
		Grid(ClosedList(A))\ListNum = 0
		A = A + 1

	Wend
	
	ShowPath()
End Function
;----------------------------------------------------------------------------------

;----------------------------------------------------------------------------------
Function ShowPath() ;draws the path onscreen
	A = 1
	While ClosedList(A) <> 0
		Marker(A) = CreateSphere()
		PositionEntity Marker(A),Grid(ClosedList(A))\X,1,Grid(ClosedList(A))\Z
		ScaleEntity Marker(A),.5,.5,.5
		EntityColor Marker(A),255,0,0
		A = A + 1
	Wend
End Function
;----------------------------------------------------------------------------------

;----------------------------------------------------------------------------------
Function ClearPath() ;Clears a previously drawn path
	A = 1
	While ClosedList(A) <> 0
		FreeEntity Marker(A)
		A = A + 1
	Wend
End Function
;----------------------------------------------------------------------------------

;----------------------------------------------------------------------------------
Function WriteFiles()
	A = 1	
	;;Save info To check it
	FileOut = WriteFile("1-Nodeinfo.dat")
	WriteLine(FileOut,"Total Cells= " +TotCells)
	While A < TotCells+1
		WriteLine(FileOut, Grid(A)\ID+", "+Grid(A)\X+", "+Grid(A)\Z+", "+Grid(A)\Walkable+", "+Grid(A)\Parent)	
		A = A + 1
	Wend
	CloseFile FileOut
	
	;Save info To check it
	FileOut = WriteFile("1-OpenListInfo.dat")
	A = 1
	While OpenList(A) <> 0
		WriteLine(FileOut,OpenList(A)+": "+Grid(OpenList(A))\X+", "+Grid(OpenList(A))\Z+", "+Grid(OpenList(A))\ListNum+", "+Grid(OpenList(A))\Walkable+", "+Grid(OpenList(A))\GVal+", "+Grid(OpenList(A))\HVal+", "+Grid(OpenList(A))\FVal+", ")
		A = A + 1
	Wend
	CloseFile FileOut
	
	;Save info To check it
	FileOut = WriteFile("1-ClosedListInfo.dat")
	A = 1
	While ClosedList(A) <> 0
		WriteLine(FileOut,ClosedList(A)+": "+Grid(ClosedList(A))\X+", "+Grid(ClosedList(A))\Z+", "+Grid(ClosedList(A))\ListNum+", "+Grid(ClosedList(A))\Walkable+", "+Grid(ClosedList(A))\GVal+", "+Grid(ClosedList(A))\HVal+", "+Grid(ClosedList(A))\FVal+", ")
		A = A + 1
	Wend
	CloseFile FileOut

	;Save info To check it
	FileOut = WriteFile("1-PathLoops.dat")
	WriteLine(FileOut,F)
	CloseFile FileOut
End Function
;----------------------------------------------------------------------------------


and here is the main program Pathfinding TestV3.bb
Graphics3D 1024,768,32,1
SetBuffer BackBuffer()

Include "Globals.bb"
Include "PathfindingV9.bb"

;-------------FPS Stuff--------------------
SeedRnd MilliSecs()
fps_timer = CreateTimer(60) ; Lock to 60FPS.
;--------------end FPS Stuff----------------

CreateCursor()
CreateObjects()


Collisions Type_Char,Type_Ground,2,3
Collisions Type_Char,Type_Scenery,2,3
Collisions Type_Char,Type_Structure,2,3

CreateGrid(GridX,GridZ)		

While Not KeyHit(1)

	If KeyDown(13) ;+
		MoveEntity Cam,0,0,1
	EndIf
	If KeyDown(12) ;-
		MoveEntity Cam,0,0,-1
	EndIf
	If KeyDown(203) 
		TurnEntity CamPiv,0,3,0
	EndIf
	If KeyDown(205) 
		TurnEntity CamPiv,0,-3,0
	EndIf
	
	PositionEntity CamPiv,EntityX(Player,True),EntityY(Player,True),EntityZ(Player,True)
	
	If MouseHit(1)=True
		
			SelEnt = CameraPick(Cam,MouseX(),MouseY())
			If SelEnt > 0
				SelName$ = EntityName(PickedEntity())
				EntX = PickedX()
				EntY = PickedY()
				EntZ = PickedZ()
				If SelName$ = "Ground"
					BegX = EntityX(Player,True)
					BegZ = EntityZ(Player,True)
					DestX = PickedX#()
					DestZ = PickedZ#()
					FindPath(BegX,BegZ,DestX,DestZ)
					Move$ = "True"
					MoveCell = 0
				ElseIf Instr(SelName$,"Tree") > 0
					Move$ = "False"
				ElseIf Instr(SelName$, "House") > 0
					Move$ = "False"
				EndIf
			EndIf
	End If
	
	
	If Move$ = "True"
		BegX = EntityX(Player,True)
		BegZ = EntityZ(Player,True)
		If BegX < DestX + 1 And BegX > DestX - 1 And BegZ < DestZ + 1 And BegZ > DestZ -1
			Move$ = "False"
		EndIf
		
		
		
	
		If MoveCell <> 0
			If BegX < TarX + 1 And BegX > TarX - 1 And BegZ < TarZ + 1 And BegZ > TarZ -1
				A = 1
				While ClosedList(A) <> 0
					If ClosedList(A) = MoveCell
						A = A + 1
						If ClosedList(A) <> 0 Then MoveCell = ClosedList(A)
						Exit
					EndIf
					A = A + 1
				Wend
			EndIf
		Else
			A = 1
			While ClosedList(A) <> 0
				If ClosedList(A) 
					MoveCell = ClosedList(A)
					Exit
				EndIf
				A = A + 1
			Wend
		EndIf
		
		TarX = Grid(MoveCell)\X
		TarZ = Grid(MoveCell)\Z
		
		Point_Entity(Player,TarX,EntityY(Player,True),TarZ)
		
		MoveEntity Player,0,0,.05
		

	EndIf
	
	MoveEntity Player,0,-0.035,0 ; gravity 
	
	UpdateWorld
	RenderWorld
	
	
	
	Color 255,0,0
	frame_time = MilliSecs() - frame_start
	Show_FPS()
	Color 255,255,0
	Text 10,15,"Press esc to exit, Arrow keys turn cam, - and = zoom camera"
	Text 10,30,"Click an object to select it, click the ground to move"
	Color 255,255,255
	Text 10,90,"TotNodes = "+TotCells
	Text 10,105,"Sel Entity: "+SelName$+"("+EntX+","+EntY+","+EntZ+")"
	Text 10,120,"Ground Dims: "+MeshWidth(Ground) +", "+MeshHeight(Ground)+", "+MeshDepth(Ground)
	Text 10,135,"Ground Pos: "+EntityX(Ground,True)+", "+EntityZ(Ground,True)
	Text 10,150,"Tris: "+TrisRendered()
	Text 10,165,"Cam Coords: " +EntityX(Cam,True)+", "+EntityY(Cam,True)+", "+EntityZ(Cam,True)
	Text 10,180,"Player Coords: "+EntityX(Player,True)+", "+EntityY(Player,True)+", "+EntityZ(Player,True)
	Text 10,225,"MoveCell: "+MoveCell
	

	DrawImage Cursor,MouseX(),MouseY()
	Flip False
Wend

End

;------------------------------------------------------------------------CreateGround Function
Function CreateGround()
	Ground = CreateMesh()
	GSurf = CreateSurface(Ground)
	v0 = AddVertex(GSurf,-100,0,100)
	v1 = AddVertex(GSurf,100,0,-100)
	v2 = AddVertex(GSurf,-100,0,-100)
	v3 = AddVertex(GSurf,100,0,100)

	t0 = AddTriangle(GSurf,v0,v1,v2)
	t1 = AddTriangle(GSurf,v0,v3,v1)
End Function

;--------------------------------------------------------------------End CreateGround Function

;------------------------------------------------------------------------CreateObjects Function
Function CreateObjects()
	;CreateGround()
	Ground = CreateMesh()
	GSurf = CreateSurface(Ground)
	v0 = AddVertex(GSurf,-100,0,100)
	v1 = AddVertex(GSurf,100,0,-100)
	v2 = AddVertex(GSurf,-100,0,-100)
	v3 = AddVertex(GSurf,100,0,100)

	t0 = AddTriangle(GSurf,v0,v1,v2)
	t1 = AddTriangle(GSurf,v0,v3,v1)
	EntityColor Ground,130,95,0
	EntityPickMode Ground,2
	EntityType Ground,Type_Ground
	NameEntity Ground,"Ground"
		
	Player = CreateSphere()
	PositionEntity Player,0,2,0
	ScaleEntity Player,.5,1,.5
	EntityColor Player,255,255,0
	EntityRadius Player,2
	EntityPickMode Player,2
	EntityType Player,Type_Char
	NameEntity Player,"Player"
	
	House1 = CreateCube()
	PositionEntity House1,30,4,30
	ScaleEntity House1,5,3,10
	EntityColor House1,99,65,7
	EntityPickMode House1,2
	EntityType House1, Type_Structure
	NameEntity House1, "House1"
	
	House2 = CreateCube()
	PositionEntity House2,-30,4,-30
	ScaleEntity House2,5,3,10
	EntityColor House2,99,65,7
	EntityPickMode House2,2
	EntityType House2, Type_Structure
	NameEntity House2, "House2"
	
	A = 0
	B = 0
	While A <> 50
		Trees(A) = CreateCone()
		B = 0
		While B <> 1
			Tx = Rand(-100,100)
			Tz = Rand(-100,100)
			SelEnt = LinePick(Tx,50,Tz,0,-50,0)
			SelName$ = EntityName(PickedEntity())
			If SelName$ = "Ground"
				PositionEntity Trees(A),Tx,3,Tz
				B = 1
			EndIf
		Wend
		
		ScaleEntity Trees(A),2,2,2
		EntityColor Trees(A),0,128,0
		EntityPickMode Trees(A),2
		EntityType Trees(A), Type_Scenery
		NameEntity Trees(A), "Tree"+A
		A = A + 1
	Wend
		
	Cam = CreateCamera()
	PositionEntity Cam,-20,60,-20
	PointEntity Cam,Player
		
	CamPiv = CreatePivot()
	PositionEntity CamPiv,0,0,0
		
	EntityParent Cam,CamPiv
		
	Lite = CreateLight()
	PositionEntity Lite,0,50,0
	PointEntity Lite, Ground
End Function
;---------------------------------------------------------------------End CreateObjects Function

;------------------------------------------------------------------------Show_FPS Function

Function Show_FPS()
	If fps_timeout
		frame_count = frame_count + 1
		If MilliSecs() > fps_timeout Then
			fps_timeout = MilliSecs() + 1000 
			FPS = frame_count 
			frame_count = 0 
			If FPS < slowest_fps Or slowest_fps = 0 Then slowest_fps = FPS	
		EndIf 	
		If frame_time > slowest_frame Then slowest_frame = frame_time	
	Else
		; First call initialization.
		fps_timeout = MilliSecs() + 1000 
	EndIf
	Text 10,0,"FPS: " + FPS
End Function

;------------------------------------------------------------------------End Show_FPS Func


;-------------------------------------------------------------CreateCursor Func
Function CreateCursor()

	;Make a quick mouse cursor
	Color 255,255,0
	Cursor = CreateImage(15,15)
	Rect(0,0,10,10)
	Color 0,0,0
	Rect(3,3,10,10)
	Color 255,255,0
	Line(0,3,15,15)
	Line(0,2,15,15)
	Line(0,1,15,15)
	Line(0,0,15,15)
	Line(1,0,15,15)
	Line(2,0,15,15)
	Line(3,0,15,15)
	GrabImage Cursor,0,0

End Function
;-------------------------------------------------------------End CreateCursor Func

;-------------------------------------------------------------Point_Entity Func
Function Point_Entity(entity,x#,y#,z#)
	xdiff# = EntityX(entity)-x#
	ydiff# = EntityY(entity)-y#
	zdiff# = EntityZ(entity)-z#
	PEdist#=Sqr#((xdiff#*xdiff#)+(zdiff#*zdiff#))
	pitch# = ATan2(ydiff#,PEdist#)
	yaw# = ATan2(xdiff#,-zdiff#)
	RotateEntity entity,pitch#,yaw#,0
End Function
;-------------------------------------------------------------End Point_Entity