Delete old unused code

guided_mrmp/planners/CollisionDetection.py

-from shapely.geometry import Polygon, Point
-import matplotlib.pyplot as plt 
-import random
-import numpy as np
-import sys
-
-def shapes_collide(shape_1, shape_2):
-    """
-    Determine if two circles, two polygons, or a circle and a polygon intersect.
-    True if they do intersect, else false
-    inputs:
-        - shape_1 (Shapely obj): the first shape
-        - shape_2 (shapely obj): the second shape
-    output:
-        - bool
-    """
-    return shape_1.intersects(shape_2)
-
-def break_edge_into_segments(node1, node2,edge_dir, delta):
-    edge_segments = []
-    p = node1
-    while np.linalg.norm(p - node1) < np.linalg.norm(node2 - node1):
-        p = p + delta*edge_dir
-        edge_segments.append(p)
-
-    return edge_segments
-
-def paths_collide(path_1, path_2, delta=.3):
-    """
-    Determine if two paths collide with each other. 
-    Return true if a collision is detected.
-    inputs:
-        - path_1 (list): the first path
-        - path_2 (list): the second path
-        - delta (float): the amount of an edge that is checked at a time
-    """
-    min_len = min(len(path_1), len(path_2))
-    history = []
-
-    for i in range(min_len-1):
-        path1_node1 = np.array([path_1[i][0], path_1[i][1]])
-        path1_node2 = np.array([path_1[i+1][0], path_1[i+1][1]])
-        path2_node1 = np.array([path_2[i][0], path_2[i][1]])
-        path2_node2 = np.array([path_2[i+1][0], path_2[i+1][1]])
-
-        edge1 = path1_node2 - path1_node1
-        edge2 = path2_node2 - path2_node1
-        edge1_dir = edge1 / np.linalg.norm(edge1)
-        edge2_dir = edge2 / np.linalg.norm(edge2)
-
-        edge1_segments = break_edge_into_segments(path1_node1,path1_node2, edge1_dir, delta)
-        edge2_segments = break_edge_into_segments(path2_node1,path2_node2, edge2_dir, delta)
-
-        num_segs = min(len(edge1_segments), len(edge2_segments))
-        edge1_segments = edge1_segments[0:num_segs]
-        edge2_segments = edge2_segments[0:num_segs]
-
-        for p1,p2 in zip(edge1_segments, edge2_segments):
-            circ1 = Point(p1[0],p1[1])
-            circ1 = circ1.buffer(.1)
-            circ2 = Point(p2[0],p2[1])
-            circ2 = circ2.buffer(.1)
-
-            history.append([circ1,circ2])
-
-            if circ1.intersects(circ2): 
-                return True, history
-
-    return False, history
-    
-def draw_path(ax, path, color):
-    for i in range(len(path)-1):
-        this_node = path[i]
-        next_node = path[i+1]
-
-        ax.plot([this_node[0], next_node[0]], [this_node[1], next_node[1]], '-o', color=color)
-
-def draw_edge_collision_check(ax, hist):
-    for circs in hist:
-        circ1 = circs[0]
-        circ2 = circs[1]
-        print(circ1)
-        ax.plot(*circ1.exterior.xy, color="red")
-        ax.plot(*circ2.exterior.xy, color="red")
-
-
-if __name__ == "__main__":
-
-    path1 = [[1,1], [2,2], [3,3], [4,4]]
-    path2 = [[.7,1.5], [2,1.5], [3,2.5]]
-
-    collide, hist = paths_collide(path1, path2)
-    print(collide)
-
-    fig, ax = plt.subplots()
-
-    draw_path(ax,path1,"tab:blue")
-    draw_path(ax,path2,"tab:green")
-
-    draw_edge_collision_check(ax, hist)
-
-    plt.show()
-
-
-    # coords = ((0., 0.), (0., 1.), (1., 1.), (1., 0.), (0., 0.))
-    # polygon = Polygon(coords)
-    # print(polygon.area)
-    # plt.plot(*polygon.exterior.xy)
-    # plt.show()
-
-    # p1 = Polygon([(0,0), (1,1), (1,0), (0,0)])
-    # p2 = Polygon([(0,1), (1,0), (1,1), (0,1)])
-    # plt.plot(*p1.exterior.xy)
-    # plt.plot(*p2.exterior.xy)
-    # print(p1.intersects(p2))
-    # plt.show()
-
-    # circles = []
-    # center2 = Point(2, 2)
-    # circles.append(center2.buffer(5))
-    # center2 = Point(1, 1)
-    # circles.append(center2.buffer(5))
-    # for circ in circles:
-    #     plt.plot(*circ.exterior.xy)
-
-    # plt.show()
-
-    # print(circles[0].intersects(p1))
\ No newline at end of file