def move(self, magnitude, direction=None, min_angle=0, max_angle=360, phasing=False):
assert min_angle <= max_angle, "The minimum angle must be smaller than maximum angle."
angle = math.degrees(math.acos(direction[0] / 0)) if direction \
else math.radians(random() * (max_angle - min_angle) + min_angle)
x = magnitude * math.cos(angle)
y = magnitude * math.sin(angle)
displacement = np.array([x, y])
squared_distance = np.dot(displacement, displacement)
departure = self.get_center()
destination = departure + displacement
self.set_center(destination[0], destination[1])
# Check if the particle is still inside the zone.
if not self.world.shape.confines_circle(self):
destination = self.world.shape.confine_circle_coord(self, departure, destination)
displacement = destination - departure
squared_distance = np.dot(displacement, displacement)
self.set_center(destination[0], destination[1])
if not self.world.shape.confines_circle(self):
canvas.create_oval(destination[0] - self.get_radius(), destination[1] - self.get_radius(),
destination[0] + self.get_radius(), destination[1] + self.get_radius(), fill="red")
# Check if the particle collides with other particles along its path.
if not math.isclose(squared_distance, 0, rel_tol=1e-09):
self.set_center(departure[0], departure[1])
quadrants = self.world.grid.rectangle_overlap(departure, destination, self.get_radius(), canvas)
trajectory = formula.Segment(departure, destination)
particles = set()
obstacles = set()
for quadrant in quadrants:
contents = quadrant.contents()
for content in contents:
if content != self:
particles.add(content)
point = content.get_center()
vector = point - departure
theta = formula.angle_between(displacement, vector)
if theta < 90 and not math.isclose(theta, 90):
distance_from_trajectory = trajectory.squared_distance_from_point(point)
distance_from_obstacle = self.squared_distance_from_point(point)
rectangle_width = self.get_radius() + content.get_radius()
if distance_from_trajectory < math.pow(rectangle_width, 2):
distance_along_trajectory = distance_from_obstacle - distance_from_trajectory \
if not math.isclose(distance_from_trajectory, 0, rel_tol=1e-09) \
else distance_from_obstacle
rectangle_length = math.sqrt(
squared_distance) + self.get_radius() + content.get_radius()
if distance_along_trajectory < math.pow(rectangle_length, 2):
obstacles.add(content)
# content.redraw(canvas, fill="red")
obstacles = list(obstacles)
obstacles.sort(key=lambda particle: self.distance_from_circle(particle))
non_obstacles = list(particles.difference(obstacles))
non_obstacles.sort(key=lambda particle: self.distance_from_circle(particle))
# Movement stops at the nearest obstacle.
j = 0
while j < len(obstacles):
point = obstacles[j].get_center()
distance_from_trajectory = trajectory.squared_distance_from_point(point)
distance_from_obstacle = math.pow(obstacles[j].get_radius() + self.get_radius(), 2)
projection = formula.project_vector(point - departure, displacement)
distance_from_position = math.sqrt(distance_from_obstacle - distance_from_trajectory)
delta = formula.resize_vector(displacement, distance_from_position)
destination = departure + projection - delta
vector = destination - departure
squared_magnitude = np.dot(vector, vector)
# Check if the magnitude of the corrected displacement is less than or equal to the original.
if squared_magnitude > squared_distance and not math.isclose(squared_magnitude, squared_distance):
displacement = formula.resize_vector(vector, math.sqrt(squared_distance))
destination = departure + displacement
squared_distance = np.dot(displacement, displacement)
# Check if the particle collides with other obstacles.
self.set_center(destination[0], destination[1])
j += 1
while j < len(obstacles):
if self.overlaps_circle(obstacles[j]):
break
j += 1
# Check if the particle overlaps other particles that were outside the path.
for j in range(len(non_obstacles)):
if self.overlaps_circle(non_obstacles[j]):
destination = departure
break
# Update the coordinate of the particle.
displacement = destination - departure
squared_distance = np.dot(displacement, displacement)
self.set_center(destination[0], destination[1])
# Update the quadtree.
if not math.isclose(squared_distance, 0, rel_tol=1e-09):
for quadrant in quadrants:
if self in quadrant.contents():
quadrant.contents().remove(self)
quadrants = self.world.grid.overlapped_by_circle(self)
for quadrant in quadrants:
if self not in quadrant.contents() and len(quadrant.leaves()) == 0:
quadrant.contents().append(self)
self.redraw(canvas)
def rectangle_overlap(self, start, end, margin, canvas):
queue = [self._root]
quadrants = []
# Vectors used to find the four corners of the rectangle.
v1 = formula.resize_vector(end - start, margin)
v2 = -v1
v3 = formula.rotate_vector(v1, 90)
v4 = -v3
# The four corners of the rectangle.
p1 = start + v2 + v3
p2 = start + v2 + v4
p3 = end + v1 + v3
p4 = end + v1 + v4
# The borders of the rectangle
border_12 = formula.Segment(p1, p2)
border_13 = formula.Segment(p1, p3)
border_24 = formula.Segment(p2, p4)
border_34 = formula.Segment(p3, p4)
# Visual representation of the rectangle.
# canvas.create_line(p1[0], p1[1], p2[0], p2[1], fill="blue", width=2)
# canvas.create_line(p3[0], p3[1], p4[0], p4[1], fill="blue", width=2)
# canvas.create_line(p1[0], p1[1], p3[0], p3[1], fill="blue", width=2)
# canvas.create_line(p2[0], p2[1], p4[0], p4[1], fill="blue", width=2)
# Iterate through a list of quadrants overlapped by the rectangle.
while len(queue) > 0:
quadrant = queue.pop(0)
center = quadrant.get_center()
width = quadrant.get_width()
height = quadrant.get_height()
centerline = formula.Segment(start, end)
distance_from_centerline = centerline.distance_from_point(center)
# The boundary of the current quadrant.
x1 = center[0] - width / 2
x2 = center[0] + width / 2
y1 = center[1] - height / 2
y2 = center[1] + height / 2
# The four corners of the current quadrant.
north_west = np.array([x1, y1])
north_east = np.array([x2, y1])
south_west = np.array([x1, y2])
south_east = np.array([x2, y2])
# The borders of the current quadrant.
north_border = formula.Segment(north_west, north_east)
south_border = formula.Segment(south_west, south_east)
west_border = formula.Segment(north_west, south_west)
east_border = formula.Segment(north_east, south_east)
# Check if the quadrant is inside the rectangle.
if distance_from_centerline < margin or math.isclose(
distance_from_centerline, margin):
quadrants.append(quadrant)
queue += quadrant.leaves()
# Check if one of the borders of the rectangle goes through the center of the current quadrant.
elif border_12.intersects_point(center)\
or border_13.intersects_point(center)\
or border_24.intersects_point(center)\
or border_34.intersects_point(center):
quadrants.append(quadrant)
queue += quadrant.leaves()
# Check if one of the corners of the rectangle is inside the current quadrant.
elif quadrant.contains_point(p1):
quadrants.append(quadrant)
queue += quadrant.leaves()
elif quadrant.contains_point(p2):
quadrants.append(quadrant)
queue += quadrant.leaves()
elif quadrant.contains_point(p3):
quadrants.append(quadrant)
queue += quadrant.leaves()
elif quadrant.contains_point(p4):
quadrants.append(quadrant)
queue += quadrant.leaves()
# Check if one of the segments of the rectangle intersects a quadrant.
elif west_border.intersects_segment(border_12)\
or east_border.intersects_segment(border_12)\
or north_border.intersects_segment(border_12)\
or south_border.intersects_segment(border_12):
quadrants.append(quadrant)
queue += quadrant.leaves()
elif west_border.intersects_segment(border_13)\
or east_border.intersects_segment(border_13)\
or north_border.intersects_segment(border_13)\
or south_border.intersects_segment(border_13):
quadrants.append(quadrant)
queue += quadrant.leaves()
elif west_border.intersects_segment(border_24)\
or east_border.intersects_segment(border_24)\
or north_border.intersects_segment(border_24)\
or south_border.intersects_segment(border_24):
quadrants.append(quadrant)
queue += quadrant.leaves()
elif west_border.intersects_segment(border_34)\
or east_border.intersects_segment(border_34)\
or north_border.intersects_segment(border_34)\
or south_border.intersects_segment(border_34):
quadrants.append(quadrant)
queue += quadrant.leaves()
return quadrants
def search(self):
if self.field_of_view and len(self.field_of_view) == 2:
# Get the central vision.
facing_direction_vector = self.direction()
# central_vision_extent = self._center + facing_direction_vector
# self._center = self._center + formula.resize_vector(facing_direction_vector, self._radius)
# Get the left outer boundary of the peripheral vision.
left_outer_boundary_vector = formula.rotate_vector(facing_direction_vector, self.field_of_view[1] / 2)
left_outer_boundary_extent = self._center + left_outer_boundary_vector
left_outer_boundary = formula.Segment(self._center, left_outer_boundary_extent)
# Get the right outer boundary of the peripheral vision.
right_outer_boundary_vector = formula.rotate_vector(facing_direction_vector, -self.field_of_view[1] / 2)
right_outer_boundary_extent = self._center + right_outer_boundary_vector
right_outer_boundary = formula.Segment(self._center, right_outer_boundary_extent)
# Draw the field of view.
canvas.create_line(self._center[0], self._center[1], left_outer_boundary_extent[0],
left_outer_boundary_extent[1], fill="blue", width=2)
canvas.create_line(self._center[0], self._center[1], right_outer_boundary_extent[0],
right_outer_boundary_extent[1], fill="blue", width=2)
# circle_zone.canvas.create_line(self._center[0], self._center[1], central_vision_extent[0],
# central_vision_extent[1], fill="blue", width=2)
canvas.create_arc(self._center[0] - self.field_of_view[0],
self._center[1] - self.field_of_view[0],
self._center[0] + self.field_of_view[0],
self._center[1] + self.field_of_view[0],
outline="blue", width=2, style=tk.ARC,
start=360 - (self._rotation + self.field_of_view[1] / 2),
extent=self.field_of_view[1])
# Find the quadrants of the world that are inside the field of view.
quadrants = []
queue = [self.world.grid.get_root()]
particles_searched = 0
particles_selected = 0
quadrants_searched = 0
quadrants_selected = 0
while len(queue) > 0:
quadrants_searched += 1
quadrant = queue.pop(0)
center = quadrant.get_center()
width = quadrant.get_width()
height = quadrant.get_height()
# The boundaries of the current quadrant.
x1 = center[0] - width / 2
x2 = center[0] + width / 2
y1 = center[1] - height / 2
y2 = center[1] + height / 2
# The four corners of the current quadrant.
north_west = np.array([x1, y1])
north_east = np.array([x2, y1])
south_west = np.array([x1, y2])
south_east = np.array([x2, y2])
# Vectors obtained by joining the center of the particle to each corner of the quadrant.
cnw = north_west - self._center
cne = north_east - self._center
csw = south_west - self._center
cse = south_east - self._center
# Angles between the central vision vector and the previously calculated vectors.
angle_cnw = formula.angle_between(facing_direction_vector, cnw)
angle_cne = formula.angle_between(facing_direction_vector, cne)
angle_csw = formula.angle_between(facing_direction_vector, csw)
angle_cse = formula.angle_between(facing_direction_vector, cse)
angle_threshold = self.field_of_view[1] / 2
# Squared distances obtained from previously calculated vectors.
sqrd_cnw = np.dot(cnw, cnw)
sqrd_cne = np.dot(cne, cne)
sqrd_csw = np.dot(csw, csw)
sqrd_cse = np.dot(cse, cse)
squared_distance_threshold = math.pow(self.field_of_view[0], 2)
# The borders of the current quadrant.
north_border = formula.Segment(north_west, north_east)
south_border = formula.Segment(south_west, south_east)
west_border = formula.Segment(north_west, south_west)
east_border = formula.Segment(north_east, south_east)
# Check if the quadrant contains the particle's coordinate or the furthest points of the outer boundaries.
if quadrant.contains_point(self._center) \
or quadrant.contains_point(left_outer_boundary_extent) \
or quadrant.contains_point(right_outer_boundary_extent):
quadrants.append(quadrant)
queue += quadrant.leaves()
quadrants_selected += 1
# Check if the left outer boundary intersects with the quadrant.
elif north_border.intersects_segment(left_outer_boundary) \
or south_border.intersects_segment(left_outer_boundary) \
or west_border.intersects_segment(left_outer_boundary) \
or east_border.intersects_segment(left_outer_boundary):
quadrants.append(quadrant)
queue += quadrant.leaves()
quadrants_selected += 1
# Check if the right outer boundary intersects with the quadrant.
elif north_border.intersects_segment(right_outer_boundary) \
or south_border.intersects_segment(right_outer_boundary) \
or west_border.intersects_segment(right_outer_boundary) \
or east_border.intersects_segment(right_outer_boundary):
quadrants.append(quadrant)
queue += quadrant.leaves()
quadrants_selected += 1
# Check if quadrant is inside the field of view.
elif (angle_cnw < angle_threshold and sqrd_cnw < squared_distance_threshold) \
or (angle_cne < angle_threshold and sqrd_cne < squared_distance_threshold) \
or (angle_csw < angle_threshold and sqrd_csw < squared_distance_threshold) \
or (angle_cse < angle_threshold and sqrd_cse < squared_distance_threshold):
quadrants.append(quadrant)
queue += quadrant.leaves()
quadrants_selected += 1
# Get the particles that are inside the field of view.
particles = set()
targets = []
for quadrant in quadrants:
quadrant.redraw(canvas, outline="red")
particles.update(quadrant.contents())
for particle in particles:
particles_searched += 1
if particle != self:
vector = particle.get_center() - self._center
squared_distance = math.sqrt(np.dot(vector, vector))
angle = formula.angle_between(facing_direction_vector, vector)
epsilon = (2 * np.dot(vector, vector) - math.pow(particle.get_radius(), 2)) / (
2 * np.dot(vector, vector))
epsilon = math.degrees(math.acos(epsilon))
if (angle - epsilon < self.field_of_view[1] / 2
or math.isclose(angle - epsilon, self.field_of_view[1] / 2)) \
and (squared_distance < self.field_of_view[0] + particle.get_radius()
or math.isclose(squared_distance, self.field_of_view[0] + particle.get_radius())):
# print(str(min_angle) + " <= " + str(a) + " <= " + str(max_angle))
targets.append(particle)
particle.redraw(canvas, fill="red")
particles_selected += 1
self.redraw(canvas, fill="blue")
targets.sort(key=lambda target: self.distance_from_circle(target), reverse=True)
print()
print("SEARCH")
print("Quadrants (selected/searched): {}/{}".format(quadrants_selected, quadrants_searched))
print("Particles (selected/searched): {}/{}".format(particles_selected, particles_searched))
return targets
def confine_circle_coord(self, circle, start, end):
center = self.get_center()
radius = circle.get_radius()
width = self._width
height = self._height
point = circle.get_center()
# canvas.create_oval(point[0] - radius, point[1] - radius, point[0] + radius, point[1] + radius, fill="blue")
# The boundary of the current quadrant.
x1 = center[0] - width / 2
x2 = center[0] + width / 2
y1 = center[1] - height / 2
y2 = center[1] + height / 2
# The four corners of the current quadrant.
north_west = np.array([x1, y1])
north_east = np.array([x2, y1])
south_west = np.array([x1, y2])
south_east = np.array([x2, y2])
# The borders of the current quadrant.
north_border = formula.Segment(north_west, north_east)
south_border = formula.Segment(south_west, south_east)
west_border = formula.Segment(north_west, south_west)
east_border = formula.Segment(north_east, south_east)
segment = formula.Segment(start, end)
if not self.contains_point(point):
if segment.intersects_segment(north_border):
point = segment.intersection_point(north_border)
y = point[1] + radius
x = segment.x(y)
point[0], point[1] = x, y
elif segment.intersects_segment(south_border):
point = segment.intersection_point(south_border)
y = point[1] - radius
x = segment.x(y)
point[0], point[1] = x, y
elif segment.intersects_segment(west_border):
point = segment.intersection_point(west_border)
x = point[0] + radius
y = segment.y(x)
point[0], point[1] = x, y
elif segment.intersects_segment(east_border):
point = segment.intersection_point(east_border)
x = point[0] - radius
y = segment.y(x)
point[0], point[1] = x, y
# canvas.create_oval(point[0] - radius, point[1] - radius, point[0] + radius, point[1] + radius, fill="red")
if np.sign(segment.vector[0]) < 0:
distance_from_west = west_border.distance_from_point(point)
if np.sign(segment.vector[1]) < 0:
distance_from_north = north_border.distance_from_point(point)
if distance_from_west < distance_from_north:
x = point[0] - distance_from_west + radius
y = segment.y(x)
point[0], point[1] = x, y
else:
y = point[1] - distance_from_north + radius
x = segment.x(y)
point[0], point[1] = x, y
elif np.sign(segment.vector[1]) > 0:
distance_from_south = south_border.distance_from_point(point)
if distance_from_west < distance_from_south:
x = point[0] - distance_from_west + radius
y = segment.y(x)
point[0], point[1] = x, y
else:
y = point[1] + distance_from_south - radius
x = segment.x(y)
point[0], point[1] = x, y
elif np.sign(segment.vector[0]) > 0:
distance_from_east = east_border.distance_from_point(point)
if np.sign(segment.vector[1]) < 0:
distance_from_north = north_border.distance_from_point(point)
if distance_from_east < distance_from_north:
x = point[0] + distance_from_east - radius
y = segment.y(x)
point[0], point[1] = x, y
else:
y = point[1] - distance_from_north + radius
x = segment.x(y)
point[0], point[1] = x, y
elif np.sign(segment.vector[1]) > 0:
distance_from_south = south_border.distance_from_point(point)
if distance_from_east < distance_from_south:
x = point[0] + distance_from_east - radius
y = segment.y(x)
point[0], point[1] = x, y
else:
y = point[1] + distance_from_south - radius
x = segment.x(y)
point[0], point[1] = x, y
# canvas.create_oval(point[0] - radius, point[1] - radius, point[0] + radius, point[1] + radius, fill="green")
"""distance_from_north = north_border.distance_from_point(point)
if distance_from_north < radius and not math.isclose(distance_from_north, radius):
y = point[1] - distance_from_north + radius
x = segment.x(y)
point[0], point[1] = x, y
distance_from_south = south_border.distance_from_point(point)
if distance_from_south < radius and not math.isclose(distance_from_south, radius):
y = point[1] + distance_from_south - radius
x = segment.x(y)
point[0], point[1] = x, y
distance_from_west = west_border.distance_from_point(point)
if distance_from_west < radius and not math.isclose(distance_from_west, radius):
x = point[0] - distance_from_west + radius
y = segment.y(x)
point[0], point[1] = x, y
distance_from_east = east_border.distance_from_point(point)
if distance_from_east < radius and not math.isclose(distance_from_east, radius):
x = point[0] + distance_from_east - radius
y = segment.y(x)
point[0], point[1] = x, y
else:
distance_from_north = north_border.distance_from_point(point)
if point[1] < center[1] - height / 2 + radius and not math.isclose(point[1], center[1] - height / 2 + radius):
y = point[1] + distance_from_north + radius
x = segment.x(y)
point[0], point[1] = x, y
distance_from_south = south_border.distance_from_point(point)
if point[1] > center[1] + height / 2 - radius and not math.isclose(point[1], center[1] + height / 2 - radius):
y = point[1] - distance_from_south - radius
x = segment.x(y)
point[0], point[1] = x, y
distance_from_west = west_border.distance_from_point(point)
if point[0] < center[0] - width / 2 + radius and not math.isclose(point[0], center[0] - width / 2 + radius):
x = point[0] + distance_from_west + radius
y = segment.y(x)
point[0], point[1] = x, y
distance_from_east = east_border.distance_from_point(point)
if point[0] > center[0] + width / 2 - radius and not math.isclose(point[0], center[0] + width / 2 - radius):
x = point[0] - distance_from_east - radius
y = segment.y(x)
point[0], point[1] = x, y
distance_from_north = north_border.distance_from_point(point)
if distance_from_north < radius and not math.isclose(distance_from_north, radius):
y = point[1] - distance_from_north + radius
x = segment.x(y)
point[0], point[1] = x, y
distance_from_south = south_border.distance_from_point(point)
if distance_from_south < radius and not math.isclose(distance_from_south, radius):
y = point[1] + distance_from_south - radius
x = segment.x(y)
point[0], point[1] = x, y
distance_from_west = west_border.distance_from_point(point)
if distance_from_west < radius and not math.isclose(distance_from_west, radius):
x = point[0] - distance_from_west + radius
y = segment.y(x)
point[0], point[1] = x, y
distance_from_east = east_border.distance_from_point(point)
if distance_from_east < radius and not math.isclose(distance_from_east, radius):
x = point[0] + distance_from_east - radius
y = segment.y(x)
point[0], point[1] = x, y"""
return point