NavisworksTransport/doc/working/路径曲线化实施方案_20251230.md

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

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

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

/// 路径段类型 ///

public enum PathSegmentType { ///

/// 直线段 ///

Straight,

/// <summary>
/// 圆弧段
/// </summary>
Arc

} [NEW] ArcTrajectory 类 ///

/// 圆弧轨迹数据 ///

[Serializable] public class ArcTrajectory { ///

/// 进入切点 ///

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 类 ///

/// 路径边 - 连接两个连续控制点的物理路径段 ///

[Serializable] public class PathEdge { ///

/// 边唯一标识符 ///

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 新增属性：

///

/// 自定义转向半径（米），用于局部急转弯场景 /// 为 null 时使用全局默认值 ///

public double? CustomTurnRadius { get; set; } [MODIFY] PathRoute.cs 新增属性：

///

/// 路径边集合 - 存储物理路径段 ///

public List Edges { get; set; } = new List(); ///

/// 默认转向半径（米） ///

public double TurnRadius { get; set; } = 1.5; ///

/// 是否已曲线化 ///

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 方法 ///

/// 计算圆弧切点和轨迹参数 ///

/// 前一个控制点 /// 当前控制点 /// 下一个控制点 /// 转向半径（米） /// 圆弧轨迹数据 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 方法 ///

/// 圆弧采样为离散点序列 ///

/// 圆弧轨迹 /// 采样步长（米） /// 采样点列表 public static List SampleArc( ArcTrajectory trajectory, double samplingStep) { var points = new List();

// 根据采样步长计算采样点数量
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 方法 ///

/// 对路径应用曲线化处理 ///

/// 待处理的路径 /// 采样步长（米） 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 类中新增：

///

/// 默认转向半径（米） /// 推荐值：1.0-2.0 ///

public double DefaultTurnRadius { get; set; } = 1.5; ///

/// 圆弧采样步长（米） /// 推荐值：0.02-0.1 ///

public double ArcSamplingStep { get; set; } = 0.05;

配置文件示例 (TOML) [PathEditing] CellSizeMeters = 0.5 MaxHeightDiffMeters = 0.35 VehicleLengthMeters = 1.0 VehicleWidthMeters = 1.0 VehicleHeightMeters = 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 edges) LoadPathEdges(string routeId) : List DeletePathEdges(string routeId) 修改方法：

SavePathRoute(PathRoute route)

同时保存 Edges GetAllPathRoutes()

同时加载 Edges DeletePathRoute(string routeId)

级联删除 Edges 5.3 导出格式扩展 [MODIFY] PathDataManager.cs DELMIA XML 格式：

1.5 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包含完整的 节点 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: 测试与验证 功能测试 性能测试 边界场景测试