Add code/cube3d/training/rotateLdr.py

code/cube3d/training/rotateLdr.py

@@ -0,0 +1,145 @@
+import os
+import numpy as np
+
+def checkEncoding(filepath):
+    filepath = filepath
+    with open(filepath, "rb") as encode_check:
+        encoding = encode_check.readline(3)
+    if encoding == b"\xfe\xff\x00":
+        return "utf_16_be"
+    elif encoding == b"\xff\xfe0":
+        return "utf_16_le"
+    else:
+        return "utf_8"
+
+def readTextFile(filepath):
+    filepath = filepath
+    lines = None
+    if os.path.exists(filepath):
+        file_encoding = checkEncoding(filepath)
+        try:
+            with open(filepath, "rt", encoding=file_encoding) as f_in:
+                lines = f_in.readlines()
+        except:
+            with open(filepath, "rt", encoding="latin_1") as f_in:
+                lines = f_in.readlines()
+
+    return lines
+
+def apply_rotation(x, y, z, rot_mat):
+    # 旋转矩阵乘法: [x', y', z'] = [x, y, z] * rot_mat
+    new_x = rot_mat[0][0] * x + rot_mat[0][1] * y + rot_mat[0][2] * z
+    new_y = rot_mat[1][0] * x + rot_mat[1][1] * y + rot_mat[1][2] * z
+    new_z = rot_mat[2][0] * x + rot_mat[2][1] * y + rot_mat[2][2] * z
+    return new_x, new_y, new_z
+
+
+def apply_rotation_matrix(orig_mat, rot_mat):
+    # numpy 矩阵乘法
+    orig_mat_np = np.array(orig_mat).reshape((3,3))
+    rot_mat_np = np.array(rot_mat)
+    new_mat = rot_mat_np @ orig_mat_np
+    return new_mat.flatten().tolist()
+
+def getModulePart(start_line, end_line, lines, main_section, rot_mat):
+    for j in range(start_line, end_line):
+        line = lines[j]
+        if line.startswith("1 "):
+            parts = line.strip().split()
+            if len(parts) >= 12:  # x,y,z + 9 个矩阵元素
+                try:
+                    # 获取原坐标
+                    x = float(parts[2])
+                    y = float(parts[3])
+                    z = float(parts[4])
+                    
+                    # 使用旋转矩阵进行坐标变换
+                    x_rot, y_rot, z_rot = apply_rotation(x, y, z, rot_mat)
+                    
+                    # 更新坐标
+                    parts[2] = str(x_rot)
+                    parts[3] = str(y_rot)
+                    parts[4] = str(z_rot)
+                    
+                    # 获取原旋转矩阵
+                    orig_mat = [float(p) for p in parts[5:14]]
+                    
+                    # 旋转零件自身旋转矩阵
+                    new_mat = apply_rotation_matrix(orig_mat, rot_mat)
+                    
+                    # 更新矩阵
+                    for idx, val in enumerate(new_mat):
+                        parts[5 + idx] = str(val)
+                    
+                    # 重新拼接并更新行
+                    line = " ".join(parts) + "\n"
+                except ValueError as e:
+                    print(e)
+                    pass
+        main_section.append(line)
+
+
+
+def process_ldr(filepath, output_file, rot_mat):
+    if os.path.isfile(filepath):
+        lines = readTextFile(filepath)
+        if lines is None:
+            print("Could not read file {0}".format(filepath))
+            lines = []
+    else:
+        lines = []
+
+    main_section = []
+    startLine = 0
+    endLine = 0
+    lineCount = 0
+    foundEnd = False
+    
+    for line in lines:
+        parameters = line.strip().split()
+        if len(parameters) > 2:
+            if parameters[0] == "0" and parameters[1] == "FILE":
+                if foundEnd == False:
+                    endLine = lineCount
+                    if endLine > startLine:
+                        getModulePart(startLine, endLine, lines, main_section, rot_mat)
+                        foundEnd = True
+                        break
+
+                startLine = lineCount
+                foundEnd = False
+
+            if parameters[0] == "0" and parameters[1] == "NOFILE":
+                endLine = lineCount
+                foundEnd = True
+                getModulePart(startLine, endLine, lines, main_section, rot_mat)
+                break
+        lineCount += 1
+
+    if lineCount < len(lines):
+        remaining = lines[lineCount:]
+    else:
+        remaining = []
+
+    if foundEnd == False:
+        endLine = lineCount
+        if endLine > startLine:
+            getModulePart(startLine, endLine, lines, main_section, rot_mat)
+
+    new_section = main_section + remaining
+    with open(output_file, "w", encoding="utf-8") as f:
+        f.writelines(new_section)
+        return True
+
+# 旋转矩阵：绕 Y 轴旋转 90 度
+rot_mat = [
+    [0, 0, 1],
+    [0, 1, 0],
+    [-1, 0, 0]
+]
+
+# # 调用函数处理 LDR 文件，进行旋转
+# def main():
+#     process_ldr("/public/home/wangshuo/gap/assembly/data/car_1k/ldr/car_5/car_5.ldr", "car_5_rotate.ldr", rot_mat)
+
+# main()