def test_rotate_vector():
v = Vec3(1, 1, 0)
q1 = Quat.from_rotation_on_axis(0, np.pi / 3)
q2 = Quat.from_rotation_on_axis(1, np.pi / 3)
q3 = Quat.from_rotation_on_axis(2, np.pi / 3)
t1 = Transform(q1, Vec3.zero())
t2 = Transform(q2, Vec3.zero())
t3 = Transform(q3, Vec3.zero())
# forward rotation
v1 = t1.rotate(v)
v2 = t2.rotate(v)
v3 = t3.rotate(v)
theta1 = (60) * np.pi / 180.0
v1_true = Vec3(1, math.cos(theta1), math.sin(theta1))
v2_true = Vec3(math.cos(theta1), 1, -math.sin(theta1))
theta2 = (45 + 60) * np.pi / 180.0
v3_true = math.sqrt(2) * Vec3(math.cos(theta2), math.sin(theta2), 0)
assert v1 == v1_true
assert v2 == v2_true
assert v3 == v3_true
# inverse rotate
v1_rev = t1.inverse_rotate(v1_true)
v2_rev = t2.inverse_rotate(v2_true)
v3_rev = t3.inverse_rotate(v3_true)
assert v == v1_rev
assert v == v2_rev
assert v == v3_rev
def test_normalize():
q1 = Quat(0.4619398, 0.1913417, 0.4619398, 0.7325378) # 45 around X then Y then Z
t1 = Transform(q1, Vec3.zero())
assert t1.is_normalized()
q2 = Quat(3, 0, 4, 7)
t2 = Transform(q2, Vec3.zero())
assert not t2.is_normalized()
t2.normalize()
assert t2.is_normalized()
def test_quick_constructors():
v1 = Vec3.zero()
assert v1 == Vec3(0, 0, 0)
v2 = Vec3.x_axis()
assert v2 == Vec3(1, 0, 0)
v3 = Vec3.y_axis()
assert v3 == Vec3(0, 1, 0)
v4 = Vec3.z_axis()
assert v4 == Vec3(0, 0, 1)
def test_init_and_eq_and_ne():
t1 = Transform()
assert t1.rotation == Quat.identity()
assert t1.translation == Vec3.zero()
rot = Quat.from_rotation_on_axis(2, np.pi)
trans = Vec3(1, 2, 3)
t2 = Transform(rot, trans)
assert t2.rotation == rot
assert t2.translation == trans
t3 = Transform.identity()
assert t1 == t3
assert t1 != t2
assert t3 != t2
def identity():
return Transform(Quat.identity(), Vec3.zero())
def __init__(
self, orientation: Quat = Quat.identity(), position: Vec3 = Vec3.zero()
):
self.rotation = orientation
self.translation = position