Pathfinding using waypoints help

Blitz3D Forums/Blitz3D Programming/Pathfinding using waypoints help

Hey, i have been trying to get my head round pathfinding for a while. I mostly grasp the grid idea, but struggle in finding the shortest path. I can find a path and navigate around obstacles, hugging the terrain, but obviously this isn't the shortest path. Anyyyyyyyyway...

I figured i'd set up nodes or waypoints in my editor, to try and reduce the time taken to calculate a path. But, i'm a bit stuck on how to go about finding the shortest path...

www.rosscrooks.pwp.blueyonder.co.uk/waypoints.PNG

Take that picture for example, can someone outline the process for finding the shortest path. Just a quick text description would be lovely :o) I thought choosing the nodes, based on how close they were to the destination, but as that path may lead to a far longer route, i don't think i can use that.

You could calculate the distance in between each set of nodes and then compare them, choose the one with the shortest distance.

read up on djekstra (sp?) i coded my pathfinding using it.

Does it use nodes? Or a grid? I'll see if i can find anything. Was just wondering a rough process of steps to take to achieve this.

Chroma. The problem with calculating the distances, is you would need to loop through all the possible different combinations, marking nodes, so you can't go back on yourself. Depending on the number of nodes and connecting nodes, it could take a while to calculate.

Hi Ross - I use waypoints in my game and have a simple program I run outside of the actual game to pre calculate the pathfinding solution so that in the game the units only need to specify their starting and target positions and they are given the 'next node' to move to on their way to their target.

However the same system could be used in real time. (with some modifications)

Here is a cutdown version of what I use, hopefully it is helpful. The IsInLOS function will need to be changed if you are doing a 3d world, as it is currently for a 2d environment.

;;
;Path Node Pre Calculator Waypoints Etc
;
;Read In Node File
;
;
;Go Through Each Node And Create List Of Visible Nodes From Each Node (connect them)
;
;Go Through Each Node and calc path to each other node storing the id num of the next node to get there
;
;
;




Type nodeobject
Field id
Field x
Field y

End Type

Dim NodeVisArray(255,255)

infile=ReadFile("pathnodes.txt")

While Not(Eof(infile))

node.nodeobject=New nodeobject
node\id=ReadByte(infile)
node\x=ReadShort(infile)
node\y=ReadShort(infile)

Wend

CloseFile infile

Dim NodePathArray(255,255)

Dim NodeSearchedArray(255)
Dim BestNodePath(255)
Dim MapArray(3200,2400) ;This is used for a 2d world to get the Line of Sight ->Change Function IsInLOS for 3d environments.

DoStuff() ;precalculate the pathfinding data

End





Function CalcNodePaths()

For Node.NodeObject=Each NodeObject
	For OtherNode.NodeObject=Each NodeObject
		Distance=0
		For i=0 To 255
			NodeSearchedArray(i)=255
			BestNodePath(i)=0
		Next
		LeastDistance=255
		If Handle(node)<>Handle(othernode) Then
		
			If NodeVisArray(Node\ID,OtherNode\ID)=1 Then
			 NodePathArray(Node\ID,OtherNode\ID)=OtherNode\ID 
			Else
		
			NextNodeToDestination(Node.NodeObject,OtherNode.NodeObject,0)
			If LeastDistance<255 Then 
				NodePathArray(Node\ID,OtherNode\ID)=BestNodePath(1)
			EndIf 		
			
			
			EndIf 
		EndIf 
	Next 
Next







End Function


Function WriteNodePathArray(Filename$)

outfile=WriteFile(filename$)
For i=0 To 255
For j=0 To 255
WriteByte outfile,NodePathArray(i,j)
Next
Next



CloseFile outfile

End Function



Global LeastDistance
Function NextNodeToDestination(CurrentNode.NodeObject,FinalNode.NodeObject,NodeCount)
NodeSearchedArray(CurrentNode\ID)=NodeCount
If NodeCount>=255 Then Return 255

If Handle(CurrentNode)=Handle(FinalNode) Then Return NodeCount

For OtherNode.NodeObject=Each NodeObject
If Handle(CurrentNode)<>Handle(OtherNode) Then 
	If NodeVisArray(OtherNode\ID,CurrentNode\ID)=1 Then 
		If NodeCount<NodeSearchedArray(OtherNode\ID) Then 
			NodeDist=NextNodeToDestination(OtherNode.NodeObject,FinalNode.NodeObject,NodeCount+1)
			If NodeDist<LeastDistance Then 
				LeastDistance=NodeDist
				BestNodePath(NodeCount)=CurrentNode\ID
				
			EndIf 
		EndIf 		
	Else
	
		
	EndIf 
EndIf 
Next


Return 255
End Function



Function CalcNodesVisibleToNode()
For Node.NodeObject=Each NodeObject
	For OtherNode.NodeObject=Each NodeObject
		 
		If IsInLos(Node.NodeObject,OtherNode\X,OtherNode\Y)=True Then NodeVisarray(Node\ID,OtherNode\ID)=1 Else NodeVisarray(Node\ID,OtherNode\ID)=0
	
	Next 
