print_setting/get_lowest_position_of_center_ext.py

import open3d as o3d
import numpy as np
import copy
import time
from get_lowest_position_of_z_out import get_lowest_position_of_z_out

# 对外部提供的获取最低z的接口
def get_lowest_position_of_center_out2(obj_path):

    total_matrix = np.eye(4)

    mesh_obj = o3d.io.read_triangle_mesh(obj_path)

    voxel_size = 3
 
    return get_lowest_position_of_center(mesh_obj, obj_path, total_matrix, voxel_size)

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_rotation4(points, angle_step=4):
    """仅绕 Y 轴旋转（假设 X/Z 轴不影响目标函数）"""
    min_center = float('inf')

    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)

import numpy as np
from numba import cuda
import math

# CUDA核函数：计算所有旋转角度下的重心高度
@cuda.jit
def compute_centers_kernel(points, centers, angle_x_start, angle_x_step, angle_y_start, angle_y_step, num_x, num_y):
    i = cuda.blockIdx.x * cuda.blockDim.x + cuda.threadIdx.x
    j = cuda.blockIdx.y * cuda.blockDim.y + cuda.threadIdx.y
    
    if i >= num_x or j >= num_y:
        return
    
    # 获取整数角度值
    angle_x = angle_x_start + i * angle_x_step
    angle_y = angle_y_start + j * angle_y_step
    
    rx = math.radians(float(angle_x))  # 使用 float() 进行转换
    ry = math.radians(float(angle_y))
    rz = 0.0
    
    # 计算旋转矩阵
    cos_x = math.cos(rx)
    sin_x = math.sin(rx)
    cos_y = math.cos(ry)
    sin_y = math.sin(ry)
    cos_z = math.cos(rz)
    sin_z = math.sin(rz)
    
    # 旋转矩阵: R = R_z * R_y * R_x
    R00 = cos_z * cos_y
    R01 = cos_z * sin_y * sin_x - sin_z * cos_x
    R02 = cos_z * sin_y * cos_x + sin_z * sin_x
    R10 = sin_z * cos_y
    R11 = sin_z * sin_y * sin_x + cos_z * cos_x
    R12 = sin_z * sin_y * cos_x - cos_z * sin_x
    R20 = -sin_y
    R21 = cos_y * sin_x
    R22 = cos_y * cos_x
    
    n = points.shape[0]
    min_z = 1e10
    
    # 第一遍：计算最小Z值
    for k in range(n):
        x = points[k, 0]
        y = points[k, 1]
        z = points[k, 2]
        
        x_rot = R00 * x + R01 * y + R02 * z
        y_rot = R10 * x + R11 * y + R12 * z
        z_rot = R20 * x + R21 * y + R22 * z
        
        if z_rot < min_z:
            min_z = z_rot
    
    total_z = 0.0
    # 第二遍：平移并计算Z坐标和
    for k in range(n):
        x = points[k, 0]
        y = points[k, 1]
        z = points[k, 2]
        
        x_rot = R00 * x + R01 * y + R02 * z
        y_rot = R10 * x + R11 * y + R12 * z
        z_rot = R20 * x + R21 * y + R22 * z
        
        z_trans = z_rot - min_z
        total_z += z_trans
    
    center_z = total_z / n
    centers[i, j] = center_z

