Code archives/3D Graphics - Misc/A* Pathfinding

; **************************************************************************************************
; A* Pathfinding tutorial by Kevin Lee Legge (kevin8084@gmail.com)
; **************************************************************************************************
;
; First of all, what IS A* Pathfinding? In a nutshell, it is an algorithm that allows an AI to
; find the shortest (or ALMOST shortest) path from point A to point B, avoiding any obstacles
; that may be in the way.
; The most important equation in the algorithm is F = G * H
; G = the movement cost from the current location to the next, adjacent, location. If the next
; location is diagonal to the current location, then the G cost is 14. If the next location is
; either horizontal or vertical to the current location, then the G cost is 10. One important point
; to remember is that the G costs add up. Meaning that if the child is horizontal to its parent,
; for example, its G cost is 10 ADDED to its parent's G cost. So, if the parent has a G cost of
; 14, then the child's total G cost is 24 (this is the movement cost "in total" from the
; starting square.)
; H = the Heuristic cost (heuristic, meaning guess). What this means is the cost of moving from
; point A to point B in either horizontal or vertical steps. The total number of steps is then 
; multiplied by 10. This is called the Manhatten method.
; F = Final cost. This is the sum of G and H. The lower the F cost, the better.
;
; ---  ---  ---  ---  ---
; | |  | |  | |  | |  | |
; ---  ---  ---  ---  ---
; | |  | |  |O|  | |  | |
; ---  ---  ---  ---  ---
; | |  |A|  |O|  |B|  | |
; ---  ---  ---  ---  ---
; | |  | |  |O|  | |  | |
; ---  ---  ---  ---  ---
; | |  | |  | |  | |  | |
; ---  ---  ---  ---  ---
;
; In the above diagram, A=starting point, O=obstacles, B=ending point. We want to find the shortest
; path from point A to point B, avoiding the obstacles.
; Let's take a closer look at point A and the squares that are adjacent to A.
;
; ---  ---  ---  ---  ---
; |3|  |2|  |O|  | |  | |
; ---  ---  ---  ---  ---
; |4|  |A|  |O|  |B|  | |
; ---  ---  ---  ---  ---
; |5|  |1|  |O|  | |  | |
; ---  ---  ---  ---  ---
;
; In the above view you'll notice that the squares that are adjacent to point A are numbered from 1
; to 5. Let's take square1. Is it diagonal to point A? Nope. Therefore it's G cost is 10. How about
; it's H cost? If you take 2 steps to the right of square1 and 1 step up, you'll arrive at point B.
; That's a total of 3 steps. Multiply those 3 steps by 10 and you get the H cost - 30.
; So, we now know that the F value of square1 is 10 + 30 = 40
; Let's take square2 now. Again, it is vertically adjacent to point A, so the G cost is also 10.
; How many steps, horizontal and vertical, does it take to arrive at point B from square2? 2 steps
; horizontally and 1 step vertically for a combined total of 3 steps. Multiplying by 10, we get an
; H cost of 30. F = 10 + 30 = 40. The F value of square2 is the same as square1.
; Ready to tackle square3? Good! If you look at square3, you'll see that it is not vertically
; adjacent to point A. Neither is it horizontally adjacent. No, it is diagonal to point A. So it's
; G cost is 14. How about it's H cost? Well, to arrive at point B from square3 you'd need to take
; 3 horizontal steps and 1 vertical step for a total of 4 steps. Multiplying by 10, you get an H
; cost of 40. What's the F value? 14 + 40 = 54.
; Last we check square5. No surprises here - it is also diagonal to point A so it's G cost is 14.
; Like square4, square5 is also located 4 steps away from point B (3 horizontal and 1 vertical step),
; giving it an H cost of 40. The F cost is then 14 + 40 = 54.
; Notice that we didn't calculate the G, H, or F costs of the squares labelled 'O'? Why? We ignored
; them because they aren't part of the walkable area that we must traverse.
; Ready for more? Good!
;
; ---  ---  ---  ---  ---
; | |  | |  | |  | |  | |
; ---  ---  ---  ---  ---
; |3|  |2|  |O|  | |  | |
; ---  ---  ---  ---  ---
; |4|  |A|  |O|  |B|  | |
; ---  ---  ---  ---  ---
; |5|  |1|  |O|  | |  | |
; ---  ---  ---  ---  ---
; | |  | |  | |  | |  | |
; ---  ---  ---  ---  ---
;
; In the above diagram (looks familiar, yes?) the squares numbered from 1 to 5 are children of
; the starting square, point A.
; Before we go any further, we need to learn about open, closed, and undefined lists. What are they?
; Well, when we calculate the F value of the adjacent squares, we put the squares on the "open" list.
; That means that we have looked at them, though we may not be finished with them, yet.
; If a square is put on the "closed" list, that means that we are finished with the square - that
; it has been examined already. For the most part this is true, but there ARE some few exceptions.
; The "undefined" list is just that - all of the squares that we haven't looked at yet are still
; undefined.
; Another point - when we look at the adjacent squares of point A, we put point A on the "closed"
; list. So, where are we now?
; We have put point A on the closed list, so we need another square to act as our "point A"
; That is where the F value comes in. We take the square with the lowest F value and make that
; our current square - our "point A". What is the lowest F value of the squares that we have
; already looked at? It is 40. But wait - there are TWO squares that have the F value of 40! What
; do we do? We take whichever one of those 2 squares that we wish and make it our current square.
; It doesn't matter which one we take. For the sake of this tutorial, let's take square2 to
; be our current square.
;
; ---  ---  ---  ---  ---
; | |  | |  | |  | |  | |
; ---  ---  ---  ---  ---
; |3|  |A|  |O|  | |  | |
; ---  ---  ---  ---  ---
; |4|  |X|  |O|  |B|  | |
; ---  ---  ---  ---  ---
; |5|  |1|  |O|  | |  | |
; ---  ---  ---  ---  ---
; | |  | |  | |  | |  | |
; ---  ---  ---  ---  ---
;
; If you look at the above diagram you'll see that the former point A is now labelled with an "X".
; This is to let you know that this square is on the "closed" list. The former square2 is now
; labelled with an "A" to designate the new point A. Place the new point A on the "closed" list.
; And we start again.
;
; ---  ---  ---  ---  ---
; |2|  |1|  | |  | |  | |
; ---  ---  ---  ---  ---
; |3|  |A|  |O|  | |  | |
; ---  ---  ---  ---  ---
; |4|  |X|  |O|  |B|  | |
; ---  ---  ---  ---  ---
; | |  | |  |O|  | |  | |
; ---  ---  ---  ---  ---
; | |  | |  | |  | |  | |
; ---  ---  ---  ---  ---
;
; If you look at square1 you'll see that it is located directly above point A. Therefore it's G
; cost is 20 - its parent's G cost of 10 added to its OWN G cost of 10.
; What is it's H cost? 40. Why? Because you need To take 2 horizontal steps And 2
; vertical steps to get from square1 to point B. This makes its F value 60.
; Square2 is diagonal to point A, so it's G cost is 24. It takes 3 horizontal steps and 2 vertical
; steps to arrive at point B, giving it an H cost of 50 (remember, the total number of steps
; multiplied by 10). It's F value, then, is 74.
; Square3 is a square that is already on the "open" list from the former point A. So, what do we do?
; We determine if the path through the current square to square3 is shorter than from the former point A
; to square3. How do we do that? We take the G cost of the current square, which is 10, and add it
; to the G cost of square3. Since square3 is horizontally adjacent to the current square, its G cost
; is 10. So, the total G cost is 20 which is WAY higher than square3's original G cost of 14! 
; So we don't do anything with square3 because the path through the current square to square3 is NOT
; shorter (what this means is that it would be shorter to go directly from the former point A to square3).
; Finally we check square4. This square is also on the "open" list. So, again, we check to see if the
; path would be shorter if we get to square4 from the current square. Is it? The recalculated G
; cost is 24. This is much higher than the original G cost of square4, which is 10. Again, we don't
; do anything with this square.
; Now, if you look closely at the diagram, you will see that there is another, non-obstacle, square
; that is adjacent to the current square. Why aren't we checking THIS one? Well, we COULD, but we
; would be clipping the corner of the topmost obstacle if we did. If this doesn't matter in your game
; then, by all means, check this square too.
;
; ---  ---  ---  ---  ---
; |2|  |A|  |1|  | |  | |
; ---  ---  ---  ---  ---
; |3|  |X|  |O|  | |  | |
; ---  ---  ---  ---  ---
; | |  |X|  |O|  |B|  | |
; ---  ---  ---  ---  ---
; | |  | |  |O|  | |  | |
; ---  ---  ---  ---  ---
; | |  | |  | |  | |  | |
; ---  ---  ---  ---  ---
;
; The above diagram shows where we are now. I've taken the liberty of labelling the squares for you.
; The former square1 is now the current square - the point A - because it had the lowest F value of
; 60. Place the current square on the closed list and let's calculate the F value of square1.
; First, square1's G cost is 30 because it is directly horizontal to the current square. Next, it's
; H cost is 30 because you have to take 1 horizontal step and 2 vertical steps to reach point B from
; square1. That makes its F value 60.
; Square2 is already on the open list so let's see if the path is shorter to square2 by going through
; the current square. The G cost from the former point A to the current point A is 20 added to the G
; cost of square2 from the current point A - which is also 10 - makes the total G cost 30, which is
; higher than the original G cost of square2 - 24. We leave square2 alone. Checking square3 we find
; much the same, so we leave THAT square alone, as well.
; Guess that leaves us with 1 square to make the current point A, eh? Don't forget to put it on the
; closed list.
;
; ---  ---  ---  ---  ---
; | |  |X|  |A|  |1|  | |
; ---  ---  ---  ---  ---
; | |  |X|  |O|  | |  | |
; ---  ---  ---  ---  ---
; | |  |X|  |O|  |B|  | |
; ---  ---  ---  ---  ---
; | |  | |  |O|  | |  | |
; ---  ---  ---  ---  ---
; | |  | |  | |  | |  | |
; ---  ---  ---  ---  ---
; 
; Looking at square1 we find that the G cost is 40 and the H cost is 20. This means that the F value
; of square1 is 60. We don't have any more squares to check because of that clipping issue with the
; obstacle. So, what do we do? We make square1 the current square - point A - and put it on the closed
; list.
;
; ---  ---  ---  ---  ---
; | |  |X|  |X|  |A|  |1|
; ---  ---  ---  ---  ---
; | |  |X|  |O|  |3|  |2|
; ---  ---  ---  ---  ---
; | |  |X|  |O|  |B|  | |
; ---  ---  ---  ---  ---
; | |  | |  |O|  | |  | |
; ---  ---  ---  ---  ---
; | |  | |  | |  | |  | |
; ---  ---  ---  ---  ---
;
; Okay. Checking square1 we find the G cost is 50, the H cost is 30, the F value is 80. Square2's
; G cost is 54, it's H cost is 20, it's F value is 74. Square3's G cost is 50, it's H cost is 10, 
; it's F value is 60. Guess which square we'll make our current square? Yep, the one with the lowest
; F value - square3.
; Make square3 the current square - point A - and place it on the closed list.
;
; ---  ---  ---  ---  ---
; | |  |X|  |X|  |X|  |1|
; ---  ---  ---  ---  ---
; | |  |X|  |O|  |A|  |2|
; ---  ---  ---  ---  ---
; | |  |X|  |O|  |B|  |3|
; ---  ---  ---  ---  ---
; | |  | |  |O|  | |  | |
; ---  ---  ---  ---  ---
; | |  | |  | |  | |  | |
; ---  ---  ---  ---  ---
;
; Square1 is already on the open list, so we see if the path to that square is shorter by going through
; the current square. Nope. Square2 is also on the open list and the path is NOT shorter, either. How
; about square3? Since it's not on the open list, put it there. Its G cost is 64, its H cost is 10,
; it's F value is 74. That leaves one more square that is neither an obstacle, nor on the closed list.
; Yep, it's point B. For the sake of completeness let's calculate. SquareB's G cost is 70, it's H
; cost is 0, its F value is 70. Since it has the lowest F value, make it the current square and put
; it on the closed list. NOW we are done. We have reached point B from point A.
;
; ---  ---  ---  ---  ---
; | |  |X|  |X|  |X|  | |
; ---  ---  ---  ---  ---
; | |  |X|  |O|  |X|  | |
; ---  ---  ---  ---  ---
; | |  |X|  |O|  |B|  | |
; ---  ---  ---  ---  ---
; | |  | |  |O|  | |  | |
; ---  ---  ---  ---  ---
; | |  | |  | |  | |  | |
; ---  ---  ---  ---  ---
;
; Now, to get the path we just traversed, follow backwards from point B to the original point A.
; That is, follow the path backwards from the current square to its parent, to ITS parent, etc..
; In the diagram above the parents are labelled with an "X" which shows that they are on the
; closed list.
;
; I hope that this little tutorial has made understanding A* Pathfinding a little easier.
; *******************************************
; **************************************************************************************************
; A Star Pathfinding example in 3d by Kevin Lee Legge (kevin8084@gmail.com)
; **************************************************************************************************



