def test_from_ypr(self):
y = 1.2
p = 0.3
r = -1.0
# XXX
rot = RotationD(y, p, r)
ry, rp, rr = rot.yaw_pitch_roll()
nose.tools.assert_almost_equal(y, ry, 14)
nose.tools.assert_almost_equal(p, rp, 14)
nose.tools.assert_almost_equal(r, rr, 14)
# 0XX
rot = RotationD(0, p, r)
ry, rp, rr = rot.yaw_pitch_roll()
nose.tools.assert_almost_equal(0, ry, 14)
nose.tools.assert_almost_equal(p, rp, 14)
nose.tools.assert_almost_equal(r, rr, 14)
# X0X
rot = RotationD(y, 0, r)
ry, rp, rr = rot.yaw_pitch_roll()
nose.tools.assert_almost_equal(y, ry, 14)
nose.tools.assert_almost_equal(0, rp, 14)
nose.tools.assert_almost_equal(r, rr, 14)
# XX0
rot = RotationD(y, p, 0)
ry, rp, rr = rot.yaw_pitch_roll()
nose.tools.assert_almost_equal(y, ry, 14)
nose.tools.assert_almost_equal(p, rp, 14)
nose.tools.assert_almost_equal(0, rr, 14)
# 00X
rot = RotationD(0, 0, r)
ry, rp, rr = rot.yaw_pitch_roll()
nose.tools.assert_almost_equal(0, ry, 14)
nose.tools.assert_almost_equal(0, rp, 14)
nose.tools.assert_almost_equal(r, rr, 14)
# 0X0
rot = RotationD(0, p, 0)
ry, rp, rr = rot.yaw_pitch_roll()
nose.tools.assert_almost_equal(0, ry, 14)
nose.tools.assert_almost_equal(p, rp, 14)
nose.tools.assert_almost_equal(0, rr, 14)
# X00
rot = RotationD(y, 0, 0)
ry, rp, rr = rot.yaw_pitch_roll()
nose.tools.assert_almost_equal(y, ry, 14)
nose.tools.assert_almost_equal(0, rp, 14)
nose.tools.assert_almost_equal(0, rr, 14)
# 000
rot = RotationD(0, 0, 0)
ry, rp, rr = rot.yaw_pitch_roll()
nose.tools.assert_almost_equal(0, ry, 14)
nose.tools.assert_almost_equal(0, rp, 14)
nose.tools.assert_almost_equal(0, rr, 14)
def test_to_ypr(self):
# ypr identity: (pi/2, 0, pi)
ident_ypr = (math.pi / 2, 0, -math.pi)
rot_d = RotationD()
rot_f = RotationF()
numpy.testing.assert_almost_equal(rot_d.yaw_pitch_roll(), ident_ypr, 15)
numpy.testing.assert_almost_equal(rot_f.yaw_pitch_roll(), ident_ypr)
