Path Finding Example

Advanced game related topics
User avatar
J. Baker
Addict
Addict
Posts: 2178
Joined: Sun Apr 27, 2003 8:12 am
Location: USA
Contact:

Path Finding Example

Post by J. Baker »

I edited the following in order to create a Player point and click path example...
viewtopic.php?f=16&t=27638&hilit=%2Aunit%5CpathBank

XIncludeFile "aStarLibrary.pbi"

Code: Select all

;A* Pathfinder (Version 1.82) by Patrick Lester. Used by permission.
;==================================================================

;An article describing A* and this code in particular can be found at:
;http://www.policyalmanac.org/games/aStarTutorial.htm

;Setup
;-----
;1. Include "aStarLibrary.pbi" at the top of your program.

;2. Create an array called walkability(x,y) that contains information
;  about the walkability of each square/tile on your map, with
;  0 = walkable (the default value) and 1 = unwalkable. The array
;  should range from (0,0) in the upper left hand corner to 
;  (mapWidth-1,mapHeight-1) in the bottom right hand corner.

;3. Adjust the following variables at the top of the .declareVariables
;  subroutine below. All three should be made global.
;  - tileSize = the width and height of your square tiles in pixels
;  - mapWidth = the width of your map in tiles = x value in
;    walkability array.
;  - mapHeight = the height of your map in tiles = y value in
;    walkability array.


;Calling the Procedures
;---------------------
;There are three main Procedures

;1.  FindPath(unit.unit,targetX,targetY)
;  - unit.unit = unit that is doing the pathfinding
;  - targetX,targetY = location of the target destination (pixel based coordinates)

;  The FindPath() Procedure returns whether a path could be found (1) or
;  if it's nonexistent (2). If there is a path, it stores it in a bank
;  called unit\pathBank.

;2.   CheckPathStepAdvance(unit.unit)
;  This Procedure updates the current path.

;3.  ReadPath(unit.unit)
;   This Procedure reads the path data generated by FindPath() and returns
;  the x and y coordinates of the next step on the path. They are stored
;  as xPath and yPath. These coordinates are pixel coordinates 
;  on the screen. See the Procedure for more info.

;==========================================================
;DECLARE VARIABLES
;Adjust these variables to match your map dimensions (see "setup" above)
Global tileSize = #Path_Grid, mapWidth = #Scr_Width, mapHeight = #Scr_Height

;Create needed arrays
Global Dim walkability(mapWidth+1,mapHeight+1) ;array that holds wall/obstacle information  
Global Dim openlist(mapWidth*mapHeight+2) ;1 dimensional array holding ID# of open list items
Global Dim whichList(mapWidth+1,mapHeight+1)  ;2 dimensional array used to record 
;whether a cell is on the open list or on the closed list.
Global Dim openX(mapWidth*mapHeight+2) ;1d array stores the x location of an item on the open list
Global Dim openY(mapWidth*mapHeight+2) ;1d array stores the y location of an item on the open list
Global Dim parentX(mapWidth+1,mapHeight+1) ;2d array to store parent of each cell (x)
Global Dim parentY(mapWidth+1,mapHeight+1) ;2d array to store parent of each cell (y)
Global Dim Fcost(mapWidth*mapHeight+2)  ;1d array to store F cost of a cell on the open list
Global Dim Gcost(mapWidth+1,mapHeight+1)   ;2d array to store G cost for each cell.
Global Dim Hcost(mapWidth*mapHeight+2)  ;1d array to store H cost of a cell on the open list    

;Declare constants
Global onClosedList = 10 ;openList variable  
#notfinished = 0
#notStarted = 0
#found = 1
#nonexistent = 2; pathStatus constants 
#walkable = 0
#unwalkable = 1; walkability array constants

Structure unit
  xLoc.l
  yLoc.l
  targetX.l
  targetY.l
  pathLocation.l
  pathLength.l
  pathBank.l
  xPath.l
  yPath.l
  pathStatus.l
EndStructure

Global unit.unit
unit\pathBank = AllocateMemory(1)

;{ Declarations
Declare CheckPathStepAdvance(*unit.unit)
Declare ReadPathY(*unit.unit,pathLocation)
Declare ReadPathX(*unit.unit,pathLocation)
Declare ReadPath(*unit.unit)      
Declare FindPath(*unit.unit,targetX,targetY)
;}

;==========================================================
;FIND PATH: This Procedure finds the path and saves it. Non-Blitz users please note,
;the first parameter is a pointer to a user-defined object called a unit, which contains all
;relevant info about the unit in question (its current location, speed, etc.). As an
;object-oriented data structure, types are similar to structs in C.
;  Please note that targetX and targetY are pixel-based coordinates relative to the
;upper left corner of the map, which is 0,0.
Procedure FindPath(*unit.unit,targetX,targetY)
  
;1.  Convert location data (in pixels) to coordinates in the walkability array.
  startx = Round(*unit\xLoc/tileSize,0) : starty = Round(*unit\yLoc/tileSize,0)  
  targetX = Round(targetX/tileSize,0) : targetY = Round(targetY/tileSize,0)
;2.  Quick Path Checks: Under the some circumstances no path needs to
  ;be generated ...
  
  ;If starting location and target are in the same location...
  If startx = targetX And starty = targetY And *unit\pathLocation > 0 
    ProcedureReturn #found
  EndIf
  If startx = targetX And starty = targetY And *unit\pathLocation = 0 
    ProcedureReturn #nonexistent
  EndIf
  ;If target square is unwalkable, return that it's a nonexistent path.
  If walkability(targetX,targetY) = #unwalkable 
    Goto noPath
  EndIf
  
;3.  Reset some variables that need to be cleared
  If onClosedList > 1000000 ;occasionally redim whichList
    ReDim whichList(mapWidth,mapHeight) 
    onClosedList = 10
  EndIf
  onClosedList = onClosedList+2 ;changing the values of onOpenList and onClosed list is faster than redimming whichList() array
  onOpenList = onClosedList-1
  *unit\pathLength = #notStarted ;i.e, = 0
  *unit\pathLocation = #notStarted ;i.e, = 0
  Gcost(startx,starty) = 0 ;reset starting square's G value to 0
  
;4.  Add the starting location to the open list of squares to be checked.
  numberOfOpenListItems = 1
  openlist(1) = 1 ;assign it as the top (and currently only) item in the open list, which is maintained as a binary heap (explained below)
  openX(1) = startx : openY(1) = starty
  
  
  ;5.  Do the following until a path is found or deemed nonexistent.
  Repeat
  
  
;6.  If the open list is not empty, take the first cell off of the list.
  ;This is the lowest F cost cell on the open list.
  If numberOfOpenListItems <> 0
  
  ;Pop the first item off the open list.
  parentXval = openX(openlist(1)) : parentYVal = openY(openlist(1)) ;record cell coordinates of the item
  whichList(parentXval,parentYVal) = onClosedList ;add the item to the closed list
  
  ;Open List = Binary Heap: Delete this item from the open list, which
  ;is maintained as a binary heap. For more information on binary heaps, see:
  ;http://www.policyalmanac.org/games/binaryHeaps.htm
  numberOfOpenListItems = numberOfOpenListItems - 1 ;reduce number of open list items by 1  
  openlist(1) = openlist(numberOfOpenListItems+1) ;move the last item in the heap up to slot #1
  v = 1  
  Repeat ;Repeat the following until the new item in slot #1 sinks to its proper spot in the heap.
  u = v  
  If 2*u+1 <= numberOfOpenListItems ;if both children exist
    ;Check if the F cost of the parent is greater than each child.
    ;Select the lowest of the two children.  
    If Fcost(openlist(u)) >= Fcost(openlist(2*u)) 
      v = 2*u
      EndIf
      If Fcost(openlist(v)) >= Fcost(openlist(2*u+1)) 
      v = 2*u+1    
      EndIf
    Else
      If 2*u <= numberOfOpenListItems ;if only child #1 exists
      ;Check if the F cost of the parent is greater than child #1  
      If Fcost(openlist(u)) >= Fcost(openlist(2*u)) 
        v = 2*u
        EndIf
      EndIf  
    EndIf
    If u<>v ;if parent's F is > one of its children, swap them
    temp = openlist(u)
    openlist(u) = openlist(v)
    openlist(v) = temp        
    Else
    Break ;otherwise, exit loop
  EndIf  
  ForEver
  
  
