forked from BinghengNUS/LearningAgileFlight_SE3
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathsolid_geometry.py
168 lines (141 loc) · 7.35 KB
/
solid_geometry.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
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
# solid geometry
# this file is to do some calculation of solid geometry to do the collision detection of quadrotor
# this file consists of several classes
import numpy as np
## return the maginitude of a vector
def magni(vector):
return np.sqrt(np.dot(np.array(vector),np.array(vector)))
## return the unit vector of a vector
def norm(vector):
return np.array(vector)/magni(np.array(vector))
## define a class of a plane (using three points on the plane)
class plane():
def __init__(self, point1, point2, point3):
self.point1 = np.array(point1)
self.point2 = np.array(point2)
self.point3 = np.array(point3)
self.vec1 = self.point2 - self.point1
self.vec2 = self.point3 - self.point1
self.normal = norm(np.cross(self.vec2,self.vec1))
# normal vector of the plane
def nor_vec(self, ):
return self.normal
# normal vector of one side
def n1(self):
return norm(np.cross(self.vec1,self.normal))
# normal vector of one side
def n2(self):
return norm(np.cross(self.normal,self.vec2))
# normal vector of one side
def n3(self):
self.vec3 = self.point3 - self.point2
return norm(np.cross(self.normal,self.vec3))
## intersection with another line
def interpoint(self, point1, point2):
dir = norm(np.array(point1)-np.array(point2))
t = 1/(np.dot(dir,self.normal))*(np.dot(self.normal,np.array(point1)-self.point1))
point = np.array(point1) - t * dir
return point
## define a class of a line
class line():
def __init__(self, point1, point2):
self.point1 = np.array(point1)
self.point2 = np.array(point2)
self.dir = norm(self.point1 - self.point2)
## return the distance from a point to the line
def vertical(self, point):
point3 = np.array(point)
normal = np.cross(point3 - self.point1, self.dir)
return magni(normal)
## return the distance from a point to the line section
def distance(self,point):
a = self.vertical(point)
b = magni(point-self.point1)
c = magni(point-self.point2)
d = magni(self.point1-self.point2)
if(b>c):
if((b**2-d**2)>a**2):
dis = c
else:
dis = a
else:
if((c**2-d**2)>a**2):
dis = b
else:
dis = a
return dis
## define the narrow window which is also the obstacle for the quadrotor
class obstacle():
def __init__(self, point1, point2, point3, point4):
self.point1 = np.array(point1)
self.point2 = np.array(point2)
self.point3 = np.array(point3)
self.point4 = np.array(point4)
#define the centroid of obstacle
self.centroid = np.array([(point1[0]+point2[0]+point3[0]+point4[0])/4,\
(point1[1]+point2[1]+point3[1]+point4[1])/4,(point1[2]+point2[2]+point3[2]+point4[2])/4])
#define the cross section
self.plane1 = plane(self.centroid,point1,point2)
self.plane2 = plane(self.centroid,point2,point3)
self.plane3 = plane(self.centroid,point3,point4)
self.plane4 = plane(self.centroid,point4,point1)
#define the bound
self.line1 = line(point1, point2)
self.line2 = line(point2, point3)
self.line3 = line(point3, point4)
self.line4 = line(point4, point1)
def collis_det(self, vert_traj, horizon):
## define the state whether find corresponding plane
collision = 0
self.co = 0
## judge if the trajectory traverse through the plane
if((np.dot(self.plane1.nor_vec(),vert_traj[0]-self.centroid)<0)):
return 0
## judge whether the first plane is the traversal plane
# find two points of traverse
d_min = 0.2
for t in range(horizon):
if(np.dot(self.plane1.nor_vec(),vert_traj[t]-self.centroid)<0):
intersect = self.plane1.interpoint(vert_traj[t],vert_traj[t-1])
# judge whether they belong to plane1 and calculate the distance
if(np.dot(self.plane1.n1(),intersect-self.centroid)>0 and np.dot(self.plane1.n2(),intersect-self.centroid)>0):
if(np.dot(self.point1-intersect,self.plane1.n3())>0):
m = min(self.line1.vertical(intersect),self.line2.vertical(intersect),\
self.line3.vertical(intersect),self.line4.vertical(intersect))
collision = - max(0,d_min-m)**2
self.co = 1
else:
m = min(self.line4.distance(intersect),self.line1.distance(intersect),self.line2.distance(intersect))
collision = - 2*d_min*m - d_min**2
# judge whether the intersection belongs to plane2 and calculate the distance
if(np.inner(self.plane2.n1(),intersect-self.centroid)>0 and np.inner(self.plane2.n2(),intersect-self.centroid)>0):
if(np.dot(self.point2-intersect,self.plane2.n3())>0):
m = min(self.line1.vertical(intersect),self.line2.vertical(intersect),\
self.line3.vertical(intersect),self.line4.vertical(intersect))
collision = - max(0,d_min-m)**2
self.co = 1
else:
m = min(self.line1.distance(intersect),self.line2.distance(intersect),self.line3.distance(intersect))
collision = - 2*d_min*m - d_min**2
# judge whether the intersection belongs to plane3 and calculate the distance
if(np.inner(self.plane3.n1(),intersect-self.centroid)>0 and np.inner(self.plane3.n2(),intersect-self.centroid)>0):
if(np.dot(self.point3-intersect,self.plane3.n3())>0):
m = min(self.line1.vertical(intersect),self.line2.vertical(intersect),\
self.line3.vertical(intersect),self.line4.vertical(intersect))
collision = - max(0,d_min-m)**2
self.co = 1
else:
m = min(self.line2.distance(intersect),self.line3.distance(intersect),self.line4.distance(intersect))
collision = - 2*d_min*m - d_min**2
# judge whether the intersection belongs to plane4 and calculate the distance
if(np.inner(self.plane4.n1(),intersect-self.centroid)>0 and np.inner(self.plane4.n2(),intersect-self.centroid)>0):
if(np.dot(self.point4-intersect,self.plane4.n3())>0):
m = min(self.line1.vertical(intersect),self.line2.vertical(intersect),\
self.line3.vertical(intersect),self.line4.vertical(intersect))
collision = - max(0,d_min-m)**2
self.co = 1
else:
m = min(self.line3.distance(intersect),self.line4.distance(intersect),self.line1.distance(intersect))
collision = - 2*d_min*m - d_min**2
break
return collision