Pathfinding Node Based

Monkey Forums/Monkey Code/Pathfinding Node Based

NoOdle (Posted 2012) [#1]

Node based pathfinding. Controls: CreateNodes: Press mouse to place a Node on the screen. Create links: Press mouse on a node hold the mouse button down and drag to another node Delete: Press mouse and drag a line through the links and or nodes (bisect) Set Start: Hover mouse over a node and press S key Set Goal: Hover mouse over a node and press G key Start Pathfinding: Press Enter Import mojo Const PATHNODE_RADIUS = 20 Global START_NODE : PathNode = Null Global GOAL_NODE : PathNode = Null Class Path Field nodes : List< PathNode > Field travelledDistance : Float Field underestimate : Float Method New() Self.nodes = New List< PathNode > End Method Method Copy : Path() Local this : Path = New Path For Local n : PathNode = Eachin Self.nodes this.nodes.AddLast( n ) Next this.travelledDistance = Self.travelledDistance Return this End Method Method CalculateUnderestimate : Float( goal : PathNode ) If goal = Null Then Return Self.underestimate = nodes.Last().Distance( goal.x, goal.y ) End Method Method PrintPath : String() Local output : String For Local this : PathNode = Eachin Self.nodes output = output + String( this.id ) If this <> Self.nodes.Last() output = output + " -> " Endif Next Return output End Method Function Create : Path( start : PathNode ) Local this : Path = New Path this.nodes.AddLast( start ) Return this End Function End Class 'extends the list Class PathfindList Extends List < Path > 'compares score value Method Compare( a : Path, b : Path ) If a.travelledDistance + a.underestimate > b.travelledDistance + b.underestimate Then Return 1 If a.travelledDistance + a.underestimate < b.travelledDistance + b.underestimate Then Return -1 Return 0 End Method End Class 'static pathfind class Class PathFind Global start : PathNode Global goal : PathNode Global pathList : PathfindList = New PathfindList 'sets up the pathfinding Function Setup( node1 : PathNode, node2 : PathNode ) start = node1 goal = node2 pathList.Clear() pathList.AddLast( Path.Create( start )) pathList.First().CalculateUnderestimate( node2 ) End Function 'finds a path Function FindPath : Bool() Repeat 'if pathList is empty then exit If pathList.Count = 0 Then Return False 'if first path ends at goal then we have found our path If pathList.First().nodes.Last() = goal Then Return True 'extract the first path Local firstPath : Path = pathList.RemoveFirst() 'extend path to all neighbours creating x new paths For Local neighbour : PathLink = Eachin firstPath.nodes.Last().linked 'copy path Local newPath : Path = firstPath.Copy() 'get the neighbour node Local newNode : PathNode = neighbour.linkedNode 'reject if loop For Local child : PathNode = Eachin newPath.nodes If child = newNode newPath = Null Exit Endif Next 'if newPath is null we found a loop If newPath <> Null 'add new node and travel cost newPath.nodes.AddLast( newNode ) newPath.travelledDistance = newPath.travelledDistance + neighbour.cost Endif 'For all paths that End at the same node, keep only the shortest one. If newPath <> Null For Local shortPath : Path = Eachin pathList 'if ends in the same node If shortPath.nodes.Last() = newNode 'keep the shortest If shortPath.travelledDistance <= newPath.travelledDistance newPath = Null Exit Else pathList.RemoveEach( shortPath ) Exit Endif Endif Next Endif 'if we still have a valid new path add it to the list If newPath <> Null pathList.AddLast( newPath ) newPath.CalculateUnderestimate( goal ) Endif Next 'Sort all paths by total underestimate, shortest first. pathList.Sort( True ) Forever End Function End Class Class PathLink Field linkedNode : PathNode Field cost : Float Function Create : PathLink( node1 : PathNode, node2 : PathNode ) Local this : PathLink = New PathLink this.linkedNode = node2 this.cost = node1.Distance( node2.x, node2.y ) Return this End Function End Class Class PathNode Global nodeCount : Int = 0 Global nodeList : List< PathNode > = New List< PathNode > Field nodeId : list.Node< PathNode > Field id : Int Field x : Int Field y : Int Field linked : List< PathLink > 'links two nodes Method Link( other : PathNode ) 'if same node then exit If other = Self Then Return 'if link already exists then exit For Local this : PathLink = Eachin Self.linked If this.linkedNode = other Then Return Next 'create links other.linked.AddLast( PathLink.Create( other, Self )) Self.linked.AddLast( PathLink.Create( Self, other )) End Method 'draws node, coloured to indicate start/goal Method Draw() SetColor 255, 255, 255 DrawOval Self.x - PATHNODE_RADIUS, Self.y - PATHNODE_RADIUS, PATHNODE_RADIUS * 2, PATHNODE_RADIUS * 2 SetColor 0, 0, 0 If GOAL_NODE = Self Then SetColor 160, 0, 0 If START_NODE = Self Then SetColor 0, 160, 0 DrawOval Self.x - PATHNODE_RADIUS + 1, Self.y - PATHNODE_RADIUS + 1, PATHNODE_RADIUS * 2 - 2, PATHNODE_RADIUS * 2 - 2 SetColor 255, 255, 255 DrawText Self.id, Self.x, Self.y, 0.5, 0.5 End Method 'draws a line to all linked nodes Method DrawLinked() SetColor 255, 255, 255 For Local this : PathLink = Eachin Self.linked DrawLine Self.x, Self.y, this.linkedNode.x, this.linkedNode.y Next End Method 'returns distance from self to point x, y Method Distance : Float( px : Int, py : Int ) Local dx : Int = px - Self.x Local dy : Int = py - Self.y Return Sqrt( dx * dx + dy * dy ) End Method Function Create : PathNode( px : Int, py : Int ) nodeCount = nodeCount + 1 Local this : PathNode = New PathNode this.id = nodeCount this.x = px this.y = py this.nodeId = nodeList.AddLast( this ) this.linked = New List< PathLink > Return this End Function Function DrawNodes() If nodeList.Count = 0 Then Return For Local this : PathNode = Eachin nodeList this.Draw() Next End Function Function DrawLinks() If nodeList.Count = 0 Then Return For Local this : PathNode = Eachin nodeList this.DrawLinked() Next End Function Function Touch : PathNode( px : Int, py : Int ) If nodeList.Count = 0 Then Return Null 'loop all nodes and return if we are inside For Local this : PathNode = Eachin nodeList If this.Distance( px, py ) < PATHNODE_RADIUS Return this Endif Next Return Null End Function Function RemoveIntersectingNodes( x1 : Int, y1 : Int, x2 : Int, y2 : Int ) For Local this : PathNode = Eachin nodeList 'if line is intersecting the node If DistanceToLineSegment( x1, y1, x2, y2, this.x, this.y ) <= PATHNODE_RADIUS 'find all linked nodes to this one For Local link : PathLink = Eachin this.linked 'loop all of their links and remove link to this Local testNode : PathNode = link.linkedNode For Local rLink : PathLink = Eachin testNode.linked If rLink.linkedNode = this testNode.linked.RemoveEach( rLink ) Exit Endif Next Next 'remove the node this.nodeId.Remove() Endif Next End Function Function RemoveIntersectingLinks( x1 : Int, y1 : Int, x2 : Int, y2 : Int ) 'cycle all nodes For Local this : PathNode = Eachin nodeList 'cycle all links For Local link : PathLink = Eachin this.linked 'if the line intersects the link If Intersect( x1, y1, x2, y2, this.x, this.y, link.linkedNode.x, link.linkedNode.y ) 'remove the link this.linked.RemoveEach( link ) 'check and remove link from other node For Local rLink : PathLink = Eachin link.linkedNode.linked If rLink.linkedNode = this link.linkedNode.linked.RemoveEach( rLink ) Exit Endif Next Endif Next Next End Function End Class ' 2D math functions by Jasu 'http://blitzbasic.com/codearcs/codearcs.php?code=2180 Function Intersect : Int( x1 : Float, y1 : Float, x2 : Float, y2 : Float, x3 : Float, y3 : Float, x4 : Float, y4 : Float) ' This Function returns True If lines x1,y1,x2,y2 And x3,y3,x4,y4 intersect at some point. Return (Orientation(x1, y1, x2, y2, x3, y3) <> Orientation(x1, y1, x2, y2, x4, y4)) And (Orientation(x3, y3, x4, y4, x1, y1) <> Orientation(x3, y3, x4, y4, x2, y2)) End Function Function Orientation : Int ( x1 : Float,y1 : Float, x2 : Float,y2 : Float, Px : Float,Py : Float ) ' Linear determinant of the 3 points. ' This Function returns the orientation of px,py on line x1,y1,x2,y2. ' Look from x2,y2 To the direction of x1,y1. ' If px,py is on the right, Function returns +1 ' If px,py is on the left, Function returns -1 ' If px,py is directly ahead Or behind, Function returns 0 Return Sgn((x2 - x1) * (Py - y1) - (Px - x1) * (y2 - y1)) End Function Function DistanceToLineSegment : Float ( x1 : Float,y1 : Float, x2 : Float,y2 : Float, Px : Float, Py : Float ) ' This Function calculates the distance between a line segment And a point. ' So this Function is useful To determine If line intersects a circle. ' To also determine the point on the line x1,y1,x2,y2 which is the closest To px,py , use Function NearestPointInLine# Local Dx : Float Local Dy : Float Local Ratio : Float If (x1 = x2) And (y1 = y2) Then Return Sqrt( (Px-x1)*(Px-x1)+(Py-y1)*(Py-y1) ) Else Dx = x2 - x1 Dy = y2 - y1 Ratio = ((Px - x1) * Dx + (Py - y1) * Dy) / (Dx * Dx + Dy * Dy) If Ratio < 0 Then Return Sqrt( (Px-x1)*(Px-x1)+(Py-y1)*(Py-y1) ) Elseif Ratio > 1 Then Return Sqrt( (Px-x2)*(Px-x2)+(Py-y2)*(Py-y2) ) Else Return Sqrt((Px - ((1 - Ratio) * x1 + Ratio * x2))*(Px - ((1 - Ratio) * x1 + Ratio * x2))+(Py - ((1 - Ratio) * y1 + Ratio * y2))*(Py - ((1 - Ratio) * y1 + Ratio * y2))) Endif Endif End Function Class MyApp Extends App 'stores mouse positions Field mx : Int Field my : Int 'bool to detect mouse pressing Field mousePressed : Int 'stores clicked nodes Field node1 : PathNode Field node2 : PathNode 'stores found path Field foundPath : Path Field foundTime : Int Method OnCreate() SetUpdateRate 60 End Method Method OnUpdate() 'finds a path If KeyHit( KEY_ENTER ) 'sets up pathfinding routing foundPath = Null PathFind.Setup( START_NODE, GOAL_NODE ) 'starts pathfinding foundTime = Millisecs() Local result : Bool = PathFind.FindPath() foundTime = Millisecs() - foundTime 'stores found path If result = True Then foundPath = PathFind.pathList.First() Endif 'sets start node If KeyHit( KEY_S ) Local test : PathNode = PathNode.Touch( MouseX(), MouseY() ) If test <> Null Then START_NODE = test Endif 'sets goal node If KeyHit( KEY_G ) Local test : PathNode = PathNode.Touch( MouseX(), MouseY() ) If test <> Null Then GOAL_NODE = test Endif 'handles mouse If MouseDown( MOUSE_LEFT ) = 1 'store initial mouse click location (used to detect mouse movement) If mousePressed = 0 'grab mouse position mx = MouseX() my = MouseY() 'grab clicked node if any node1 = PathNode.Touch( mx, my ) Endif 'mouse has been pressed mousePressed = 1 Endif If MouseDown( MOUSE_LEFT ) = 0 And mousePressed = 1 'if we didnt click a path node If node1 = Null 'if the mouse position has not changed since we first clicked If MouseX() = mx And MouseY() = my 'create a path node PathNode.Create( mx, my ) 'position has moved so we have dragged an invisible line Else 'use the mouse path to find intersecting nodes and links and remove them PathNode.RemoveIntersectingNodes( mx, my, MouseX(), MouseY() ) PathNode.RemoveIntersectingLinks( mx, my, MouseX(), MouseY() ) Endif 'if we did click inside a node Else 'grab clicked node if any node2 = PathNode.Touch( MouseX(), MouseY() ) 'check if we intersected with another node If node2 <> Null And node2 <> node1 'create a link node1.Link( node2 ) Endif Endif 'reset mouse mousePressed = 0 node1 = Null Endif End Method Method OnRender() Cls 0, 0, 0 'draws mouse line If mousePressed = 1 If MouseX() <> mx Or MouseY() <> my DrawLine mx, my, MouseX(), MouseY() Endif Endif 'draws node network PathNode.DrawLinks() PathNode.DrawNodes() 'draws path If foundPath <> Null SetColor 255, 255, 255 DrawText foundPath.PrintPath() + " " + foundTime + "ms", 10, 0 Endif End Method End Class Function Main() New MyApp() End Function

pinete (Posted 2012) [#2]

Thanks!