def test_ang_dist():
o1 = m3d.Orientation()
o1.rotate_yb(1)
o1.rotate_xb(1)
o1.rotate_zb(1)
o2 = m3d.Orientation()
o2.rotate_yb(2)
o2.rotate_xb(2)
o2.rotate_zb(2)
o_dist_m3d = o1.ang_dist(o2)
o_inv_dist_m3d = o2.ang_dist(o1)
o1_m = math3d.Orientation()
o1_m.rotate_yb(1)
o1_m.rotate_xb(1)
o1_m.rotate_zb(1)
o2_m = math3d.Orientation()
o2_m.rotate_yb(2)
o2_m.rotate_xb(2)
o2_m.rotate_zb(2)
o_dist_math3d = o1_m.ang_dist(o2_m)
o_inv_dist_math3d = o2_m.ang_dist(o1_m)
print('o_inv_dist_m3d', o_inv_dist_m3d)
print('o_inv_dist_math3d', o_inv_dist_math3d)
print('o_dist_m3d', o_dist_m3d)
print('o_dist_math3d', o_dist_math3d)
assert abs(o_dist_m3d - o_dist_math3d) <= m3d.float_eps
assert abs(o_inv_dist_m3d - o_inv_dist_math3d) <= m3d.float_eps
def test_orient_except():
with pytest.raises(ValueError):
o = m3d.Orientation(np.identity(4))
with pytest.raises(ValueError):
o = m3d.Orientation("whatever")
o = m3d.Orientation()
with pytest.raises(ValueError):
o * np.identity(4)
def test_rotation_vector_2():
o = m3d.Orientation()
o.rotate_yb(1.1)
v = o.to_rotation_vector()
assert _are_equals(v[1], 1.1)
o = m3d.Orientation()
o.rotate_zb(1.1)
v = o.to_rotation_vector()
assert _are_equals(v[2], 1.1)
o = m3d.Orientation()
o.rotate_xb(1.1)
v = o.to_rotation_vector()
assert _are_equals(v[0], 1.1)
def test_rotation_vector():
o = m3d.Orientation()
o.rotate_yb(1)
o.rotate_zb(1)
v = o.to_rotation_vector()
o2 = m3d.Orientation.from_rotation_vector(v)
assert o.similar(o2, CONVERTION_ERROR)
def test_eq():
t = m3d.Transform()
t.orient.rotate_yb(1)
t.orient.rotate_zb(1)
t.pos.x = 1
v = m3d.Vector()
o = m3d.Orientation()
with pytest.raises(ValueError):
assert t != v
with pytest.raises(ValueError):
assert v != t
with pytest.raises(ValueError):
assert o != v
t2 = t.copy()
assert t == t2
assert t.pos == t2.pos
assert t.orient == t2.orient
t2.pos.y = 2
assert t != t2
assert t.pos != t2.pos
assert t.orient == t2.orient
t3 = t.copy()
t3.orient.rotate_xb(1)
assert t2.pos != t3.pos
assert t2.orient != t3.orient
assert t != t3
def test_axis_angle():
o = m3d.Orientation()
o.rotate_xb(np.pi / 3)
o.rotate_zb(np.pi / 3)
v, a = o.to_axis_angle()
o2 = m3d.Orientation.from_axis_angle(v, a)
assert o.similar(o2, CONVERTION_ERROR)
def test_transform_init_ref():
v = m3d.Vector(1, 2, 3)
o = m3d.Orientation()
o.rotate_zb(1)
t = m3d.Transform(o, v)
assert t.orient == o
o.rotate_xb(1)
assert t.orient != o
def test_rotation():
t = m3d.Transform()
t.pos.x = 1
t.orient.rotate_yb(1)
res = m3d.Transform(
m3d.Orientation([[0.54030228, 0, 0.84147096], [0., 1, 0.],
[-0.84147096, 0., 0.54030228]]), m3d.Vector(1, 0, 0))
assert _are_equals(res.data, t.data)
def test_init():
t = m3d.Transform()
assert t.pos.x == 0
assert t.pos.y == 0
t.pos.x = 2
assert t.pos.x == 2
i = t.inverse()
assert m3d.Vector(-2, 0, 0) == i.pos
assert m3d.Orientation() == i.orient
def test_inverse_orient():
o = m3d.Orientation()
o.rotate_xb(3)
o.rotate_yb(1)
o.rotate_xb(1)
v = m3d.Vector(-1, -2, -3)
assert o * v != v
assert o.inverse() * o * v == v
assert o * o.inverse() * v == v
assert o * o.inverse() * o == o
def test_multiplication_orient():
o = m3d.Orientation()
o.rotate_zb(np.pi / 2)
v = m3d.Vector(1, 0, 0)
r = o * v
assert r == m3d.Vector(0, 1, 0)
o.rotate_zb(-np.pi)
v = m3d.Vector(2, 0, 0)
r = o * v
assert r == m3d.Vector(0, -2, 0)
def test_quaternion_class():
o = m3d.Orientation()
o.rotate_xb(np.pi / 3)
o.rotate_zb(np.pi / 3)
q = o.to_quaternion()
q_m3d = m3d.Quaternion(q)
q_math3d = math3d.Quaternion(*q_m3d.data)
q_m3d_product = q_m3d * q_m3d
q_math3d_product = q_math3d * q_math3d
assert q_m3d_product.data == q_math3d_product.array
def test_construct():
o = m3d.Orientation()
o.rotate_zb(1)
v = m3d.Vector()
v[0] = 1
v[2] = 2
t = m3d.Transform(o, v)
assert t.pos.x == 1
assert t.pos.z == 2
t.pos = m3d.Vector()
t.orient.rotate_zb(-1)
assert t == m3d.Transform()
t.orient = o
assert t != m3d.Transform()
def test_quaternion():
o = m3d.Orientation()
o.rotate_xb(np.pi / 3)
o.rotate_zb(np.pi / 3)
q = o.to_quaternion()
o2 = m3d.Orientation.from_quaternion(*q)
assert o.similar(o2, CONVERTION_ERROR)
o_math = math3d.Orientation()
o_math.rotate_xb(np.pi / 3)
o_math.rotate_zb(np.pi / 3)
q_math = o_math.get_unit_quaternion()
o2_math = math3d.Orientation(q_math)
assert _are_equals(np.array(q),
np.array((q_math.s, q_math.x, q_math.y, q_math.z)))
def test_from_xz():
x = m3d.Vector(1, 0, 0)
z = m3d.Vector(0, 0, 1)
orient = m3d.Orientation.from_xz(x, z)
assert _are_equals(orient.data, np.identity(3))
x = m3d.Vector(2, 0, 0.1)
z = m3d.Vector(0.1, -0.1, 3)
o = m3d.Orientation()
o.rotate_yb(1)
o.rotate_zb(1)
x = o @ x
z = o @ z
orient = m3d.Orientation.from_xz(x, z)
assert _are_equals(o, orient, eps=0.1)
def test_transform_init():
v = m3d.Vector(1, 2, 3)
o = m3d.Orientation()
o.rotate_zb(1)
t = m3d.Transform(o, v)
assert t.pos == v
assert t.orient == o
t2 = m3d.Transform(o.data, v.data)
assert t2.pos == v
assert t2.orient == o
assert t == t2
with pytest.raises(ValueError):
m3d.Transform(np.array([1, 2, 3]), np.array([1, 2, 3]))
with pytest.raises(ValueError):
m3d.Transform(o.data, np.array([1, 2]))
def test_orient():
o = m3d.Orientation()
o.rotate_zb(2)
o2 = m3d.Orientation()
o2.rotate_zb(2 * np.pi)
assert o * o2 == o
