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_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_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)