def cal_perpendicular(si, ei, sj, ej):
"""
:param si:
:param ei:
:param sj:
:param ej:
:return:
"""
sisj = to_vec_sub(sj, si)
siei = to_vec_sub(ei, si)
siej = to_vec_sub(ej, si)
_base = to_vec_dot(siei, siei)
if _base == 0:
return np.spacing(1)
u1 = to_vec_dot(sisj, siei) / _base
u2 = to_vec_dot(siej, siei) / _base
ps = to_vec_add(si, to_vec_times(u1, siei))
pe = to_vec_add(si, to_vec_times(u2, siei))
lp1 = vlen(ps, sj)
lp2 = vlen(pe, ej)
if lp1 + lp2 == 0:
outD = 0
else:
outD = (lp1**2 + lp2**2) / (lp1 + lp2)
return outD
def lttilde(t_traj, start_ind, curr_ind):
"""cal L(T|T~)
:param t_traj:
:param start_ind:
:param curr_ind:
:return:
"""
score1 = 0
score2 = 0
for j in range(start_ind, curr_ind):
if t_traj[start_ind] == t_traj[j] and t_traj[curr_ind] == t_traj[j +
1]:
continue
# 更长的放前面
if vlen(t_traj[start_ind], t_traj[curr_ind]) > vlen(
t_traj[j], t_traj[j + 1]):
presult = cal_perpendicular(t_traj[start_ind], t_traj[curr_ind],
t_traj[j], t_traj[j + 1])
aresult = angular(t_traj[start_ind], t_traj[curr_ind], t_traj[j],
t_traj[j + 1])
else:
presult = cal_perpendicular(t_traj[j], t_traj[j + 1],
t_traj[start_ind], t_traj[curr_ind])
aresult = angular(t_traj[j], t_traj[j + 1], t_traj[start_ind],
t_traj[curr_ind])
score1 += presult
score2 += aresult
score1 = np.log2(score1) if score1 != 0 else 0
score2 = np.log2(score2) if score2 != 0 else 0
return score1 + score2
def calculate_avg_speed(self):
"""
Calculates average speed of pedestrian projecting their actual position on destination vector.
"""
pos = self._agent.position - self._agent.initial_position
dest = self._agent.dest - self._agent.initial_position
proj_dist = utils.vproject(pos, dest)
if self._agent.time == 0:
avg_speed = utils.vlen(self.velocity)
else:
avg_speed = utils.vlen(proj_dist) / self._agent.time
return avg_speed
def calculate_avg_speed(self):
"""
Calculates average speed of pedestrian projecting their actual position on destination vector.
"""
pos = self._agent.position - self._agent.initial_position
dest = self._agent.dest - self._agent.initial_position
proj_dist = utils.vproject(pos, dest)
if self._agent.time == 0:
avg_speed = utils.vlen(self.velocity)
else:
avg_speed = utils.vlen(proj_dist) / self._agent.time
return avg_speed
def _calculate_wall_repulsion(self, segments):
total_force = np.array([0, 0])
end_points = None
for segment in segments:
closest = \
utils.closest_on_segment(self._agent.position,
segment)
if utils.vlen(closest - self._agent.position) \
> self.WALL_DIST_TOL:
continue
rep_potential = np.array([0, 0])
if end_points is None:
rep_potential = \
self._calculate_wall_rep_potential(closest)
end_points = np.array(closest)
elif not utils.check_point_repetition(closest,
end_points):
rep_potential = \
self._calculate_wall_rep_potential(closest)
end_points = np.vstack([end_points, closest])
total_force = total_force + rep_potential
return total_force
def _calculate_reaction_time(self, reaction_time):
nearest_src = utils.find_nearest_segment(self._position,
self._tunnel.fire_sources)
src_mid = nearest_src.start + utils.vmiddle(nearest_src.end -
nearest_src.start)
dist = utils.vlen(src_mid - self._position)
self._reaction_time = self.FS_DIST_COEFF * dist + reaction_time
def _set_checkpoint(self):
if self._checkpoint is not None and utils.vlen(self._checkpoint
- self.position) < self.CHECKPOINT_TOL \
or self._checkpoint is None:
self._find_nearest_end()
self._find_checkpoint()
def _calculate_reaction_time(self, reaction_time):
nearest_src = utils.find_nearest_segment(self._position,
self._tunnel.fire_sources)
src_mid = nearest_src.start + utils.vmiddle(nearest_src.end
- nearest_src.start)
dist = utils.vlen(src_mid - self._position)
self._reaction_time = self.FS_DIST_COEFF * dist + reaction_time
def _calculate_wall_repulsion(self, segments):
total_force = np.array([0, 0])
end_points = None
for segment in segments:
closest = \
utils.closest_on_segment(self._agent.position,
segment)
if utils.vlen(closest - self._agent.position) \
> self.WALL_DIST_TOL:
continue
rep_potential = np.array([0, 0])
if end_points is None:
rep_potential = \
self._calculate_wall_rep_potential(closest)
end_points = np.array(closest)
elif not utils.check_point_repetition(closest, end_points):
rep_potential = \
self._calculate_wall_rep_potential(closest)
end_points = np.vstack([end_points, closest])
total_force = total_force + rep_potential
return total_force
def _set_checkpoint(self):
if self._checkpoint is not None and utils.vlen(self._checkpoint
- self.position) < self.CHECKPOINT_TOL \
or self._checkpoint is None:
self._find_nearest_end()
self._find_checkpoint()
def lt(t_traj, start_ind, curr_ind):
"""cal L(T~)
:param t_traj:
:param start_ind:
:param curr_ind:
:return:
"""
vlength = vlen(t_traj[start_ind], t_traj[curr_ind])
return np.log2(vlength) if vlength else np.spacing(1)
def _calculate_wall_rep_potential(self, closest):
target_vector = closest - self._agent.position
dist = utils.vlen(target_vector)
target_dir = utils.vnormalize(target_vector)
target_dir = self._rotate_direction_vetor(target_dir, closest)
return np.exp(-dist / self._agent.radius) * target_dir
def _calculate_wall_rep_potential(self, closest):
target_vector = closest - self._agent.position
dist = utils.vlen(target_vector)
target_dir = utils.vnormalize(target_vector)
target_dir = self._rotate_direction_vetor(target_dir,
closest)
return np.exp(-dist / self._agent.radius) * target_dir
def _calculate_soc_rep_potential(self, agent):
target_vector = agent.position - self._agent.position
dist = utils.vlen(target_vector)
target_dir = utils.vnormalize(target_vector)
target_dir = self._rotate_direction_vetor(target_dir,
agent.position)
return np.exp(agent.radius + self._agent.radius - dist
/ self.SOCIAL_DIST_COEFF) * target_dir
def _calculate_soc_rep_potential(self, agent):
target_vector = agent.position - self._agent.position
dist = utils.vlen(target_vector)
target_dir = utils.vnormalize(target_vector)
target_dir = self._rotate_direction_vetor(target_dir,
agent.position)
return np.exp(agent.radius + self._agent.radius -
dist / self.SOCIAL_DIST_COEFF) * target_dir
def _remove_rescued(self):
rescued = []
for agent in self._agents:
if utils.vlen(agent.position - agent.dest) < self.DEST_TOL:
rescued.append(agent)
for rescued_agent in rescued:
self._agents.remove(rescued_agent)
self._rescued.extend(rescued)
def _remove_rescued(self):
rescued = []
for agent in self._agents:
if utils.vlen(agent.position - agent.dest) < self.DEST_TOL:
rescued.append(agent)
for rescued_agent in rescued:
self._agents.remove(rescued_agent)
self._rescued.extend(rescued)
def _calculate_social_repulsion(self, agents):
total_force = np.array([0, 0])
for agent in agents:
if agent != self._agent and utils.vlen(agent.position
- self._agent.position) < self.SOCIAL_DIST_TOL:
angle_coeff = self._calculate_angle_coeff(agent)
rep_potential = \
self._calculate_soc_rep_potential(agent)
total_force = total_force + angle_coeff \
* rep_potential
return total_force
def actions(self, state):
"""
les actions sont déterminées en retournant les possibilitiés qui
manquent simultanément dans une ligne, colonne et grille correspondantes
à une case.
La position de la case et la nouvelle valeur possible est retournée sous
forme d'un triplet (i, j, k).
"""
state = numpify_state(state)
for i, j in zip(*np.where(vlen(state) > 1)):
for k in state[i, j]:
yield i, j, k
def _calculate_social_repulsion(self, agents):
total_force = np.array([0, 0])
for agent in agents:
if agent != self._agent and utils.vlen(
agent.position -
self._agent.position) < self.SOCIAL_DIST_TOL:
angle_coeff = self._calculate_angle_coeff(agent)
rep_potential = \
self._calculate_soc_rep_potential(agent)
total_force = total_force + angle_coeff \
* rep_potential
return total_force
