-
Notifications
You must be signed in to change notification settings - Fork 2
/
obstacle.py
257 lines (226 loc) · 11.4 KB
/
obstacle.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
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
#-------------------------------------------------------------------------------
#
# obstacle.py -- new flock experiments
#
# Obstacle base class, some specializations, and utilities.
#
# An Obstacle is a type of geometry which Boids avoid.
#
# MIT License -- Copyright © 2023 Craig Reynolds
#
#-------------------------------------------------------------------------------
import math
import shape
from Vec3 import Vec3
from Draw import Draw
import Utilities as util
class Obstacle:
def __init__(self):
self.tri_mesh = None
# Seems hackish, related to Draw.temp_camera_lookat and the way Open3D
# does translation relative to center of geometry.
self.original_center = Vec3()
# Where a ray (Agent's path) will intersect the obstacle, or None.
def ray_intersection(self, origin, tangent, body_radius):
pass
#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
# TODO 20240127 make "agent_position" non-optional.
# # Normal to the obstacle at a given point of interest.
# def normal_at_poi(self, poi, agent_position=None):
# pass
# Normal to the obstacle at a given point of interest.
def normal_at_poi(self, poi, agent_position):
pass
#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
# Point on surface of obstacle nearest the given query_point
def nearest_point(self, query_point):
pass
# Compute direction for agent's static avoidance of nearby obstacles.
def fly_away(self, agent_position, agent_forward, max_distance, body_radius):
pass
# Signed distance function.
# (From a query point to the nearest point on Obstacle's surface: negative
# inside, positive outside, zero at surface. Very similar to nearest_point(),
# maybe they can be combined?)
def signed_distance(self, query_point):
return 0
def draw(self):
pass
def __str__(self):
return self.__class__.__name__
class EvertedSphereObstacle(Obstacle):
def __init__(self, radius, center):
Obstacle.__init__(self)
self.radius = radius
self.center = center
# Where a ray (Agent's path) will intersect the obstacle, or None.
def ray_intersection(self, origin, tangent, body_radius):
return shape.ray_sphere_intersection(origin, tangent,
self.radius, self.center)
#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
# TODO 20240127 make "agent_position" non-optional.
# Normal to the obstacle at a given point of interest.
def normal_at_poi(self, poi, agent_position):
return (self.center - poi).normalize()
#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
# Point on surface of obstacle nearest the given query_point
def nearest_point(self, query_point):
return (query_point - self.center).normalize() * self.radius
# Compute direction for agent's static avoidance of nearby obstacles.
def fly_away(self, agent_position, agent_forward, max_distance, body_radius):
avoidance = Vec3()
p = agent_position
r = self.radius
c = self.center
offset_to_sphere_center = c - p
distance_to_sphere_center = offset_to_sphere_center.length()
dist_from_wall = r - distance_to_sphere_center
# Close enough to obstacle surface to use static replusion.
if dist_from_wall < max_distance:
normal = offset_to_sphere_center / distance_to_sphere_center
# Unless agent is already facing away from obstacle.
if normal.dot(agent_forward) < 0.9:
# Weighting falls off further from obstacle surface
weight = 1 - (dist_from_wall / max_distance)
avoidance = normal * weight
return avoidance
# Signed distance function. (From a query point to the nearest point on
# Obstacle's surface: negative inside, positive outside, zero at surface.)
def signed_distance(self, query_point):
distance_to_center = (query_point - self.center).length()
return distance_to_center - self.radius
def draw(self):
if not self.tri_mesh:
self.tri_mesh = Draw.make_everted_sphere(self.radius, self.center)
Draw.adjust_static_scene_object(self.tri_mesh)
class PlaneObstacle(Obstacle):
def __init__(self, normal=Vec3(0, 1, 0), center=Vec3(0, 0, 0)):
Obstacle.__init__(self)
self.normal = normal
self.center = center
# Where a ray (Agent's path) will intersect the obstacle, or None.
def ray_intersection(self, origin, tangent, body_radius):
return shape.ray_plane_intersection(origin, tangent,
self.center, self.normal)
# Normal to the obstacle at a given point of interest.
def normal_at_poi(self, poi, agent_position=None):
normal = self.normal
# If a reference position is given.
if agent_position:
# Project it to obstacle surface.
on_obstacle = self.nearest_point(agent_position)
# Normalized vector FROM obstacle surface TOWARD agent.
normal = (agent_position - on_obstacle).normalize()
return normal
# Point on surface of obstacle nearest the given query_point
def nearest_point(self, query_point):
# Offset from center point (origin) of plane.
offset = query_point - self.center
# Signed distance from plane.
distance = offset.dot(self.normal)
# Translate offset point onto plane (in plane's local space).
on_plane = offset - (self.normal * distance)
# Translate back to global space.
return on_plane + self.center
# Compute direction for agent's static avoidance of nearby obstacles.
def fly_away(self, agent_position, agent_forward, max_distance, body_radius):
avoidance = Vec3()
# Project agent_position to obstacle surface.
on_obstacle = self.nearest_point(agent_position)
dist_from_obstacle = (on_obstacle - agent_position).length()
# Close enough to obstacle surface to use static replusion.
if dist_from_obstacle < max_distance:
normal = self.normal_at_poi(on_obstacle, agent_position)
# Unless agent is already facing away from obstacle.
if normal.dot(agent_forward) < 0.9:
# Weighting falls off further from obstacle surface
weight = 1 - (dist_from_obstacle / max_distance)
avoidance = normal * weight
return avoidance
# Signed distance function. (From a query point to the nearest point on
# Obstacle's surface: negative inside, positive outside, zero at surface.)
def signed_distance(self, query_point):
nearest_point_on_plane = self.nearest_point(query_point)
from_plane_to_query_point = query_point - nearest_point_on_plane
return from_plane_to_query_point.dot(self.normal)
# A bounded cylinder (between two endpoints) with given radius
class CylinderObstacle(Obstacle):
def __init__(self, radius, endpoint0, endpoint1):
Obstacle.__init__(self)
self.radius = radius
self.endpoint = endpoint0
offset = endpoint1 - endpoint0
(self.tangent, self.length) = offset.normalize_and_length()
# Nearest point on the infinite line containing cylinder's axis.
def nearest_point_on_axis(self, query_point):
offset = query_point - self.endpoint
projection = offset.dot(self.tangent)
return self.endpoint + self.tangent * projection
# Where a ray (Agent's path) will intersect the obstacle, or None.
def ray_intersection(self, origin, tangent, body_radius):
return shape.ray_cylinder_intersection(origin, tangent,
self.endpoint, self.tangent,
self.radius + 2 * body_radius,
self.length)
#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
# TODO 20240127 make "agent_position" non-optional.
# Normal to the obstacle at a given point of interest.
def normal_at_poi(self, poi, agent_position):
on_axis = self.nearest_point_on_axis(poi)
return (poi - on_axis).normalize()
#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
# Point on surface of obstacle nearest the given query_point
def nearest_point(self, query_point):
on_axis = self.nearest_point_on_axis(query_point)
return on_axis + ((query_point - on_axis).normalize() * self.radius)
# Compute direction for agent's static avoidance of nearby obstacles.
def fly_away(self, agent_position, agent_forward, max_distance, body_radius):
avoidance = Vec3()
# Distance between this cylinder's axis and the agent's current path.
path_to_axis_dist = shape.distance_between_lines(agent_position,
agent_forward,
self.endpoint,
self.tangent)
# When too close, avoidance is unit normal to cylinder surface.
margin = 3 * body_radius
if path_to_axis_dist < self.radius + margin:
on_surface = self.nearest_point(agent_position)
if (on_surface - agent_position).length_squared() < margin ** 2:
#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
# TODO 20240127 make "agent_position" non-optional.
# avoidance = self.normal_at_poi(agent_position)
avoidance = self.normal_at_poi(agent_position, agent_position)
#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
return avoidance
# Signed distance function. (From a query point to the nearest point on
# Obstacle's surface: negative inside, positive outside, zero at surface.)
def signed_distance(self, query_point):
point_on_axis = self.nearest_point_on_axis(query_point)
distance_to_axis = (query_point - point_on_axis).length()
return distance_to_axis - self.radius
def draw(self):
if not self.tri_mesh:
self.tri_mesh = Draw.new_empty_tri_mesh()
Draw.add_line_segment(self.endpoint,
self.endpoint + (self.tangent * self.length),
color = Vec3(1, 1, 1) * 0.8,
radius = self.radius,
sides = 50,
tri_mesh = self.tri_mesh,
flat_end_caps=True)
self.tri_mesh.compute_vertex_normals()
self.original_center = Vec3.from_array(self.tri_mesh.get_center())
Draw.adjust_static_scene_object(self.tri_mesh, self.original_center)
# Class to contain statistics of a predicted collision with an Obstacle.
class Collision:
def __init__(self,
obstacle,
time_to_collision,
dist_to_collision,
point_of_impact,
normal_at_poi):
self.obstacle = obstacle
self.time_to_collision = time_to_collision
self.dist_to_collision = dist_to_collision
self.point_of_impact = point_of_impact
self.normal_at_poi = normal_at_poi