已知空间中某个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