print("j*j = {0}".format(j*j)) print("k*k = {0}".format(k*k)) print() print("Products of components:") print("o*i = {0}\ti*o = {1}".format(o*i, i*o)) print("o*j = {0}\tj*o = {1}".format(o*j, j*o)) print("o*k = {0}\tk*o = {1}".format(o*k, k*o)) print("i*j = {0}\tj*i = {1}".format(i*j, j*i)) print("i*k = {0}\tk*i = {1}".format(i*k, k*i)) print("j*k = {0}\tk*j = {1}".format(j*k, k*j)) print() p = Quaternion(i) p.setScalar(-3).setJ(2).setK(-1) print("p=({0}, {1}, {2}, {3})".format(p.getScalar(), p.getI(), p.getJ(), p.getK())) # sqrt(15) = 3.87298 print("||p|| = {0} (correct: 3.87298)".format(p.norm())) q = p.reciprocal() qc = "-0.2-0.0666666666667i-0.133333333333j+0.0666666666667k" print("p**(-1) = {0}\n(correct: {1})".format(q, qc)) print("p*p**(-1) = {0}".format(p*q)) print("p**(-1)*p = {0}".format(q*p)) print() q = Quaternion(1, -2, 3, -4) print("q = {0}".format(q)) print("p+q = {0}".format(p+q)) print("p-q = {0}".format(p-q))
class Rotation: """ 3D rotation around an axis, based on quaternion arithmetics. For the mathematical background about quaternion based rotation, see http://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation """ # Private internal instance members: # __r - a vector representing the axis of ratoation (Point3D) # __theta - angle of rotation (in radians) # __q - roatation quaternion def __init__(self, rx=0.0, ry=0.0, rz=0.0, angle=0.0): """ A "constructor" that initializes an object.__class__ Input: - rx - x - component of the vector (default: 0) - ry - y - component of the vector (default: 0) - rz - z - component of the vector (default: 0) - angle - angle of rotation in radians (default: 0) 'rx' may also be an instance of a Point3D. In this case, ry and rz are ignored and rx's components are copied into this object. Note that the vector will automatically be converted into a unit vector. A RotationException is raised if input arguments are of invalid types. """ if not InstanceCheck.isFloat(angle): raise RotationException("Angle must be a float value") self.__theta = angle self.setAxis(rx, ry, rz) def setAxis(self, rx=0.0, ry=0.0, rz=0.0): """ Enters a new vector of the rotation. Rotation angle remains unmodified. Input: - rx - x - component of the vector (default: 0) - ry - y - component of the vector (default: 0) - rz - z - component of the vector (default: 0) 'rx' may also be an instance of a Point3D. In this case, ry and rz are ignored and rx's components are copied into this object. Note that the vector will automatically be converted into a unit vector. A RotationException is raised if input arguments are of invalid types. """ try: self.__r = Point3D(rx, ry, rz) except PointException: raise RotationException("Invalid input argument") self.__update() def setAngle(self, angle=0.0): """ Sets a new angle of rotation. Rotation vector's components remain unmodified. Input: - angle - angle of rotation in radians (default: 0) A RotationException is raised if 'angle' is not a float or integer value. """ if not InstanceCheck.isFloat(angle): raise RotationException("Angle must be float value") self.__theta = angle self.__update() def __update(self): # A private method, called after any rotation component (vector or angle) # is modified. It normalizes the vector (its length must be 1), # updates self.__r and recalculates the rotation quaternion (self.__q). # # A RotationException is raised if any quaternion operation fails. try: # copy vector's components into the quaternion and normalize it: self.__q = Quaternion(0.0, self.__r.x, self.__r.y, self.__r.z).unit() # update __r to a unit vector self.__r.x = self.__q.getI() self.__r.y = self.__q.getJ() self.__r.z = self.__q.getK() # Calculate the rotation quaternion, depending on rot. vector and angle: # For more info, see:: # http://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation self.__q *= math.sin(0.5 * self.__theta) self.__q += math.cos(0.5 * self.__theta) except QuaternionException as qex: raise RotationException("Could not generate a rotation quaternion: '{0}'".format(qex)) def getAxis(self, factor=1.0): """ Returns the axis of rotation, represented by a vector (an instance of Point3D). Note that a unit vector, multiplied by the factor (default: 1) will be returned. """ return Point3D(self.__r.x * factor, self.__r.y * factor, self.__r.z * factor) def getAngle(self): """Returns the angle of rotation in radians.""" return self.__theta def getRotationQuaternion(self): """Returns a rotation quaternion.""" return self.__q def rotate(self, p): """ Performs a rotation of point 'p' around the previously specified axis of rotation by the previoulsy specifed angle. Input: - p - a point ot be rotated (an instance of Point3D) Returns coordinates of the rotated point (an instance of Point3D). A RotationException is raised if 'p' is not an instance of Point3D. """ if not Point3D.isPoint3D(p): raise RotationException("Input must be an instance of Point3D") # For more info about rotation using quaternions, see: # http://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation pq = Quaternion(0, p.x, p.y, p.z) pqr = self.__q * pq * self.__q.conj() return Point3D(pqr.getI(), pqr.getJ(), pqr.getK()) @staticmethod def deg2rad(deg): """Conversion from angle degrees to radians""" return deg * math.pi / 180.0 @staticmethod def rad2deg(rad): """Conversion from radians to angle degrees""" return rad * 180.0 / math.pi