# CUDA版本的并行旋转计算
def parallel_rotation4_cuda(points, angle_step=4):
    angle_x_start = -45
    angle_x_end = 45
    angle_y_start = 0
    angle_y_end = 360
    
    num_x = int((angle_x_end - angle_x_start) / angle_step)
    num_y = int((angle_y_end - angle_y_start) / angle_step)
    
    # 将点云数据复制到GPU
    d_points = cuda.to_device(points.astype(np.float32))
    d_centers = cuda.device_array((num_x, num_y), dtype=np.float32)
    
    # 配置线程块和网格
    threadsperblock = (16, 16)
    blockspergrid_x = (num_x + threadsperblock[0] - 1) // threadsperblock[0]
    blockspergrid_y = (num_y + threadsperblock[1] - 1) // threadsperblock[1]
    blockspergrid = (blockspergrid_x, blockspergrid_y)
    
    # 启动核函数
    compute_centers_kernel[blockspergrid, threadsperblock](
        d_points, d_centers, angle_x_start, angle_step, angle_y_start, angle_step, num_x, num_y
    )
    
    # 将结果复制回主机
    centers = d_centers.copy_to_host()
    
    # 找到最小重心值的索引
    min_index = np.argmin(centers)
    i = min_index // num_y
    j = min_index % num_y
    
    best_angle_x = angle_x_start + i * angle_step
    best_angle_y = angle_y_start + j * angle_step
    min_center = centers[i, j]
    
    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)
    return mesh_obj

def compute_mesh_center2(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
    
    start_time1 = time.time()
    # 遍历所有顶点累加坐标值
    for vertex in vertices:
        sum_x += vertex[0]
        sum_y += vertex[1]
        sum_z += vertex[2]
    print("compute_mesh_center1 time", time.time()-start_time1)
    
    # 计算各坐标轴的平均值
    centroid = np.array([sum_x / n, sum_y / n, sum_z / n])
    return centroid

def compute_mesh_center(vertices):
    """
    计算网格质心（优化版）
    
    参数:
        vertices: 顶点坐标数组，形状为(N, 3)的NumPy数组或列表
    
    返回:
        centroid: 质心坐标的NumPy数组 [x, y, z]
    """
    if len(vertices) == 0:
        raise ValueError("顶点数组不能为空")
    
    # 确保vertices是NumPy数组
    vertices_np = np.asarray(vertices)
    
    # 使用NumPy的mean函数直接计算均值（向量化操作）
    centroid = np.mean(vertices_np, axis=0)
    
    return centroid

def get_lowest_position_of_center_ext3(mesh_obj, obj_path, voxel_size = 3):
    
    best_angle_x, best_angle_y, best_angle_z, z_mean_min, pcd_transformed= get_lowest_position_of_center2(mesh_obj, obj_path, voxel_size)

    return  best_angle_x, best_angle_y, best_angle_z, z_mean_min


def get_lowest_position_of_center_ext2(mesh_obj, obj_path, total_matrix, voxel_size):
    
    # total_matrix, z_mean_min = get_lowest_position_of_center(obj_path, total_matrix, voxel_size)
    temp_matrix, z_mean_min = get_lowest_position_of_center(mesh_obj, obj_path, np.eye(4), voxel_size)
    # print("temp_matrix=",temp_matrix,voxel_size,mesh_obj)

    total_matrix = temp_matrix @ total_matrix

    return  total_matrix, z_mean_min

def get_lowest_position_of_center_ext(obj_path, total_matrix):
    
    temp_matrix, z_max = get_lowest_position_of_z_out(obj_path)

    total_matrix = temp_matrix @ total_matrix

    return  total_matrix, z_max

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(mesh_obj, obj_path, total_matrix, voxel_size):

    # print(f"obj_path={obj_path}, get_lowest_position_of_center voxel_size={voxel_size}")
    start_time1 = time.time()

    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: {mesh_obj}")
        return None
    
    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(vertices)

    # 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)     
    
    # print("get_lowest_position_of_center1 time", time.time()-start_time1)
    start_time2 = time.time()
    # 确保下采样后有点云
    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
    best_angle_x, best_angle_y, best_angle_z, min_center_of_mass_y = parallel_rotation4(points, angle_step=3)

    # 使用最佳角度进行旋转并平移obj
    pcd_transformed = copy.deepcopy(mesh_obj)


    # 最佳角度旋转
    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)

    T_x = np.eye(4)
    T_x[:3, :3] = R_x
    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
    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
    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

    #试着旋转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])
    z_max1 = np.max(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])

    #
    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])
    z_max2 = np.max(vertices[:, 2])
    
    # print(f"get_lowest_position_of_center z_max1={z_max1}, z_max2={z_max2}, len={len(pcd_transformed.vertices)}, obj_path={obj_path}")
    
    if (z_mean2 > z_mean1):
    # if (z_max2 > z_max1):
        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

    # z_mean_min = min(z_mean1, z_mean2)
    z_max_min = min(z_max1, z_max2)

    # print("get_lowest_position_of_center2 time", time.time()-start_time2)
    return total_matrix, z_max_min

