F# Pathfinding

I've recently started to have a play around with F# and for my first non-trivial application I'm thinking of implementing a path finding algorithm as seen in games. Seeing as this is a learning exercise I'm going to try and be as "functional" as possible (trying to stay as "pure" as I can with immutable data).

First things first:
We'll need a world to 'path find' our way around. Lets start off with defining some simple utility functions. Below is my Tools module that contains functions to:

• Square ints to a float 
• Remove items from lists 
• Load ints from a file (2D arrays of numbers will serve as our tile maps, collision maps, monster & item locations, etc)

	module Tools
	open System
	let sqr x = float(x * x) (* Square two ints into a float *)
	(* Remove element where predicate *)
	let rec remove l predicate =
	match l with
	| [] -> []
	| hd::tl -> if predicate(hd) then
	(remove tl predicate)
	else
	hd::(remove tl predicate)
	let loadMap path =
	IO.File.ReadAllText(path).Split(';')
	|> Array.filter(fun s -> s<> String.Empty)
	|> Array.map(fun s-> Int32.Parse(s.Replace(";",String.Empty)))

view raw Tools.fs hosted with ❤ by GitHub

The map:
Ok we still need to describe the world we want to navigate: In our world a 0 represents an open space and a 1 represents an area we can not enter. For example a 4 unit long corridor, running left to right, would look like this:

* Wall 1 1 1 1
* Void 0 0 0 0
* Wall 1 1 1 1

Here is my level module that describes:

• Map point: an x,y location in our 2D space, plus the value of the tile at that location and a function to calculate distance from another map point
• Map: a list of map points and a function to get the neighboring tiles of any given tile

	module Level
	open Tools
	type MapPoint =
	{x:int;y:int;value:int} (* a point in 2d map space *)
	(*Calculate distance to other map point*)
	member this.Distance mp = sqrt (sqr(this.x+mp.x) + sqr(this.y+mp.y))
	(*Simple construct to hold the 2D map data*)
	type Map =
	(* Width & Height of map and map data in 1D array *)
	{width:int; height:int; map:int list}
	(* function to wrap 1D array into 2D array to retrive map point *)
	member this.GetElement x y = {x=x;y=y;value=this.map.[x % this.height + y * this.width]}
	(* return list of map points that surround current map point *)
	member this.GetNeighborsOf p =
	[for y in p.y-1..p.y+1 do
	for x in p.x-1..p.x+1 do
	if ((y<>p.y || x <>p.x) (*bounds checking *)
	&& y>=0 && x>=0
	&& x<this.width
	&& y<this.height)
	then yield this.GetElement x y]

view raw Level.fs hosted with ❤ by GitHub

