def testEulerAnglesToQuaternion(self):
#Verified with Rollett, Tony (2008) Advanced Characterization and Microstructural Analysis
#Test calculation
e = eulers.Eulers(0, 0, 0)
q = quaternions.eulerangles_to_quaternion(e)
expected_q = quaternions.Quaternion(1, 0, 0, 0)
self.assertTrue(quaternions.almostequal(q, expected_q))
e = eulers.Eulers(0, pi / 4, 0)
q = quaternions.eulerangles_to_quaternion(e)
expected_q = quaternions.Quaternion(cos(0.5 * pi / 4),
sin(0.5 * pi / 4), 0, 0)
self.assertTrue(quaternions.almostequal(q, expected_q))
e = eulers.Eulers(35 / 180.0 * pi, 27 / 180.0 * pi, 102 / 180.0 * pi)
q = quaternions.eulerangles_to_quaternion(e)
expected_q = quaternions.axisangle_to_quaternion(35 / 180.0 * pi, (0, 0, 1)) * \
quaternions.axisangle_to_quaternion(27 / 180.0 * pi, (1, 0, 0)) * \
quaternions.axisangle_to_quaternion(102 / 180.0 * pi, (0, 0, 1))
self.assertTrue(quaternions.almostequal(q, expected_q))
for _i in range(REPETITIONS):
e1 = random.random() * 360
e2 = random.random() * 180
e3 = random.random() * 360
e = eulers.eulers_deg_to_eulers_rad(e1, e2, e3)
q = quaternions.eulerangles_to_quaternion(e)
expected_q = quaternions.axisangle_to_quaternion(e[1], (0, 0, 1)) * \
quaternions.axisangle_to_quaternion(e[2], (1, 0, 0)) * \
quaternions.axisangle_to_quaternion(e[3], (0, 0, 1))
self.assertTrue(quaternions.almostequal(q, expected_q))
def test__eq__(self):
euler = eulers.Eulers(0.1, 0.2, 0.3)
self.assertTrue(self.e1 == euler)
euler = eulers.Eulers(0.4, 0.5, 0.6)
self.assertTrue(self.e2 == euler)
euler = eulers.Eulers(0.7, 0.8, 0.9)
self.assertTrue(self.e3 == euler)
def testnegative(self):
e1 = eulers.negative(eulers.Eulers(0, 0, 0)).to_rad()
self.assertAlmostEqual(e1[0], 0.0)
self.assertAlmostEqual(e1[1], 0.0)
self.assertAlmostEqual(e1[2], 0.0)
eulers1 = eulers.Eulers(pi, 0, pi)
eulers1.negative()
e1 = eulers1.to_rad()
self.assertAlmostEqual(e1[0], pi)
self.assertAlmostEqual(e1[1], 0.0)
self.assertAlmostEqual(e1[2], pi)
eulers1 = eulers.Eulers(3.0 * pi / 2.0, 0, 5 * pi / 3)
eulers1.negative()
e1 = eulers1.to_rad()
self.assertAlmostEqual(e1[0], -pi / 2.0)
self.assertAlmostEqual(e1[1], 0)
self.assertAlmostEqual(e1[2], -pi / 3)
def testpositive(self):
e1 = eulers.positive(eulers.Eulers(0, 0, 0)).to_rad()
self.assertAlmostEqual(e1[0], 0.0)
self.assertAlmostEqual(e1[1], 0.0)
self.assertAlmostEqual(e1[2], 0.0)
eulers1 = eulers.Eulers(pi, 0, pi)
eulers1.positive()
e1 = eulers1.to_rad()
self.assertAlmostEqual(e1[0], pi)
self.assertAlmostEqual(e1[1], 0.0)
self.assertAlmostEqual(e1[2], pi)
eulers1 = eulers.Eulers(-pi / 2.0, 0, -pi / 3)
eulers1.positive()
e1 = eulers1.to_rad()
self.assertAlmostEqual(e1[0], 3.0 * pi / 2.0)
self.assertAlmostEqual(e1[1], 0.0)
self.assertAlmostEqual(e1[2], 5.0 * pi / 3.0)
def testrotate(self):
#Test of successive rotations
q = quaternions.Quaternion(0, 1, 0, 0) #Vector (1,0,0)
q1 = quaternions.axisangle_to_quaternion(
pi / 2.0, (0, 0, 1)) #90deg rotation along z-axis
q2 = quaternions.axisangle_to_quaternion(
pi / 2.0, (1, 0, 0)) #90deg rotation along x-axis
qq1 = quaternions.rotate(q, [q1])
expected_qq1 = quaternions.Quaternion(0, 0, 1, 0)
self.assertTrue(quaternions.almostequal(expected_qq1, qq1))
# Vector (0,1,0)
qq1q2 = quaternions.rotate(qq1, [q2])
expected_qq1q2 = quaternions.Quaternion(0, 0, 0, 1)
self.assertTrue(quaternions.almostequal(
qq1q2, expected_qq1q2)) #Vector (0,0,1)
expected_qq1q2 = quaternions.rotate(q, [q1, q2])
self.assertTrue(quaternions.almostequal(
qq1q2, expected_qq1q2)) #Order of rotation q1 then q2
notexpected_qq1q2 = quaternions.rotate(q, [q2, q1])
self.assertFalse(quaternions.almostequal(qq1q2, notexpected_qq1q2))
#Test of successive rotations
q = quaternions.Quaternion(0, 1, 0, 0) #Vector (1,0,0)
q1 = quaternions.eulerangles_to_quaternion(eulers.Eulers(13, 0, 93))
q2 = quaternions.eulerangles_to_quaternion(eulers.Eulers(60, 80, 152))
q3 = quaternions.eulerangles_to_quaternion(eulers.Eulers(150, 0, 12))
qq1 = quaternions.rotate(q, [q1])
qq1q2 = quaternions.rotate(qq1, [q2])
qq1q2q3 = quaternions.rotate(qq1q2, [q3])
#Order of rotation q1, q2 then q3
expected_qq1q2q3 = quaternions.rotate(q, [q1, q2, q3])
self.assertTrue(quaternions.almostequal(qq1q2q3, expected_qq1q2q3))
notexpected_qq1q2q3 = quaternions.rotate(q, [q3, q2, q1])
self.assertFalse(quaternions.almostequal(qq1q2q3, notexpected_qq1q2q3))
def to_eulerangles(self):
"""
Give the euler angles for the quaternion.
**References**
Martin Baker (2008) Euclidean Space http://www.euclideansplace.com
:rtype: :class:`eulers.Eulers`
"""
qcalc = normalize(self)
q0 = qcalc._a
q1 = qcalc._A[0]
q2 = qcalc._A[1]
q3 = qcalc._A[2]
x = (q0**2 + q3**2) * (q1**2 + q2**2)
if round(abs(x), 7 * 2) == 0.0:
if round(abs(q1), 7) == 0.0 and round(abs(q2), 7) == 0.0:
theta1 = atan2(2 * q0 * q3, q0**2 - q3**2)
theta2 = 0
theta3 = 0
elif round(abs(q0), 7) == 0.0 and round(abs(q3), 7) == 0.0:
theta1 = atan2(2 * q1 * q2, q1**2 - q2**2)
theta2 = pi
theta3 = 0
else:
theta1 = atan2(q3, q0) + atan2(q2, q1)
theta2 = acos(1 - 2 * q1**2 - 2 * q2**2)
theta3 = atan2(q3, q0) - atan2(q2, q1)
assert round(abs(theta2 - acos(q0**2 - q1**2 - q2**2 + q3**2)),
7) == 0.0
assert round(
abs(2 * atan2(sqrt(q1**2 + q2**2), sqrt(q0**2 + q3**2)) -
theta2)) == 0.0
e1 = eulers.Eulers(theta1, theta2, theta3)
e1.positive()
return e1
def testto_eulerangles(self):
# Random eulers
for _i in range(REPETITIONS):
euler1 = random.random() * 360
euler2 = random.random() * 180
euler3 = random.random() * 360
e = eulers.eulers_deg_to_eulers_rad(euler1, euler2, euler3)
q1 = quaternions.eulerangles_to_quaternion(e)
q2 = quaternions.axisangle_to_quaternion(e[1], (0, 0, 1)) * \
quaternions.axisangle_to_quaternion(e[2], (1, 0, 0)) * \
quaternions.axisangle_to_quaternion(e[3], (0, 0, 1))
self.assertTrue(quaternions.almostequal(q1, q2))
qangles = q1.to_eulerangles().to_deg()
self.assertAlmostEqual(euler2, qangles[1])
self.assertAlmostEqual(euler1, qangles[0])
self.assertAlmostEqual(euler3, qangles[2])
# Special case when theta2 = 0
for _i in range(REPETITIONS):
euler1 = random.random() * 360
euler2 = 0.0
euler3 = random.random() * (360 - euler1)
e = eulers.eulers_deg_to_eulers_rad(euler1, euler2, euler3)
q = quaternions.eulerangles_to_quaternion(e)
qangles = q.to_eulerangles().to_deg()
euler13 = euler1 + euler3
self.assertAlmostEqual(euler13, qangles[0])
self.assertAlmostEqual(0.0, qangles[1])
self.assertAlmostEqual(0.0, qangles[2])
euler3 = random.random() * 360
euler2 = 0.0
euler1 = random.random() * (360 - euler3)
e = eulers.eulers_deg_to_eulers_rad(euler1, euler2, euler3)
q = quaternions.eulerangles_to_quaternion(e)
qangles = q.to_eulerangles().to_deg()
euler13 = euler1 + euler3
self.assertAlmostEqual(euler13, qangles[0])
self.assertAlmostEqual(0.0, qangles[1])
self.assertAlmostEqual(0.0, qangles[2])
# Special case when theta2 = pi
for _i in range(REPETITIONS):
euler1 = random.random() * 360
euler2 = 180
euler3 = random.random() * (360 - euler1)
e = eulers.eulers_deg_to_eulers_rad(euler1, euler2, euler3)
q = quaternions.eulerangles_to_quaternion(e)
qangles = q.to_eulerangles().to_deg()
euler13 = euler1 - euler3
e = eulers.Eulers(euler13 / 180.0 * pi, pi, 0.0)
e.positive()
angles = e.to_deg()
self.assertAlmostEqual(angles[0], qangles[0])
self.assertAlmostEqual(angles[1], qangles[1])
self.assertAlmostEqual(angles[2], qangles[2])
euler3 = random.random() * 360
euler2 = 180
euler1 = random.random() * (360 - euler3)
e = eulers.eulers_deg_to_eulers_rad(euler1, euler2, euler3)
q = quaternions.eulerangles_to_quaternion(e)
qangles = q.to_eulerangles().to_deg()
euler13 = euler1 - euler3
e = eulers.Eulers(euler13 / 180.0 * pi, pi, 0.0)
e.positive()
angles = e.to_deg()
self.assertAlmostEqual(angles[0], qangles[0])
self.assertAlmostEqual(angles[1], qangles[1])
self.assertAlmostEqual(angles[2], qangles[2])
def setUp(self):
unittest.TestCase.setUp(self)
self.e1 = eulers.Eulers(0.1, 0.2, 0.3)
self.e2 = eulers.Eulers([0.4, 0.5, 0.6])
self.e3 = eulers.Eulers((0.7, 0.8, 0.9))