Skip to content

Commit cd3fc5b

Browse files
micah-huthmicah-huth
authored
Merge pull request petercorke#125 from petercorke/micah-dev
Grid scaling mechanics
2 parents 2cd925f + 0f52b51 commit cd3fc5b

File tree

3 files changed

+230
-90
lines changed

3 files changed

+230
-90
lines changed

‎graphics/graphics_canvas.py

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -336,6 +336,9 @@ def __handle_keyboard_inputs(self):
336336
def __reload_caption(self, new_list):
337337
"""
338338
Reload the UI with the new list of robot names
339+
340+
:param new_list: The new list to apply to the menu
341+
:type new_list: `list`
339342
"""
340343
# Remove all UI elements
341344
for item in self.__ui_controls:
@@ -351,7 +354,10 @@ def __reload_caption(self, new_list):
351354

352355
def __load_mode_ui(self, new_list):
353356
"""
357+
Load the UI menu depending on the current mode
354358
359+
:param new_list: The new list to apply to the menu
360+
:type new_list: `list`
355361
"""
356362
if self.__ui_mode == UImode.CANVASCONTROL:
357363
self.__ui_controls = self.__setup_ui_controls(new_list)

‎graphics/graphics_grid.py

Lines changed: 128 additions & 51 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
from vpython import vector, compound, mag, box
2-
from numpy import sign, ceil
2+
from numpy import sign, ceil, arange
33
from graphics.graphics_text import update_grid_numbers
44
from graphics.graphics_object2d import Marker2D
55
from spatialmath import SE2
@@ -21,6 +21,7 @@ def __init__(self, scene):
2121

2222
self.__relative_cam = True
2323
self.__num_squares = 10
24+
self.__scale = 1
2425

2526
# Save the current camera settings
2627
self.camera_pos = self.__scene.camera.pos
@@ -56,7 +57,8 @@ def __init_grid(self):
5657
self.grid_object[self.__planes_idx] = the_grid
5758

5859
# Update the labels instead of recreating them
59-
update_grid_numbers(self.__focal_point, self.grid_object[self.__labels_idx], self.__num_squares, self.__scene)
60+
update_grid_numbers(self.__focal_point, self.grid_object[self.__labels_idx],
61+
self.__num_squares, self.__scale, self.__is_3d, self.__scene)
6062

