Using Dijkstra Algorithm to get shortest path between two nodes in 3d model

Question

I want to implement Dijkstra Algorithm to get the shortest path between two nodes in this 3D interface. Previously I worked in 2D surface using graph in Dijkstra Algorithm. But this time I stuck in. Looking for an ultimate solution. The star node is my destination node and any of the rest can be the source node.

The position of these nodes were estimated by the following code.

import numpy as np
import matplotlib.pyplot as plt

n = 50

x1, x2 = 0.1, 0.5
y1, y2 = 0.1, 0.5
z1, z2 = 0.1, 0.5

xs = (x2 - x1)*np.random.rand(n) + x1
ys = (y2 - y1)*np.random.rand(n) + y1
zs = (z2 - z1)*np.random.rand(n) + z1

xs1 = (x2 - x1)*np.random.rand(n) + x1
ys1 = (y2 - y1)*np.random.rand(n) + y1
zs1 = (z2 - z1)*np.random.rand(n) + z1

fig = plt.figure()
ax = fig.add_subplot(111,projection='3d')

for i in range(n):
    if zs[i] == max(zs):
        ax.plot(xs[i], ys[i], zs[i], "k*")
    else:
        ax.plot(xs[i], ys[i], zs[i], "go")

ax.set_xlabel('X Label')
ax.set_ylabel('Y Label')
ax.set_zlabel('Z Label')

plt.show()

For Estimating the distance between the nodes:

def distance(p1,p2):
    squared_dist = np.sum((p1-p2)**2, axis=0)
    dist = np.sqrt(squared_dist)
    return dist

graph = []

for i in range(len(xs)):
    graph.append([])
    for j in range(len(xs)):
        p1 = np.array([xs[i], ys[i], zs[i]])
        p2 = np.array([xs[j], ys[j], zs[j]])
        graph[i].append(distance(p1,p2))
graph = np.array(graph)
print(graph)

Dijkstra Algorithm to get the shortest path

M = []
class DijkstraAlgoWithPath:
    global M
    
    def minDistance(self, dist, queue):
        minimum = float("Inf")
        min_index = -1
        
        for i in range(len(dist)):
            if dist[i] < minimum and i in queue:
                minimum = dist[i] 
                min_index = i
        return min_index
    
    def printPath(self, parent, j):
        if parent[j] == -1:                 # If 'j' is the source
            print (j+1, end="  ")
            M.append(j+1)
            return 0
        self.printPath(parent, parent[j])   #If 'j' is not the source, call the recursive function
        M.append(j+1)
        print (j+1, end="  ")
        


    def dijkstraWithPath(self, graph, src, des):
        s = src - 1
        row = len(graph)
        col = len(graph[0])
        
        dist = [float('Infinity')] * row    # initializing all distances are inifinity
        parent = [-1] * row                 # The parent array where to store the shortest path tree
        
        dist[s] = 0                         # Distance of source from itself is zero
        
        q = []                              # An empty list to store all vertices in queue
        for i in range(row):
            q.append(i)
        
        # Find the shortest path for all vertices
        while q:
            # Select the minimum distance vertex 
            # from the set of vertices 
            # which are still in the queue
            u = self.minDistance(dist, q)
            q.remove(u)     # Now remove the minimum distance element which already got
            
            # Consider the vertices which are still in the queue,
            # update the distance and parent index of the adjacent vertices
            # which are selected 
            for i in range(col):
                if graph[u][i] and i in q:  # If dist[i] in the queue
                    if dist[u] + graph[u][i] < dist[i]: # and if the total weight of path from source to destination is less than the current value of dist[i]
                        dist[i] = dist[u] + graph[u][i]
                        parent[i] = u
        self.printPath(parent, des-1)
        

def main():
    global graph
    
    x = DijkstraAlgoWithPath()
    source = 5   # Take input of the source value
    des = 41
    x.dijkstraWithPath(graph, source, des)
    
if __name__ == '__main__':
    main()

Answer 1

Very interesting problem.

You might need to define some edges in the graph in order to use the Dijkstra Algorithm. Otherwise (every pair of notes has an edge with Euclidean Distance), then the direct line should be the shortest path.

This problem is related to motion planning. It is NP-Hard.

J. Canny and J. H. Reif, New lower bound techniques for robot motion planning problems, Proc. 28th Annu. IEEE Sympos. Found. Comput. Sci., 1987, pp. 49-60.