def test_transform_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(1, 1, -1))
t2 = Transform(q2, Vec3(4, -3, -1))
t3 = Transform(q3, Vec3(-1, 1, -1))
# forward rotation
v1 = t1.transform(v)
v2 = t2.transform(v)
v3 = t3.transform(v)
theta1 = (60) * np.pi / 180.0
v1_true = Vec3(1 + 1, math.cos(theta1) + 1, math.sin(theta1) - 1)
v2_true = Vec3(math.cos(theta1) + 4, 1 - 3, -math.sin(theta1) - 1)
theta2 = (45 + 60) * np.pi / 180.0
v3_true = Vec3(
math.sqrt(2) * math.cos(theta2) - 1, math.sqrt(2) * math.sin(theta2) + 1, 0 - 1
)
assert v1 == v1_true
assert v2 == v2_true
assert v3 == v3_true
# inverse rotate
v1_rev = t1.inverse_transform(v1_true)
v2_rev = t2.inverse_transform(v2_true)
v3_rev = t3.inverse_transform(v3_true)
assert v == v1_rev
assert v == v2_rev
assert v == v3_rev
