目的

從Excel的單元格中讀取數(shù)據(jù)，根據(jù)Yaw、Roll、Pitch、世界坐標、A點，通過計算出偏移后的點A'，最后將結果輸出在Excel的單元格中。

參考文章

在實現(xiàn)過程中，4*4矩陣計算參考了部分公式但是還是計算錯誤。后來看了Java的Matrix庫以及以下鏈接的文章，才算出正確的結果。

• 旋轉矩陣（Rotation Matrix)的推導 - 無名氏的文章 - 知乎
  https://zhuanlan.zhihu.com/p/433389563

繞X軸

繞Y軸

繞Z軸

代碼實現(xiàn)

import xlwings as xw
import math
import numpy as np
 
def process_data(sheet, row_number):

    # 讀取單元格數(shù)據(jù)
    pitch_angle = sheet.range(f"A{row_number}").value
    roll_angle = sheet.range(f"B{row_number}").value
    avr_angle = sheet.range(f"C{row_number}").value

        # 定位天線坐標
    posX = sheet.range(f"E{row_number}").value # E
    posY = sheet.range(f"D{row_number}").value # N
    posZ = sheet.range(f"F{row_number}").value

    # 需要計算的左點坐標
    relativePtX_left = sheet.range(f"H{row_number}").value # E
    relativePtY_left = sheet.range(f"G{row_number}").value # N
    relativePtZ_left = sheet.range(f"I{row_number}").value

    relativePtX_right = sheet.range(f"K{row_number}").value # E
    relativePtY_right = sheet.range(f"J{row_number}").value # N
    relativePtZ_right = sheet.range(f"L{row_number}").value

    
    # 將角度轉換為弧度
    avr_angle = math.radians(-(avr_angle - 90))
    roll_angle = math.radians(roll_angle)
    pitch_angle = math.radians(pitch_angle)
   
    pt = np.zeros(3, dtype=float)

    worldPos_left = np.array([relativePtX_left - posX, relativePtY_left - posY, relativePtZ_left - posZ, 1], dtype=float)
    localPos_left = np.zeros(4, dtype=float)

    worldPos_right = np.array([relativePtX_right - posX, relativePtY_right - posY, relativePtZ_right - posZ, 1], dtype=float)
    localPos_right = np.zeros(4, dtype=float)

    mat = np.identity(4, dtype=np.float32)
    # print(worldPos)

    # 創(chuàng)建旋轉矩陣
    rotation_matrix_yaw = np.array([
        [np.cos(avr_angle), np.sin(avr_angle), 0, 0],
        [-np.sin(avr_angle), np.cos(avr_angle), 0, 0],
        [0, 0, 1, 0],
        [0, 0, 0, 1]
    ], dtype=np.float32)

    rotation_matrix_roll = np.array([
        [np.cos(roll_angle), 0, -np.sin(roll_angle), 0],
        [0, 1, 0, 0],
        [np.sin(roll_angle), 0, np.cos(roll_angle), 0],
        [0, 0, 0, 1]
    ], dtype=np.float32)

    rotation_matrix_pitch = np.array([
        [1, 0, 0, 0],
        [0, np.cos(pitch_angle), np.sin(pitch_angle), 0],
        [0, -np.sin(pitch_angle), np.cos(pitch_angle), 0],
        [0, 0, 0, 1]
    ], dtype=np.float32)

    # 將旋轉矩陣應用到原始矩陣
    mat = np.dot(rotation_matrix_yaw, mat)
    mat = np.dot(rotation_matrix_roll, mat)
    mat = np.dot(rotation_matrix_pitch, mat)

    # 計算逆矩陣
    invert = np.linalg.inv(mat)
    # 計算本地坐標
    localPos_left = np.dot(worldPos_left, invert)
    localPos_right = np.dot(worldPos_right, invert)
    # 提取結果
    pt_left = localPos_left[:3]
    pt_right = localPos_right[:3]
    # 打印結果
    # print("pt_left:", pt_left)
    # print("pt_right:", pt_right)
    
    # 將結果寫回Excel
    sheet.range(f"M{row_number}").value = pt_left[0]
    sheet.range(f"N{row_number}").value = pt_left[1]
    sheet.range(f"O{row_number}").value = pt_left[2]

    sheet.range(f"P{row_number}").value = pt_right[0]
    sheet.range(f"Q{row_number}").value = pt_right[1]
    sheet.range(f"R{row_number}").value = pt_right[2]

    sheet.range(f"T{row_number}").value = pt_left[0] + posX # E
    sheet.range(f"S{row_number}").value = pt_left[1] + posY # N
    sheet.range(f"U{row_number}").value = pt_left[2] + posZ

    sheet.range(f"W{row_number}").value = pt_right[0] + posX # E
    sheet.range(f"V{row_number}").value = pt_right[1] + posY # N
    sheet.range(f"X{row_number}").value = pt_right[2] + posZ


def main():
    wb = xw.Book.caller()
    sheet = wb.sheets[0]

    # 遍歷處理多行數(shù)據(jù)，從第2行到最后一行
    current_row = 2
    while sheet.range(f"A{current_row}").value:
        process_data(sheet, current_row)
        current_row += 1

    # 保存并關閉工作簿
    wb.save()

if __name__ == "__main__":
    xw.Book("table.xlsm").set_mock_caller()
    main()