''' An implementation of quaternion.
Quaternion implementation but only for rotation related part.
Rotation can be done using :class:`Qrotate` object,
produced from the rotatin axis and the angle.
You may also use :meth:`Qrotate2points`
to create :class:`Qrotate` from two points of before
and after the conversion.
.. codeauthor:: Yoshifumi Futaana
.. autosummary::
Quaternion
Qrotate
QrotateSeries
Qrotate2points
qrotate_to_matrix
matrix_to_qrotate
qrotate_to_params
'''
import logging
_logger = logging.getLogger(__name__)
import numpy as np
[docs]class Quaternion:
''' Quaternion class.
Quaternion, namely, :math:`Q = a + bi + cj + dk`.
>>> q = Quaternion(1, 2, 3, 4)
>>> print(q)
1, 2i, 3j, 4k
Array also supported.
>>> qs = Quaternion([1, 3], [2, -1], [-2, 3], [4, -2])
>>> print(qs)
1, 2i, -2j, 4k
3, -1i, 3j, -2k
``abs``, ``add``, ``sub``, ``mul`` and ``neg`` supported.
For ``radd``, ``rsub`` and ``rmul`` supported.
'''
def __init__(self, rval, ival, jval, kval):
""" Initializer
Instance a quaternion, :math:`Q = a + bi + cj + dk`.
:param rval: *a*
:param ival: *b*
:param jval: *c*
:param kval: *d*
:return: *Q*
.. automethod:: __abs__
.. automethod:: __add__
.. automethod:: __sub__
.. automethod:: __mul__
"""
self.r = np.array(rval)
self.i = np.array(ival)
self.j = np.array(jval)
self.k = np.array(kval)
def __str__(self):
if self.r.shape == ():
return '%.6g, %.6gi, %.6gj, %.6gk' % (self.r, self.i, self.j, self.k)
else:
s = ''
for r, i, j, k in zip(self.r, self.i, self.j, self.k):
s += '%.6g, %.6gi, %.6gj, %.6gk\n' % (r, i, j, k)
return s.strip()
[docs] def conjugate(self):
""" Produce a new conjugate Quaternion.
Conjugate of :math:`Q = a + bi + cj + dk` is
:math:`Q^* = a - bi - cj -dk`.
:return: Conjugate
"""
return Quaternion(self.r, -self.i, -self.j, -self.k)
def __eq__(self, r):
return self.r == r.r and self.i == r.i and self.j == r.j and self.k == r.k
[docs] def __abs__(self):
r''' Absolute value defined by :math:`\sqrt{qq^*}`.
>>> q = Quaternion(1, 3, -2, 5)
>>> print('%.5f' % abs(q))
6.24500
>>> qs = Quaternion([1, 4], [-3, 1], [2, 2], [5, 1])
>>> absqs = abs(qs)
>>> print('%.5f %.5f' % (absqs[0], absqs[1]))
6.24500 4.69042
'''
return np.sqrt(self.r ** 2 + self.i ** 2 + self.j ** 2 + self.k ** 2)
[docs] def __add__(self, b):
'''
Quaternion + Quaternion
>>> print(Quaternion(1, -1, 3, 5) + Quaternion(-2, 3, -1, 4))
-1, 2i, 2j, 9k
Quaternion array + Quaternion array
>>> print((Quaternion([1, 3], [-1, 2], [5, -1], [8, -1]) +
... Quaternion([-2, 1], [7, 2], [9, -1], [2, -5])))
-1, 6i, 14j, 10k
4, 4i, -2j, -6k
Quaternion + Quaternion array
>>> print((Quaternion([1, 3], [-1, 2], [5, -1], [8, -1]) +
... Quaternion(-2, 7, 2, -2)))
-1, 6i, 7j, 6k
1, 9i, 1j, -3k
Quaternion array + Quaternion
>>> print((Quaternion(-2, 7, 2, -2) +
... Quaternion([1, 3], [-1, 2], [5, -1], [8, -1])))
-1, 6i, 7j, 6k
1, 9i, 1j, -3k
Quaternion + scalar
>>> print(Quaternion(-1, 3, 7, 2) + 2.8)
1.8, 3i, 7j, 2k
scalar + Quaternion + scalar
>>> print(2.8 + Quaternion(-1, 3, 7, 2))
1.8, 3i, 7j, 2k
Quaternion array + scalar
>>> print(Quaternion([-1, 0], [3, 8], [7, -2], [2, -3]) + 2.8)
1.8, 3i, 7j, 2k
2.8, 8i, -2j, -3k
scalar + Quaternion array
>>> print(-2.8 + Quaternion([-1, 0], [3, 8], [7, -2], [2, -3]))
-3.8, 3i, 7j, 2k
-2.8, 8i, -2j, -3k
'''
try:
return Quaternion(self.r + b.r, self.i + b.i, self.j + b.j, self.k + b.k)
except AttributeError as e:
return Quaternion(self.r + b, self.i, self.j, self.k)
def __radd__(self, b):
return self.__add__(b)
[docs] def __mul__(self, b):
'''
Quaternion * Quaternion
(Calculation not yet confirmed. Just copy and paste from calc)
>>> print(Quaternion(1, -1, 3, 5) * Quaternion(-2, 3, -1, 4))
-16, 22i, 12j, -14k
Quaternion array - Quaternion array
(Calculation not yet confirmed. Just copy and paste from calc)
>>> print((Quaternion([1, 3], [-1, 2], [5, -1], [8, -1]) *
... Quaternion([-2, 1], [7, 2], [9, -1], [2, -5])))
-56, -53i, 57j, -58k
-7, 12i, 4j, -16k
Quaternion - Quaternion array
(Calculation not yet confirmed. Just copy and paste from calc)
>>> print((Quaternion([1, 3], [-1, 2], [5, -1], [8, -1]) *
... Quaternion(-2, 7, 2, -2)))
11, -17i, 46j, -55k
-20, 21i, 5j, 7k
Quaternion array * Quaternion
(Calculation not yet confirmed. Just copy and paste from calc)
>>> print((Quaternion(-2, 7, 2, -2) *
... Quaternion([1, 3], [-1, 2], [5, -1], [8, -1])))
11, 35i, -62j, 19k
-20, 13i, 11j, -15k
Quaternion * scalar
>>> print(Quaternion(-1, 3, 7, 2) * 3)
-3, 9i, 21j, 6k
scalar * Quaternion
>>> print(4 * Quaternion(-1, 3, 7, 2))
-4, 12i, 28j, 8k
Quaternion array * scalar
>>> print(Quaternion([-1, 0], [3, 8], [7, -2], [2, -3]) * (-2))
2, -6i, -14j, -4k
0, -16i, 4j, 6k
scalar * Quaternion array
>>> print(-2 * Quaternion([-1, 0], [3, 8], [7, -2], [2, -3]))
2, -6i, -14j, -4k
0, -16i, 4j, 6k
'''
r1, i1, j1, k1 = self.r, self.i, self.j, self.k
try:
r2, i2, j2, k2 = b.r, b.i, b.j, b.k
except AttributeError as e:
r2, i2, j2, k2 = b, 0, 0, 0 # Assume the given b is real.
r = r1 * r2 - i1 * i2 - j1 * j2 - k1 * k2
i = r1 * i2 + i1 * r2 + j1 * k2 - k1 * j2
j = r1 * j2 - i1 * k2 + j1 * r2 + k1 * i2
k = r1 * k2 + i1 * j2 - j1 * i2 + k1 * r2
return Quaternion(r, i, j, k)
def __rmul__(self, b):
return self.__mul__(b)
def __neg__(self):
return Quaternion(-self.r, -self.i, -self.j, -self.k)
def __pos__(self):
return Quaternion(self.r, self.i, self.j, self.k)
[docs] def __sub__(self, b):
'''
Quaternion - Quaternion
>>> print(Quaternion(1, -1, 3, 5) - Quaternion(-2, 3, -1, 4))
3, -4i, 4j, 1k
Quaternion array - Quaternion array
>>> print((Quaternion([1, 3], [-1, 2], [5, -1], [8, -1]) -
... Quaternion([-2, 1], [7, 2], [9, -1], [2, -5])))
3, -8i, -4j, 6k
2, 0i, 0j, 4k
Quaternion - Quaternion array
>>> print((Quaternion([1, 3], [-1, 2], [5, -1], [8, -1]) -
... Quaternion(-2, 7, 2, -2)))
3, -8i, 3j, 10k
5, -5i, -3j, 1k
Quaternion array - Quaternion
>>> print((Quaternion(-2, 7, 2, -2) -
... Quaternion([1, 3], [-1, 2], [5, -1], [8, -1])))
-3, 8i, -3j, -10k
-5, 5i, 3j, -1k
Quaternion - scalar
>>> print(Quaternion(-1, 3, 7, 2) - 2.8)
-3.8, 3i, 7j, 2k
scalar - Quaternion + scalar
>>> print(2.8 - Quaternion(-1, 3, 7, 2))
3.8, -3i, -7j, -2k
Quaternion array - scalar
>>> print(Quaternion([-1, 0], [3, 8], [7, -2], [2, -3]) - 2.8)
-3.8, 3i, 7j, 2k
-2.8, 8i, -2j, -3k
scalar - Quaternion array
>>> print(-2.8 - Quaternion([-1, 0], [3, 8], [7, -2], [2, -3]))
-1.8, -3i, -7j, -2k
-2.8, -8i, 2j, 3k
'''
try:
return Quaternion(self.r - b.r, self.i - b.i, self.j - b.j, self.k - b.k)
except AttributeError as e:
return Quaternion(self.r - b, self.i, self.j, self.k)
def __rsub__(self, b):
try:
return Quaternion(-self.r + b.r, -self.i + b.i, -self.j + b.j, -self.k + b.k)
except AttributeError as e:
return Quaternion(-self.r + b, -self.i, -self.j, -self.k)
[docs]def qrotate_to_matrix(qrot):
''' Obtain rotation matrix from quaternian-expression.
Quaternian rotation (:class:`Qrotate` or :class:`QrotateSeries`)
is converted to a matrix. The obtained matrix can convert the vector
equivalently as the quaternion rotation does.
For example, consider the rotation around *x* axis by +90 degrees.
If you use quaternian, the following will do.
>>> rot_x90 = Qrotate([1, 0, 0], 90)
In this case, the vector (0, 1, 0) will be converted to (0, 0, 1)
and the other vector (0, 0, 2) will be converted to (0, -2, 0).
>>> y2 = rot_x90.rotate(0, 1, 0)
>>> print('%.2f %.2f %.2f' % (y2[0], y2[1], y2[2]))
0.00 0.00 1.00
>>> z2 = rot_x90.rotate(0, 0, 2)
>>> print('%.2f %.2f %.2f' % (z2[0], z2[1], z2[2]))
0.00 -2.00 0.00
The corresponding matrix expression is [[1, 0, 0], [0, 0, -1], [0, 1, 0]].
>>> m = qrotate_to_matrix(rot_x90)
>>> from irfpy.util.with_context import printoptions
>>> with printoptions(precision=2, suppress=True):
... print(m)
[[ 1. 0. 0.]
[ 0. 0. -1.]
[ 0. 1. 0.]]
>>> with printoptions(precision=2, suppress=True):
... print(m.dot([0, 1, 0])) # doctest: +NORMALIZE_WHITESPACE
[0. 0. 1.]
>>> with printoptions(precision=2, suppress=True):
... print(m.dot([0, 0, 4])) # doctest: +NORMALIZE_WHITESPACE
[ 0. -4. 0.]
'''
x2 = qrot(1, 0, 0)
y2 = qrot(0, 1, 0)
z2 = qrot(0, 0, 1)
return np.array([[x2[0], y2[0], z2[0]],
[x2[1], y2[1], z2[1]],
[x2[2], y2[2], z2[2]]])
[docs]def matrix_to_qrotate(matrix):
''' Matrix form of rotation converted to quaternion.
The function converts the rotation in matrix-form to quaterion expression.
:param matrix: (3,3) shaped rotation matrix
:returns: Quatenion expression of the rotation
:rtype: :class:`Qrotate`
The conversion from rotation matrix to quaternion is not straightforward.
The theory is in http://marupeke296.com/DXG_No58_RotQuaternionTrans.html
First, an arbitrary quaternion is prepared.
>>> q0 = QrotateSeries()
>>> q0.add_rotation(Qrotate([1, 0, 0], 45))
>>> q0.add_rotation(Qrotate([0, 1, 0], 30))
>>> q0.add_rotation(Qrotate([1, 0, 0], 70))
>>> from irfpy.util.with_context import printoptions
>>> print(q0.q0)
0.518992, 0.814654i, 0.252684j, 0.0560187k
>>> axis, angle = qrotate_to_params(q0)
>>> with printoptions(precision=2, suppress=True): # doctest: +NORMALIZE_WHITESPACE
... print(axis)
[0.95 0.3 0.07]
>>> print('%.2f' % angle)
117.47
To get the rotation in matrix form, you can use :func:`qrotate_to_matrix`.
>>> m0 = qrotate_to_matrix(q0)
>>> with printoptions(precision=2, suppress=True):
... print(m0)
[[ 0.87 0.35 0.35]
[ 0.47 -0.33 -0.82]
[-0.17 0.87 -0.46]]
Now, it can revert to the quaternion form back.
>>> q1 = matrix_to_qrotate(m0)
>>> print(q1.q0)
0.518992, 0.814654i, 0.252684j, 0.0560187k
>>> axis1, angle1 = qrotate_to_params(q1)
>>> with printoptions(precision=2, suppress=True): # doctest: +NORMALIZE_WHITESPACE
... print(axis1)
[0.95 0.3 0.07]
>>> print('%.2f' % angle1)
117.47
.. warning::
Bug left.
The returned quaternion (:class:`Qrotate`) does not have correct
axis and rotation angles.
'''
logger = logging.getLogger(__name__ + '.matrix_to_qrotate')
[[m11, m12, m13], [m21, m22, m23], [m31, m32, m33]] = matrix.T
elem = np.array([m11 - m22 - m33 + 1.,
-m11 + m22 - m33 + 1.,
-m11 - m22 + m33 + 1.,
m11 + m22 + m33 + 1.])
biggestindex = elem.argmax()
logger.debug('Biggest idx=%d' % biggestindex)
logger.debug(' %s' % str(elem))
if elem[biggestindex] < 0:
raise ValueError('Given matrix is something wrong. It is not rotation matrix')
q = np.array([0, 0, 0, 0.])
v = np.sqrt(elem[biggestindex]) * .5
logger.debug('v = %f' % v)
q[biggestindex] = v
mult = 0.25 / v
logger.debug('mult = %f' % mult)
if biggestindex == 0: # x
q[1] = (m12 + m21) * mult
q[2] = (m31 + m13) * mult
q[3] = (m23 - m32) * mult
elif biggestindex == 1: # y
q[0] = (m12 + m21) * mult
q[2] = (m23 + m32) * mult
q[3] = (m31 - m13) * mult
elif biggestindex == 2: # z
q[0] = (m31 + m13) * mult
q[1] = (m23 + m32) * mult
q[3] = (m12 - m21) * mult
elif biggestindex == 3: # w
q[0] = (m23 - m32) * mult
q[1] = (m31 - m13) * mult
q[2] = (m12 - m21) * mult
q = Quaternion(q[3], q[0], q[1], q[2])
logger.debug('q = %s' % str(q))
qr = Qrotate([1, 0, 0], 0)
qr.q0 = q
qr.qc = q.conjugate()
return qr
[docs]def qrotate_to_params(qrot):
r''' Quaternian version of rotation converted to the axis and angle
The quaternian is straightforward to know the axis of rotation
and the rotation angle.
:param qrot: Quaternion rotation
:type qrot: :class:`Qrotate` or :class:`QrotateSeries`
:returns: The rotation axis and the angle in degrees
:rtype: (np.array(*nx*, *ny*, *nz*), double)
A rotation around [3, 4, 5] (= [ 0.42426394, 0.56568525, 0.70710656])
is defined as follows.
>>> rotax = (3, 4, 5) / np.sqrt(50.)
>>> qbase = Qrotate(rotax, 49.0)
Simply apply the function qrotate_to_params to get the parameters.
>>> axis, angle = qrotate_to_params(qbase)
>>> from irfpy.util.with_context import printoptions
>>> with printoptions(precision=2, suppress=True):
... print(axis) # doctest: +NORMALIZE_WHITESPACE
[0.42 0.57 0.71]
>>> print('%.2f' % angle)
49.00
For multiple of ``qbase`` will give us rotation around [3, 4, 5]
by 98 degrees.
>>> q2 = QrotateSeries()
>>> q2.add_rotation(qbase) # First rotation
>>> q2.add_rotation(qbase) # Second rotation
>>> axis, angle = qrotate_to_params(q2)
>>> with printoptions(precision=2, suppress=True):
... print(axis) # doctest: +NORMALIZE_WHITESPACE
[0.42 0.57 0.71]
>>> print('%.2f' % angle)
98.00
If you rotate 4 times, the rotation is 196 degrees.
It exceeds 180 degrees, so that the rotation is considered as
flipped axis, [-3, -4, -5], with 164 degrees (= 360 - 196).
>>> q2.add_rotation(qbase) # Third rotation
>>> q2.add_rotation(qbase) # Fourth rotation
>>> axis, angle = qrotate_to_params(q2)
>>> with printoptions(precision=2, suppress=True):
... print(axis)
[-0.42 -0.57 -0.71]
>>> print('%.2f' % angle)
164.00
**Theory**
The quaternian for a rotation q=(w; x, y, z) means:
.. math::
w = \cos\frac{\theta}{2} \\
x = n_x \sin\frac{\theta}{2} \\
y = n_y \sin\frac{\theta}{2} \\
z = n_z \sin\frac{\theta}{2}
The **n** vector can be simply obtained by
.. math::
\textbf{n} = \pm\frac{(x, y, z)}{\sqrt{x^2+y^2+z^2}}
The ambiguity of the sign is due to which direction you assume
to rotate.
Then, the rotation angle, :math:`\theta`, is retrieved from:
.. math::
\frac{\theta}{2} = \arctan\frac{\sqrt{x^2+y^2+z^2}}{w}
Here I assumed that :math:`\theta` is between 0 and 180 degrees,
because this definition will give us "minimum" rotation.
Under this assumption, *w* should be positive
(:math:`w=\cos\frac{\theta}{2}`)
and if *w* is negative, you can flip the rotation axis by taking minus.
Thus, the formulation is uniquely defined as:
.. math::
\textbf{n} = \textrm{sign}(w)\frac{(x, y, z)}{\sqrt{x^2+y^2+z^2}} \\
\theta = 2\arctan\frac{\sqrt{x^2+y^2+z^2}}{|w|}
'''
q = qrot.q0
w, x, y, z = q.r, q.i, q.j, q.k
sigw = np.sign(w)
absw = np.abs(w)
absxyz = np.sqrt(x * x + y * y + z * z)
if absw == 0: ### Rotation by 180 degrees
return np.array([x, y, z]) / absxyz, 180.
if absxyz == 0: ### Rotation by 0 degrees
return np.array([1, 0, 0]), 0.
nvec = sigw * np.array([x, y, z]) / absxyz
theta = np.rad2deg(2 * np.arctan2(absxyz, absw))
return nvec, theta
[docs]class Qrotate:
''' Rotation with quaternion
The class rotate an object (vector) around the given axis
with the given angle.
First sample is the rotate around z by 90 degrees.
>>> rot_z90 = Qrotate([0, 0, 1], 90)
[1, 0, 0] vector is rotated to [0, 1, 0].
>>> vec = rot_z90.rotate(1, 0, 0)
>>> print('%.2f %.2f %.2f' % (vec[0], vec[1], vec[2]))
0.00 1.00 0.00
Array is also accepted.
>>> vecs = rot_z90.rotate([2, 0], [0, 3], [-1, -2])
>>> print('%.2f %.2f %.2f' % (vecs[0][0], vecs[1][0], vecs[2][0]))
0.00 2.00 -1.00
>>> print('%.2f %.2f %.2f' % (vecs[0][1], vecs[1][1], vecs[2][1]))
-3.00 0.00 -2.00
:meth:`__call__` is implemented in the same manner as :meth:`rotate` does.
.. automethod: __call__
'''
def __init__(self, rotation_axis, rotation_angle_deg):
self.rotax = np.array(rotation_axis)
''' Rotation axis. Normalized. '''
self.rotax = self.rotax / np.linalg.norm(self.rotax)
self.thalf = np.deg2rad(rotation_angle_deg / 2.)
''' Rotation angle (half angle in radians) '''
cos_thalf = np.cos(self.thalf)
sin_thalf = np.sin(self.thalf)
self.q0 = Quaternion(cos_thalf, self.rotax[0] * sin_thalf, self.rotax[1] * sin_thalf, self.rotax[2] * sin_thalf)
''' Quaternion '''
self.qc = Quaternion(cos_thalf, -self.rotax[0] * sin_thalf, -self.rotax[1] * sin_thalf, -self.rotax[2] * sin_thalf)
''' Conjugate quaternion '''
[docs] def rotate(self, vx, vy, vz):
''' Return the rotated vector.
:param vx: X component of vector(s) in the original frame.
:returns: (r_vx, r_vy, r_vz) where r_vi is the converted coordinate of vi
(i=x, y, z).
The same shape of vi. If vi is scalar, r_vi is ()-shaped np.array.
'''
vx = np.array(vx)
vy = np.array(vy)
vz = np.array(vz)
p = Quaternion(np.zeros_like(vx), vx, vy, vz)
converted_p = self.q0 * p * self.qc
return converted_p.i, converted_p.j, converted_p.k
def __call__(self, *args, **kwds):
return self.rotate(*args, **kwds)
[docs]class QrotateSeries:
''' A series of rotation.
>>> qx = Qrotate([1, 0, 0], 90) # 90 deg rotation around x.
>>> axes1 = qx.rotate(0, 1, 0) # y axis rotate 90 deg arnd x, -> +z
>>> print('%.2f %.2f %.2f' % (axes1[0], axes1[1], axes1[2]))
0.00 0.00 1.00
>>> qy = Qrotate([0, 1, 0], 45) # 45 deg rotation around y.
>>> axes2 = qy.rotate(0, 0, 1) # Then, rotate around y. (1/sq2, 0, 1/sq2)
>>> print('%.2f %.2f %.2f' % (axes2[0], axes2[1], axes2[2]))
0.71 0.00 0.71
Using QrotateSeries. First use qx, then qy.
>>> qs = QrotateSeries()
>>> qs.add_rotation(qx)
>>> qs.add_rotation(qy)
>>> axes3 = qs.rotate(0, 1, 0) # Should be same as axes2
>>> print('%.2f %.2f %.2f' % (axes3[0], axes3[1], axes3[2]))
0.71 0.00 0.71
The order is important.
>>> qs2 = QrotateSeries()
>>> qs2.add_rotation(qy) # Y rotation first.
>>> qs2.add_rotation(qx) # Then, x rotataion
>>> axes4 = qs2.rotate(0, 1, 0) # Will give +z.
>>> print('%.2f %.2f %.2f' % (axes4[0], axes4[1], axes4[2]))
0.00 0.00 1.00
'''
logger = logging.getLogger(__name__ + '.QrotateSeries')
def __init__(self, qfirst=Qrotate([1, 0, 0], 0)):
self.q0 = qfirst.q0
self.qc = self.q0.conjugate()
[docs] def add_rotation(self, qnext):
self.q0 = qnext.q0 * self.q0
self.qc = self.q0.conjugate()
self.logger.debug('Q0= %s' % self.q0)
self.logger.debug('Qc= %s' % self.qc)
[docs] def rotate(self, vx, vy, vz):
vx = np.array(vx)
vy = np.array(vy)
vz = np.array(vz)
p = Quaternion(np.zeros_like(vx), vx, vy, vz)
converted_p = self.q0 * p * self.qc
return converted_p.i, converted_p.j, converted_p.k
def __call__(self, *args, **kwds):
return self.rotate(*args, **kwds)
[docs]def Qrotate2points(p0, p1):
''' Qrotate the rotate from p0 to p1 along great circle
Rotation matrix that convert p0 to p1 is derived in a
quaternion form. Length of p0 and p1 is NOT considered.
Therefore, it is more precise that convert p0n to p1n,
where p0n and p1n is the projection of p0 and p1 to
the unit sphere centered at the surface.
For example, rotation from p0n=(1, 0, 0) to p1n=(0, 1, 0)
is the rotation along (0, 0, 1) with 90 degrees.
>>> qrot = Qrotate2points([1, 0, 0], [0, 1, 0])
>>> print(qrot.rotax) # doctest: +NORMALIZE_WHITESPACE
[0. 0. 1.]
>>> print(np.rad2deg(qrot.thalf)) # Half angle so that 45
45.0
If p1n=(0, -1, 0), the rotation is considered along (0, 0, -1)
and angle of 90 degress.
>>> qrot = Qrotate2points([1, 0, 0], [0, -1, 0])
>>> print(qrot.rotax)
[ 0. 0. -1.]
>>> print(np.rad2deg(qrot.thalf)) # Half angle so that 45
45.0
If p0 and p1 is generic,
>>> qrot2 = Qrotate2points([1.8, 2.1, -1.9], [-7.8, 2.6, 3.9])
>>> r2 = qrot2.rotate(1.8, 2.1, -1.9) # Convert p0 getting parallel to p1
>>> print('%.2f %.2f %.2f' % (r2[0], r2[1], r2[2])) #p1 // [-6, 2, 3]
-2.88 0.96 1.44
'''
outer = np.cross(p0, p1)
outerlen = np.linalg.norm(outer)
if outerlen == 0.:
axis = [1., 0, 0]
angle = 0.
else:
axis = outer
p0 = np.array(p0)
p1 = np.array(p1)
p0n = p0 / np.linalg.norm(p0)
p1n = p1 / np.linalg.norm(p1)
angle = np.rad2deg(np.arccos(p0n.dot(p1n)))
return Qrotate(axis, angle)
import unittest
import doctest
[docs]def doctests():
return unittest.TestSuite((
doctest.DocTestSuite(),
))
if __name__ == '__main__':
unittest.main(defaultTest='doctests')