Quaternion Rotation Python Class
Spent a few days learning Quaternions using the "3D Maths Primer for Graphics and Game Development" book written by F. Dunn and I. Parberry. Created a simple Quaternion class in Python and tested it in Maya. The video shows the results.
import math
import Vector3 as vc
class Quaternion(object):
def __init__(self, w = 1, Nx = 0, Ny = 0, Nz = 0, **kwargs):
self.q = [w, vc.Vector3(Nx, Ny, Nz)]
try:
if kwargs["degree"]:
self.q[0] = self.degree_to_radians(w)
except:
pass
try:
if kwargs["rotation"]:
self.q[1].unit_vector()
self.q[1].v[0] = math.sin(self.q[0] / 2.0) * self.q[1].x
self.q[1].v[1] = math.sin(self.q[0] / 2.0) * self.q[1].y
self.q[1].v[2] = math.sin(self.q[0] / 2.0) * self.q[1].z
self.q[0] = math.cos(self.q[0] / 2.0)
except:
pass
def __repr__(self):
return "Quaternion(%s, %s)" %(self.q[0], self.q[1])
def __mul__(self, b):
"""
Multiply two quternions. Retrun result as a quaternion
"""
w = (self.q[0] * b.q[0]) - (self.q[1].x * b.q[1].x) - (self.q[1].y * b.q[1].y) - (self.q[1].z * b.q[1].z)
Nx = (self.q[0] * b.q[1].x) + (self.q[1].x * b.q[0]) + (self.q[1].y * b.q[1].z) - (self.q[1].z * b.q[1].y)
Ny = (self.q[0] * b.q[1].y) + (self.q[1].y * b.q[0]) + (self.q[1].z * b.q[1].x) - (self.q[1].x * b.q[1].z)
Nz = (self.q[0] * b.q[1].z) + (self.q[1].z * b.q[0]) + (self.q[1].x * b.q[1].y) - (self.q[1].y * b.q[1].x)
return Quaternion(w, Nx, Ny, Nz)
def dot(self, other):
"""
return the dot product between two quaternions
"""
return (self.q[0] * other.q[0]) + (self.q[1].x * other.q[1].x) + (self.q[1].y * other.q[1].y) + (self.q[1].z * other.q[1].z)
def conjugate(self, **kwargs):
"""
Conjugates a quaternion. If argument "mutate" is set to True
the object values are altered. Else returns a new Quaternion
with the values.
"""
try:
if kwargs["mutate"]:
self.q[1].v[0] = -self.q[1].x
self.q[1].v[1] = -self.q[1].y
self.q[1].v[2] = -self.q[1].z
except:
return Quaternion(self.q[0], -self.q[1].x, -self.q[1].y, -self.q[1].z)
def negative(self):
return Quaternion( -self.q[0], -self.q[1].v[0], -self.q[1].v[1], -self.q[1].v[2])
def exponentiation(self, exponent):
# Check for the case of an identity quaternion
# This will protect against divide by zero
if (abs(self.q[0]) < 0.9999):
# Check vector is unit vector
self.q[1].unit_vector()
# Extract th half angle alhpa (alpha = theta/2)
# Compute new alpha value
# Compute new w value
alpha = math.acos(self.q[0])
new_alpha = alpha * float(exponent)
self.q[0] = math.cos(new_alpha)
#Compute new x, y and z values
mult = math.sin(new_alpha)
self.q[1].v[0] *= mult
self.q[1].v[1] *= mult
self.q[1].v[2] *= mult
def slerp(self, other, t):
"""slerp between two quaternions based on the value of t, which
ranges between 0 and 1. Slerp finds the shortest distance therefore
a rotation of 270 clockwise is equivalent to a 90 degrees counter-clockwise
rotation."""
t = float(t)
cos_omega = self.dot(other)
if (cos_omega < 0.0):
other.negative()
cos_omega = -cos_omega
k = [0, 0]
if (cos_omega > 0.9999):
k[0] = 1.0 - t
k[1] = t
else:
sin_omega = math.sqrt(1.0 - (cos_omega * cos_omega))
omega = math.atan2(sin_omega, cos_omega)
one_over_sin_omega = 1.0 / sin_omega
k[0] = math.sin((1.0 - t) * omega) * one_over_sin_omega
k[1] = math.sin(t * omega) * one_over_sin_omega
return Quaternion( (self.q[0] * k[0]) + (other.q[0] * k[1]),
(self.q[1].x * k[0]) + (other.q[1].x * k[1]),
(self.q[1].y * k[0]) + (other.q[1].y * k[1]),
(self.q[1].z * k[0]) + (other.q[1].z * k[1]))
def difference(self, other):
return other * self.conjugate()
def degree_to_radians(self, w):
return (w / 180.0) * 3.14169265
@property
def w(self):
"""
return the w component of the quaternion
"""
return self.q[0]