Point'n Click 2D pathFinding

thyphoon · Post by **thyphoon** » Wed Oct 27, 2021 8:00 am

Hello,

Edit : 16/08/2025 New version with lots of bugs fixed
I discovered a very interesting article for making a pathfinding for an adventure game. Having started an engine for this kind of game a few years ago I thought it would be nice to implement.
http://www.groebelsloot.com/2015/12/24/ ... ng-part-1/
http://www.groebelsloot.com/2016/03/13/ ... ng-part-2/

My code is not perfect but it works (If you have high DPI screen don't forget to Enable Dpi aware on Compiler Option

Code: Select all

; ******************************************************************** 
; Program:           Point'n Click 2D pathFinding
; Description:       pathfinding geometric moves with obstacles
; Author:            Thyphoon by following the articles of www.groebelsloot.com
; Date:              August, 2025
; 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.vector::pointf, *v1.vector::pointf, *v2.vector::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.vector::pointf, *v.vector::pointf)
  Protected len.l = length(*v) ; (et non length(@*v))
  If len <> 0
    *vResult\x = *v\x / len
    *vResult\y = *v\y / len
  Else
    *vResult\x = 0
    *vResult\y = 0
  EndIf
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
  
  ; Numerical tolerance for geometric predicates (pixels)
  #Epsilon = 0.01
  
  ;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)
  ; Distance from point P to segment AB with epsilon robustness
  Protected.f dx, dy, denom, ratio, qx, qy

  dx    = x2 - x1
  dy    = y2 - y1
  denom = dx * dx + dy * dy

  ; If A and B are (almost) the same point, return distance to A
  If denom <= #Epsilon
    ProcedureReturn distanceBetwenPoint(Px, Py, x1, y1)
  EndIf

  ratio = ((Px - x1) * dx + (Py - y1) * dy) / denom

  ; Clamp ratio into [0, 1] with epsilon guard
  If ratio < 0.0 - #Epsilon
    ratio = 0.0
  ElseIf ratio > 1.0 + #Epsilon
    ratio = 1.0
  EndIf

  qx = (1.0 - ratio) * x1 + ratio * x2
  qy = (1.0 - ratio) * y1 + ratio * y2

  ProcedureReturn distanceBetwenPoint(Px, Py, qx, qy)
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)
  ; Returns:
  ; 0 = lines do not intersect
  ; 1 = segments intersect (or touch within epsilon)
  ; 2 = infinite lines intersect but OUTSIDE the segments

  Protected.f A1, B1, C1, A2, B2, C2, det
  Protected.f tx, ty

  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

  det = A1 * B2 - A2 * B1

  ; Nearly parallel?
  If Abs(det) <= #Epsilon
    ; Check colinearity: both endpoints of L2 near L1's line
    Protected.f dpa, dpb
    dpa = Abs(A1 * *L2PA\x + B1 * *L2PA\y - C1)
    dpb = Abs(A1 * *L2PB\x + B1 * *L2PB\y - C1)

    If dpa <= #Epsilon And dpb <= #Epsilon
      ; Colinear: check 1D overlap with epsilon
      Protected.f min1x, max1x, min2x, max2x
      Protected.f min1y, max1y, min2y, max2y

      min1x = Min(*L1PA\x, *L1PB\x) : max1x = Max(*L1PA\x, *L1PB\x)
      min2x = Min(*L2PA\x, *L2PB\x) : max2x = Max(*L2PA\x, *L2PB\x)
      min1y = Min(*L1PA\y, *L1PB\y) : max1y = Max(*L1PA\y, *L1PB\y)
      min2y = Min(*L2PA\y, *L2PB\y) : max2y = Max(*L2PA\y, *L2PB\y)

      If (Min(max1x, max2x) + #Epsilon >= Max(min1x, min2x)) And
         (Min(max1y, max2y) + #Epsilon >= Max(min1y, min2y))
        ; They overlap or touch: pick an endpoint for Cross
        *Cross\x = *L2PA\x
        *Cross\y = *L2PA\y
        ProcedureReturn 1
      Else
        ProcedureReturn 0
      EndIf
    EndIf

    ProcedureReturn 0
  EndIf

  ; Proper intersection point of infinite lines
  tx = (B2 * C1 - B1 * C2) / det
  ty = (A1 * C2 - A2 * C1) / det

  *Cross\x = tx
  *Cross\y = ty

  ; Inside segment L1 (expanded by epsilon)?
  If tx < Min(*L1PA\x, *L1PB\x) - #Epsilon Or tx > Max(*L1PA\x, *L1PB\x) + #Epsilon Or
     ty < Min(*L1PA\y, *L1PB\y) - #Epsilon Or ty > Max(*L1PA\y, *L1PB\y) + #Epsilon
    ProcedureReturn 2
  EndIf

  ; Inside segment L2 (expanded by epsilon)?
  If tx < Min(*L2PA\x, *L2PB\x) - #Epsilon Or tx > Max(*L2PA\x, *L2PB\x) + #Epsilon Or
     ty < Min(*L2PA\y, *L2PB\y) - #Epsilon Or ty > Max(*L2PA\y, *L2PB\y) + #Epsilon
    ProcedureReturn 2
  EndIf

  ProcedureReturn 1
EndProcedure


Procedure.b inLineOfSight(*p1.pointf, *p2.pointf, List Polygon.pointf(), obstacle.b = #False)
  Protected.pointf tmp, tmp2, c, d, mid
  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 exactly 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 p1 is a polygon vertex and p2 lies on that edge (within 3 px + ε), allow it
    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.0 + #Epsilon) 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.0 + #Epsilon)
      ProcedureReturn #True
    EndIf

    ; Real intersection test
    Protected cross.pointf
    If LineIntersection(@tmp, @tmp2, @c, @d, @cross) = 1
      ; If both endpoints are not (within ε) on the edge, it's a blocking hit
      If polygon::distanceToSegment(tmp\x,  tmp\y,  c\x, c\y, d\x, d\y)  > #Epsilon And
         polygon::distanceToSegment(tmp2\x, tmp2\y, c\x, c\y, d\x, d\y)  > #Epsilon
        ProcedureReturn #False
      EndIf
    EndIf
  Next

  ; Midpoint inside main walk area / outside obstacles logic
  vector::add(@mid, @tmp, @tmp2)
  mid\x = mid\x / 2
  mid\y = mid\y / 2

  Protected.b result = PointInside(@mid, 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.l(2),List walkPath.l(), 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.vector::pointf, *Target.vector::pointf, List walkArea.polygon::Polygon(), List walkPath.vector::pointf())
  Protected pa.vector::pointf
  Protected pb.vector::pointf
  Protected.l dist,i
  Protected p.vector::pointf
  NewList walkPointCoord.vector::pointf() ; List with only walkable point
  NewList walkPointIndex.l()              ; Sort walkable point with shortest path

  ; --- Clamp la cible sur le bord si elle tombe hors zone ---
  Protected.b result
  ForEach walkArea()
    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())
        *Target\x = p\x
        *Target\y = p\y
      EndIf
    EndIf
  Next

  ; --- Points Start & Target ---
  AddElement(walkPointCoord())
  walkPointCoord()\x = *Start\x
  walkPointCoord()\y = *Start\y

  AddElement(walkPointCoord())
  walkPointCoord()\x = *Target\x
  walkPointCoord()\y = *Target\y

  ; --- VISIBLE DIRECTEMENT ? utiliser la version GLOBALE (zone + obstacles) ---
  If Not polygon::inLineOfSightGlobal(*Start, *Target, walkArea())
    ; sélectionner les sommets utiles (concaves pour contour, convexes pour obstacles)
    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
          If polygon::isVertexConcave(walkArea()\Polygon(), i) = #True
            AddElement(walkPointCoord())
            walkPointCoord()\x = pa\x
            walkPointCoord()\y = pa\y
          EndIf
        Else
          If polygon::isVertexConcave(walkArea()\Polygon(), i) = #False
            AddElement(walkPointCoord())
            walkPointCoord()\x = pa\x
            walkPointCoord()\y = pa\y
          EndIf
        EndIf
      Next
    Next

    ; --- Graphe de visibilité ---
    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

          If polygon::inLineOfSightGlobal(@pa, @pb, walkArea()) = #True
            Graph(i, j) = polygon::distanceBetwenPoint(pa\x, pa\y, pb\x, pb\y)
          Else
            Graph(i, j) = 999999
          EndIf
        Else
          Graph(i, j) = 0
        EndIf
      Next
    Next

    Pathfinding::dijkstra(Graph(), walkPointIndex(), 0, 1)
  Else
    AddElement(walkPointIndex()) : walkPointIndex() = 0
    AddElement(walkPointIndex()) : walkPointIndex() = 1
  EndIf

  drawWalkPath(Graph(), walkPointCoord())

  ; --- Transfert du chemin ---
  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

BarryG · Post by **BarryG** » Wed Oct 27, 2021 9:11 am

Was interesting to play with, but I noticed it's very buggy: for example, you can see below that it walks right through the circle sometimes. Not sure how to fix this - it's way above me.

thyphoon · Post by **thyphoon** » Wed Oct 27, 2021 9:16 am

Me too

Some time i have this problem... but when i want to reproduce i can't

I work on this problem

What Purebasic version do you use ?

BarryG · Post by **BarryG** » Wed Oct 27, 2021 9:42 am

Here's how to reproduce the bug. I don't think the PureBasic version is the issue (it looks like a math issue), but v5.73 LTS (32-bit).

thyphoon · Post by **thyphoon** » Wed Oct 27, 2021 10:57 am

Thanks i reproduce it ! i will search where is the problem

STARGÅTE · Post by **STARGÅTE** » Wed Oct 27, 2021 11:51 am

I like it. It remembers me, on my one code written 2008 in the german forum: https://www.purebasic.fr/german/viewtop ... 12&t=17653

#NULL · Post by **#NULL** » Wed Oct 27, 2021 12:21 pm

Good stuff @thyphoon and @STARGÅTE. I like it for the guybrush alone.

I did an RTS game once with a-star pathfinding wich was pretty slow for large maps. Could have been faster with this if only selected areas are considered instead of all tiles.

I initialized the buggy route like this directly before the main loop

Code: Select all

mainChara\coord\x=290
mainChara\coord\y=140
mainChara\target\x=40
mainChara\target\y=230
Pathfinding::getWalkPath(@mainChara\coord,@mainChara\target,room\walkArea(),mainChara\Path())
mainChara\move=#True
mainChara\pathLength=Pathfinding::getPathLength(mainChara\Path())

interestingly the bug goes away if you modify the last coord of the square..

Code: Select all

polygon::addpoint(room\walkArea()\Polygon(),140,280)

to something more odd like [139,280] or [140,281] for example.

thyphoon · Post by **thyphoon** » Wed Oct 27, 2021 2:37 pm

STARGÅTE wrote: Wed Oct 27, 2021 11:51 am I like it. It remembers me, on my one code written 2008 in the german forum: https://www.purebasic.fr/german/viewtop ... 12&t=17653

Great, I had missed it. I love. But your code gave me ideas

#NULL wrote: Wed Oct 27, 2021 12:21 pm Good stuff @thyphoon and @STARGÅTE. I like it for the guybrush alone.

Thanks ^_^

#NULL wrote: Wed Oct 27, 2021 12:21 pm interestingly the bug goes away if you modify the last coord of the square..

Yes, have the same effect when you change step in circle shape.

Code: Select all

For z=0 To 360 Step 31
  polygon::addpoint(room\walkArea()\Polygon(),60+20*Cos(Radian(z)),300+20*Sin(Radian(z)))
Next

i can trace all path possible... and i see the problem, a will update my code as soon as possible

thyphoon · Post by **thyphoon** » Thu Oct 28, 2021 10:41 am

Hello i found the problem,

BarryG wrote: Wed Oct 27, 2021 9:42 am Here's how to reproduce the bug. I don't think the PureBasic version is the issue (it looks like a math issue), but v5.73 LTS (32-bit).

Line Segments cross function problem. I change by another code. And it's Ok.
But i have another bug who can enter in obstace some time....

Code: Select all

; ******************************************************************** 
; 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

BarryG · Post by **BarryG** » Thu Oct 28, 2021 12:04 pm

thyphoon wrote: Thu Oct 28, 2021 10:41 ami have another bug who can enter in obstace some time

Yep, I saw that bug - you can click in the circle to go inside it, but then you can't walk out again.

thyphoon · Post by **thyphoon** » Sat Aug 16, 2025 10:53 am

new version ^_^

Code: Select all

; ******************************************************************** 
; Program:           Point'n Click 2D pathFinding
; Description:       pathfinding geometric moves with obstacles
; Author:            Thyphoon by following the articles of www.groebelsloot.com
; Date:              August, 2025
; 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.vector::pointf, *v1.vector::pointf, *v2.vector::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.vector::pointf, *v.vector::pointf)
  Protected len.l = length(*v) ; (et non length(@*v))
  If len <> 0
    *vResult\x = *v\x / len
    *vResult\y = *v\y / len
  Else
    *vResult\x = 0
    *vResult\y = 0
  EndIf
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
  
  ; Numerical tolerance for geometric predicates (pixels)
  #Epsilon = 0.01
  
  ;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)
  ; Distance from point P to segment AB with epsilon robustness
  Protected.f dx, dy, denom, ratio, qx, qy

  dx    = x2 - x1
  dy    = y2 - y1
  denom = dx * dx + dy * dy

  ; If A and B are (almost) the same point, return distance to A
  If denom <= #Epsilon
    ProcedureReturn distanceBetwenPoint(Px, Py, x1, y1)
  EndIf

  ratio = ((Px - x1) * dx + (Py - y1) * dy) / denom

  ; Clamp ratio into [0, 1] with epsilon guard
  If ratio < 0.0 - #Epsilon
    ratio = 0.0
  ElseIf ratio > 1.0 + #Epsilon
    ratio = 1.0
  EndIf

  qx = (1.0 - ratio) * x1 + ratio * x2
  qy = (1.0 - ratio) * y1 + ratio * y2

  ProcedureReturn distanceBetwenPoint(Px, Py, qx, qy)
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)
  ; Returns:
  ; 0 = lines do not intersect
  ; 1 = segments intersect (or touch within epsilon)
  ; 2 = infinite lines intersect but OUTSIDE the segments

  Protected.f A1, B1, C1, A2, B2, C2, det
  Protected.f tx, ty

  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

  det = A1 * B2 - A2 * B1

  ; Nearly parallel?
  If Abs(det) <= #Epsilon
    ; Check colinearity: both endpoints of L2 near L1's line
    Protected.f dpa, dpb
    dpa = Abs(A1 * *L2PA\x + B1 * *L2PA\y - C1)
    dpb = Abs(A1 * *L2PB\x + B1 * *L2PB\y - C1)

    If dpa <= #Epsilon And dpb <= #Epsilon
      ; Colinear: check 1D overlap with epsilon
      Protected.f min1x, max1x, min2x, max2x
      Protected.f min1y, max1y, min2y, max2y

      min1x = Min(*L1PA\x, *L1PB\x) : max1x = Max(*L1PA\x, *L1PB\x)
      min2x = Min(*L2PA\x, *L2PB\x) : max2x = Max(*L2PA\x, *L2PB\x)
      min1y = Min(*L1PA\y, *L1PB\y) : max1y = Max(*L1PA\y, *L1PB\y)
      min2y = Min(*L2PA\y, *L2PB\y) : max2y = Max(*L2PA\y, *L2PB\y)

      If (Min(max1x, max2x) + #Epsilon >= Max(min1x, min2x)) And
         (Min(max1y, max2y) + #Epsilon >= Max(min1y, min2y))
        ; They overlap or touch: pick an endpoint for Cross
        *Cross\x = *L2PA\x
        *Cross\y = *L2PA\y
        ProcedureReturn 1
      Else
        ProcedureReturn 0
      EndIf
    EndIf

    ProcedureReturn 0
  EndIf

  ; Proper intersection point of infinite lines
  tx = (B2 * C1 - B1 * C2) / det
  ty = (A1 * C2 - A2 * C1) / det

  *Cross\x = tx
  *Cross\y = ty

  ; Inside segment L1 (expanded by epsilon)?
  If tx < Min(*L1PA\x, *L1PB\x) - #Epsilon Or tx > Max(*L1PA\x, *L1PB\x) + #Epsilon Or
     ty < Min(*L1PA\y, *L1PB\y) - #Epsilon Or ty > Max(*L1PA\y, *L1PB\y) + #Epsilon
    ProcedureReturn 2
  EndIf

  ; Inside segment L2 (expanded by epsilon)?
  If tx < Min(*L2PA\x, *L2PB\x) - #Epsilon Or tx > Max(*L2PA\x, *L2PB\x) + #Epsilon Or
     ty < Min(*L2PA\y, *L2PB\y) - #Epsilon Or ty > Max(*L2PA\y, *L2PB\y) + #Epsilon
    ProcedureReturn 2
  EndIf

  ProcedureReturn 1
EndProcedure


Procedure.b inLineOfSight(*p1.pointf, *p2.pointf, List Polygon.pointf(), obstacle.b = #False)
  Protected.pointf tmp, tmp2, c, d, mid
  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 exactly 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 p1 is a polygon vertex and p2 lies on that edge (within 3 px + ε), allow it
    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.0 + #Epsilon) 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.0 + #Epsilon)
      ProcedureReturn #True
    EndIf

    ; Real intersection test
    Protected cross.pointf
    If LineIntersection(@tmp, @tmp2, @c, @d, @cross) = 1
      ; If both endpoints are not (within ε) on the edge, it's a blocking hit
      If polygon::distanceToSegment(tmp\x,  tmp\y,  c\x, c\y, d\x, d\y)  > #Epsilon And
         polygon::distanceToSegment(tmp2\x, tmp2\y, c\x, c\y, d\x, d\y)  > #Epsilon
        ProcedureReturn #False
      EndIf
    EndIf
  Next

  ; Midpoint inside main walk area / outside obstacles logic
  vector::add(@mid, @tmp, @tmp2)
  mid\x = mid\x / 2
  mid\y = mid\y / 2

  Protected.b result = PointInside(@mid, 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.l(2),List walkPath.l(), 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.vector::pointf, *Target.vector::pointf, List walkArea.polygon::Polygon(), List walkPath.vector::pointf())
  Protected pa.vector::pointf
  Protected pb.vector::pointf
  Protected.l dist,i
  Protected p.vector::pointf
  NewList walkPointCoord.vector::pointf() ; List with only walkable point
  NewList walkPointIndex.l()              ; Sort walkable point with shortest path

  ; --- Clamp la cible sur le bord si elle tombe hors zone ---
  Protected.b result
  ForEach walkArea()
    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())
        *Target\x = p\x
        *Target\y = p\y
      EndIf
    EndIf
  Next

  ; --- Points Start & Target ---
  AddElement(walkPointCoord())
  walkPointCoord()\x = *Start\x
  walkPointCoord()\y = *Start\y

  AddElement(walkPointCoord())
  walkPointCoord()\x = *Target\x
  walkPointCoord()\y = *Target\y

  ; --- VISIBLE DIRECTEMENT ? utiliser la version GLOBALE (zone + obstacles) ---
  If Not polygon::inLineOfSightGlobal(*Start, *Target, walkArea())
    ; sélectionner les sommets utiles (concaves pour contour, convexes pour obstacles)
    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
          If polygon::isVertexConcave(walkArea()\Polygon(), i) = #True
            AddElement(walkPointCoord())
            walkPointCoord()\x = pa\x
            walkPointCoord()\y = pa\y
          EndIf
        Else
          If polygon::isVertexConcave(walkArea()\Polygon(), i) = #False
            AddElement(walkPointCoord())
            walkPointCoord()\x = pa\x
            walkPointCoord()\y = pa\y
          EndIf
        EndIf
      Next
    Next

    ; --- Graphe de visibilité ---
    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

          If polygon::inLineOfSightGlobal(@pa, @pb, walkArea()) = #True
            Graph(i, j) = polygon::distanceBetwenPoint(pa\x, pa\y, pb\x, pb\y)
          Else
            Graph(i, j) = 999999
          EndIf
        Else
          Graph(i, j) = 0
        EndIf
      Next
    Next

    Pathfinding::dijkstra(Graph(), walkPointIndex(), 0, 1)
  Else
    AddElement(walkPointIndex()) : walkPointIndex() = 0
    AddElement(walkPointIndex()) : walkPointIndex() = 1
  EndIf

  drawWalkPath(Graph(), walkPointCoord())

  ; --- Transfert du chemin ---
  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

miso · Post by **miso** » Sat Aug 16, 2025 11:26 am

Thank you for sharing this. Not perfect yet, but very cool none the less. The license is also appreciated.

minimy · Post by **minimy** » Sat Aug 16, 2025 3:58 pm

Ohhh guys!! This is perfect!
Thank you very much for share!

zikitrake · Post by **zikitrake** » Sat Aug 16, 2025 4:23 pm

Really nice code!

I have found a point where it gets stuck and does not move towards the destination.

thyphoon · Post by **thyphoon** » Sat Aug 16, 2025 6:03 pm

miso wrote: Sat Aug 16, 2025 11:26 am Thank you for sharing this. Not perfect yet, but very cool none the less. The license is also appreciated.

Thanks, I still have a lot of work to do.
it was by using other people's code that I learned to code. I find it normal to share in turn.

minimy wrote: Sat Aug 16, 2025 3:58 pm Ohhh guys!! This is perfect!
Thank you very much for share!

Not perfect yet, there are improvements to be made... But I'm already quite happy with the result

zikitrake wrote: Sat Aug 16, 2025 4:23 pm Really nice code!

I have found a point where it gets stuck and does not move towards the destination.

Thans yes ... Not easy ... but i search how to correct it

PureBasic Forums - English

Point'n Click 2D pathFinding

Point'n Click 2D pathFinding

Re: Point'n Click 2D pathFinding

Re: Point'n Click 2D pathFinding

Re: Point'n Click 2D pathFinding

Re: Point'n Click 2D pathFinding

Re: Point'n Click 2D pathFinding

Re: Point'n Click 2D pathFinding

Re: Point'n Click 2D pathFinding

Re: Point'n Click 2D pathFinding

Re: Point'n Click 2D pathFinding

Re: Point'n Click 2D pathFinding

Re: Point'n Click 2D pathFinding

Re: Point'n Click 2D pathFinding

Re: Point'n Click 2D pathFinding

Re: Point'n Click 2D pathFinding