December 04, 2020, 11:06:00 AM

Author Topic: [bb] 3D Short est Path by Moraldi [ 1+ years ago ]  (Read 486 times)

Offline BlitzBot

  • Jr. Member
  • **
  • Posts: 1
[bb] 3D Short est Path by Moraldi [ 1+ years ago ]
« on: June 29, 2017, 12:28:41 AM »
Title : 3D Short est Path
Author : Moraldi
Posted : 1+ years ago

Description : I am not sure if this has been discussed before but I implemented the Dijkstra's algorithm trying to solve the Single-pair shortest-path problem in a 3D graph.

1. First you must create a graph using the following command:

mygraph = Graph_Create(0)

Where 0 is the graph id (you can create many graphs using different id's)

2. Build the graph by adding vertices:
v1.Vertex = Graph_CreateVertex(mygraph, x1, y1, z1)
v2.Vertex = Graph_CreateVertex(mygraph, x2, y2, z2)
...

3. Create connections between vertices use:
Vertex_Connection(v1, v2)
...

4. Make a function call:

Graph_FindShortestPath(mygraph, src, dest)

where src and dest are existing vertices within mygraph
Each graph has the bestsearch vertex and after the previous call you can trace the shortest path following the predecessor member of the bestsearch vertex

The most interesting is that you can define your own 'cost' between two vertices by modifying the Vertex_GetCost() function in the way you like. Currently in the program the 'cost' between two vertices is their distance

If you are confused check the source.

If you build a graph and you find that the program does not calculate the shortest path please let me now in order to debug it.

Enjoy!


Code :
Code: BlitzBasic
  1. Const MAXCONPERVER%=20                                          ; Max connections per vertex
  2.  
  3. Type Vertex
  4.  
  5.         Field d#                                                                ; shortest path estimation. A value of d = -1 represents infinity
  6.         Field p.Vertex                                                  ; predecessor (used in Dijkstra's algorithm)
  7.         Field m%                                                                ; visual representation of vertex
  8.         Field graphid%                                                  ; Graph id where vertex belongs. If graphid = -1 then it not belongs to any graph
  9.         Field connection.Vertex[MAXCONPERVER]   ; array of connected vertices.
  10.         Field conidx%                                                   ; index to connection matrix. Range: From 0 to MAXCONPERVER-1
  11.  
  12. End Type
  13.  
  14. Type Graph
  15.  
  16.         Field id%                               ; graph id
  17.         Field visible%
  18.         Field vcount%                   ; vertices counter
  19.         Field bestreach.Vertex
  20.        
  21. End Type
  22.  
  23. Graphics3D 800, 600, 32
  24.  
  25. Global l% = CreateLight() : RotateEntity l,0,90,0
  26. Global cam% = CreateCamera() : MoveEntity cam, 0,10,-20
  27. Global camerapivot% = CreatePivot() :EntityParent cam, camerapivot
  28. Global fntArial = LoadFont("Arial",15) : SetFont fntArial
  29.  
  30. Global done% = False
  31.  
  32. Global g_source.Vertex=Null
  33. Global g_dest.Vertex=Null
  34.  
  35. Global Graph1.Graph = Graph_Create(0)
  36.  
  37. Global v1.Vertex = Graph_CreateVertex(Graph1, 0, 0, 0)
  38. Global v2.Vertex = Graph_CreateVertex(Graph1, 5, 0, 5)
  39. Global v3.Vertex = Graph_CreateVertex(Graph1, 0, 20, 5)
  40. Global v4.Vertex = Graph_CreateVertex(Graph1, -5, 0, 5)
  41. Global v5.Vertex = Graph_CreateVertex(Graph1, 5, 5, 10)
  42. Global v6.Vertex = Graph_CreateVertex(Graph1, 2, 4, 10)
  43. Global v7.Vertex = Graph_CreateVertex(Graph1, -5, 6,8)
  44. Global v8.Vertex = Graph_CreateVertex(Graph1, 0, 0, 15)
  45.  
  46. Vertex_Connection(v1, v2)
  47. Vertex_Connection(v1, v3)
  48. Vertex_Connection(v1, v4)
  49. Vertex_Connection(v2, v4)
  50. Vertex_Connection(v3, v6)
  51. Vertex_Connection(v4, v7)
  52. Vertex_Connection(v4, v6)
  53. Vertex_Connection(v6, v8)
  54. Vertex_Connection(v7, v8)
  55.  
  56. Graph_SetVisible(Graph1, True)
  57.  
  58. Repeat
  59.  
  60.         CaptureWorld
  61.         UpdateWorld
  62.         RenderWorld
  63.         Graph_DrawConnections(Graph1, cam)
  64.         Info()
  65.         Flip
  66.         CameraDrive(cam, camerapivot)
  67.         If KeyHit(1) Then done = True
  68.         If KeyHit(34) Then Graph_SetVisible(Graph1, Not Graph1visible)
  69.         If KeyHit(32)
  70.                 If g_source<>Null And g_dest<>Null
  71.                         Graph_SetNodeColor(Graph1, 255, 255, 255)
  72.                         Graph_FindShortestPath(Graph1, g_source, g_dest)
  73.                         v.Vertex = Graph1estreach
  74.                         While v<>Null
  75.                                 EntityColor vm, 255,255,0
  76.                                 v = vp
  77.                         Wend
  78.                         If g_source<>Null Then EntityColor g_sourcem, 0,255,0
  79.                         If g_dest<>Null Then EntityColor g_destm, 255,0,0
  80.                 EndIf
  81.         EndIf
  82.  
  83. Until done
  84.  
  85. Graph_Delete(Graph1)
  86. End
  87.  
  88. ;******************
  89. ;
  90. ; UTILITY FUNCTIONS
  91. ;
  92. ;******************
  93. Function Info()
  94.  
  95.         y = 0
  96.         Text 0,y,"Camera navigation"
  97.         y = y + FontHeight()
  98.         Text 0,y,"LMB: Move"
  99.         y = y + FontHeight()
  100.         Text 0,y,"RMB: Rotate"
  101.         y = y + FontHeight()
  102.         Text 0,y,"[ALT]+LMB: Up/Down"
  103.         y = y + FontHeight()
  104.         Text 0,y,"LMB+RMB: Pitch"
  105.         y = y + 2*FontHeight()
  106.         Text 0,y,"Commands"
  107.         y = y + FontHeight()
  108.         Text 0,y,"LMB: Select source node"
  109.         y = y + FontHeight()
  110.         Text 0,y,"RMB: Select destination node"
  111.         y = y + FontHeight()
  112.         Text 0,y,"[D]: Run Dijkstra's algorithm"
  113.         y = y + FontHeight()
  114.         Text 0,y,"[G]: Toggle graph visibility"
  115.  
  116. End Function
  117.  
  118. Function CameraDrive(c%, cp%)
  119.  
  120.         If MouseHit(1)
  121.                 m% =CameraPick(c, MouseX(), MouseY())
  122.                 For v.Vertex = Each Vertex
  123.                         If m = vm
  124.                                 If g_source<>Null Then EntityColor g_sourcem, 255,255,255
  125.                                 g_source = v
  126.                                 EntityColor g_sourcem, 0,255,0
  127.                         EndIf
  128.                 Next
  129.         EndIf
  130.         If MouseHit(2)
  131.                 m% =CameraPick(c, MouseX(), MouseY())
  132.                 For v.Vertex = Each Vertex
  133.                         If m = vm
  134.                                 If g_dest<>Null Then EntityColor g_destm, 255,255,255
  135.                                 g_dest = v
  136.                                 EntityColor g_destm, 255,0,0
  137.                         EndIf
  138.                 Next
  139.         EndIf
  140.         If MouseDown(1) And MouseDown(2)=0 And KeyDown(56) = 0
  141.                 MoveEntity c, -MouseXSpeed(),0,0
  142.                 MoveEntity c, 0,0,MouseYSpeed()
  143.         EndIf
  144.         If MouseDown(1)=0 And MouseDown(2) Then TurnEntity cp, 0,Sgn(MouseXSpeed())*5,0
  145.         If MouseDown(1) And MouseDown(2) Then TurnEntity c,MouseYSpeed(),0,0
  146.         If KeyDown(56)=1 And MouseDown(1) Then MoveEntity c, 0,MouseYSpeed(),0
  147.  
  148.         MouseXSpeed()
  149.         MouseYSpeed()
  150.         MouseZSpeed()
  151.  
  152. End Function
  153.  
  154. ;*****************
  155. ;
  156. ; VERTEX FUNCTIONS
  157. ;
  158. ;*****************
  159.  
  160. Function Vertex_Create.Vertex(x#, y#, z#)
  161.  
  162.         v.Vertex = New Vertex
  163.         vd = -1
  164.         vp = Null
  165.         vm = CreateSphere() : PositionEntity vm, x,y,z : EntityPickMode vm,2
  166.         vgraphid = -1
  167.         vconidx = 0
  168.         For i%=1 To MAXCONPERVER
  169.                 vconnection[i] = Null
  170.         Next
  171.         Return v
  172.        
  173. End Function
  174.  
  175.  
  176. Function Vertex_Copy.Vertex(src.Vertex)
  177.  
  178.         If src = Null Then Return
  179.         dest.Vertex = New Vertex
  180.         destd = srcd
  181.         destp = srcp
  182.         destm = CreateSphere() : PositionEntity destm, EntityX(srcm),EntityY(srcm),EntityZ(srcm)
  183.         destgraphid = srcgraphid
  184.         destconidx = srcconidx
  185.         For i%=o To destconidx
  186.                 destconnection[i] = srcconnection[i]
  187.         Next
  188.         Return dest
  189.  
  190. End Function
  191.  
  192. Function Vertex_Delete(v.Vertex)
  193.  
  194.         If v = Null Then Return
  195.         FreeEntity vm
  196.         Delete v
  197.         v = Null
  198.        
  199. End Function
  200.  
  201. Function Vertex_Connection(src.Vertex, dest.Vertex)
  202.  
  203.         If src = Null Or dest = Null Then Return
  204.         If srcconidx = MAXCONPERVER-1 Then Return       ; we reched maximum number of connections in source
  205.         If destconidx = MAXCONPERVER-1 Then Return              ; we reched maximum number of connections in dest
  206.         srcconnection[srcconidx] = dest
  207.         srcconidx = srcconidx + 1
  208.         destconnection[destconidx] = src
  209.         destconidx = destconidx + 1
  210.  
  211. End Function
  212.  
  213. Function Vertex_RemoveConnection(v.Vertex, src.Vertex)
  214.  
  215.         Local i%, j%, n%, doremove%=False
  216.        
  217.         If v = Null Or n > vconidx-1 Or vconidx=0 Then Return
  218.         For i=0 To vconidx-1
  219.                 If vconnection[i] = src
  220.                         n = i
  221.                         doremove = True
  222.                         Exit
  223.                 EndIf
  224.         Next
  225.         If Not doremove Then Return
  226.         For i=0 To vconidx
  227.                 If i=n
  228.                         For j=i+1 To vconidx-1
  229.                                 vconnection[j-1] = vconnection[j]
  230.                         Next
  231.                 EndIf
  232.         Next
  233.         vconidx = vconidx - 1
  234.  
  235. End Function
  236.  
  237. Function Vertex_DrawConnections(v.Vertex, c%, r%=255, g%=255, b%=255)
  238.  
  239.         Local x1#, y1#, z1#
  240.         If Not EntityInView(vm, c) Then Return
  241.         CameraProject c, EntityX(vm), EntityY(vm), EntityZ(vm)
  242.         x1 = ProjectedX()
  243.         y1 = ProjectedY()
  244.         z1 = ProjectedZ()
  245.         Color r, g, b
  246.         For i=0 To vconidx-1
  247.                 If EntityInView(vm, c)
  248.                         CameraProject c, EntityX(vconnection[i]m), EntityY(vconnection[i]m), EntityZ(vconnection[i]m)
  249.                         Line x1, y1, ProjectedX(), ProjectedY()
  250.                 EndIf
  251.         Next
  252.  
  253. End Function
  254.  
  255. ;
  256. ; returns the distance between v and its neigbor specified by n index
  257. ; If v does not exist or n index is invalid then returns infinite (-1)
  258. ;
  259. Function Vertex_GetCost#(v.Vertex, n%)
  260.  
  261.         If v = Null Then Return -1
  262.         If n<0 Or n>vconidx-1 Then Return -1
  263.         Return EntityDistance(vm, vconnection[n]m)
  264.  
  265. End Function
  266.  
  267. ;*****************
  268. ;
  269. ; GRAPH FUNCTIONS
  270. ;
  271. ;*****************
  272. Function Graph_Create.Graph(id%)
  273.  
  274.         gr.Graph = New Graph
  275.         If gr = Null Then Return
  276.         grid = id
  277.         grvisible = False
  278.         grvcount = 0
  279.         grestreach = Null
  280.         Return gr
  281.        
  282. End Function
  283.  
  284. Function Graph_Copy.Graph(src.Graph, id%)
  285.  
  286.         Local doconnect%
  287.  
  288.         If src = Null Then Return Null
  289.         dest.Graph = New Graph
  290.         If dest = Null Then Return Null
  291.         destid = id
  292.         destestreach = srcestreach
  293.         ; create new vertices and add into the destination graph
  294.         For v.Vertex = Each Vertex
  295.                 If vgraphid = srcid
  296.                         newv.Vertex = Vertex_Create(EntityX(vm), EntityY(vm), EntityZ(vm))
  297.                         newvd = vd
  298.                         Graph_AddVertex(dest, newv)
  299.                 EndIf
  300.         Next
  301.         ; create connections in the destination graph
  302.         For v.Vertex = Each Vertex
  303.                 If vgraphid = srcid
  304.                         v1.Vertex = Graph_FindVertex(dest, EntityX(vm), EntityY(vm), EntityZ(vm))
  305.                         For i%=0 To vconidx-1
  306.                                 v2.Vertex = Graph_FindVertex(dest, EntityX(vconnection[i]m), EntityY(vconnection[i]m), EntityZ(vconnection[i]m))
  307.                                 ; do not duplicate the same connection
  308.                                 doconnect = True
  309.                                 For j%=0 To v2conidx-1
  310.                                         If v2connection[j] = v1 Then doconnect = False
  311.                                 Next
  312.                                 If doconnect Then Vertex_Connection(v1, v2)
  313.                         Next
  314.                 EndIf
  315.         Next
  316.         Graph_SetVisible(dest, srcvisible)
  317.         Return dest
  318.        
  319. End Function
  320.  
  321. Function Graph_Delete(gr.Graph)
  322.  
  323.         If gr = Null Then Return
  324.         Graph_DeleteAllVertices(gr)
  325.         Delete gr
  326.         gr = Null
  327.  
  328. End Function
  329.  
  330. Function Graph_Move(gr.Graph, dx#, dy#, dz#)
  331.  
  332.         If gr = Null Then Return
  333.         For v.Vertex = Each Vertex
  334.                 If vgraphid = grid
  335.                         MoveEntity vm, dx, dy, dz
  336.                 EndIf
  337.         Next
  338.  
  339. End Function
  340.  
  341. Function Graph_CreateVertex.Vertex(gr.Graph, x#, y#, z#)
  342.  
  343.         If gr = Null Then Return
  344.         v.Vertex = Vertex_Create(x, y, z)
  345.         vgraphid = grid
  346.         HideEntity vm
  347.         grvcount = grvcount + 1
  348.         Return v
  349.        
  350. End Function
  351.  
  352. Function Graph_AddVertex(gr.Graph, v.Vertex)
  353.  
  354.         If gr = Null Or v = Null Then Return
  355.         vgraphid = grid
  356.         If grvisible
  357.                 ShowEntity vm
  358.         Else
  359.                 HideEntity vm
  360.         EndIf
  361.         grvcount = grvcount + 1
  362.  
  363. End Function
  364.  
  365. Function Graph_RemoveVertexShallow.Vertex(gr.Graph, v.Vertex)
  366.  
  367.         If gr = Null Or v = Null Or grvcount = 0 Then Return Null
  368.         vgraphid = -1
  369.         grvcount = grvcount - 1
  370.         Return v
  371.  
  372. End Function
  373.  
  374. Function Graph_RemoveVertexDeep.Vertex(gr.Graph, v.Vertex)
  375.  
  376.         If gr = Null Or v = Null Or grvcount = 0 Then Return Null
  377.         vgraphid = -1
  378.         grvcount = grvcount - 1
  379.         ; remove connections of vertex v from graph
  380.         For vv.Vertex = Each Vertex
  381.                 If vvgraphid = grid
  382.                         Vertex_RemoveConnection(vv, v)
  383.                 EndIf
  384.         Next
  385.         Return v
  386.  
  387. End Function
  388.  
  389. Function Graph_DeleteAllVertices(gr.Graph)
  390.  
  391.         If gr = Null Then Return
  392.         For v.Vertex = Each Vertex
  393.                 If vgraphid = grid
  394.                         Vertex_Delete(v)                       
  395.                 EndIf
  396.         Next
  397.         grvcount = 0
  398.  
  399. End Function
  400.  
  401. Function Graph_FindVertex.Vertex(gr.Graph, x#, y#, z#)
  402.  
  403.         For v.Vertex = Each Vertex
  404.                 If vgraphid = grid
  405.                         If (x=EntityX(vm) And y=EntityY(vm) And z=EntityZ(vm)) Return v
  406.                 EndIf
  407.         Next
  408.         Return Null
  409.  
  410. End Function
  411.  
  412. Function Graph_SetNodeColor(gr.Graph, r%, g%, b%)
  413.  
  414.         If gr = Null Then Return
  415.         For v.Vertex = Each Vertex
  416.                 If vgraphid = grid
  417.                         EntityColor vm, r, g, b
  418.                 EndIf
  419.         Next
  420.  
  421. End Function
  422.  
  423. Function Graph_SetVisible(gr.Graph, visible%=True)
  424.  
  425.         If grvisible = visible Then Return
  426.         grvisible = visible
  427.         For v.Vertex = Each Vertex
  428.                 If vgraphid = grid
  429.                         If grvisible
  430.                                 ShowEntity vm
  431.                         Else
  432.                                 HideEntity vm
  433.                         EndIf
  434.                 EndIf
  435.         Next
  436.  
  437. End Function
  438.  
  439. Function Graph_DrawConnections(gr.Graph, c%, r%=255, g%=255, b%=255)
  440.  
  441.         If gr = Null Then Return
  442.         If grvisible = False Then Return
  443.         For v.Vertex = Each Vertex
  444.                 If vgraphid = grid Then Vertex_DrawConnections(v, c)
  445.         Next
  446.  
  447. End Function
  448.  
  449. Function Graph_DrawDistances(gr.Graph, c%, r%=255, g%=255, b%=255)
  450.  
  451.         If gr = Null Then Return
  452.         If grvisible = False Then Return
  453.         Color r, g, b
  454.         For v.Vertex = Each Vertex
  455.                 If vgraphid = grid
  456.                         If EntityInView(vm, c)
  457.                                 CameraProject(c, EntityX(vm), EntityY(vm), EntityZ(vm))
  458.                                 Text ProjectedX(), ProjectedY(), vd
  459.                         EndIf
  460.                 EndIf
  461.         Next
  462.  
  463. End Function
  464.  
  465. Function Graph_FindMinD.Vertex(gr.Graph)
  466.  
  467.         Local mind# = -1
  468.         Local rv.Vertex = Null
  469.  
  470.         For v.Vertex = Each Vertex
  471.                 If vgraphid = grid
  472.                         If vd <> -1
  473.                                 If mind = -1
  474.                                         rv = v
  475.                                         mind = vd
  476.                                 Else
  477.                                         If vd < mind
  478.                                                 rv = v
  479.                                                 mind = vd
  480.                                         EndIf
  481.                                 EndIf
  482.                         EndIf
  483.                 EndIf
  484.         Next
  485.         Return rv
  486.  
  487. End Function
  488.  
  489. Function Graph_FindShortestPath(gr.Graph, src.Vertex, dest.Vertex)
  490.  
  491.         Local u.Vertex
  492.         Local prevu.Vertex
  493.  
  494.         If (gr=Null) Or (src=Null) Or (dest=Null) Or (src=dest) Then Return
  495.         grestreach = Null
  496.         u = src
  497.         For v.Vertex = Each Vertex
  498.                 vp = Null
  499.                 If v = src
  500.                         vd = 0
  501.                 Else
  502.                         vd = -1
  503.                 EndIf
  504.         Next
  505.         q.Graph = Graph_Copy(gr, 1) : Graph_SetVisible(q, False)
  506.         s.Graph = Graph_Create(2) : Graph_SetVisible(s, False)
  507.         qdest.Vertex = Graph_FindVertex(q, EntityX(destm), EntityY(destm), EntityZ(destm))
  508.         While qvcount
  509.                 prevu = u
  510.                 u = Graph_FindMinD(q)
  511.                 Graph_RemoveVertexShallow(q, u)
  512.                 Graph_AddVertex(s, u)
  513.                 If u = Null             ; destination is unreachable
  514.                         u = prevu       ; rollback
  515.                         Exit
  516.                 EndIf
  517.                 If u = qdest Then Exit
  518.                 For i% = 0 To uconidx-1
  519.                         If (uconnection[i]d=-1 Or uconnection[i]d>(ud + Vertex_GetCost(u, i)))
  520.                                 uconnection[i]d = ud + Vertex_GetCost(u, i)
  521.                                 uconnection[i]p = u
  522.                         EndIf
  523.                 Next
  524.         Wend
  525.         v = u
  526.         ; setup predecessors in gr Graph
  527.         While vp <> Null
  528.                 vv.Vertex = Graph_FindVertex(gr, EntityX(vm), EntityY(vm), EntityZ(vm))
  529.                 If v = u Then grestreach = vv
  530.                 vp.Vertex = Graph_FindVertex(gr, EntityX(vpm), EntityY(vpm), EntityZ(vpm))
  531.                 vvp = vp
  532.                 v = vp
  533.         Wend
  534.         ; clean up
  535.         Graph_Delete(q)
  536.         Graph_Delete(s)
  537.  
  538. End Function


Comments : none...

 

SimplePortal 2.3.6 © 2008-2014, SimplePortal