6163
def __create_grid_objects(self):
6264
"""
@@ -73,10 +75,17 @@ def __create_grid_objects(self):
7375
camera_axes = self.camera_axes
7476
# Locate centre of axes
7577
if self.__relative_cam:
76-
x_origin, y_origin, z_origin = round(self.__scene.center.x),\
77-
round(self.__scene.center.y),\
78-
round(self.__scene.center.z)
78+
x_origin, y_origin, z_origin = round(self.__scene.center.x, 2), \
79+
round(self.__scene.center.y, 2), \
80+
round(self.__scene.center.z, 2)
7981
self.__focal_point = [x_origin, y_origin, z_origin]
82+
# Convert focal point for 2D rendering. Puts focus point in centre of the view
83+
if not self.__is_3d:
84+
self.__focal_point = [val - int(self.__num_squares / 2) for val in self.__focal_point]
85+
x_origin = self.__focal_point[0]
86+
y_origin = self.__focal_point[1]
87+
z_origin = 0
88+
self.__focal_point[2] = z_origin
8089
else:
8190
x_origin, y_origin, z_origin = self.__focal_point[0], \
8291
self.__focal_point[1], \
@@ -96,67 +105,104 @@ def __create_grid_objects(self):
96105
# min = -num_squares or 0, around the default position
97106
# max = +num_squares or 0, around the default position
98107
# e.g. at the origin, for negative axes: -10 -> 0, positive axes: 0 -> 10
99-
min_x_coord = x_origin + int(-(self.__num_squares / 2) + (sign(camera_axes.x) * -1) * (self.__num_squares / 2))
100-
max_x_coord = x_origin + int((self.__num_squares / 2) + (sign(camera_axes.x) * -1) * (self.__num_squares / 2))
108+
min_x_coord = round(x_origin + (-(self.__num_squares / 2) +
109+
(sign(camera_axes.x) * -1) * (self.__num_squares / 2)) * self.__scale, 2)
110+
max_x_coord = round(x_origin + ((self.__num_squares / 2) +
111+
(sign(camera_axes.x) * -1) * (self.__num_squares / 2)) * self.__scale, 2)
112+
113+
min_y_coord = round(y_origin + (-(self.__num_squares / 2) +
114+
(sign(camera_axes.y) * -1) * (self.__num_squares / 2)) * self.__scale, 2)
115+
max_y_coord = round(y_origin + ((self.__num_squares / 2) +
116+
(sign(camera_axes.y) * -1) * (self.__num_squares / 2)) * self.__scale, 2)
117+
118+
min_z_coord = round(z_origin + (-(self.__num_squares / 2) +
119+
(sign(camera_axes.z) * -1) * (self.__num_squares / 2)) * self.__scale, 2)
120+
max_z_coord = round(z_origin + ((self.__num_squares / 2) +
121+
(sign(camera_axes.z) * -1) * (self.__num_squares / 2)) * self.__scale, 2)
101122

102-
min_y_coord = y_origin + int(-(self.__num_squares / 2) + (sign(camera_axes.y) * -1) * (self.__num_squares / 2))
103-
max_y_coord = y_origin + int((self.__num_squares / 2) + (sign(camera_axes.y) * -1) * (self.__num_squares / 2))
123+
x_coords = arange(min_x_coord, max_x_coord + self.__scale, self.__scale)
124+
y_coords = arange(min_y_coord, max_y_coord + self.__scale, self.__scale)
125+
z_coords = arange(min_z_coord, max_z_coord + self.__scale, self.__scale)
104126

105-
min_z_coord = z_origin + int(-(self.__num_squares / 2) + (sign(camera_axes.z) * -1) * (self.__num_squares / 2))
106-
max_z_coord = z_origin + int((self.__num_squares / 2) + (sign(camera_axes.z) * -1) * (self.__num_squares / 2))
127+
# Compound origins are in the middle of the bounding boxes. Thus new pos will be between max and min.
128+
x_middle = (max_x_coord + min_x_coord) / 2
129+
y_middle = (max_y_coord + min_y_coord) / 2
130+
z_middle = (max_z_coord + min_z_coord) / 2
131+
132+
line_thickness = min(max(self.__scale / 25, 0.01), 5) # 0.01 -> 5
107133

108134
# XZ plane
109-
for x_point in range(min_x_coord, max_x_coord + 1):
135+
for x_point in x_coords:
110136
# Draw a line across for each x coord, along the same y-axis, from min to max z coord
111137
xz_lines.append(create_line(
112138
vector(x_point, y_origin, min_z_coord),
113139
vector(x_point, y_origin, max_z_coord),
114-
self.__scene
140+
self.__scene,
141+
thickness=line_thickness
115142
))
116-
for z_point in range(min_z_coord, max_z_coord + 1):
143+
for z_point in z_coords:
117144
# Draw a line across each z coord, along the same y-axis, from min to max z coord
118145
xz_lines.append(create_line(
119146
vector(min_x_coord, y_origin, z_point),
120147
vector(max_x_coord, y_origin, z_point),
121-
self.__scene
148+
self.__scene,
149+
thickness=line_thickness
122150
))
123151

124152
# XY plane
125-
for x_point in range(min_x_coord, max_x_coord + 1):
153+
for x_point in x_coords:
126154
# Draw a line across each x coord, along the same z-axis, from min to max y coord
127155
xy_lines.append(create_line(
128156
vector(x_point, min_y_coord, z_origin),
129157
vector(x_point, max_y_coord, z_origin),
130-
self.__scene
158+
self.__scene,
159+
thickness=line_thickness
131160
))
132-
for y_point in range(min_y_coord, max_y_coord + 1):
161+
for y_point in y_coords:
133162
# Draw a line across each y coord, along the same z-axis, from min to max x coord
134163
xy_lines.append(create_line(
135164
vector(min_x_coord, y_point, z_origin),
136165
vector(max_x_coord, y_point, z_origin),
137-
self.__scene
166+
self.__scene,
167+
thickness=line_thickness
138168
))
139169

140170
# YZ plane
141-
for y_point in range(min_y_coord, max_y_coord + 1):
171+
for y_point in y_coords:
142172
# Draw a line across each y coord, along the same x-axis, from min to max z coord
143173
yz_lines.append(create_line(
144174
vector(x_origin, y_point, min_z_coord),
145175
vector(x_origin, y_point, max_z_coord),
146-
self.__scene
176+
self.__scene,
177+
thickness=line_thickness
147178
))
148-
for z_point in range(min_z_coord, max_z_coord + 1):
179+
for z_point in z_coords:
149180
# Draw a line across each z coord, along the same x-axis, from min to max y coord
150181
yz_lines.append(create_line(
151182
vector(x_origin, min_y_coord, z_point),
152183
vector(x_origin, max_y_coord, z_point),
153-
self.__scene
184+
self.__scene,
185+
thickness=line_thickness
154186
))
155187

156188
# Compound the lines together into respective objects
157-
xz_plane = compound(xz_lines)
158-
xy_plane = compound(xy_lines)
159-
yz_plane = compound(yz_lines)
189+
# XY Plane
190+
if camera_axes.z < 0:
191+
xy_plane = compound(xy_lines, origin=vector(x_middle, y_middle, min_z_coord))
192+
else:
193+
xy_plane = compound(xy_lines, origin=vector(x_middle, y_middle, max_z_coord))
194+
195+
# XZ Plane
196+
if camera_axes.y < 0:
197+
xz_plane = compound(xz_lines, origin=vector(x_middle, min_y_coord, z_middle))
198+
else:
199+
xz_plane = compound(xz_lines, origin=vector(x_middle, max_y_coord, z_middle))
200+
201+
# YZ Plane
202+
if camera_axes.x < 0:
203+
yz_plane = compound(yz_lines, origin=vector(min_x_coord, y_middle, z_middle))
204+
else:
205+
yz_plane = compound(yz_lines, origin=vector(max_x_coord, y_middle, z_middle))
160206

161207
# Combine all into one list
162208
grid = [None, None, None]
@@ -173,9 +219,9 @@ def __move_grid_objects(self):
173219
camera_axes = self.camera_axes
174220
# Locate centre of axes
175221
if self.__relative_cam:
176-
x_origin, y_origin, z_origin = round(self.__scene.center.x), \
177-
round(self.__scene.center.y), \
178-
round(self.__scene.center.z)
222+
x_origin, y_origin, z_origin = round(self.__scene.center.x, 2), \
223+
round(self.__scene.center.y, 2), \
224+
round(self.__scene.center.z, 2)
179225
self.__focal_point = [x_origin, y_origin, z_origin]
180226
# Convert focal point for 2D rendering. Puts focus point in centre of the view
181227
if not self.__is_3d:
@@ -203,14 +249,20 @@ def __move_grid_objects(self):
203249
# min = -num_squares or 0, around the default position
204250
# max = +num_squares or 0, around the default position
205251
# e.g. at the origin, for negative axes: -10 -> 0, positive axes: 0 -> 10
206-
min_x_coord = x_origin + int(-(self.__num_squares / 2) + (sign(camera_axes.x) * -1) * (self.__num_squares / 2))
207-
max_x_coord = x_origin + int((self.__num_squares / 2) + (sign(camera_axes.x) * -1) * (self.__num_squares / 2))
252+
min_x_coord = round(x_origin + (-(self.__num_squares / 2) +
253+
(sign(camera_axes.x) * -1) * (self.__num_squares / 2)) * self.__scale, 2)
254+
max_x_coord = round(x_origin + ((self.__num_squares / 2) +
255+
(sign(camera_axes.x) * -1) * (self.__num_squares / 2)) * self.__scale, 2)
208256

209-
min_y_coord = y_origin + int(-(self.__num_squares / 2) + (sign(camera_axes.y) * -1) * (self.__num_squares / 2))
210-
max_y_coord = y_origin + int((self.__num_squares / 2) + (sign(camera_axes.y) * -1) * (self.__num_squares / 2))
257+
min_y_coord = round(y_origin + (-(self.__num_squares / 2) +
258+
(sign(camera_axes.y) * -1) * (self.__num_squares / 2)) * self.__scale, 2)
259+
max_y_coord = round(y_origin + ((self.__num_squares / 2) +
260+
(sign(camera_axes.y) * -1) * (self.__num_squares / 2)) * self.__scale, 2)
211261

212-
min_z_coord = z_origin + int(-(self.__num_squares / 2) + (sign(camera_axes.z) * -1) * (self.__num_squares / 2))
213-
max_z_coord = z_origin + int((self.__num_squares / 2) + (sign(camera_axes.z) * -1) * (self.__num_squares / 2))
262+
min_z_coord = round(z_origin + (-(self.__num_squares / 2) +
263+
(sign(camera_axes.z) * -1) * (self.__num_squares / 2)) * self.__scale, 2)
264+
max_z_coord = round(z_origin + ((self.__num_squares / 2) +
265+
(sign(camera_axes.z) * -1) * (self.__num_squares / 2)) * self.__scale, 2)
214266

215267
# Compound origins are in the middle of the bounding boxes. Thus new pos will be between max and min.
216268
x_middle = (max_x_coord + min_x_coord) / 2
@@ -240,23 +292,30 @@ def update_grid(self):
240292
Update the grid axes and numbers if the camera position/rotation has changed.
241293
"""
242294
# Obtain the new camera settings
243-
new_camera_pos = self.__scene.camera.pos
244-
new_camera_axes = self.__scene.camera.axis
295+
new_camera_pos = vector(self.__scene.camera.pos)
296+
new_camera_axes = vector(self.__scene.camera.axis)
297+
298+
old_camera_pos = vector(self.camera_pos)
299+
old_camera_axes = vector(self.camera_axes)
300+
301+
# Update old positions
302+
self.camera_pos = new_camera_pos
303+
self.camera_axes = new_camera_axes
245304

