# import numpy as np
# import matplotlib.pyplot as plt
# from mpl_toolkits.mplot3d import Axes3D

# # 假设 points_and_normals 是你的 Nx6 矩阵
# # 前三列是点坐标，后三列是法线
# points_and_normals = np.loadtxt("/Users/hofee/Downloads/temp_output/cad_model_world/points_and_normals.txt")  # 这里用随机点代替你的数据
# points = points_and_normals[:100, :3]
# normals = points_and_normals[:100, 3:]

# # 创建3D图形
# fig = plt.figure()
# ax = fig.add_subplot(111, projection='3d')

# # 绘制点云
# ax.scatter(points[:, 0], points[:, 1], points[:, 2], color='b', marker='o')

# # 绘制法线 (从每个点出发的一小段箭头)
# ax.quiver(points[:, 0], points[:, 1], points[:, 2],
#           normals[:, 0], normals[:, 1], normals[:, 2], length=0.1, color='r')

# plt.show()

import numpy as np

# 假设 points_and_normals 是你的 Nx6 矩阵
# points_and_normals[:,:3] 是点的坐标
# points_and_normals[:,3:] 是法线
points_and_normals = np.loadtxt("/Users/hofee/Downloads/temp_output/cad_model_world/points_and_normals.txt")  # 这里用随机点代替你的数据
print(points_and_normals.shape)
points = points_and_normals[300:400, :3]
normals = points_and_normals[300:400, 3:]

# 设置你想在法线方向上采样的距离范围和点数
num_samples_per_point = 20  # 每个法线方向采样的点数
sampling_distances = np.linspace(0, 0.5, num_samples_per_point)  # 采样距离范围

# 创建一个空列表来保存采样点
sampled_points = []

# 对每个点进行法线方向的采样
for point, normal in zip(points, normals):
    for dist in sampling_distances:
        # 在法线方向上偏移点
        sampled_point = point + dist * normal
        sampled_points.append(sampled_point)

# 转换为 numpy 数组
sampled_points = np.array(sampled_points)

# 保存为点云文件 (例如 .txt 或 .xyz 格式)
np.savetxt('sampled_points.txt', sampled_points)

print("采样点云已保存为 'sampled_points.xyz'")