Spaces:
Paused
Paused
| 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() |