PureBasic

Code : Tout sélectionner

; ******************************************************************** 
; Program:           Point'n Click 2D pathFinding
; Description:       Permits the use of tiled maps like 
;                    OpenStreetMap in a handy PureBASIC module
; Author:            Thyphoon by following the articles of www.groebelsloot.com
; Date:              October, 2021
; License:           Free, unrestricted, credit 
;                    appreciated but not required.
;
; Note:              Please share improvement !
; ******************************************************************** 
; Source who help me :
; 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
  Structure pointf
    x.l
    y.l
  EndStructure
  
  Declare new(*v.pointf,x.l,y.l)
  Declare set(*from.pointf,*target.pointf)
  Declare.s toString(*v.pointf)
  Declare add(*vResult.pointf,*v1.pointf, *v2.pointf )
  Declare diff(*vResult.pointf,*v1.pointf, *v2.pointf )
  Declare.l dist(*v1.pointf, *v2.pointf )
  Declare.l length(*v.pointf)
  Declare normalized(*vResult.pointf,*v.pointf)
EndDeclareModule

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



;-Polygon

DeclareModule polygon
  
  UseModule vector
  
  Structure Polygon
    List Polygon.pointf()
  EndStructure
  
  Declare addPoint(List polygon.pointf(), x.l, y.l )
  Declare.f distanceBetwenPoint(x1, y1, x2, y2)
  Declare pointInside( *p.pointf,List polygon.pointf())
  Declare.f distanceToSegment(Px.l, Py.l, x1.l, y1.l, x2.l, y2.l)
  Declare getClosestPointOnEdge(*resultV.pointf,*p3.pointf,List polygon.pointf() )
  Declare isVertexConcave(List Polygon.pointf(), Index.i)
  Declare.b inLineOfSight(*p1.pointf, *p2.pointf,List Polygon.pointf(),obstacle.b=#False)
  Declare.b inLineOfSightGlobal(*p1.pointf, *p2.pointf,List walkArea.Polygon())
  Declare drawPoly(List polygon.pointf(),lineColor=#White,pointColor=#White)
  
EndDeclareModule

Module polygon
  
  EnableExplicit
  
  ;UseModule vector
  
  Procedure addPoint(List polygon.pointf(), 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.pointf,List polygon.pointf())
    Protected cn.l = 0                              ;the crossing number counter
    Define pa.pointf, pb.pointf
    ; 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.pointf,*p3.pointf,List polygon.pointf() )
    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.pointf 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.pointf(), 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 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.pointf,List polygon.pointf(),distMax=5)
    Protected pa.pointf
    Protected pb.pointf
    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.pointf,*L1PB.pointf,*L2PA.pointf,*L2PB.pointf, *Cross.pointf)
    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 inLineOfSight(*p1.pointf, *p2.pointf,List Polygon.pointf(),obstacle.b=#False)
    Protected.pointf 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 
      
      Protected e.pointf
      If LineIntersection(@tmp,@tmp2, @c, @d,@e)=1;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.pointf, *p2.pointf,List walkArea.Polygon())
    Protected.b result=#False,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.pointf(),lineColor=#White,pointColor=#White)
    Protected pa.pointf
    Protected pb.pointf
    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
  
EndModule

;-PathFinding

