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)