246-
old_camera_pos = self.camera_pos
247-
old_camera_axes = self.camera_axes
305+
distance_from_center = mag(self.__scene.center - self.__scene.camera.pos)
306+
new_scale = round(distance_from_center / 30.0, 1)
307+
if not new_scale == self.__scale:
308+
self.set_scale(new_scale)
248309

249310
# If camera is different to previous: update
250311
if (not new_camera_axes.equals(old_camera_axes)) or (not new_camera_pos.equals(old_camera_pos)):
251-
# Update old positions
252-
self.camera_pos = new_camera_pos
253-
self.camera_axes = new_camera_axes
254-
255312
# Update grid
256313
self.__move_grid_objects()
257314
update_grid_numbers(self.__focal_point,
258315
self.grid_object[self.__labels_idx],
259316
self.__num_squares,
317+
self.__scale,
318+
self.__is_3d,
260319
self.__scene)
261320

262321
def toggle_2d_3d(self):
@@ -312,6 +371,24 @@ def set_relative(self, is_relative):
312371
self.__relative_cam = is_relative
313372
self.update_grid()
314373

374+
def set_scale(self, value):
375+
"""
376+
Set the scale and redraw the grid
377+
378+
:param value: The value to set the scale to
379+
:type value: `float`
380+
"""
381+
value = max(min(value, 100), 0.1) # Between 0.1 and 100
382+
self.__scale = value
383+
# Turn off grid then delete
384+
for plane in self.grid_object[self.__planes_idx]:
385+
plane.visible = False
386+
for text in self.grid_object[self.__labels_idx]:
387+
text.visible = False
388+
389+
self.grid_object = [[], []]
390+
self.__init_grid()
391+
315392

