传统路径算法-Dijastra

实现Dijastra 算法

## 实现Dijkstra算法进行传统的路径规划
import numpy as np

# 定义了权重网络图
class Dijkstra():
    def __init__(self, start_node, target_node):
        self.node = 6
        inf = 10000
        self.graph = inf * np.ones((6,6), dtype=int)
        self.graph[0][1] = 1
        self.graph[0][2] = 12
        self.graph[1][2] = 9
        self.graph[1][3] = 3
        self.graph[2][4] = 5
        self.graph[3][2] = 4
        self.graph[3][4] = 13
        self.graph[3][5] = 15
        self.graph[4][5] = 4
        self.P = []
        for i in range(self.node):
            self.P.append(i)
        self.Q = []
        # start_node = 0
        self.matrix =[inf, inf, inf, inf, inf, inf]
        print('起始点：{}'.format(start_node))
        self.matrix[start_node - 1] = 0
        self.target = target_node - 1
        self.list_P(start_node - 1)

    def list_P(self, node_p):
        self.P.remove(node_p)
        self.list_Q(node_p)

    def list_Q(self, node_q):
        self.Q.append(node_q)
        self.find_min()

    def find_min(self):
        if self.target in self.Q:
            print('到达终点位置')
            print('矩阵的大小:{}'.format(self.matrix))
            breakpoint()
        b = np.zeros((len(self.Q), 1), dtype=int)
        a = np.zeros((len(self.Q), 1), dtype=int)
        for i in range(len(self.Q)):
            row = self.graph[self.Q[i]][:]
            c = np.where(row == np.min(row))
            if c[0].size == 1:
                b[i] = c
                a[i] = self.Q[i]
            else:
                print('无法到达指定的目标点')
                breakpoint()
        # 还要判读是否在list_Q内
        while np.min(b) in self.Q:
            b = np.delete(b, np.where(b == np.min(b)))
            a = np.delete(a, np.where(b == np.min(b)))
        self.array(b, a)

    def array(self, node, idx):
        # idx 代表父节点，node 代表最小子节点
        for i, j in zip(node, idx):
            a = int(i)
            b = int(j)
            if self.matrix[a] > self.graph[b][a] + self.matrix[b]:
                self.matrix[a] = self.graph[b][a] + self.matrix[b]
        if self.target in self.P:
            if len(self.P) != 0:
                d = []
                for c in zip(self.P):
                    d.append(self.matrix[c[0]])
                next_node = int(np.where(self.matrix == min(d))[0])
                print('下一个点:{}'.format(next_node + 1))
                self.list_P(next_node)
        else:
            print('到达最终目的地')
            print('矩阵的大小:{}'.format(self.matrix))
            breakpoint()

a = Dijkstra(3, 4)