import requests
import json
import re

def is_valid_float_string(s):
    # 匹配科学计数法或普通小数，允许开头和末尾的空格
    pattern = r'^[-+]?[0-9]*\.?[0-9]+([eE][-+]?[0-9]+)?$'
    return bool(re.match(pattern, s.strip()))

def safe_convert_to_float(s):
    """
    尝试将字符串安全转换为float，处理一些不完整情况。
    """
    s = s.strip().lower()  # 去除空格，统一为小写

    # 处理空字符串或完全非数字的情况
    if not s or s in ['na', 'nan', 'inf', 'null', 'none']:
        return None  # 或者可以根据需要返回 float('nan')

    # 检查是否为有效的浮点数字符串
    if is_valid_float_string(s):
        return float(s)

    # 处理类似 '0.00000000e' 的情况（缺少指数）
    if s.endswith('e'):
        # 尝试添加指数 '0' -> 'e0'
        try:
            return float(s + '0')
        except ValueError:
            pass # 如果添加'e0'后仍然失败，则继续下面的异常处理

    # 更激进的清理：移除非数字、小数点、负号和指数e以外的所有字符
    # 注意：这可能破坏有特定格式的字符串，慎用
    cleaned_s = re.sub(r'[^\d\.eE-]', '', s)
    try:
        return float(cleaned_s)
    except ValueError:
        pass

    # 如果所有尝试都失败，返回None或抛出异常
    return None

def string_to_matrix(data_string):
    """
    将字符串转换为NumPy浮点数矩阵，并处理可能的转换错误。
    """
    # 分割字符串
    lines = data_string.strip().split(';')
    matrix = []

    for line in lines:
        num_list = line.split()
        float_row = []
        for num in num_list:
            # 使用安全转换函数
            value = safe_convert_to_float(num)
            if value is None:
                # 处理转换失败，例如记录日志、使用NaN代替
                print(f"警告: 无法转换字符串 '{num}'，将其替换为NaN。")
                value = np.nan  # 用NaN标记缺失或无效值
            float_row.append(value)
        matrix.append(float_row)

    return np.array(matrix)

import ast

