def test_spherical_coordinates(self):
pos = RoboticTrackball.spherical_to_cartesian(1,
np.pi / 2,
np.pi / 4,
target=np.zeros(3))
d, a, e = RoboticTrackball.cartesian_to_spherical(pos,
target=np.zeros(3))
self.assertAlmostEqual(d, 1.)
self.assertAlmostEqual(a, np.pi / 2)
self.assertAlmostEqual(e, np.pi / 4)
def test_spherical_coordinates_with_target(self):
target = np.array([1, 2, 3])
pos = RoboticTrackball.spherical_to_cartesian(1,
np.pi / 2,
np.pi / 4,
target=target)
d, a, e = RoboticTrackball.cartesian_to_spherical(pos, target=target)
self.assertAlmostEqual(d, 1.)
self.assertAlmostEqual(a, np.pi / 2)
self.assertAlmostEqual(e, np.pi / 4)
def test_rotate(self):
pose = np.eye(4)
pose[:3,
3] = RoboticTrackball.spherical_to_cartesian(3.,
0.,
np.deg2rad(45.),
target=np.zeros(3))
pose[:3, :3] = RoboticTrackball.look_at_rotation(eye=pose[:3, 3],
target=np.zeros(3),
up=[0, 0, 1])
trackball = RoboticTrackball(pose, (640, 480), 1.)
trackball.set_state(RoboticTrackball.STATE_ROTATE)
trackball.drag([10, 20]) # 10px in azimuth, 20px in elevation
npose = trackball._n_pose
d, a, e = RoboticTrackball.cartesian_to_spherical(
npose[:3, 3], trackball._target)
self.assertAlmostEqual(d, 3.)
self.assertAlmostEqual(a, 0. - 10 * 5e-1 / (0.3 * 480))
self.assertAlmostEqual(e, np.deg2rad(45.) + 20 * 5e-1 / (0.3 * 480))