def as_quat(self):
"""
Returns the rotation matrix as quaternion.
:return: quat
"""
t = self.trace()
from pector import quat
if t > 0.:
r = math.sqrt(1. + t)
s = 0.5 / r
return quat((self.yz - self.zy) * s, (self.zx - self.xz) * s,
(self.xy - self.yx) * s, 0.5 * r)
elif self.xx > self.xy and self.xx > self.zz:
r = math.sqrt(1. + self.xx - self.yy - self.zz) * 2.
s = 1. / r
return quat(.25 * r, (self.xy + self.yx) * s,
(self.xz + self.zx) * s, (self.zy - self.yz) * s)
elif self.yy > self.zz:
r = math.sqrt(1. + self.yy - self.xx - self.zz) * 2.
s = 1. / r
return quat((self.xy + self.yx) * s, .25 * r,
(self.yz + self.zy) * s, (self.xz - self.zx) * s)
else:
r = math.sqrt(1. + self.zz - self.xx - self.yy) * 2.
s = 1. / r
return quat((self.xz + self.zx) * s, (self.yz + self.zy) * s,
.25 * r, (self.yx - self.xy) * s)
def test_rotate_to(self):
self.assertEqual(quat().set_rotate_axis((1, 0, 0), 90),
vec3(0, 0, -1).get_rotation_to((0, 1, 0)))
self.assertEqual(quat().set_rotate_axis((0, 1, 0), 90),
vec3(0, 0, 1).get_rotation_to((1, 0, 0)))
self.assertEqual(quat().set_rotate_axis((0, 0, 1), 90),
vec3(0, 1, 0).get_rotation_to((-1, 0, 0)))
def _compare_mat3_quat_rotation(self, axis, deg):
self.assertEqual(mat3().set_rotate_axis(axis, deg).round(4),
quat().set_rotate_axis(axis, deg).as_mat3().round(4))
self.assertEqual(mat3().set_rotate_axis(axis, deg).round(4),
(mat3() * quat().set_rotate_axis(axis, deg)).round(4))
self.assertEqual(
mat3().set_rotate_axis(axis, deg).rotate_axis(axis, deg).round(4),
(mat3() * quat().set_rotate_axis(axis, deg).rotate_axis(
axis, deg)).round(4))
def test_setitem(self):
a = quat()
a[0] = 1
self.assertEqual(quat(1, 0, 0, 1), a)
a[1] = 2
self.assertEqual(quat(1, 2, 0, 1), a)
a[2] = 3
self.assertEqual(quat(1, 2, 3, 1), a)
a[3] = 4
self.assertEqual(quat(1, 2, 3, 4), a)
with self.assertRaises(IndexError):
a[4] = 1
def test_properties(self):
self.assertEqual(quat(1, 2, 3, 4).x, 1)
self.assertEqual(quat(1, 2, 3, 4).y, 2)
self.assertEqual(quat(1, 2, 3, 4).z, 3)
self.assertEqual(quat(1, 2, 3, 4).w, 4)
a = quat()
a.x = 5
self.assertEqual((5, 0, 0, 1), a)
a.y = 6
self.assertEqual((5, 6, 0, 1), a)
a.z = 7
self.assertEqual((5, 6, 7, 1), a)
a.w = 8
self.assertEqual((5, 6, 7, 8), a)
def test_floor(self):
self.assertEqual(quat(1, 2, 3, 4), quat(1.4, 2.5, 3.6, 4.7).floor())
self.assertEqual(quat(-2, -3, -4, -5),
quat(-1.4, -2.5, -3.6, -4.7).floor())
self.assertEqual(quat(1, 2, 3, 4), quat(1.4, 2.5, 3.6, 4.7).floored())
self.assertEqual(quat(-2, -3, -4, -5),
quat(-1.4, -2.5, -3.6, -4.7).floored())
def test_rotate(self):
self.assertEqual(quat().set_rotate_axis((1, 2, 3), 4),
quat((1, 2, 3), 4))
self.assertEqual(
mat3().set_rotate_axis((1, 0, 0), 0).round(),
quat().set_rotate_axis((1, 0, 0), 0).as_mat3().round())
self.assertEqual(
mat3().set_rotate_axis((1, 0, 0), 90).round(),
quat().set_rotate_axis((1, 0, 0), 90).as_mat3().round())
for i in range(100):
axis = vec3(self.r.gauss(0, 1), self.r.gauss(0, 1),
self.r.gauss(0, 1)).normalize()
deg = self.r.uniform(-360, 360)
self._compare_mat3_quat_rotation(axis, deg)
def test_getitem(self):
a = quat(1, 2, 3, 4)
self.assertEqual(1, a[0])
self.assertEqual(2, a[1])
self.assertEqual(3, a[2])
self.assertEqual(4, a[3])
with self.assertRaises(IndexError):
b = a[4]
def as_quat2(self):
from pector import quat
i = 0
if self.get(1, 1) > self.get(0, 0):
i = 1
if self.get(2, 2) > self.get(i, i):
i = 2
j = (i + 1) % 3
k = (j + 1) % 3
r = math.sqrt(1. + self.get(i, i) - self.get(j, j) - self.get(k, k))
if not r:
return quat()
#print(self.get(i,i), self.get(j,j), self.get(k,k) )
s = .5 / r
q = quat()
q[i] = .5 * r
q[j] = (self.get(j, i) + self.get(i, j)) * s
q[k] = (self.get(k, i) + self.get(i, k)) * s
q[3] = (self.get(k, j) - self.get(j, k)) * s
return q
def test_round(self):
self.assertEqual((0, 0, 1, 0), quat(0.49, 0.5, 0.51, 0).round())
self.assertEqual((0, 0, -1, 0), quat(-0.49, -0.5, -0.51, 0).round())
self.assertEqual((0.5, 0.5, 0.5, 0), quat(0.49, 0.5, 0.51, 0).round(1))
self.assertEqual((-0.5, -0.5, -0.5, 0),
quat(-0.49, -0.5, -0.51, 0).round(1))
self.assertEqual((0.12346, 0.12346, 0.12341, 0),
quat(0.123456, 0.123456789, 0.1234123456789,
0).round(5))
self.assertEqual((0, 0, 1, 0), quat(0.49, 0.5, 0.51, 0).rounded())
self.assertEqual((0, 0, -1, 0), quat(-0.49, -0.5, -0.51, 0).rounded())
def test_mat_conversion_concat(self):
for i in range(100):
q = quat()
m = mat3()
# TODO: more than one transform is way off
for j in range(1):
axis = vec3(self.r.gauss(0, 1), self.r.gauss(0, 1),
self.r.gauss(0, 1)).normalize()
deg = self.r.uniform(-180 / 5, 180 / 5)
q.rotate_axis(axis, deg)
m.rotate_axis(axis, deg)
qm = q.as_mat3().round(3)
mm = m.rounded(3)
diff = abs(sum(qm - mm))
if diff > .1:
self.assertEqual(qm, mm)
self.assertLess(diff, 0.1)
def test_mat_conversion(self):
self.assertEqual(quat(), mat3().as_quat())
self._compare_quat_mat(quat((1, 0, 0), 90), mat3().set_rotate_x(90.))
self._compare_quat_mat(quat((0, 1, 0), 90), mat3().set_rotate_y(90.))
self._compare_quat_mat(quat((0, 0, 1), 90), mat3().set_rotate_z(90.))
for i in range(100):
axis = vec3(self.r.gauss(0, 1), self.r.gauss(0, 1),
self.r.gauss(0, 1)).normalize()
deg = self.r.uniform(-119, 119)
self._compare_quat_mat(quat(axis, deg),
mat3().set_rotate_axis(axis, deg))
self._compare_quat_mat(quat(-axis, deg),
mat3().set_rotate_axis(-axis, deg))
def test_assignment(self):
self.assertEqual(
"quat(0, 0, 0, 1)",
str(quat()),
)
self.assertEqual("quat(1, 2, 3, 4)", str(quat(1, 2, 3, 4)))
with self.assertRaises(TypeError):
quat(1, 2, 3)
with self.assertRaises(TypeError):
quat((1, 2, 3))
with self.assertRaises(TypeError):
quat((1, 2, 3), 4, 5)
with self.assertRaises(TypeError):
quat("bla")
with self.assertRaises(TypeError):
quat({"x": 23})
def test_dot(self):
self.assertEqual(60, quat(1, 2, 3, 4).dot((4, 5, 6, 7)))
with self.assertRaises(TypeError):
quat().dot((1, 2))
def test_length(self):
self.assertAlmostEqual(1., quat(1, 0, 0, 0).length())
self.assertAlmostEqual(math.sqrt(2.), quat(1, 1, 0, 0).length())
self.assertAlmostEqual(1., quat(1, 2, 3, 4).normalized().length())
def test_equal(self):
self.assertTrue(quat() == (0, 0, 0, 1))
self.assertFalse(quat() == (0, 0, 0))
self.assertFalse(quat() == (0, 0, 0, 0, 0))
self.assertTrue(quat(1, 2, 3, 4) == (1, 2, 3, 4))
def test_abs(self):
self.assertEqual(quat(1, 2, 3, 4), abs(quat(-1, -2, -3, -4)))
self.assertEqual(quat(1, 2, 3, 4), abs(quat(1, -2, 3, -4)))
def test_iter(self):
self.assertEqual([1, 2, 3, 4], [x for x in quat(1, 2, 3, 4)])
