def rotational_euler(agents, dt):
"""Update rotational motion using Euler's method"""
for agent in agents:
angular_acceleration = agent['torque'] / agent['inertia_rot']
agent['orientation'] += agent['angular_velocity'] * dt + \
angular_acceleration / 2 * dt ** 2
agent['angular_velocity'] += angular_acceleration * dt
agent['orientation'] = wrap_to_pi(agent['orientation'])
def test_wrap_to_pi(phi):
ans = wrap_to_pi(phi)
assert isinstance(ans, float)
assert -np.pi <= ans <= np.pi
if (phi + np.pi) % (2 * np.pi) == 0.0:
if phi > 0:
assert ans == np.pi
else:
assert ans == -np.pi
def rotational_verlet(agents, dt):
"""Rotational motion using velocity verlet method"""
for agent in agents:
old_angular_acceleration = agent['torque_prev'] / agent['inertia_rot']
new_angular_acceleration = agent['torque'] / agent['inertia_rot']
agent['torque_prev'] = agent['torque']
agent['angular_velocity'] += (old_angular_acceleration +
new_angular_acceleration) / 2 * dt
agent['orientation'] += agent['angular_velocity'] * dt + \
new_angular_acceleration / 2 * dt ** 2
agent['orientation'] = wrap_to_pi(agent['orientation'])
def torque_adjust(inertia_rot, tau_rot, phi_0, phi, omega_0, omega):
r"""Adjusting torque accounts for agent's desire to rotate it orientation.
.. math::
M_{adj} = \frac{I_{rot}}{\tau_{rot}} \left( \omega_{0} \left (
\frac{\varphi - \varphi_{0}}{\pi} \right ) - \omega\right),
Angular difference :math:`\varphi - \varphi_{0}` is wrapped between interval
:math:`[-\pi, \pi]` so that division by :math:`\pi` returns value between
:math:`[-1, 1]`. This gives direction and magnitude for the torque.
Args:
inertia_rot (float):
Rotational inertia :math:`I_{rot}`
tau_rot (float):
Characteristic time :math:`\tau_{rot}` time for agent to adjust it
orientation.
phi_0 (float):
Target orientation :math:`\varphi_{0}`. In low and medium crowd
densities the angle of the target direction can be sufficient for
target orientation. In high crowd densities agents may twist their
body differently for example to try to squeeze through narrow
spaces, requiring more sophisticated algorithms.
phi (float):
Current orientation :math:`\varphi`
omega_0 (float):
Maximum angular velocity :math:`\omega_{0}`.
omega (float):
Angular velocity :math:`\omega`
Returns:
float: Adjusting torque scalar :math:`M_{adj}`
"""
return inertia_rot / tau_rot * \
(wrap_to_pi(phi_0 - phi) / np.pi * omega_0 - omega)