路径规划算法的定义是：移动机器人在具有障碍物的环境中按照一定的评价标准，寻找一条从起始状态到目标状态的无碰路径。路径规划算法的其中一种分类方法是分为全局路径规划和局部路径规划。

全局路径规划是根据环境全局的信息，这包括机器人在当前状态下探测不到的信息。全局规划将环境信息存储在一张图中，利用这张图找到可行的路径。全局算法往往需要耗费大量的计算时间，不适于快速变化的动态环境，同时由于全局路径规划需要事先获得全局环境信息，也不适于未知环境下的规划任务。

import numpy as np
from random import random
import matplotlib.pyplot as plt
from matplotlib import collections  as mc
from collections import deque
import math
import time

class Line():
    ''' Define line '''
    def __init__(self, p0, p1):
        self.p = np.array(p0)
        self.dirn = np.array(p1) - np.array(p0)
        self.dist = np.linalg.norm(self.dirn)
        self.dirn /= self.dist # normalize

    def path(self, t):
        return self.p + t * self.dirn


def Intersection(line, center, radius):
    ''' Check line-sphere (circle) intersection '''
    radius=radius+0.1
    a = np.dot(line.dirn, line.dirn)
    b = 2 * np.dot(line.dirn, line.p - center)
    c = np.dot(line.p - center, line.p - center) - radius * radius

    discriminant = b * b - 4 * a * c
    if discriminant < 0:
        return False

    t1 = (-b + np.sqrt(discriminant)) / (2 * a)
    t2 = (-b - np.sqrt(discriminant)) / (2 * a)

    if (t1 < 0 and t2 < 0) or (t1 > line.dist and t2 > line.dist):
        return False

    return True



def distance(x, y):
    return np.linalg.norm(np.array(x) - np.array(y))


def isInObstacle(vex, obstacles, radius):
    for obs in obstacles:
        if distance(obs, vex) < radius+0.01:



            return True
    return False


def isThruObstacle(line, obstacles, radius):
    for obs in obstacles:
        if Intersection(line, obs, radius):
            return True
    return False


def nearest(G, vex, obstacles, radius):
    Nvex = None
    Nidx = None
    minDist = float("inf")

    for idx, v in enumerate(G.vertices):
        line = Line(v, vex)
        if isThruObstacle(line, obstacles, radius):
            continue
        dist = distance(v, vex)
        if dist < minDist:
            minDist = dist
            Nidx = idx
            Nvex = v

    return Nvex, Nidx

def newVertex(randvex, nearvex, stepSize):
    dirn = np.array(randvex) - np.array(nearvex)
    length = np.linalg.norm(dirn)
    dirn = (dirn / length) * min (stepSize, length)

    newvex = (nearvex[0]+dirn[0], nearvex[1]+dirn[1])
    return newvex


def window(startpos, endpos):
    ''' Define seach window - 2 times of start to end rectangle'''
    width = endpos[0] - startpos[0]
    height = endpos[1] - startpos[1]
    winx = startpos[0] - (width / 2.)
    winy = startpos[1] - (height / 2.)
    return winx, winy, width, height


def isInWindow(pos, winx, winy, width, height):
    ''' Restrict new vertex insides search window'''
    if winx < pos[0] < winx+width and \
        winy < pos[1] < winy+height:
        return True
    else:
        return False


class Graph:
    ''' Define graph '''
    def __init__(self, startpos, endpos):
        self.startpos = startpos
        self.endpos = endpos

        self.vertices = [startpos]
        self.edges = []
        self.success = False

        self.vex2idx = {startpos:0}
        self.neighbors = {0:[]}
        self.distances = {0:0.}
        self.parent={0:-1}

        self.sx = endpos[0] - startpos[0]
        self.sy = endpos[1] - startpos[1]
    def add_vex(self, pos):
        try:
            idx = self.vex2idx[pos]
            self.parent[idx]=-1
        except:
            idx = len(self.vertices)
            self.vertices.append(pos)
            self.vex2idx[pos] = idx
            self.neighbors[idx] = []
            self.parent[idx]=-1
        return idx

    def add_edge(self, idx1, idx2, cost):
        self.edges.append((idx1, idx2))
        self.neighbors[idx1].append((idx2, cost))
        self.neighbors[idx2].append((idx1, cost))
        self.parent[idx1]=idx2

    def randomPosition(self):
        rx = random()
        ry = random()

        posx = self.startpos[0] - (self.sx / 2.) + rx * self.sx * 2
        posy = self.startpos[1] - (self.sy / 2.) + ry * self.sy * 2
        return posx, posy