Next

End Function



Function IsInLOS(Node.NodeObject,X,Y)

If Node.NodeObject<>Null Then 
	StartX#=X
	StartY#=Y
	FinalX#=Node\X
	FinalY#=Node\Y
	Dist#=Sqr((FinalX-StartX)^2+(FinalY-StartY)^2)
	If Dist=0 Then Return True 
	If Dist>0 Then 
	dx#=(FinalX-StartX)/Dist
	dy#=(FinalY-StartY)/Dist
	Repeat
	StartX=StartX+dx
	StartY=StartY+dy
	If StartX>=0 And StartY>=0 And StartX<=3199 And StartY<=2399 Then 
		If MapArray(StartX,StartY)=0 Then HitWall=True 
	Else
		HitWall=True 
	EndIf 
	Until HitWall=True Or (Abs(StartX-FinalX)<2.0 And Abs(StartY-FinalY)<2.0)
	If HitWall=True Then Return False Else Return True 
	EndIf 
EndIf 

End Function



Function DoStuff()

CalcNodesVisibleToNode()
CalcNodePaths()
WriteNodePathArray("PreCalculatedPaths.txt")


End Function


There are a few examples in the old Blitzcoder Showcase: http://www.blitzcoder.com/cgi-bin/showcase/showcase_frontpage.pl

I think the solution is really going to depend on the actual game and the requirements of the pathfinding.

If it were a FPS-type (corridors) game then, personally, I'd precalc as much as possible.

Just google for A* algorithm. All grid methods can alo be used for node graphs with slight modification. But A* is clearly the best in most situations (some other algos work better in special cases).

Read this as a base:
http://www.policyalmanac.org/games/aStarTutorial.htm

and continue with this:
http://www.policyalmanac.org/games/twoTiered.htm

and when actually coding the thing... better do it with binary heaps, so read this:
http://www.policyalmanac.org/games/binaryHeaps.htm

And to make this all easier for you... here's my pretty fast heap functions:

;HEAP FUNCTIONS
Global HEAPSIZE=0
Global HMAXSIZE=100			;this is your heaps size
Dim HEAPITEM%(HMAXSIZE)
Dim HEAPKEY#(HMAXSIZE)

