print_setting/print_show_weight_max_obj.py

import os.path
import shutil

import open3d as o3d
import numpy as np
import copy
from tqdm import tqdm
import time
import multiprocessing
import oss2
from joblib import Parallel, delayed
import itertools
from numba import njit, prange
#import bpy
from plyfile import PlyData, PlyElement
from multiprocessing import Pool, RawArray
import ctypes
import itertools

from get_lowest_position_of_center_ext import get_lowest_position_of_center_ext
from get_lowest_position_of_center_ext import get_lowest_position_of_center_ext2
from get_lowest_position_of_center_ext import get_lowest_position_of_center_ext3
from get_lowest_position_of_center_ext import get_lowest_position_of_center_net

def make_pcd_plane():
    # 创建Y=0的平面点云
    width = 60.0  # 平面的宽度
    height = 60.0  # 平面的高度
    resolution = 50  # 点云的分辨率，越高越密集

    # 生成平面点云
    x = np.linspace(-width / 2, width / 2, resolution)
    y = np.linspace(-height / 2, height / 2, resolution)
    xv, yv = np.meshgrid(x, y)
    zv = np.zeros_like(xv)  # Y 坐标恒为 0

    # 将网格转换为点云的顶点数组
    points_plane = np.vstack((xv.flatten(), yv.flatten(), zv.flatten())).T

    # 创建Open3D点云对象
    pcd_plane = o3d.geometry.PointCloud()
    pcd_plane.points = o3d.utility.Vector3dVector(points_plane)
    pcd_plane.paint_uniform_color([0, 1, 0])
    return pcd_plane


def calculate_rotation_and_center_of_mass(angle_x, angle_y, angle_z, points):
    """计算某一组旋转角度后的重心"""
    # 计算绕X轴、Y轴和Z轴的旋转矩阵
    R_x = np.array([
        [1, 0, 0],
        [0, np.cos(np.radians(angle_x)), -np.sin(np.radians(angle_x))],
        [0, np.sin(np.radians(angle_x)), np.cos(np.radians(angle_x))]
    ])

    R_y = np.array([
        [np.cos(np.radians(angle_y)), 0, np.sin(np.radians(angle_y))],
        [0, 1, 0],
        [-np.sin(np.radians(angle_y)), 0, np.cos(np.radians(angle_y))]
    ])

    R_z = np.array([
        [np.cos(np.radians(angle_z)), -np.sin(np.radians(angle_z)), 0],
        [np.sin(np.radians(angle_z)), np.cos(np.radians(angle_z)), 0],
        [0, 0, 1]
    ])

    # 综合旋转矩阵
    R = R_z @ R_y @ R_x

    # 执行旋转
    rotated_points = points @ R.T

    # 计算最小z值
    min_z = np.min(rotated_points[:, 2])

    # 计算平移向量，将最小Z值平移到0
    translation_vector = np.array([0, 0, -min_z])
    rotated_points += translation_vector

    # 计算重心
    center_of_mass = np.mean(rotated_points, axis=0)

    return center_of_mass[2], angle_x, angle_y, angle_z


# def parallel_rotation(points, angle_step=3):
#     """并行计算最优旋转角度"""
#     # 记录最优结果的初始化
#     min_center_of_mass_y = float('inf')
#     best_angle_x, best_angle_y, best_angle_z = 0, 0, 0
#
#     # 计算每个角度的组合
#     angles_x = range(0, 360, angle_step)
#     angles_y = range(0, 360, angle_step)
#     angles_z = range(0, 360, angle_step)
#
#     # 创建一个进程池并行处理
#     with multiprocessing.Pool(processes=multiprocessing.cpu_count()) as pool:
#         results = []
#
#         # 提交任务
#         for angle_x in angles_x:
#             for angle_y in angles_y:
#                 for angle_z in angles_z:
#                     results.append(
#                         pool.apply_async(calculate_rotation_and_center_of_mass, (angle_x, angle_y, angle_z, points)))
#
#         # 获取所有结果
#         for result in results:
#             center_of_mass_z, angle_x, angle_y, angle_z = result.get()
#
#             # 更新最优旋转角度
#             if center_of_mass_z < min_center_of_mass_y:
#                 min_center_of_mass_y = center_of_mass_z
#                 best_angle_x, best_angle_y, best_angle_z = angle_x, angle_y, angle_z
#
#     return best_angle_x, best_angle_y, best_angle_z, min_center_of_mass_y
def parallel_rotation(points, angle_step=3):
    """顺序计算最优旋转角度（单线程）"""
    min_center_of_mass_y = float('inf')
    best_angle_x, best_angle_y, best_angle_z = 0, 0, 0

    # 遍历所有角度组合
    for angle_x in range(0, 360, angle_step):
        for angle_y in range(0, 360, angle_step):
            for angle_z in range(0, 360, angle_step):
                center_of_mass_z, ax, ay, az = calculate_rotation_and_center_of_mass(
                    angle_x, angle_y, angle_z, points
                )
                if center_of_mass_z < min_center_of_mass_y:
                    min_center_of_mass_y = center_of_mass_z
                    best_angle_x, best_angle_y, best_angle_z = ax, ay, az

    return best_angle_x, best_angle_y, best_angle_z, min_center_of_mass_y

#"""
@njit
def safe_min(arr):
    if arr.size == 0:
        return np.inf
    return np.min(arr)

