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 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 testalmostequal(self):
q1 = quaternions.Quaternion(1, -2, 3, 5)
self.assertFalse(quaternions.almostequal(self.q1, q1))
q1 = quaternions.Quaternion(1, -2, 3, 4.00000001)
self.assertTrue(quaternions.almostequal(self.q1, q1))
self.assertTrue(quaternions.almostequal(q1, self.q1))
def testalmostequal(self):
q1 = quaternions.Quaternion(1, -2, 3, 5)
self.assertFalse(quaternions.almostequal(self.q1, q1))
q1 = quaternions.Quaternion(1, -2, 3, 4.00000001)
self.assertTrue(quaternions.almostequal(self.q1, q1))
self.assertTrue(quaternions.almostequal(q1, self.q1))
def test__div__(self):
division = self.q1 / self.q2
answer = quaternions.Quaternion(5.196152422706632, 1.1547005383792515, 0.57735026918962584, -1.1547005383792519)
self.assertTrue(quaternions.almostequal(division, answer))
division = self.q1 / 4.0
answer = quaternions.Quaternion(0.25, -0.5, 0.75, 1.0)
self.assertTrue(quaternions.almostequal(division, answer))
def test__div__(self):
division = self.q1 / self.q2
answer = quaternions.Quaternion(5.196152422706632, 1.1547005383792515,
0.57735026918962584,
-1.1547005383792519)
self.assertTrue(quaternions.almostequal(division, answer))
division = self.q1 / 4.0
answer = quaternions.Quaternion(0.25, -0.5, 0.75, 1.0)
self.assertTrue(quaternions.almostequal(division, answer))
def test__invert__(self):
#Test for validity
#http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/arithmetic/index.htm
q = ~quaternions.Quaternion(4, 3, -1, 2)
expected_q = quaternions.Quaternion(0.7302967433402214,
-0.5477225575051661,
0.18257418583505536,
-0.3651483716701107)
self.assertTrue(quaternions.almostequal(q, expected_q))
def testaxisangle_to_quaternion(self):
#Verified with Martin Baker (2008) Quaternion to AxisAngle, \url{http://www.euclideansplace.com}
#Test input data
q = quaternions.axisangle_to_quaternion(0, (1, 0, 0))
expected_q = quaternions.Quaternion(1, 0, 0, 0)
self.assertTrue(quaternions.almostequal(q, expected_q))
#Test calculation
q = quaternions.axisangle_to_quaternion(pi / 2, (1, 0, 0))
expected_q = quaternions.Quaternion(sqrt(2) / 2.0, sqrt(2) / 2.0, 0, 0)
self.assertTrue(quaternions.almostequal(q, expected_q))
#Test back-conversion
q1 = quaternions.axisangle_to_quaternion(pi / 3, (1, 1, 1))
angle, axis = q1.to_axisangle()
self.assertAlmostEqual(angle, pi / 3.0)
self.assertAlmostEqual(axis[0], 1.0 / sqrt(3))
self.assertAlmostEqual(axis[1], 1.0 / sqrt(3))
self.assertAlmostEqual(axis[2], 1.0 / sqrt(3))
def testaxisangle_to_quaternion(self):
#Verified with Martin Baker (2008) Quaternion to AxisAngle, \url{http://www.euclideansplace.com}
#Test input data
q = quaternions.axisangle_to_quaternion(0, (1, 0, 0))
expected_q = quaternions.Quaternion(1, 0, 0, 0)
self.assertTrue(quaternions.almostequal(q, expected_q))
#Test calculation
q = quaternions.axisangle_to_quaternion(pi / 2, (1, 0, 0))
expected_q = quaternions.Quaternion(sqrt(2) / 2.0, sqrt(2) / 2.0, 0, 0)
self.assertTrue(quaternions.almostequal(q, expected_q))
#Test back-conversion
q1 = quaternions.axisangle_to_quaternion(pi / 3, (1, 1, 1))
angle, axis = q1.to_axisangle()
self.assertAlmostEqual(angle, pi / 3.0)
self.assertAlmostEqual(axis[0], 1.0 / sqrt(3))
self.assertAlmostEqual(axis[1], 1.0 / sqrt(3))
self.assertAlmostEqual(axis[2], 1.0 / sqrt(3))
def testso3matrix_to_quaternion(self):
# Verified with Martin Baker (2008) Quaternion to AxisAngle, \url{http://www.euclideansplace.com}
# Test calculation
m = matrices.SO3Matrix([[1, 0, 0], [0, 0, -1], [0, 1, 0]])
q = quaternions.so3matrix_to_quaternion(m)
expected_q = quaternions.Quaternion(
sqrt(2) / 2.0, -sqrt(2) / 2.0, 0, 0)
self.assertTrue(quaternions.almostequal(q, expected_q))
# Test back-conversion
q1 = quaternions.so3matrix_to_quaternion(m)
mq = q1.to_so3matrix()
self.assertTrue(matrices.almostequal(m, mq))
# Random test
for _i in range(REPETITIONS):
n = []
for _j in range(2):
x = random.random() * (-1)**(int(random.random() * 10))
z = random.random() * (-1)**(int(random.random() * 10))
y = random.random() * (-1)**(int(random.random() * 10))
n.append(vectors.normalize(vectors.Vector3D(x, y, z)))
eP1 = vectors.normalize(n[0])
eP2 = vectors.cross(n[0], n[1])
eP2.normalize()
eP3 = vectors.normalize(vectors.cross(eP1, eP2))
m = matrices.SO3Matrix([[eP1[0], eP2[0], eP3[0]],
[eP1[1], eP2[1], eP3[1]],
[eP1[2], eP2[2], eP3[2]]])
q = quaternions.so3matrix_to_quaternion(m)
mq = q.to_so3matrix()
self.assertTrue(matrices.almostequal(m, mq))
def testso3matrix_to_quaternion(self):
# Verified with Martin Baker (2008) Quaternion to AxisAngle, \url{http://www.euclideansplace.com}
# Test calculation
m = matrices.SO3Matrix([[1, 0, 0], [0, 0, -1], [0, 1, 0]])
q = quaternions.so3matrix_to_quaternion(m)
expected_q = quaternions.Quaternion(sqrt(2) / 2.0, -sqrt(2) / 2.0, 0, 0)
self.assertTrue(quaternions.almostequal(q, expected_q))
# Test back-conversion
q1 = quaternions.so3matrix_to_quaternion(m)
mq = q1.to_so3matrix()
self.assertTrue(matrices.almostequal(m, mq))
# Random test
for _i in range(REPETITIONS):
n = []
for _j in range(2):
x = random.random() * (-1) ** (int(random.random()*10))
z = random.random() * (-1) ** (int(random.random()*10))
y = random.random() * (-1) ** (int(random.random()*10))
n.append(vectors.normalize(vectors.Vector3D(x, y, z)))
eP1 = vectors.normalize(n[0])
eP2 = vectors.cross(n[0], n[1])
eP2.normalize()
eP3 = vectors.normalize(vectors.cross(eP1, eP2))
m = matrices.SO3Matrix([[eP1[0], eP2[0], eP3[0]],
[eP1[1], eP2[1], eP3[1]],
[eP1[2], eP2[2], eP3[2]]])
q = quaternions.so3matrix_to_quaternion(m)
mq = q.to_so3matrix()
self.assertTrue(matrices.almostequal(m, mq))
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 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 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 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 test__invert__(self):
#Test for validity
#http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/arithmetic/index.htm
q = ~quaternions.Quaternion(4, 3, -1, 2)
expected_q = quaternions.Quaternion(0.7302967433402214, -0.5477225575051661, 0.18257418583505536, -0.3651483716701107)
self.assertTrue(quaternions.almostequal(q, expected_q))