Function HEAP_INSERT(n,key#)
HEAPSIZE=HEAPSIZE+1
HEAPITEM(HEAPSIZE) = n

m = HEAPSIZE
HEAPKEY(HEAPITEM(m))=key

  While m <> 1 ;While item hasn't bubbled to the top (m=1)

     ;Check if child is <= parent. If so, swap them.
     If HEAPKEY(HEAPITEM(m)) <= HEAPKEY(HEAPITEM(m/2)) Then
        temp = HEAPITEM(m/2)
        HEAPITEM(m/2) = HEAPITEM(m)
        HEAPITEM(m) = temp
        m = m/2 
     Else

        Exit ;exit the while/wend loop
     End If
  Wend
End Function

Function HEAP_REMOVELOWEST()
;If HEAP_EMPTY()=0
L = HEAPITEM(1)

;GET THE LAST ITEM AND PUT IT IN THE FIRST POSITION
;(FIRST GET*S DELETED)
HEAPITEM(1) = HEAPITEM(HEAPSIZE)
HEAPSIZE = HEAPSIZE - 1

v = 1
;Repeat the following until the item sinks to its proper spot in the binary heap.
Repeat
  u = v
  If 2*u+1 <= HEAPSIZE;if both children exist
    ;Select the lowest of the two children.
    If HEAPKEY(HEAPITEM(u)) >= HEAPKEY(HEAPITEM(2*u)) Then v = 2*u 
    If HEAPKEY(HEAPITEM(v)) >= HEAPKEY(HEAPITEM(2*u+1)) Then v = 2*u+1

  Else If 2*u <= HEAPSIZE ;if only child #1 exists
    ;Check if the F cost is greater than the child
    If HEAPKEY(HEAPITEM(u)) >= HEAPKEY(HEAPITEM(2*u))Then v = 2*u
  End If

  If u <> v Then ; If parent's F > one or both of its children, swap them
    temp = HEAPITEM(u)
    HEAPITEM(u) = HEAPITEM(v)
    HEAPITEM(v) = temp
  Else
    Exit ;if item <= both children, exit repeat/forever loop
  End If
Forever

Return L
;End If
End Function

Function UPDATE_HEAPITEM(item,newkey#,HEAPPOS=0)
If HEAPPOS=0
For i=1 To HEAPSIZE
If HEAPITEM(i) = item Then m=i:Exit
Next
Else 
m=HEAPPOS
End If

HEAPKEY(HEAPITEM(m)) = newkey

  While m <> 1 ;While item hasn't bubbled to the top (m=1)
     ;Check if child is <= parent. If so, swap them.
     If HEAPKEY(HEAPITEM(m)) <= HEAPKEY(HEAPITEM(m/2)) Then
        temp = HEAPITEM(m/2)
        HEAPITEM(m/2) = HEAPITEM(m)
        HEAPITEM(m) = temp
        m = m/2 
     Else
        Exit ;exit the while/wend loop
     End If
  Wend
End Function

Function HEAP_EMPTY()
If HEAPSIZE>0 Return 0 Else Return 1
End Function
Function CLEAR_HEAP()
Dim HEAPITEM%(HMAXSIZE)
Dim HEAPKEY#(HMAXSIZE)
HEAPSIZE=0
End Function



ALso here's an example A* loop using the above heap code for you to study. It's ripped out of my game so it doesn't work as is... but it's pretty well commented.

Function P_ASTAR(node1%,GOAL%,o%=0)
Local CLOSED%[P_MAXNODES]
Local PARENT%[P_MAXNODES]
Local G#[P_MAXNODES]					;GENERATED COST
Local H#[P_MAXNODES]					;ESTIMATED COST (to target)
goal_found=0

If o=0 Then P_GOAL_NODE(o) = 0


If NODE1 <> GOAL
CLEAR_HEAP()
HEAP_INSERT(node1,0)


;P_ELAPSEDTIME=MilliSecs()
Repeat
;GET THE MOST LOWEST COST NODE FROM THE HEAP
current = HEAP_REMOVELOWEST()

;PUT IT TO THE CLOSED LIST
CLOSED[current] = 1

	;CHECK ALL THE NODES CONNECTED TO CURRENT NODE
	For i=1 To P_NODE_CONNECTIONS(current)
	nn = P_NODE_CON(current,i)

	;IF NOT ON THE CLOSED LIST PROCEED
	If CLOSED[nn]=0
	
	;SEARCH THE OPEN LIST -> PUT THE HEAP POS IN found ELSE found = 0
	found=0
	For ii=1 To HEAPSIZE
	If HEAPITEM(ii) = nn Then found=ii:Exit
	Next

	;NOT ON OPENLIST ---> CALCULATE COST AND PUT IT IN!
	If found=0
	PARENT[nn] = current
	H[nn] = P_MANHATTAN_DISTANCE(nn,GOAL)					;ESTIMATE THE DISTANCE TO GOAL
	G[nn] = P_NODE_ARCLENGTH(current,i) + G[current]		;G = distance to this node + G of the parent node
	F# = G[nn] + H[nn]																	;CALCULATE FINAL COST

	;PUT TO OPENLIST
	HEAP_INSERT(nn,F)
	End If

	;IS ON THE OPEN LIST --- > CHECK IF THIS IS BETTER PATH TO  IT
	If found>0
	NG# = P_NODE_ARCLENGTH(current,i) + G[current]

		;IF THIS PATH*S G SCORE = LOWER THAN OLD G --> UPDATE
		If NG<G[nn]
		PARENT[nn] = current
		G[nn] = NG																				;REPLACE THE OLD G WITH THE CURRENT G
		F# = G[nn] + H[nn]																	;CALCULATE FINAL COST
		UPDATE_HEAPITEM(nn,F,found)											;UPDATE THIS NODE
		End If
		
	End If

	End If
	Next

If current=GOAL Then goal_found=1
Until  HEAP_EMPTY()=1 Or goal_found=1 
End If

	P_PATHNODES(o)=0
	P_PATHCOST(o)=0
	
	If P_GOAL_NODE(o)>0
	P_PATHNODES(o) = P_PATHNODES(o) + 1
	P_PATHNODE(o,P_PATHNODES(o)) = P_GOAL_NODE(o)
	End If

;TRACE THE PATH!
If goal_found=1
current=GOAL
P_PATHCOST(o) = G[current]

	Repeat
	P_PATHNODES(o) = P_PATHNODES(o)+1
	P_PATHNODE(o,P_PATHNODES(o)) = current
	current = PARENT[current]
	Until current=node1
	P_PATHNODES(o) = P_PATHNODES(o)+1
	P_PATHNODE(o,P_PATHNODES(o)) = current

End If



;P_ELAPSEDTIME=MilliSecs()-P_ELAPSEDTIME

Return P_PATHNODES(o)
End Function


Thanks alot guys, i'll have a good look through this lot. My needs are fairly simple. Just a basic 2d solution would suit my 3d world. :o) Thanks again!

Looking at the picture, I would think you just need to add all distances ?

Hope this would make any sense than:
You can determine "direct" distances. That would be distances between two points that are connected.
And "indirect" distances. Distances between two points that are connected via another point.

First, calculate all direct distances.
Then, calculate all indirect distances by adding the direct distances to each other.

example:

Phase 1
Point 0->1=100 units
Point 1->2=125 units
Point 2->3= 75 units

Phase 2
Point 0->2=225 units
Point 0->3=300 units
etc.

Check these recent Gamasutra Feature, pretty good stuff on advanced pathfinding...

http://www.gamasutra.com/features/20060626/strom_01.shtml
and
http://www.stromcode.com/wiki/index.php?title=Main_Page/Articles/Genetic_Programming:_Evolving_an_Intelligent_Pathfinding_Algorithm