diff options
Diffstat (limited to 'intern/python/modules/util/quat.py')
-rw-r--r-- | intern/python/modules/util/quat.py | 109 |
1 files changed, 109 insertions, 0 deletions
diff --git a/intern/python/modules/util/quat.py b/intern/python/modules/util/quat.py new file mode 100644 index 00000000000..d23b1c3f6d9 --- /dev/null +++ b/intern/python/modules/util/quat.py @@ -0,0 +1,109 @@ +"""Quaternion module + + This module provides conversion routines between Matrices, Quaternions (rotations around + an axis) and Eulers. + + (c) 2000, onk@section5.de """ + +# NON PUBLIC XXX + +from math import sin, cos, acos +from util import vect +reload(vect) + +Vector = vect.Vector + +Matrix = vect.Matrix + +class Quat: + """Simple Quaternion class + +Usually, you create a quaternion from a rotation axis (x, y, z) and a given +angle 'theta', defining the right hand rotation: + + q = fromRotAxis((x, y, z), theta) + +This class supports multiplication, providing an efficient way to +chain rotations""" + + def __init__(self, w = 1.0, x = 0.0, y = 0.0, z = 0.0): + self.v = (w, x, y, z) + + def asRotAxis(self): + """returns rotation axis (x, y, z) and angle phi (right hand rotation)""" + phi2 = acos(self.v[0]) + if phi2 == 0.0: + return Vector(0.0, 0.0, 1.0), 0.0 + else: + s = 1 / (sin(phi2)) + + v = Vector(s * self.v[1], s * self.v[2], s * self.v[3]) + return v, 2.0 * phi2 + + def __mul__(self, other): + w1, x1, y1, z1 = self.v + w2, x2, y2, z2 = other.v + + w = w1*w2 - x1*x2 - y1*y2 - z1*z2 + x = w1*x2 + x1*w2 + y1*z2 - z1*y2 + y = w1*y2 - x1*z2 + y1*w2 + z1*x2 + z = w1*z2 + x1*y2 - y1*x2 + z1*w2 + return Quat(w, x, y, z) + + def asMatrix(self): + w, x, y, z = self.v + + v1 = Vector(1.0 - 2.0 * (y*y + z*z), 2.0 * (x*y + w*z), 2.0 * (x*z - w*y)) + v2 = Vector(2.0 * (x*y - w*z), 1.0 - 2.0 * (x*x + z*z), 2.0 * (y*z + w*x)) + v3 = Vector(2.0 * (x*z + w*y), 2.0 * (y*z - w*x), 1.0 - 2.0 * (x*x + y*y)) + + return Matrix(v1, v2, v3) + +# def asEuler1(self, transp = 0): +# m = self.asMatrix() +# if transp: +# m = m.transposed() +# return m.asEuler() + + def asEuler(self, transp = 0): + from math import atan, asin, atan2 + w, x, y, z = self.v + x2 = x*x + z2 = z*z + tmp = x2 - z2 + r = (w*w + tmp - y*y ) + phi_z = atan2(2.0 * (x * y + w * z) , r) + phi_y = asin(2.0 * (w * y - x * z)) + phi_x = atan2(2.0 * (w * x + y * z) , (r - 2.0*tmp)) + + return phi_x, phi_y, phi_z + +def fromRotAxis(axis, phi): + """computes quaternion from (axis, phi)""" + phi2 = 0.5 * phi + s = sin(phi2) + return Quat(cos(phi2), axis[0] * s, axis[1] * s, axis[2] * s) + +#def fromEuler1(eul): + #qx = fromRotAxis((1.0, 0.0, 0.0), eul[0]) + #qy = fromRotAxis((0.0, 1.0, 0.0), eul[1]) + #qz = fromRotAxis((0.0, 0.0, 1.0), eul[2]) + #return qz * qy * qx + +def fromEuler(eul): + from math import sin, cos + e = eul[0] / 2.0 + cx = cos(e) + sx = sin(e) + e = eul[1] / 2.0 + cy = cos(e) + sy = sin(e) + e = eul[2] / 2.0 + cz = cos(e) + sz = sin(e) + + w = cx * cy * cz - sx * sy * sz + x = sx * cy * cz - cx * sy * sz + y = cx * sy * cz + sx * cy * sz + z = cx * cy * sz + sx * sy * cz + return Quat(w, x, y, z) |