def Parent(G,vex):
    vexidx=G.vex2idx[vex]
    parentidx=G.parent[vexidx]
    if parentidx==-1:
        return startpos
    else:
        return G.vertices[parentidx]
def FindReachest(G,nearvex,newvex):
    reachest=nearvex
    while reachest!=startpos:
        parent=Parent(G,reachest)
        line=Line(newvex,parent)
        if not isThruObstacle(line,obstacles,radius):
            reachest=parent
        else:
            return  reachest
    return reachest
def CreateNode(G,reachestvex,newvex,dichotomy):
    allow=reachestvex
    if reachestvex!=startpos:
        forbid=Parent(G,reachestvex)
        while distance(allow,forbid)>dichotomy:
            mid=((allow[0]+forbid[0])/2,(allow[1]+forbid[1])/2) # 일단 이 부분은 다시 수정
            line=Line(newvex,mid)
            if not isInObstacle(mid,obstacles,radius) and not isThruObstacle(line,obstacles,radius):
                allow=mid
            else:
                forbid=mid
        forbid=newvex
        while distance(allow,forbid)>dichotomy:
            mid=((allow[0]+forbid[0])/2,(allow[1]+forbid[1])/2) #일단 이 부분은 다시 수정
            parent=Parent(G,reachestvex)
            line=Line(parent,mid)
            if not isInObstacle(mid,obstacles,radius) and not isThruObstacle(line,obstacles,radius):
                allow=mid
            else:
                forbid=mid
    if allow!=reachestvex:
        create=allow
    else:
        create=None
    return create
def farfrom(vex1,vex2):
    return math.sqrt(((vex1[0]-vex2[0])**2+(vex1[1]-vex2[1])**2))
def RRT(startpos, endpos, obstacles, n_iter, radius, stepSize):
    ''' RRT algorithm '''
    G = Graph(startpos, endpos)

    for _ in range(n_iter):
        randvex = G.randomPosition()
        if isInObstacle(randvex, obstacles, radius):
            continue

        nearvex, nearidx = nearest(G, randvex, obstacles, radius)
        if nearvex is None:
            continue

        newvex = newVertex(randvex, nearvex, stepSize)

        newidx = G.add_vex(newvex)
        dist = distance(newvex, nearvex)
        G.add_edge(newidx, nearidx, dist)

        dist = distance(newvex, G.endpos)
        if dist < 2 * radius:
            endidx = G.add_vex(G.endpos)
            G.add_edge(newidx, endidx, dist)
            G.success = True
            #print('success')
            # break
    return G


def RRT_star(startpos, endpos, obstacles, n_iter, radius, stepSize,dichotomy):
    ''' RRT star algorithm '''
    G = Graph(startpos, endpos)

    for i in range(n_iter):
        # plot(G, obstacles, radius)
        randvex = G.randomPosition()
        if isInObstacle(randvex, obstacles, radius):
            continue
        nearvex, nearidx = nearest(G, randvex, obstacles, radius)

        if nearvex is None:
            continue

        newvex = newVertex(randvex, nearvex, stepSize)
        if isInObstacle(newvex,obstacles,radius):
            continue
        reachestvex=FindReachest(G,nearvex,newvex)
        # print('reachestvex:',reachestvex)
        createvex=CreateNode(G,reachestvex,newvex,dichotomy)
        # print('createvex',createvex)
        if createvex is not None:
            newidx = G.add_vex(newvex)
            createvexidx=G.add_vex(createvex)
            reachestParent=Parent(G,reachestvex)
            reachestParentidx=G.vex2idx[reachestParent]
            dist = distance(reachestParent,createvex)
            G.add_edge(createvexidx,reachestParentidx,dist)
            G.distances[createvexidx]=G.distances[reachestParentidx]+dist
            dist = distance(newvex,createvex)
            G.add_edge(newidx,createvexidx,dist)
            G.distances[newidx]=G.distances[createvexidx]+dist
        else:
            line = Line(nearvex,newvex)
            if isThruObstacle(line, obstacles, radius):
                continue
            newidx = G.add_vex(newvex)
            dist = distance(newvex, reachestvex)
            reachestidx=G.vex2idx[reachestvex]
            G.add_edge(newidx, reachestidx, dist)
            G.distances[newidx] = G.distances[reachestidx] + dist
        # update nearby vertices distance (if shorter)
        for vex in G.vertices:
            if vex == newvex:
                continue

            dist = distance(vex, newvex)
            if dist > radius:
                continue

            line = Line(vex, newvex)
            if isThruObstacle(line, obstacles, radius):
                continue

            idx = G.vex2idx[vex]
            if G.distances[newidx] + dist < G.distances[idx]:
                G.add_edge(idx, newidx, dist)
                G.distances[idx] = G.distances[newidx] + dist

        dist = distance(newvex, G.endpos)
        if dist < 2 * radius:
            endidx = G.add_vex(G.endpos)
            G.add_edge(newidx, endidx, dist)
            try:
                G.distances[endidx] = min(G.distances[endidx], G.distances[newidx]+dist)
            except:
                G.distances[endidx] = G.distances[newidx]+dist

            G.success = True
            # print('success')
            break
    return G