# 核心计算函数（支持Numba加速）
@njit(fastmath=True, cache=True)
def calculate_rotation_z(angle_x, angle_y, angle_z, points, cos_cache, sin_cache, angle_step):
    #计算单个旋转组合后的重心Z坐标（无显式平移）

    if points.shape[0] == 0:
        return np.inf  # 返回极大值避免干扰最优解
    
    # 获取预计算的三角函数值
    idx_x = angle_x // angle_step
    idx_y = angle_y // angle_step
    idx_z = angle_z // angle_step

    cos_x = cos_cache[idx_x]
    sin_x = sin_cache[idx_x]
    cos_y = cos_cache[idx_y]
    sin_y = sin_cache[idx_y]
    cos_z = cos_cache[idx_z]
    sin_z = sin_cache[idx_z]

    # 构造旋转矩阵（展开矩阵乘法优化）
    # R = Rz @ Ry @ Rx
    # 计算矩阵元素（手动展开矩阵乘法）
    m00 = cos_z * cos_y
    m01 = cos_z * sin_y * sin_x - sin_z * cos_x
    m02 = cos_z * sin_y * cos_x + sin_z * sin_x

    m10 = sin_z * cos_y
    m11 = sin_z * sin_y * sin_x + cos_z * cos_x
    m12 = sin_z * sin_y * cos_x - cos_z * sin_x

    m20 = -sin_y
    m21 = cos_y * sin_x
    m22 = cos_y * cos_x

    # 计算所有点的Z坐标
    z_values = np.empty(points.shape[0], dtype=np.float64)
    for i in prange(points.shape[0]):
        x, y, z = points[i, 0], points[i, 1], points[i, 2]
        # 应用旋转矩阵
        rotated_z = m20 * x + m21 * y + m22 * z
        z_values[i] = rotated_z

    # 计算重心Z（等效于平移后的重心）
    # min_z = np.min(z_values)
    min_z = safe_min(z_values)
    avg_z = np.mean(z_values)
    return avg_z - min_z  # 等效于平移后的重心Z坐标

@njit(parallel=True, fastmath=True)
def _process_batch(batch, points, cos_cache, sin_cache, angle_step, results):
    for i in prange(len(batch)):
        ax, ay, az = batch[i]
        results[i] = calculate_rotation_z(
            ax, ay, az, points,
            cos_cache, sin_cache, angle_step
        )


