December 04, 2020, 11:06:00 AM

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

#### 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
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 = Graph1estreach
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
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.
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.         grestreach = 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.         destestreach = srcestreach
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
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.
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.         grestreach = 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)
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 grestreach = 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