无人物流车转弯路径曲线化功能 - 实施方案

1. 目标概述
实现基于圆弧过渡（Arc Fillet）的路径曲线化功能，替代现有的直线连接方式，确保仿真系统能准确检测物体转弯时的扫掠路径（Swept Path）碰撞。

核心原理
控制点与物理点分离：用户操作的
PathPoint
 作为控制点，实际物理路径由 PathEdge 表示
圆弧过渡法：在相邻路径段转折处插入切圆弧，使用进入切点 Ts 和退出切点 Te 连接
安全截断：当计算的切线长度超过线段长度45%时，自动缩减半径，防止圆弧过大
2. 数据结构改动
2.1 新增数据模型
[NEW] PathSegmentType 枚举
/// <summary>
/// 路径段类型
/// </summary>
public enum PathSegmentType
{
    /// <summary>
    /// 直线段
    /// </summary>
    Straight,

    /// <summary>
    /// 圆弧段
    /// </summary>
    Arc
}
[NEW] ArcTrajectory 类
/// <summary>
/// 圆弧轨迹数据
/// </summary>
[Serializable]
public class ArcTrajectory
{
    /// <summary>
    /// 进入切点
    /// </summary>
    public Point3D Ts { get; set; }

    /// <summary>
    /// 退出切点
    /// </summary>
    public Point3D Te { get; set; }
    
    /// <summary>
    /// 圆心位置
    /// </summary>
    public Point3D ArcCenter { get; set; }
    
    /// <summary>
    /// 请求半径（配置的转向半径）
    /// </summary>
    public double RequestedRadius { get; set; }
    
    /// <summary>
    /// 实际半径（安全截断后）
    /// </summary>
    public double ActualRadius { get; set; }
    
    /// <summary>
    /// 偏转角（弧度）
    /// </summary>
    public double DeflectionAngle { get; set; }
    
    /// <summary>
    /// 圆弧长度（米）
    /// </summary>
    public double ArcLength { get; set; }
}
[NEW] PathEdge 类
/// <summary>
/// 路径边 - 连接两个连续控制点的物理路径段
/// </summary>
[Serializable]
public class PathEdge
{
    /// <summary>
    /// 边唯一标识符
    /// </summary>
    public string Id { get; set; }

    /// <summary>
    /// 起始控制点ID
    /// </summary>
    public string StartPointId { get; set; }
    
    /// <summary>
    /// 结束控制点ID
    /// </summary>
    public string EndPointId { get; set; }
    
    /// <summary>
    /// 路径段类型
    /// </summary>
    public PathSegmentType SegmentType { get; set; }
    
    /// <summary>
    /// 圆弧轨迹数据（仅当 SegmentType == Arc 时有效）
    /// </summary>
    public ArcTrajectory Trajectory { get; set; }
    
    /// <summary>
    /// 边的物理长度（米）
    /// 直线段：两点间距离；圆弧段：直线段长度 + 圆弧长度
    /// </summary>
    public double PhysicalLength { get; set; }
    
    /// <summary>
    /// 采样点序列（用于碰撞检测和动画）
    /// </summary>
    [XmlIgnore]
    public List<Point3D> SampledPoints { get; set; }
}
2.2 修改现有数据模型
[MODIFY]
PathPoint.cs
新增属性：

/// <summary>
/// 自定义转向半径（米），用于局部急转弯场景
/// 为 null 时使用全局默认值
/// </summary>
public double? CustomTurnRadius { get; set; }
[MODIFY]
PathRoute.cs
新增属性：

/// <summary>
/// 路径边集合 - 存储物理路径段
/// </summary>
public List<PathEdge> Edges { get; set; } = new List<PathEdge>();
/// <summary>
/// 默认转向半径（米）
/// </summary>
public double TurnRadius { get; set; } = 1.5;
/// <summary>
/// 是否已曲线化
/// </summary>
public bool IsCurved { get; set; } = false;
修改方法：

