def _get_force(self):
lambda_importance = self.config("lambda_importance", 2.0)
gamma = self.config("gamma", 0.35)
n = self.config("n", 2)
n_prime = self.config("n_prime", 3)
pos_diff = stateutils.each_diff(self.peds.pos()) # n*(n-1)x2 other - self
diff_direction, diff_length = stateutils.normalize(pos_diff)
vel_diff = -1.0 * stateutils.each_diff(self.peds.vel()) # n*(n-1)x2 self - other
# compute interaction direction t_ij
interaction_vec = lambda_importance * vel_diff + diff_direction
interaction_direction, interaction_length = stateutils.normalize(interaction_vec)
# compute angle theta (between interaction and position difference vector)
theta = stateutils.vector_angles(interaction_direction) - stateutils.vector_angles(
diff_direction
)
# compute model parameter B = gamma * ||D||
B = gamma * interaction_length
force_velocity_amount = np.exp(-1.0 * diff_length / B - np.square(n_prime * B * theta))
force_angle_amount = -np.sign(theta) * np.exp(
-1.0 * diff_length / B - np.square(n * B * theta)
)
force_velocity = force_velocity_amount.reshape(-1, 1) * interaction_direction
force_angle = force_angle_amount.reshape(-1, 1) * stateutils.left_normal(
interaction_direction
)
force = force_velocity + force_angle # n*(n-1) x 2
force = np.sum(force.reshape((self.peds.size(), -1, 2)), axis=1)
return force * self.factor
def _get_force(self):
threshold = self.config("threshold", 0.5)
forces = np.zeros((self.peds.size(), 2))
if self.peds.has_group():
for group in self.peds.groups:
size = len(group)
member_pos = self.peds.pos()[group, :]
diff = stateutils.each_diff(member_pos) # others - self
_, norms = stateutils.normalize(diff)
diff[norms > threshold, :] = 0
# forces[group, :] += np.sum(diff, axis=0)
forces[group, :] += np.sum(diff.reshape((size, -1, 2)), axis=1)
return forces * self.factor