316393
def create_line(pos1, pos2, scene, colour=None, thickness=0.01):
317394
"""
@@ -338,14 +415,14 @@ def create_line(pos1, pos2, scene, colour=None, thickness=0.01):
338415
colour = [0, 0, 0]
339416

340417
if colour[0] > 1.0 or colour[1] > 1.0 or colour[2] > 1.0 or \
341-
colour[0] < 0.0 or colour[1] < 0.0 or colour[2] < 0.0:
418+
colour[0] < 0.0 or colour[1] < 0.0 or colour[2] < 0.0:
342419
raise ValueError("RGB values must be normalised between 0 and 1")
343420

344421
if thickness < 0.0:
345422
raise ValueError("Thickness must be greater than 0")
346423

347424
# Length of the line using the magnitude
348-
line_len = mag(pos2-pos1)
425+
line_len = mag(pos2 - pos1)
349426

350427
# Position of the line is the midpoint (centre) between the ends
351428
position = (pos1 + pos2) / 2
@@ -391,14 +468,14 @@ def create_segmented_line(pos1, pos2, scene, segment_len, colour=None, thickness
391468
colour = [0, 0, 0]
392469

393470
if colour[0] > 1.0 or colour[1] > 1.0 or colour[2] > 1.0 or \
394-
colour[0] < 0.0 or colour[1] < 0.0 or colour[2] < 0.0:
471+
colour[0] < 0.0 or colour[1] < 0.0 or colour[2] < 0.0:
395472
raise ValueError("RGB values must be normalised between 0 and 1")
396473

397474
if thickness < 0.0:
398475
raise ValueError("Thickness must be greater than 0")
399476

400477
# Length of the line using the magnitude
401-
line_len = mag(pos2-pos1)
478+
line_len = mag(pos2 - pos1)
402479

403480
# Axis direction of the line (to align the box (line) to intersect the two points)
404481
axis_dir = pos2 - pos1
@@ -407,24 +484,24 @@ def create_segmented_line(pos1, pos2, scene, segment_len, colour=None, thickness
407484
# Return a compound of boxes of thin width and height to resemble a dashed line
408485
dash_positions = []
409486
boxes = []
410-
pos1 = pos1 + (axis_dir * segment_len/2) # Translate centre pos to centre of where dashes will originate from
487+
pos1 = pos1 + (axis_dir * segment_len / 2) # Translate centre pos to centre of where dashes will originate from
411488

412489
# Range = number of dashes (vis and invis)
413-
for idx in range(0, int(ceil(line_len / (segment_len / axis_dir.mag)))):
490+
for idx in range(0, int(ceil(line_len / (segment_len / axis_dir.mag)))):
414491
# Add every even point (zeroth, second...) to skip gaps between boxes
415492
if idx % 2 == 0:
416493
dash_positions.append(pos1)
417494
pos1 = (pos1 + axis_dir * segment_len)
418495
# If the axis between points changes, then the line has surpassed the end point. The line is done
419-
check_dir = pos2-pos1
496+
check_dir = pos2 - pos1
420497
check_dir.mag = 1.0
421498
if not vectors_approx_equal(axis_dir, check_dir):
422499
break
423500

424501
for xyz in dash_positions:
425502
length = segment_len
426503
# If the box will surpass the end point
427-
len_to_end = (pos2-xyz).mag
504+
len_to_end = (pos2 - xyz).mag
428505
if len_to_end < segment_len / 2:
429506
# Length is equal to dist to the end * 2 (as pos is middle of box)
430507
length = len_to_end * 2

0 commit comments

Comments
 (0)