UpdateTotalLength()
: 综合计算所有 Edges 的物理长度
Clone()
: 需要深拷贝 Edges 集合
2.3 数据库表结构扩展
[MODIFY]
PathDatabase.cs
新增表 PathEdges：

CREATE TABLE IF NOT EXISTS PathEdges (
    Id TEXT PRIMARY KEY,
    RouteId TEXT NOT NULL,
    StartPointId TEXT NOT NULL,
    EndPointId TEXT NOT NULL,
    SegmentType INTEGER NOT NULL,  -- 0=Straight, 1=Arc
    PhysicalLength REAL NOT NULL,

    -- 圆弧轨迹数据 (仅 SegmentType=1 时有效)
    Ts_X REAL,
    Ts_Y REAL,
    Ts_Z REAL,
    Te_X REAL,
    Te_Y REAL,
    Te_Z REAL,
    ArcCenter_X REAL,
    ArcCenter_Y REAL,
    ArcCenter_Z REAL,
    RequestedRadius REAL,
    ActualRadius REAL,
    DeflectionAngle REAL,
    ArcLength REAL,
    
    FOREIGN KEY (RouteId) REFERENCES PathRoutes(Id) ON DELETE CASCADE
);
修改表
PathRoutes
：

ALTER TABLE PathRoutes ADD COLUMN TurnRadius REAL DEFAULT 1.5;
ALTER TABLE PathRoutes ADD COLUMN IsCurved INTEGER DEFAULT 0;
修改表
PathPoints
：

ALTER TABLE PathPoints ADD COLUMN CustomTurnRadius REAL;
3. 核心算法实现
3.1 PathCurveEngine 类
位置: src/Core/PathCurveEngine.cs (新建)

