def test_operators_quaternion(self):
q1 = Quaternion()
q2 = Quaternion.from_x_rotation(0.5)
# add
self.assertRaises(ValueError, lambda: q1 + q2)
# subtract
# we had to add this to enable np.array_equal to work
# as it uses subtraction
#self.assertRaises(ValueError, lambda: q1 - q2)
# multiply
self.assertTrue(
np.array_equal(
q1 * q2,
quaternion.cross(quaternion.create(),
quaternion.create_from_x_rotation(0.5))))
# divide
self.assertRaises(ValueError, lambda: q1 / q2)
# or
self.assertTrue(
np.array_equal(
q1 | q2,
quaternion.dot(quaternion.create(),
quaternion.create_from_x_rotation(0.5))))
# inverse
self.assertTrue(
np.array_equal(
~q2,
quaternion.conjugate(quaternion.create_from_x_rotation(0.5))))
def test_operators_quaternion(self):
q1 = Quaternion()
q2 = Quaternion.from_x_rotation(0.5)
# add
self.assertRaises(ValueError, lambda: q1 + q2)
# subtract
# we had to add this to enable np.array_equal to work
# as it uses subtraction
#self.assertRaises(ValueError, lambda: q1 - q2)
# multiply
self.assertTrue(np.array_equal(q1 * q2, quaternion.cross(quaternion.create(), quaternion.create_from_x_rotation(0.5))))
# divide
self.assertRaises(ValueError, lambda: q1 / q2)
# or
self.assertTrue(np.array_equal(q1 | q2, quaternion.dot(quaternion.create(), quaternion.create_from_x_rotation(0.5))))
# inverse
self.assertTrue(np.array_equal(~q2, quaternion.conjugate(quaternion.create_from_x_rotation(0.5))))
# ==
self.assertTrue(Quaternion() == Quaternion())
self.assertFalse(Quaternion() == Quaternion([0., 0., 0., 0.]))
# !=
self.assertTrue(Quaternion() != Quaternion([1., 1., 1., 1.]))
self.assertFalse(Quaternion() != Quaternion())
def rotate(self, w, i, j, k):
ct = quaternion.create(i, j, k, w, dtype="float32")
rot_y = Matrix44.from_quaternion(ct).flatten().astype("float32")
c_rotate = numpy.ctypeslib.as_ctypes(rot_y)
glUniformMatrix4fv(self.rotate_loc, 1, GL_FALSE, c_rotate)
glUniformMatrix4fv(self.light_loc, 1, GL_FALSE, c_rotate)
def test_procedural_examples(self):
from pyrr import quaternion, matrix44, vector3
import numpy as np
point = vector3.create(1.,2.,3.)
orientation = quaternion.create()
translation = vector3.create()
scale = vector3.create(1,1,1)
# translate along X by 1
translation += [1.0, 0.0, 0.0]
# rotate about Y by pi/2
rotation = quaternion.create_from_y_rotation(np.pi / 2.0)
orientation = quaternion.cross(rotation, orientation)
# create a matrix
# start our matrix off using the scale
matrix = matrix44.create_from_scale(scale)
# apply our orientation
orientation = matrix44.create_from_quaternion(orientation)
matrix = matrix44.multiply(matrix, orientation)
# apply our translation
translation_matrix = matrix44.create_from_translation(translation)
matrix = matrix44.multiply(matrix, translation_matrix)
# transform our point by the matrix
point = matrix44.apply_to_vector(matrix, point)
def test_procedural_examples(self):
from pyrr import quaternion, matrix44, vector3
import numpy as np
point = vector3.create(1.,2.,3.)
orientation = quaternion.create()
translation = vector3.create()
scale = vector3.create(1,1,1)
# translate along X by 1
translation += [1.0, 0.0, 0.0]
# rotate about Y by pi/2
rotation = quaternion.create_from_y_rotation(np.pi / 2.0)
orientation = quaternion.cross(rotation, orientation)
# create a matrix
# start our matrix off using the scale
matrix = matrix44.create_from_scale(scale)
# apply our orientation
orientation = matrix44.create_from_quaternion(orientation)
matrix = matrix44.multiply(matrix, orientation)
# apply our translation
translation_matrix = matrix44.create_from_translation(translation)
matrix = matrix44.multiply(matrix, translation_matrix)
# transform our point by the matrix
point = matrix44.apply_to_vector(matrix, point)
def __init__(self, data):
self.name = data.get("name")
self.children = data.get("children")
self.matrix = data.get("matrix")
self.mesh = data.get("mesh")
self.camera = data.get("camera")
self.translation = data.get("translation")
self.rotation = data.get("rotation")
self.scale = data.get("scale")
if self.matrix:
self.matrix = Matrix44(self.matrix)
else:
self.matrix = Matrix44.identity()
if self.translation is not None:
self.matrix = self.matrix * Matrix44.from_translation(
self.translation)
if self.rotation is not None:
quat = quaternion.create(
x=self.rotation[0],
y=self.rotation[1],
z=self.rotation[2],
w=self.rotation[3],
)
self.matrix = self.matrix * Matrix44.from_quaternion(
quat).transpose()
if self.scale is not None:
self.matrix = self.matrix * Matrix44.from_scale(self.scale)
def __init__(self, data):
self.children = data.get('children')
self.matrix = data.get('matrix')
self.mesh = data.get('mesh')
self.camera = data.get('camera')
self.translation = data.get('translation')
self.rotation = data.get('rotation')
self.scale = data.get('scale')
if self.matrix is None:
self.matrix = matrix44.create_identity()
if self.translation is not None:
self.matrix = matrix44.create_from_translation(self.translation)
if self.rotation is not None:
quat = quaternion.create(self.rotation[0], self.rotation[1],
self.rotation[2], self.rotation[3])
mat = matrix44.create_from_quaternion(quat)
self.matrix = matrix44.multiply(mat, self.matrix)
if self.scale is not None:
self.matrix = matrix44.multiply(
matrix44.create_from_scale(self.scale), self.matrix)
def __init__(self, data):
self.name = data.get('name')
self.children = data.get('children')
self.matrix = data.get('matrix')
self.mesh = data.get('mesh')
self.camera = data.get('camera')
self.translation = data.get('translation')
self.rotation = data.get('rotation')
self.scale = data.get('scale')
if self.matrix:
self.matrix = Matrix44(self.matrix)
else:
self.matrix = Matrix44.identity()
if self.translation is not None:
self.matrix = self.matrix * Matrix44.from_translation(self.translation)
if self.rotation is not None:
quat = quaternion.create(
x=self.rotation[0],
y=self.rotation[1],
z=self.rotation[2],
w=self.rotation[3],
)
self.matrix = self.matrix * Matrix44.from_quaternion(quat).transpose()
if self.scale is not None:
self.matrix = self.matrix * Matrix44.from_scale(self.scale)
def test_quaternion_matrix_conversion(self):
# https://au.mathworks.com/help/robotics/ref/quat2rotm.html?requestedDomain=www.mathworks.com
q = quaternion.create(0.7071, 0., 0., 0.7071)
m33 = matrix33.create_from_quaternion(q)
expected = np.array([
[1., 0., 0.],
[0.,-0.,-1.],
[0., 1.,-0.],
])
self.assertTrue(np.allclose(m33, expected))
# issue #42
q = quaternion.create(*[0.80087974, 0.03166748, 0.59114721,-0.09018753])
m33 = matrix33.create_from_quaternion(q)
q2 = quaternion.create_from_matrix(m33)
print(q, q2)
self.assertTrue(np.allclose(q, q2))
q3 = quaternion.create_from_matrix(m33.T)
self.assertFalse(np.allclose(q, q3))
def test_quaternion_matrix_conversion(self):
# https://au.mathworks.com/help/robotics/ref/quat2rotm.html?requestedDomain=www.mathworks.com
q = quaternion.create(0.7071, 0., 0., 0.7071)
m33 = matrix33.create_from_quaternion(q)
expected = np.array([
[1., 0., 0.],
[0., -0., -1.],
[0., 1., -0.],
])
self.assertTrue(np.allclose(m33, expected))
# issue #42
q = quaternion.create(
*[0.80087974, 0.03166748, 0.59114721, -0.09018753])
m33 = matrix33.create_from_quaternion(q)
q2 = quaternion.create_from_matrix(m33)
print(q, q2)
self.assertTrue(np.allclose(q, q2))
q3 = quaternion.create_from_matrix(m33.T)
self.assertFalse(np.allclose(q, q3))
def test_operators_quaternion(self):
q1 = Quaternion()
q2 = Quaternion.from_x_rotation(0.5)
# add
self.assertRaises(ValueError, lambda: q1 + q2)
# subtract
self.assertRaises(ValueError, lambda: q1 - q2)
# multiply
self.assertTrue(np.array_equal(q1 * q2, quaternion.cross(quaternion.create(), quaternion.create_from_x_rotation(0.5))))
# divide
self.assertRaises(ValueError, lambda: q1 / q2)
# or
self.assertTrue(np.array_equal(q1 | q2, quaternion.dot(quaternion.create(), quaternion.create_from_x_rotation(0.5))))
# inverse
self.assertTrue(np.array_equal(~q2, quaternion.conjugate(quaternion.create_from_x_rotation(0.5))))
def test_operators_matrix44(self):
q = Quaternion()
m = Matrix44.from_x_rotation(0.5)
# add
self.assertRaises(ValueError, lambda: q + m)
# subtract
self.assertRaises(ValueError, lambda: q - m)
# multiply
self.assertTrue(np.array_equal(q * m, quaternion.cross(quaternion.create(), quaternion.create_from_matrix(matrix44.create_from_x_rotation(0.5)))))
# divide
self.assertRaises(ValueError, lambda: q / m)
def test_create(self):
result = quaternion.create()
np.testing.assert_almost_equal(result, [0., 0., 0., 1.], decimal=5)
self.assertTrue(result.dtype == np.float)
def test_cross(self):
q1 = quaternion.create_from_x_rotation(np.pi / 2.0)
q2 = quaternion.create_from_x_rotation(-np.pi / 2.0)
result = quaternion.cross(q1, q2)
np.testing.assert_almost_equal(result, quaternion.create(), decimal=5)
def test_create_from_quaternion_identity(self):
quat = quaternion.create()
result = matrix33.create_from_quaternion(quat)
expected = np.eye(3)
self.assertTrue(np.array_equal(result, expected))
def test_identity(self):
# https://en.wikipedia.org/wiki/Quaternion
i = quaternion.create(1., 0., 0., 0.)
j = quaternion.create(0., 1., 0., 0.)
k = quaternion.create(0., 0., 1., 0.)
one = quaternion.create(0., 0., 0., 1.)
# i * 1 = i
# j * 1 = j
# k * 1 = k
# 1 * i = i
# 1 * j = j
# 1 * k = k
i1 = quaternion.cross(i, one)
j1 = quaternion.cross(j, one)
k1 = quaternion.cross(k, one)
_1i = quaternion.cross(one, i)
_1j = quaternion.cross(one, j)
_1k = quaternion.cross(one, k)
self.assertTrue(np.allclose(i1, _1i, i))
self.assertTrue(np.allclose(j1, _1j, j))
self.assertTrue(np.allclose(k1, _1k, k))
# result = -1
ii = quaternion.cross(i, i)
kk = quaternion.cross(k, k)
jj = quaternion.cross(j, j)
ijk = quaternion.cross(quaternion.cross(i, j), k)
self.assertTrue(np.allclose(ii, -one))
self.assertTrue(np.allclose(jj, -one))
self.assertTrue(np.allclose(kk, -one))
self.assertTrue(np.allclose(ijk, -one))
# ij = k
# ji = -k
# jk = i
# kj = -i
# ki = j
# ik = -j
ij = quaternion.cross(i, j)
ji = quaternion.cross(j, i)
jk = quaternion.cross(j, k)
kj = quaternion.cross(k, j)
ki = quaternion.cross(k, i)
ik = quaternion.cross(i, k)
self.assertTrue(np.allclose(ij, k))
self.assertTrue(np.allclose(ji, -k))
self.assertTrue(np.allclose(jk, i))
self.assertTrue(np.allclose(kj, -i))
self.assertTrue(np.allclose(ki, j))
self.assertTrue(np.allclose(ik, -j))
# -k = ijkk = ij(k^2) = ij(-1)
ijkk = quaternion.cross(quaternion.cross(ij, k), k)
ijk2 = quaternion.cross(ij, quaternion.cross(k, k))
ij_m1 = quaternion.cross(ij, -one)
self.assertTrue(np.allclose(ijkk, ijk2))
self.assertTrue(np.allclose(ijk2, ij_m1))
def test_create_parameters(self):
result = quaternion.create(1.0, 2.0, 3.0, 4.0)
np.testing.assert_almost_equal(result, [1.0, 2.0, 3.0, 4.0], decimal=5)
self.assertTrue(result.dtype == np.float)
def test_cross(self):
q1 = quaternion.create_from_x_rotation(np.pi / 2.0)
q2 = quaternion.create_from_x_rotation(-np.pi / 2.0)
result = quaternion.cross(q1, q2)
np.testing.assert_almost_equal(result, quaternion.create(), decimal=5)
def test_create_parameters(self):
result = quaternion.create(1.0, 2.0, 3.0, 4.0)
np.testing.assert_almost_equal(result, [1.0, 2.0, 3.0, 4.0], decimal=5)
self.assertTrue(result.dtype == np.float)
def test_create(self):
result = quaternion.create()
np.testing.assert_almost_equal(result, [0., 0., 0., 1.], decimal=5)
self.assertTrue(result.dtype == np.float)
def test_identity(self):
# https://en.wikipedia.org/wiki/Quaternion
i = quaternion.create(1., 0., 0., 0.)
j = quaternion.create(0., 1., 0., 0.)
k = quaternion.create(0., 0., 1., 0.)
one = quaternion.create(0., 0., 0., 1.)
# i * 1 = i
# j * 1 = j
# k * 1 = k
# 1 * i = i
# 1 * j = j
# 1 * k = k
i1 = quaternion.cross(i, one)
j1 = quaternion.cross(j, one)
k1 = quaternion.cross(k, one)
_1i = quaternion.cross(one, i)
_1j = quaternion.cross(one, j)
_1k = quaternion.cross(one, k)
self.assertTrue(np.allclose(i1, _1i, i))
self.assertTrue(np.allclose(j1, _1j, j))
self.assertTrue(np.allclose(k1, _1k, k))
# result = -1
ii = quaternion.cross(i, i)
kk = quaternion.cross(k, k)
jj = quaternion.cross(j, j)
ijk = quaternion.cross(quaternion.cross(i, j), k)
self.assertTrue(np.allclose(ii, -one))
self.assertTrue(np.allclose(jj, -one))
self.assertTrue(np.allclose(kk, -one))
self.assertTrue(np.allclose(ijk, -one))
# ij = k
# ji = -k
# jk = i
# kj = -i
# ki = j
# ik = -j
ij = quaternion.cross(i, j)
ji = quaternion.cross(j, i)
jk = quaternion.cross(j, k)
kj = quaternion.cross(k, j)
ki = quaternion.cross(k, i)
ik = quaternion.cross(i, k)
self.assertTrue(np.allclose(ij, k))
self.assertTrue(np.allclose(ji, -k))
self.assertTrue(np.allclose(jk, i))
self.assertTrue(np.allclose(kj, -i))
self.assertTrue(np.allclose(ki, j))
self.assertTrue(np.allclose(ik, -j))
# -k = ijkk = ij(k^2) = ij(-1)
ijkk = quaternion.cross(quaternion.cross(ij, k), k)
ijk2 = quaternion.cross(ij, quaternion.cross(k, k))
ij_m1 = quaternion.cross(ij, -one)
self.assertTrue(np.allclose(ijkk, ijk2))
self.assertTrue(np.allclose(ijk2, ij_m1))
def test_create_from_quaternion_identity(self):
quat = quaternion.create()
result = matrix33.create_from_quaternion(quat)
expected = np.eye(3)
self.assertTrue(np.array_equal(result, expected))
