3D Short est Path

This code has been declared by its author to be Public Domain code.

3D Short est Path by Moraldi

2007

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!

Const MAXCONPERVER%=20						; Max connections per vertex

Type Vertex

	Field d#								; shortest path estimation. A value of d = -1 represents infinity
	Field p.Vertex							; predecessor (used in Dijkstra's algorithm)
	Field m%								; visual representation of vertex
	Field graphid%							; Graph id where vertex belongs. If graphid = -1 then it not belongs to any graph
	Field connection.Vertex[MAXCONPERVER]	; array of connected vertices.
	Field conidx%							; index to connection matrix. Range: From 0 to MAXCONPERVER-1

End Type

Type Graph

	Field id%				; graph id
	Field visible%
	Field vcount%			; vertices counter
	Field bestreach.Vertex
	
End Type

Graphics3D 800, 600, 32

Global l% = CreateLight() : RotateEntity l,0,90,0
Global cam% = CreateCamera() : MoveEntity cam, 0,10,-20
Global camerapivot% = CreatePivot() :EntityParent cam, camerapivot
Global fntArial = LoadFont("Arial",15) : SetFont fntArial

Global done% = False

Global g_source.Vertex=Null
Global g_dest.Vertex=Null

Global Graph1.Graph = Graph_Create(0)

Global v1.Vertex = Graph_CreateVertex(Graph1, 0, 0, 0)
Global v2.Vertex = Graph_CreateVertex(Graph1, 5, 0, 5)
Global v3.Vertex = Graph_CreateVertex(Graph1, 0, 20, 5)
Global v4.Vertex = Graph_CreateVertex(Graph1, -5, 0, 5)
Global v5.Vertex = Graph_CreateVertex(Graph1, 5, 5, 10)
Global v6.Vertex = Graph_CreateVertex(Graph1, 2, 4, 10)
Global v7.Vertex = Graph_CreateVertex(Graph1, -5, 6,8)
Global v8.Vertex = Graph_CreateVertex(Graph1, 0, 0, 15)

Vertex_Connection(v1, v2)
Vertex_Connection(v1, v3)
Vertex_Connection(v1, v4)
Vertex_Connection(v2, v4)
Vertex_Connection(v3, v6)
Vertex_Connection(v4, v7)
Vertex_Connection(v4, v6)
Vertex_Connection(v6, v8)
Vertex_Connection(v7, v8)

Graph_SetVisible(Graph1, True)

Repeat

	CaptureWorld
	UpdateWorld
	RenderWorld
	Graph_DrawConnections(Graph1, cam)
	Info()
	Flip
	CameraDrive(cam, camerapivot)
	If KeyHit(1) Then done = True
	If KeyHit(34) Then Graph_SetVisible(Graph1, Not Graph1\visible)
	If KeyHit(32)
		If g_source<>Null And g_dest<>Null
			Graph_SetNodeColor(Graph1, 255, 255, 255)
			Graph_FindShortestPath(Graph1, g_source, g_dest)
			v.Vertex = Graph1\bestreach
			While v<>Null
				EntityColor v\m, 255,255,0
				v = v\p
			Wend
			If g_source<>Null Then EntityColor g_source\m, 0,255,0
			If g_dest<>Null Then EntityColor g_dest\m, 255,0,0
		EndIf
	EndIf

Until done

Graph_Delete(Graph1)
End

;******************
;
; UTILITY FUNCTIONS
;
;******************
Function Info()

	y = 0
	Text 0,y,"Camera navigation"
	y = y + FontHeight()
	Text 0,y,"LMB: Move"
	y = y + FontHeight()
	Text 0,y,"RMB: Rotate"
	y = y + FontHeight()
	Text 0,y,"[ALT]+LMB: Up/Down"
	y = y + FontHeight()
	Text 0,y,"LMB+RMB: Pitch"
	y = y + 2*FontHeight()
	Text 0,y,"Commands"
	y = y + FontHeight()
	Text 0,y,"LMB: Select source node"
	y = y + FontHeight()
	Text 0,y,"RMB: Select destination node"
	y = y + FontHeight()
	Text 0,y,"[D]: Run Dijkstra's algorithm"
	y = y + FontHeight()
	Text 0,y,"[G]: Toggle graph visibility"

End Function

Function CameraDrive(c%, cp%)

	If MouseHit(1)
		m% =CameraPick(c, MouseX(), MouseY())
		For v.Vertex = Each Vertex
			If m = v\m
				If g_source<>Null Then EntityColor g_source\m, 255,255,255
				g_source = v
				EntityColor g_source\m, 0,255,0
			EndIf
		Next
	EndIf
	If MouseHit(2)
		m% =CameraPick(c, MouseX(), MouseY())
		For v.Vertex = Each Vertex
			If m = v\m
				If g_dest<>Null Then EntityColor g_dest\m, 255,255,255
				g_dest = v
				EntityColor g_dest\m, 255,0,0
			EndIf
		Next
	EndIf
	If MouseDown(1) And MouseDown(2)=0 And KeyDown(56) = 0
		MoveEntity c, -MouseXSpeed(),0,0
		MoveEntity c, 0,0,MouseYSpeed()
	EndIf
	If MouseDown(1)=0 And MouseDown(2) Then TurnEntity cp, 0,Sgn(MouseXSpeed())*5,0
	If MouseDown(1) And MouseDown(2) Then TurnEntity c,MouseYSpeed(),0,0
	If KeyDown(56)=1 And MouseDown(1) Then MoveEntity c, 0,MouseYSpeed(),0

	MouseXSpeed()
	MouseYSpeed()
	MouseZSpeed()

End Function

;*****************
;
; VERTEX FUNCTIONS
;
;*****************

Function Vertex_Create.Vertex(x#, y#, z#)

	v.Vertex = New Vertex
	v\d = -1
	v\p = Null
	v\m = CreateSphere() : PositionEntity v\m, x,y,z : EntityPickMode v\m,2
	v\graphid = -1
	v\conidx = 0
	For i%=1 To MAXCONPERVER
		v\connection[i] = Null
	Next
	Return v
	
End Function


Function Vertex_Copy.Vertex(src.Vertex)

	If src = Null Then Return
	dest.Vertex = New Vertex
	dest\d = src\d
	dest\p = src\p
	dest\m = CreateSphere() : PositionEntity dest\m, EntityX(src\m),EntityY(src\m),EntityZ(src\m)
	dest\graphid = src\graphid
	dest\conidx = src\conidx
	For i%=o To dest\conidx
		dest\connection[i] = src\connection[i]
	Next
	Return dest

End Function

Function Vertex_Delete(v.Vertex)

	If v = Null Then Return
	FreeEntity v\m
	Delete v
	v = Null
	
End Function

Function Vertex_Connection(src.Vertex, dest.Vertex)

	If src = Null Or dest = Null Then Return
	If src\conidx = MAXCONPERVER-1 Then Return	; we reched maximum number of connections in source
	If dest\conidx = MAXCONPERVER-1 Then Return		; we reched maximum number of connections in dest
	src\connection[src\conidx] = dest
	src\conidx = src\conidx + 1
	dest\connection[dest\conidx] = src
	dest\conidx = dest\conidx + 1

End Function

Function Vertex_RemoveConnection(v.Vertex, src.Vertex)

	Local i%, j%, n%, doremove%=False
	
	If v = Null Or n > v\conidx-1 Or v\conidx=0 Then Return
	For i=0 To v\conidx-1
		If v\connection[i] = src
			n = i
			doremove = True
			Exit
		EndIf
	Next
	If Not doremove Then Return
	For i=0 To v\conidx
		If i=n
			For j=i+1 To v\conidx-1
				v\connection[j-1] = v\connection[j]
			Next
		EndIf
	Next
	v\conidx = v\conidx - 1

End Function

Function Vertex_DrawConnections(v.Vertex, c%, r%=255, g%=255, b%=255)

	Local x1#, y1#, z1#
	If Not EntityInView(v\m, c) Then Return
	CameraProject c, EntityX(v\m), EntityY(v\m), EntityZ(v\m)
	x1 = ProjectedX()
	y1 = ProjectedY()
	z1 = ProjectedZ()
	Color r, g, b
	For i=0 To v\conidx-1
		If EntityInView(v\m, c)
			CameraProject c, EntityX(v\connection[i]\m), EntityY(v\connection[i]\m), EntityZ(v\connection[i]\m)
			Line x1, y1, ProjectedX(), ProjectedY()
		EndIf
	Next

End Function

;
; returns the distance between v and its neigbor specified by n index
; If v does not exist or n index is invalid then returns infinite (-1)
;
Function Vertex_GetCost#(v.Vertex, n%)

	If v = Null Then Return -1
	If n<0 Or n>v\conidx-1 Then Return -1
	Return EntityDistance(v\m, v\connection[n]\m)

End Function

;*****************
;
; GRAPH FUNCTIONS
;
;*****************
Function Graph_Create.Graph(id%)

	gr.Graph = New Graph
	If gr = Null Then Return
	gr\id = id
	gr\visible = False
	gr\vcount = 0
	gr\bestreach = Null
	Return gr
	
End Function

Function Graph_Copy.Graph(src.Graph, id%)

	Local doconnect%

	If src = Null Then Return Null
	dest.Graph = New Graph
	If dest = Null Then Return Null
	dest\id = id
	dest\bestreach = src\bestreach
	; create new vertices and add into the destination graph
	For v.Vertex = Each Vertex
		If v\graphid = src\id
			newv.Vertex = Vertex_Create(EntityX(v\m), EntityY(v\m), EntityZ(v\m))
			newv\d = v\d
			Graph_AddVertex(dest, newv)
		EndIf
	Next
	; create connections in the destination graph
	For v.Vertex = Each Vertex
		If v\graphid = src\id
			v1.Vertex = Graph_FindVertex(dest, EntityX(v\m), EntityY(v\m), EntityZ(v\m))
			For i%=0 To v\conidx-1
				v2.Vertex = Graph_FindVertex(dest, EntityX(v\connection[i]\m), EntityY(v\connection[i]\m), EntityZ(v\connection[i]\m))
				; do not duplicate the same connection
				doconnect = True
				For j%=0 To v2\conidx-1
					If v2\connection[j] = v1 Then doconnect = False
				Next
				If doconnect Then Vertex_Connection(v1, v2)
			Next
		EndIf
	Next
	Graph_SetVisible(dest, src\visible)
	Return dest
	
End Function

Function Graph_Delete(gr.Graph)

	If gr = Null Then Return
	Graph_DeleteAllVertices(gr)
	Delete gr
	gr = Null

End Function

Function Graph_Move(gr.Graph, dx#, dy#, dz#)

	If gr = Null Then Return
	For v.Vertex = Each Vertex
		If v\graphid = gr\id
			MoveEntity v\m, dx, dy, dz
		EndIf
	Next

End Function

Function Graph_CreateVertex.Vertex(gr.Graph, x#, y#, z#)

	If gr = Null Then Return
	v.Vertex = Vertex_Create(x, y, z)
	v\graphid = gr\id
	HideEntity v\m
	gr\vcount = gr\vcount + 1
	Return v
	
End Function

Function Graph_AddVertex(gr.Graph, v.Vertex)

	If gr = Null Or v = Null Then Return
	v\graphid = gr\id
	If gr\visible
		ShowEntity v\m
	Else
		HideEntity v\m
	EndIf
	gr\vcount = gr\vcount + 1

End Function

Function Graph_RemoveVertexShallow.Vertex(gr.Graph, v.Vertex)

	If gr = Null Or v = Null Or gr\vcount = 0 Then Return Null
	v\graphid = -1
	gr\vcount = gr\vcount - 1
	Return v

End Function

Function Graph_RemoveVertexDeep.Vertex(gr.Graph, v.Vertex)

	If gr = Null Or v = Null Or gr\vcount = 0 Then Return Null
	v\graphid = -1
	gr\vcount = gr\vcount - 1
	; remove connections of vertex v from graph
	For vv.Vertex = Each Vertex
		If vv\graphid = gr\id
			Vertex_RemoveConnection(vv, v)
		EndIf
	Next
	Return v

End Function

Function Graph_DeleteAllVertices(gr.Graph)

	If gr = Null Then Return
	For v.Vertex = Each Vertex
		If v\graphid = gr\id
			Vertex_Delete(v)			
		EndIf
	Next
	gr\vcount = 0

End Function

Function Graph_FindVertex.Vertex(gr.Graph, x#, y#, z#)

	For v.Vertex = Each Vertex
		If v\graphid = gr\id
			If (x=EntityX(v\m) And y=EntityY(v\m) And z=EntityZ(v\m)) Return v
		EndIf
	Next
	Return Null

End Function

Function Graph_SetNodeColor(gr.Graph, r%, g%, b%)

	If gr = Null Then Return
	For v.Vertex = Each Vertex
		If v\graphid = gr\id
			EntityColor v\m, r, g, b
		EndIf
	Next

End Function

Function Graph_SetVisible(gr.Graph, visible%=True)

	If gr\visible = visible Then Return
	gr\visible = visible
	For v.Vertex = Each Vertex
		If v\graphid = gr\id
			If gr\visible
				ShowEntity v\m
			Else
				HideEntity v\m
			EndIf
		EndIf
	Next

End Function

Function Graph_DrawConnections(gr.Graph, c%, r%=255, g%=255, b%=255)

	If gr = Null Then Return
	If gr\visible = False Then Return
	For v.Vertex = Each Vertex
		If v\graphid = gr\id Then Vertex_DrawConnections(v, c)
	Next

End Function

Function Graph_DrawDistances(gr.Graph, c%, r%=255, g%=255, b%=255)

	If gr = Null Then Return
	If gr\visible = False Then Return
	Color r, g, b
	For v.Vertex = Each Vertex
		If v\graphid = gr\id
			If EntityInView(v\m, c)
				CameraProject(c, EntityX(v\m), EntityY(v\m), EntityZ(v\m))
				Text ProjectedX(), ProjectedY(), v\d
			EndIf
		EndIf
	Next

End Function

Function Graph_FindMinD.Vertex(gr.Graph)

	Local mind# = -1
	Local rv.Vertex = Null

	For v.Vertex = Each Vertex
		If v\graphid = gr\id
			If v\d <> -1
				If mind = -1
					rv = v
					mind = v\d
				Else
					If v\d < mind
						rv = v
						mind = v\d
					EndIf
				EndIf
			EndIf
		EndIf
	Next
	Return rv

End Function

Function Graph_FindShortestPath(gr.Graph, src.Vertex, dest.Vertex)

	Local u.Vertex
	Local prevu.Vertex

	If (gr=Null) Or (src=Null) Or (dest=Null) Or (src=dest) Then Return
	gr\bestreach = Null
	u = src
	For v.Vertex = Each Vertex
		v\p = Null
		If v = src
			v\d = 0
		Else
			v\d = -1
		EndIf
	Next
	q.Graph = Graph_Copy(gr, 1) : Graph_SetVisible(q, False)
	s.Graph = Graph_Create(2) : Graph_SetVisible(s, False)
	qdest.Vertex = Graph_FindVertex(q, EntityX(dest\m), EntityY(dest\m), EntityZ(dest\m))
	While q\vcount
		prevu = u
		u = Graph_FindMinD(q)
		Graph_RemoveVertexShallow(q, u)
		Graph_AddVertex(s, u)
		If u = Null		; destination is unreachable
			u = prevu	; rollback
			Exit
		EndIf
		If u = qdest Then Exit
		For i% = 0 To u\conidx-1
			If (u\connection[i]\d=-1 Or u\connection[i]\d>(u\d + Vertex_GetCost(u, i)))
				u\connection[i]\d = u\d + Vertex_GetCost(u, i)
				u\connection[i]\p = u
			EndIf
		Next
	Wend
	v = u
	; setup predecessors in gr Graph
	While v\p <> Null
		vv.Vertex = Graph_FindVertex(gr, EntityX(v\m), EntityY(v\m), EntityZ(v\m))
		If v = u Then gr\bestreach = vv
		vp.Vertex = Graph_FindVertex(gr, EntityX(v\p\m), EntityY(v\p\m), EntityZ(v\p\m))
		vv\p = vp
		v = v\p
	Wend
	; clean up
	Graph_Delete(q)
	Graph_Delete(s)

End Function

Comments

None.

Code Archives Forum