3.1.1 CalculateFillet 方法
/// <summary>
/// 计算圆弧切点和轨迹参数
/// </summary>
/// <param name="pPrev">前一个控制点</param>
/// <param name="pCurr">当前控制点</param>
/// <param name="pNext">下一个控制点</param>
/// <param name="turnRadius">转向半径（米）</param>
/// <returns>圆弧轨迹数据</returns>
public static ArcTrajectory CalculateFillet(
    Point3D pPrev,
    Point3D pCurr,
    Point3D pNext,
    double turnRadius)
{
    // 1. 计算单位向量
    Vector3D v1 = (pPrev - pCurr).Normalize();
    Vector3D v2 = (pNext - pCurr).Normalize();

    // 2. 计算夹角 α
    double cosAlpha = Vector3D.DotProduct(v1, v2);
    double angleRad = Math.Acos(Math.Clamp(cosAlpha, -1.0, 1.0));
    
    // 3. 计算切线长 Lt = R / tan(α/2)
    double Lt = turnRadius / Math.Tan(angleRad / 2.0);
    
    // 4. 安全截断检查
    double seg1Length = (pPrev - pCurr).Length;
    double seg2Length = (pNext - pCurr).Length;
    double maxAllowedLt = Math.Min(seg1Length, seg2Length) * 0.45;
    
    double actualRadius = turnRadius;
    if (Lt > maxAllowedLt)
    {
        Lt = maxAllowedLt;
        actualRadius = Lt * Math.Tan(angleRad / 2.0);
    }
    
    // 5. 计算切点
    Point3D ts = pCurr + v1 * Lt;
    Point3D te = pCurr + v2 * Lt;
    
    // 6. 计算圆心 (使用角平分线方向)
    Vector3D bisector = (v1 + v2).Normalize();
    double distToCenter = actualRadius / Math.Sin(angleRad / 2.0);
    Point3D arcCenter = pCurr + bisector * distToCenter;
    
    // 7. 计算圆弧长度
    double arcLength = actualRadius * angleRad;
    
    return new ArcTrajectory
    {
        Ts = ts,
        Te = te,
        ArcCenter = arcCenter,
        RequestedRadius = turnRadius,
        ActualRadius = actualRadius,
        DeflectionAngle = angleRad,
        ArcLength = arcLength
    };
}
3.1.2 SampleArc 方法
/// <summary>
/// 圆弧采样为离散点序列
/// </summary>
/// <param name="trajectory">圆弧轨迹</param>
/// <param name="samplingStep">采样步长（米）</param>
/// <returns>采样点列表</returns>
public static List<Point3D> SampleArc(
    ArcTrajectory trajectory,
    double samplingStep)
{
    var points = new List<Point3D>();

    // 根据采样步长计算采样点数量
    int sampleCount = Math.Max(2, (int)Math.Ceiling(trajectory.ArcLength / samplingStep));
    
    // 计算起始和结束向量
    Vector3D startVec = (trajectory.Ts - trajectory.ArcCenter).Normalize();
    Vector3D endVec = (trajectory.Te - trajectory.ArcCenter).Normalize();
    
    // 计算旋转轴（叉乘）
    Vector3D rotationAxis = Vector3D.CrossProduct(startVec, endVec).Normalize();
    
    // 等角度插值
    for (int i = 0; i <= sampleCount; i++)
    {
        double t = i / (double)sampleCount;
        double theta = t * trajectory.DeflectionAngle;
        
        // Rodrigues旋转公式
        Point3D sampledPoint = RotatePointAroundAxis(
            trajectory.Ts, 
            trajectory.ArcCenter, 
            rotationAxis, 
            theta
        );
        
        points.Add(sampledPoint);
    }
    
    return points;
}
3.1.3 ApplyCurvatureToRoute 方法
/// <summary>
/// 对路径应用曲线化处理
/// </summary>
/// <param name="route">待处理的路径</param>
/// <param name="samplingStep">采样步长（米）</param>
public static void ApplyCurvatureToRoute(PathRoute route, double samplingStep)
{
    route.Edges.Clear();
    var sortedPoints = route.GetSortedPoints();

    if (sortedPoints.Count < 2)
    {
        route.IsCurved = false;
        return;
    }
    
    for (int i = 0; i < sortedPoints.Count - 1; i++)
    {
        var p1 = sortedPoints[i];
        var p2 = sortedPoints[i + 1];
        
        // 判断是否为转折点（需要圆弧过渡）
        bool needsArc = (i > 0 && i < sortedPoints.Count - 2);
        
        if (needsArc)
        {
            var p0 = sortedPoints[i - 1];
            var p2Next = sortedPoints[i + 2];
            
            // 获取转向半径（优先使用自定义值）
            double radius = p1.CustomTurnRadius ?? route.TurnRadius;
            
            // 计算圆弧
            var arcTraj = CalculateFillet(p0, p1, p2, radius);
            
            // 创建Edge（包含直线段 + 圆弧段）
            var edge = BuildEdgeWithArc(p1, p2, arcTraj, samplingStep);
            route.Edges.Add(edge);
        }
        else
        {
            // 直线边
            var edge = BuildStraightEdge(p1, p2, samplingStep);
            route.Edges.Add(edge);
        }
    }
    
    route.IsCurved = true;
    route.RecalculateLength();
}
4. 配置文件扩展
[MODIFY]
SystemConfig.cs
在
PathEditingConfig
 类中新增：

/// <summary>
/// 默认转向半径（米）
/// 推荐值：1.0-2.0
/// </summary>
public double DefaultTurnRadius { get; set; } = 1.5;
/// <summary>
/// 圆弧采样步长（米）
/// 推荐值：0.02-0.1
/// </summary>
public double ArcSamplingStep { get; set; } = 0.05;

配置文件示例 (TOML)
[PathEditing]
CellSizeMeters = 0.5
MaxHeightDiffMeters = 0.35
ObjectLengthMeters = 1.0
ObjectWidthMeters = 1.0
ObjectHeightMeters = 2.0
SafetyMarginMeters = 0.05

# 路径曲线化配置

DefaultTurnRadius = 1.5
ArcSamplingStep = 0.05

1. 业务流程集成
5.1 路径编辑完成流程
[MODIFY]
PathPlanningManager.cs

-

FinishEditing
 方法