Graphics3D 800,600
SetBuffer BackBuffer()


Const wall_type=1
Const player_type=2
Const ground_type=3
Const target_type=4
Const walker_type=5

Dim grid(9,9) 				; used to hold the world data

Global path[99] 			; used to store the found path
Global pathCount=0 			; how many nodes are stored in path() array

Global query=0 				; return value for checking if shorter path

Dim heap(99)  				; this is the heap
Dim nodeheap(99) 			; this corresponds to the node's position in the heap
Global heapCount=0 			; this is the index into the heap

Global currentX,currentY 	; the current node's x/y coordinate
Global currentNode 			; the current node
Global targetX,targetY 		; the target's x/y coordinate
Global found=0 				; flag for if target node is found

Type node
	Field x,y               ; x/y coordinate of the node
	Field parent.node       ; holds the node's parent
	Field gCost             ; node's G Cost
	Field hCost             ; node's H Cost
	Field fCost             ; node's F Cost
	Field state 			; 0=undefined,1=open,2=closed
	Field walkable 			; 0=no,1=yes
	Field number  			; holds the node number
End Type

Global pivot=CreatePivot()  ; just something for the walker to aim at

Global walker               ; this will be our walker

Global target=CreateSphere(); this is our target
EntityColor target,255,0,0
EntityShininess target,1
EntityRadius target,1
EntityType target,target_type
PositionEntity target,0,50,0; positioned so far out of range to avoid collision problem when
							; setting up the world

