from sympy.core.numbers import Rational
from sympy.core.singleton import S
from sympy.core.relational import is_eq
from sympy.functions.elementary.complexes import (conjugate, im, re, sign)
from sympy.functions.elementary.exponential import (exp, log as ln)
from sympy.functions.elementary.miscellaneous import sqrt
from sympy.functions.elementary.trigonometric import (acos, asin, atan2)
from sympy.functions.elementary.trigonometric import (cos, sin)
from sympy.simplify.trigsimp import trigsimp
from sympy.integrals.integrals import integrate
from sympy.matrices.dense import MutableDenseMatrix as Matrix
from sympy.core.sympify import sympify, _sympify
from sympy.core.expr import Expr
from sympy.core.logic import fuzzy_not, fuzzy_or
from mpmath.libmp.libmpf import prec_to_dps
def _check_norm(elements, norm):
"""validate if input norm is consistent"""
if norm is not None and norm.is_number:
if norm.is_positive is False:
raise ValueError("Input norm must be positive.")
numerical = all(i.is_number and i.is_real is True for i in elements)
if numerical and is_eq(norm**2, sum(i**2 for i in elements)) is False:
raise ValueError("Incompatible value for norm.")
def _is_extrinsic(seq):
"""validate seq and return True if seq is lowercase and False if uppercase"""
if type(seq) != str:
raise ValueError('Expected seq to be a string.')
if len(seq) != 3:
raise ValueError("Expected 3 axes, got `{}`.".format(seq))
intrinsic = seq.isupper()
extrinsic = seq.islower()
if not (intrinsic or extrinsic):
raise ValueError("seq must either be fully uppercase (for extrinsic "
"rotations), or fully lowercase, for intrinsic "
"rotations).")
i, j, k = seq.lower()
if (i == j) or (j == k):
raise ValueError("Consecutive axes must be different")
bad = set(seq) - set('xyzXYZ')
if bad:
raise ValueError("Expected axes from `seq` to be from "
"['x', 'y', 'z'] or ['X', 'Y', 'Z'], "
"got {}".format(''.join(bad)))
return extrinsic
class Quaternion(Expr):
"""Provides basic quaternion operations.
Quaternion objects can be instantiated as Quaternion(a, b, c, d)
as in (a + b*i + c*j + d*k).
Parameters
==========
norm : None or number
Pre-defined quaternion norm. If a value is given, Quaternion.norm
returns this pre-defined value instead of calculating the norm
Examples
========
>>> from sympy import Quaternion
>>> q = Quaternion(1, 2, 3, 4)
>>> q
1 + 2*i + 3*j + 4*k
Quaternions over complex fields can be defined as :
>>> from sympy import Quaternion
>>> from sympy import symbols, I
>>> x = symbols('x')
>>> q1 = Quaternion(x, x**3, x, x**2, real_field = False)
>>> q2 = Quaternion(3 + 4*I, 2 + 5*I, 0, 7 + 8*I, real_field = False)
>>> q1
x + x**3*i + x*j + x**2*k
>>> q2
(3 + 4*I) + (2 + 5*I)*i + 0*j + (7 + 8*I)*k
Defining symbolic unit quaternions:
>>> from sympy import Quaternion
>>> from sympy.abc import w, x, y, z
>>> q = Quaternion(w, x, y, z, norm=1)
>>> q
w + x*i + y*j + z*k
>>> q.norm()
1
References
==========
.. [1] https://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/
.. [2] https://en.wikipedia.org/wiki/Quaternion
"""
_op_priority = 11.0
is_commutative = False
def __new__(cls, a=0, b=0, c=0, d=0, real_field=True, norm=None):
a, b, c, d = map(sympify, (a, b, c, d))
if any(i.is_commutative is False for i in [a, b, c, d]):
raise ValueError("arguments have to be commutative")
else:
obj = Expr.__new__(cls, a, b, c, d)
obj._a = a
obj._b = b
obj._c = c
obj._d = d
obj._real_field = real_field
obj.set_norm(norm)
return obj
def set_norm(self, norm):
"""Sets norm of an already instantiated quaternion.
Parameters
==========
norm : None or number
Pre-defined quaternion norm. If a value is given, Quaternion.norm
returns this pre-defined value instead of calculating the norm
Examples
========
>>> from sympy import Quaternion
>>> from sympy.abc import a, b, c, d
>>> q = Quaternion(a, b, c, d)
>>> q.norm()
sqrt(a**2 + b**2 + c**2 + d**2)
Setting the norm:
>>> q.set_norm(1)
>>> q.norm()
1
Removing set norm:
>>> q.set_norm(None)
>>> q.norm()
sqrt(a**2 + b**2 + c**2 + d**2)
"""
norm = sympify(norm)
_check_norm(self.args, norm)
self._norm = norm
@property
def a(self):
return self._a
@property
def b(self):
return self._b
@property
def c(self):
return self._c
@property
def d(self):
return self._d
@property
def real_field(self):
return self._real_field
@property
def product_matrix_left(self):
r"""Returns 4 x 4 Matrix equivalent to a Hamilton product from the
left. This can be useful when treating quaternion elements as column
vectors. Given a quaternion $q = a + bi + cj + dk$ where a, b, c and d
are real numbers, the product matrix from the left is:
.. math::
M = \begin{bmatrix} a &-b &-c &-d \\
b & a &-d & c \\
c & d & a &-b \\
d &-c & b & a \end{bmatrix}
Examples
========
>>> from sympy import Quaternion
>>> from sympy.abc import a, b, c, d
>>> q1 = Quaternion(1, 0, 0, 1)
>>> q2 = Quaternion(a, b, c, d)
>>> q1.product_matrix_left
Matrix([
[1, 0, 0, -1],
[0, 1, -1, 0],
[0, 1, 1, 0],
[1, 0, 0, 1]])
>>> q1.product_matrix_left * q2.to_Matrix()
Matrix([
[a - d],
[b - c],
[b + c],
[a + d]])
This is equivalent to:
>>> (q1 * q2).to_Matrix()
Matrix([
[a - d],
[b - c],
[b + c],
[a + d]])
"""
return Matrix([
[self.a, -self.b, -self.c, -self.d],
[self.b, self.a, -self.d, self.c],
[self.c, self.d, self.a, -self.b],
[self.d, -self.c, self.b, self.a]])
@property
def product_matrix_right(self):
r"""Returns 4 x 4 Matrix equivalent to a Hamilton product from the
right. This can be useful when treating quaternion elements as column
vectors. Given a quaternion $q = a + bi + cj + dk$ where a, b, c and d
are real numbers, the product matrix from the left is:
.. math::
M = \begin{bmatrix} a &-b &-c &-d \\
b & a & d &-c \\
c &-d & a & b \\
d & c &-b & a \end{bmatrix}
Examples
========
>>> from sympy import Quaternion
>>> from sympy.abc import a, b, c, d
>>> q1 = Quaternion(a, b, c, d)
>>> q2 = Quaternion(1, 0, 0, 1)
>>> q2.product_matrix_right
Matrix([
[1, 0, 0, -1],
[0, 1, 1, 0],
[0, -1, 1, 0],
[1, 0, 0, 1]])
Note the switched arguments: the matrix represents the quaternion on
the right, but is still considered as a matrix multiplication from the
left.
>>> q2.product_matrix_right * q1.to_Matrix()
Matrix([
[ a - d],
[ b + c],
[-b + c],
[ a + d]])
This is equivalent to:
>>> (q1 * q2).to_Matrix()
Matrix([
[ a - d],
[ b + c],
[-b + c],
[ a + d]])
"""
return Matrix([
[self.a, -self.b, -self.c, -self.d],
[self.b, self.a, self.d, -self.c],
[self.c, -self.d, self.a, self.b],
[self.d, self.c, -self.b, self.a]])
def to_Matrix(self, vector_only=False):
"""Returns elements of quaternion as a column vector.
By default, a Matrix of length 4 is returned, with the real part as the
first element.
If vector_only is True, returns only imaginary part as a Matrix of
length 3.
Parameters
==========
vector_only : bool
If True, only imaginary part is returned.
Default value: False
Returns
=======
Matrix
A column vector constructed by the elements of the quaternion.
Examples
========
>>> from sympy import Quaternion
>>> from sympy.abc import a, b, c, d
>>> q = Quaternion(a, b, c, d)
>>> q
a + b*i + c*j + d*k
>>> q.to_Matrix()
Matrix([
[a],
[b],
[c],
[d]])
>>> q.to_Matrix(vector_only=True)
Matrix([
[b],
[c],
[d]])
"""
if vector_only:
return Matrix(self.args[1:])
else:
return Matrix(self.args)
@classmethod
def from_Matrix(cls, elements):
"""Returns quaternion from elements of a column vector`.
If vector_only is True, returns only imaginary part as a Matrix of
length 3.
Parameters
==========
elements : Matrix, list or tuple of length 3 or 4. If length is 3,
assume real part is zero.
Default value: False
Returns
=======
Quaternion
A quaternion created from the input elements.
Examples
========
>>> from sympy import Quaternion
>>> from sympy.abc import a, b, c, d
>>> q = Quaternion.from_Matrix([a, b, c, d])
>>> q
a + b*i + c*j + d*k
>>> q = Quaternion.from_Matrix([b, c, d])
>>> q
0 + b*i + c*j + d*k
"""
length = len(elements)
if length != 3 and length != 4:
raise ValueError("Input elements must have length 3 or 4, got {} "
"elements".format(length))
if length == 3:
return Quaternion(0, *elements)
else:
return Quaternion(*elements)
@classmethod
def from_euler(cls, angles, seq):
"""Returns quaternion equivalent to rotation represented by the Euler
angles, in the sequence defined by ``seq``.
Parameters
==========
angles : list, tuple or Matrix of 3 numbers
The Euler angles (in radians).
seq : string of length 3
Represents the sequence of rotations.
For intrinsic rotations, seq must be all lowercase and its elements
must be from the set ``{'x', 'y', 'z'}``
For extrinsic rotations, seq must be all uppercase and its elements
must be from the set ``{'X', 'Y', 'Z'}``
Returns
=======
Quaternion
The normalized rotation quaternion calculated from the Euler angles
in the given sequence.
Examples
========
>>> from sympy import Quaternion
>>> from sympy import pi
>>> q = Quaternion.from_euler([pi/2, 0, 0], 'xyz')
>>> q
sqrt(2)/2 + sqrt(2)/2*i + 0*j + 0*k
>>> q = Quaternion.from_euler([0, pi/2, pi] , 'zyz')
>>> q
0 + (-sqrt(2)/2)*i + 0*j + sqrt(2)/2*k
>>> q = Quaternion.from_euler([0, pi/2, pi] , 'ZYZ')
>>> q
0 + sqrt(2)/2*i + 0*j + sqrt(2)/2*k
"""
if len(angles) != 3:
raise ValueError("3 angles must be given.")
extrinsic = _is_extrinsic(seq)
i, j, k = seq.lower()
# get elementary basis vectors
ei = [1 if n == i else 0 for n in 'xyz']
ej = [1 if n == j else 0 for n in 'xyz']
ek = [1 if n == k else 0 for n in 'xyz']
# calculate distinct quaternions
qi = cls.from_axis_angle(ei, angles[0])
qj = cls.from_axis_angle(ej, angles[1])
qk = cls.from_axis_angle(ek, angles[2])
if extrinsic:
return trigsimp(qk * qj * qi)
else:
return trigsimp(qi * qj * qk)
def to_euler(self, seq, angle_addition=True, avoid_square_root=False):
r"""Returns Euler angles representing same rotation as the quaternion,
in the sequence given by ``seq``. This implements the method described
in [1]_.
For degenerate cases (gymbal lock cases), the third angle is
set to zero.
Parameters
==========
seq : string of length 3
Represents the sequence of rotations.
For intrinsic rotations, seq must be all lowercase and its elements
must be from the set ``{'x', 'y', 'z'}``
For extrinsic rotations, seq must be all uppercase and its elements
must be from the set ``{'X', 'Y', 'Z'}``
angle_addition : bool
When True, first and third angles are given as an addition and
subtraction of two simpler ``atan2`` expressions. When False, the
first and third angles are each given by a single more complicated
``atan2`` expression. This equivalent expression is given by:
.. math::
\operatorname{atan_2} (b,a) \pm \operatorname{atan_2} (d,c) =
\operatorname{atan_2} (bc\pm ad, ac\mp bd)
Default value: True
avoid_square_root : bool
When True, the second angle is calculated with an expression based
on ``acos``, which is slightly more complicated but avoids a square
root. When False, second angle is calculated with ``atan2``, which
is simpler and can be better for numerical reasons (some
numerical implementations of ``acos`` have problems near zero).
Default value: False
Returns
=======
Tuple
The Euler angles calculated from the quaternion
Examples
========
>>> from sympy import Quaternion
>>> from sympy.abc import a, b, c, d
>>> euler = Quaternion(a, b, c, d).to_euler('zyz')
>>> euler
(-atan2(-b, c) + atan2(d, a),
2*atan2(sqrt(b**2 + c**2), sqrt(a**2 + d**2)),
atan2(-b, c) + atan2(d, a))
References
==========
.. [1] https://doi.org/10.1371/journal.pone.0276302
"""
if self.is_zero_quaternion():
raise ValueError('Cannot convert a quaternion with norm 0.')
angles = [0, 0, 0]
extrinsic = _is_extrinsic(seq)
i, j, k = seq.lower()
# get index corresponding to elementary basis vectors
i = 'xyz'.index(i) + 1
j = 'xyz'.index(j) + 1
k = 'xyz'.index(k) + 1
if not extrinsic:
i, k = k, i
# check if sequence is symmetric
symmetric = i == k
if symmetric:
k = 6 - i - j
# parity of the permutation
sign = (i - j) * (j - k) * (k - i) // 2
# permutate elements
elements = [self.a, self.b, self.c, self.d]
a = elements[0]
b = elements[i]
c = elements[j]
d = elements[k] * sign
if not symmetric:
a, b, c, d = a - c, b + d, c + a, d - b
if avoid_square_root:
if symmetric:
n2 = self.norm()**2
angles[1] = acos((a * a + b * b - c * c - d * d) / n2)
else:
n2 = 2 * self.norm()**2
angles[1] = asin((c * c + d * d - a * a - b * b) / n2)
else:
angles[1] = 2 * atan2(sqrt(c * c + d * d), sqrt(a * a + b * b))
if not symmetric:
angles[1] -= S.Pi / 2
# Check for singularities in numerical cases
case = 0
if is_eq(c, S.Zero) and is_eq(d, S.Zero):
case = 1
if is_eq(a, S.Zero) and is_eq(b, S.Zero):
case = 2
if case == 0:
if angle_addition:
angles[0] = atan2(b, a) + atan2(d, c)
angles[2] = atan2(b, a) - atan2(d, c)
else:
angles[0] = atan2(b*c + a*d, a*c - b*d)
angles[2] = atan2(b*c - a*d, a*c + b*d)
else: # any degenerate case
angles[2 * (not extrinsic)] = S.Zero
if case == 1:
angles[2 * extrinsic] = 2 * atan2(b, a)
else:
angles[2 * extrinsic] = 2 * atan2(d, c)
angles[2 * extrinsic] *= (-1 if extrinsic else 1)
# for Tait-Bryan angles
if not symmetric:
angles[0] *= sign
if extrinsic:
return tuple(angles[::-1])
else:
return tuple(angles)
@classmethod
def from_axis_angle(cls, vector, angle):
"""Returns a rotation quaternion given the axis and the angle of rotation.
Parameters
==========
vector : tuple of three numbers
The vector representation of the given axis.
angle : number
The angle by which axis is rotated (in radians).
Returns
=======
Quaternion
The normalized rotation quaternion calculated from the given axis and the angle of rotation.
Examples
========
>>> from sympy import Quaternion
>>> from sympy import pi, sqrt
>>> q = Quaternion.from_axis_angle((sqrt(3)/3, sqrt(3)/3, sqrt(3)/3), 2*pi/3)
>>> q
1/2 + 1/2*i + 1/2*j + 1/2*k
"""
(x, y, z) = vector
norm = sqrt(x**2 + y**2 + z**2)
(x, y, z) = (x / norm, y / norm, z / norm)
s = sin(angle * S.Half)
a = cos(angle * S.Half)
b = x * s
c = y * s
d = z * s
# note that this quaternion is already normalized by construction:
# c^2 + (s*x)^2 + (s*y)^2 + (s*z)^2 = c^2 + s^2*(x^2 + y^2 + z^2) = c^2 + s^2 * 1 = c^2 + s^2 = 1
# so, what we return is a normalized quaternion
return cls(a, b, c, d)
@classmethod
def from_rotation_matrix(cls, M):
"""Returns the equivalent quaternion of a matrix. The quaternion will be normalized
only if the matrix is special orthogonal (orthogonal and det(M) = 1).
Parameters
==========
M : Matrix
Input matrix to be converted to equivalent quaternion. M must be special
orthogonal (orthogonal and det(M) = 1) for the quaternion to be normalized.
Returns
=======
Quaternion
The quaternion equivalent to given matrix.
Examples
========
>>> from sympy import Quaternion
>>> from sympy import Matrix, symbols, cos, sin, trigsimp
>>> x = symbols('x')
>>> M = Matrix([[cos(x), -sin(x), 0], [sin(x), cos(x), 0], [0, 0, 1]])
>>> q = trigsimp(Quaternion.from_rotation_matrix(M))
>>> q
sqrt(2)*sqrt(cos(x) + 1)/2 + 0*i + 0*j + sqrt(2 - 2*cos(x))*sign(sin(x))/2*k
"""
absQ = M.det()**Rational(1, 3)
a = sqrt(absQ + M[0, 0] + M[1, 1] + M[2, 2]) / 2
b = sqrt(absQ + M[0, 0] - M[1, 1] - M[2, 2]) / 2
c = sqrt(absQ - M[0, 0] + M[1, 1] - M[2, 2]) / 2
d = sqrt(absQ - M[0, 0] - M[1, 1] + M[2, 2]) / 2
b = b * sign(M[2, 1] - M[1, 2])
c = c * sign(M[0, 2] - M[2, 0])
d = d * sign(M[1, 0] - M[0, 1])
return Quaternion(a, b, c, d)
def __add__(self, other):
return self.add(other)
def __radd__(self, other):
return self.add(other)
def __sub__(self, other):
return self.add(other*-1)
def __mul__(self, other):
return self._generic_mul(self, _sympify(other))
def __rmul__(self, other):
return self._generic_mul(_sympify(other), self)
def __pow__(self, p):
return self.pow(p)
def __neg__(self):
return Quaternion(-self._a, -self._b, -self._c, -self.d)
def __truediv__(self, other):
return self * sympify(other)**-1
def __rtruediv__(self, other):
return sympify(other) * self**-1
def _eval_Integral(self, *args):
return self.integrate(*args)
def diff(self, *symbols, **kwargs):
kwargs.setdefault('evaluate', True)
return self.func(*[a.diff(*symbols, **kwargs) for a in self.args])
def add(self, other):
"""Adds quaternions.
Parameters
==========
other : Quaternion
The quaternion to add to current (self) quaternion.
Returns
=======
Quaternion
The resultant quaternion after adding self to other
Examples
========
>>> from sympy import Quaternion
>>> from sympy import symbols
>>> q1 = Quaternion(1, 2, 3, 4)
>>> q2 = Quaternion(5, 6, 7, 8)
>>> q1.add(q2)
6 + 8*i + 10*j + 12*k
>>> q1 + 5
6 + 2*i + 3*j + 4*k
>>> x = symbols('x', real = True)
>>> q1.add(x)
(x + 1) + 2*i + 3*j + 4*k
Quaternions over complex fields :
>>> from sympy import Quaternion
>>> from sympy import I
>>> q3 = Quaternion(3 + 4*I, 2 + 5*I, 0, 7 + 8*I, real_field = False)
>>> q3.add(2 + 3*I)
(5 + 7*I) + (2 + 5*I)*i + 0*j + (7 + 8*I)*k
"""
q1 = self
q2 = sympify(other)
# If q2 is a number or a SymPy expression instead of a quaternion
if not isinstance(q2, Quaternion):
if q1.real_field and q2.is_complex:
return Quaternion(re(q2) + q1.a, im(q2) + q1.b, q1.c, q1.d)
elif q2.is_commutative:
return Quaternion(q1.a + q2, q1.b, q1.c, q1.d)
else:
raise ValueError("Only commutative expressions can be added with a Quaternion.")
return Quaternion(q1.a + q2.a, q1.b + q2.b, q1.c + q2.c, q1.d
+ q2.d)
def mul(self, other):
"""Multiplies quaternions.
Parameters
==========
other : Quaternion or symbol
The quaternion to multiply to current (self) quaternion.
Returns
=======
Quaternion
The resultant quaternion after multiplying self with other
Examples
========
>>> from sympy import Quaternion
>>> from sympy import symbols
>>> q1 = Quaternion(1, 2, 3, 4)
>>> q2 = Quaternion(5, 6, 7, 8)
>>> q1.mul(q2)
(-60) + 12*i + 30*j + 24*k
>>> q1.mul(2)
2 + 4*i + 6*j + 8*k
>>> x = symbols('x', real = True)
>>> q1.mul(x)
x + 2*x*i + 3*x*j + 4*x*k
Quaternions over complex fields :
>>> from sympy import Quaternion
>>> from sympy import I
>>> q3 = Quaternion(3 + 4*I, 2 + 5*I, 0, 7 + 8*I, real_field = False)
>>> q3.mul(2 + 3*I)
(2 + 3*I)*(3 + 4*I) + (2 + 3*I)*(2 + 5*I)*i + 0*j + (2 + 3*I)*(7 + 8*I)*k
"""
return self._generic_mul(self, _sympify(other))
@staticmethod
def _generic_mul(q1, q2):
"""Generic multiplication.
Parameters
==========
q1 : Quaternion or symbol
q2 : Quaternion or symbol
It is important to note that if neither q1 nor q2 is a Quaternion,
this function simply returns q1 * q2.
Returns
=======
Quaternion
The resultant quaternion after multiplying q1 and q2
Examples
========
>>> from sympy import Quaternion
>>> from sympy import Symbol, S
>>> q1 = Quaternion(1, 2, 3, 4)
>>> q2 = Quaternion(5, 6, 7, 8)
>>> Quaternion._generic_mul(q1, q2)
(-60) + 12*i + 30*j + 24*k
>>> Quaternion._generic_mul(q1, S(2))
2 + 4*i + 6*j + 8*k
>>> x = Symbol('x', real = True)
>>> Quaternion._generic_mul(q1, x)
x + 2*x*i + 3*x*j + 4*x*k
Quaternions over complex fields :
>>> from sympy import I
>>> q3 = Quaternion(3 + 4*I, 2 + 5*I, 0, 7 + 8*I, real_field = False)
>>> Quaternion._generic_mul(q3, 2 + 3*I)
(2 + 3*I)*(3 + 4*I) + (2 + 3*I)*(2 + 5*I)*i + 0*j + (2 + 3*I)*(7 + 8*I)*k
"""
# None is a Quaternion:
if not isinstance(q1, Quaternion) and not isinstance(q2, Quaternion):
return q1 * q2
# If q1 is a number or a SymPy expression instead of a quaternion
if not isinstance(q1, Quaternion):
if q2.real_field and q1.is_complex:
return Quaternion(re(q1), im(q1), 0, 0) * q2
elif q1.is_commutative:
return Quaternion(q1 * q2.a, q1 * q2.b, q1 * q2.c, q1 * q2.d)
else:
raise ValueError("Only commutative expressions can be multiplied with a Quaternion.")
# If q2 is a number or a SymPy expression instead of a quaternion
if not isinstance(q2, Quaternion):
if q1.real_field and q2.is_complex:
return q1 * Quaternion(re(q2), im(q2), 0, 0)
elif q2.is_commutative:
return Quaternion(q2 * q1.a, q2 * q1.b, q2 * q1.c, q2 * q1.d)
else:
raise ValueError("Only commutative expressions can be multiplied with a Quaternion.")
# If any of the quaternions has a fixed norm, pre-compute norm
if q1._norm is None and q2._norm is None:
norm = None
else:
norm = q1.norm() * q2.norm()
return Quaternion(-q1.b*q2.b - q1.c*q2.c - q1.d*q2.d + q1.a*q2.a,
q1.b*q2.a + q1.c*q2.d - q1.d*q2.c + q1.a*q2.b,
-q1.b*q2.d + q1.c*q2.a + q1.d*q2.b + q1.a*q2.c,
q1.b*q2.c - q1.c*q2.b + q1.d*q2.a + q1.a * q2.d,
norm=norm)
def _eval_conjugate(self):
"""Returns the conjugate of the quaternion."""
q = self
return Quaternion(q.a, -q.b, -q.c, -q.d, norm=q._norm)
def norm(self):
"""Returns the norm of the quaternion."""
if self._norm is None: # check if norm is pre-defined
q = self
# trigsimp is used to simplify sin(x)^2 + cos(x)^2 (these terms
# arise when from_axis_angle is used).
self._norm = sqrt(trigsimp(q.a**2 + q.b**2 + q.c**2 + q.d**2))
return self._norm
def normalize(self):
"""Returns the normalized form of the quaternion."""
q = self
return q * (1/q.norm())
def inverse(self):
"""Returns the inverse of the quaternion."""
q = self
if not q.norm():
raise ValueError("Cannot compute inverse for a quaternion with zero norm")
return conjugate(q) * (1/q.norm()**2)
def pow(self, p):
"""Finds the pth power of the quaternion.
Parameters
==========
p : int
Power to be applied on quaternion.
Returns
=======
Quaternion
Returns the p-th power of the current quaternion.
Returns the inverse if p = -1.
Examples
========
>>> from sympy import Quaternion
>>> q = Quaternion(1, 2, 3, 4)
>>> q.pow(4)
668 + (-224)*i + (-336)*j + (-448)*k
"""
p = sympify(p)
q = self
if p == -1:
return q.inverse()
res = 1
if not p.is_Integer:
return NotImplemented
if p < 0:
q, p = q.inverse(), -p
while p > 0:
if p % 2 == 1:
res = q * res
p = p//2
q = q * q
return res
def exp(self):
"""Returns the exponential of q (e^q).
Returns
=======
Quaternion
Exponential of q (e^q).
Examples
========
>>> from sympy import Quaternion
>>> q = Quaternion(1, 2, 3, 4)
>>> q.exp()
E*cos(sqrt(29))
+ 2*sqrt(29)*E*sin(sqrt(29))/29*i
+ 3*sqrt(29)*E*sin(sqrt(29))/29*j
+ 4*sqrt(29)*E*sin(sqrt(29))/29*k
"""
# exp(q) = e^a(cos||v|| + v/||v||*sin||v||)
q = self
vector_norm = sqrt(q.b**2 + q.c**2 + q.d**2)
a = exp(q.a) * cos(vector_norm)
b = exp(q.a) * sin(vector_norm) * q.b / vector_norm
c = exp(q.a) * sin(vector_norm) * q.c / vector_norm
d = exp(q.a) * sin(vector_norm) * q.d / vector_norm
return Quaternion(a, b, c, d)
def _ln(self):
"""Returns the natural logarithm of the quaternion (_ln(q)).
Examples
========
>>> from sympy import Quaternion
>>> q = Quaternion(1, 2, 3, 4)
>>> q._ln()
log(sqrt(30))
+ 2*sqrt(29)*acos(sqrt(30)/30)/29*i
+ 3*sqrt(29)*acos(sqrt(30)/30)/29*j
+ 4*sqrt(29)*acos(sqrt(30)/30)/29*k
"""
# _ln(q) = _ln||q|| + v/||v||*arccos(a/||q||)
q = self
vector_norm = sqrt(q.b**2 + q.c**2 + q.d**2)
q_norm = q.norm()
a = ln(q_norm)
b = q.b * acos(q.a / q_norm) / vector_norm
c = q.c * acos(q.a / q_norm) / vector_norm
d = q.d * acos(q.a / q_norm) / vector_norm
return Quaternion(a, b, c, d)
def _eval_subs(self, *args):
elements = [i.subs(*args) for i in self.args]
norm = self._norm
try:
norm = norm.subs(*args)
except AttributeError:
pass
_check_norm(elements, norm)
return Quaternion(*elements, norm=norm)
def _eval_evalf(self, prec):
"""Returns the floating point approximations (decimal numbers) of the quaternion.
Returns
=======
Quaternion
Floating point approximations of quaternion(self)
Examples
========
>>> from sympy import Quaternion
>>> from sympy import sqrt
>>> q = Quaternion(1/sqrt(1), 1/sqrt(2), 1/sqrt(3), 1/sqrt(4))
>>> q.evalf()
1.00000000000000
+ 0.707106781186547*i
+ 0.577350269189626*j
+ 0.500000000000000*k
"""
nprec = prec_to_dps(prec)
return Quaternion(*[arg.evalf(n=nprec) for arg in self.args])
def pow_cos_sin(self, p):
"""Computes the pth power in the cos-sin form.
Parameters
==========
p : int
Power to be applied on quaternion.
Returns
=======
Quaternion
The p-th power in the cos-sin form.
Examples
========
>>> from sympy import Quaternion
>>> q = Quaternion(1, 2, 3, 4)
>>> q.pow_cos_sin(4)
900*cos(4*acos(sqrt(30)/30))
+ 1800*sqrt(29)*sin(4*acos(sqrt(30)/30))/29*i
+ 2700*sqrt(29)*sin(4*acos(sqrt(30)/30))/29*j
+ 3600*sqrt(29)*sin(4*acos(sqrt(30)/30))/29*k
"""
# q = ||q||*(cos(a) + u*sin(a))
# q^p = ||q||^p * (cos(p*a) + u*sin(p*a))
q = self
(v, angle) = q.to_axis_angle()
q2 = Quaternion.from_axis_angle(v, p * angle)
return q2 * (q.norm()**p)
def integrate(self, *args):
"""Computes integration of quaternion.
Returns
=======
Quaternion
Integration of the quaternion(self) with the given variable.
Examples
========
Indefinite Integral of quaternion :
>>> from sympy import Quaternion
>>> from sympy.abc import x
>>> q = Quaternion(1, 2, 3, 4)
>>> q.integrate(x)
x + 2*x*i + 3*x*j + 4*x*k
Definite integral of quaternion :
>>> from sympy import Quaternion
>>> from sympy.abc import x
>>> q = Quaternion(1, 2, 3, 4)
>>> q.integrate((x, 1, 5))
4 + 8*i + 12*j + 16*k
"""
# TODO: is this expression correct?
return Quaternion(integrate(self.a, *args), integrate(self.b, *args),
integrate(self.c, *args), integrate(self.d, *args))
@staticmethod
def rotate_point(pin, r):
"""Returns the coordinates of the point pin(a 3 tuple) after rotation.
Parameters
==========
pin : tuple
A 3-element tuple of coordinates of a point which needs to be
rotated.
r : Quaternion or tuple
Axis and angle of rotation.
It's important to note that when r is a tuple, it must be of the form
(axis, angle)
Returns
=======
tuple
The coordinates of the point after rotation.
Examples
========
>>> from sympy import Quaternion
>>> from sympy import symbols, trigsimp, cos, sin
>>> x = symbols('x')
>>> q = Quaternion(cos(x/2), 0, 0, sin(x/2))
>>> trigsimp(Quaternion.rotate_point((1, 1, 1), q))
(sqrt(2)*cos(x + pi/4), sqrt(2)*sin(x + pi/4), 1)
>>> (axis, angle) = q.to_axis_angle()
>>> trigsimp(Quaternion.rotate_point((1, 1, 1), (axis, angle)))
(sqrt(2)*cos(x + pi/4), sqrt(2)*sin(x + pi/4), 1)
"""
if isinstance(r, tuple):
# if r is of the form (vector, angle)
q = Quaternion.from_axis_angle(r[0], r[1])
else:
# if r is a quaternion
q = r.normalize()
pout = q * Quaternion(0, pin[0], pin[1], pin[2]) * conjugate(q)
return (pout.b, pout.c, pout.d)
def to_axis_angle(self):
"""Returns the axis and angle of rotation of a quaternion.
Returns
=======
tuple
Tuple of (axis, angle)
Examples
========
>>> from sympy import Quaternion
>>> q = Quaternion(1, 1, 1, 1)
>>> (axis, angle) = q.to_axis_angle()
>>> axis
(sqrt(3)/3, sqrt(3)/3, sqrt(3)/3)
>>> angle
2*pi/3
"""
q = self
if q.a.is_negative:
q = q * -1
q = q.normalize()
angle = trigsimp(2 * acos(q.a))
# Since quaternion is normalised, q.a is less than 1.
s = sqrt(1 - q.a*q.a)
x = trigsimp(q.b / s)
y = trigsimp(q.c / s)
z = trigsimp(q.d / s)
v = (x, y, z)
t = (v, angle)
return t
def to_rotation_matrix(self, v=None, homogeneous=True):
"""Returns the equivalent rotation transformation matrix of the quaternion
which represents rotation about the origin if v is not passed.
Parameters
==========
v : tuple or None
Default value: None
homogeneous : bool
When True, gives an expression that may be more efficient for
symbolic calculations but less so for direct evaluation. Both
formulas are mathematically equivalent.
Default value: True
Returns
=======
tuple
Returns the equivalent rotation transformation matrix of the quaternion
which represents rotation about the origin if v is not passed.
Examples
========
>>> from sympy import Quaternion
>>> from sympy import symbols, trigsimp, cos, sin
>>> x = symbols('x')
>>> q = Quaternion(cos(x/2), 0, 0, sin(x/2))
>>> trigsimp(q.to_rotation_matrix())
Matrix([
[cos(x), -sin(x), 0],
[sin(x), cos(x), 0],
[ 0, 0, 1]])
Generates a 4x4 transformation matrix (used for rotation about a point
other than the origin) if the point(v) is passed as an argument.
"""
q = self
s = q.norm()**-2
# diagonal elements are different according to parameter normal
if homogeneous:
m00 = s*(q.a**2 + q.b**2 - q.c**2 - q.d**2)
m11 = s*(q.a**2 - q.b**2 + q.c**2 - q.d**2)
m22 = s*(q.a**2 - q.b**2 - q.c**2 + q.d**2)
else:
m00 = 1 - 2*s*(q.c**2 + q.d**2)
m11 = 1 - 2*s*(q.b**2 + q.d**2)
m22 = 1 - 2*s*(q.b**2 + q.c**2)
m01 = 2*s*(q.b*q.c - q.d*q.a)
m02 = 2*s*(q.b*q.d + q.c*q.a)
m10 = 2*s*(q.b*q.c + q.d*q.a)
m12 = 2*s*(q.c*q.d - q.b*q.a)
m20 = 2*s*(q.b*q.d - q.c*q.a)
m21 = 2*s*(q.c*q.d + q.b*q.a)
if not v:
return Matrix([[m00, m01, m02], [m10, m11, m12], [m20, m21, m22]])
else:
(x, y, z) = v
m03 = x - x*m00 - y*m01 - z*m02
m13 = y - x*m10 - y*m11 - z*m12
m23 = z - x*m20 - y*m21 - z*m22
m30 = m31 = m32 = 0
m33 = 1
return Matrix([[m00, m01, m02, m03], [m10, m11, m12, m13],
[m20, m21, m22, m23], [m30, m31, m32, m33]])
def scalar_part(self):
r"""Returns scalar part($\mathbf{S}(q)$) of the quaternion q.
Explanation
===========
Given a quaternion $q = a + bi + cj + dk$, returns $\mathbf{S}(q) = a$.
Examples
========
>>> from sympy.algebras.quaternion import Quaternion
>>> q = Quaternion(4, 8, 13, 12)
>>> q.scalar_part()
4
"""
return self.a
def vector_part(self):
r"""
Returns vector part($\mathbf{V}(q)$) of the quaternion q.
Explanation
===========
Given a quaternion $q = a + bi + cj + dk$, returns $\mathbf{V}(q) = bi + cj + dk$.
Examples
========
>>> from sympy.algebras.quaternion import Quaternion
>>> q = Quaternion(1, 1, 1, 1)
>>> q.vector_part()
0 + 1*i + 1*j + 1*k
>>> q = Quaternion(4, 8, 13, 12)
>>> q.vector_part()
0 + 8*i + 13*j + 12*k
"""
return Quaternion(0, self.b, self.c, self.d)
def axis(self):
r"""
Returns the axis($\mathbf{Ax}(q)$) of the quaternion.
Explanation
===========
Given a quaternion $q = a + bi + cj + dk$, returns $\mathbf{Ax}(q)$ i.e., the versor of the vector part of that quaternion
equal to $\mathbf{U}[\mathbf{V}(q)]$.
The axis is always an imaginary unit with square equal to $-1 + 0i + 0j + 0k$.
Examples
========
>>> from sympy.algebras.quaternion import Quaternion
>>> q = Quaternion(1, 1, 1, 1)
>>> q.axis()
0 + sqrt(3)/3*i + sqrt(3)/3*j + sqrt(3)/3*k
See Also
========
vector_part
"""
axis = self.vector_part().normalize()
return Quaternion(0, axis.b, axis.c, axis.d)
def is_pure(self):
"""
Returns true if the quaternion is pure, false if the quaternion is not pure
or returns none if it is unknown.
Explanation
===========
A pure quaternion (also a vector quaternion) is a quaternion with scalar
part equal to 0.
Examples
========
>>> from sympy.algebras.quaternion import Quaternion
>>> q = Quaternion(0, 8, 13, 12)
>>> q.is_pure()
True
See Also
========
scalar_part
"""
return self.a.is_zero
def is_zero_quaternion(self):
"""
Returns true if the quaternion is a zero quaternion or false if it is not a zero quaternion
and None if the value is unknown.
Explanation
===========
A zero quaternion is a quaternion with both scalar part and
vector part equal to 0.
Examples
========
>>> from sympy.algebras.quaternion import Quaternion
>>> q = Quaternion(1, 0, 0, 0)
>>> q.is_zero_quaternion()
False
>>> q = Quaternion(0, 0, 0, 0)
>>> q.is_zero_quaternion()
True
See Also
========
scalar_part
vector_part
"""
return self.norm().is_zero
def angle(self):
r"""
Returns the angle of the quaternion measured in the real-axis plane.
Explanation
===========
Given a quaternion $q = a + bi + cj + dk$ where a, b, c and d
are real numbers, returns the angle of the quaternion given by
.. math::
angle := atan2(\sqrt{b^2 + c^2 + d^2}, {a})
Examples
========
>>> from sympy.algebras.quaternion import Quaternion
>>> q = Quaternion(1, 4, 4, 4)
>>> q.angle()
atan(4*sqrt(3))
"""
return atan2(self.vector_part().norm(), self.scalar_part())
def arc_coplanar(self, other):
"""
Returns True if the transformation arcs represented by the input quaternions happen in the same plane.
Explanation
===========
Two quaternions are said to be coplanar (in this arc sense) when their axes are parallel.
The plane of a quaternion is the one normal to its axis.
Parameters
==========
other : a Quaternion
Returns
=======
True : if the planes of the two quaternions are the same, apart from its orientation/sign.
False : if the planes of the two quaternions are not the same, apart from its orientation/sign.
None : if plane of either of the quaternion is unknown.
Examples
========
>>> from sympy.algebras.quaternion import Quaternion
>>> q1 = Quaternion(1, 4, 4, 4)
>>> q2 = Quaternion(3, 8, 8, 8)
>>> Quaternion.arc_coplanar(q1, q2)
True
>>> q1 = Quaternion(2, 8, 13, 12)
>>> Quaternion.arc_coplanar(q1, q2)
False
See Also
========
vector_coplanar
is_pure
"""
if (self.is_zero_quaternion()) or (other.is_zero_quaternion()):
raise ValueError('Neither of the given quaternions can be 0')
return fuzzy_or([(self.axis() - other.axis()).is_zero_quaternion(), (self.axis() + other.axis()).is_zero_quaternion()])
@classmethod
def vector_coplanar(cls, q1, q2, q3):
r"""
Returns True if the axis of the pure quaternions seen as 3D vectors
q1, q2, and q3 are coplanar.
Explanation
===========
Three pure quaternions are vector coplanar if the quaternions seen as 3D vectors are coplanar.
Parameters
==========
q1
A pure Quaternion.
q2
A pure Quaternion.
q3
A pure Quaternion.
Returns
=======
True : if the axis of the pure quaternions seen as 3D vectors
q1, q2, and q3 are coplanar.
False : if the axis of the pure quaternions seen as 3D vectors
q1, q2, and q3 are not coplanar.
None : if the axis of the pure quaternions seen as 3D vectors
q1, q2, and q3 are coplanar is unknown.
Examples
========
>>> from sympy.algebras.quaternion import Quaternion
>>> q1 = Quaternion(0, 4, 4, 4)
>>> q2 = Quaternion(0, 8, 8, 8)
>>> q3 = Quaternion(0, 24, 24, 24)
>>> Quaternion.vector_coplanar(q1, q2, q3)
True
>>> q1 = Quaternion(0, 8, 16, 8)
>>> q2 = Quaternion(0, 8, 3, 12)
>>> Quaternion.vector_coplanar(q1, q2, q3)
False
See Also
========
axis
is_pure
"""
if fuzzy_not(q1.is_pure()) or fuzzy_not(q2.is_pure()) or fuzzy_not(q3.is_pure()):
raise ValueError('The given quaternions must be pure')
M = Matrix([[q1.b, q1.c, q1.d], [q2.b, q2.c, q2.d], [q3.b, q3.c, q3.d]]).det()
return M.is_zero
def parallel(self, other):
"""
Returns True if the two pure quaternions seen as 3D vectors are parallel.
Explanation
===========
Two pure quaternions are called parallel when their vector product is commutative which
implies that the quaternions seen as 3D vectors have same direction.
Parameters
==========
other : a Quaternion
Returns
=======
True : if the two pure quaternions seen as 3D vectors are parallel.
False : if the two pure quaternions seen as 3D vectors are not parallel.
None : if the two pure quaternions seen as 3D vectors are parallel is unknown.
Examples
========
>>> from sympy.algebras.quaternion import Quaternion
>>> q = Quaternion(0, 4, 4, 4)
>>> q1 = Quaternion(0, 8, 8, 8)
>>> q.parallel(q1)
True
>>> q1 = Quaternion(0, 8, 13, 12)
>>> q.parallel(q1)
False
"""
if fuzzy_not(self.is_pure()) or fuzzy_not(other.is_pure()):
raise ValueError('The provided quaternions must be pure')
return (self*other - other*self).is_zero_quaternion()
def orthogonal(self, other):
"""
Returns the orthogonality of two quaternions.
Explanation
===========
Two pure quaternions are called orthogonal when their product is anti-commutative.
Parameters
==========
other : a Quaternion
Returns
=======
True : if the two pure quaternions seen as 3D vectors are orthogonal.
False : if the two pure quaternions seen as 3D vectors are not orthogonal.
None : if the two pure quaternions seen as 3D vectors are orthogonal is unknown.
Examples
========
>>> from sympy.algebras.quaternion import Quaternion
>>> q = Quaternion(0, 4, 4, 4)
>>> q1 = Quaternion(0, 8, 8, 8)
>>> q.orthogonal(q1)
False
>>> q1 = Quaternion(0, 2, 2, 0)
>>> q = Quaternion(0, 2, -2, 0)
>>> q.orthogonal(q1)
True
"""
if fuzzy_not(self.is_pure()) or fuzzy_not(other.is_pure()):
raise ValueError('The given quaternions must be pure')
return (self*other + other*self).is_zero_quaternion()
def index_vector(self):
r"""
Returns the index vector of the quaternion.
Explanation
===========
Index vector is given by $\mathbf{T}(q)$ multiplied by $\mathbf{Ax}(q)$ where $\mathbf{Ax}(q)$ is the axis of the quaternion q,
and mod(q) is the $\mathbf{T}(q)$ (magnitude) of the quaternion.
Returns
=======
Quaternion: representing index vector of the provided quaternion.
Examples
========
>>> from sympy.algebras.quaternion import Quaternion
>>> q = Quaternion(2, 4, 2, 4)
>>> q.index_vector()
0 + 4*sqrt(10)/3*i + 2*sqrt(10)/3*j + 4*sqrt(10)/3*k
See Also
========
axis
norm
"""
return self.norm() * self.axis()
def mensor(self):
"""
Returns the natural logarithm of the norm(magnitude) of the quaternion.
Examples
========
>>> from sympy.algebras.quaternion import Quaternion
>>> q = Quaternion(2, 4, 2, 4)
>>> q.mensor()
log(2*sqrt(10))
>>> q.norm()
2*sqrt(10)
See Also
========
norm
"""
return ln(self.norm())