470 lines
19 KiB
C#
470 lines
19 KiB
C#
using System;
|
||
using System.Collections.Generic;
|
||
using System.Linq;
|
||
using Autodesk.Navisworks.Api;
|
||
using NavisworksTransport.Core;
|
||
|
||
namespace NavisworksTransport.PathPlanning
|
||
{
|
||
/// <summary>
|
||
/// 路径优化器
|
||
/// 提供多种路径优化算法，包括简化、平滑、避障等
|
||
/// </summary>
|
||
public class PathOptimizer
|
||
{
|
||
/// <summary>
|
||
/// 路径优化配置参数
|
||
/// </summary>
|
||
public class OptimizationConfig
|
||
{
|
||
/// <summary>
|
||
/// 是否启用路径简化（去除共线点）
|
||
/// </summary>
|
||
public bool EnableSimplification { get; set; } = true;
|
||
|
||
/// <summary>
|
||
/// 共线检测的容差值（米）
|
||
/// </summary>
|
||
public double CollinearTolerance { get; set; } = 0.01;
|
||
|
||
/// <summary>
|
||
/// 是否启用路径平滑（未来扩展）
|
||
/// </summary>
|
||
public bool EnableSmoothing { get; set; } = false;
|
||
|
||
/// <summary>
|
||
/// 转弯半径（米，未来扩展）
|
||
/// </summary>
|
||
public double TurningRadius { get; set; } = 1.0;
|
||
|
||
/// <summary>
|
||
/// 圆弧分段数（未来扩展）
|
||
/// </summary>
|
||
public int ArcSegments { get; set; } = 8;
|
||
|
||
/// <summary>
|
||
/// 是否启用碰撞检测验证（未来扩展）
|
||
/// </summary>
|
||
public bool EnableCollisionCheck { get; set; } = false;
|
||
|
||
/// <summary>
|
||
/// 车辆长度（米，未来扩展）
|
||
/// </summary>
|
||
public double VehicleLength { get; set; } = 2.0;
|
||
|
||
/// <summary>
|
||
/// 车辆宽度（米，未来扩展）
|
||
/// </summary>
|
||
public double VehicleWidth { get; set; } = 1.0;
|
||
}
|
||
|
||
private readonly OptimizationConfig _config;
|
||
|
||
/// <summary>
|
||
/// 构造函数
|
||
/// </summary>
|
||
/// <param name="config">优化配置，为null时使用默认配置</param>
|
||
public PathOptimizer(OptimizationConfig config = null)
|
||
{
|
||
_config = config ?? new OptimizationConfig();
|
||
}
|
||
|
||
/// <summary>
|
||
/// 优化路径的主入口
|
||
/// </summary>
|
||
/// <param name="originalPath">原始路径</param>
|
||
/// <param name="gridMap">网格地图（用于网格坐标转换）</param>
|
||
/// <returns>优化后的路径</returns>
|
||
public PathRoute OptimizePath(PathRoute originalPath, GridMap gridMap = null)
|
||
{
|
||
if (originalPath == null)
|
||
{
|
||
LogManager.Warning("[路径优化] 原始路径为null");
|
||
return null;
|
||
}
|
||
|
||
if (originalPath.Points.Count <= 2)
|
||
{
|
||
LogManager.Info("[路径优化] 路径点数量≤2，无需优化");
|
||
return originalPath;
|
||
}
|
||
|
||
try
|
||
{
|
||
var startTime = DateTime.Now;
|
||
var originalCount = originalPath.Points.Count;
|
||
|
||
LogManager.Info($"[路径优化] 开始优化路径：{originalPath.Name}，原始点数：{originalCount}");
|
||
|
||
// 复制路径点进行优化
|
||
var optimizedPoints = new List<PathPoint>(originalPath.Points);
|
||
|
||
// 1. 基础简化 - 使用基于网格的算法
|
||
if (_config.EnableSimplification)
|
||
{
|
||
if (gridMap != null)
|
||
{
|
||
LogManager.Info("[路径优化] 执行基于网格的路径简化");
|
||
optimizedPoints = SimplifyGridBasedPath(optimizedPoints, gridMap);
|
||
LogManager.Info($"[路径优化] 简化完成，点数：{originalCount} -> {optimizedPoints.Count}");
|
||
}
|
||
else
|
||
{
|
||
LogManager.Info("[路径优化] 执行传统共线点简化");
|
||
optimizedPoints = SimplifyCollinearPoints(optimizedPoints);
|
||
LogManager.Info($"[路径优化] 简化完成，点数：{originalCount} -> {optimizedPoints.Count}");
|
||
}
|
||
}
|
||
|
||
// 2. 未来扩展：路径平滑
|
||
if (_config.EnableSmoothing)
|
||
{
|
||
LogManager.Info("[路径优化] 路径平滑功能尚未实现");
|
||
// optimizedPoints = SmoothPath(optimizedPoints);
|
||
}
|
||
|
||
// 3. 未来扩展：碰撞检测验证
|
||
if (_config.EnableCollisionCheck)
|
||
{
|
||
LogManager.Info("[路径优化] 碰撞检测功能尚未实现");
|
||
// ValidatePathCollision(optimizedPoints);
|
||
}
|
||
|
||
// 创建优化后的路径
|
||
var optimizedRoute = CreateOptimizedRoute(originalPath, optimizedPoints);
|
||
|
||
var duration = DateTime.Now - startTime;
|
||
LogManager.Info($"[路径优化] 优化完成，耗时：{duration.TotalMilliseconds:F1}ms，优化率：{(1.0 - (double)optimizedPoints.Count / originalCount) * 100:F1}%");
|
||
|
||
return optimizedRoute;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"[路径优化] 优化过程发生异常：{ex.Message}", ex);
|
||
return originalPath; // 发生异常时返回原始路径
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 基于网格坐标的路径简化算法
|
||
/// </summary>
|
||
/// <param name="points">原始路径点列表</param>
|
||
/// <param name="gridMap">网格地图</param>
|
||
/// <returns>简化后的路径点列表</returns>
|
||
private List<PathPoint> SimplifyGridBasedPath(List<PathPoint> points, GridMap gridMap)
|
||
{
|
||
if (points == null || points.Count < 3)
|
||
return points;
|
||
|
||
// 转换为网格坐标
|
||
var worldPath = points.Select(p => p.Position).ToList();
|
||
var gridPath = worldPath.Select(p => gridMap.WorldToGrid(p)).ToList();
|
||
|
||
// 检查重复点（调试用）
|
||
int duplicateCount = 0;
|
||
for (int i = 1; i < gridPath.Count; i++)
|
||
{
|
||
if (gridPath[i].Equals(gridPath[i-1]))
|
||
{
|
||
duplicateCount++;
|
||
LogManager.Warning($"[路径优化] 发现重复网格点：索引 {i-1} 和 {i} 都是 ({gridPath[i].X}, {gridPath[i].Y})");
|
||
}
|
||
}
|
||
|
||
if (duplicateCount > 0)
|
||
{
|
||
LogManager.Warning($"[路径优化] 总共发现 {duplicateCount} 个重复网格点，开始去重处理");
|
||
}
|
||
else
|
||
{
|
||
LogManager.Info("[路径优化] 未发现重复网格点，A*算法输出正常");
|
||
}
|
||
|
||
// 步骤1：先去除重复点
|
||
var dedupedGridPath = new List<GridPoint2D>();
|
||
var dedupedPoints = new List<PathPoint>();
|
||
for (int i = 0; i < gridPath.Count; i++)
|
||
{
|
||
if (i == 0 || !gridPath[i].Equals(gridPath[i-1]))
|
||
{
|
||
dedupedGridPath.Add(gridPath[i]);
|
||
dedupedPoints.Add(points[i]);
|
||
}
|
||
}
|
||
|
||
LogManager.Info($"[路径优化] 去重完成：{gridPath.Count} -> {dedupedGridPath.Count} 个点");
|
||
|
||
if (dedupedGridPath.Count < 3)
|
||
{
|
||
LogManager.Info("[路径优化] 去重后点数不足3个，直接返回");
|
||
return dedupedPoints;
|
||
}
|
||
|
||
// 步骤2：基于去重后的网格路径进行方向优化
|
||
var simplified = new List<GridPoint2D> { dedupedGridPath[0] };
|
||
|
||
// 获取初始方向（归一化）
|
||
int initialDx = dedupedGridPath[1].X - dedupedGridPath[0].X;
|
||
int initialDy = dedupedGridPath[1].Y - dedupedGridPath[0].Y;
|
||
GridPoint2D prevDirection = new GridPoint2D(
|
||
initialDx == 0 ? 0 : Math.Sign(initialDx),
|
||
initialDy == 0 ? 0 : Math.Sign(initialDy)
|
||
);
|
||
|
||
LogManager.Debug($"[路径优化] 初始方向：({prevDirection.X}, {prevDirection.Y})");
|
||
|
||
// 简化路径：只保留转弯点（使用归一化方向）
|
||
for (int i = 2; i < dedupedGridPath.Count; i++)
|
||
{
|
||
// 计算当前线段的位移
|
||
int dx = dedupedGridPath[i].X - dedupedGridPath[i-1].X;
|
||
int dy = dedupedGridPath[i].Y - dedupedGridPath[i-1].Y;
|
||
|
||
// 归一化为单位方向向量 (-1, 0, 1)
|
||
var currentDirection = new GridPoint2D(
|
||
dx == 0 ? 0 : Math.Sign(dx),
|
||
dy == 0 ? 0 : Math.Sign(dy)
|
||
);
|
||
|
||
// 只有真正的方向改变时才保留转弯点
|
||
if (!currentDirection.Equals(prevDirection))
|
||
{
|
||
simplified.Add(dedupedGridPath[i - 1]);
|
||
prevDirection = currentDirection;
|
||
}
|
||
}
|
||
|
||
// 添加终点
|
||
simplified.Add(dedupedGridPath[dedupedGridPath.Count - 1]);
|
||
|
||
LogManager.Info($"[网格路径优化] 最终优化完成：{gridPath.Count} -> {dedupedGridPath.Count}(去重) -> {simplified.Count}(简化) 个点");
|
||
|
||
// 步骤3：转换回PathPoint格式
|
||
var simplifiedPoints = new List<PathPoint>();
|
||
foreach (var gridPoint in simplified)
|
||
{
|
||
// 在去重后的路径中找到对应的点
|
||
int originalIndex = dedupedGridPath.FindIndex(g => g.Equals(gridPoint));
|
||
if (originalIndex >= 0)
|
||
{
|
||
simplifiedPoints.Add(dedupedPoints[originalIndex]);
|
||
}
|
||
}
|
||
|
||
return simplifiedPoints;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 简化共线点（去除直线上的冗余点）
|
||
/// </summary>
|
||
/// <param name="points">原始路径点列表</param>
|
||
/// <returns>简化后的路径点列表</returns>
|
||
private List<PathPoint> SimplifyCollinearPoints(List<PathPoint> points)
|
||
{
|
||
if (points.Count <= 2) return points;
|
||
|
||
var simplified = new List<PathPoint>();
|
||
simplified.Add(points[0]); // 添加起点
|
||
|
||
LogManager.Info($"[共线简化] 开始简化，原始点数：{points.Count}");
|
||
int removedCount = 0;
|
||
|
||
// 遍历中间点，只保留转折点
|
||
for (int i = 1; i < points.Count - 1; i++)
|
||
{
|
||
var prevPoint = points[i - 1];
|
||
var currPoint = points[i];
|
||
var nextPoint = points[i + 1];
|
||
|
||
// 检查是否在同一直线上
|
||
bool isCollinear = IsCollinear(prevPoint.Position, currPoint.Position, nextPoint.Position);
|
||
|
||
if (!isCollinear)
|
||
{
|
||
// 这是一个转折点，需要保留
|
||
simplified.Add(currPoint);
|
||
LogManager.Debug($"[共线简化] 保留转折点 {i}：{currPoint.Name}");
|
||
}
|
||
else
|
||
{
|
||
// 这是直线上的冗余点，跳过
|
||
removedCount++;
|
||
LogManager.Debug($"[共线简化] 移除冗余点 {i}：{currPoint.Name}");
|
||
}
|
||
}
|
||
|
||
simplified.Add(points[points.Count - 1]); // 添加终点
|
||
|
||
LogManager.Info($"[共线简化] 简化完成，移除了 {removedCount} 个冗余点");
|
||
return simplified;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 检查三点是否真正共线（只有当三点都在同一条直线上且方向一致时才返回true）
|
||
/// 🔥 关键修复：严格检查两个线段的方向是否一致，防止在转折点处错误合并
|
||
/// </summary>
|
||
/// <param name="p1">第一个点</param>
|
||
/// <param name="p2">第二个点（中间点）</param>
|
||
/// <param name="p3">第三个点</param>
|
||
/// <returns>如果三点在同一直线上且方向一致返回true</returns>
|
||
private bool IsCollinear(Point3D p1, Point3D p2, Point3D p3)
|
||
{
|
||
const double tolerance = 0.1; // 容差值，单位：模型单位
|
||
|
||
// 🔥 修复：首先检查Z坐标，确保是2D路径
|
||
if (Math.Abs(p1.Z - p2.Z) > tolerance || Math.Abs(p2.Z - p3.Z) > tolerance)
|
||
{
|
||
LogManager.Debug($"[共线检测] ❌ 拒绝Z方向变化：({p1.X:F3},{p1.Y:F3},{p1.Z:F3}) -> ({p2.X:F3},{p2.Y:F3},{p2.Z:F3}) -> ({p3.X:F3},{p3.Y:F3},{p3.Z:F3})");
|
||
return false;
|
||
}
|
||
|
||
// 🔥 关键修复：分别检查两个线段的方向
|
||
// 线段1: p1 -> p2
|
||
double dx12 = p2.X - p1.X;
|
||
double dy12 = p2.Y - p1.Y;
|
||
|
||
// 线段2: p2 -> p3
|
||
double dx23 = p3.X - p2.X;
|
||
double dy23 = p3.Y - p2.Y;
|
||
|
||
// 🔥 修复：使用更严格的容差检查真正的重复点（坐标完全相同）
|
||
const double duplicatePointTolerance = 0.001; // 用很小的值检查真正的重复点
|
||
bool isSegment1Zero = Math.Abs(dx12) < duplicatePointTolerance && Math.Abs(dy12) < duplicatePointTolerance;
|
||
bool isSegment2Zero = Math.Abs(dx23) < duplicatePointTolerance && Math.Abs(dy23) < duplicatePointTolerance;
|
||
|
||
if (isSegment1Zero || isSegment2Zero)
|
||
{
|
||
// 🔥 关键修复：只有在真正重复点时才认为共线
|
||
LogManager.Debug($"[共线检测] ✅ 包含真正重复点，视为共线：({p1.X:F3},{p1.Y:F3}) -> ({p2.X:F3},{p2.Y:F3}) -> ({p3.X:F3},{p3.Y:F3})");
|
||
return true;
|
||
}
|
||
|
||
// 🔥 关键修复：严格检查两个线段是否都是水平或都是垂直
|
||
bool isSegment1Horizontal = Math.Abs(dy12) < tolerance && Math.Abs(dx12) > tolerance;
|
||
bool isSegment1Vertical = Math.Abs(dx12) < tolerance && Math.Abs(dy12) > tolerance;
|
||
|
||
bool isSegment2Horizontal = Math.Abs(dy23) < tolerance && Math.Abs(dx23) > tolerance;
|
||
bool isSegment2Vertical = Math.Abs(dx23) < tolerance && Math.Abs(dy23) > tolerance;
|
||
|
||
// 🔥 核心逻辑：两个线段必须都是同一种类型（都水平或都垂直）
|
||
if (isSegment1Horizontal && isSegment2Horizontal)
|
||
{
|
||
// 检查水平方向是否一致（同向或反向都可以）
|
||
bool sameDirection = (dx12 * dx23 > 0) || Math.Abs(dx12) < tolerance || Math.Abs(dx23) < tolerance;
|
||
if (sameDirection)
|
||
{
|
||
LogManager.Debug($"[共线检测] ✅ 水平共线：({p1.X:F3},{p1.Y:F3}) -> ({p2.X:F3},{p2.Y:F3}) -> ({p3.X:F3},{p3.Y:F3})");
|
||
return true;
|
||
}
|
||
else
|
||
{
|
||
LogManager.Debug($"[共线检测] ❌ 水平线段方向相反：dx12={dx12:F3}, dx23={dx23:F3}");
|
||
return false;
|
||
}
|
||
}
|
||
else if (isSegment1Vertical && isSegment2Vertical)
|
||
{
|
||
// 检查垂直方向是否一致（同向或反向都可以）
|
||
bool sameDirection = (dy12 * dy23 > 0) || Math.Abs(dy12) < tolerance || Math.Abs(dy23) < tolerance;
|
||
if (sameDirection)
|
||
{
|
||
LogManager.Debug($"[共线检测] ✅ 垂直共线：({p1.X:F3},{p1.Y:F3}) -> ({p2.X:F3},{p2.Y:F3}) -> ({p3.X:F3},{p3.Y:F3})");
|
||
return true;
|
||
}
|
||
else
|
||
{
|
||
LogManager.Debug($"[共线检测] ❌ 垂直线段方向相反：dy12={dy12:F3}, dy23={dy23:F3}");
|
||
return false;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
// 🔥 关键：不同类型的线段（一个水平一个垂直，或包含斜线）一律拒绝
|
||
string seg1Type = isSegment1Horizontal ? "水平" : (isSegment1Vertical ? "垂直" : "斜线");
|
||
string seg2Type = isSegment2Horizontal ? "水平" : (isSegment2Vertical ? "垂直" : "斜线");
|
||
LogManager.Debug($"[共线检测] ❌ 线段类型不匹配：线段1={seg1Type}({dx12:F3},{dy12:F3}), 线段2={seg2Type}({dx23:F3},{dy23:F3})");
|
||
return false;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 创建优化后的路径对象
|
||
/// </summary>
|
||
/// <param name="originalPath">原始路径</param>
|
||
/// <param name="optimizedPoints">优化后的路径点</param>
|
||
/// <returns>优化后的路径对象</returns>
|
||
private PathRoute CreateOptimizedRoute(PathRoute originalPath, List<PathPoint> optimizedPoints)
|
||
{
|
||
var optimizedRoute = new PathRoute($"{originalPath.Name}_优化")
|
||
{
|
||
Id = originalPath.Id,
|
||
IsComplete = originalPath.IsComplete,
|
||
OriginalEndPoint = originalPath.OriginalEndPoint,
|
||
ActualEndPoint = originalPath.ActualEndPoint,
|
||
CompletionPercentage = originalPath.CompletionPercentage
|
||
};
|
||
|
||
// 重新分配索引和更新点类型
|
||
for (int i = 0; i < optimizedPoints.Count; i++)
|
||
{
|
||
var point = optimizedPoints[i];
|
||
point.Index = i;
|
||
|
||
// 重新确定点类型
|
||
if (i == 0)
|
||
point.Type = PathPointType.StartPoint;
|
||
else if (i == optimizedPoints.Count - 1)
|
||
point.Type = PathPointType.EndPoint;
|
||
else
|
||
point.Type = PathPointType.WayPoint;
|
||
|
||
optimizedRoute.Points.Add(point);
|
||
}
|
||
|
||
// 重新计算路径长度
|
||
optimizedRoute.RecalculateLength();
|
||
|
||
return optimizedRoute;
|
||
}
|
||
|
||
#region 未来扩展方法（暂时保留接口）
|
||
|
||
/// <summary>
|
||
/// 路径平滑处理（未来实现）
|
||
/// </summary>
|
||
/// <param name="points">路径点列表</param>
|
||
/// <returns>平滑后的路径点列表</returns>
|
||
private List<PathPoint> SmoothPath(List<PathPoint> points)
|
||
{
|
||
// TODO: 实现贝塞尔曲线或圆弧平滑
|
||
LogManager.Info("[路径平滑] 功能尚未实现");
|
||
return points;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 在转角处添加圆弧（未来实现）
|
||
/// </summary>
|
||
/// <param name="points">路径点列表</param>
|
||
/// <returns>添加圆弧后的路径点列表</returns>
|
||
private List<PathPoint> AddTurningArcs(List<PathPoint> points)
|
||
{
|
||
// TODO: 在转角处添加圆弧
|
||
LogManager.Info("[圆弧转弯] 功能尚未实现");
|
||
return points;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 验证路径碰撞（未来实现）
|
||
/// </summary>
|
||
/// <param name="points">路径点列表</param>
|
||
/// <returns>是否通过碰撞检测</returns>
|
||
private bool ValidatePathCollision(List<PathPoint> points)
|
||
{
|
||
// TODO: 实现碰撞检测验证
|
||
LogManager.Info("[碰撞检测] 功能尚未实现");
|
||
return true;
|
||
}
|
||
|
||
#endregion
|
||
}
|
||
} |