Global camera=CreateCamera(); we need to see
CameraRange camera,.1,300
PositionEntity camera,0,50,0
EntityRadius camera,1
EntityType camera,player_type

light=CreateLight()
PositionEntity light,0,100,0
LightRange light,1000
RotateEntity light,90,0,0

Global ground=CreatePlane()
PositionEntity ground,0,0,0
EntityColor ground,10,50,10
EntityType ground,ground_type

Collisions player_type,wall_type,2,2
Collisions player_type,ground_type,2,2
Collisions walker_type,wall_type,2,1
Collisions walker_type,ground_type,2,2

; set up the world
readData()
; make sure that the camera is pointing in the right direction
PointEntity camera,walker

; since we already read in currentX and currentY, we can position the pivot now
PositionEntity pivot,currentX,1,currentY

quit=0

While Not quit
If KeyHit(1) Then quit=1
If KeyDown(200) Then MoveEntity camera,0,0,.1
If KeyDown(208) Then MoveEntity camera,0,0,-.1
If KeyDown(203) Then TurnEntity camera,0,1,0
If KeyDown(205) Then TurnEntity camera,0,-1,0
If KeyHit(57) And found<>1 Then findChildren() ; spacebar to walk through path

TranslateEntity camera,0,-1,0

updateWalker() ; let the walker move

