矩阵旋转，仿射变换

import numpy as np
import math as m
  
def Rx(theta):
  return np.matrix([[ 1, 0           , 0           ],
                   [ 0, m.cos(theta),-m.sin(theta)],
                   [ 0, m.sin(theta), m.cos(theta)]])
  
def Ry(theta):
  return np.matrix([[ m.cos(theta), 0, m.sin(theta)],
                   [ 0           , 1, 0           ],
                   [-m.sin(theta), 0, m.cos(theta)]])
  
def Rz(theta):
  return np.matrix([[ m.cos(theta), -m.sin(theta), 0 ],
                   [ m.sin(theta), m.cos(theta) , 0 ],
                   [ 0           , 0            , 1 ]])

phi = m.pi/2
theta = m.pi/4
psi = m.pi/2
print("phi =", phi)
print("theta  =", theta)
print("psi =", psi)
  
  
R = Rz(psi) * Ry(theta) * Rx(phi)
print(np.round(R, decimals=2))

eul1 = m.atan2(R.item(1,2),R.item(0,2))
sp = m.sin(eul1)
cp = m.cos(eul1)
eul2 = m.atan2(cp*R.item(0,2)+sp*R.item(1,2), R.item(2,2))
eul3 = m.atan2(-sp*R.item(0,0)+cp*R.item(1,0),-sp*R.item(0,1)+cp*R.item(1,1))
  
print("phi =", eul1)
print("theta =", eul2)
print("psi =", eul3)

参考：
[1] https://en.wikipedia.org/wiki/Transformation_matrix#Examples_in_3D_computer_graphics
[2] https://www.brainvoyager.com/bv/doc/UsersGuide/CoordsAndTransforms/SpatialTransformationMatrices.html
[3] https://people.computing.clemson.edu/~dhouse/courses/401/notes/affines-matrices.pdf
[4] https://blog.csdn.net/shenquanyue/article/details/103262512
[5] https://zhuanlan.zhihu.com/p/64458650
[6] https://www.meccanismocomplesso.org/en/3d-rotations-and-euler-angles-in-python/
[7] https://www.symbolab.com/solver/matrix-multiply-calculator