def parallel_rotation2(points, angle_step=3):

    #参数：
    #points : numpy.ndarray (N,3) - 三维点云
    #angle_step : int - 角度搜索步长（度数）

    #返回：
    #(best_angle_x, best_angle_y, best_angle_z, min_z)

    points_np = np.asarray(points)
    points_float64 = points_np.astype(np.float64)
    points = np.ascontiguousarray(points_float64)

    # points = np.ascontiguousarray(points.astype(np.float64))

    # 生成所有可能角度
    angles = np.arange(0, 360, angle_step)
    n_angles = len(angles)

    # 预计算三角函数值（大幅减少重复计算）
    rads = np.radians(angles)
    cos_cache = np.cos(rads).astype(np.float64)
    sin_cache = np.sin(rads).astype(np.float64)

    # 生成所有角度组合（内存优化版）
    total_combinations = n_angles ** 3
    # print(f"Total combinations: {total_combinations:,}")

    # 分块处理以避免内存溢出
    best_z = np.inf
    best_angles = (0, 0, 0)
    batch_size = 10 ** 6  # 根据可用内存调整

    for x_chunk in range(0, n_angles, max(1, n_angles // 4)):
        angles_x = angles[x_chunk:x_chunk + max(1, n_angles // 4)]
        for y_chunk in range(0, n_angles, max(1, n_angles // 4)):
            angles_y = angles[y_chunk:y_chunk + max(1, n_angles // 4)]

            # 生成当前分块的所有组合
            xx, yy, zz = np.meshgrid(angles_x, angles_y, angles)
            current_batch = np.stack([xx.ravel(), yy.ravel(), zz.ravel()], axis=1)

            # 处理子批次
            for i in range(0, len(current_batch), batch_size):
                batch = current_batch[i:i + batch_size]
                results = np.zeros(len(batch), dtype=np.float64)
                _process_batch(batch, points, cos_cache, sin_cache, angle_step, results)

                # 更新最佳结果
                min_idx = np.argmin(results)
                if results[min_idx] < best_z:
                    best_z = results[min_idx]
                    best_angles = tuple(batch[min_idx])
                    # print(f"New best: {best_angles} -> Z={best_z:.4f}")

    return (*best_angles, best_z)
#"""

#"""
def rotate_x(angle):
    theta = np.radians(angle)
    return np.array([
        [1, 0, 0],
        [0, np.cos(theta), -np.sin(theta)],
        [0, np.sin(theta), np.cos(theta)]
    ])

def rotate_y(angle):
    theta = np.radians(angle)
    return np.array([
        [np.cos(theta), 0, np.sin(theta)],
        [0, 1, 0],
        [-np.sin(theta), 0, np.cos(theta)]
    ])

def rotate_z(angle):
    theta = np.radians(angle)
    return np.array([
        [np.cos(theta), -np.sin(theta), 0],
        [np.sin(theta), np.cos(theta), 0],
        [0, 0, 1]
    ])

def compute_z_height_and_center(rotated_points):
    z_min, z_max = rotated_points[:, 2].min(), rotated_points[:, 2].max()
    y_min, y_max = rotated_points[:, 1].min(), rotated_points[:, 1].max()
    return (z_max - z_min), (y_min + y_max) / 2

def init_worker(shared_array, shape, dtype):
    global global_points
    global_points = np.frombuffer(shared_array, dtype=dtype).reshape(shape)

def compute_rotation(args):
    angle_x, angle_y, angle_z = args
    R = rotate_z(angle_z) @ rotate_y(angle_y) @ rotate_x(angle_x)
    rotated_points = global_points @ R.T
    z_height, center_y = compute_z_height_and_center(rotated_points)
    return (angle_x, angle_y, angle_z, z_height, center_y)

def parallel_rotation3(points, angle_step=5):
    # 生成所有旋转角度组合
    angles = itertools.product(
        np.arange(0, 360, angle_step),
        np.arange(0, 360, angle_step),
        np.arange(0, 360, angle_step)
    )
    
    # 共享内存初始化
    shape, dtype = points.shape, points.dtype
    shared_array = RawArray(ctypes.c_double, points.size)
    shared_points = np.frombuffer(shared_array, dtype=dtype).reshape(shape)
    np.copyto(shared_points, points)
    
    # 多进程计算
    with Pool(initializer=init_worker, initargs=(shared_array, shape, dtype)) as pool:
        results = pool.imap_unordered(compute_rotation, angles, chunksize=100)
        
        # 寻找最优解
        best_angle = (0, 0, 0)
        min_z_height = float('inf')
        min_center_y = 0
        for result in results:
            if result[3] < min_z_height:
                best_angle = result[:3]
                min_z_height = result[3]
                min_center_y = result[4]
        
        return (*best_angle, min_center_y)
#"""

#"""
def parallel_rotation4(points, angle_step=4):
    """仅绕 Y 轴旋转（假设 X/Z 轴不影响目标函数）"""
    min_center = float('inf')
    best_angle = 0    # 遍历所有角度组合
    """
    for angle_x in range(0, 360, angle_step):
        for angle_y in range(0, 360, angle_step):
            for angle_z in range(0, 360, angle_step):
                center_of_mass_z, ax, ay, az = calculate_rotation_and_center_of_mass(
                    angle_x, angle_y, angle_z, points
                )
                if center_of_mass_z < min_center_of_mass_y:
                    min_center_of_mass_y = center_of_mass_z
                    best_angle_x, best_angle_y, best_angle_z = ax, ay, az

    """ 
    #""" 
    for angle_x in range(-45, 45, angle_step):
        for angle_y in range(0, 360, angle_step):
            center_z, ax, ay, _ = calculate_rotation_and_center_of_mass(angle_x, angle_y, 0, points)
            if center_z < min_center:
                min_center = center_z
                best_angle_x = ax
                best_angle_y = ay
    #""" 
    return (best_angle_x, best_angle_y, 0, min_center)

#"""

def read_mesh(obj_path, simple=True):
    mesh_obj = o3d.io.read_triangle_mesh(obj_path, enable_post_processing=True) #
    return mesh_obj
    if not simple:
        return mesh_obj
    original_triangles = len(mesh_obj.triangles) 
    target_triangles = original_triangles if original_triangles <= 10000 else 10000
    if original_triangles > 10000:
        mesh_obj = mesh_obj.simplify_quadric_decimation(
            target_number_of_triangles=target_triangles,
            maximum_error=0.0001, 
            boundary_weight=1.0
        )

    return mesh_obj

def compute_mesh_center(vertices):
    """
    计算网格质心
    
    参数:
        vertices: 顶点坐标数组，形状为(N, 3)的NumPy数组或列表
    
    返回:
        centroid: 质心坐标的NumPy数组 [x, y, z]
    """
    if len(vertices) == 0:
        raise ValueError("顶点数组不能为空")
    
    n = len(vertices)  # 顶点数量
    # 初始化坐标累加器
    sum_x, sum_y, sum_z = 0.0, 0.0, 0.0
    
    # 遍历所有顶点累加坐标值
    for vertex in vertices:
        sum_x += vertex[0]
        sum_y += vertex[1]
        sum_z += vertex[2]
    
    # 计算各坐标轴的平均值
    centroid = np.array([sum_x / n, sum_y / n, sum_z / n])
    return centroid

def down_sample(pcd, voxel_size, farthest_sample = False):
    original_num = len(pcd.points)  
    target_samples = 1500 # 1000
    num_samples = min(target_samples, original_num)

    # 第一步：使用体素下采样快速减少点数量
    # voxel_size = 3
    if farthest_sample:
        pcd_voxel = pcd.farthest_point_down_sample(num_samples=num_samples)
    else:
        pcd_voxel = pcd.voxel_down_sample(voxel_size)
    down_num = len(pcd_voxel.points)  
    # print(f"original_num={original_num}, down_num={down_num}")
    
    # 第二步：仅在必要时进行最远点下采样
    if len(pcd_voxel.points) > target_samples and False:
        pcd_downsampled = pcd_voxel.farthest_point_down_sample(num_samples=num_samples)
    else:
        pcd_downsampled = pcd_voxel

    return pcd_downsampled

def get_lowest_position_of_center(obj_path,voxel_size,dict_origin,total_matrix):

    mesh_obj = read_mesh(obj_path)
    dict_origin[obj_path] = mesh_obj
    # dict_origin[obj_path] = copy.deepcopy(mesh_obj)

    #o3d.visualization.draw_geometries([mesh_obj])
    vertices = np.asarray(mesh_obj.vertices)

    # 确保网格有顶点
    if len(vertices) == 0:
        # raise ValueError(f"Mesh has no vertices: {obj_path}")
        print(f"Warning: Mesh has no vertices: {obj_path}")
        return None
    
    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(vertices)
    # o3d.visualization.draw_geometries([pcd])
    # pcd = o3d.io.read_point_cloud(ply_file_path)

    # print("voxel_size",voxel_size,obj_path, len(pcd.points), len(mesh_obj.vertices))

    # 对点云进行下采样（体素网格法）
    #"""
    pcd_downsampled = down_sample(pcd, voxel_size)
    pcd_downsampled.paint_uniform_color([0, 0, 1])

    if len(np.asarray(pcd_downsampled.points)) <= 0:
        bbox = pcd.get_axis_aligned_bounding_box()
        volume = bbox.volume()

        print(f"len(pcd.points)={len(pcd.points)}, volume={volume}")

        # 处理体积为零的情况
        if volume <= 0:
            # 计算点云的实际范围
            points = np.asarray(pcd.points)
            if len(points) > 0:
                min_bound = np.min(points, axis=0)
                max_bound = np.max(points, axis=0)
                extent = max_bound - min_bound
                
                # 确保最小维度至少为0.01
                min_dimension = max(0.01, np.min(extent))
                volume = min_dimension ** 3
            else:
                volume = 1.0  # 最后的安全回退
            
            print(f"Warning: Zero volume detected, using approximated volume {volume:.6f} for {obj_path}")

        # 安全计算密度 - 防止除零错误
        if len(pcd.points) > 0 and volume > 0:
            original_density = len(pcd.points) / volume
            voxel_size = max(0.01, min(10.0, 0.5 / (max(1e-6, original_density) ** 0.33)))
        else:
            # 当点数为0或体积为0时使用默认体素大小
            voxel_size = 1.0  # 默认值
        
        print(f"Recalculated voxel_size: {voxel_size} for {obj_path}")
        
        pcd_downsampled = down_sample(pcd, voxel_size)
        pcd_downsampled.paint_uniform_color([0, 0, 1])
    #"""
    
    original_num = len(pcd.points)  
    target_samples = 1000
    num_samples = min(target_samples, original_num) 
    
    # 确保下采样后有点云
    if len(np.asarray(pcd_downsampled.points)) == 0:
        # 使用原始点云作为后备
        pcd_downsampled = pcd
        print(f"Warning: Using original point cloud for {obj_path} as downsampling produced no points")

    points = np.asarray(pcd_downsampled.points)

    # 初始化最小重心Y的值
    min_center_of_mass_y = float('inf')
    best_angle_x, best_angle_y, best_angle_z = 0, 0, 0
    start_time = time.time()
    # 旋转并计算最优角度：绕X、Y、Z轴进行每度的旋转
    # best_angle_x, best_angle_y, best_angle_z, min_center_of_mass_y = parallel_rotation(points, angle_step=3)
    # best_angle_x, best_angle_y, best_angle_z, min_center_of_mass_y = parallel_rotation2(points, angle_step=3)
    # best_angle_x, best_angle_y, best_angle_z, min_center_of_mass_y = parallel_rotation3(points, angle_step=3)
    best_angle_x, best_angle_y, best_angle_z, min_center_of_mass_y = parallel_rotation4(points, angle_step=3)

    print("get_lowest_position_of_center", obj_path, best_angle_x,best_angle_y,best_angle_z,"time",time.time()-start_time)
    
    """
    # if best_angle_x >= 180:
    if best_angle_x >= 155 and best_angle_x <= 325:
        best_angle_x += 180
    if best_angle_y >= 180:
        best_angle_y += 180
    if best_angle_z >= 180:
        best_angle_z += 180
    #""" 

    # 记录结束时间
    end_time = time.time()
    elapsed_time = end_time - start_time
    # print(f"Time taken: {elapsed_time:.2f} seconds")
    # 输出最佳的旋转角度
    # print(f"Best Rotation Angles: angle_x = {best_angle_x}, angle_y = {best_angle_y}, angle_z = {best_angle_z}")
    # print(f"Minimum Y Center of Mass: {min_center_of_mass_y}")
    #time.sleep(1000)

    # 使用最佳角度进行旋转并平移obj
    pcd_transformed = copy.deepcopy(mesh_obj)
    #"""
    center = pcd_transformed.get_center()
    #arrow = o3d.geometry.TriangleMesh.create_arrow(0.05, 0.1)
    #arrow.translate(center)
    #o3d.visualization.draw_geometries([pcd_transformed, arrow])

    # 最佳角度旋转
    R_x = pcd_transformed.get_rotation_matrix_from_axis_angle(np.array([1, 0, 0]) * np.radians(best_angle_x))
    pcd_transformed.rotate(R_x)
    R_y = pcd_transformed.get_rotation_matrix_from_axis_angle(np.array([0, 1, 0]) * np.radians(best_angle_y))
    pcd_transformed.rotate(R_y)
    R_z = pcd_transformed.get_rotation_matrix_from_axis_angle(np.array([0, 0, 1]) * np.radians(best_angle_z))
    pcd_transformed.rotate(R_z)

    # centroid = pcd.get_center()
    centroid = pcd_transformed.get_center()
    # z_mean1 = centroid[2]

    T_x = np.eye(4)
    T_x[:3, :3] = R_x
    aabb = pcd_transformed.get_axis_aligned_bounding_box()
    # center_point = aabb.get_center()
    center_point = compute_mesh_center(mesh_obj.vertices)
    T_center_to_origin = np.eye(4)
    T_center_to_origin[:3, 3] = -center_point
    T_origin_to_center = np.eye(4)
    T_origin_to_center[:3, 3] = center_point
    T_rot_center = T_origin_to_center @ T_x @ T_center_to_origin
    total_matrix = T_rot_center @ total_matrix

    T_y = np.eye(4)
    T_y[:3, :3] = R_y
    aabb = pcd_transformed.get_axis_aligned_bounding_box()
    # center_point = aabb.get_center()
    center_point = compute_mesh_center(mesh_obj.vertices)
    T_center_to_origin = np.eye(4)
    T_center_to_origin[:3, 3] = -center_point
    T_origin_to_center = np.eye(4)
    T_origin_to_center[:3, 3] = center_point
    T_rot_center = T_origin_to_center @ T_y @ T_center_to_origin
    total_matrix = T_rot_center @ total_matrix

    T_z = np.eye(4)
    T_z[:3, :3] = R_z
    aabb = pcd_transformed.get_axis_aligned_bounding_box()
    # center_point = aabb.get_center()
    center_point = compute_mesh_center(mesh_obj.vertices)
    T_center_to_origin = np.eye(4)
    T_center_to_origin[:3, 3] = -center_point
    T_origin_to_center = np.eye(4)
    T_origin_to_center[:3, 3] = center_point
    T_rot_center = T_origin_to_center @ T_z @ T_center_to_origin

    total_matrix = T_rot_center @ total_matrix
    
    #arrow = o3d.geometry.TriangleMesh.create_arrow(0.05, 0.1)
    #arrow.translate(center)
    #o3d.visualization.draw_geometries([pcd_transformed, arrow])
    #"""

    #"""
    #试着旋转180，让脸朝上

    #
    vertices = np.asarray(pcd_transformed.vertices)
    # 计算平移向量，将最小Y值平移到0
    min_z = np.min(vertices[:, 2])
    translation_vector = np.array([0,0,-min_z,])
    pcd_transformed.translate(translation_vector)

    T_trans1 = np.eye(4)
    T_trans1[:3, 3] = translation_vector
    total_matrix = T_trans1 @ total_matrix
    #

    # 计算 z 坐标均值
    vertices = np.asarray(pcd_transformed.vertices)
    z_mean1 = np.mean(vertices[:, 2])

    angle_rad = np.pi
    #print("旋转前质心:", pcd_transformed.get_center())
    #print("旋转前点示例:", np.asarray(pcd_transformed.vertices)[:3])
    R_y = pcd_transformed.get_rotation_matrix_from_axis_angle(np.array([0, 1, 0]) * angle_rad)
    centroid = pcd_transformed.get_center()
    pcd_transformed.translate(-center_point)
    pcd_transformed.rotate(R_y, center=(0, 0, 0))
    pcd_transformed.translate(center_point)

    aabb = pcd_transformed.get_axis_aligned_bounding_box()
    # center_point = aabb.get_center()
    center_point = compute_mesh_center(mesh_obj.vertices)
    # 构建绕中心点旋转的变换矩阵[3](@ref)
    T_center_to_origin = np.eye(4)
    T_center_to_origin[:3, 3] = -center_point
    R_y180 = pcd_transformed.get_rotation_matrix_from_axis_angle(np.array([0, 1, 0]) * angle_rad)
    T_rotate = np.eye(4)
    T_rotate[:3, :3] = R_y180
    T_origin_to_center = np.eye(4)
    T_origin_to_center[:3, 3] = center_point
    T_rot_center = T_origin_to_center @ T_rotate @ T_center_to_origin
    total_matrix = T_rot_center @ total_matrix
    
    #print("旋转后质心:", pcd_transformed.get_center())
    #print("旋转后点示例:", np.asarray(pcd_transformed.vertices)[:3])

    # centroid = pcd.get_center()
    centroid = pcd_transformed.get_center()
    # z_mean2 = centroid[2]

    #
    vertices = np.asarray(pcd_transformed.vertices)
    # 计算平移向量，将最小Y值平移到0
    min_z = np.min(vertices[:, 2])
    max_z = np.max(vertices[:, 2])
    # print("min_z1", min_z, obj_path)
    translation_vector = np.array([0,0,-min_z,])
    # translation_vector = np.array([0,0,-min_z + (min_z-max_z),])
    # print("translation_vector1",translation_vector)
    pcd_transformed.translate(translation_vector)

    T_trans2 = np.eye(4)
    T_trans2[:3, 3] = translation_vector
    translation = total_matrix[:3, 3]
    # print("translation_vector2",translation_vector)
    # print(1,translation)

    total_matrix = T_trans2 @ total_matrix
    translation = total_matrix[:3, 3]
    # print(2,translation)

    # 计算 z 坐标均值
    vertices = np.asarray(pcd_transformed.vertices)
    z_mean2 = np.mean(vertices[:, 2])
    
    # print("z_mean",z_mean1,z_mean2,len(pcd_transformed.vertices),obj_path)
    
    if (z_mean2 > z_mean1):
        R_y = pcd_transformed.get_rotation_matrix_from_axis_angle(np.array([0, 1, 0]) * -angle_rad)
        centroid = pcd_transformed.get_center()

        aabb = pcd_transformed.get_axis_aligned_bounding_box()
        # center_point = aabb.get_center()
        center_point = compute_mesh_center(mesh_obj.vertices)

        pcd_transformed.translate(-center_point)
        pcd_transformed.rotate(R_y, center=(0, 0, 0))
        pcd_transformed.translate(center_point)

        T_center_to_origin = np.eye(4)
        T_center_to_origin[:3, 3] = -center_point
        T_origin_to_center = np.eye(4)
        T_origin_to_center[:3, 3] = center_point
        # 构建反向旋转矩阵
        R_y = pcd_transformed.get_rotation_matrix_from_axis_angle(np.array([0, 1, 0]) * -angle_rad)
        T_rotate_inv = np.eye(4)
        T_rotate_inv[:3, :3] = R_y
        # 完整的反向绕中心旋转矩阵
        T_rot_center_inv = T_origin_to_center @ T_rotate_inv @ T_center_to_origin
        total_matrix = T_rot_center_inv @ total_matrix
    #"""

    vertices = np.asarray(pcd_transformed.vertices)
    # 计算平移向量，将最小Y值平移到0
    min_z = np.min(vertices[:, 2])
    # print("min_z2", min_z, obj_path)
    translation_vector = np.array([0,0,-min_z,])
    pcd_transformed.translate(translation_vector)

    T_trans3 = np.eye(4)
    T_trans3[:3, 3] = translation_vector
    total_matrix = T_trans3 @ total_matrix

    translation = total_matrix[:3, 3]
    # print(3,translation)

    return pcd_transformed, total_matrix

def axis_angle_to_rotation_matrix(axis, angle):
    """手动生成旋转矩阵"""
    axis = axis / np.linalg.norm(axis)  # 单位化
    cos_a = np.cos(angle)
    sin_a = np.sin(angle)
    return np.array([
        [cos_a + axis[0]**2*(1-cos_a), 
         axis[0]*axis[1]*(1-cos_a) - axis[2]*sin_a,
         axis[0]*axis[2]*(1-cos_a) + axis[1]*sin_a],
        [axis[1]*axis[0]*(1-cos_a) + axis[2]*sin_a,
         cos_a + axis[1]**2*(1-cos_a),
         axis[1]*axis[2]*(1-cos_a) - axis[0]*sin_a],
        [axis[2]*axis[0]*(1-cos_a) - axis[1]*sin_a,
         axis[2]*axis[1]*(1-cos_a) + axis[0]*sin_a,
         cos_a + axis[2]**2*(1-cos_a)]
    ])

def arrange_box_correctly(obj_transformed, voxel_size,total_matrix):

    vertices = np.asarray(obj_transformed.vertices)
    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(vertices)
    
    # 降采样与特征计算
    pcd_downsampled = down_sample(pcd, voxel_size)

    original_num = len(pcd.points)  
    target_samples = 1000

    points = np.asarray(pcd_downsampled.points)
    cov = np.cov(points.T)
    
    center = obj_transformed.get_center()

    # 特征分解与方向约束（关键修改点）
    eigen_vals, eigen_vecs = np.linalg.eigh(cov)
    max_axis = eigen_vecs[:, np.argmax(eigen_vals)]
    
    # arrow = o3d.geometry.TriangleMesh.create_arrow(0.05, 0.1)
    # arrow.translate(center)
    # o3d.visualization.draw_geometries([obj_transformed, arrow])

    # print("max_axis", max_axis)
    # 强制主方向向量X分量为正（指向右侧）
    if max_axis[0] < 0 or (max_axis[0] == 0 and max_axis[1] < 0):
        max_axis = -max_axis

    target_dir = np.array([1, 0])  # 目标方向为X正轴
    current_dir = max_axis[:2] / np.linalg.norm(max_axis[:2])
    dot_product = np.dot(current_dir, target_dir)

    # print("dot_product", dot_product)
    if dot_product < 0.8:  # 阈值控制方向敏感性（建议0.6~0.9）
        max_axis = -max_axis  # 强制翻转方向
    
    # 计算旋转角度
    angle_z = np.arctan2(max_axis[1], max_axis[0]) % (2 * np.pi)


    if max_axis[0] <= 0 and max_axis[1] <= 0:
        angle_z += np.pi

    # print("max_axis2", max_axis, angle_z / np.pi * 180 % 360)
    
    # angle_z = 0
    R = o3d.geometry.get_rotation_matrix_from_axis_angle([0, 0, -angle_z])
    
    T = np.eye(4)
    T[:3, :3] = R
    T[:3, 3] = center - R.dot(center)  # 保持中心不变
    obj_transformed.transform(T)

    total_matrix = T @ total_matrix
    
    #arrow = o3d.geometry.TriangleMesh.create_arrow(0.05, 0.1)
    #arrow.translate(center)
    #o3d.visualization.draw_geometries([obj_transformed, arrow])

    return obj_transformed, total_matrix

def get_new_bbox(obj_transformed_second,obj_name,weight_fix_out_dir,weight_fix_out_ply_dir,voxel_size,show_chart,dict_fix,compact_min_dis,total_matrix):

    # 计算点云的边界
    points = np.asarray(obj_transformed_second.vertices)

    min_bound = np.min(points, axis=0)  # 获取点云的最小边界
    max_bound = np.max(points, axis=0)  # 获取点云的最大边界

    # 确保包围盒的Y坐标不低于0
    min_bound[2] = max(min_bound[2], 0)  # 确保Y坐标的最小值不低于0

    # 重新计算包围盒的中心和半长轴
    bbox_center = (min_bound + max_bound) / 2  # 计算包围盒的中心点
    bbox_extent = (max_bound - min_bound) # 计算包围盒的半长轴（尺寸）

    # 创建包围盒，确保尺寸正确
    new_bbox = o3d.geometry.OrientedBoundingBox(center=bbox_center,
                                                R=np.eye(3),  # 旋转矩阵，默认没有旋转
                                                extent=bbox_extent)
    # 获取包围盒的长、宽和高
    x_length = round(bbox_extent[0],3)  # X 方向的长
    y_length = round(bbox_extent[1],3)  # Y 方向的宽
    z_length = round(bbox_extent[2],3)  # Z 方向的高
    bbox_points = np.array([
        [min_bound[0], min_bound[1], min_bound[2]],
        [max_bound[0], min_bound[1], min_bound[2]],
        [max_bound[0], max_bound[1], min_bound[2]],
        [min_bound[0], max_bound[1], min_bound[2]],
        [min_bound[0], min_bound[1], max_bound[2]],
        [max_bound[0], min_bound[1], max_bound[2]],
        [max_bound[0], max_bound[1], max_bound[2]],
        [min_bound[0], max_bound[1], max_bound[2]]
    ])
    first_corner = bbox_points[2]
    translation_vector = -first_corner
    # start_time = time.time()
    obj_transformed_second.translate(translation_vector)

    T_trans = np.eye(4)
    T_trans[:3, 3] = translation_vector  # 设置平移分量 [2,3](@ref)
    total_matrix = T_trans @ total_matrix  # 矩阵乘法顺序：最新变换左乘[4,5](@ref)
    
    new_bbox.translate(translation_vector)
    # print("get_new_bbox1",time.time()-start_time)

    vertices = np.asarray(obj_transformed_second.vertices)
    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(vertices)

    ply_print_pid = obj_name.replace(".obj","")
    ply_name = f"{ply_print_pid}={z_length}+{y_length}+{x_length}.ply"
    ply_out_path = os.path.join(weight_fix_out_ply_dir,ply_name)
    # o3d.io.write_point_cloud(ply_out_path, pcd_downsampled)
    # o3d.io.write_point_cloud(ply_out_path, pcd)

    if compact_min_dis:
        original_num = len(pcd.points)  
        target_samples = 1500 # 1000
        num_samples = min(target_samples, original_num)  
        start_time = time.time()
        pcd_downsampled = down_sample(pcd, voxel_size, False)
        # print("down_sample time =",time.time()-start_time)
        dict_fix[ply_name] = pcd_downsampled
    else:
        dict_fix[ply_name] = pcd

    # print("dict_fix write",ply_name)
    # print("voxel_down_sample&&write_point_cloud",time.time()-start_time)

    if show_chart:
        # 创建包围盒的轮廓（线框）
        new_bbox_lines = o3d.geometry.LineSet.create_from_oriented_bounding_box(new_bbox)
        new_bbox_lines.paint_uniform_color([1, 0, 0])  # 红色

        #创建坐标系
        coordinate_frame = o3d.geometry.TriangleMesh.create_coordinate_frame(size=2, origin=[0, 0, 0])

        # 创建Y=0的平面点云
        pcd_plane = make_pcd_plane()
        # 可视化点云和边界框
        o3d.visualization.draw_geometries([obj_transformed_second, coordinate_frame, pcd_plane, new_bbox_lines])

    return obj_transformed_second, total_matrix

def load_obj_data(get_pid,base_out_dir):
    """下载obj文件"""
    access_key_id = os.getenv('OSS_TEST_ACCESS_KEY_ID', 'LTAI5tBWbfkZntfJij4Fg9gz')
    access_key_secret = os.getenv('OSS_TEST_ACCESS_KEY_SECRET', 'sYDps1i9NSge6hn130EgpPJezKM6Gx')
    bucket_name = os.getenv('OSS_TEST_BUCKET', 'suwa3d-securedata')
    endpoint = os.getenv('OSS_TEST_ENDPOINT', 'https://oss-cn-shanghai.aliyuncs.com')
    bucket = oss2.Bucket(oss2.Auth(access_key_id, access_key_secret), endpoint, bucket_name)
    # if not os.path.exists(base_out_dir):
    #     os.makedirs(base_out_dir)
    prefix = f"objs/print/{get_pid}/"
    out_pid_dir = os.path.join(base_out_dir,str(get_pid))
    for obj in oss2.ObjectIterator(bucket, prefix=prefix):
        # 如果迭代器至少返回一个对象，那么表示该"文件夹"存在
        if not os.path.exists(out_pid_dir):
            os.makedirs(out_pid_dir)
        #print("Folder exists.")
        filename = obj.key[len(prefix):]
        if get_pid not in filename:
            continue
        print("获取的文件名称",filename)
        local_path = os.path.join(out_pid_dir, filename)
        # 下载对象到本地文件
        try:
            bucket.get_object_to_file(obj.key, local_path)
            print(f"Downloaded {obj.key} to {local_path}")
        except:
            print("下载错误",get_pid)
            record_text = "./error_load.txt"
            with open(record_text, 'a') as f:
                f.write(get_pid + '\n')

    else:
        # 如果迭代器没有返回任何对象，那么表示该"文件夹"不存在
        print("ossFolder does not exist.",get_pid)

def custom_mesh_transform(vertices, transform_matrix):
    """
    手动实现网格变换：对每个顶点应用齐次变换矩阵
    参数:
        vertices: 网格顶点数组 (N, 3)
        transform_matrix: 4x4 齐次变换矩阵
    返回:
        变换后的顶点数组 (N, 3)
    """
    # 1. 顶点转齐次坐标 (N, 3) → (N, 4)
    homogeneous_vertices = np.hstack((vertices, np.ones((vertices.shape[0], 1))))
    
    # 2. 应用变换矩阵：矩阵乘法 (4x4) * (4xN) → (4xN)
    transformed_homogeneous = transform_matrix @ homogeneous_vertices.T
    
    # 3. 转回非齐次坐标 (3xN) → (N, 3)
    transformed_vertices = transformed_homogeneous[:3, :].T
    return transformed_vertices

def make_bbox_for_print(base_out_dir,weight_fix_out_dir,weight_fix_out_ply_dir,show_chart,dict_bad, dict_best_angel,dict_fix,dict_origin,dict_origin_real, compact_min_dis,dict_total_matrix):
    """获取需要的盒子大小"""
    # 加载点云数据
    start_time1 = time.time()
    obj_id_list = [aa.split(".o")[0] for aa in os.listdir(base_out_dir) if aa.endswith(".obj")]
    #print(obj_id_list)
    #print(len(obj_id_list))
    #random.shuffle(obj_id_list)
    obj_id_list = obj_id_list
    #print(obj_id_list)
    voxel_size = 3  # 设置体素的大小，决定下采样的密度

    #for pid in tqdm(obj_id_list,desc="get new bbox"):
    dict_mesh_obj = {}
    for pid_t_y in obj_id_list:
        start_time2 = time.time()
        obj_name = pid_t_y+".obj"
        obj_path = os.path.join(base_out_dir,obj_name)

        total_matrix = np.eye(4)
        #放置最大接触面
        # obj_transformed, total_matrix = get_lowest_position_of_center(obj_path,voxel_size,dict_origin,total_matrix)
        
        mesh_obj = read_mesh(obj_path)

        # dict_origin_real[obj_path] = copy.deepcopy(mesh_obj)
        dict_origin[obj_path] = copy.deepcopy(mesh_obj)

        start_time3 = time.time()
        total_matrix, z_min= get_lowest_position_of_center_ext(obj_path, total_matrix)
        # print("get_lowest_position_of_center_ext time", time.time()-start_time3)
        # print(f"total_matrix={total_matrix}")
        print(f"z_min={z_min}")
        printId = ""
        match = re.search(r"P(\d+)", obj_name)  # 匹配 "P" 后的连续数字
        if match:
            printId = match.group(1)
            # print("printId", printId)

        # total_matrix, z_mean_min = get_lowest_position_of_center_net(printId, total_matrix)

        # print("total_matrix=", total_matrix)

        original_vertices = np.asarray(mesh_obj.vertices)
        transformed_vertices = custom_mesh_transform(original_vertices, total_matrix)
        mesh_obj.vertices = o3d.utility.Vector3dVector(transformed_vertices)

        # print("dict_origin[] obj_path=", obj_path)
        # dict_origin[obj_path] = mesh_obj

        obj_transformed = copy.deepcopy(mesh_obj)
        translation = total_matrix[:3, 3]
        # print("make_bbox_for_print0", obj_name, translation)

        if obj_transformed is None:
            dict_bad[obj_name]=obj_name
            # print(len(dict_bad))
            # print(obj_name)
            # 记录错误文件
            error_log = os.path.join(base_out_dir, "error_files.txt")
            with open(error_log, 'a') as f:
                f.write(f"{obj_path}\n")
            print(f"Skipping invalid file: {obj_path}")
            continue

        start_time3 = time.time()
        best_angle_x, best_angle_y, best_angle_z, z_mean_min = get_lowest_position_of_center_ext3(mesh_obj, obj_path,voxel_size)
        # print("get_lowest_position_of_center_ext2 time", time.time()-start_time3)
        # print("best_angle=", best_angle_x, best_angle_y, best_angle_z, z_mean_min)
        dict_best_angel[obj_name] =  [int(round(best_angle_x)), int(round(best_angle_y)), int(round(best_angle_z))]
        
        start_time3 = time.time()
        #将点云摆正和X轴平衡
        obj_transformed_second,total_matrix = arrange_box_correctly(obj_transformed,voxel_size,total_matrix)
        # print("arrange_box_correctly time", time.time()-start_time3)
        """
        # 创建可视化窗口
        vis = o3d.visualization.Visualizer()
        vis.create_window(window_name='模型展示')
        
        # 添加所有模型到场景
        vis.add_geometry(obj_transformed_second)
        
        # 设置相机视角
        vis.get_render_option().mesh_show_back_face = True
        vis.get_render_option().light_on = True
        
        # 运行可视化
        vis.run()
        vis.destroy_window()
        #""" 

        #print("摆正后的obj")
        #o3d.visualization.draw_geometries([obj_transformed_second, ])
        start_time3 = time.time()
        mesh_obj,total_matrix = get_new_bbox(obj_transformed_second,obj_name,weight_fix_out_dir,weight_fix_out_ply_dir,voxel_size,show_chart,dict_fix,compact_min_dis,total_matrix)
        dict_mesh_obj[obj_name] = mesh_obj
        # print("get_new_bbox time", time.time()-start_time3)

        dict_total_matrix[obj_name] = total_matrix
        print(f"make_bbox_for_print {obj_name} time={time.time()-start_time2}")

    print(f"make_bbox_for_print total_time={time.time()-start_time1}")

    return dict_mesh_obj       
import re
def copy_obj_2x(base_obj_dir):
    obj_list = [aa for aa in os.listdir(base_obj_dir) if aa.endswith(".obj")]
    for obj_name in obj_list:
        if "_F" in obj_name:
            continue
        obj_count = obj_name.split("x")[-1].replace(".obj","")
        if not obj_count.isnumeric():
            match = re.search(r"x(\d+)", obj_name)
            if match:
                obj_count = match.group(1)
            else:
                print("未找到 x 后的数字")

        obj_count_num = int(obj_count)
        if obj_count_num!=1:
            for i in range(obj_count_num-1):
                origin_path = os.path.join(base_obj_dir,obj_name)
                dis_path = os.path.join(base_obj_dir,obj_name.replace(".obj",f"_F{i+1}.obj"))
                if not os.path.exists(dis_path):
                    shutil.copy(origin_path,dis_path)
                    print(dis_path,"复制成功")

def save_mesh_with_blender(obj_transformed, obj_path, apply_transform=True):
    # o3d.io.write_triangle_mesh(obj_path, obj_transformed,write_triangle_uvs=True)
    # return
    """使用 Blender 导出变换后的网格"""
    # 创建新网格对象
    mesh = bpy.data.meshes.new("TempMesh")
    mesh.from_pydata(
        np.asarray(obj_transformed.vertices), 
        [],
        np.asarray(obj_transformed.triangles).tolist()
    )
    obj = bpy.data.objects.new("TempObject", mesh)
    
    # 链接到场景
    bpy.context.collection.objects.link(obj)
    
    # 设置上下文
    original_selection = bpy.context.selected_objects.copy()
    original_active = bpy.context.view_layer.objects.active
    bpy.ops.object.select_all(action='DESELECT')
    obj.select_set(True)
    bpy.context.view_layer.objects.active = obj

    # 应用变换
    if apply_transform:
        bpy.ops.object.transform_apply(
            location=True, 
            rotation=True, 
            scale=True
        )

    # 配置导出参数
    export_settings = {
        'filepath': obj_path,
        'export_selected_objects': True,
        'export_triangulated_mesh': True,
        'forward_axis': 'Y',
        'up_axis': 'Z',
        'global_scale': 1.0
    }

    # 执行导出
    try:
        bpy.ops.wm.obj_export(**export_settings)
    finally:
        # 清理临时对象
        bpy.data.objects.remove(obj, do_unlink=True)
        bpy.data.meshes.remove(mesh)
        
        # 恢复原始上下文
        bpy.ops.object.select_all(action='DESELECT')
        for o in original_selection:
            o.select_set(True)
        bpy.context.view_layer.objects.active = original_active

if __name__ == '__main__':
    out_dir = "/data/datasets_20t/type_setting_test_data/"
    base_out_dir = f"{out_dir}/8/"
    weight_fix_out_dir = f"{out_dir}/print_weight_fix_data_obj"
    weight_fix_out_ply_dir = f"{out_dir}/print_weight_fix_data_ply"
    copy_obj_2x(base_out_dir)