def test_homtrans(self):
#3D
T = trotx(pi/2, t=[1,2,3])
v = [10,12,14]
v2 = homtrans(T, v)
nt.assert_almost_equal(v2, [11, -12, 15])
v = np.c_[[10,12,14], [-3,-4,-5]]
v2 = homtrans(T, v)
nt.assert_almost_equal(v2, np.c_[[11, -12, 15], [-2,7,-1]])
T = trotx(pi/2, t=[1,2,3])
v = [10,12,14,1]
v2 = homtrans(T, v)
nt.assert_almost_equal(v2, [11, -12, 15,1])
v = np.c_[[10,12,14,1], [-3,-4,-5,1]]
v2 = homtrans(T, v)
nt.assert_almost_equal(v2, np.c_[[11, -12, 15,1], [-2,7,-1,1]])
#2D
T = trot2(pi/2, t=[1,2])
v = [10,12]
v2 = homtrans(T, v)
nt.assert_almost_equal(v2, [-11, 12])
v = np.c_[[10,12], [-3,-4]]
v2 = homtrans(T, v)
nt.assert_almost_equal(v2, np.c_[[-11, 12], [5, -1]])
T = trot2(pi/2, t=[1,2])
v = [10,12,1]
v2 = homtrans(T, v)
nt.assert_almost_equal(v2, [-11, 12, 1])
v = [20,24,2]
v2 = homtrans(T, v)
nt.assert_almost_equal(v2, [-22, 24, 2])
v = np.c_[[10,12, 1], [-3,-4, 1]]
v2 = homtrans(T, v)
nt.assert_almost_equal(v2, np.c_[[-11, 12, 1], [5, -1, 1]])
with self.assertRaises(ValueError):
T = trotx(pi/2, t=[1,2,3])
v = [10,12]
v2 = homtrans(T, v)
def test_tr2rt(self):
# 3D
T = trotx(0.3, t=[1, 2, 3])
R, t = tr2rt(T)
nt.assert_array_almost_equal(T[:3, :3], R)
nt.assert_array_almost_equal(T[:3, 3], t)
# 2D
T = trot2(0.3, t=[1, 2])
R, t = tr2rt(T)
nt.assert_array_almost_equal(T[:2, :2], R)
nt.assert_array_almost_equal(T[:2, 2], t)
with self.assertRaises(ValueError):
R, t = tr2rt(3)
with self.assertRaises(ValueError):
R, t = tr2rt(np.eye(3, 4))
def test_t2r(self):
# 3D
t = [1, 2, 3]
T = trotx(0.3, t=t)
R = t2r(T)
nt.assert_array_almost_equal(T[:3, :3], R)
nt.assert_array_almost_equal(transl(T), np.array(t))
# 2D
t = [1, 2]
T = trot2(0.3, t=t)
R = t2r(T)
nt.assert_array_almost_equal(T[:2, :2], R)
nt.assert_array_almost_equal(transl2(T), np.array(t))
with self.assertRaises(ValueError):
t2r(3)
with self.assertRaises(ValueError):
r2t(np.eye(3, 4))