求Python/R脚本：计算3D路径点列中连续三点的圆弧半径

我来帮你搞定这个3D路径曲率半径计算的需求！下面分别提供Python和R的实现方案，严格按照你要求的逻辑：取连续三个点计算半径并赋值给中间点，从第一组前三点开始迭代处理所有点对。

Python实现方案 #

思路说明 #

1. 读取文本文件中的3D点数据，解析为坐标数组
2. 对每一组连续三点（Pn, Pn+1, Pn+2），计算向量Pn+1 - Pn和Pn+2 - Pn+1
3. 通过向量叉乘的模长、向量模长计算曲率，进而得到曲率半径
4. 处理边界情况：若三点共线（叉乘模长为0）或两点重合，则半径设为无穷大Inf

完整代码 #

import numpy as np

def read_3d_points(file_path):
    """读取分号分隔的3D点文件，返回numpy数组"""
    points = []
    with open(file_path, 'r') as f:
        for line in f:
            # 去除换行符，按分号分割坐标
            coords = line.strip().split(';')
            # 转换为浮点数
            x, y, z = map(float, coords)
            points.append([x, y, z])
    return np.array(points)

def compute_curvature_radius(p1, p2, p3):
    """计算三个3D点对应的中间点p2的曲率半径"""
    # 计算向量
    vec1 = p2 - p1
    vec2 = p3 - p2
    
    # 计算向量模长
    norm_vec1 = np.linalg.norm(vec1)
    norm_vec2 = np.linalg.norm(vec2)
    
    # 如果任意向量模长为0（两点重合），返回无穷大
    if norm_vec1 == 0 or norm_vec2 == 0:
        return np.inf
    
    # 计算叉乘及其模长
    cross_product = np.cross(vec1, vec2)
    norm_cross = np.linalg.norm(cross_product)
    
    # 三点共线时叉乘模长为0，半径无穷大
    if norm_cross == 0:
        return np.inf
    
    # 曲率公式：k = |vec1 × vec2| / |vec1|^3
    curvature = norm_cross / (norm_vec1 ** 3)
    # 半径为曲率的倒数
    radius = 1 / curvature
    return radius

if __name__ == "__main__":
    # 替换为你的点文件路径
    file_path = "path_points.txt"
    points = read_3d_points(file_path)
    
    # 检查点数量是否足够（至少3个）
    if len(points) < 3:
        raise ValueError("文件中至少需要3个点才能计算半径")
    
    # 初始化半径数组，长度与点数量一致，首尾点无对应半径（设为NaN）
    radii = np.full(len(points), np.nan)
    
    # 迭代计算每个中间点的半径
    for n in range(len(points) - 2):
        p1 = points[n]
        p2 = points[n+1]
        p3 = points[n+2]
        radii[n+1] = compute_curvature_radius(p1, p2, p3)
    
    # 输出结果（也可以保存到文件）
    print("点坐标 | 对应半径")
    print("-------------------")
    for idx, (point, r) in enumerate(zip(points, radii)):
        print(f"({point[0]:.2f};{point[1]:.2f};{point[2]:.2f}) | {r:.2f}" if not np.isnan(r) else f"({point[0]:.2f};{point[1]:.2f};{point[2]:.2f}) | 无对应半径")
    
    # 可选：保存结果到文件
    # np.savetxt("radii_result.txt", np.column_stack((points, radii)), fmt="%.2f;%.2f;%.2f;%.2f", header="x;y;z;radius")

R实现方案 #

思路说明 #

1. 读取文本文件，将分号分隔的坐标转换为数据框
2. 循环遍历每一组连续三点，计算向量和叉乘
3. 用相同的曲率公式计算半径，处理共线/重合点的边界情况
4. 输出结果或保存到文件

完整代码 #

# 读取3D点文件的函数
read_3d_points <- function(file_path) {
  # 读取所有行
  lines <- readLines(file_path)
  # 分割每一行的坐标
  coords <- lapply(lines, function(line) {
    as.numeric(strsplit(trimws(line), ";")[[1]])
  })
  # 转换为数据框
  df <- do.call(rbind, coords)
  colnames(df) <- c("x", "y", "z")
  return(df)
}

# 计算三点曲率半径的函数
compute_curvature_radius <- function(p1, p2, p3) {
  # 转换为向量
  vec1 <- p2 - p1
  vec2 <- p3 - p2
  
  # 计算向量模长
  norm_vec1 <- sqrt(sum(vec1^2))
  norm_vec2 <- sqrt(sum(vec2^2))
  
  # 处理两点重合的情况
  if (norm_vec1 == 0 || norm_vec2 == 0) {
    return(Inf)
  }
  
  # 计算叉乘：3D叉乘公式
  cross_product <- c(
    vec1[2]*vec2[3] - vec1[3]*vec2[2],
    vec1[3]*vec2[1] - vec1[1]*vec2[3],
    vec1[1]*vec2[2] - vec1[2]*vec2[1]
  )
  norm_cross <- sqrt(sum(cross_product^2))
  
  # 三点共线时返回无穷大
  if (norm_cross == 0) {
    return(Inf)
  }
  
  # 计算曲率和半径
  curvature <- norm_cross / (norm_vec1^3)
  radius <- 1 / curvature
  return(radius)
}

# 主程序
file_path <- "path_points.txt"  # 替换为你的文件路径
points_df <- read_3d_points(file_path)

# 检查点数量
if (nrow(points_df) < 3) {
  stop("文件中至少需要3个点才能计算半径")
}

# 初始化半径列
points_df$radius <- NA

# 迭代计算每个中间点的半径
for (n in 1:(nrow(points_df)-2)) {
  p1 <- as.numeric(points_df[n, c("x", "y", "z")])
  p2 <- as.numeric(points_df[n+1, c("x", "y", "z")])
  p3 <- as.numeric(points_df[n+2, c("x", "y", "z")])
  points_df$radius[n+1] <- compute_curvature_radius(p1, p2, p3)
}

# 输出结果
print(points_df)

# 可选：保存结果到文件
# write.table(points_df, "radii_result.txt", sep=";", row.names=FALSE, quote=FALSE)

使用说明 #

1. 将你的3D点文件保存为path_points.txt（或替换代码中的文件路径），每行格式为x;y;z
2. 运行脚本后，会得到每个点对应的半径：首尾点无对应半径（设为NA/NaN），中间点为对应三点组计算的结果
3. 脚本已处理了三点共线、两点重合等异常情况，此时半径设为无穷大Inf

内容的提问来源于stack exchange，提问作者google Nutzer