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_normalise_non_identity(self):
# normalise an identity quaternion
result = quaternion.normalise([1., 2., 3., 4.])
np.testing.assert_almost_equal(result, [
1. / np.sqrt(30.),
np.sqrt(2. / 15.),
np.sqrt(3. / 10.), 2. * np.sqrt(2. / 15.)
],
decimal=5)
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_normalise_batch(self):
# normalise an identity quaternion
result = quaternion.normalise([
[0.,0.,0.,1.],
[1.,2.,3.,4.],
])
expected = [
[0.,0.,0.,1.],
[1./np.sqrt(30.),np.sqrt(2./15.),np.sqrt(3./10.),2.*np.sqrt(2./15.)],
]
np.testing.assert_almost_equal(result, expected, decimal=5)
def test_normalise_batch(self):
# normalise an identity quaternion
result = quaternion.normalise([
[0., 0., 0., 1.],
[1., 2., 3., 4.],
])
expected = [
[0., 0., 0., 1.],
[1. / np.sqrt(30.), np.sqrt(2. / 15.), np.sqrt(3. / 10.), 2. * np.sqrt(2. / 15.)],
]
np.testing.assert_almost_equal(result, expected, decimal=5)
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 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 test_normalise_non_identity(self):
# normalise an identity quaternion
result = quaternion.normalise([1.,2.,3.,4.])
np.testing.assert_almost_equal(result, [1./np.sqrt(30.),np.sqrt(2./15.),np.sqrt(3./10.),2.*np.sqrt(2./15.)], decimal=5)
def test_normalise_identity(self):
# normalise an identity quaternion
result = quaternion.normalise([0.,0.,0.,1.])
np.testing.assert_almost_equal(result, [0.,0.,0.,1.], decimal=5)
def test_normalise_identity(self):
# normalise an identity quaternion
result = quaternion.normalise([0., 0., 0., 1.])
np.testing.assert_almost_equal(result, [0., 0., 0., 1.], decimal=5)
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)
