def get_boundary(self, vector: np.ndarray((2,)), tower: CommsTower, is_lower: bool = False) -> np.ndarray((2, 2)): """ Calculates one of the boundaries of the comms tower's strip. Parameters ---------- vector : numpy.ndarray shape - (2,) vector from the tower to the target tower : domain.CommsTower.CommsTower comms tower instance that emits the strip is_lower : bool True if calculating the lower boundary, False if calculating the upper one Returns ------- numpy.ndarray shape - (2, 2) start and end points of the boundary """ if is_lower: rotated_vector = MathHelper.rotate_vector(vector, -1 * self.strip_half_rads_sin, self.strip_half_rads_cos) else: rotated_vector = MathHelper.rotate_vector(vector, self.strip_half_rads_sin, self.strip_half_rads_cos) tower_position = tower.get_position() space_size = tower.get_space_size() return MathHelper.get_strip_start_end_points_from_vector(rotated_vector, tower_position, space_size)
def get_distance_to_collision_with_polyshape(
poly_1: np.ndarray, poly_2: np.ndarray, heading_offset: np.ndarray(
(2, ))) -> (bool, float):
"""
Returns the distance to collision between poly_1 and poly_2 if the polyshape 1 moves in the direction
and magnitude given by the heading_offset parameter.
Parameters
----------
poly_1 : numpy.ndarray
shape - (8, 2)
vertices of the moving polyshape
poly_2 : numpy.ndarray
shape - (8, 2)
vertices of the stationary polyshape
heading_offset : numpy.ndarray
shape - (2,)
direction and magnitude of the desired movement
Returns
-------
(bool, float)
distance to collision or maximum movement length. True if collision, False otherwise
"""
is_colliding = False
earliest = math.inf
poly_1_lines, poly_1_lines_origins = MathHelper.get_lines(
poly_1, heading_offset)
poly_1_edges = MathHelper.get_edges(poly_1)
for line_index in range(np.size(poly_1_lines, 0)):
line = poly_1_lines[line_index, :]
line_start = poly_1_lines_origins[line_index, :]
result, distance = MathHelper.get_earliest_intersection_between_line_polyshape(
poly_2, line, line_start)
if result and distance < earliest:
if distance == 0:
return True, 0
earliest = distance
is_colliding = True
poly_2_lines, poly_2_lines_origins = MathHelper.get_lines(
poly_2, -1 * heading_offset)
for line_index in range(np.size(poly_2_lines, 0)):
line = poly_2_lines[line_index, :]
line_start = poly_2_lines_origins[line_index, :]
result, distance = MathHelper.get_earliest_intersection_between_line_polyshape(
poly_1, line, line_start, edges=poly_1_edges)
if result and distance < earliest:
if distance == 0:
return True, 0
earliest = distance
is_colliding = True
return is_colliding, earliest
def is_initial_position_colliding(agent_index: int, position: np.ndarray(
(2, )), positions: np.ndarray) -> bool:
"""
Checks if a circle in a given position is colliding with any other circle, whose center is given by the
positions parameter. This uses basic circle intersection, instead of polytope intersection, because it is
simpler, faster, and in the initial positions it is best if the agents are spread apart, while being close
enough to be in the same area.
Parameters
----------
agent_index : int
index of the circle for which to check collisions
position : numpy.ndarray
shape - (2,)
position of the circle
positions : numpy.ndarray
shape - (agents_number, 2)
positions of the other circles
Returns
-------
bool
True is it is colliding with any of the other, False otherwise
"""
radius = AgentProperties.MEASUREMENT_ERROR_RADIUS + AgentProperties.MAX_MOVEMENT_LENGTH
for other_position_index in range(agent_index):
other_position = positions[other_position_index, :]
if MathHelper.is_intersecting_circle_circle(
position, other_position, radius):
return True
return False
def is_in_strip_boundaries(polytope: Polytope, lower_boundary: np.ndarray((2, 2)), upper_boundary: np.ndarray((2, 2))) -> bool: """ Calculates if the polytope given by the polytope parameter is inside the strip defined by the boundaries given by the lower_boundary and upper_boundary parameters. Parameters ---------- polytope : domain.Polytope.Polytope lower_boundary : numpy.ndarray shape - (2, 2) start and end points of the comms tower's strip's lower boundary upper_boundary : numpy.ndarray shape - (2, 2) start and end points of the comms tower's strip's upper boundary Returns ------- bool True if polytope is inside the strip, False otherwise """ position = polytope.get_center_position() lower_boundary_vector = lower_boundary[0, :] - lower_boundary[1, :] lower_boundary_gradient = lower_boundary_vector[1] / lower_boundary_vector[0] lower_boundary_b = lower_boundary[0, 1] lower_boundary_y = lower_boundary_gradient * position[0] + lower_boundary_b upper_boundary_vector = upper_boundary[0, :] - upper_boundary[1, :] upper_boundary_gradient = upper_boundary_vector[1] / upper_boundary_vector[0] upper_boundary_b = upper_boundary[0, 1] upper_boundary_y = upper_boundary_gradient * position[0] + upper_boundary_b # Checks if the center of the polytope is inside the boundaries if not MathHelper.is_point_between_points(position[1], lower_boundary_y, upper_boundary_y): return False # If the center is inside the boundaries, checks the polytope if MathHelper.is_line_intersecting_polytope(polytope, lower_boundary[0, :], lower_boundary[1, :]): return False if MathHelper.is_line_intersecting_polytope(polytope, upper_boundary[0, :], upper_boundary[1, :]): return False return True
def get_attraction_vector(
self, agent_index: int, positions: np.ndarray,
distribution: np.ndarray,
neighbors_number_to_consider: int) -> np.ndarray((2, )):
"""
Calculates the attraction vector based on the height map values of the agents.
Parameters
----------
agent_index : int
index of the agent that is moving
positions : numpy.ndarray
shape - (agents_number, 2)
positions of the agents
distribution : numpy.ndarray
shape - (agents_number,)
1D array that represents if each agent is a neighbor of the moving agent
neighbors_number_to_consider : int
maximum number of agents to consider relevant neighbors
Returns
-------
numpy.ndarray
shape - (2,)
attraction vector
"""
agent_cluster = distribution[agent_index]
agents_in_cluster = np.array(distribution == agent_cluster)
agents_in_cluster[agent_index] = 0
positions_in_cluster = positions[agents_in_cluster, :]
res, considered_positions_in_cluster = MathHelper.get_relevant_elements_by_number(
positions_in_cluster, neighbors_number_to_consider)
if not res:
return np.zeros((2, ))
vector = np.zeros((2, ))
agent_position = positions[agent_index]
agent_attraction_factor = self.gradient_map.get_value(agent_position)
for i in range(np.size(considered_positions_in_cluster, 0)):
other_agent_position = considered_positions_in_cluster[i]
other_agent_attraction_factor = self.gradient_map.get_value(
other_agent_position)
attraction_vector = (other_agent_attraction_factor - agent_attraction_factor) * \
(other_agent_position - agent_position)
vector += attraction_vector
norm = np.linalg.norm(vector)
if norm != 0:
vector /= norm
return vector
def get_alignment_vector(
agent_index: int, velocities: np.ndarray, distribution: np.ndarray,
neighbors_number_to_consider: int) -> np.ndarray((2, )):
"""
Calculates the alignment vector.
Parameters
----------
agent_index : int
index of the agent that is moving
velocities : numpy.ndarray
shape - (agents_number, 2)
velocities of the agents
distribution : numpy.ndarray
shape - (agents_number,)
1D array that represents if each agent is a neighbor of the moving agent
neighbors_number_to_consider : int
maximum number of agents to consider relevant agents
Returns
-------
numpy.ndarray
shape - (2,)
alignment vector
"""
agent_cluster = distribution[agent_index]
agents_in_cluster = np.array(distribution == agent_cluster)
agents_in_cluster[agent_index] = 0
velocities_in_cluster = velocities[agents_in_cluster, :]
res, considered_velocities_in_cluster = MathHelper.get_relevant_elements_by_number(
velocities_in_cluster, neighbors_number_to_consider)
if not res:
return np.zeros((2, ))
vector = np.mean(considered_velocities_in_cluster, axis=0)
vector -= velocities[agent_index, :]
norm = np.linalg.norm(vector)
if norm != 0:
vector /= norm
return vector