irfpy.util.quaternion

An implementation of quaternion.

Quaternion implementation but only for rotation related part.

Rotation can be done using Qrotate object, produced from the rotatin axis and the angle. You may also use Qrotate2points() to create Qrotate from two points of before and after the conversion.

Code author: Yoshifumi Futaana

Quaternion(rval, ival, jval, kval)	Quaternion class.
Qrotate(rotation_axis, rotation_angle_deg)	Rotation with quaternion
QrotateSeries([qfirst])	A series of rotation.
Qrotate2points(p0, p1)	Qrotate the rotate from p0 to p1 along great circle
qrotate_to_matrix(qrot)	Obtain rotation matrix from quaternian-expression.
matrix_to_qrotate(matrix)	Matrix form of rotation converted to quaternion.
qrotate_to_params(qrot)	Quaternian version of rotation converted to the axis and angle

class irfpy.util.quaternion.Quaternion(rval, ival, jval, kval)

Bases: object

Quaternion class.

Quaternion, namely, \(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.

Initializer

Instance a quaternion, \(Q = a + bi + cj + dk\).

Parameters:

• rval – a

• ival – b

• jval – c

• kval – d

Returns:

Q

__abs__()

Absolute value defined by \(\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

__add__(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

__sub__(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

__mul__(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

conjugate()

Produce a new conjugate Quaternion.

Conjugate of \(Q = a + bi + cj + dk\) is \(Q^* = a - bi - cj -dk\).

Returns:

Conjugate

irfpy.util.quaternion.qrotate_to_matrix(qrot)

Obtain rotation matrix from quaternian-expression.

Quaternian rotation (Qrotate or 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]))   
[0.  0.  1.]

>>> with printoptions(precision=2, suppress=True):
...     print(m.dot([0, 0, 4]))   
[ 0. -4.  0.]

irfpy.util.quaternion.matrix_to_qrotate(matrix)

Matrix form of rotation converted to quaternion.

The function converts the rotation in matrix-form to quaterion expression.

Parameters:

matrix – (3,3) shaped rotation matrix

Returns:

Quatenion expression of the rotation

Return type:

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):   
...     print(axis)
[0.95  0.3   0.07]
>>> print('%.2f' % angle)
117.47

To get the rotation in matrix form, you can use 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):   
...     print(axis1)
[0.95  0.3   0.07]
>>> print('%.2f' % angle1)
117.47

Warning

Bug left.

The returned quaternion (Qrotate) does not have correct axis and rotation angles.

irfpy.util.quaternion.qrotate_to_params(qrot)

Quaternian version of rotation converted to the axis and angle

The quaternian is straightforward to know the axis of rotation and the rotation angle.

Parameters:

qrot (Qrotate or QrotateSeries) – Quaternion rotation

Returns:

The rotation axis and the angle in degrees

Return type:

(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)   
[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)   
[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:

\[\begin{split}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}\end{split}\]

The n vector can be simply obtained by

\[\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, \(\theta\), is retrieved from:

\[\frac{\theta}{2} = \arctan\frac{\sqrt{x^2+y^2+z^2}}{w}\]

Here I assumed that \(\theta\) is between 0 and 180 degrees, because this definition will give us “minimum” rotation. Under this assumption, w should be positive (\(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:

\[\begin{split}\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|}\end{split}\]

class irfpy.util.quaternion.Qrotate(rotation_axis, rotation_angle_deg)

Bases: object

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

__call__() is implemented in the same manner as rotate() does.

rotax

Rotation axis. Normalized.

thalf

Rotation angle (half angle in radians)

q0

Quaternion

qc

Conjugate quaternion

rotate(vx, vy, vz)

Return the rotated vector.

Parameters:

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.

class irfpy.util.quaternion.QrotateSeries(qfirst=<irfpy.util.quaternion.Qrotate object>)

Bases: object

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 = <Logger irfpy.util.quaternion.QrotateSeries (DEBUG)>

add_rotation(qnext)

rotate(vx, vy, vz)

irfpy.util.quaternion.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)   
[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

irfpy.util.quaternion.doctests()