def test_get_distance(self):
n = 100
ps = 100 * q.mm
p = np.linspace(0, np.pi, n)
sin = np.sin(p)
cos = np.cos(p)
zeros = np.zeros(n)
# Simple body
# -----------
traj = Trajectory(zip(cos, sin, zeros) * q.m, velocity=1 * q.m / q.s)
ball = MetaBall(traj, .5 * q.m)
ball.bind_trajectory(ps)
dist = ball.get_distance(0 * q.s, ball.trajectory.time)
# Maximum along the x axis where the body travels 2 m by translation and rotates by 180
# degrees compared to position at t0, so the rotational displacement is 2 * furthest point,
# in this case 2 m, so altogether 4 m
self.assertAlmostEqual(dist.simplified.magnitude, 4)
# Composite body
# --------------
traj_m = Trajectory([(0, 0, 0)] * q.m)
ball = MetaBall(traj_m, .5 * q.m)
comp = CompositeBody(traj, bodies=[ball])
comp.bind_trajectory(ps)
d = comp.get_distance(0 * q.s, comp.time).simplified.magnitude
# 2 by translation and 180 degrees means 2 * furthest
gt = 2 * ball.furthest_point.simplified.magnitude + 2
self.assertAlmostEqual(gt, d, places=4)
d = comp.get_distance(0 * q.s, comp.time / 2).simplified.magnitude
# 1 by translation by either x or y and sqrt(2) * furthest by rotation
gt = 1 + comp.furthest_point.simplified.magnitude * np.sqrt(2)
self.assertAlmostEqual(gt, d, places=4)
def test_get_next_time(self):
n = 100
ps = 100 * q.mm
psm = ps.simplified.magnitude
p = np.linspace(0, np.pi, n)
sin = np.sin(p) * 1e-3
cos = np.cos(p) * 1e-3
zeros = np.zeros(n)
traj_m_0 = Trajectory(zip(p * 1e-3, zeros, zeros) * q.m,
velocity=1 * q.mm / q.s)
traj_m_1 = Trajectory([(0, 0, 0)] * q.m)
ball_0 = MetaBall(traj_m_0, .5 * q.m)
ball_1 = MetaBall(traj_m_1, .5 * q.m)
traj = Trajectory(zip(cos, sin, zeros) * q.m, velocity=1 * q.mm / q.s)
comp = CompositeBody(traj, bodies=[ball_0, ball_1])
dt = comp.get_maximum_dt(ps)
# Normal trajectories
# Test the beginning, middle and end because of time complexity
for t_0 in [0 * q.s, comp.time / 2, comp.time - 10 * dt]:
t_1 = comp.get_next_time(t_0, ps)
d = comp.get_distance(t_0, t_1).simplified.magnitude
np.testing.assert_almost_equal(psm, d)
# Trajectories which sum up to no movement
traj_m = Trajectory(zip(zeros, -p, zeros) * q.m,
velocity=1 * q.mm / q.s)
ball = MetaBall(traj_m, .5 * q.m)
traj = Trajectory(zip(p, zeros, zeros) * q.m, velocity=1 * q.mm / q.s)
comp = CompositeBody(traj, bodies=[ball])
self.assertEqual(np.inf * q.s, comp.get_next_time(0 * q.s, ps))