-
Notifications
You must be signed in to change notification settings - Fork 0
/
utils.py
126 lines (94 loc) · 3.64 KB
/
utils.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
import math
import numpy as np
import os
import sys
from node import Node
import env
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
def update_obs(self, obs_cir, obs_bound, obs_rec):
self.obs_circle = obs_cir
self.obs_boundary = obs_bound
self.obs_rectangle = obs_rec
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)
obs_vertex = self.get_obs_vertex()
for (v1, v2, v3, v4) in 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)