Pathfinding 2D

Monkey Forums/Monkey Code/Pathfinding 2D

NoOdle (Posted 2011) [#1]

EDIT: New Code further down!! I decided to try out an idea I had last night to simplify grid based pathfinding. The typical method I have seen a lot of people use is to have a grid separated evenly by some pre-determined tile size. This works well for 2D games but I wanted to speed this up by removing all the tiles that wouldn't need to be considered in pathfinding. My method was to create nodes on the corners of any solid tile, linking these nodes using line of sight visibility. This node map could then be used for normal A* pathfinding with much fewer iterations. There is a slight limitation with this method. If you wanted to having different costs for moving from one tile to another this wouldn't be able to account for that. A fairly decent way to combat this however is to use the same method for these tiles as above. For example if you have an area of muddy swamp you would create the nodes around the solid tiles first (walls) and also create nodes around the swamp area. When the links are calculated the cost of moving across this link could take into account the type of tile it has to cross. The mud tiles would have a higher cost than a grass tile for example. This would then yield the desired result, a path around the mud would have a lower cost than the direct path through the mud. Anyway, I will be updating this tonight with an A* pathfinding algorithm implemented to show this. But for now here is a simple demo showing how to generate a simple node map. Left Mouse to create a tile D Key to delete a tile Enter creates the node map It does take a second or two to make the node map because I was quite lazy implementing this! Import mojo Class pNode Global NodeList : List<pNode> = New List<pNode> Field x : Int Field y : Int Field link : List<pNode> Method New( _x : Int, _y : Int ) Self.x = _x Self.y = _y Self.link = New List<pNode> NodeList.AddLast( Self ) End Method Method Draw() DrawOval Self.x - 10, Self.y - 10, 20, 20 Local this : pNode For this = Eachin Self.link DrawLine Self.x, Self.y, this.x, this.y Next End Method Function CheckForNode : Bool( x, y ) Local this : pNode For this = Eachin NodeList If this.x = x And this.y = y Return True Endif Next Return False End Function Function CreateNode( x, y ) If CheckForNode( x, y ) = False New pNode( x, y ) Endif End Function Function Render() Local this : pNode For this = Eachin NodeList this.Draw() Next End Function End Class Class MyApp Extends App Global TileMap : Int[ 20 ][] Method OnCreate() SetUpdateRate 60 'creates tile map array For Local n = 0 To 19 TileMap[ n ] = New Int[ 15 ] Next End Method Method OnUpdate() 'get array mouse co-ords Local ax : Int = MouseX() / 32 Local ay : Int = MouseY() / 32 'adds cell to TileMap If MouseDown( MOUSE_LEFT ) If ax < 20 And ay < 15 And ax > -1 And ay > -1 TileMap[ ax ][ ay ] = 1 Endif Endif If KeyDown( KEY_D ) If ax < 20 And ay < 15 And ax > -1 And ay > -1 TileMap[ ax ][ ay ] = 0 Endif Endif 'creates path nodes from TileMap If KeyHit( KEY_ENTER ) pNode.NodeList.Clear() 'Check the surrounding tiles up, right, down, left and adds node if none exist at that location For Local x : Int = 0 To 19 For Local y : Int = 0 To 14 Local cu : Int = y - 1 Local cr : Int = x + 1 Local cd : Int = y + 1 Local cl : Int = x - 1 If TileMap[ x ][ y ] = 1 If cu > -1 And cr < 20 If TileMap[ x ][ cu ] = 0 And TileMap[ cr ][ cu ] = 0 And TileMap[ cr ][ y ] = 0 pNode.CreateNode( cr * 32 + 16, cu * 32 + 16 ) Endif Endif If cu > -1 And cl < 20 If TileMap[ x ][ cu ] = 0 And TileMap[ cl ][ cu ] = 0 And TileMap[ cl ][ y ] = 0 pNode.CreateNode( cl * 32 + 16, cu * 32 + 16 ) Endif Endif If cd < 15 And cr < 20 If TileMap[ x ][ cd ] = 0 And TileMap[ cr ][ cd ] = 0 And TileMap[ cr ][ y ] = 0 pNode.CreateNode( cr * 32 + 16, cd * 32 + 16 ) Endif Endif If cd < 15 And cl < 20 If TileMap[ x ][ cd ] = 0 And TileMap[ cl ][ cd ] = 0 And TileMap[ cl ][ y ] = 0 pNode.CreateNode( cl * 32 + 16, cd * 32 + 16 ) Endif Endif Endif Next Next 'links nodes based on visibility Local this : pNode Local LinkCount : Int For this = Eachin pNode.NodeList Local target : pNode For target = Eachin pNode.NodeList If target <> this Local dx : Float = target.x - this.x Local dy : Float = target.y - this.y Local len : Float = Sqrt( dx * dx + dy * dy ) Local didCollide = False If len <> 0 dx = dx / len dy = dy / len Local x1 : Float = this.x Local y1 : Float = this.y For Local n = 0 To len Step 1 Local ax : Int = x1 / 32 Local ay : Int = y1 / 32 'if line passes through tile that is solid, then exit with collision flag If ax > 0 And ay > 0 And ax < 20 And ay < 15 If TileMap[ ax ][ ay ] = 1 didCollide = True Exit Endif Endif 'sample area around x1, y1 and check if solid tile (this stops links being to close to a wall) Local aSize : Float = 10 For Local x2 = x1 - aSize To x1 + aSize For Local y2 = y1 - aSize To y1 + aSize Local ax2 : Int = x2 / 32 Local ay2 : Int = y2 / 32 If ax2 > 0 And ay2 > 0 And ax2 < 20 And ay2 < 15 If TileMap[ ax2 ][ ay2 ] = 1 didCollide = True Exit Endif Endif Next Next If didCollide = True Then Exit 'check if x1, y1 lies on another node position (checks unneccessary links that pass through an existing node) Local checkA : pNode For checkA = Eachin pNode.NodeList If checkA <> this And checkA <> target If checkA.x = x1 And checkA.y = y1 didCollide = True Exit Endif Endif Next If didCollide = True Then Exit x1 = x1 + dx * 1 y1 = y1 + dy * 1 Next If didCollide = False this.link.AddLast( target ) LinkCount = LinkCount + 1 Endif Endif Endif Next Next Print LinkCount / 2 Endif End Method Method OnRender() Cls 255, 255, 255 'draws a feint grid SetColor 200, 200, 200 Local n : Int For n = 0 To DeviceWidth() Step 32 DrawLine n, 0, n, DeviceHeight() Next For n = 0 To DeviceHeight() Step 32 DrawLine 0, n, DeviceWidth(), n Next 'draws grid cells SetColor 255, 200, 100 For Local x : Int = 0 To 19 For Local y : Int = 0 To 14 If TileMap[ x ][ y ] DrawRect x * 32, y * 32, 32, 32 Endif Next Next 'draws all pNodes SetColor 0, 0, 0 pNode.Render() End Method End Class Function Main() New MyApp() End Function

matt (Posted 2011) [#2]

I like this, very nice

NoOdle (Posted 2011) [#3]

As promised, here is a pathfinding demo. I haven't had time to implement it into the code above, it is a stand alone example for now. Press S to set start node Press G to set goal node Press Enter to start pathfinding Import mojo Global StartNode : PathNode = Null Global GoalNode : PathNode = Null Class PathLink Field LinkNode : PathNode Field Distance : Float Method New( n : PathNode, d : Float ) Self.LinkNode = n Self.Distance = d End Method End Class Class PathNode Global NodeCount : Int Global NodeList : List< PathNode > = New List< PathNode > Field x : Int Field y : Int Field ID : Int Field NodeLink : List< PathLink > Method New( _x : Int, _y : Int ) NodeCount = NodeCount + 1 Self.x = _x Self.y = _y Self.NodeLink = New List< PathLink > Self.ID = NodeCount NodeList.AddLast( Self ) End Method Method DrawNode() SetColor 0, 0, 0 DrawOval Self.x - 20, Self.y - 20, 40, 40 SetColor 255, 255, 255 Local foundP : Path = Path.PathList.First() If foundP Local n : PathNode For n = Eachin foundP.NodeList If n = Self SetColor 255, 255, 0 Endif Next Endif If StartNode = Self Then SetColor 0, 255, 0 If GoalNode = Self Then SetColor 255, 0, 0 DrawOval Self.x - 16, Self.y - 16, 32, 32 SetColor 255, 255, 255 DrawText Self.ID, Self.x, Self.y, 0.4, 0.4 End Method Method DrawLink() Local this : PathLink For this = Eachin Self.NodeLink SetColor 0, 0, 0 DrawLine Self.x, Self.y, this.LinkNode.x, this.LinkNode.y Next End Method Function FindClosestNode : PathNode( x : Int, y : Int ) Local this : PathNode Local dist : Float = -1 Local closestNode : PathNode = Null For this = Eachin NodeList Local dx : Float = this.x - x Local dy : Float = this.y - y Local d : Float = Sqrt( dx * dx + dy * dy ) If dist = -1 Or d < dist dist = d closestNode = this Endif Next Return closestNode End Function Function LinkNodes( a : PathNode, b : PathNode ) Local dx : Float = a.x - b.x Local dy : Float = a.y - b.y Local dist : Float = Sqrt( dx * dx + dy * dy ) Local LinkA : PathLink = New PathLink( b, dist ) Local LinkB : PathLink = New PathLink( a, dist ) a.NodeLink.AddLast( LinkA ) b.NodeLink.AddLast( LinkB ) End Function Function Render() Local this : PathNode For this = Eachin NodeList this.DrawLink() Next For this = Eachin NodeList this.DrawNode() Next End Function End Class Class PathSortList Extends List< Path > Method Compare : Int( a : Path, b : Path ) If a.Distance + a.Underestimate < b.Distance + b.Underestimate Return 1 If a.Distance + a.Underestimate > b.Distance + b.Underestimate Return -1 Return 0 End Method End Class Class Path Global PathList : PathSortList = New PathSortList Field NodeList : List< PathNode > Field Distance : Float Field Underestimate : Float Method New( start : PathNode ) Self.NodeList = New List< PathNode > Self.NodeList.AddLast( start ) PathList.AddLast( Self ) End Method Method Copy : Path() Local this : Path = New Path this.Distance = Self.Distance this.Underestimate = Self.Underestimate this.NodeList = New List< PathNode > Local thisNode : PathNode For thisNode = Eachin Self.NodeList this.NodeList.AddLast( thisNode ) Next PathList.AddLast( this ) Return this End Method End Class Class PathExtends Field pDist : Float Field pNode : PathNode Method New( n : PathNode, d : Float ) Self.pDist = d Self.pNode = n End Method End Class Class MyApp Extends App Method OnCreate() SetUpdateRate 60 'create some nodes Local p1 : PathNode = New PathNode( 100, 100 ) Local p2 : PathNode = New PathNode( 300, 150 ) Local p3 : PathNode = New PathNode( 50, 300 ) Local p4 : PathNode = New PathNode( 200, 250 ) Local p5 : PathNode = New PathNode( 530, 400 ) Local p6 : PathNode = New PathNode( 400, 320 ) Local p7 : PathNode = New PathNode( 470, 160 ) Local p8 : PathNode = New PathNode( 360, 30 ) Local p9 : PathNode = New PathNode( 580, 60 ) 'links the nodes PathNode.LinkNodes( p1, p2 ) PathNode.LinkNodes( p4, p1 ) PathNode.LinkNodes( p4, p2 ) PathNode.LinkNodes( p3, p4 ) PathNode.LinkNodes( p5, p6 ) PathNode.LinkNodes( p6, p2 ) PathNode.LinkNodes( p7, p2 ) PathNode.LinkNodes( p8, p2 ) PathNode.LinkNodes( p8, p1 ) PathNode.LinkNodes( p7, p8 ) PathNode.LinkNodes( p7, p9 ) PathNode.LinkNodes( p9, p5 ) End Method Method OnUpdate() 'key s sets start node If KeyHit( KEY_S ) Local check : PathNode = PathNode.FindClosestNode( MouseX(), MouseY()) If GoalNode <> check Then StartNode = check Endif 'key g sets goal node If KeyHit( KEY_G ) Local check : PathNode = PathNode.FindClosestNode( MouseX(), MouseY()) If StartNode <> check Then GoalNode = check Endif 'starts pathfinding If KeyHit( KEY_ENTER ) And StartNode <> Null And GoalNode <> Null 'create New path and add starting node Path.PathList.Clear() New Path( StartNode ) 'loop until path is found (this could be done one game loop at a time as the logic works) Repeat 'grab first path in list Local First : Path = Path.PathList.RemoveFirst() Local FirstNode : PathNode = First.NodeList.Last() 'grab all neighbours and extend path Local extend : List< PathExtends > = New List< PathExtends > Local Linked : PathLink For Linked = Eachin FirstNode.NodeLink 'add each neighbour to temporary extend list Local e : PathExtends = New PathExtends( Linked.LinkNode, Linked.Distance ) extend.AddLast( e ) Next 'check the first path for any loops with extends Local cNode : PathNode For cNode = Eachin First.NodeList Local ext : PathExtends For ext = Eachin extend If ext.pNode = cNode extend.Remove( ext ) Endif Next Next 'keep only the shortest paths from extend and pathlist Local shortPath : Path For shortPath = Eachin Path.PathList Local lastNode : PathNode = shortPath.NodeList.Last() Local ext : PathExtends For ext = Eachin extend If ext.pNode = lastNode If ext.pDist + First.Distance < shortPath.Distance Path.PathList.Remove( shortPath ) Exit Else extend.Remove( ext ) Endif Endif Next Next 'add remaining extended paths to path list Local ext : PathExtends For ext = Eachin extend Local newP : Path = First.Copy() newP.Distance = newP.Distance + ext.pDist newP.NodeList.AddLast( ext.pNode ) Local dx : Float = ext.pNode.x - GoalNode.x Local dy : Float = ext.pNode.y - GoalNode.y newP.Underestimate = Sqrt( dx * dx + dy * dy ) Next 'sort paths based on distance travelled and underestimate Path.PathList.Sort( False ) 'if the first path isnt at the goal, go back and repeat the steps again Until GoalNode = Path.PathList.First().NodeList.Last() Endif End Method Method OnRender() Cls 255, 255, 255 'draws node map PathNode.Render() 'writes the path order if a path was found Local FoundPath : Path = Path.PathList.First() If FoundPath Local eNode : PathNode Local s : String = "" For eNode = Eachin FoundPath.NodeList s = s + eNode.ID If eNode <> GoalNode s = s + "->" Endif Next DrawText s, 10, 10 Endif End Method End Class Function Main() New MyApp() End Function

NoOdle (Posted 2011) [#4]

Here is a newer demo of the both examples combined. Left mouse places wall tiles on the grid D Key deletes the wall tile under the cursor S places start location for pathfinding G places goal location for the pathfinding Before you can run the pathfinding you have to create the node map. This would be done when the level is loaded but for this example it is dynamically created during runtime. Press Space to create the node map and links Then you can press enter to run the pathfinding. If Start -> Goal is visible it will show a link between the two. Otherwise it will highlight the waypoints and the path route will be displayed in the left hand corner. You can edit the tiles at any point, but remember to press Space to recreate the node map before running the pathfinding again by pressing Enter. Enjoy :) Import mojo Global StartNode : PathNode = Null Global GoalNode : PathNode = Null Global StartX : Int = -1 Global StartY : Int = -1 Global GoalX : Int = -1 Global GoalY : Int = -1 Global isLOS : Bool = False Global TileMap : Int[ 20 ][] Class PathLink Field LinkNode : PathNode Field Distance : Float Method New( n : PathNode, d : Float ) Self.LinkNode = n Self.Distance = d End Method End Class Class PathNode Global NodeCount : Int Global NodeList : List< PathNode > = New List< PathNode > Field x : Int Field y : Int Field ID : Int Field NodeLink : List< PathLink > Method New( _x : Int, _y : Int ) NodeCount = NodeCount + 1 Self.x = _x Self.y = _y Self.NodeLink = New List< PathLink > Self.ID = NodeCount NodeList.AddLast( Self ) End Method Method DrawNode() SetColor 0, 0, 0 DrawOval Self.x - 20, Self.y - 20, 40, 40 SetColor 255, 255, 255 Local foundP : Path = Path.PathList.First() If foundP Local n : PathNode For n = Eachin foundP.NodeList If n = Self SetColor 255, 255, 0 Endif Next Endif If StartNode = Self Then SetColor 0, 255, 0 If GoalNode = Self Then SetColor 255, 0, 0 DrawOval Self.x - 16, Self.y - 16, 32, 32 SetColor 255, 255, 255 DrawText Self.ID, Self.x, Self.y, 0.4, 0.4 End Method Method DrawLink() Local this : PathLink For this = Eachin Self.NodeLink SetColor 0, 0, 0 DrawLine Self.x, Self.y, this.LinkNode.x, this.LinkNode.y Next End Method Function FindClosestNode : PathNode( x : Int, y : Int ) Local this : PathNode Local dist : Float = -1 Local closestNode : PathNode = Null For this = Eachin NodeList Local dx : Float = this.x - x Local dy : Float = this.y - y Local d : Float = Sqrt( dx * dx + dy * dy ) If dist = -1 Or d < dist dist = d closestNode = this Endif Next Return closestNode End Function Function CheckForNode : Bool( x, y ) Local this : PathNode For this = Eachin NodeList If this.x = x And this.y = y Return True Endif Next Return False End Function Function CreateNode( x, y ) If CheckForNode( x, y ) = False New PathNode( x, y ) Endif End Function Function LinkNodes( a : PathNode, b : PathNode ) Local dx : Float = a.x - b.x Local dy : Float = a.y - b.y Local dist : Float = Sqrt( dx * dx + dy * dy ) Local LinkA : PathLink = New PathLink( b, dist ) Local LinkB : PathLink = New PathLink( a, dist ) a.NodeLink.AddLast( LinkA ) b.NodeLink.AddLast( LinkB ) End Function Function Render() Local this : PathNode For this = Eachin NodeList this.DrawLink() Next For this = Eachin NodeList this.DrawNode() Next End Function 'creates nodes at corner locations Function CreateNodeMap() For Local x : Int = 0 To 19 For Local y : Int = 0 To 14 Local cu : Int = y - 1 Local cr : Int = x + 1 Local cd : Int = y + 1 Local cl : Int = x - 1 If TileMap[ x ][ y ] = 1 If cu > -1 And cr < 20 If TileMap[ x ][ cu ] = 0 And TileMap[ cr ][ cu ] = 0 And TileMap[ cr ][ y ] = 0 PathNode.CreateNode( cr * 32 + 16, cu * 32 + 16 ) Endif Endif If cu > -1 And cl > -1 If TileMap[ x ][ cu ] = 0 And TileMap[ cl ][ cu ] = 0 And TileMap[ cl ][ y ] = 0 PathNode.CreateNode( cl * 32 + 16, cu * 32 + 16 ) Endif Endif If cd < 15 And cr < 20 If TileMap[ x ][ cd ] = 0 And TileMap[ cr ][ cd ] = 0 And TileMap[ cr ][ y ] = 0 PathNode.CreateNode( cr * 32 + 16, cd * 32 + 16 ) Endif Endif If cd < 15 And cl > -1 If TileMap[ x ][ cd ] = 0 And TileMap[ cl ][ cd ] = 0 And TileMap[ cl ][ y ] = 0 PathNode.CreateNode( cl * 32 + 16, cd * 32 + 16 ) Endif Endif Endif Next Next End Function 'links nodes based on visibilty Function CreateNodeLinks() Local this : PathNode Local LinkCC : Int = 0 For this = Eachin PathNode.NodeList Local target : PathNode For target = Eachin PathNode.NodeList If target <> this Local dx : Float = target.x - this.x Local dy : Float = target.y - this.y Local len : Float = Sqrt( dx * dx + dy * dy ) Local didCollide = False If len <> 0 dx = dx / len dy = dy / len Local x1 : Float = this.x Local y1 : Float = this.y For Local n = 0 To len Local ax : Int = x1 / 32 Local ay : Int = y1 / 32 'if line passes through tile that is solid, then exit with collision flag If ax > 0 And ay > 0 And ax < 20 And ay < 15 If TileMap[ ax ][ ay ] = 1 didCollide = True Exit Endif Endif 'sample area around x1, y1 and check if solid tile (this stops links being to close to a wall) Local aSize : Float = 10 For Local x2 = x1 - aSize To x1 + aSize For Local y2 = y1 - aSize To y1 + aSize Local ax2 : Int = x2 / 32 Local ay2 : Int = y2 / 32 If ax2 > 0 And ay2 > 0 And ax2 < 20 And ay2 < 15 If TileMap[ ax2 ][ ay2 ] = 1 didCollide = True Exit Endif Endif Next Next If didCollide = True Then Exit 'check if x1, y1 lies on another node position (checks unneccessary links that pass through an existing node) Local checkA : PathNode For checkA = Eachin PathNode.NodeList If checkA <> this And checkA <> target Local ndx : Float = x1 - checkA.x Local ndy : Float = y1 - checkA.y Local nDist : Float = Sqrt( ndx * ndx + ndy * ndy ) If nDist < aSize didCollide = True Exit Endif Endif Next If didCollide = True Then Exit x1 = x1 + dx y1 = y1 + dy Next If didCollide = False PathNode.LinkNodes( this, target ) LinkCC = LinkCC + 1 Endif Endif Endif Next Next Print LinkCC End Function Function ResetNodes() PathNode.NodeList.Clear() PathNode.NodeCount = 0 End Function Function FindClosestVisibleNode : PathNode( targetX : Float, targetY : Float ) Local ClosestDist = -1 Local ClosestNode : PathNode = Null For Local this : PathNode = Eachin PathNode.NodeList Local result : Float = LOS( targetX, targetY, this.x, this.y ) If result <> -1 If result < ClosestDist Or ClosestDist = -1 ClosestDist = result ClosestNode = this Endif Endif Next Return ClosestNode End Function End Class 'los of function Function LOS : Int( lx1, ly1, lx2, ly2 ) Local dx : Float = lx2 - lx1 Local dy : Float = ly2 - ly1 Local len : Float = Sqrt( dx * dx + dy * dy ) Local didCollide : Bool = False If len <> 0 dx = dx / len dy = dy / len Local x1 : Float = lx1 Local y1 : Float = ly1 For Local n = 0 To len Local ax : Int = x1 / 32 Local ay : Int = y1 / 32 If ax > 0 And ax < 20 And ay > 0 And ay < 15 If TileMap[ ax ][ ay ] = 1 didCollide = True Exit Endif Endif x1 = x1 + dx y1 = y1 + dy Next If didCollide = False 'los true so return distance to target Return len Endif Endif 'no line of sight, return -1 as distance Return -1.0 End Function Class PathSortList Extends List< Path > Method Compare : Int( a : Path, b : Path ) If a.Distance + a.Underestimate < b.Distance + b.Underestimate Return 1 If a.Distance + a.Underestimate > b.Distance + b.Underestimate Return -1 Return 0 End Method End Class Class Path Global PathList : PathSortList = New PathSortList Field NodeList : List< PathNode > Field Distance : Float Field Underestimate : Float Method New( start : PathNode ) Self.NodeList = New List< PathNode > Self.NodeList.AddLast( start ) PathList.AddLast( Self ) End Method Method Copy : Path() Local this : Path = New Path this.Distance = Self.Distance this.Underestimate = Self.Underestimate this.NodeList = New List< PathNode > Local thisNode : PathNode For thisNode = Eachin Self.NodeList this.NodeList.AddLast( thisNode ) Next PathList.AddLast( this ) Return this End Method Function CalculatePath() 'create New path and add starting node Path.PathList.Clear() New Path( StartNode ) 'loop until path is found (this could be done one game loop at a time as the logic works) Repeat 'grab first path in list Local First : Path = Path.PathList.RemoveFirst() Local FirstNode : PathNode = First.NodeList.Last() 'grab all neighbours and extend path Local extend : List< PathExtends > = New List< PathExtends > Local Linked : PathLink For Linked = Eachin FirstNode.NodeLink 'add each neighbour to temporary extend list Local e : PathExtends = New PathExtends( Linked.LinkNode, Linked.Distance ) extend.AddLast( e ) Next 'check the first path for any loops with extends Local cNode : PathNode For cNode = Eachin First.NodeList Local ext : PathExtends For ext = Eachin extend If ext.pNode = cNode extend.Remove( ext ) Endif Next Next 'keep only the shortest paths from extend and pathlist Local shortPath : Path For shortPath = Eachin Path.PathList Local lastNode : PathNode = shortPath.NodeList.Last() Local ext : PathExtends For ext = Eachin extend If ext.pNode = lastNode If ext.pDist + First.Distance < shortPath.Distance Path.PathList.Remove( shortPath ) Exit Else extend.Remove( ext ) Endif Endif Next Next 'add remaining extended paths to path list Local ext : PathExtends For ext = Eachin extend Local newP : Path = First.Copy() newP.Distance = newP.Distance + ext.pDist newP.NodeList.AddLast( ext.pNode ) Local dx : Float = ext.pNode.x - GoalNode.x Local dy : Float = ext.pNode.y - GoalNode.y newP.Underestimate = Sqrt( dx * dx + dy * dy ) Next 'sort paths based on distance travelled and underestimate Path.PathList.Sort( False ) 'if the first path isnt at the goal, go back and repeat the steps again Until GoalNode = Path.PathList.First().NodeList.Last() End Function Function PrintPath() 'writes the path order if a path was found Local FoundPath : Path = Path.PathList.First() If FoundPath Local eNode : PathNode Local s : String = "" For eNode = Eachin FoundPath.NodeList s = s + eNode.ID If eNode <> GoalNode s = s + "->" Endif Next DrawText s, 10, 10 Endif End Function End Class Class PathExtends Field pDist : Float Field pNode : PathNode Method New( n : PathNode, d : Float ) Self.pDist = d Self.pNode = n End Method End Class Class MyApp Extends App Method OnCreate() SetUpdateRate 60 'creates tile map array For Local n = 0 To 19 TileMap[ n ] = New Int[ 15 ] Next End Method Method OnUpdate() 'get array mouse co-ords Local ax : Int = MouseX() / 32 Local ay : Int = MouseY() / 32 'adds cell to TileMap If MouseDown( MOUSE_LEFT ) If ax < 20 And ay < 15 And ax > -1 And ay > -1 TileMap[ ax ][ ay ] = 1 Endif Endif If KeyDown( KEY_D ) If ax < 20 And ay < 15 And ax > -1 And ay > -1 TileMap[ ax ][ ay ] = 0 Endif Endif 's places start node If KeyDown( KEY_S ) If ax < 20 And ay < 15 And ax > -1 And ay > -1 StartX = ax StartY = ay StartNode = Null isLOS = False Endif Endif 's places start node If KeyDown( KEY_G ) If ax < 20 And ay < 15 And ax > -1 And ay > -1 GoalX = ax GoalY = ay GoalNode = Null isLOS = False Endif Endif 'creates node map If KeyHit( KEY_SPACE ) 'reset stuff StartNode = Null GoalNode = Null isLOS = False 'Check the surrounding tiles up, right, down, left and adds node if none exist at that location PathNode.ResetNodes() PathNode.CreateNodeMap() PathNode.CreateNodeLinks() Endif 'starts pathfinding If KeyHit( KEY_ENTER ) And StartX <> -1 And StartY <> -1 And GoalX <> -1 And GoalY <> -1 'check line of sight between start and goal Local result : Float = LOS(( StartX * 32 ) + 16, ( StartY * 32 ) + 16, ( GoalX * 32 ) + 16, ( GoalY * 32 ) + 16 ) 'if visible If result <> -1 isLOS = True StartNode = Null GoalNode = Null Path.PathList.Clear() Else 'reset flag isLOS = False 'find start and goal nodes StartNode = PathNode.FindClosestVisibleNode( StartX * 32, StartY * 32 ) GoalNode = PathNode.FindClosestVisibleNode( GoalX * 32, GoalY * 32 ) 'Calculate path Path.CalculatePath() Endif Endif End Method Method OnRender() Cls 255, 255, 255 'draws feint grid SetColor 200, 200, 200 Local n : Int For n = 0 To DeviceWidth() Step 32 DrawLine n, 0, n, DeviceHeight() Next For n = 0 To DeviceHeight() Step 32 DrawLine 0, n, DeviceWidth(), n Next 'draws grid cells SetColor 100, 100, 100 For Local x : Int = 0 To 19 For Local y : Int = 0 To 14 If TileMap[ x ][ y ] DrawRect x * 32, y * 32, 32, 32 Endif Next Next 'draws start and goal SetColor 0, 0, 0 If isLOS = True DrawLine StartX * 32 + 16, StartY * 32 + 16, GoalX * 32 + 16, GoalY * 32 + 16 Endif SetColor 0, 255, 0 DrawRect StartX * 32, StartY * 32, 32, 32 If StartNode <> Null And isLOS = False DrawLine ( StartX * 32 ) + 16, ( StartY * 32 ) + 16, StartNode.x, StartNode.y Endif SetColor 255, 0, 0 DrawRect GoalX * 32, GoalY * 32, 32, 32 If GoalNode <> Null And isLOS = False DrawLine ( GoalX * 32 ) + 16, ( GoalY * 32 ) + 16, GoalNode.x, GoalNode.y Endif 'draws node map PathNode.Render() 'prints path Path.PrintPath() End Method End Class Function Main() New MyApp() End Function

NoOdle (Posted 2011) [#5]

I will be converting, cleaning and optimising this soon and will post it as a module. Please let me know if that will be useful, it will probably make me work a bit quicker ;)

VicViper (Posted 2011) [#6]

Your pathfinding code is useful. Great idea. Thanks you for your work. VicViper

slenkar (Posted 2011) [#7]

this will be useful thanks

Neuro (Posted 2011) [#8]

The is definitely pretty damm cool :). Actually, works pretty quick alreadt but wouldn't mind seeing it faster :)!