Source code for compas.geometry.primitives.quaternion


from __future__ import print_function
from __future__ import absolute_import
from __future__ import division

import math

from compas.geometry import quaternion_multiply
from compas.geometry import quaternion_conjugate
from compas.geometry import quaternion_unitize
from compas.geometry import quaternion_canonize
from compas.geometry import quaternion_norm
from compas.geometry import quaternion_is_unit
from compas.geometry import quaternion_from_matrix

from compas.geometry.primitives import Primitive


__all__ = ['Quaternion']


[docs]class Quaternion(Primitive): r"""Creates a ``Quaternion`` object. Parameters ---------- w : float The scalar (real) part of a quaternion. x, y, z : float Components of the vector (complex, imaginary) part of a quaternion. Attributes ---------- wxyz : list of float, read-only Quaternion data listing the real part first. xyzw : list of float, read-only Quaternion data listing the real part last. norm : float, read-only The length of the quaternion. is_unit : bool, read-only True if the quaternion has unit length. Notes ----- The default convention to represent a quaternion :math:`q` in this module is by four real values :math:`w`, :math:`x`, :math:`y`, :math:`z`. The first value :math:`w` is the scalar (real) part, and :math:`x`, :math:`y`, :math:`z` form the vector (complex, imaginary) part [1]_, so that: .. math:: q = w + xi + yj + zk where :math:`i, j, k` are basis components with following multiplication rules [2]_: .. math:: \begin{align} ii &= jj = kk = ijk = -1 \\ ij &= k, \quad ji = -k \\ jk &= i, \quad kj = -i \\ ki &= j, \quad ik = -j \end{align} Quaternions are associative but not commutative. **Quaternion as rotation.** A rotation through an angle :math:`\theta` around an axis defined by a euclidean unit vector :math:`u = u_{x}i + u_{y}j + u_{z}k` can be represented as a quaternion: .. math:: q = cos(\frac{\theta}{2}) + sin(\frac{\theta}{2}) [u_{x}i + u_{y}j + u_{z}k] i.e.: .. math:: \begin{align} w &= cos(\frac{\theta}{2}) \\ x &= sin(\frac{\theta}{2}) u_{x} \\ y &= sin(\frac{\theta}{2}) u_{y} \\ z &= sin(\frac{\theta}{2}) u_{z} \end{align} For a quaternion to represent a rotation or orientation, it must be unit-length. A quaternion representing a rotation :math:`p` resulting from applying a rotation :math:`r` to a rotation :math:`q`, i.e.: :math:`p = rq`, is also unit-length. References ---------- .. [1] http://mathworld.wolfram.com/Quaternion.html .. [2] http://mathworld.wolfram.com/HamiltonsRules.html .. [3] https://github.com/matthew-brett/transforms3d/blob/master/transforms3d/quaternions.py Examples -------- >>> Q = Quaternion(1.0, 1.0, 1.0, 1.0).unitized() >>> R = Quaternion(0.0,-0.1, 0.2,-0.3).unitized() >>> P = R*Q >>> P.is_unit True """ __slots__ = ['_w', '_x', '_y', '_z']
[docs] def __init__(self, w, x, y, z): super(Quaternion, self).__init__() self._w = None self._x = None self._y = None self._z = None self.w = w self.x = x self.y = y self.z = z
@property def w(self): """float : The W component of the quaternion.""" return self._w @w.setter def w(self, w): self._w = float(w) @property def x(self): """float : The X component of the quaternion.""" return self._x @x.setter def x(self, x): self._x = float(x) @property def y(self): """float : The Y component of the quaternion.""" return self._y @y.setter def y(self, y): self._y = float(y) @property def z(self): """float : The Z component of the quaternion.""" return self._z @z.setter def z(self, z): self._z = float(z) @property def data(self): return {'w': self.w, 'x': self.x, 'y': self.y, 'z': self.z} @data.setter def data(self, data): self.w = data['w'] self.x = data['x'] self.y = data['y'] self.z = data['z'] @property def wxyz(self): """list of float : Quaternion as a list of float in the "wxyz" convention. """ return [self.w, self.x, self.y, self.z] @property def xyzw(self): """list of float : Quaternion as a list of float in the "xyzw" convention. """ return [self.x, self.y, self.z, self.w] @property def norm(self): """float : The length (euclidean norm) of the quaternion. """ return quaternion_norm(self) @property def is_unit(self): """bool : ``True`` if the quaternion is unit-length or ``False`` if otherwise. """ return quaternion_is_unit(self) # ========================================================================== # customization # ========================================================================== def __getitem__(self, key): if key == 0: return self.w if key == 1: return self.x if key == 2: return self.y if key == 3: return self.z raise KeyError def __setitem__(self, key, value): if key == 0: self.w = value return if key == 1: self.x = value return if key == 2: self.y = value if key == 3: self.z = value raise KeyError def __eq__(self, other, tol=1e-05): for v1, v2 in zip(self, other): if math.fabs(v1 - v2) > tol: return False return True def __iter__(self): return iter(self.wxyz) def __repr__(self): return 'Quaternion({:.{prec}f}, {:.{prec}f}, {:.{prec}f}, {:.{prec}f})'.format(self.w, self.x, self.y, self.z, prec=3) def __mul__(self, other): """Multiply operator for two quaternions. Parameters ---------- other : :class:`compas.geometry.Quaternion` or list A Quaternion. Returns ------- :class:`compas.geometry.Quaternion` The product P = R * Q of this quaternion (R) multiplied by other quaternion (Q). Notes ----- Multiplication of two quaternions R*Q can be interpreted as applying rotation R to an orientation Q, provided that both R and Q are unit-length. The result is also unit-length. Multiplication of quaternions is not commutative! Examples -------- >>> Q = Quaternion(1.0, 1.0, 1.0, 1.0).unitized() >>> R = Quaternion(0.0,-0.1, 0.2,-0.3).unitized() >>> P = R*Q >>> P.is_unit True """ p = quaternion_multiply(list(self), list(other)) return Quaternion(*p) # ========================================================================== # constructors # ==========================================================================
[docs] @classmethod def from_data(cls, data): """Construct a quaternion from a data dict. Parameters ---------- data : dict The data dictionary. Returns ------- :class:`compas.geometry.Quaternion` The constructed quaternion. Examples -------- >>> """ return cls(data['w'], data['x'], data['y'], data['z'])
[docs] @classmethod def from_frame(cls, frame): """Creates a quaternion object from a frame. Parameters ---------- frame : :class:`compas.geometry.Frame` Returns ------- :class:`compas.geometry.Quaternion` The new quaternion. Examples -------- >>> from compas.geometry import Frame >>> q = [1., -2., 3., -4.] >>> F = Frame.from_quaternion(q) >>> Q = Quaternion.from_frame(F) >>> allclose(list(Q.canonized()), quaternion_canonize(quaternion_unitize(q))) True """ w, x, y, z = frame.quaternion return cls(w, x, y, z)
[docs] @classmethod def from_matrix(cls, M): """Create a :class:`Quaternion` from a transformation matrix. Parameters ---------- M : :obj:`list` of :obj:`list` of :obj:`float` Returns ------- :class:`compas.geometry.Quaternion` The new quaternion. Example ------- >>> from compas.geometry import matrix_from_euler_angles >>> ea = [0.2, 0.6, 0.2] >>> M = matrix_from_euler_angles(ea) >>> Quaternion.from_matrix(M) Quaternion(0.949, 0.066, 0.302, 0.066) """ return cls(*quaternion_from_matrix(M))
[docs] @classmethod def from_rotation(cls, R): """Create a :class:`Quaternion` from a :class:`compas.geometry.Rotatation`. Parameters ---------- R : :class:`compas.geometry.Rotation` Returns ------- :class:`compas.geometry.Quaternion` The new quaternion. Example ------- >>> from compas.geometry import Frame, Rotation >>> R = Rotation.from_frame(Frame.worldYZ()) >>> Quaternion.from_rotation(R) Quaternion(0.500, 0.500, 0.500, 0.500) """ return cls.from_matrix(R.matrix)
# ========================================================================== # methods # ==========================================================================
[docs] def unitize(self): """Scales the quaternion to make it unit-length. Examples -------- >>> q = Quaternion(1.0, 1.0, 1.0, 1.0) >>> q.is_unit False >>> q.unitize() >>> q.is_unit True """ qu = quaternion_unitize(self) self.w, self.x, self.y, self.z = qu
[docs] def unitized(self): """Returns a quaternion with a unit-length. Examples -------- >>> q = Quaternion(1.0, 1.0, 1.0, 1.0) >>> q.is_unit False >>> p = q.unitized() >>> p.is_unit True """ qu = quaternion_unitize(self) return Quaternion(*qu)
[docs] def canonize(self): """Makes the quaternion canonic. Examples -------- >>> from compas.geometry import Frame >>> q = Quaternion.from_frame(Frame.worldZX()) >>> q Quaternion(-0.500, 0.500, 0.500, 0.500) >>> q.canonize() >>> q Quaternion(0.500, -0.500, -0.500, -0.500) """ qc = quaternion_canonize(self) self.w, self.x, self.y, self.z = qc
[docs] def canonized(self): """Returns a quaternion in canonic form. Returns ------- :class:`compas.geometry.Quaternion` A quaternion in canonic form. Examples -------- >>> from compas.geometry import Frame >>> q = Quaternion.from_frame(Frame.worldZX()) >>> q Quaternion(-0.500, 0.500, 0.500, 0.500) >>> p = q.canonized() >>> p Quaternion(0.500, -0.500, -0.500, -0.500) """ qc = quaternion_canonize(self) return Quaternion(*qc)
[docs] def conjugate(self): """Conjugate the quaternion. Examples -------- >>> q = Quaternion(1.0, 1.0, 1.0, 1.0) >>> q.conjugate() >>> q Quaternion(1.000, -1.000, -1.000, -1.000) """ qc = quaternion_conjugate(self) self.w, self.x, self.y, self.z = qc
[docs] def conjugated(self): """Returns a conjugate quaternion. Returns ------- :class:`compas.geometry.Quaternion` The conjugated quaternion. Examples -------- >>> q = Quaternion(1.0, 1.0, 1.0, 1.0) >>> p = q.conjugated() >>> q Quaternion(1.000, 1.000, 1.000, 1.000) >>> p Quaternion(1.000, -1.000, -1.000, -1.000) """ qc = quaternion_conjugate(self) return Quaternion(*qc)
# ============================================================================== # Main # ============================================================================== if __name__ == "__main__": import doctest from compas.geometry import allclose # noqa: F401 doctest.testmod(globs=globals())