def get_lowest_position_of_center_net(printId, total_matrix):
    print("get_lowest_position_of_center_net", printId)

    url = f"https://mp.api.suwa3d.com/api/printOrder/infoByPrintId?printId={printId}"
    res = requests.get(url)

    datas = res.json()["data"]["layout"]
    print("datas=", datas)
    
    homo_matrix_str = datas.get("homo_matrix")
    print("homo_matrix_str=", homo_matrix_str)
    
    # 1. 去除字符串首尾的方括号
    str_cleaned = homo_matrix_str.strip('[]')
    # 2. 按行分割字符串
    rows = str_cleaned.split('\n')
    
    # 3. 修复：处理每行中的逗号问题
    matrix_list = []
    for row in rows:
        if row.strip() == '':
            continue
        
        # 去除行首尾的方括号和空格
        row_cleaned = row.strip(' []')
        # 按逗号分割，但过滤掉空字符串
        elements = [elem.strip() for elem in row_cleaned.split(',') if elem.strip() != '']
        
        # 进一步清理每个元素：去除可能残留的逗号和方括号
        cleaned_elements = []
        for elem in elements:
            # 去除元素中可能存在的逗号、方括号和空格
            elem_cleaned = elem.strip(' ,[]')
            if elem_cleaned != '':
                cleaned_elements.append(elem_cleaned)
        
        if cleaned_elements:  # 只添加非空行
            matrix_list.append(cleaned_elements)
    
    print("matrix_list=", matrix_list)
    
    # 4. 安全地转换为浮点数数组（带错误处理）
    try:
        reconstructed_matrix = np.array(matrix_list, dtype=float)
    except ValueError as e:
        print(f"转换矩阵时出错: {e}")
        print("尝试逐个元素转换...")
        
        # 逐个元素转换，便于定位问题元素
        float_matrix = []
        for i, row in enumerate(matrix_list):
            float_row = []
            for j, elem in enumerate(row):
                try:
                    # 再次清理元素并转换
                    cleaned_elem = elem.strip(' ,')
                    float_val = float(cleaned_elem)
                    float_row.append(float_val)
                except ValueError as ve:
                    print(f"无法转换的元素: 行{i}, 列{j}, 值'{elem}', 错误: {ve}")
                    # 可以选择设置为0或NaN，或者抛出异常
                    float_row.append(0.0)  # 或者 np.nan
            float_matrix.append(float_row)
        
        reconstructed_matrix = np.array(float_matrix, dtype=float)
    
    layout_z = datas.get("layout_z", 0)
    print("layout_z", layout_z)

    reconstructed_matrix = reconstructed_matrix @ total_matrix
    print("reconstructed_matrix=", reconstructed_matrix)

    return reconstructed_matrix, layout_z

def get_lowest_position_of_center2(mesh_obj, obj_path, voxel_size = 3):

    # mesh_obj = read_mesh(obj_path)

    vertices = np.asarray(mesh_obj.vertices)

    # 确保网格有顶点
    if len(vertices) == 0:
        print(f"Warning: Mesh has no vertices: {obj_path}")
        return None
    
    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(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])
    
    # 确保下采样后有点云
    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的值
    best_angle_x, best_angle_y, best_angle_z = 0, 0, 0
    # 旋转并计算最优角度：绕X、Y、Z轴进行每度的旋转
    best_angle_x, best_angle_y, best_angle_z, min_center_of_mass_y = parallel_rotation4(points, angle_step=3)
    # print("best_angle1", best_angle_x, best_angle_y, best_angle_z)

    # 使用最佳角度进行旋转并平移obj
    pcd_transformed = copy.deepcopy(mesh_obj)

    # 最佳角度旋转
    """
    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)
    #"""

    aabb = pcd_transformed.get_axis_aligned_bounding_box()
    center_point = aabb.get_center()

    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)

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

    angle_rad = np.pi

    R_y = pcd_transformed.get_rotation_matrix_from_axis_angle(np.array([0, 1, 0]) * angle_rad)

    pcd_transformed.translate(-center_point)
    pcd_transformed.rotate(R_y, center=(0, 0, 0))
    pcd_transformed.translate(center_point)
    best_angle_y += angle_rad / np.pi * 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)
    
    # 计算 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)

        aabb = pcd_transformed.get_axis_aligned_bounding_box()
        center_point = aabb.get_center()

        pcd_transformed.translate(-center_point)
        pcd_transformed.rotate(R_y, center=(0, 0, 0))
        pcd_transformed.translate(center_point)
        best_angle_y += -angle_rad / np.pi * 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)

    z_mean_min = min(z_mean1, z_mean2)

    angle_z_delta = arrange_box_correctly(pcd_transformed, voxel_size)
    best_angle_z += angle_z_delta
    # print("angle_z_delta", angle_z_delta, best_angle_z)

    return best_angle_x, best_angle_y, best_angle_z, z_mean_min, pcd_transformed

def arrange_box_correctly(obj_transformed, voxel_size):

    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
    num_samples = min(target_samples, original_num)

    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)]

    # 强制主方向向量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.rad2deg(-angle_z))
    
    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)

    return np.rad2deg(-angle_z)