'''
Routines for working with rotation matrices
'''
'''
author : Thomas Haslwanter
date : oct-2013
version : 1.5
'''
import numpy as np
import sympy
from collections import namedtuple
[docs]def R1(psi):
'''Rotation about the 1-axis.
The argument is entered in degree.
Parameters
----------
psi : rotation angle about the 1-axis [deg]
Returns
-------
R1 : rotation matrix, for rotation about the 1-axis
Example
-------
>>> thLib.Rotmat.R1(45)
array([[ 1. , 0. , 0. ],
[ 0. , 0.70710678, -0.70710678],
[ 0. , 0.70710678, 0.70710678]])
'''
# convert from degrees into radian:
psi = psi * np.pi/180;
R = np.array([[1, 0, 0],
[0, np.cos(psi), -np.sin(psi)],
[0, np.sin(psi), np.cos(psi)]])
return R
[docs]def R2(phi):
'''Rotation about the 2-axis.
The argument is entered in degree.
Parameters
----------
phi : rotation angle about the 1-axis [deg]
Returns
-------
R2 : rotation matrix, for rotation about the 2-axis
Example
-------
>>> thLib.Rotmat.R2(45)
array([[ 0.70710678, 0. , 0.70710678],
[ 0. , 1. , 0. ],
[-0.70710678, 0. , 0.70710678]])
'''
# convert from degrees into radian:
phi = phi * np.pi/180;
R = np.array([[np.cos(phi), 0, np.sin(phi)],
[0, 1, 0],
[ -np.sin(phi), 0, np.cos(phi)]])
return R
[docs]def R3(theta):
'''Rotation about the 3-axis.
The argument is entered in degree.
Parameters
----------
theta : rotation angle about the 1-axis [deg]
Returns
-------
R3 : rotation matrix, for rotation about the 3-axis
Example
-------
>>> thLib.Rotmat.R3(45)
array([[ 0.70710678, -0.70710678, 0. ],
[ 0.70710678, 0.70710678, 0. ],
[ 0. , 0. , 1. ]])
'''
# convert from degrees into radian:
theta = theta * np.pi/180;
R = np.array([[np.cos(theta), -np.sin(theta), 0],
[np.sin(theta), np.cos(theta), 0],
[0, 0, 1]])
return R
[docs]def rotmat2Fick(R):
'''
This function takes a rotation matrix, and calculates
the corresponding Fick-angles.
Parameters
----------
R : rotation matrix
Returns
-------
psi : torsional position (rotation about 1-axis)
phi : vertical position (rotation about 2-axis)
theta : horizontal position (rotation about 3-axis)
Notes
-----
The following formulas are used:
phi = -asin(R31);
theta = asin(R21/cos(phi));
psi = asin(R32/cos(phi));
R_31 = n(1)*n(3) - n(2)*sin(rho) - n(1)*n(3)*cos(rho);
R_21 = n(1)*n(2) + n(3)*sin(rho) - n(1)*n(2)*cos(rho);
R_32 = n(2)*n(3) + n(1)*sin(rho) - n(2)*n(3)*cos(rho);
Note that it is assumed that psi < pi;
'''
phi = -np.arcsin(R[2,0])
theta = np.arcsin(R[1,0]/np.cos(phi))
psi = np.arcsin(R[2,1]/np.cos(phi))
Fick = namedtuple('Fick', ['psi', 'phi', 'theta'])
return Fick(psi, phi, theta)
[docs]def rotmat2Helmholtz(R):
'''
This function takes a rotation matrix, and calculates
the corresponding Helmholtz-angles.
Parameters
----------
R : rotation matrix
Returns
-------
psi : torsional position (rotation about 1-axis)
phi : vertical position (rotation about 2-axis)
theta : horizontal position (rotation about 3-axis)
Notes
-----
The following forulas are used:
theta = asin(R21);
phi = -asin(R31/cos(theta));
psi = -asin(R23/cos(theta));
R_21 = n(1)*n(2) + n(3)*sin(rho) - n(1)*n(2)*cos(rho);
R_31 = n(1)*n(3) - n(2)*sin(rho) - n(1)*n(3)*cos(rho);
R_23 = n(2)*n(3) - n(1)*sin(rho) - n(2)*n(3)*cos(rho);
Note that it is assumed that psi < pi;
'''
theta = np.arcsin(R[1,0])
phi = -np.arcsin(R[2,0]/np.cos(theta))
psi = -np.arcsin(R[1,2]/np.cos(theta))
Helm = namedtuple('Helm', ['psi', 'phi', 'theta'])
return Fick(psi, phi, theta)
[docs]def R1_s():
''' Symbolic rotation matrix about the 1-axis, by an angle psi
You can e.g. generate a Fick-matrix, with
R_Fick = R3_s() * R2_s() * R1_s()
'''
psi = sympy.Symbol('psi')
return sympy.Matrix([[1,0,0],
[0, sympy.cos(psi), -sympy.sin(psi)],
[0, sympy.sin(psi), sympy.cos(psi)]])
[docs]def R2_s():
''' Symbolic rotation matrix about the 2-axis, by an angle phi
You can e.g. generate a Fick-matrix, with
R_Fick = R3_s() * R2_s() * R1_s()
'''
phi = sympy.Symbol('phi')
return sympy.Matrix([[sympy.cos(phi),0, sympy.sin(phi)],
[0,1,0],
[-sympy.sin(phi), 0, sympy.cos(phi)]])
[docs]def R3_s():
''' Symbolic rotation matrix about the 3-axis, by an angle theta
You can e.g. generate a Fick-matrix, with
R_Fick = R3_s() * R2_s() * R1_s()
'''
theta = sympy.Symbol('theta')
return sympy.Matrix([[sympy.cos(theta), -sympy.sin(theta), 0],
[sympy.sin(theta), sympy.cos(theta), 0],
[0, 0, 1]])