DeclareModule Pathfinding
   UseModule vector
  
  Declare dijkstra(Array graph(2),List walkPath.i(), srcIndex.i,destIndex.i)
  Declare getWalkPath(*Start.pointf,*Target.pointf,List walkArea.polygon::Polygon(),List walkPath.pointf())
  Declare getPositionOnPath(List walkPath.pointf(),Distance.l,*Position.pointf)
  Declare drawPath(List walkPath.pointf())
  Declare.l getPathLength(List walkPath.pointf())
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
  
     Procedure drawWalkPath(Array graph.l(2),List walkPointCoord.pointf())
    Protected pa.pointf
    Protected pb.pointf
    
    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
          If polygon::distanceToSegment(MouseX(),MouseY(),pa\x,pa\y,pb\x,pb\y)<1
            LineXY(pa\x,pa\y,pb\x,pb\y,#Green)
          Else  
            LineXY(pa\x,pa\y,pb\x,pb\y,#Gray)
          EndIf 
          Circle(pa\x,pa\y,5,#Yellow)
          Circle(pb\x,pb\y,5,#Yellow)
        EndIf 
      Next
    Next
    
  EndProcedure
  
  
  Procedure getWalkPath(*Start.pointf,*Target.pointf,List walkArea.polygon::Polygon(),List walkPath.pointf())
    Protected pa.pointf
    Protected pb.pointf
    Protected.l dist,i
    Protected p.pointf
    NewList walkPointCoord.pointf() ; 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=polygon::pointInside(*Target,walkArea()\Polygon())
      If ListIndex(walkArea())=0
        result=1-result 
      EndIf 
      If result=#True
        If *Target\x>0 And *Target\y>0
          polygon::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 Polygon::inLineOfSight(*Start, *Target,walkArea()\Polygon()) ;Polygon::inLineOfSight(*Start, *Target,walkArea());
                                                                   ;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 polygon::IsVertexConcave(walkArea()\Polygon(),i)=#True
              AddElement(walkPointCoord())
              walkPointCoord()\x=pa\x
              walkPointCoord()\y=pa\y
            EndIf
          Else
            ; Add only convex point
            If polygon::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 polygon::inLineOfSightGlobal(@pa, @pb,walkArea())=#True
              Graph(i,j)=polygon::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
    
    drawWalkPath(graph(),walkPointCoord())
    
    ;Put path to WalkPath
    ClearList(WalkPath())
    Protected.l n
    For n=0 To ListSize(walkPointIndex())-1
      SelectElement(walkPointIndex(),n)
      SelectElement(walkPointCoord(),walkPointIndex())
      AddElement(WalkPath())
      WalkPath()\x=walkPointCoord()\x
      WalkPath()\y=walkPointCoord()\y
    Next
  EndProcedure
  
  Procedure getPositionOnPath(List walkPath.pointf(),Distance.l,*Position.pointf)
    Protected.l PathDistance.l=0,OldPathDistance,SegmentDistance
    For n=0 To ListSize(walkPath())-1
      SelectElement(walkPath(),n)
      x1=walkPath()\x
      y1=walkPath()\y
      If SelectElement(walkPath(),n+1)
        x2=walkPath()\x
        y2=walkPath()\y
        ;Calcul Path Distance
        OldPathDistance=PathDistance
        hypothenus=polygon::distanceBetwenPoint(x1, y1, x2, y2)
        PathDistance=PathDistance+hypothenus
        If Distance>=OldPathDistance And Distance<=PathDistance ; If you are one this segment
          Protected.f x,y
          SegmentDistance=Distance-OldPathDistance
          x=x1+(SegmentDistance/hypothenus*(x2-x1))
          y=y1+(SegmentDistance/hypothenus*(y2-y1))
          *Position\x=x
          *Position\y=y
          Break;
        EndIf 
      EndIf 
    Next 
  EndProcedure 
  

  
  Procedure drawPath(List walkPath.pointf())
    Protected.l PathDistance.l=0,OldPathDistance,SegmentDistance
    For n=0 To ListSize(walkPath())-1
      SelectElement(walkPath(),n)
      x1=walkPath()\x
      y1=walkPath()\y
      If SelectElement(walkPath(),n+1)
        x2=walkPath()\x
        y2=walkPath()\y
        ;Calcul Path Distance
        OldPathDistance=PathDistance
        hypothenus=polygon::distanceBetwenPoint(x1, y1, x2, y2)
        PathDistance=PathDistance+hypothenus
        ;Draw
        LineXY(x1,y1,x2,Y2,#White)
        Circle(x1,y1,5,#Red)
        Circle(x2,y2,5,#Red)
      EndIf 
    Next 
  EndProcedure 
  
  Procedure.l getPathLength(List walkPath.pointf())
    Protected.l PathDistance.l=0,OldPathDistance,SegmentDistance
    For n=0 To ListSize(walkPath())-1
      SelectElement(walkPath(),n)
      x1=walkPath()\x
      y1=walkPath()\y
      If SelectElement(walkPath(),n+1)
        x2=walkPath()\x
        y2=walkPath()\y
        ;Calcul Path Distance
        hypothenus=polygon::distanceBetwenPoint(x1, y1, x2, y2)
        PathDistance=PathDistance+hypothenus
      EndIf 
    Next 
    ProcedureReturn PathDistance
  EndProcedure  
EndModule

;-Test

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

Structure chara
  coord.vector::pointf
  target.vector::pointf
  List Path.vector::pointf()
  pathLength.l
  move.b              ;#True / #False : Move to the target 
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
  polygon::addpoint(room\walkArea()\Polygon(),60+20*Cos(Radian(z)),300+20*Sin(Radian(z)))
Next   

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

  Define Distance.l
  Repeat
    Delay(1)
      Repeat : Until WindowEvent() = 0  
    ExamineKeyboard()
    ExamineMouse()
    ClearScreen(0)
    
    StartDrawing(ScreenOutput())
    mainChara\target\x=MouseX();WindowMouseX(winMain)
    mainChara\target\y=MouseY();WindowMouseY(winMain)
    ForEach room\walkArea()
      Define.vector::pointf target
      
      If ListIndex(room\walkArea())=0
        color=RGB(0,0,255)
      Else
        color=RGB(100,100,255)
      EndIf 
      polygon::drawPoly(room\walkArea()\Polygon(),color)
      
    Next

    
    If MouseButton(#PB_MouseButton_Left)
      Pathfinding::getWalkPath(@mainChara\coord,@mainChara\target,room\walkArea(),mainChara\Path())
      mainChara\move=#True
      mainChara\pathLength=Pathfinding::getPathLength(mainChara\Path())
    EndIf 
    
    If mainChara\move=#True

      Pathfinding::getPositionOnPath(mainChara\Path(),Distance,@mainChara\coord)
      Distance=Distance+5
   
 
      If Distance>=mainChara\pathLength
        mainChara\move=#False
        Distance=0
      EndIf 
    EndIf 
    
    Pathfinding::drawPath(mainChara\Path())
    
    Circle(mainChara\coord\x,mainChara\coord\y,5,#Green)
    DrawText(20,20,Str(Distance))
    DrawingMode(#PB_2DDrawing_XOr)
    Line(MouseX()-1,MouseY(),-10,1,#White)
    Line(MouseX()+1,MouseY(),10,1,#White)
    Line(MouseX(),MouseY()-1,1,-10,#White)
    Line(MouseX(),MouseY()+1,1,10,#White)
    DrawingMode(#PB_2DDrawing_Default)
    ;Circle(MouseX(),MouseY(),5,#Green)
    StopDrawing()
    FlipBuffers()
  Until KeyboardReleased(#PB_Key_Escape) Or EventID = #PB_Event_CloseWindow