UpdateWorld
RenderWorld
If found=1 Then ; target node has been found
	Text 0,0,"Found target node: "+currentNode
End If
Text 0,10,"Press spacebar to step through the nodes. First node takes two presses of space bar..."
Flip
Delay 10
Wend
End

Function findChildren()
; this function searches the 8 nodes that surround the current node
PositionEntity pivot,currentX,1,currentY ; keep positioning the pivot on current node location
For y=currentY-10 To currentY+10 ; the 10 is because we scaled the world by 10
	For x=currentX-10 To currentX+10
		this.node = getNode(x,y)    ; get the node
		If this <> Null Then        ; is it a real, live node?
			If this\walkable=1 And this\state <> 2 Then ; yes? Is it walkable and not on closed list?
				If this\state=1 Then                    ; Is it already on open list?
					query=isShorterPath(this)           ; yep...so see if we have a shorter path
				End If
					If this\state=0 Then                ; node is not on open list
						this\state=1                    ; so put it there
						this\parent=getNodeParent()     ; record its parent node
						calculateGCost(this)            ; get its G Cost
						calculateHCost(this)            ; get its H Cost
						calculateFCost(this)            ; get its F Cost
						this\number=returnNodeNumber(this) ; record the node's number
						heap(heapCount)=this\fCost         ; put the F Cost on the heap
						nodeheap(heapCount)=this\number    ; put the node's number on another heap
						heapCount=heapCount+1              ; increment counter for next node
					End If
				
			End If
		End If
	Next