;7.  Check the adjacent squares. (Its "children" -- these path children
  ;are similar, conceptually, to the binary heap children mentioned
  ;above, but don't confuse them. They are different. Path children
  ;are portrayed in Demo 1 with grey pointers pointing toward
  ;their parents.) Add these adjacent child squares to the open list
  ;for later consideration if appropriate (see various if statements
  ;below).
  For b = parentYVal-1 To parentYVal+1
    For a = parentXval-1 To parentXval+1

      ;If not off the map (do this first to avoid array out-of-bounds errors)
      If a <> -1 And b <> -1 And a <> mapWidth And b <> mapHeight
    
        ;If not already on the closed list (items on the closed list have
        ;already been considered and can now be ignored).      
        If whichList(a,b) <> onClosedList 
      
          ;If not a wall/obstacle square.
          If walkability(a,b) <> #unwalkable 
        
            ;Don't cut across corners (this is optional)
            corner = #walkable  
            If a = parentXval-1 
              If b = parentYVal-1 
                If walkability(parentXval-1,parentYVal) = #unwalkable Or walkability(parentXval,parentYVal-1) = #unwalkable 
                  corner = #unwalkable
                EndIf
              ElseIf b = parentYVal+1 
                If walkability(parentXval,parentYVal+1) = #unwalkable Or walkability(parentXval-1,parentYVal) = #unwalkable 
                corner = #unwalkable 
                EndIf
              EndIf
            ElseIf a = parentXval+1 
              If b = parentYVal-1 
                If walkability(parentXval,parentYVal-1) = #unwalkable Or walkability(parentXval+1,parentYVal) = #unwalkable 
                  corner = #unwalkable 
                EndIf
              ElseIf b = parentYVal+1 
                If walkability(parentXval+1,parentYVal) = #unwalkable Or walkability(parentXval,parentYVal+1) = #unwalkable 
                corner = #unwalkable 
                EndIf
              EndIf
            EndIf
        
            If corner = #walkable
              ;If not already on the open list, add it to the open list.      
              If whichList(a,b) <> onOpenList  
  
                ;Create a new open list item in the binary heap.
                newOpenListItemID = newOpenListItemID + 1; each new item has a unique ID #
                m = numberOfOpenListItems+1
                openlist(m) = newOpenListItemID   ;place the new open list item (actually, its ID#) at the bottom of the heap
                openX(newOpenListItemID) = a : openY(newOpenListItemID) = b ;record the x and y coordinates of the new item
            
                ;Figure out its G cost
                If Abs(a-parentXval) = 1 And Abs(b-parentYVal) = 1
                  addedGCost = 14 ;cost of going to diagonal squares  
                Else  
                  addedGCost = 10 ;cost of going to non-diagonal squares        
                EndIf
                Gcost(a,b) = Gcost(parentXval,parentYVal)+addedGCost
              
                ;Figure out its H and F costs and parent
                Hcost(openlist(m)) = 10*(Abs(a - targetX) + Abs(b - targetY)) ; record the H cost of the new square
                Fcost(openlist(m)) = Gcost(a,b) + Hcost(openlist(m)) ;record the F cost of the new square
                parentX(a,b) = parentXval : parentY(a,b) = parentYVal  ;record the parent of the new square  
              
                ;Move the new open list item to the proper place in the binary heap.
                ;Starting at the bottom, successively compare to parent items,
                ;swapping as needed until the item finds its place in the heap
                ;or bubbles all the way to the top (if it has the lowest F cost).
                While m <> 1 ;While item hasn't bubbled to the top (m=1)  
                  ;Check if child's F cost is < parent's F cost. If so, swap them.  
                  If Fcost(openlist(m)) <= Fcost(openlist(m/2))
                    temp = openlist(m/2)
                    openlist(m/2) = openlist(m)
                    openlist(m) = temp
                    m = m/2
                  Else
                    Break
                  EndIf
                Wend 
                numberOfOpenListItems = numberOfOpenListItems+1 ;add one to the number of items in the heap
              
                ;Change whichList to show that the new item is on the open list.
                whichList(a,b) = onOpenList
              
  ;8.  If adjacent cell is already on the open list, check to see if this 
      ;path to that cell from the starting location is a better one. 
      ;If so, change the parent of the cell and its G and F costs.  
              Else; If whichList(a,b) = onOpenList
              
                ;Figure out the G cost of this possible new path
                If Abs(a-parentXval) = 1 And Abs(b-parentYVal) = 1
                  addedGCost = 14;cost of going to diagonal tiles  
                Else  
                  addedGCost = 10 ;cost of going to non-diagonal tiles        
                EndIf
                tempGcost = Gcost(parentXval,parentYVal)+addedGCost
              
                ;If this path is shorter (G cost is lower) then change
                ;the parent cell, G cost and F cost.     
                If tempGcost < Gcost(a,b) ;if G cost is less,
                  parentX(a,b) = parentXval   ;change the square's parent
                  parentY(a,b) = parentYVal
                  Gcost(a,b) = tempGcost   ;change the G cost      
              
                  ;Because changing the G cost also changes the F cost, if
                  ;the item is on the open list we need to change the item's
                  ;recorded F cost and its position on the open list to make
                  ;sure that we maintain a properly ordered open list.
                  For x = 1 To numberOfOpenListItems ;look for the item in the heap
                    If openX(openlist(x)) = a And openY(openlist(x)) = b ;item found
                      Fcost(openlist(x)) = Gcost(a,b) + Hcost(openlist(x)) ;change the F cost
                    
                      ;See if changing the F score bubbles the item up from it's current location in the heap
                      m = x
                      While m <> 1 ;While item hasn't bubbled to the top (m=1)  
                        ;Check if child is < parent. If so, swap them.  
                        If Fcost(openlist(m)) < Fcost(openlist(m/2)) 
                          temp = openlist(m/2)
                          openlist(m/2) = openlist(m)
                          openlist(m) = temp
                          m = m/2
                        Else
                          Break ;while/wend
                        EndIf
                      Wend 
                      Break ;for x = loop
                    EndIf ;If openX(openList(x)) = a
                  Next ;For x = 1 To numberOfOpenListItems
                EndIf ;If tempGcost < Gcost(a,b) Then      
              EndIf ;If not already on the open list        
            EndIf ;If corner = walkable
          EndIf ;If not a wall/obstacle cell.  
        EndIf ;If not already on the closed list  
      EndIf ;If not off the map.  
    Next
  Next
  
;9.  If open list is empty then there is no path.  
  Else
    Path = #nonexistent 
    Break
  EndIf
  
  ;If target is added to open list then path has been found.
  If whichList(targetX,targetY) = onOpenList 
    Path = found
    Break
  EndIf
  
  ForEver ;repeat until path is found or deemed nonexistent
  
  
;10.  Save the path if it exists. Copy it to a bank. 
  If Path = found
    
    ;a. Working backwards from the target to the starting location by checking
    ;each cell's parent, figure out the length of the path.
    pathX = targetX : pathY = targetY  
    Repeat
      tempx = parentX(pathX,pathY)    
      pathY = parentY(pathX,pathY)
      pathX = tempx
      *unit\pathLength = *unit\pathLength + 1  
      Until pathX = startx And pathY = starty
      
      ;b. Resize the data bank to the right size (leave room to store step 0,
      ;which requires storing one more step than the length)
      ReAllocateMemory(*unit\pathBank, (*unit\pathLength+1)*4)
      
      ;c. Now copy the path information over to the databank. Since we are
      ;working backwards from the target to the start location, we copy
      ;the information to the data bank in reverse order. The result is
      ;a properly ordered set of path data, from the first step to the
      ;last.  
      pathX = targetX : pathY = targetY        
      cellPosition = *unit\pathLength*4 ;start at the end  
      While Not (pathX = startx And pathY = starty)      
      PokeW(*unit\pathBank+cellPosition,pathX) ;store x value  
      PokeW(*unit\pathBank+cellPosition+2,pathY) ;store y value  
      cellPosition = cellPosition - 4 ;work backwards    
      tempx = parentX(pathX,pathY)    
      pathY = parentY(pathX,pathY)
      pathX = tempx
    Wend  
    PokeW(*unit\pathBank,startx) ;store starting x value  
    PokeW(*unit\pathBank+2,starty) ;store starting y value
    
    FoundPath = 1
  Else
    FoundPath = 0
      
  EndIf ;If path = found Then 
  
  
;11. Return info on whether a path has been found.
  ProcedureReturn FoundPath; Returns 1 if a path has been found, 2 if no path exists. 
  
;12.If there is no path to the selected target, set the pathfinder's
  ;xPath and yPath equal to its current location and return that the
  ;path is nonexistent.
noPath:
  *unit\xPath = startingX
  *unit\yPath = startingY
  ProcedureReturn #nonexistent
  
EndProcedure


;==========================================================
;READ PATH DATA: These Procedures read the path data and convert
;it to screen pixel coordinates.
Procedure ReadPath(*unit.unit)      
  *unit\xPath = ReadPathX(*unit.unit,*unit\pathLocation)
  *unit\yPath = ReadPathY(*unit.unit,*unit\pathLocation)
EndProcedure

Procedure ReadPathX(*unit.unit,pathLocation)
  If pathLocation <= *unit\pathLength
    x = PeekW(*unit\pathBank+(pathLocation*4))
    ProcedureReturn tileSize * x + (1/2) * tileSize;align w/center of square  
  EndIf
EndProcedure  

Procedure ReadPathY(*unit.unit,pathLocation)
  If pathLocation <= *unit\pathLength
  y = PeekW(*unit\pathBank+(pathLocation*4+2))
  ProcedureReturn tileSize*y + (1/2) * tileSize ;align w/center of square
  EndIf
EndProcedure


;This Procedure checks whether the unit is close enough to the next
;path node to advance to the next one or, if it is the last path step,
;to stop.
Procedure CheckPathStepAdvance(*unit.unit)
  If (*unit\xLoc = *unit\xPath And *unit\yLoc = *unit\yPath) Or *unit\pathLocation = 0
  If *unit\pathLocation = *unit\pathLength 
    *unit\pathStatus = #notStarted  
  Else     
    *unit\pathLocation = *unit\pathLocation + 1
    ReadPath(*unit) ;update xPath and yPath
  EndIf  
  EndIf  
EndProcedure
Example.pb

Code: Select all

#Scr_Width = 1280 ;These three are needed to setup the include file.
#Scr_Height = 720
#Path_Grid = 40

XIncludeFile "aStarLibrary.pbi"

Procedure PathCollisions() ;Setup our collision(s) data.
  
  Restore Path_Collision_Data
    For Path_Y = 0 To #Scr_Height - #Path_Grid Step #Path_Grid
      For Path_X = 0 To #Scr_Width - #Path_Grid Step #Path_Grid
        Read.i Collision
        If Collision = 1
          walkability(Path_X / #Path_Grid, Path_Y / #Path_Grid) = 1
        EndIf
      Next Path_X
    Next Path_Y
  
EndProcedure

If InitSprite() = 0 Or InitKeyboard() = 0 Or InitMouse() = 0
    MessageRequester("Error", "Can't open DirectX", 0)
  End
EndIf

If OpenScreen(#Scr_Width, #Scr_Height, 32, "Path Finder")

    CreateSprite(0, 40, 40) ;Player
    StartDrawing(SpriteOutput(0))
      Box(0, 0, 40, 40, RGB(255, 0, 128))
    StopDrawing()
      
    CreateSprite(1, 40, 40) ;Collision
    StartDrawing(SpriteOutput(1))
      Box(0, 0, 40, 40, RGB(0, 255, 128))
    StopDrawing()
    
    CreateSprite(2, 40, 40) ;Path Point(s)
    StartDrawing(SpriteOutput(2))
      Box(0, 0, 40, 40, RGB(255, 128, 0))
    StopDrawing()
      
    CreateSprite(3, 40, 40) ;Destination
    StartDrawing(SpriteOutput(3))
      Box(0, 0, 40, 40, RGB(128, 128, 255))
    StopDrawing()
      
    CreateSprite(4, 40, 40) ;MouseXY
    StartDrawing(SpriteOutput(4))
      Box(18, 0, 4, 40, RGB(20, 20, 20))
      Box(0, 18, 40, 4, RGB(20, 20, 20))
    StopDrawing()
    
    PX = 160 ;Player X/Y
    PY = 160
    MX = -40 ;Mouse X/Y
    MY = -40
    
    PathCollisions() ;The collsion setup procedure.
    
    Repeat
      
      FlipBuffers()
      ClearScreen(RGB(255, 255, 255))
      ExamineKeyboard()
      ExamineMouse()
      
      If MouseButton(#PB_MouseButton_Left)
        MX = MouseX() + (#Path_Grid / 2) ;Lets get the crosshair center of mouse.
        MY = MouseY() + (#Path_Grid / 2)
        unit\xLoc = PX ;Get player position before finding the path.
        unit\yLoc = PY 
        If FindPath(unit, MX, MY) = 1 ;Only if we find a path.
          For a = 0 To unit\pathLength
            XX = ReadPathX(unit, a)
            YY = ReadPathY(unit, a)
            DisplaySprite(2, XX, YY) ;Path Point(s)
          Next a
        EndIf
      EndIf
      
      Restore Path_Collision_Data ;Lets draw our collisions for testing purposes.
      For Y = 0 To #Scr_Height - #Path_Grid Step #Path_Grid
        For X = 0 To #Scr_Width - #Path_Grid Step #Path_Grid
          Read.i Collision
          If Collision = 1
            DisplaySprite(1, X, Y) ;Collisions
          EndIf
        Next X
      Next Y
      
      StartDrawing(ScreenOutput())
        DrawText(10, 10, "Path Memory Size: " + Str(MemorySize(unit\pathBank)), RGB(0, 0, 0), RGB(255, 255, 255))
      StopDrawing()
      
      DisplaySprite(0, PX, PY) ;Player
      
      ;DisplaySprite(3, MX - (#Path_Grid / 2), MY - (#Path_Grid / 2)) ;Destination
      
      TransparentSpriteColor(4, RGB(0, 0, 0))
      DisplayTransparentSprite(4, MouseX(), MouseY()) ;Mouse X/Y
      
    Until KeyboardPushed(#PB_Key_Escape)

EndIf

DataSection
  Path_Collision_Data:
    Data.i 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1
    Data.i 1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1
    Data.i 1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1
    Data.i 1,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1
    Data.i 1,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1
    Data.i 1,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1
    Data.i 1,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,1,1,1,1,1,0,0,0,0,0,0,0,0,1
    Data.i 1,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,1,0,0,0,1,0,0,0,0,0,0,0,0,1
    Data.i 1,0,0,0,0,1,1,1,1,1,1,0,0,0,0,0,0,0,1,0,0,0,1,0,0,0,0,0,0,0,0,1
    Data.i 1,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,1,0,0,0,1,0,0,0,0,0,0,0,0,1
    Data.i 1,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,0,0,0,0,1
    Data.i 1,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1
    Data.i 1,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1
    Data.i 1,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1
    Data.i 1,0,0,0,0,0,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1
    Data.i 1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1
    Data.i 1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1
    Data.i 1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1
EndDataSection
Last edited by J. Baker on Mon Oct 18, 2021 10:00 pm, edited 1 time in total.
www.posemotion.com

PureBasic Tools for OS X: PureMonitor, plist Tool, Data Maker & App Chef

Mac: 10.13.6 / 1.4GHz Core 2 Duo / 2GB DDR3 / Nvidia 320M
PC: Win 7 / AMD 64 4000+ / 3GB DDR / Nvidia 720GT


Even the vine knows it surroundings but the man with eyes does not.
User avatar
Caronte3D
Addict
Addict
Posts: 1014
Joined: Fri Jan 22, 2016 5:33 pm
Location: Some Universe

Re: Path Finding Example

Post by Caronte3D »

Nice!
Thanks for sharing it :wink:
User avatar
J. Baker
Addict
Addict
Posts: 2178
Joined: Sun Apr 27, 2003 8:12 am
Location: USA
Contact:

Re: Path Finding Example

Post by J. Baker »

No problem. ;)

Now I think I will work on one from scratch. :D
www.posemotion.com

PureBasic Tools for OS X: PureMonitor, plist Tool, Data Maker & App Chef

Mac: 10.13.6 / 1.4GHz Core 2 Duo / 2GB DDR3 / Nvidia 320M
PC: Win 7 / AMD 64 4000+ / 3GB DDR / Nvidia 720GT


Even the vine knows it surroundings but the man with eyes does not.
User avatar
thyphoon
Enthusiast
Enthusiast
Posts: 327
Joined: Sat Dec 25, 2004 2:37 pm

Re: Path Finding Example

Post by thyphoon »

Thanks for sharing it !

I don't know if that might interest you. But on the French forum I posted another approach about Point and ckick path with vector
https://www.purebasic.fr/french/viewtop ... =2&t=17904
Directly the code ;)

Code: Select all

;
;AdventurePathfinding
;
;http://www.groebelsloot.com/2015/12/24/pathfinding-part-1/
;http://www.groebelsloot.com/2016/03/13/pathfinding-part-2/
;
;Haxe Source : https://github.com/MicUurloon/AdventurePathfinding
;
;LUA Source : https://github.com/To-mos/love2d-pathfinding-polygon
;https://www.maths-cours.fr/methode/algorithme-de-dijkstra-etape-par-etape/

;https://github.com/bladecoder/bladecoder-adventure-engine

;-Vector

DeclareModule vector
  Declare new(*v.point,x.l,y.l)
  Declare set(*from.point,*target.point)
  Declare.s toString(*v.point)
  Declare add(*vResult.point,*v1.point, *v2.point )
  Declare diff(*vResult.point,*v1.point, *v2.point )
  Declare.l dist(*v1.point, *v2.point )
  Declare.l length(*v.point)
  Declare normalized(*vResult.point,*v.point)
EndDeclareModule

Module vector
  EnableExplicit
  
  Procedure new(*v.point,x.l,y.l)
    *v\x=x
    *v\y=y
  EndProcedure
  
  Procedure set(*from.point,*target.point)
    *target\x=*from\x
    *target\y=*from\y
  EndProcedure
  
  
  Procedure.s toString(*v.point)
    ProcedureReturn "<"+Str(*v\x)+", "+Str(*v\y)+">"
  EndProcedure
  
  Procedure add(*vResult.point,*v1.point, *v2.point )
    *vResult\x=*v1\x+*v2\x
    *vResult\y=*v1\y+*v2\y
  EndProcedure
  
  Procedure diff(*vResult.point,*v1.point, *v2.point )
    *vResult\x=*v2\x-*v1\x
    *vResult\y=*v2\y+*v1\y
  EndProcedure
  
  Procedure.l dist(*v1.point, *v2.point )
    Protected vResult.point
    vResult\x=*v2\x-*v1\x
    vResult\y=*v2\y-*v1\y
    ProcedureReturn length(@vResult)
  EndProcedure
  
  Procedure.l length(*v.point)
    ProcedureReturn Sqr(*v\x * *v\x + *v\y * *v\y)
  EndProcedure
  
  Procedure normalized(*vResult.point,*v.point)
    Protected len.l = length(@*v)
    *vResult\x = *v\x / len
    *vResult\y = *v\y / len
  EndProcedure
  
  
EndModule

;-PathFinding

DeclareModule Pathfinding
  Declare dijkstra(Array graph(2),List walkPath.i(), srcIndex.i,destIndex.i)
EndDeclareModule

Module Pathfinding
  #M=999999 ; Infinity
  
  Procedure dijkstra(Array graph.l(2),List walkPath.l(), srcIndex.i,destIndex.i)
    Protected Nb.l=ArraySize(graph(),1)
    ; Démarrer et arrêter le nœud
    
    
    ; Distances par rapport au nœud de départ
    Dim Distance(Nb - 1)
    
    ; Prédécesseur du nœud
    Dim Predecessor(Nb - 1)
    
    ; Noeuds où vous connaissez déjà le chemin le plus court
    Dim Mark(Nb-1)
    For z = 0 To Nb - 1
      Mark(z) = #False
    Next
    
    ; Nœud de départ déjà visité!
    Mark(srcIndex) = #True
    ; Distance par rapport à vous-même = 0
    Distance(srcIndex) = 0
    ; Prédécesseurs d'eux-mêmes
    Predecessor(srcIndex) = srcIndex
    
    ; Définissez les distances et les prédécesseurs pour tous les autres nœuds.
    For z = 0 To Nb - 1
      ; ausser srcIndexnoten (s.o.)
      If z <> srcIndex
        Distance(z)    = Graph(srcIndex, z)
        Predecessor(z) = srcIndex
      EndIf
    Next
    
    
    ; Tant que le plus court Chemin vers le noeud cible est introuvable
    While Mark(destIndex) = #False
      ; Trouvez la distance la plus courte pour les nœuds qui ne sont pas marqués
      MinK = -1
      MinD = #M
      For z = 0 To Nb - 1
        ; Si non marqué
        If Mark(z) = #False
          ; sidistance plus courte
          If Distance(z) < MinD
            MinK = z
            MinD = Distance(z)
          EndIf
        EndIf
      Next
      
      
      ; Si aucun plus court n'a été trouvé (ie distance = infini) -> aucun chemin disponible
      If MinD = #M
        Debug "Il n'y a pas de connexion entre srcIndex et destIndex"
        Break
        
      ElseIf MinK = destIndex
        ; Dle plus court trouvé
        Debug "Walk Path Found"
        Break
        
      Else
        ; Marquez-le, donc un moyen le plus court a été trouvé
        Mark(MinK) = #True
      EndIf
      
      
      ; Pour tous les nœuds non marqués: vérifiez s'il existe un chemin plus court via MinK
      For z = 0 To Nb - 1
        If Mark(z) = #False
          ; Si le détour par MinK est plus court que l'itinéraire direct
          If Distance(MinK) + Graph(MinK, z) < Distance(z)
            ; Calculer la nouvelle longueur de chemin
            Distance(z)    = Distance(MinK) + Graph(MinK, z)
            ; Enregistrez le détour à 'z'
            Predecessor(z) = MinK
          EndIf
        EndIf
      Next
      
    Wend
    
    
    If MinK = destIndex
      ClearList(walkPath())
      ; Retracer le chemin depuis le destIndex
      s.s  = Str(destIndex)
      z    = MinK
      AddElement(walkPath())
      walkPath()=destIndex
      While Predecessor(z) <> srcIndex
        FirstElement(walkPath())
        InsertElement(walkPath())
        walkPath()=Predecessor(z)
        
        s = ">"+Str(Predecessor(z)) + ", " + s
        z = Predecessor(z)
      Wend
      FirstElement(walkPath())
      InsertElement(walkPath())
      walkPath()=Predecessor(z)
      s = Str(Predecessor(z)) + ", " + s
      Debug "Distance: " + Str(Distance(destIndex))
    EndIf
    ForEach walkPath()
      Debug ">"+walkPath()
    Next
  EndProcedure
  
EndModule

;-Polygon

DeclareModule polygon
  
  Structure Polygon
    List Polygon.point()
  EndStructure
  
  Declare addPoint(List polygon.point(), x.l, y.l )
  Declare.f distanceBetwenPoint(x1, y1, x2, y2)
  Declare pointInside( *p.point,List polygon.point())
  Declare.f distanceToSegment(Px.l, Py.l, x1.l, y1.l, x2.l, y2.l)
  Declare getClosestPointOnEdge(*resultV.point,*p3.point,List polygon.point() )

  Declare.b inLineOfSight(*p1.point, *p2.point,List Polygon.point(),obstacle.b=#False)
  Declare.b inLineOfSightGlobal(*p1.point, *p2.point,List walkArea.Polygon())
  Declare drawPoly(List polygon.point(),lineColor=#White,pointColor=#White)
  Declare walkInit(List walkArea.Polygon(),*Start.point,*Target.point,drawWalkPath.b=#True)
  
  
  
EndDeclareModule

Module polygon
  
  EnableExplicit
  
  ;UseModule vector
  
  Procedure addPoint(List polygon.point(), x.l, y.l )
    AddElement(polygon())
    polygon()\x=x*2
    polygon()\y=y*2-400
  EndProcedure
  
  Procedure.f distanceBetwenPoint(x1, y1, x2, y2)
    ProcedureReturn Sqr((x1-x2)*(x1-x2)+(y1-y2)*(y1-y2))
  EndProcedure
  
  Procedure pointInside( *p.point,List polygon.point())
    Protected cn.l = 0                              ;the crossing number counter
    Define pa.point, pb.point
    ; loop through all edges of the polygon
    Protected i.l
    For i=0 To ListSize(polygon())-1    ; edge from V[i] To V[i+1]
      SelectElement(polygon(),i)
      pa\x=polygon()\x
      pa\y=polygon()\y
      ;si on arrive a la fin de la liste on prend comme point i+1 le premier point
      If i=ListSize(polygon())-1
        SelectElement(polygon(),0)
      Else
        SelectElement(polygon(),i+1)
      EndIf
      pb\x=polygon()\x
      pb\y=polygon()\y
      If (((pa\y <= *P\y) And (pb\y > *P\y))  Or ((pa\y > *P\y) And (pb\y <= *P\y))) ; an upward crossing Or // a downward crossing
                                                                                     ; compute the actual edge-ray intersect x-coordinate
        Protected vt.f
        vt= (*P\y - pa\y) / (pb\y - pa\y);
        If (*P\x < pa\x + vt * (pb\x - pa\x)) ; *P\x < intersect
          cn+1                                ;   ; a valid crossing of y=*P\y right of *P\x
        EndIf
      EndIf
    Next
    ProcedureReturn (cn&1);    // 0 if even (out), and 1 if odd (in)
  EndProcedure
  
  Procedure.f distanceToSegment(Px.l, Py.l, x1.l, y1.l, x2.l, y2.l)
    Define.f Ratio, Dx, Dy, Result
    If x1 = x2 And y1 = y2
      Result = distanceBetwenPoint(Px, Py, x1, y1)
    Else
      Dx    = x2 - x1
      Dy    = y2 - y1
      Ratio = ((Px - x1) * Dx + (Py - y1) * Dy) / (Dx * Dx + Dy * Dy)
      If Ratio < 0
        Result = distanceBetwenPoint(Px, Py, x1, y1)
      ElseIf Ratio > 1
        Result = distanceBetwenPoint(Px, Py, x2, y2)
      Else
        Result = distanceBetwenPoint(Px, Py, (1 - Ratio) * x1 + Ratio * x2, (1 - Ratio) * y1 + Ratio * y2)
      EndIf
    EndIf
    ProcedureReturn Result
    ;   If Result > -#Epsilon And Result < #Epsilon
    ;     ProcedureReturn 1
    ;   Else
    ;     ProcedureReturn 0
    ;   EndIf   
  EndProcedure
  
  Procedure getClosestPointOnEdge(*resultV.point,*p3.point,List polygon.point() )
    Protected tx.f = *p3\x
    Protected ty.f = *p3\y
    Protected vi1 = -1
    Protected vi2 = -1
    Protected mindist.f = 100000
    
    Protected.l ia,ib
    For ia=0 To ListSize(polygon())-1
      Protected.point va,vb
      SelectElement(polygon(),ia)
      va\x=polygon()\x
      va\y=polygon()\y
      If ia=ListSize(polygon())-1
        ib=0
      Else
        ib=ia+1
      EndIf
      SelectElement(polygon(),ib)
      vb\x=polygon()\x
      vb\y=polygon()\y
      Protected dist.l = distanceToSegment(tx, ty, va\x, va\y, vb\x, vb\y)
      If dist< mindist
        mindist = dist
        vi1 =ia
        vi2 =ib
      EndIf
    Next
    
    Protected.f x1,x2,x3,y1,y2,y3
    
    SelectElement(polygon(),vi1)
    x1 = polygon()\x
    y1 = polygon()\y
    SelectElement(polygon(),vi2)
    x2 = polygon()\x
    y2 = polygon()\y
    x3 = *p3\x
    y3 = *p3\y
    Protected u.f = (((x3 - x1) * (x2 - x1)) + ((y3 - y1) * (y2 - y1))) / (((x2 - x1) * (x2 - x1)) + ((y2 - y1) * (y2 - y1)))
    
    Protected xu.f = x1 + u * (x2 - x1)
    Protected yu.f = y1 + u * (y2 - y1)
    
    If u < 0
      vector::new(*resultV,x1, y1)
    ElseIf u > 1
      vector::new(*resultV,x2, y2)
    Else
      vector::new(*resultV,xu, yu)
    EndIf
  EndProcedure
  
  Procedure isVertexConcave(List Polygon.point(), Index.i)
    
    Protected.i nextIndex,prevIndex
    Protected.f currentX,currentY
    
    SelectElement(Polygon(),Index)
    currentX = Polygon()\x
    currentY = Polygon()\y
    
    Protected.f nextX,nextY
    nextIndex=Index+1
    If nextIndex>ListSize(Polygon())-1:nextIndex=0:EndIf
    SelectElement(Polygon(),nextIndex)
    nextX =  Polygon()\x
    nextY = Polygon()\y
    
    Protected.f previousX,previousY
    prevIndex=Index-1
    If prevIndex<0:prevIndex=ListSize(Polygon())-1:EndIf
    SelectElement(Polygon(),prevIndex)
    previousX = Polygon()\x
    previousY = Polygon()\y
    
    ;Debug Str(prevIndex)+" "+Str(Index)+" "+Str(nextIndex)
    
    Protected.f leftX,leftY,rightX,rightY,cross
    leftX = currentX - previousX;
    leftY = currentY - previousY;
    rightX = nextX - currentX   ;
    rightY = nextY - currentY   ;
    
    cross = (leftX * rightY) - (leftY * rightX)
    ;Debug "cross :"+StrF(cross)+" ="+Str(Bool(cross < 0))
    ProcedureReturn Bool(cross < 0)
  EndProcedure
  
  Procedure LineSegmentsCross(*A.point,*B.point, *C.point, *D.point)
    Protected.f denominator,numerator1,numerator2,r,s
    
    denominator = ((*B\x - *A\x) * (*D\y - *C\y)) - ((*B\y - *A\y) * (*D\x - *C\x));
    
    If denominator = 0
      ProcedureReturn #False
    EndIf
    
    numerator1 = ((*A\y - *C\y) * (*D\x - *C\x)) - ((*A\x - *C\x) * (*D\y - *C\y));
    
    numerator2 = ((*A\y - *C\y) * (*B\x- *A\x)) - ((*A\x - *C\x) * (*B\y - *A\y));
    
    If numerator1 = 0 Or numerator2 = 0
      ProcedureReturn #False
    EndIf
    
    r = numerator1 / denominator
    s = numerator2 / denominator
    
    ProcedureReturn Bool((Bool(r > 0) And Bool(r < 1)) And (Bool(s > 0) And Bool(s < 1)))
  EndProcedure
  
  Procedure Max(A, B)
    If A > B
      ProcedureReturn A
    EndIf
    ProcedureReturn B
  EndProcedure
  
  Procedure Min(A, B)
    If A < B
      ProcedureReturn A
    EndIf
    ProcedureReturn B
  EndProcedure
  
  Procedure nearestSegment(*P.point,List polygon.point(),distMax=5)
    Protected pa.point
    Protected pb.point
    Protected segment.l=-1
    Protected minDist.l=4096
    Protected i.l
    For i=0 To ListSize(polygon())-1
      SelectElement(polygon(),i)
      pa\x=polygon()\x
      pa\y=polygon()\y
      ;si on arrive a la fin de la liste on prend comme point i+1 le premier point
      If i=ListSize(polygon())-1
        SelectElement(polygon(),0)
      Else  
        SelectElement(polygon(),i+1)
      EndIf  
      pb\x=polygon()\x
      pb\y=polygon()\y
      Protected dist.l
      dist=distanceToSegment(*P\x,*P\y,pa\x, pa\y, pb\x, pb\y)
      If dist<=distMax And dist<minDist
        segment=ListIndex(polygon())
        Debug "Trouve Segment:"+Str(segment)
        minDist=dist
      EndIf
    Next
    ProcedureReturn segment
  EndProcedure
  
  Procedure LineIntersection(*L1PA.point,*L1PB.point,*L2PA.point,*L2PB.point, *Cross.point)
    Protected.l A1,B1,C1,A2,B2,C2
    A1 = *L1PB\y - *L1PA\y
    B1 = *L1PA\x - *L1PB\x
    C1 = A1 * *L1PA\x + B1 * *L1PA\y
    
    A2 = *L2PB\y - *L2PA\y
    B2 = *L2PA\x - *L2PB\x
    C2 = A2 * *L2PA\x + B2 * *L2PA\y
    
    Protected det.d = A1*B2 - A2*B1
    If det = 0
      ProcedureReturn 0 ; No intersection
    Else
      *cross\x = (B2*C1 - B1*C2)/det
      *Cross\y = (A1*C2 - A2*C1)/det
      
      ; On *L1 line segment?
      If Min(*L1PA\x, *L1PB\x) <= *cross\x And Max(*L1PA\x, *L1PB\x) >= *cross\x
        If Min(*L1PA\y, *L1PB\y) <= *cross\y And Max(*L1PA\y, *L1PB\y) >= *cross\y
          
          ; On *L2 line segment?
          If Min(*L2PA\x, *L2PB\x) <= *cross\x And Max(*L2PA\x, *L2PB\x) >= *cross\x
            If Min(*L2PA\y, *L2PB\y) <= *cross\y And Max(*L2PA\y, *L2PB\y) >= *cross\y
              ProcedureReturn 1
            EndIf
          EndIf
        EndIf
      EndIf
      
      ProcedureReturn 2 ; Lines intersect, but line segments do not
    EndIf
  EndProcedure
  
  
  Procedure.b inLineOfSightA(*p1.point, *p2.point,List Polygon.point())
    Protected   epsilon.f   =   0.5 ;
    
    ;Not in LOS If any of the ends is outside the polygon
    If Not pointInside(*p1,Polygon()) Or Not pointInside(*p2,Polygon())
      ProcedureReturn #False
    EndIf
    
    
    ;In LOS If it's the same start and end location
    ; If   ( Vector . Subtract ( start ,   End ) . length   & lt ;   epsilon )   { 
    ;    Return   false ; 
    ; }
    
    ;Not in LOS If any edge is intersected by the start-End line segment 
    Protected inSight.b=#True ; 
                              ;For   ( polygon  in   polygons )   { 
    Protected.l i,ni      
    For i=0 To ListSize(Polygon())-1
      
      SelectElement(Polygon(),i)
      Protected.point v1,v2
      vector::set(@Polygon(),@v1)
      ni=i+1:If ni>ListSize(Polygon())-1:ni=0:EndIf
      SelectElement(Polygon(),ni)
      vector::set(@Polygon(),@v2)
      
      If LineSegmentsCross ( *p1 ,   *p2 ,   @v1 ,   @v2 )
        ;In some cases a 'snapped' endpoint is just a little over the line due To rounding errors. So a 0.5 margin is used To tackle those cases. 
        If    polygon::distanceToSegment( *p1\x ,   *p1\y ,   v1\x ,   v1\y ,   v2\x ,   v2\y   )  >   0.5   And   polygon::distanceToSegment ( *p2\x ,   *p2\y ,   v1\x ,   v1\y ,   v2\x ,   v2\y   )  >   0.5
          ProcedureReturn   #False ;
        EndIf
      EndIf
    Next
    ;}
    
    ;Finally the middle point in the segment determines If in LOS Or Not
    vector::add(@v1,*p1,*p2)
    vector::new(@v2,v1\x/2,v1\y/2)
    Protected inside.b = polygon::pointInside(@v2,Polygon())
    
    ;For   ( i   in   1...polygons.length )   {
    ;If   ( polygons [ i ] . pointInside ( v2 ,   false ) )   {
    ; inside   =   #False ;
    ;EndIf 
    ;Next
    ProcedureReturn   inside ;
  EndProcedure
  
  
  Procedure.b inLineOfSight(*p1.point, *p2.point,List Polygon.point(),obstacle.b=#False)
    Protected.point tmp,tmp2,c,d,tmp3
    vector::set(*p1,@tmp)
    vector::set(*p2,@tmp2)
    
    Protected.l i,ni      
    For i=0 To ListSize(Polygon())-1
      SelectElement(Polygon(),i)
      vector::set(@Polygon(),@c)
      ni=i+1:If ni>ListSize(Polygon())-1:ni=0:EndIf
      SelectElement(Polygon(),ni)
      vector::set(@Polygon(),@d)
      
      ;if points are a polygon segment
      If (*p1\x=c\x And *p1\y=c\y And *p2\x=d\x And *p2\y=d\y) Or (*p1\x=d\x And *p1\y=d\y And *p2\x=c\x And *p2\y=c\y)
        ProcedureReturn #True
      EndIf 
      
      ;if first point is start of polygon segment and second point is on the segment
      If (*p1\x=c\x And *p1\y=c\y And polygon::distanceToSegment ( *p2\x ,   *p2\y ,   c\x ,   c\y ,   d\x ,   d\y   )  < 3) Or (*p1\x=d\x And *p1\y=d\y And polygon::distanceToSegment ( *p2\x ,   *p2\y ,   c\x ,   c\y ,   d\x ,   d\y   )  < 3)
        ProcedureReturn #True
      EndIf 
      
      If LineSegmentsCross(@tmp,@tmp2, @c, @d)
        
        If    polygon::distanceToSegment( tmp\x ,   tmp\y ,   c\x ,   c\y ,   d\x ,   d\y   )  >   0   And   polygon::distanceToSegment ( tmp2\x ,   tmp2\y ,   c\x ,   c\y ,   d\x ,   d\y   )  >   0
          ProcedureReturn   #False ;
        EndIf
      EndIf
    Next   
    
    vector::add(@tmp3,@tmp,@tmp2)
    tmp3\x=tmp3\x/2
    tmp3\y=tmp3\y/2
    
    Protected result.b = PointInside(@tmp3,polygon());
    
    
    If obstacle=#True
      result=1-result 
    EndIf
    
    
    ProcedureReturn result
    
  EndProcedure
  
  Procedure.b inLineOfSightGlobal(*p1.point, *p2.point,List walkArea.Polygon())
    Protected.b result,obstacle=#False
    ForEach walkArea()
      If ListIndex(walkArea())>0
        obstacle=#True
      EndIf  
      
   
      
      result.b=inLineOfSight(*p1, *p2,walkArea()\Polygon(),obstacle)
      
      If result=#False:Break:EndIf 
    Next
    ProcedureReturn result
  EndProcedure
  
  
  Procedure drawPoly(List polygon.point(),lineColor=#White,pointColor=#White)
    Protected pa.point
    Protected pb.point
    Protected dist.l
    Protected i.l
    For i=0 To ListSize(polygon())-1
      SelectElement(polygon(),i)
      pa\x=polygon()\x
      pa\y=polygon()\y
      ;si on arrive a la fin de la liste on prend comme point i+1 le premier point
      If i=ListSize(polygon())-1
        SelectElement(polygon(),0)
      Else 
        SelectElement(polygon(),i+1)
      EndIf 
      pb\x=polygon()\x
      pb\y=polygon()\y
      LineXY(pa\x,pa\y,pb\x,pb\y,lineColor)
      Circle(pa\x,pa\y,5,pointColor)
      ;DrawText(pa\x+10,pa\y+10,Str(i),pointColor)
      
    Next
    
  EndProcedure
  
  Procedure drawWalkPath(Array graph.l(2),List walkPointCoord.point())
    Protected pa.point
    Protected pb.point
    
    Protected.l i,j,Nb=ArraySize(graph(),1)
    For i=0 To Nb-1
      For j=0 To Nb-1
        If graph(i,j)<>0 And graph(i,j)<>999999
          SelectElement(walkPointCoord(),i)
          pa\x=walkPointCoord()\x
          pa\y=walkPointCoord()\y
          SelectElement(walkPointCoord(),j)
          pb\x=walkPointCoord()\x
          pb\y=walkPointCoord()\y
          LineXY(pa\x,pa\y,pb\x,pb\y,#Green)
          Circle(pa\x,pa\y,5,#Yellow)
          Circle(pb\x,pb\y,5,#Yellow)
        EndIf 
      Next
    Next
    
  EndProcedure
  
  Procedure drawShortestPath(List walkPointCoord.point(),List walkPointIndex.l())
    ;Draw Walk
    Protected.l Ax,Ay,Bx,By,n
    For n=0 To ListSize(walkPointIndex())-1
      SelectElement(walkPointIndex(),n)
      SelectElement(walkPointCoord(),walkPointIndex())
      Ax.l=walkPointCoord()\x
      Ay.l=walkPointCoord()\y
      If SelectElement(walkPointIndex(),n+1)
        SelectElement(walkPointCoord(),walkPointIndex())
        Bx.l=walkPointCoord()\x
        By.l=walkPointCoord()\y
        Circle(Ax,Ay,5,#Yellow)
        LineXY(Ax,Ay,Bx,By,RGB(255,255,255))
        Circle(Bx,By,5,#Yellow)
      EndIf 
    Next
  EndProcedure
  
  
  Procedure walkInit(List walkArea.Polygon(),*Start.point,*Target.point,drawWalkPath.b=#True)
    Protected pa.point
    Protected pb.point
    Protected.l dist,i
    Protected p.point
    NewList walkPointCoord.Point() ; List with only walkable point
    NewList walkPointIndex.l()     ;Sort wakable point with shortest path
    
    
    ;-Id Target out walk area
    Protected.b result
    ForEach walkArea()
      ;SelectElement(walkArea(),0)
      result=pointInside(*Target,walkArea()\Polygon())
      If ListIndex(walkArea())=0
        result=1-result 
      EndIf 
    If result=#True
      If *Target\x>0 And *Target\y>0
        getClosestPointOnEdge(@p,*Target,walkArea()\Polygon()) ;get Closest Point on Edge
        *Target\x=p\x
        *Target\y=p\y
      EndIf 
    EndIf 
    Next 
    ;Add point to list with only walkable point
    ;Add Start Point
    AddElement(walkPointCoord())
    walkPointCoord()\x=*Start\x
    walkPointCoord()\y=*Start\y
    ;Add Target Point
    AddElement(walkPointCoord())
    walkPointCoord()\x=*Target\x
    walkPointCoord()\y=*Target\y   
    
    
    ;If not in line of Sight we must to found the path
    If Not inLineOfSightGlobal(*Start, *Target,walkArea());inLineOfSight(*Start, *Target,walkArea()\Polygon())
      ;Select all point to found the way
      ;SelectElement(walkArea(),0)
      ForEach(walkArea())
        For i=0 To ListSize(walkArea()\Polygon())-1
          SelectElement(walkArea()\Polygon(),i)
          pa\x=walkArea()\Polygon()\x
          pa\y=walkArea()\Polygon()\y
          If ListIndex(walkArea())=0
            ; Add only concave point
            If IsVertexConcave(walkArea()\Polygon(),i)=#True
              AddElement(walkPointCoord())
              walkPointCoord()\x=pa\x
              walkPointCoord()\y=pa\y
            EndIf
          Else
            ; Add only convex point
            If IsVertexConcave(walkArea()\Polygon(),i)=#False 
              AddElement(walkPointCoord())
              walkPointCoord()\x=pa\x
              walkPointCoord()\y=pa\y
            EndIf
          EndIf 
        Next
      Next 
      ;generate Graph
      
      
      Dim Graph.l(ListSize(walkPointCoord()),ListSize(walkPointCoord()))
      
      
      For i=0 To ListSize(walkPointCoord())-1
        SelectElement(walkPointCoord(),i)
        pa\x=walkPointCoord()\x
        pa\y=walkPointCoord()\y
        Protected j.l
        For j=0 To ListSize(walkPointCoord())-1
          If i<>j
            SelectElement(walkPointCoord(),j)
            pb\x=walkPointCoord()\x
            pb\y=walkPointCoord()\y
            ; (i>2 And j>2  : because Point 0 is Start and 1 target
            ; And (j=i+1 Or i=j+1)) : because in polygon previous and next point are always connected
            ; Or inLineOfSightB(@pa, @pb,polygon()) : if Points are in line of sight

            If inLineOfSightGlobal(@pa, @pb,walkArea())
              Graph(i,j)=distanceBetwenPoint(pa\x,pa\y,pb\x,pb\y)
              
            Else ; Points are not connected to do walkpath
              Graph(i,j)=999999
            EndIf
            
          Else ; it's the same point 
            Graph(i,j)=0
          EndIf
        Next
      Next
      
      Pathfinding::dijkstra(Graph(),walkPointIndex(),0,1)
      ;In line of Sight it's easy, start to target directly  
    Else
      AddElement(walkPointIndex())
      walkPointIndex()=0
      AddElement(walkPointIndex())
      walkPointIndex()=1
    EndIf
    
    If drawWalkPath=#True
      ;drawWalkPath(Graph(),walkPointCoord()) 
      drawShortestPath(walkPointCoord(),walkPointIndex())
    EndIf
    
  EndProcedure
  
EndModule



;-Test

Structure room
  List walkArea.Polygon::Polygon()
EndStructure

Structure chara
  coord.point
  target.point
  List walkPointCoord.Point() ; List with only walkable point
  List walkPointIndex.l()     ; Sort wakable point with shortest path
EndStructure

Define mainChara.chara
mainChara\coord\x=63
mainChara\coord\y=290

Define room.room


AddElement (room\walkArea())
polygon::addpoint(room\walkArea()\Polygon(),5,248)
polygon::addpoint(room\walkArea()\Polygon(),5,248)
polygon::addpoint(room\walkArea()\Polygon(),235,248)
polygon::addpoint(room\walkArea()\Polygon(),252,277)
polygon::addpoint(room\walkArea()\Polygon(),214,283)
polygon::addpoint(room\walkArea()\Polygon(),217,300)
polygon::addpoint(room\walkArea()\Polygon(),235,319)
polygon::addpoint(room\walkArea()\Polygon(),265,339)
polygon::addpoint(room\walkArea()\Polygon(),275,352)
polygon::addpoint(room\walkArea()\Polygon(),310,350)
polygon::addpoint(room\walkArea()\Polygon(),309,312)
polygon::addpoint(room\walkArea()\Polygon(),322,308)
polygon::addpoint(room\walkArea()\Polygon(),304,279)
polygon::addpoint(room\walkArea()\Polygon(),307,249)
polygon::addpoint(room\walkArea()\Polygon(),419,248)
polygon::addpoint(room\walkArea()\Polygon(),431,262)
polygon::addpoint(room\walkArea()\Polygon(),389,274)
polygon::addpoint(room\walkArea()\Polygon(),378,295)
polygon::addpoint(room\walkArea()\Polygon(),408,311)
polygon::addpoint(room\walkArea()\Polygon(),397,316)
polygon::addpoint(room\walkArea()\Polygon(),378,309)
polygon::addpoint(room\walkArea()\Polygon(),365,323)
polygon::addpoint(room\walkArea()\Polygon(),342,360)
polygon::addpoint(room\walkArea()\Polygon(),358,379)
polygon::addpoint(room\walkArea()\Polygon(),205,379)
polygon::addpoint(room\walkArea()\Polygon(),206,338)
polygon::addpoint(room\walkArea()\Polygon(),212,320)
polygon::addpoint(room\walkArea()\Polygon(),198,316)
polygon::addpoint(room\walkArea()\Polygon(),162,298)
polygon::addpoint(room\walkArea()\Polygon(),119,305)
polygon::addpoint(room\walkArea()\Polygon(),99,338)
polygon::addpoint(room\walkArea()\Polygon(),91,362)
polygon::addpoint(room\walkArea()\Polygon(),79,372)
polygon::addpoint(room\walkArea()\Polygon(),90,380)
polygon::addpoint(room\walkArea()\Polygon(),4, 379)
AddElement (room\walkArea())
polygon::addpoint(room\walkArea()\Polygon(),120,280)
polygon::addpoint(room\walkArea()\Polygon(),120,260)
polygon::addpoint(room\walkArea()\Polygon(),140,260)
polygon::addpoint(room\walkArea()\Polygon(),140,280)
AddElement (room\walkArea())
For z=0 To 360 Step 36
  Debug Str(120+10*Cos(Radian(z)))+","+Str(200+10*Sin(Radian(z)))
  polygon::addpoint(room\walkArea()\Polygon(),60+20*Cos(Radian(z)),300+20*Sin(Radian(z)))
Next   

;AddElement (room\walkArea())
;polygon::addpoint(room\walkArea()\Polygon(),250,280)
;polygon::addpoint(room\walkArea()\Polygon(),250,460)
;polygon::addpoint(room\walkArea()\Polygon(),260,460)
;polygon::addpoint(room\walkArea()\Polygon(),260,280)


If InitSprite()
  If InitKeyboard() And InitMouse()
    Define winMain.i = OpenWindow(#PB_Any,0,0,1024,800,"Press [Esc] to close",#PB_Window_ScreenCentered | #PB_Window_SystemMenu)
    OpenWindowedScreen(WindowID(winMain), 0, 0,1024,800, 1, 0, 0)
    
  EndIf
Else
  MessageRequester("","Unable to initsprite") :
EndIf

Define EventID.l,mode.b,result.l
Repeat
  Delay(1)
  EventID = WindowEvent()
  ExamineKeyboard()
  ;ExamineMouse()
  ClearScreen(0)
  
  StartDrawing(ScreenOutput())
  mainChara\target\x=WindowMouseX(winMain)
  mainChara\target\y=WindowMouseY(winMain)
  ForEach room\walkArea()
    Define.point target
    
    If ListIndex(room\walkArea())=0
      color=RGB(0,0,255)
    Else
      color=RGB(100,100,255)
    EndIf 
    polygon::drawPoly(room\walkArea()\Polygon(),color)
    
    ;ForEach polyMap()\Polygon()
    
    
    ;    Debug "no"
    ;  EndIf
    ;Next
  Next
  polygon::walkInit(room\walkArea(),@mainChara\coord,@mainChara\target,#True)
  
  Circle(mainChara\target\x,mainChara\target\y,5,#Blue)
  
  Circle(mainChara\Coord\x,mainChara\Coord\y,5,#Red)
  StopDrawing()
  FlipBuffers()
Until KeyboardReleased(#PB_Key_Escape) Or EventID = #PB_Event_CloseWindow

User avatar
J. Baker
Addict
Addict
Posts: 2178
Joined: Sun Apr 27, 2003 8:12 am
Location: USA
Contact:

Re: Path Finding Example

Post by J. Baker »

Cool! Will check it out. ;)
www.posemotion.com

PureBasic Tools for OS X: PureMonitor, plist Tool, Data Maker & App Chef

Mac: 10.13.6 / 1.4GHz Core 2 Duo / 2GB DDR3 / Nvidia 320M
PC: Win 7 / AMD 64 4000+ / 3GB DDR / Nvidia 720GT


Even the vine knows it surroundings but the man with eyes does not.
User avatar
Caronte3D
Addict
Addict
Posts: 1014
Joined: Fri Jan 22, 2016 5:33 pm
Location: Some Universe

Re: Path Finding Example

Post by Caronte3D »

thyphoon wrote: Mon Oct 18, 2021 4:13 pm ...another approach about Point and ckick path with vector...
WoW! Nice!
Thanks! :wink:
CharlesT
User
User
Posts: 19
Joined: Sun Jan 07, 2018 10:09 pm

Re: Path Finding Example

Post by CharlesT »

Thanks for this, and to JCV for porting Patrick Lester's original Blitz3D code over to PureBasic. I noticed one possible error here in JCV's code:

;3. Reset some variables that need to be cleared
If onClosedList > 1000000 ;occasionally redim whichList
ReDim whichList(mapWidth,mapHeight)
onClosedList = 10
EndIf

The ReDim statement (it's on line 127) should probably be a Dim, as it is in the original Blitz3D code, because it is meant to reset the contents of the array to 0 at the same time when the onClosedList variable is reset to 10 (this variable is increased by 2 every pathfinding call to avoid having to reset the array every time).

I've been studying this code recently, and I'm not sure why the arrays are all initialized to the map dimensions +2. Anyone know why? And I'm also puzzled why the whichList array is not given the +2 slots when it is reset as shown above.
Post Reply