def herding_relationship(x1, x2, v1, v2, phi=np.pi / 2): """Determine leader-follower relationship Args: x1: x2: v1: v2: phi: Angle between [0, pi] Returns: (bool ,bool): """ if length(v1) == 0 or length(v2) == 0: return False, False e_rel = normalize(x2 - x1) c_i = dot(e_rel, normalize(v1)) c_j = -dot(e_rel, normalize(v2)) # 1.0 = cos(0) cos_phi = np.cos(phi) if cos_phi < c_i < 1.0: if cos_phi < c_j < 1.0: return False, False else: return True, False else: if cos_phi < c_j < 1.0: return False, True else: return True, True
def test_line_intersect(x0, x1, y0, y1): assume(not np.isclose(length(x1 - x0), 0.0)) assume(not np.isclose(length(y1 - y0), 0.0)) res = line_intersect(x0, x1, y0, y1) correct = LineString([x0, x1]).intersects(LineString([y0, y1])) assert res == correct
def test_not_colliding(benchmark, agent_type, force): agent1 = agent_type( **{ 'body_type': 'adult', 'position': np.array((0.0, 0.0)), 'orientation': 0.0, 'velocity': np.array((1.0, 0.0)), 'angular_velocity': 0.0, 'target_direction': np.array((1.0, 0.0)), 'target_orientation': 0.0 }) agent2 = agent_type( **{ 'body_type': 'adult', 'position': np.array((2.0, 0.0)), 'orientation': 0.0, 'velocity': np.array((1.0, 0.0)), 'angular_velocity': 0.0, 'target_direction': np.array((1.0, 0.0)), 'target_orientation': 0.0 }) array = np.concatenate((np.array(agent1), np.array(agent2))) force_i, force_j = benchmark(force, array, 0, 1) assert length(force_i) == 0 assert length(force_j) == 0
def test_truncate(v, l): vlen = length(v) truncate(v, l) if vlen > l: assert np.isclose(length(v), l) else: assert length(v) <= l
def test_normalize(a): assume(not np.allclose(a, 0.0) or np.all(a == 0.0)) assume(not length(a) > 10**8) ans = normalize(a) assert isinstance(ans, np.ndarray) l = length(ans) if np.all(a == 0.0): assert np.isclose(l, 0.0) else: assert np.isclose(l, 1.0)
def distance_circles(x0, r0, x1, r1): r""" Skin-to-Skin distance :math:`h` with normal :math:`\mathbf{\hat{n}}` between two circles. .. math:: h &= \|\mathbf{x}_0 - \mathbf{x}_1\| - (r_0 + r_1) \\ \mathbf{\hat{n}} &= \frac{\mathbf{x}_0 - \mathbf{x}_1}{\|\mathbf{x}_0 - \mathbf{x}_1\|} Args: x0 (numpy.ndarray): r0 (float): x1 (numpy.ndarray): r1 (float): Returns: (float, numpy.ndarray): (skin-to-skin distance, normal vector) """ x = x0 - x1 d = length(x) r_tot = r0 + r1 h = d - r_tot if d == 0.0: n = np.zeros(2) else: n = x / d return h, n
def find_nearest_neighbors(position, sight, size_nearest_other, cell_indices, neigh_cells, points_indices, cells_count, cells_offset, obstacles): size = len(position) neighbors = np.full((size, size_nearest_other), fill_value=MISSING_NEIGHBOR, dtype=np.int64) '''Current nearest neighbours.''' distances = np.full((size, size_nearest_other), fill_value=sight, dtype=np.float64) '''Distance to current nearest neighbours.''' distances_max = np.full(size, fill_value=sight, dtype=np.float64) '''Distance to furthest neighbor.''' for i, j in iter_nearest_neighbors(cell_indices, neigh_cells, points_indices, cells_count, cells_offset): # Test if line of sight is obstructed by an obstacle if is_obstacle_between_points(position[i], position[j], obstacles): continue l = length(position[i] - position[j]) if l < distances_max[i]: set_neighbor(i, j, l, neighbors, distances, distances_max) if l < distances_max[j]: set_neighbor(j, i, l, neighbors, distances, distances_max) return neighbors
def validator(trait, value): l = length(value) if np.any(np.isclose(l, lengths)): return value else: raise TraitError( 'Expected an of length %s and got and array with length %s' % (lengths, l))
def test_agent_interaction(agent_type): simulation = TestAgentInteraction(agent_type=agent_type) agent_start = np.copy(simulation.agents.array) simulation.exit_condition = lambda s: s.data['iterations'] == 1000 simulation.run() agent_end = np.copy(simulation.agents.array) dist = length(agent_end[0]['position'] - agent_start[0]['position']) expected_dist = 8.0 assert dist >= expected_dist or np.isclose(dist, expected_dist)
def distance_circle_line(x, r, p0, p1): r""" Skin-to-Skin distance between circle and line Args: x (numpy.ndarray): r (float): p0 (numpy.ndarray): p1 (numpy.ndarray): Returns: (float, numpy.ndarray): (skin-to-skin distance, normal vector) """ # TODO: More docs d = p1 - p0 l_w = length(d) t_w = d / l_w n_w = rotate90(t_w) q0 = x - p0 q1 = x - p1 l_t = - dot(t_w, q1) - dot(t_w, q0) if l_t > l_w: d_iw = length(q0) n_iw = q0 / d_iw elif l_t < -l_w: d_iw = length(q1) n_iw = q1 / d_iw else: l_n = dot(n_w, q0) d_iw = np.abs(l_n) n_iw = np.sign(l_n) * n_w h_iw = d_iw - r return h_iw, n_iw
def agent_closer_to_exit(c_door, position): r"""Amount of positions (agents) closer to center of the exit. 1) Denote :math:`i` as indices of the positions :math:`\mathbf{x}`. 2) Distance from narrow exit can be estimated .. math:: d_i = \| \mathbf{c} - \mathbf{x}_{i} \| 3) By sorting the values :math:`d_i` by its indices we obtain array .. math:: a_i = \underset{i \in P}{\operatorname{arg\,sort}}(d_i) where - Values: Indices of the positions sorted by the distance from the exit - Indices: Number of positions closer to the exit 4) By sorting the values :math:`a_i` by it indices we obtain array .. math:: \lambda_i = \operatorname{arg\,sort} (a_i) where - Values: Number of positions closer to the exit - Indices: Indices of the positions Args: c_door (numpy.ndarray): Center of the exit :math:`\mathbf{c}`. position (numpy.ndarray): Positions :math:`\mathbf{x}` of the agents. Returns: numpy.ndarray: Array :math:`\lambda_i` """ distances = length(c_door - position) d_sorted = np.argsort(distances) num = np.argsort(d_sorted) return num
def agent_distance_condition(agent, start_index, i): """Test function for determining if agents are overlapping.""" condition = True if agent.three_circle: for j in range(start_index, i): if condition: t = agent_agent_distance_three_circle(agent, i, j) condition &= t[1] > 0 else: break else: for j in range(start_index, i): if condition: d = agent.position[i] - agent.position[j] s = length(d) - agent.radius[i] - agent.radius[j] condition &= s > 0 else: break return condition
def exit_detection(center_door, position, in_finlandia, obstacles, detection_range): """Exit detection. Detects closest exit in detection range that is in line of sight. Args: detection_range: center_door: position: out_finlandia: obstacles: Returns: ndarray: Selected exits. Array of indices denoting which exit was selected. If none was selected then value is set to `not_detected = -1`. """ not_detected = -1 n = len(position) distance = np.full(shape=n, fill_value=detection_range, dtype=np.float64) detected_exit = np.full(shape=n, fill_value=not_detected, dtype=np.int64) """Which exit has been detected by the agent if any.""" has_detected = np.zeros(shape=n, dtype=np.bool_) """False if agent has not detected an exit else True""" # Loop through all the agents for i in range(n): n_targets = 4 # Loop through the exits (others than fexits), calculate the distance to the exits, for which the line of # sight is not obstructed. for c in range(n_targets): # If line of sight is obstructed skip the exit if is_obstacle_between_points(position[i], center_door[c], obstacles): continue d = length(center_door[c] - position[i]) # distance to the midpoint of the exit, which is not obstructed if d < distance[i]: distance[i] = d detected_exit[i] = c #print("has_detected") #print(c) has_detected[i] = True return detected_exit, has_detected
def lines(origin, direction, lengths): """Lines Args: origin (numpy.ndarray): direction (numpy.ndarray): lengths (numpy.ndarray): """ n, m = origin.shape values = np.empty(shape=(2 * n, m)) for i in range(n): l = lengths[i] v_d = direction[i] if length(v_d) < 0.05: # Handle direction vector that is zero vector l = 0.05 v_d = np.array((1.0, 0.0)) values[2 * i, :] = origin[i, :] values[2 * i + 1, :] = origin[i, :] + normalize(v_d) * l return values
def exit_detection(center_door, position, obstacles, detection_range): """Exit detection. Detects closest exit in detection range that is in line of sight. Args: detection_range: center_door: position: obstacles: Returns: ndarray: Selected exits. Array of indices denoting which exit was selected. If none was selected then value is set to `not_detected = -1`. """ not_detected = -1 n = len(position) distance = np.full(shape=n, fill_value=detection_range, dtype=np.float64) detected_exit = np.full(shape=n, fill_value=not_detected, dtype=np.int64) """Which exit has been detected by the agent if any.""" has_detected = np.zeros(shape=n, dtype=np.bool_) """False if agent has not detected an exit else True""" for i in range(n): for c in range(len(center_door)): # If line of sight is obstructed skip the exit if is_obstacle_between_points(position[i], center_door[c], obstacles): continue d = length(center_door[c] - position[i]) if d < distance[i]: distance[i] = d detected_exit[i] = c has_detected[i] = True return detected_exit, has_detected
def exit_capacity(points, agent_radius): """Capacity of narrow exit.""" door_radius = length(points[1] - points[0]) / 2.0 capacity = door_radius // agent_radius return capacity
def generate_leader_pos(self, cell, n_lead): # FIRST THE DATA HAS TO BE CREATED # Load data of followers followers = np.load('complex/spawn_complex.npy') follower_positions = followers['position'] follower_radii = followers['radius'] # Minimal radius of a guide (the same value given in agents.py to the guides). max_r = 0.27 # Number of times spawned leaders are allowed to overlap each other before the program is # terminated. overlaps = 10000 # Import Complex floor field field = ComplexFloor().field # Bound box representing the room. width = 90 height = 90 # Create a grid structure over the room geometry. # Cell size in the grid, determines the resolution of the micro-macro converted data cell_size = 2 m = np.round(width / cell_size) n = np.round(height / cell_size) m = m.astype(int) n = n.astype(int) X = np.linspace(0, width, m + 1) Y = np.linspace(0, height, n + 1) hlines = [((x1, yi), (x2, yi)) for x1, x2 in zip(X[:-1], X[1:]) for yi in Y] vlines = [((xi, y1), (xi, y2)) for y1, y2 in zip(Y[:-1], Y[1:]) for xi in X] grids = list(polygonize(MultiLineString(hlines + vlines))) # Leaders' spawn areas leader_spawns = [] # Leader's spawn points spawn_points = [] # Loop through the cells and calculate intersections with spawn areas. for i in range(n_lead): poly = field.domain.intersection(grids[cell[i]]) if not poly.is_empty: leader_spawns.append(poly) # Import obstacles obstacles = field.obstacles # Spawn a random position from the starting area. # Loop through all the leaders. # (1) Take into account that there might be obstacles in the spawn areas, and take also # into account that agents have a buffer radius. # (2) If the spawn area is a MultiPolygon, loop through the polygons in a MultiPolygon. Create a # mesh grid of the spawn area with Delaunay triangulation. # (2.1) Spawn a random point from the mesh grid. # (2.2) Check that the position doesn't interfere with other agents' positions # (2.3) Set the Boolean value for if the leader is initially inside the Finlandiahall # (this is needed for the movement simulation). # (3) If the spawn area is not a MultiPolygon, just directly create a mesh grid of the spawn area # with Delaunay triangulation. # (3.1) Spawn a random point from the mesh grid. # (3.2) Check that the position doesn't interfere with other agents' positions # (3.3) Set the Boolean value for if the leader is initially inside the Finlandiahall (this is # is needed for the movement simulation). for i in range(n_lead): seed = 0 # (1) n_spawnpoints = len(spawn_points) geom = leader_spawns[i] - obstacles.buffer(max_r) j = 0 # set overlaps counter to zero # (2) if isinstance(geom, MultiPolygon): n_polygons = len(geom) for j in range(n_polygons): vertices = np.asarray(geom[j].convex_hull.exterior) delaunay = Delaunay(vertices) mesh = vertices[delaunay.simplices] if j == 0: meshes = mesh else: meshes = np.concatenate((mesh, meshes), axis=0) # Computes cumulative sum of the areas of the triangle mesh. weights = triangle_area_cumsum(meshes) weights /= weights[-1] while j < overlaps: seed += 1 distances = [] # temporarily store distances from the spawned point to the previously spawned n_overlaps = 0 # for each attempt to position the guide, set number of overlaps to zero # (2.1) Spawn a random point for the guide. np.random.seed(seed) x = np.random.random() k = np.searchsorted(weights, x) a, b, c = meshes[k] spawn_point = random_sample_triangle(a, b, c) # spawn_point = random_sample_triangle(a, b, c, seed) # (2.2) if n_spawnpoints != 0: # if there are no other spawned guides skip this step for k in range(0, n_spawnpoints): d = length(spawn_point - spawn_points[k]) h = d - 2 * max_r distances.append(h) distances_array = distances distances_array = np.asarray(distances_array) n_overlaps += len(np.where(distances_array < 0)[0]) for obstacle in obstacles: obstacle = list(obstacle.coords) n_obstacle_points = len(obstacle) for k in range(0, n_obstacle_points): if k == n_obstacle_points - 1: h, _ = distance_circle_line(spawn_point, max_r, np.asarray(obstacle[k]), np.asarray(obstacle[0])) else: h, _ = distance_circle_line(spawn_point, max_r, np.asarray(obstacle[k]), np.asarray(obstacle[k + 1])) if h < 0.0: n_overlaps += 1 for agent in range(len(follower_radii)): h, _ = distance_circles(follower_positions[agent], follower_radii[agent], spawn_point, max_r) if h < 0.0: n_overlaps += 1 if n_overlaps == 0: # (2.3) # Append the point to spawn points spawn_points.append([spawn_point[0], spawn_point[1]]) # print("Guide spawned") # sys.stdout.flush() break j += 1 if j == overlaps: raise Exception('Leaders do not fit in the cell') # (3) else: vertices = np.asarray(geom.convex_hull.exterior) delaunay = Delaunay(vertices) mesh = vertices[delaunay.simplices] weights = triangle_area_cumsum(mesh) weights /= weights[-1] while j < overlaps: seed += 1 distances = [] # temporarily store distances from the spawned point to the previously spawned n_overlaps = 0 # for each attempt to position the guide, set number of overlaps to zero # (3.1) Spawn a random point for the guide np.random.seed(seed) x = np.random.random() k = np.searchsorted(weights, x) a, b, c = mesh[k] spawn_point = random_sample_triangle(a, b, c) # spawn_point = random_sample_triangle(a, b, c, seed) if n_spawnpoints != 0: for k in range(0, n_spawnpoints): d = length(spawn_point - spawn_points[k]) h = d - 2 * max_r distances.append(h) distances_array = distances distances_array = np.asarray(distances_array) n_overlaps += len(np.where(distances_array < 0)[0]) for obstacle in obstacles: obstacle = list(obstacle.coords) n_obstacle_points = len(obstacle) for k in range(0, n_obstacle_points): if k == n_obstacle_points - 1: h, _ = distance_circle_line(spawn_point, max_r, np.asarray(obstacle[k]), np.asarray(obstacle[0])) else: h, _ = distance_circle_line(spawn_point, max_r, np.asarray(obstacle[k]), np.asarray(obstacle[k + 1])) if h < 0.0: n_overlaps += 1 for agent in range(len(follower_radii)): h, _ = distance_circles(follower_positions[agent], follower_radii[agent], spawn_point, max_r) if h < 0.0: n_overlaps += 1 if n_overlaps == 0: # (3.3) # Append the point to spawn points spawn_points.append([spawn_point[0], spawn_point[1]]) # print("Guide spawned") # sys.stdout.flush() break j += 1 if j == overlaps: raise Exception('Leaders do not fit in the cell') return spawn_points
def adaptive_timestep(agents, dt_min, dt_max): r""" Timestep is selected from interval :math:`[\Delta t_{min}, \Delta t_{max}]` by bounding the maximum step size :math:`\Delta x` an agent can take per iteration cycle, obtained from .. math:: \Delta x = c \Delta t_{max} \max_{i\in A} v_i^0 \\ where - :math:`c > 0` is scaling coefficient - :math:`v_i^0` is agent's target velocity - :math:`\max_{i\in A} v_i^0` is the maximum of all target velocities Timestep is then obtained from .. math:: \Delta t_{mid} &= \frac{\Delta x}{\max_{i \in A} v_i} \\ \Delta t &= \begin{cases} \Delta t_{min} & \Delta t_{mid} < \Delta t_{min} \\ \Delta t_{mid} & \\ \Delta t_{max} & \Delta t_{mid} > \Delta t_{max} \\ \end{cases} where - :math:`v_i` is agent's current velocity Args: dt_min: Minimum timestep :math:`\Delta x_{min}` for adaptive integration. dt_max: Maximum timestep :math:`\Delta x_{max}` for adaptive integration. velocity: target_velocity: Returns: float: References https://en.wikipedia.org/wiki/Adaptive_stepsize """ v_max = 0.0 for agent in agents: l = length(agent['velocity']) if l > v_max: v_max = l if v_max == 0.0: return dt_max c = 1.1 dx_max = c * np.max(agents[:]['target_velocity']) * dt_max dt = dx_max / v_max if dt > dt_max: return dt_max elif dt < dt_min: return dt_min else: return dt
def test_unit_vectors(v): assert np.isclose(length(v), 1.0)
while k < overlaps: seed += 1 distances = [ ] # temporarily store distances from the spawned point to the previously spawned # During a single spawn, the number of times the guide overlaps with an obstacle/guide n_overlaps = 0 # Spawn a random point for the guide. x = np.random.random(seed) rand_triangle = np.searchsorted(weights, x) a, b, c = meshes[rand_triangle] spawn_point = random_sample_triangle(a, b, c) #print(spawn_point) if n_spawnpoints != 0: # if there are no other spawned guides skip this step for l in range(0, n_spawnpoints): d = length(spawn_point - spawn_points[l]) h = d - 2 * max_r distances.append(h) distances_array = distances distances_array = np.asarray(distances_array) n_overlaps += len(np.where(distances_array < 0)[0]) for obstacle in obstacles: obstacle = list(obstacle.coords) n_obstacle_points = len(obstacle) for l in range(0, n_obstacle_points): if l == n_obstacle_points - 1: h, _ = distance_circle_line( spawn_point, max_r, np.asarray(obstacle[l]), np.asarray(obstacle[0])) else: h, _ = distance_circle_line(
def leader_follower_interaction_brute(is_follower, is_leader, position, velocity, weight_position, phi, target, index_leader, obstacles, sight): has_strategy = np.zeros(len(position), dtype=np.bool_) new_direction = np.zeros_like(position) # new_target = np.zeros_like(target) # new_index_leader = np.zeros_like(index_leader) indices = np.arange(len(position)) leaders = indices[is_leader] distances = np.zeros(leaders.shape, dtype=np.float64) # Iterate over followers for i in indices[is_follower]: behind_obstacle = 0 heading_away = 0 # Find distances to the leaders for k, j in enumerate(leaders): distances[k] = length(position[i] - position[j]) # Iterate over leaders in orders from closest to furthest leader # and choose a strategy. for k in np.argsort(distances): if distances[k] > sight: continue j = leaders[k] if is_obstacle_between_points(position[i], position[j], obstacles): # We are not seeing this leader. leader = index_leader[i] # Check if we were following this leader before. if leader != NO_LEADER and leader == j: behind_obstacle += 1 # Use navigation of this leader. target[i] = target[leader] has_strategy[i] = True break else: continue else: is_heading_away, _ = herding_relationship( position[i], position[j], velocity[i], velocity[j], phi) if is_heading_away: heading_away += 1 # We see this leader. Remember it and follow. index_leader[i] = j target[i] = NO_TARGET new_direction[i, :] = normalize( weighted_average( normalize(position[j, :] - position[i, :]), normalize(velocity[j, :]), weight_position)) has_strategy[i] = True break if behind_obstacle == 0 and heading_away == 0: leader = index_leader[i] if leader != NO_LEADER: target[i] = target[leader] has_strategy[i] = True return new_direction, has_strategy
def test_length(a): ans = length(a) assert isinstance(ans, float) assert ans >= 0
def test_length_vec(a): ans = length(a) assert isinstance(ans, np.ndarray) assert np.all(ans >= 0)