Next
sortHeap()  ; sort the heap
End Function

Function isShorterPath%(node.node)
; if path to this already open node is shorter then process it, otherwise ignore it
If Abs(node\x-currentX)=10 And Abs(node\y-currentY)=10 Then 
	tempGCost=14 ; node is diagonal to current node
Else
	tempGCost=10 ; node is horizontal or vertical to current node
End If
For this.node = Each node ; iterate through each node
	If this\x=currentX And this\y=currentY Then
		myGCost=this\gCost+tempGCost ; adding together current node's GCost and child's GCost
	End If
Next
If myGCost<node\gCost Then
; if node's gCost is higher then the total found above, it's a shorter path through current node
	node\state=0
	Return 1
Else
	Return 0 ; nope...not a shorter path
End If
End Function


Function getNode.node(x,y)
; returns the node with the x/y coordinates given
For this.node = Each node
	If this\x=x And this\y=y Then
		Return this
	End If
Next
End Function

Function calculateGCost(this.node)
; calculates the G Cost of the node
If Abs(this\x-currentX)=10 And Abs(this\y-currentY)=10 Then
	this\gCost=14+this\parent\gCost ; node is diagonal to parent
Else
	this\gCost=10+this\parent\gCost ; node is horizontal or vertical to parent
End If
End Function

Function calculateHCost(this.node)
; calculates the heuristic (H) cost. This is nothing but a guess at the distance
this\hCost=(Abs(this\x-targetX)+Abs(this\y-targetY))*10 ; absolute distance multiplied by 10
End Function

Function calculateFCost(this.node)
; add together the G Cost and H Cost of the node
this\fCost=this\gCost+this\hCost
End Function

Function sortHeap()
; sorts the heap so that heap(0) contains the lowest number
; this is just a quick and dirty bubble sort routine
For i=0 To heapCount-1
	For j=0 To heapCount-1
		If heap(j+1)>0 Then ; make sure that the heap isn't at the end
			If heap(j)>heap(j+1) Then ; first heap item is larger than second heap item
				temp=heap(j) ; store first item in temporary variable
				tempn=nodeheap(j) ; store first item in nodeheap in temp variable, as well
				heap(j)=heap(j+1) ; take second item in heap and put it in first position
				nodeheap(j)=nodeheap(j+1) ; same with the nodeheap
				heap(j+1)=temp ; take the item from temp and put it in the heap's second position
				nodeheap(j+1)=tempn ; same with nodeheap
			End If
		End If
	Next