public bool FinishEditing()
{
    try
    {
        if (_currentRoute == null)
            return false;

        // **新增：应用曲线化**
        if (ConfigManager.Instance.Current.PathEditing.EnablePathCurving)
        {
            double samplingStep = ConfigManager.Instance.Current.PathEditing.ArcSamplingStep;
            _currentRoute.TurnRadius = ConfigManager.Instance.Current.PathEditing.DefaultTurnRadius;
            
            PathCurveEngine.ApplyCurvatureToRoute(_currentRoute, samplingStep);
            LogManager.Info($"路径曲线化完成: {_currentRoute.Name}, 边数: {_currentRoute.Edges.Count}");
        }
        
        // 保存到数据库（包含Edges）
        SaveRouteToDatabase(_currentRoute);
        
        // ... 现有逻辑 ...
    }
    catch (Exception ex)
    {
        LogManager.Error($"完成编辑失败: {ex.Message}");
        return false;
    }
}
5.2 数据持久化
[MODIFY]
PathDatabase.cs
新增方法：

SavePathEdges(string routeId, List<PathEdge> edges)
LoadPathEdges(string routeId) : List<PathEdge>
DeletePathEdges(string routeId)
修改方法：

SavePathRoute(PathRoute route)
: 同时保存 Edges
GetAllPathRoutes()
: 同时加载 Edges
DeletePathRoute(string routeId)
: 级联删除 Edges
5.3 导出格式扩展
[MODIFY]
PathDataManager.cs
DELMIA XML 格式：

<Path name="路径1">
  <Edges>
    <Edge type="arc">
      <Trajectory>
        <Ts x="10.5" y="5.2" z="0.0"/>
        <Te x="11.3" y="6.8" z="0.0"/>
        <ArcCenter x="10.9" y="6.0" z="0.0"/>
        <Radius>1.5</Radius>
      </Trajectory>
    </Edge>
  </Edges>
</Path>
CSV 格式（扁平化采样点）：

RouteId,EdgeIndex,SegmentType,PointIndex,X,Y,Z
route_001,0,Arc,0,10.5,5.2,0.0
route_001,0,Arc,1,10.52,5.25,0.0
...
6. 碰撞检测与动画集成
6.1 碰撞检测
[MODIFY] PathAnimationManager.cs - 碰撞检测逻辑
// 从 Edges 获取采样点而非直接使用 Points
foreach (var edge in route.Edges)
{
    if (edge.SampledPoints == null || edge.SampledPoints.Count == 0)
    {
        // 懒加载：若采样点未生成，现场生成
        edge.SampledPoints = GenerateSampledPoints(edge, arcSamplingStep);
    }

    foreach (var point in edge.SampledPoints)
    {
        // 执行OBB碰撞检测 ...
    }
}
6.2 动画生成
使用 edge.SampledPoints 代替原始控制点生成动画帧序列。

1. 验证计划
7.1 单元测试
新建测试文件: test/Core/PathCurveEngineTests.cs

测试用例：

基础几何计算

验证90度直角转弯的圆弧计算
验证锐角/钝角场景
验证3D空间旋转正确性
安全截断

验证半径过大时自动缩减
边界条件：最小夹角（~0°）和最大夹角（~180°）
采样精度

验证采样点数量符合步长要求
验证采样点均匀分布在圆弧上
运行命令（需确认实际测试框架）：

# 假设使用 xUnit

dotnet test NavisworksTransport.Tests --filter "FullyQualifiedName~PathCurveEngine"
7.2 集成测试
场景测试路径：

创建包含3个控制点的简单L型路径
设置转向半径 = 1.5m
完成编辑触发曲线化
验证：
Edges 数量= 2
第一个Edge为直线段，第二个Edge包含圆弧
TotalLength 正确（直线段 + 圆弧段）
重新加载路径验证持久化
7.3 手动验证
IMPORTANT

以下步骤需要用户在Navisworks中手动验证

测试步骤：

