摘要：尋路模塊通過一番尋找，發(fā)現(xiàn)這系列文章，其不僅包含算法，連尋路算法中的一些基礎知識也一并介紹了，不愧是斯坦福出品，也很感謝譯者要實現(xiàn)點到點最短路徑，還需要做一些微小的工作，下面逐個說明計算曼哈頓距離的函數(shù)目的是尋路，肯定需要一個方法來估算兩點

通過一番尋找，發(fā)現(xiàn)這系列文章，其不僅包含A*算法，連尋路算法中的一些基礎知識也一并介紹了，不愧是斯坦福出品，也很感謝譯者
要實現(xiàn)點A到點B最短路徑，還需要做一些微小的工作，下面逐個說明

目的是尋路，肯定需要一個方法來估算兩點間距離，由于我在移動方式上限定了只有上下左右4個方向可動，那么很自然，兩點間最短距離就是就是曼哈頓距離
代碼:

import numpy as np
def get_manhanttan_distance(coordinate1, coordinate2):
    return np.abs(np.array(coordinate1) - np.array(coordinate2)).sum()

之前提到 “U型走法清掃一次，然后通過A*算法走到最近的未清潔點，如此反復直到所有點都被清潔/走過”
說起來容易，具體實現(xiàn)還幾個步驟：

找出未清潔的點的合集

找出這些點中，離當前所在點最近的點

如果其中有多個點到離當前所在點的距離一樣，則需要按某個規(guī)則，在這些點中取一個出來

未清潔的點=未路過的點，我們現(xiàn)在手頭上有coordinate_list，impassable_coordinate_list和robot.path_log，所以我們可以:

在coordinate_list中去掉impassable_coordinate_list的那些點（差集）得到passable_coordinate_list

在passable_coordinate_list中，中去掉robot.path_log的那些點（差集），得到uncleaned_coordinate_list

這里有需要做兩次交集，本來打算直接用numpy中的setdiff1d函數(shù)，但因setdiff1d不支持多維(多維指(x,y)的形式)，參考了SO上大神的辦法寫一個（用了view來降維）

def multidim_diff(coordinate_list_1, coordinate_list_2):  # 在1不在2中的集合
    coordinate_list_1, coordinate_list_2 = np.array(coordinate_list_1), np.array(coordinate_list_2)
    arr1_view = coordinate_list_1.view([("", coordinate_list_1.dtype)] * coordinate_list_1.shape[1])
    arr2_view = coordinate_list_2.view([("", coordinate_list_2.dtype)] * coordinate_list_2.shape[1])
    intersected = np.setdiff1d(arr1_view, arr2_view)
    return np.ndarray.tolist(intersected)

然后就可以得到未清潔的點列表了

def get_uncleaned_coordinate_list(self):
    passable_coordinate_list = multidim_diff(self.coordinate_list, self.impassable_coordinate_list)
    uncleaned_coordinate_list = multidim_diff(passable_coordinate_list, self.path_log)
    return uncleaned_coordinate_list

寫完上面這段時候我突然想到，上面的曼哈頓距離沒考慮障礙，如果未清潔點和當前所在點隔了一堵墻，反而會繞遠路。但不用曼哈頓距離，對每個未清潔點求最短路徑似乎也不太合適
再次思考，想到這個搜索動作是在U型走法后執(zhí)行的，U型走法覆蓋區(qū)域還是比較規(guī)律的，當前點到之前路過的區(qū)域，不至于很繞路，那么是不是可以找U型走法覆蓋區(qū)域的周圍的點，在這些點里再找最近的呢？
于是把上面的2和3改為:
2 . 找出所有走過的點，取其上下左右4個點到passed_by_coordinate_list
3 . 去重復，去掉非法點(和coordinate_list做交集)，找到未清潔的點(和uncleaned_coordinate_list做交集)，更新passed_by_coordinate_list
4 . 計算passed_by_coordinate_list中所有點到當前點的曼哈頓距離，找到距離最近的那個(多個符合只取第一個)
代碼如下：

def multidim_intersect(coordinate_list_1, coordinate_list_2):  # 兩個坐標集的交集
    coordinate_list_1, coordinate_list_2 = np.array(coordinate_list_1), np.array(coordinate_list_2)
    arr1_view = coordinate_list_1.view([("", coordinate_list_1.dtype)] * coordinate_list_1.shape[1])
    arr2_view = coordinate_list_2.view([("", coordinate_list_2.dtype)] * coordinate_list_2.shape[1])
    intersected = np.intersect1d(arr1_view, arr2_view)
    return np.ndarray.tolist(intersected)

def get_passed_by_coordinate_list(self):
    passed_by_coordinate_list = []
    for coordinate in self.path_log:
        x, y = coordinate
        up = (x, y + 1)
        down = (x, y - 1)
        left = (x - 1, y)
        right = (x + 1, y)
        passed_by_coordinate_list.append(up)
        passed_by_coordinate_list.append(down)
        passed_by_coordinate_list.append(left)
        passed_by_coordinate_list.append(right)
    passed_by_coordinate_list = list(set(passed_by_coordinate_list))
    passed_by_coordinate_list = multidim_intersect(passed_by_coordinate_list, self.coordinate_list)
    passed_by_coordinate_list = multidim_intersect(passed_by_coordinate_list, self.get_uncleaned_coordinate_list())
    return passed_by_coordinate_list
    
def find_nearest_coordinate_by_manhanttan(coordinate1, coordinate_list):
    record = 50000
    for coordinate2 in coordinate_list:
        distance = get_manhanttan_distance(coordinate1, coordinate2)
        if distance < record:
            record = distance
            result = coordinate2
    return result
    
def get_nearest_uncleaned_coordinate(self):
    passed_by_coordinate_list = get_passed_by_coordinate_list(self)
    return find_nearest_coordinate_by_manhanttan(self.current_coordinate, passed_by_coordinate_list)

至此，我們實現(xiàn)了A*算法的目標/前提，即出發(fā)點和終點，之后再說A*算法的實現(xiàn)