From 9b1f77ff3356fb90d02e4ca350e1bdeb9db54ee3 Mon Sep 17 00:00:00 2001 From: rachelmoan <moanrachel516@gmail.com> Date: Sun, 5 Jan 2025 14:06:36 -0600 Subject: [PATCH] Delete old unused code --- guided_mrmp/planners/CollisionDetection.py | 128 --------------------- 1 file changed, 128 deletions(-) delete mode 100644 guided_mrmp/planners/CollisionDetection.py diff --git a/guided_mrmp/planners/CollisionDetection.py b/guided_mrmp/planners/CollisionDetection.py deleted file mode 100644 index 1285bae..0000000 --- a/guided_mrmp/planners/CollisionDetection.py +++ /dev/null @@ -1,128 +0,0 @@ -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 -- GitLab