1674 lines
46 KiB
Python
1674 lines
46 KiB
Python
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())
|