110 lines
2.6 KiB
Python
110 lines
2.6 KiB
Python
|
"""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)
|