def test_transform(self):
# transform between space 1 and space 2
m = rotate_z(47)
# define in coordinate space 1
v1 = Vector3D(1, 0.3, -0.2)
q1 = Quaternion.from_axis_angle(Vector3D(0.1, -0.7, 0.2), 57)
# transform to coordinate space 2
v2 = v1.transform(m)
q2 = q1.transform(m)
# use quaternion to rotate vector in each space
r1 = v1.transform(q1.as_matrix())
r2 = v2.transform(q2.as_matrix())
# convert result in space 2 to space 1 for comparison
r2_1 = r2.transform(m.inverse())
self.assertAlmostEqual(r1.x,
r2_1.x,
delta=1e-10,
msg='Transform failed [X]')
self.assertAlmostEqual(r1.y,
r2_1.y,
delta=1e-10,
msg='Transform failed [Y]')
self.assertAlmostEqual(r1.z,
r2_1.z,
delta=1e-10,
msg='Transform failed [Z]')
def test_rotate(self):
"""Rotation by yaw, pitch and roll factory function."""
m = rotate(63, -40, 12)
r = rotate_y(-63) * rotate_x(40) * rotate_z(12)
for i in range(0, 4):
for j in range(0, 4):
self.assertAlmostEqual(m[i, j], r[i, j], places=14, msg="Rotate matrix generation failed (R"+str(i)+", C"+str(j)+").")
def test_rotate_z(self):
"""Rotation about z-axis matrix factory function."""
m = rotate_z(23)
a = pi * 23 / 180
r = [[cos(a), -sin(a), 0, 0],
[sin(a), cos(a), 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]]
for i, row in enumerate(r):
for j, v in enumerate(row):
self.assertAlmostEqual(m[i, j], v, places=14, msg="Rotate_z matrix generation failed (R"+str(i)+", C"+str(j)+").")