已知空间中某个3d物体的欧拉角(p,h,b)，求该物体绕固定轴(v)旋转固定角度(theta)后该物体的欧拉角p',h',b'

问题：

已知空间中某个3d物体的欧拉角(p,h,b)，求该物体绕固定轴(v)旋转固定角度(theta)后该物体的欧拉角p’,h’,b’

步骤：

1.根据欧拉角转矩阵公式，将由欧拉角（p,h,b）转为物体原状态矩阵ori_matrix

2.由轴角对转四元数，将（v,theta）转换为旋转四元数(x,y,z,w)

3.根据四元数转矩阵，将四元数(x,y,z,w)转为旋转矩阵rotate_matrix

4.原状态矩阵ori_matrix和旋转矩阵相乘得到旋转后状态矩阵result_matrix

5.根据矩阵转欧拉角，将旋转后物体矩阵转为欧拉角

# 欧拉角转矩阵
def eular_to_matrix(p, h, b):
# p,h,b：欧拉角，角度制
pi = 3.1415926
p = p*(pi/180)
h = h*(pi/180)
b = b*(pi/180)
cos_p, sin_p = math.cos(p), math.sin(p)
cos_h, sin_h = math.cos(h), math.sin(h)
cos_b, sin_b = math.cos(b), math.sin(b)
matrix = (cos_h*cos_b+sin_h*sin_p*sin_b,
-cos_h*sin_b+sin_h*sin_p*cos_b,
sin_h*cos_p,
0,

sin_b*cos_p,
cos_b*cos_p,
-sin_p,
0,

-sin_h*cos_b+cos_h*sin_p*sin_b,
sin_b*sin_h+cos_h*sin_p*cos_b,
cos_h*cos_p,
0,

0,0,0,0)
return matrix

# "轴-角"对转四元数
def axis_angle_to_quaternion(v, theta):
# v:旋转轴向量
# theta: 旋转角度（角度制）
pi = 3.1415926
theta = theta * (pi/180) # 转化为弧度制
x = n[0] * math.sin(theta/2.0)
y = n[1] * math.sin(theta/2.0)
z = n[2] * math.sin(theta/2.0)
w = math.cos(theta/2.0)
return x, y, z, w

# 四元数转矩阵
def quaternion_to_matrix(x, y, z, w):
# x, y, z, w： 四元数
matrix = (1-2*y*y-2*z*z, 2*x*y+2*w*z, 2*x*z-2*w*y, 0,
2*x*z+2*w*y, 2*y*z-2*w*x, 1-2*x*x-2*y*y, 0,
0, 0, 0, 0)
return matrix

# 矩阵转欧拉角
def matrix_to_eular(matrix):
mat = matrix # mat为3×3矩阵，必要时matrix需转换为三维矩阵
p, h, b = 0, 0, 0
pi = 3.1415926
sp = -mat[5]
if sp <= -1.0:
p = -1.570796
elif sp >= 1.0
p = 1.570796
else:
p = math.asin(sp)
if sp > 0.9999:
b = 0.0
h = math.atan2(-mat[6], mat[0])
else:
h = math.atan2(mat[2], mat[8])
b = math.atan2(mat[3], mat[4])
return p/(pi/180), h/(pi/180), b/(pi/180)