def dijkstra(G):
    '''
    Dijkstra algorithm for finding shortest path from start position to end.
    '''
    srcIdx = G.vex2idx[G.startpos]
    dstIdx = G.vex2idx[G.endpos]

    # build dijkstra
    nodes = list(G.neighbors.keys())
    dist = {node: float('inf') for node in nodes}
    prev = {node: None for node in nodes}
    dist[srcIdx] = 0

    while nodes:
        curNode = min(nodes, key=lambda node: dist[node])
        nodes.remove(curNode)
        if dist[curNode] == float('inf'):
            break

        for neighbor, cost in G.neighbors[curNode]:
            newCost = dist[curNode] + cost
            if newCost < dist[neighbor]:
                dist[neighbor] = newCost
                prev[neighbor] = curNode

    # retrieve path
    path = deque()
    curNode = dstIdx
    while prev[curNode] is not None:
        path.appendleft(G.vertices[curNode])
        curNode = prev[curNode]
    path.appendleft(G.vertices[curNode])
    return list(path)



def plot(G, obstacles, radius, path=None):
    '''
    Plot RRT, obstacles and shortest path
    '''
    px = [x for x, y in G.vertices]
    py = [y for x, y in G.vertices]
    fig, ax = plt.subplots()

    for obs in obstacles:
        circle = plt.Circle(obs, radius, color='red')
        ax.add_artist(circle)

    ax.scatter(px, py, c='cyan')
    ax.scatter(G.startpos[0], G.startpos[1], c='black')
    ax.scatter(G.endpos[0], G.endpos[1], c='black')

    lines = [(G.vertices[edge[0]], G.vertices[edge[1]]) for edge in G.edges]
    lc = mc.LineCollection(lines, colors='green', linewidths=2)
    ax.add_collection(lc)

    if path is not None:
        paths = [(path[i], path[i+1]) for i in range(len(path)-1)]
        lc2 = mc.LineCollection(paths, colors='blue', linewidths=3)
        ax.add_collection(lc2)

    ax.autoscale()
    ax.margins(0.1)
    plt.show()


def pathSearch(startpos, endpos, obstacles, n_iter, radius, stepSize,dichotomy):
    G = RRT_star(startpos, endpos, obstacles, n_iter, radius, stepSize,dichotomy)
    if G.success:
        path = dijkstra(G)
        # plot(G, obstacles, radius, path)
        return path


if __name__ == '__main__':
    startpos = (0., 0.)
    endpos = (48., 28.)

    obstacles = [(1., 1.), (2., 2.)]

    n_iter = 200
    radius = 0.5
    stepSize = 0.7
    dichotomy= 0.1
    S_time=[]
    S_dist=[]
    for _ in range(100):
        start = time.time()
        G = RRT_star(startpos, endpos, obstacles, n_iter, radius, stepSize,dichotomy)
        # G = RRT(startpos, endpos, obstacles, n_iter, radius, stepSize)

        if G.success:
            path = dijkstra(G)
            print(path)
            plot(G, obstacles, radius, path)
            end = time.time()
            S_time.append(end-start)
            # print(f"{end - start:.5f} sec")
            dist=0
            prevx=startpos[0]
            prevy=startpos[1]
            for x,y in path:
                x_diff=prevx-x
                y_diff=prevy-y
                dist+= math.sqrt(x_diff**2+y_diff**2)
            S_dist.append(dist)
        else:
            pass
            print('yo')
            plot(G, obstacles, radius)
    print('Cost Mean=',np.mean(S_dist))
    print('Cost std=',np.std(S_dist))
    print('Cost Min=', np.min(S_dist))
    print('Cost Max=', np.max(S_dist))
    print('Time Mean=',np.mean(S_time))
    print('Time std=',np.std(S_time))
    print('Time Min=', np.min(S_time))
    print('Time Max=', np.max(S_time))
# Cost Mean= 19.95158009760057
# Cost std= 2.2203056859587984
# Cost Min= 16.286848376528287
# Cost Max= 26.856049490040363
# Time Mean= 0.12551307439804077
# Time std= 0.1737030710498345
# Time Min= 0.019977331161499023
# Time Max= 0.965425968170166