mirror of
https://github.com/zhm-real/PathPlanning.git
synced 2026-08-30 00:50:46 +08:00
zhm-real
129 lines
3.6 KiB
Python
129 lines
3.6 KiB
Python
"""
|
|
utils for collision check
|
|
@author: huiming zhou
|
|
"""
|
|
|
|
import math
|
|
import numpy as np
|
|
import os
|
|
import sys
|
|
|
|
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
|
|
"/../../Sampling-based Planning/")
|
|
|
|
from rrt_2D import env
|
|
from rrt_2D.rrt import Node
|
|
|
|
|
|
class Utils:
|
|
def __init__(self):
|
|
self.env = env.Env()
|
|
|
|
self.delta = 0.5
|
|
self.obs_circle = self.env.obs_circle
|
|
self.obs_rectangle = self.env.obs_rectangle
|
|
self.obs_boundary = self.env.obs_boundary
|
|
self.obs_vertex = self.get_obs_vertex()
|
|
|
|
def get_obs_vertex(self):
|
|
delta = self.delta
|
|
obs_list = []
|
|
|
|
for (ox, oy, w, h) in self.obs_rectangle:
|
|
vertex_list = [[ox - delta, oy - delta],
|
|
[ox + w + delta, oy - delta],
|
|
[ox + w + delta, oy + h + delta],
|
|
[ox - delta, oy + h + delta]]
|
|
obs_list.append(vertex_list)
|
|
|
|
return obs_list
|
|
|
|
def is_intersect_rec(self, start, end, o, d, a, b):
|
|
v1 = [o[0] - a[0], o[1] - a[1]]
|
|
v2 = [b[0] - a[0], b[1] - a[1]]
|
|
v3 = [-d[1], d[0]]
|
|
|
|
div = np.dot(v2, v3)
|
|
|
|
if div == 0:
|
|
return False
|
|
|
|
t1 = np.linalg.norm(np.cross(v2, v1)) / div
|
|
t2 = np.dot(v1, v3) / div
|
|
|
|
if t1 >= 0 and 0 <= t2 <= 1:
|
|
shot = Node((o[0] + t1 * d[0], o[1] + t1 * d[1]))
|
|
dist_obs = self.get_dist(start, shot)
|
|
dist_seg = self.get_dist(start, end)
|
|
if dist_obs <= dist_seg:
|
|
return True
|
|
|
|
return False
|
|
|
|
def is_intersect_circle(self, o, d, a, r):
|
|
d2 = np.dot(d, d)
|
|
delta = self.delta
|
|
|
|
if d2 == 0:
|
|
return False
|
|
|
|
t = np.dot([a[0] - o[0], a[1] - o[1]], d) / d2
|
|
|
|
if 0 <= t <= 1:
|
|
shot = Node((o[0] + t * d[0], o[1] + t * d[1]))
|
|
if self.get_dist(shot, Node(a)) <= r + delta:
|
|
return True
|
|
|
|
return False
|
|
|
|
def is_collision(self, start, end):
|
|
if self.is_inside_obs(start) or self.is_inside_obs(end):
|
|
return True
|
|
|
|
o, d = self.get_ray(start, end)
|
|
|
|
for (v1, v2, v3, v4) in self.obs_vertex:
|
|
if self.is_intersect_rec(start, end, o, d, v1, v2):
|
|
return True
|
|
if self.is_intersect_rec(start, end, o, d, v2, v3):
|
|
return True
|
|
if self.is_intersect_rec(start, end, o, d, v3, v4):
|
|
return True
|
|
if self.is_intersect_rec(start, end, o, d, v4, v1):
|
|
return True
|
|
|
|
for (x, y, r) in self.obs_circle:
|
|
if self.is_intersect_circle(o, d, [x, y], r):
|
|
return True
|
|
|
|
return False
|
|
|
|
def is_inside_obs(self, node):
|
|
delta = self.delta
|
|
|
|
for (x, y, r) in self.obs_circle:
|
|
if math.hypot(node.x - x, node.y - y) <= r + delta:
|
|
return True
|
|
|
|
for (x, y, w, h) in self.obs_rectangle:
|
|
if 0 <= node.x - (x - delta) <= w + 2 * delta \
|
|
and 0 <= node.y - (y - delta) <= h + 2 * delta:
|
|
return True
|
|
|
|
for (x, y, w, h) in self.obs_boundary:
|
|
if 0 <= node.x - (x - delta) <= w + 2 * delta \
|
|
and 0 <= node.y - (y - delta) <= h + 2 * delta:
|
|
return True
|
|
|
|
return False
|
|
|
|
@staticmethod
|
|
def get_ray(start, end):
|
|
orig = [start.x, start.y]
|
|
direc = [end.x - start.x, end.y - start.y]
|
|
return orig, direc
|
|
|
|
@staticmethod
|
|
def get_dist(start, end):
|
|
return math.hypot(end.x - start.x, end.y - start.y)
|