def rotatedFilledCells(self, angle):
half_size_x = self.block_size_x // 2
half_size_y = self.block_size_y // 2
corners = [(0, 0)] * 4
# Top left, top right, bottom right, bottom left (clockwise ordering of points)
# Corners of the block centered at 0,0
corners[0] = (-half_size_x, -half_size_y)
corners[1] = (half_size_x, -half_size_y)
corners[2] = (half_size_x, half_size_y)
corners[3] = (-half_size_x, half_size_y)
for i in range(4):
x, y = corners[i]
rotated_x = x * math.cos(angle) + y * math.sin(angle)
rotated_y = -x * math.sin(angle) + y * math.cos(angle)
corners[i] = (int(round(rotated_x, 0)), int(round(rotated_y, 0)))
# Add bounding polygon cells to a set
cells = set()
cells.update(raytrace(*corners[0], *corners[1]))
cells.update(raytrace(*corners[3], *corners[2]))
cells.update(raytrace(*corners[0], *corners[3]))
cells.update(raytrace(*corners[1], *corners[2]))
# breadth first search from 0,0 the remaining cells
bfs = deque([(0, 0)])
while len(bfs) > 0:
current_cell = bfs.popleft()
# If previously visited then skip
if current_cell in cells:
continue
else:
cells.add(current_cell)
# Search the neighbours
for nbr in [(current_cell[0] + 1, current_cell[1]),
(current_cell[0] - 1, current_cell[1]),
(current_cell[0], current_cell[1] + 1),
(current_cell[0], current_cell[1] - 1)]:
if nbr not in cells:
bfs.append(nbr)
# DEBUGGING
# longest_edge = max(self.block_size_x, block_size_y)
# test = np.zeros((30, 30))
# for i,j in cells:
# x = i + 15
# y = j + 15
# print(x,y)
# test[x, y] = 1
# plt.imshow(test)
# plt.show()
return list(cells)
def rotatedFilledCells(self, angle, size_x=None, size_y=None):
if size_x is None and size_y is None:
half_size_x = self.block_size_x // 2
half_size_y = self.block_size_y // 2
else:
half_size_x = size_x // 2
half_size_y = size_y // 2
corners = [(0, 0)] * 4
# Top left, top right, bottom right, bottom left (clockwise ordering of points)
# Corners of the block centered at 0,0
corners[0] = (-half_size_x, -half_size_y)
corners[1] = (half_size_x, -half_size_y)
corners[2] = (half_size_x, half_size_y)
corners[3] = (-half_size_x, half_size_y)
for i in range(4):
x, y = corners[i]
rotated_x = x * math.cos(angle) - y * math.sin(angle)
rotated_y = x * math.sin(angle) + y * math.cos(angle)
# print(rotated_x, rotated_y)
corners[i] = (int(round(rotated_x, 0)), int(round(rotated_y, 0)))
cells = set()
cells.update(raytrace(*corners[0], *corners[1]))
cells.update(raytrace(*corners[3], *corners[2]))
cells.update(raytrace(*corners[0], *corners[3]))
cells.update(raytrace(*corners[1], *corners[2]))
# breadth first search from 0,0 the remaining cells
bfs = deque([(0, 0)])
while len(bfs) > 0:
current_cell = bfs.popleft()
# If previously visited then skip
if current_cell in cells:
continue
else:
cells.add(current_cell)
# Search the neighbours
for nbr in [(current_cell[0] + 1, current_cell[1]),
(current_cell[0] - 1, current_cell[1]),
(current_cell[0], current_cell[1] + 1),
(current_cell[0], current_cell[1] - 1)]:
if nbr not in cells:
bfs.append(nbr)
return list(cells)
def process_path(self, path):
final_path = []
prev_pos_x, prev_pos_y, prev_layer = path[0]
prev_pos = (prev_pos_x, prev_pos_y)
for pos_x, pos_y, layer in path:
pos = (pos_x, pos_y)
# Check if we need to interpolate between two points
if abs(pos[0] - prev_pos[0]) > 1 or abs(pos[1] - prev_pos[1]) > 1:
layer_at_cell = self.interpolatePath(prev_pos, prev_layer, pos,
layer)
assert layer_at_cell != None, "Could not find an intermediate path"
cells = raytrace(*prev_pos, *pos)
for c in cells:
final_path.append((*c, layer_at_cell[c]))
else:
final_path.append((*pos, layer))
prev_pos = pos
prev_layer = layer
return final_path
def endLayer(self, current_pos, current_layer, goal_pos):
# Need to check if we can move from current pos to goal pos
# In order for two adjacent cells to be pathable
# the cells must have an unoccupied layer 1 shift apart
# Thus for a path to be valid all adjacent cells must satisfy the above
# No possible orientations can be reached at goal
if self.oriented_occ_grid[goal_pos] == 255:
return None
# Get the direct path from current to goal
traversed_cells = raytrace(*current_pos, *goal_pos)
# Create this structure so we can efficiently get the next cell
next_cell = dict()
for num, cell in enumerate(traversed_cells[:-1]):
next_cell[cell] = traversed_cells[num + 1]
# depth first search through the path to find a suitable path
dfs = []
dfs.append((current_pos, current_layer))
visited = set()
while len(dfs) > 0:
pos, layer = dfs.pop()
if (pos, layer) in visited:
continue
else:
visited.add((pos, layer))
# Return True if we have reached the goal
if pos == goal_pos:
return layer
# Skip if layer is completely blocked
if self.oriented_occ_grid[pos] == 255:
continue
next_pos = next_cell[pos]
# Check adjacent and current layers of next position to see if it can be reached from here
for i in [-1, 1, 0]:
next_layer = (layer + i) % 8
if self.oriented_occ_grid[next_pos] & 2**next_layer == 0:
# print(pos, layer, next_pos, next_layer)
dfs.append((next_pos, next_layer))
# dfs didnt find a suitable path so return false
return None
def interpolatePath(self, current_pos, current_layer, goal_pos,
goal_layer):
# Get the direct path from current to goal
traversed_cells = raytrace(*current_pos, *goal_pos)
# Create this structure so we can efficiently get the next cell
next_cell = dict()
for num, cell in enumerate(traversed_cells[:-1]):
next_cell[cell] = traversed_cells[num + 1]
# depth first search through the path to find a suitable path
dfs = []
dfs.append((current_pos, current_layer))
visited = set()
layer_at_cell = dict()
while len(dfs) > 0:
pos, layer = dfs.pop()
if (pos, layer) in visited:
continue
else:
visited.add((pos, layer))
layer_at_cell[pos] = layer
# Return True if we have reached the goal
if pos == goal_pos and layer == goal_layer:
return layer_at_cell
next_pos = next_cell[pos]
# Check adjacent and current layers of next position to see if it can be reached from here
for i in [-1, 1, 0]:
next_layer = (layer + i) % 8
if self.oriented_occ_grid[next_pos] & 2**next_layer == 0:
# print(pos, layer, next_pos, next_layer)
dfs.append((next_pos, next_layer))
# dfs didnt find a suitable path so return false
return None