Pathfinding: Now comes the fun part. This contains the PathingNode type (that basically wraps a map location with information that the A* path-finding algorithm needs), some utility functions and the path-finding function itself.

	module Pathfinding
	open Level
	open Tools
	(* a wrapper for mapPoint that can contain pathing data as per typical A* pathfinding *)
	(* g = cost of path so far, h = estimated cost to goal, parent = tile we came here from *)
	type PathingNode =
	{point:MapPoint; h:float; g:float; parent:PathingNode option}
	member this.f = this.g + this.h
	(* returns a pathnode based on a given map point *)
	let pointToPathNode parent goal node =
	{point=node; h=node.Distance goal; g=(parent.g+1.0); parent=Some(parent)}
	let isClear t = t.value = 0 (* Tile is empty *)
	let nodePointEquals mapPoint pathNode = pathNode.point = mapPoint
	(* A has the same location as B but via a shorter route *)
	let isShorter nodeA nodeB= nodeA.point = nodeB.point && nodeA.f < nodeB.f
	let rec pathFind (area:Map) goal start (openNodes:PathingNode list) (closedNodes:PathingNode list) =
	let pointToPathNode = pointToPathNode openNodes.Head goal
	(* Loop over list of neighbors accumalating a list of open nodes *)
	let rec checkNeighbors neighbors openNodeAcc=
	match module Pathfinding
	open Level
	open Tools
	(* a wrapper for mapPoint that can contain pathing data as per typical A* pathfinding *)
	(* g = cost of path so far, h = estimated cost to goal, parent = tile we came here from *)
	type PathingNode =
	{point:MapPoint; h:float; g:float; parent:PathingNode option}
	member this.f = this.g + this.h
	(* returns a pathnode based on a given map point *)
	let pointToPathNode parent goal node =
	{point=node; h=node.Distance goal; g=(parent.g+1.0); parent=Some(parent)}
	let isClear t = t.value = 0 (* Tile is empty *)
	let nodePointEquals mapPoint pathNode = pathNode.point = mapPoint
	(* A has the same location as B but via a shorter route *)
	let isShorter nodeA nodeB= nodeA.point = nodeB.point && nodeA.f < nodeB.f
	let rec pathFind (area:Map) goal start (openNodes:PathingNode list) (closedNodes:PathingNode list) =
	let pointToPathNode = pointToPathNode openNodes.Head goal
	(* Loop over list of neighbors accumalating a list of open nodes *)
	let rec checkNeighbors neighbors openNodeAcc=
	match neighbors with
	|[] -> openNodeAcc (* When list of neighbors is exhausted return the open nodes. *)
	|hd::tl ->
	let checkNeighbors = checkNeighbors tl
	let node = {hd with parent = Some(openNodes.Head)}
	if (List.exists (isShorter hd) openNodeAcc) then (* if a higher costingnode is in open...*)
	let shorterPath = remove openNodeAcc (nodePointEquals hd.point) (*... remove it.. *)
	checkNeighbors (node::shorterPath)
	elif not(List.exists (nodePointEquals hd.point) closedNodes)
	&& not (List.exists (nodePointEquals hd.point) openNodeAcc) then
	(* If path is not open or closed... *)
	checkNeighbors (node::openNodeAcc)
	else checkNeighbors openNodeAcc (* Else carry on. *)
	let neighbors =
	(* Get the neighbors of our current node...*)
	area.GetNeighborsOf openNodes.Head.point
	|> List.filter isClear (* ...filter out collidable tiles... *)
	|> List.map pointToPathNode (*..for each neighbor we can walk to: generate a node*)
	(* Try and find the goal node in the walkable neighbor of this tile. *)
	let pathToGoal = List.tryFind (nodePointEquals goal) neighbors
	if pathToGoal.IsSome then pathToGoal //If we found our goal return it...
	else
	let nextSet =
	checkNeighbors neighbors openNodes.Tail
	|> List.sortBy(fun x -> x.f)
	if nextSet.Length > 0 then
	pathFind area goal start nextSet (nextSet.Head::closedNodes) (*...Else carry on*)
	else None with
	|[] -> openNodeAcc (* When list of neighbors is exhausted return the open nodes. *)
	|hd::tl ->
	let checkNeighbors = checkNeighbors tl
	let node = {hd with parent = Some(openNodes.Head)}
	if (List.exists (isShorter hd) openNodeAcc) then (* if a higher costingnode is in open...*)
	let shorterPath = remove openNodeAcc (nodePointEquals hd.point) (*... remove it.. *)
	checkNeighbors (node::shorterPath)
	elif not(List.exists (nodePointEquals hd.point) closedNodes)
	&& not (List.exists (nodePointEquals hd.point) openNodeAcc) then
	(* If path is not open or closed... *)
	checkNeighbors (node::openNodeAcc)
	else checkNeighbors openNodeAcc (* Else carry on. *)
	let neighbors =
	(* Get the neighbors of our current node...*)
	area.GetNeighborsOf openNodes.Head.point
	|> List.filter isClear (* ...filter out collidable tiles... *)
	|> List.map pointToPathNode (*..for each neighbor we can walk to: generate a node*)
	(* Try and find the goal node in the walkable neighbor of this tile. *)
	let pathToGoal = List.tryFind (nodePointEquals goal) neighbors
	if pathToGoal.IsSome then pathToGoal (* If we found our goal return it... *)
	else
	let nextSet =
	checkNeighbors neighbors openNodes.Tail
	|> List.sortBy(fun x -> x.f)
	if nextSet.Length > 0 then
	pathFind area goal start nextSet (nextSet.Head::closedNodes) (*...Else carry on*)
	else None

view raw Pathfinding.fs hosted with ❤ by GitHub

Level 1: Now let's create a file to represent our world. Open up a text editor and and create a delimited 2D array of 0's and 1's with a definite path between 2 points. like so:

* 0; 0; 0; 0; 0;
* 0; 0; 0; 1; 0;
* 0; 0; 0; 1; 0;
* 0; 1; 1; 1; 1;
* 0; 0; 0; 0; 0;
* 1; 1; 1; 1; 0;
* 0; 0; 0; 0; 0;
* 1; 0; 1; 1; 1;
* 0; 0; 0; 0; 0;

(I've highlighted my start tile green and end tile red for reference)
...Save this text file some where on your hard drive for later.
The Results:
The code bellow shows the pathfinding in action:

	open Tools
	open Level
	open Pathfinding
	let level1 = {width=5;height=9;map=(loadMap @"C:\Test\lvl1.txt" |> Seq.toList)}
	let start = level1.GetElement 4 8
	let goal = level1.GetElement 4 0
	let path = pathFind(level1,goal,start,[{point=start;h=start.Distance goal;g=0.0;parent=None}],[])
	let rec readPath (path:PathingNode, list:PathingNode list) =
	match path.parent.IsNone with
	| true -> list
	| false -> readPath (path.parent.Value, path.parent.Value::list)
	let waypoints = readPath(path, [path])

view raw Program.fs hosted with ❤ by GitHub