打开Navisworks，加载测试模型
进入路径规划模式，手工添加4个控制点形成Z字形路径
在配置文件中设置：
DefaultTurnRadius = 2.0
ArcSamplingStep = 0.05
EnablePathCurving = true
完成编辑，观察路径可视化是否显示平滑曲线（而非折线）
运行碰撞检测，检查转弯处的采样点密度
导出DELMIA XML，用文本编辑器验证圆弧轨迹参数存在
预期结果：

路径在转折点处显示圆弧过渡
碰撞检测能捕获内轮差区域的碰撞
XML包含完整的 <Trajectory> 节点
8. 风险与注意事项
8.1 性能影响
圆弧采样：每个转折点生成 ArcLength / SamplingStep 个采样点
缓解措施：默认步长0.05m平衡精度与性能，支持配置调整
8.2 数据兼容性
旧版路径数据库无 Edges 表
迁移策略：
检测表是否存在，不存在则创建
加载旧路径时，Edges为空视为"未曲线化"，用户完成编辑时自动曲线化
8.3 极端场景
非常小的转向半径 + 锐角：可能导致无法满足几何约束
处理：最小半径限制0.3m，警告日志记录
9. 实施顺序建议
✅ 数据结构 → 新增类和修改现有模型
✅ 配置管理 → 扩展SystemConfig和TOML
✅ 核心算法 → PathCurveEngine + 单元测试
✅ 数据库 → 表结构变更 + CRUD方法
✅ 业务集成 → FinishEditing + 数据持久化
✅ 导出扩展 → XML/CSV格式支持
✅ 碰撞检测 → 使用Edges采样点
✅ 验证测试 → 集成测试 + 手动验证
10. 后续优化方向
支持Clothoid曲线（回旋曲线）以实现更平滑的加速度变化
UI可视化：实时预览圆弧位置
性能优化：采样点缓存机制

物流车转弯路径曲线化功能实施任务规划
总体目标
实现基于圆弧过渡（Arc Fillet）的路径曲线化功能，替代现有的直线连接方式，满足仿真系统对物理真实性的要求。

实施阶段
[ ] 阶段1: 数据结构设计与扩展
 设计 Edge（边）数据结构
 PathEdge 类定义
 PathSegmentType 枚举（直线/圆弧）
 ArcTrajectory 圆弧轨迹数据
 扩展 PathPoint 支持局部半径覆盖
 新增 CustomTurnRadius 属性
 重构 PathRoute 类
 新增 Edges 集合
 新增 TurnRadius 全局转向半径
 更新长度计算逻辑
 更新数据库表结构
 设计 Edges 表结构
 设计数据迁移策略
[ ] 阶段2: 曲线化算法实现
 创建 PathCurveEngine 核心算法类
 CalculateFillet 方法（圆弧切点计算）
 安全截断逻辑实现
 SampleArc 方法（圆弧采样）
 实现路径曲线化主流程
 ApplyCurvatureToRoute 方法
 BuildEdgesFromPoints 方法
 单元测试
 基础几何计算测试
 边界条件测试
[ ] 阶段3: 配置管理
 添加曲线化相关配置项
 DefaultTurnRadius (默认转向半径)
 ArcSamplingStep (圆弧采样步长，默认0.05m)
 EnablePathCurving (启用开关)
 更新配置文件读取/保存逻辑
[ ] 阶段4: 集成到现有业务流程
 修改 PathRoute 长度计算
 直线段 + 圆弧段综合计算
 集成到路径编辑完成流程
 FinishEditing 触发曲线化
 更新数据持久化
 PathDatabase 保存/加载 Edges
 PathDataManager 导出格式扩展
[ ] 阶段5: 碰撞检测与动画集成
 碰撞检测使用曲线路径
 采样点序列生成
 集成到现有碰撞检测
 PathAnimationManager 支持曲线路径
 使用 Edges 生成动画帧
[ ] 阶段6: DELMIA 导出扩展
 XML Tag Group 格式支持圆弧
 导出 Ts, Te, ArcCenter
 CSV 格式导出采样点序列
[ ] 阶段7: 测试与验证
 功能测试
 性能测试
 边界场景测试