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))