def orient(self, pitch=None, yaw=None, roll=None):
"""Over-ride the current orientation.
"""
if yaw != None:
quat = quaternion.set_to_rotation_about_y(yaw)
self.transform.orientation = quaternion.cross(
quat, self.transform.orientation)
if pitch != None:
quat = quaternion.set_to_rotation_about_x(pitch)
self.transform.orientation = quaternion.cross(
quat, self.transform.orientation)
if roll != None:
quat = quaternion.set_to_rotation_about_z(roll)
self.transform.orientation = quaternion.cross(
quat, self.transform.orientation)
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_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 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_quaternion(self, quat):
"""Rotates the transform by the specified orientation.
"""
# check for the orientation not changing
if numpy.array_equal(quat, [1.0, 0.0, 0.0, 0.0]):
# don't bother to update anything
return
# order of operations matters here
# our orientation must be the second parameter
self.transform.orientation = quaternion.cross(
quat, self.transform._orientation)
def orient( self, pitch = None, yaw = None, roll = None ):
"""Over-ride the current orientation.
"""
if yaw != None:
quat = quaternion.set_to_rotation_about_y( yaw )
self.transform.orientation = quaternion.cross(
quat,
self.transform.orientation
)
if pitch != None:
quat = quaternion.set_to_rotation_about_x( pitch )
self.transform.orientation = quaternion.cross(
quat,
self.transform.orientation
)
if roll != None:
quat = quaternion.set_to_rotation_about_z( roll )
self.transform.orientation = quaternion.cross(
quat,
self.transform.orientation
)
def _ball_plate_contact(self, step_t: float) -> float:
# NOTE: the x_theta axis creates motion in the Y-axis, and vice versa
# x_theta, y_theta = self._xy_theta_from_nor(self.plate_nor.xyz)
x_theta = self.plate_theta_x
y_theta = self.plate_theta_y
# Equations for acceleration on a plate at rest
# accel = (mass * g * theta) / (mass + inertia / radius^2)
# (y_theta,x are intentional swapped here.)
theta = Vector3([y_theta, -x_theta, 0])
self.ball_acc = (theta / (self.ball_mass + self._ball_inertia() /
(self.ball_radius**2)) * self.ball_mass *
self.gravity)
# get contact displacement
disp, vel = self._motion_for_time(self.ball_vel, self.ball_acc, step_t)
# simplified ball mechanics against a plane
self.ball.x += disp.x
self.ball.y += disp.y
self._update_ball_z()
self.ball_vel = vel
# For rotation on plate motion we use infinite friction and
# perfect ball / plate coupling.
# Calculate the distance we traveled across the plate during
# this time slice.
rot_distance = math.hypot(disp.x, disp.y)
if rot_distance > 0:
# Calculate the fraction of the circumference that we traveled
# (in radians).
rot_angle = rot_distance / self.ball_radius
# Create a quaternion that represents the delta rotation for this time period.
# Note that we translate the (x, y) direction into (y, -x) because we're
# creating a vector that represents the axis of rotation which is normal
# to the direction the ball traveled in the x/y plane.
rot_q = quaternion.normalize(
np.array([
disp.y / rot_distance * math.sin(rot_angle / 2.0),
-disp.x / rot_distance * math.sin(rot_angle / 2.0),
0.0,
math.cos(rot_angle / 2.0),
]))
old_rot = self.ball_qat.xyzw
new_rot = quaternion.cross(quat1=old_rot, quat2=rot_q)
self.ball_qat.xyzw = quaternion.normalize(new_rot)
return 0.0
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 orientation(self):
if self._orientation == None:
if self.parent == None:
# we don't have a parent
# so just use our current local orientation
self._orientation = self._transform.orientation.copy()
else:
# multiply our rotation by our parents
# order is important, our quaternion should
# be the second parameter
self._orientation = quaternion.cross(
self.parent.orientation, self._transform.orientation)
# ensure the quaternion is normalised
self._orientation = quaternion.normalise(self._orientation)
return self._orientation
def rotate_quaternion( self, quat ):
"""Rotates the transform by the specified orientation.
"""
# check for the orientation not changing
if numpy.array_equal(
quat,
[ 1.0, 0.0, 0.0, 0.0 ]
):
# don't bother to update anything
return
# order of operations matters here
# our orientation must be the second parameter
self.transform.orientation = quaternion.cross(
quat,
self.transform._orientation
)
def orientation( self ):
if self._orientation == None:
if self.parent == None:
# we don't have a parent
# so just use our current local orientation
self._orientation = self._transform.orientation.copy()
else:
# multiply our rotation by our parents
# order is important, our quaternion should
# be the second parameter
self._orientation = quaternion.cross(
self.parent.orientation,
self._transform.orientation
)
# ensure the quaternion is normalised
self._orientation = quaternion.normalise( self._orientation )
return self._orientation
def _update( self ):
"""Updates the orientation of the object.
This should be called once all of the yaw / pitch
modifications are applied.
"""
# limit our pitch to +- 1/2 pi
if self.pitch > self.piOver2:
self.pitch = self.piOver2
if self.pitch < -self.piOver2:
self.pitch = -self.piOver2
pitchQuat = quaternion.set_to_rotation_about_x( self.pitch )
yawQuat = quaternion.set_to_rotation_about_y( self.yaw )
quat = quaternion.cross( pitchQuat, yawQuat )
quaternion.normalise( quat )
self.transform.orientation = quat
def _update(self):
"""Updates the orientation of the object.
This should be called once all of the yaw / pitch
modifications are applied.
"""
# limit our pitch to +- 1/2 pi
if self.pitch > self.piOver2:
self.pitch = self.piOver2
if self.pitch < -self.piOver2:
self.pitch = -self.piOver2
pitchQuat = quaternion.set_to_rotation_about_x(self.pitch)
yawQuat = quaternion.set_to_rotation_about_y(self.yaw)
quat = quaternion.cross(pitchQuat, yawQuat)
quaternion.normalise(quat)
self.transform.orientation = quat
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_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_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_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_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_cross(self):
q1 = Quaternion.from_x_rotation(np.pi / 2.0)
q2 = Quaternion.from_y_rotation(np.pi / 2.0)
self.assertTrue(np.allclose(q1.cross(q2), quaternion.cross(q1, q2)))
def test_cross(self):
q1 = Quaternion.from_x_rotation(np.pi / 2.0)
q2 = Quaternion.from_y_rotation(np.pi / 2.0)
self.assertTrue(np.allclose(q1.cross(q2), quaternion.cross(q1, q2)))
def load(self, md5anim, frame):
# we don't need to upload the parents values
# so just leave as 'int'
# the parent can be negative (root is -1), so it must be signed
self.parents = numpy.empty(md5anim.hierarchy.num_joints, dtype='int')
self.positions = numpy.empty((md5anim.hierarchy.num_joints, 3),
dtype='float32')
self.orientations = numpy.empty((md5anim.hierarchy.num_joints, 4),
dtype='float32')
for index, (hierarchy_joint, base_frame_joint) in enumerate(
zip(md5anim.hierarchy, md5anim.base_frame)):
# set the parent now as we can't set it in the named tuple
self.parents[index] = hierarchy_joint.parent
# get the current joint
joint = self.joint(index)
# begin with the original base frame values
joint.position[:] = base_frame_joint.position
joint.orientation[:] = base_frame_joint.orientation
# overlay with values from our frame
# we know which values to get from the joint's start_index
# and the joint's flag
frame_index = hierarchy_joint.start_index
if hierarchy_joint.flags & 1:
joint.position[0] = frame.value(frame_index)
frame_index += 1
if hierarchy_joint.flags & 2:
joint.position[1] = frame.value(frame_index)
frame_index += 1
if hierarchy_joint.flags & 4:
joint.position[2] = frame.value(frame_index)
frame_index += 1
if hierarchy_joint.flags & 8:
joint.orientation[0] = frame.value(frame_index)
frame_index += 1
if hierarchy_joint.flags & 16:
joint.orientation[1] = frame.value(frame_index)
frame_index += 1
if hierarchy_joint.flags & 32:
joint.orientation[2] = frame.value(frame_index)
frame_index += 1
# compute the W component of the quaternion
joint.orientation[3] = compute_quaternion_w(
joint.orientation[0], joint.orientation[1],
joint.orientation[2])
# parents should always be an bone we've
# previously calculated
assert joint.parent < index
# check if the joint has a parent
if joint.parent >= 0:
# get the parent joint
parent = self.joint(joint.parent)
# make this joint relative to the parent
# rotate our position by our parents
rotated_position = quaternion.apply_to_vector(
parent.orientation, joint.position)
# add our parent's position
joint.position[:] = parent.position + rotated_position
# multiply our orientation by our parent's
rotated_orientation = quaternion.cross(parent.orientation,
joint.orientation)
# normalise our orientation
joint.orientation[:] = quaternion.normalise(
rotated_orientation)
