"""
A module to hold and work with Quaternions
"""
# The MIT License (MIT)
#
# Copyright (c) 2016 GTRC.
#
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
#
# The above copyright notice and this permission notice shall be included in
# all copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.
from __future__ import division
import math
[docs]class Quaternion(object):
[docs] def __init__(self, w, x, y, z, norm_error=0.00000001):
self.w = w
self.x = x
self.y = y
self.z = z
self.norm_error = norm_error
@classmethod
[docs] def from_matrix(cls, matrix):
"""Generates a Quaternion from a rotation matrix
Args:
matrix: A rotation matrix, a list of 3, 3 member lists of numbers
Returns:
The constructed quaternion
"""
if not isinstance(matrix, list) and all(isinstance(sub, list) and all(
isinstance(val, (int, float)) for val in sub)
for sub in matrix):
raise ValueError(
"input must be a list of 3 3 member lists of numbers")
w = math.sqrt(1+matrix[0][0]+matrix[1][1]+matrix[2][2])/2
x = (matrix[2][1]-matrix[1][2])/(4*w)
y = (matrix[0][2]-matrix[2][0])/(4*w)
z = (matrix[1][0]-matrix[0][1])/(4*w)
return cls(w, x, y, z)
@classmethod
[docs] def from_euler(cls, values, axes=['x', 'y', 'z']):
"""
Constructs w quaternion using w 3 member list of Euler angles, along
with w list defining their rotation sequence.
Based off of this:
http://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/19770024290.pdf
Args:
values (list of numbers): 3 member list giving the rotations of the
Euler angles in **radians**
axes (list of chars): 3 member list specifying the order of
rotations
Returns:
The constructed quaternion
"""
axes = [x.lower() for x in axes]
if (len(values) != 3 or
not all(isinstance(x, (int, float)) for x in values)):
raise ValueError(
"You must pass exactly 3 numeric values for values")
ht = [x/2.0 for x in values]
if axes == ['x', 'y', 'z']:
w = (-math.sin(ht[0])*math.sin(ht[1])*math.sin(ht[2]) +
math.cos(ht[0])*math.cos(ht[1])*math.cos(ht[2]))
x = (math.sin(ht[0])*math.cos(ht[1])*math.cos(ht[2]) +
math.sin(ht[1])*math.sin(ht[2])*math.cos(ht[0]))
y = (-math.sin(ht[0])*math.sin(ht[2])*math.cos(ht[1]) +
math.sin(ht[1])*math.cos(ht[0])*math.cos(ht[2]))
z = (math.sin(ht[0]) * math.sin(ht[1]) * math.cos(ht[3]) +
math.sin(ht[2]) * math.cos(ht[0]) * math.cos(ht[1]))
elif axes == ['x', 'z', 'y']:
w = (math.sin(ht[0])*math.sin(ht[1])*math.sin(ht[2]) +
math.cos(ht[0])*math.cos(ht[1])*math.cos(ht[2]))
x = (-math.cos(ht[0])*math.sin(ht[1])*math.sin(ht[2]) +
math.sin(ht[0])*math.cos(ht[1])*math.cos(ht[2]))
y = (-math.sin(ht[0])*math.sin(ht[1])*math.cos(ht[2]) +
math.cos(ht[0])*math.cos(ht[1])*math.sin(ht[2]))
z = (math.sin(ht[0])*math.cos(ht[1])*math.sin(ht[2]) +
math.cos(ht[0])*math.sin(ht[1])*math.cos(ht[2]))
elif axes == ['x', 'y', 'x']:
w = math.cos(ht[1])*math.cos((values[0]+values[2])/2)
x = math.cos(ht[1])*math.sin((values[0]+values[2])/2)
y = math.sin(ht[1])*math.cos((values[0]-values[2])/2)
z = math.sin(ht[1])*math.sin((values[0]-values[2])/2)
elif axes == ['x', 'z', 'x']:
w = math.cos(ht[1]) * math.cos((values[0] + values[2]) / 2)
x = math.cos(ht[1]) * math.sin((values[0] + values[2]) / 2)
y = -math.sin(ht[1]) * math.sin((values[0] - values[2]) / 2)
z = math.sin(ht[1]) * math.cos((values[0] - values[2]) / 2)
elif axes == ['y', 'x', 'z']:
w = (math.sin(ht[0]) * math.sin(ht[1]) * math.sin(ht[2]) +
math.cos(ht[0]) * math.cos(ht[1]) * math.cos(ht[2]))
x = (math.sin(ht[0]) * math.cos(ht[1]) * math.sin(ht[2]) +
math.cos(ht[0]) * math.sin(ht[1]) * math.cos(ht[2]))
y = (-math.cos(ht[0]) * math.sin(ht[1]) * math.sin(ht[2]) +
math.sin(ht[0]) * math.cos(ht[1]) * math.cos(ht[2]))
z = (-math.sin(ht[0]) * math.sin(ht[1]) * math.cos(ht[2]) +
math.cos(ht[0]) * math.cos(ht[1]) * math.sin(ht[2]))
elif axes == ['y', 'z', 'x']:
w = (-math.sin(ht[0]) * math.sin(ht[1]) * math.sin(ht[2]) +
math.cos(ht[0]) * math.cos(ht[1]) * math.cos(ht[2]))
x = (math.sin(ht[0]) * math.sin(ht[1]) * math.cos(ht[2]) +
math.cos(ht[0]) * math.cos(ht[1]) * math.sin(ht[2]))
y = (math.sin(ht[0]) * math.cos(ht[1]) * math.cos(ht[2]) +
math.cos(ht[0]) * math.sin(ht[1]) * math.sin(ht[2]))
z = (-math.sin(ht[0]) * math.cos(ht[1]) * math.sin(ht[2]) +
math.cos(ht[0]) * math.sin(ht[1]) * math.cos(ht[2]))
elif axes == ['y', 'x', 'y']:
w = math.cos(ht[1]) * math.cos((values[0] + values[2]) / 2)
x = math.sin(ht[1]) * math.cos((values[0] - values[2]) / 2)
y = math.cos(ht[1]) * math.sin((values[0] + values[2]) / 2)
z = -math.sin(ht[1]) * math.cos((values[0] - values[2]) / 2)
elif axes == ['y', 'z', 'y']:
w = math.cos(ht[1]) * math.cos((values[0] + values[2]) / 2)
x = math.sin(ht[1]) * math.sin((values[0] - values[2]) / 2)
y = math.cos(ht[1]) * math.sin((values[0] + values[2]) / 2)
z = math.sin(ht[1]) * math.cos((values[0] - values[2]) / 2)
elif axes == ['z', 'x', 'y']:
w = (-math.sin(ht[0]) * math.sin(ht[1]) * math.sin(ht[2]) +
math.cos(ht[0]) * math.cos(ht[1]) * math.cos(ht[2]))
x = (-math.sin(ht[0]) * math.cos(ht[1]) * math.sin(ht[2]) +
math.cos(ht[0]) * math.sin(ht[1]) * math.cos(ht[2]))
y = (math.sin(ht[0]) * math.sin(ht[1]) * math.cos(ht[2]) +
math.cos(ht[0]) * math.cos(ht[1]) * math.sin(ht[2]))
z = (math.sin(ht[0]) * math.cos(ht[1]) * math.cos(ht[2]) +
math.cos(ht[0]) * math.sin(ht[1]) * math.sin(ht[2]))
elif axes == ['y', 'y', 'x']:
w = (math.sin(ht[0]) * math.sin(ht[1]) * math.sin(ht[2]) +
math.cos(ht[0]) * math.cos(ht[1]) * math.cos(ht[2]))
x = (-math.sin(ht[0]) * math.sin(ht[1]) * math.cos(ht[2]) +
math.cos(ht[0]) * math.cos(ht[1]) * math.sin(ht[2]))
y = (math.sin(ht[0]) * math.cos(ht[1]) * math.sin(ht[2]) +
math.cos(ht[0]) * math.sin(ht[1]) * math.cos(ht[2]))
z = (-math.cos(ht[0]) * math.sin(ht[1]) * math.sin(ht[2]) +
math.sin(ht[0]) * math.cos(ht[1]) * math.cos(ht[2]))
elif axes == ['z', 'x', 'z']:
w = math.cos(ht[1]) * math.cos((values[0] + values[2]) / 2)
x = math.sin(ht[1]) * math.cos((values[0] - values[2]) / 2)
y = math.sin(ht[1]) * math.sin((values[0] - values[2]) / 2)
z = math.cos(ht[1]) * math.sin((values[0] + values[2]) / 2)
elif axes == ['z', 'y', 'z']:
w = math.cos(ht[1]) * math.cos((values[0] + values[2]) / 2)
x = -math.sin(ht[1]) * math.sin((values[0] - values[2]) / 2)
y = math.sin(ht[1]) * math.cos((values[0] - values[2]) / 2)
z = math.cos(ht[1]) * math.sin((values[0] + values[2]) / 2)
else:
raise ValueError(
"You entered an invalid Euler angle axes sequence")
return cls(w, x, y, z)
@classmethod
[docs] def from_quaternion(cls, quaternion):
"""
Constructs a quaternion from another quaternion.
Args:
quaternion (Quaternion): The quaternion to copy
Returns:
The newly constructed quaternion
"""
if not isinstance(quaternion, Quaternion):
return ValueError("input must be a quaternion")
return cls(quaternion.w, quaternion.x, quaternion.y, quaternion.z)
@classmethod
[docs] def from_translation(cls, translation):
"""
Generates a quaternion from a translation. Meant to be used along with
the dual operator to generate dual quaternions
Args:
translation: A list of three numbers representing the (x,y,z)
translation
Returns:
The quaternion created from the translation
"""
if (len(translation) != 3 or
not all(isinstance(x, (int, float)) for x in translation)):
raise ValueError(
"You must pass exactly 3 numeric values for the translation")
return cls(0, translation[0]/2, translation[1]/2, translation[2]/2)
@classmethod
[docs] def from_axis_angle(cls, axis, angle):
"""
Constructs a quaternion from axis-angle measurements.
Args:
axis: A list of three numbers representing the axis of rotation.
angle: A single number representing the rotation about the axis in
**radians**
Returns:
The constructed quaternion
"""
if (len(axis) != 3 or
not all(isinstance(x, (int, float)) for x in axis)):
raise ValueError(
"You must pass exactly 3 numeric values for the axis")
if not isinstance(angle, (int, float)):
raise ValueError("The angle value must be a single numeric value")
sin_result = math.sin(angle/2)
return cls(math.cos(angle/2), axis[0]*sin_result, axis[1]*sin_result,
axis[2]*sin_result)
def __add__(self, other):
"""
Add together two quaternions.
Args:
other (Quaternion): The quaternion to add.
Returns:
The result of the addition.
"""
if not isinstance(other, Quaternion):
raise ValueError("Must pass in another Quaternion")
return Quaternion(self.w + other.w, self.x + other.x, self.y + other.y,
self.z + other.z)
def __mul__(self, other):
"""
Multiply two quaternions.
Args:
other (Quaternion): The quaternion to multiply by. The caller will
be on the left side and the callee argument
will be on the right side
Returns:
The result of the multiplication.
"""
if isinstance(other, Quaternion):
return Quaternion((self.w * other.w) - (self.x * other.x) -
(self.y * other.y) - (self.z * other.z),
(self.w * other.x) + (self.x * other.w) +
(self.y * other.z) - (self.z * other.y),
(self.w * other.y) - (self.x * other.z) +
(self.y * other.w) + (self.z * other.x),
(self.w * other.z) + (self.x * other.y) -
(self.y * other.x) + (self.z * other.w))
elif isinstance(other, (int, float)):
return Quaternion(self.w * other, self.x * other, self.y * other,
self.z * other)
else:
return NotImplemented
[docs] def conjugate(self):
"""Return the conjugate of the quaternion.
Returns:
The conjugate of the quaternion
"""
return Quaternion(self.w, -self.x, -self.y, -self.z)
# Test: q=(q*)*
# Test: (pq)* = q*p*
[docs] def norm(self):
"""Return the norm of the quaternion
Returns:
The norm of the quaternion
"""
return math.sqrt(math.pow(self.w, 2) + math.pow(self.x, 2) +
math.pow(self.y, 2) + math.pow(self.z, 2))
# Test: norm(conjugate(q))=norm(q)
# Test: norm(pq) = norm(p)norm(q)
[docs] def unit(self):
"""
Return the unit Quaternion
Returns:
Unit quaternion
"""
return self/self.norm()
def __sub__(self, other):
"""Subtract the argument from the caller
Args:
other (Quaternion): The quaternion to subtract by
Returns:
The result of subtraction
"""
if not isinstance(other, Quaternion):
raise ValueError("Must pass in another Quaternion")
return Quaternion(self.w - other.w, self.x - other.x,
self.y - other.y, self.z - other.z)
def __div__(self, other):
"""
Divide a quaternion by a scalar value.
Args:
other (number): The number to divide by
Returns:
The result of division
"""
if isinstance(other, (int, float)):
return Quaternion(self.w / other, self.x / other,
self.y / other, self.z / other)
else:
return NotImplemented
__truediv__ = __div__
[docs] def distance(self, other):
"""
Return the rotational distance between two quaternions.
Args:
other (Quaternion): The other
Returns:
"""
if not isinstance(other, Quaternion):
raise ValueError("Must pass in another Quaternion")
return (self - other).norm()
[docs] def inverse(self):
"""Return the inverse of the quaternion. Specifically, this is the
multiplicative inverse, such that multiplying the inverse of q by q
yields the multiplicative identity.
Returns:
The multiplicative inverse of the quaternion
"""
if abs(self.norm()-1) > self.norm_error:
raise ValueError("The quaternion must be normalized")
if self:
return self.conjugate()/self.norm()
else:
raise ZeroDivisionError("The quaternion has no non-zero values")
# Test: inverse(q) q = Quaternion(1,0,0,0)
# Test: q inverse(q) = Quaternion(1,0,0,0)
# Test: q inverse(q) = inverse(q) q
# Test: inverse(inverse(p))=p
# Test: inverse(pq) = inverse(q)inverse(p)
[docs] def dot(self, other):
"""
Return the quaternion dot product
Args:
other (Quaternion): Quaternion with which to calculate the dot
product
Returns:
The dot product of the two quaternions
"""
if not isinstance(other, Quaternion):
raise ValueError("Must pass in another Quaternion")
return ((self.w * other.w) + (self.x * other.x) + (self.y * other.y) +
(self.z * other.z))
def __nonzero__(self):
"""Determines whether the quaternion is non-zero
Returns:
True if the quaternion is non-zero, false otherwise
"""
return any([self.w, self.x, self.y, self.z])
def __eq__(self, other):
"""Test if two quaternions are equal
Args:
other (Quaternion): The quaternion with which to compare equality
Returns:
true if the quaternions are equal. False otherwise
"""
if not isinstance(other, Quaternion):
raise ValueError("Must pass in another Quaternion")
return (self.w == other.w and self.x == other.x and
self.y == other.y and self.z == other.z)
def __str__(self):
"""Return a string representation of the quaternion in form: <w, x, y, z>
Returns:
A string representation of the quaternion
"""
return ("<" + str(self.w) + ", " + str(self.x) + ", " + str(self.y) +
", " + str(self.z) + ">")
[docs] def almost_equal(self, other, delta=.00000001):
"""Determines whether a quaternion is approximately equal to another
using a naive comparison of the 4 values w, x, y, z
Args:
other:
delta:
Returns:
"""
result = self-other
return (abs(result.w) < delta and abs(result.x) < delta and
abs(result.y) < delta and abs(result.z) < delta)
