import numpy as np
# 定义转换矩阵R_T
R_T = np.array([
    [-0.7206, -0.6908, 0],
    [0.6884, -0.7231, 0],
    [0.0830, 0, 1]
])


def transform_data(x_coord, y_coord, z_coord):
    # 将接收到的3个坐标组成列矩阵P_L1_O2
    P_L1_O2 = np.array([[x_coord], [y_coord], [z_coord]], dtype=np.float64)

    # 执行矩阵乘法：P_L1_O1 = R_T * P_L1_O2
    P_L1_O1 = np.dot(R_T, P_L1_O2)

    # 提取转换后的三个数据
    increments_x = P_L1_O1[0][0]
    increments_y = P_L1_O1[1][0]
    increments_z = P_L1_O1[2][0]

    #print(f"矩阵转换完成 - increments_x: {increments_x:.4f}, increments_y: {increments_y:.4f}, increments_z: {increments_z:.4f}")
    return increments_x, increments_y, increments_z