def test_unique_representation_quat(self):
a = 1.2345
# These quaternions represent equivalent roatations
l = 1/numpy.sqrt(3)
q_same = [rotation.unique_representation_quat(rotation.quat(a, l, l, l)),
rotation.unique_representation_quat(rotation.quat(-a, -l, -l, -l)),
rotation.unique_representation_quat(rotation.quat(a + 2*numpy.pi, l, l, l)),
rotation.unique_representation_quat(rotation.quat(a - 2*numpy.pi, l, l, l))]
q_expected = q_same[0]
for q in q_same[1:]:
for q_i, q_expected_i in zip(q, q_expected):
self.assertAlmostEqual(q_i, q_expected_i)
def test_unique_representation_quat(self):
a = 1.2345
# These quaternions represent equivalent roatations
l = 1 / numpy.sqrt(3)
q_same = [
rotation.unique_representation_quat(rotation.quat(a, l, l, l)),
rotation.unique_representation_quat(rotation.quat(-a, -l, -l, -l)),
rotation.unique_representation_quat(
rotation.quat(a + 2 * numpy.pi, l, l, l)),
rotation.unique_representation_quat(
rotation.quat(a - 2 * numpy.pi, l, l, l))
]
q_expected = q_same[0]
for q in q_same[1:]:
for q_i, q_expected_i in zip(q, q_expected):
self.assertAlmostEqual(q_i, q_expected_i)
def test_Rotation_compare_rotation_formalisms(self):
# Test vector (before and after rotation)
v0 = numpy.array([0., 1., 0.])
v1_expected = numpy.array([0., 0., 1.])
# Positive 90 degree rotation around x-axis
a = numpy.pi / 2.
kwargs_list = [
{
"formalism": "quaternion",
"values": rotation.quat(a, 1., 0., 0.)
},
{
"formalism": "rotation_matrix",
"values": rotation.R_x(a)
},
{
"formalism": "euler_angles_xyz",
"values": numpy.array([a, 0., 0.])
},
{
"formalism": "euler_angles_zxz",
"values": numpy.array([0., a, 0.])
},
{
"formalism": "euler_angles_zyx",
"values": numpy.array([0., 0., a])
},
]
for kwargs in kwargs_list:
R = rotation.Rotation(**kwargs)
v1 = R.rotate_vector(v0)
for v1_i, v1_expected_i in zip(v1, v1_expected):
self.assertAlmostEqual(v1_i, v1_expected_i)
def test_quat(self):
angle = numpy.pi/3.
u = numpy.array([1., 2., 3.])
u /= numpy.sqrt((u**2).sum())
q = rotation.quat(angle, u[0], u[1], u[2])
q_expected = numpy.array([numpy.cos(angle/2.),
numpy.sin(angle/2.)*u[0],
numpy.sin(angle/2.)*u[1],
numpy.sin(angle/2.)*u[2]])
for q_i, q_expected_i in zip(q, q_expected):
self.assertAlmostEqual(q_i, q_expected_i)
def test_quat(self):
angle = numpy.pi / 3.
u = numpy.array([1., 2., 3.])
u /= numpy.sqrt((u**2).sum())
q = rotation.quat(angle, u[0], u[1], u[2])
q_expected = numpy.array([
numpy.cos(angle / 2.),
numpy.sin(angle / 2.) * u[0],
numpy.sin(angle / 2.) * u[1],
numpy.sin(angle / 2.) * u[2]
])
for q_i, q_expected_i in zip(q, q_expected):
self.assertAlmostEqual(q_i, q_expected_i)
def test_Rotation_compare_rotation_formalisms(self):
# Test vector (before and after rotation)
v0 = numpy.array([0., 1., 0.])
v1_expected = numpy.array([0., 0., 1.])
# Positive 90 degree rotation around x-axis
a = numpy.pi/2.
kwargs_list = [
{"formalism" : "quaternion", "values" : rotation.quat(a, 1., 0., 0.)},
{"formalism" : "rotation_matrix", "values" : rotation.R_x(a)},
{"formalism" : "euler_angles_xyz", "values" : numpy.array([a, 0., 0.])},
{"formalism" : "euler_angles_zxz", "values" : numpy.array([0., a, 0.])},
{"formalism" : "euler_angles_zyx", "values" : numpy.array([0., 0., a])},
]
for kwargs in kwargs_list:
R = rotation.Rotation(**kwargs)
v1 = R.rotate_vector(v0)
for v1_i, v1_expected_i in zip(v1, v1_expected):
self.assertAlmostEqual(v1_i, v1_expected_i)