math_utils.py 文件源码

def rotation_matrix_axis(C_values):
    # Change coordinate system through matrix C
    rx = np.deg2rad(float(C_values[0]))
    ry = np.deg2rad(float(C_values[1]))
    rz = np.deg2rad(float(C_values[2]))

    Cx = np.matrix([[1, 0, 0],
                    [0, np.cos(rx), np.sin(rx)],
                    [0, -np.sin(rx), np.cos(rx)]])

    Cy = np.matrix([[np.cos(ry), 0, -np.sin(ry)],
                    [0, 1, 0],
                    [np.sin(ry), 0, np.cos(ry)]])

    Cz = np.matrix([[np.cos(rz), np.sin(rz), 0],
                    [-np.sin(rz), np.cos(rz), 0],
                    [0, 0, 1]])

    C = Cx * Cy * Cz
    Cinv = np.linalg.inv(C)
    return C, Cinv