Next
; since we now have a new low number in the heap's first position, let's make it a new parent
getNewParent()
End Function

Function getNewParent()
; this function creates a new parent node
path[pathCount]=currentNode ; store path
pathCount=pathCount+1       ; increment counter
	For this.node = Each node
		; iterate through nodes looking for the node that is in first position in the nodeheap
		If this\number = nodeheap(0) Then
			currentX=this\x ; make currentX equal to this node's x
			currentY=this\y ; make currentY equal to this node's y
			currentNode=this\number ; make sure to record that this node is the current node
			this\state = 2 ; flag this node as "closed"
			; if this node's x/y coordinate are the same as the target's then target is found
			If this\x=targetX And this\y=targetY Then found=1
			heap(0)=heap(heapCount-1) ;since we no longer need this particular heap item, we get
			                          ;rid of it by sticking the last heap item in the first
									  ;position
			nodeheap(0)=nodeheap(heapCount-1) ; same with the nodeheap
			heapCount=heapCount-1             ; and decrementing the counter
			Exit ; we're done here, so let's leave
		End If
	Next
If found=0 Then
;findChildren() ; *************************************
End If
End Function

Function getNodeParent.node()
; returns the node's parent
;If currentNode > 9 Then
;	tempX = currentNode Mod 10
;	tempY = (currentNode-tempX)/10
;Else
;	tempX=currentNode
;	tempY=0
;End If
;For this.node = Each node
;	If this\x=tempX And this\y=tempY Then
;		Return this
;	End If
;Next
 For this.node = Each node
	If this\x=currentX And this\y=currentY Then
		Return this
	End If
 Next
End Function

Function returnNodeNumber%(node.node)
Return node\x+((node\y)*10)
End Function

Function updateWalker()
PointEntity walker,pivot
MoveEntity walker,0,0,.1
End Function

Function readData()
Restore nodeData
Read xVal,yVal
;For y=0 To yVal-1
For y=yVal-1 To 0 Step-1
	For x=0 To xVal-1
		Read grid(x,y)
		node.node=New node
		node\x=x*10:node\y=y*10
		Select grid(x,y)
			Case 0
				; we have a wall here, so let's build it
				node\walkable=0
				cube=CreateCube()
				ScaleEntity cube,5,1,5
				EntityColor cube,50,30,10
				PositionEntity cube,node\x,1,node\y
				EntityType cube,wall_type
			Case 1
				; free areas to walk on
				node\walkable=1
			Case 2
				; this is the start position
				node\walkable=1
				node\state = 2 ; flag it as closed
				currentX=node\x
				currentY=node\y
				currentNode = returnNodeNumber(node)
				node\number=currentNode
				path[pathCount]=node\number
				pathCount=pathCount+1
				walker=CreateSphere()
				EntityColor walker,0,0,255
				EntityShininess walker,1
				PositionEntity walker,node\x,1,node\y
				
			Case 3
				; this is the target
				node\walkable=1
				targetX=node\x
				targetY=node\y
				PositionEntity target,node\x,1,node\y
			Case 4
				; the camera here
				node\walkable=1
				PositionEntity camera,node\x,1,node\y
		End Select
	Next
Next
End Function


.nodeData
Data 10,10
Data 0,0,0,0,0,0,0,0,0,0
Data 0,1,1,1,0,0,1,1,1,0
Data 0,1,2,1,0,1,1,1,1,0
Data 0,1,1,1,0,1,1,1,1,0
Data 0,1,1,1,0,1,1,1,1,0
Data 0,1,1,1,1,1,1,1,1,0
Data 0,1,1,1,0,0,0,0,1,0
Data 0,1,4,1,0,1,1,1,1,0
Data 0,1,1,1,0,1,3,1,1,0
Data 0,0,0,0,0,0,0,0,0,0