524 lines
22 KiB
C#
524 lines
22 KiB
C#
using System;
|
||
using System.Collections.Generic;
|
||
using System.Linq;
|
||
using Autodesk.Navisworks.Api;
|
||
|
||
namespace NavisworksTransport.PathPlanning
|
||
{
|
||
/// <summary>
|
||
/// 路径优化器
|
||
/// 提供多种路径优化算法，包括简化、平滑、避障等
|
||
/// </summary>
|
||
public class PathOptimizer
|
||
{
|
||
/// <summary>
|
||
/// 共线检测容差（米）
|
||
/// </summary>
|
||
private const double COLLINEAR_TOLERANCE_METERS = 0.1;
|
||
|
||
/// <summary>
|
||
/// 重复点检测容差（米）- 用于检测坐标完全相同的点
|
||
/// </summary>
|
||
private const double DUPLICATE_POINT_TOLERANCE_METERS = 0.001;
|
||
|
||
/// <summary>
|
||
/// 路径优化配置参数
|
||
/// </summary>
|
||
public class OptimizationConfig
|
||
{
|
||
/// <summary>
|
||
/// 是否启用路径简化（去除共线点）
|
||
/// </summary>
|
||
public bool EnableSimplification { get; set; } = true;
|
||
|
||
/// <summary>
|
||
/// 共线检测的容差值（米）
|
||
/// </summary>
|
||
public double CollinearTolerance { get; set; }
|
||
|
||
/// <summary>
|
||
/// 是否启用路径平滑（未来扩展）
|
||
/// </summary>
|
||
public bool EnableSmoothing { get; set; }
|
||
|
||
/// <summary>
|
||
/// 转弯半径（米，未来扩展）
|
||
/// </summary>
|
||
public double TurningRadius { get; set; }
|
||
|
||
/// <summary>
|
||
/// 圆弧分段数（未来扩展）
|
||
/// </summary>
|
||
public int ArcSegments { get; set; }
|
||
|
||
/// <summary>
|
||
/// 是否启用碰撞检测验证（未来扩展）
|
||
/// </summary>
|
||
public bool EnableCollisionCheck { get; set; }
|
||
|
||
/// <summary>
|
||
/// 物体长度（米，未来扩展）
|
||
/// </summary>
|
||
public double ObjectLength { get; set; }
|
||
|
||
/// <summary>
|
||
/// 物体宽度（米，未来扩展）
|
||
/// </summary>
|
||
public double ObjectWidth { get; set; }
|
||
}
|
||
|
||
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)
|
||
{
|
||
optimizedPoints = SimplifyGridBasedPath(optimizedPoints, gridMap);
|
||
LogManager.Info($"[路径优化] 简化完成，点数：{originalCount} -> {optimizedPoints.Count}");
|
||
}
|
||
else
|
||
{
|
||
optimizedPoints = SimplifyCollinearPoints(optimizedPoints);
|
||
LogManager.Info($"[路径优化] 简化完成，点数：{originalCount} -> {optimizedPoints.Count}");
|
||
}
|
||
}
|
||
|
||
// 2. 未来扩展：路径平滑
|
||
if (_config.EnableSmoothing)
|
||
{
|
||
LogManager.Info("[路径优化] 路径平滑功能尚未实现");
|
||
// optimizedPoints = SmoothPath(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();
|
||
|
||
// 步骤1：先去除重复点，但保护限速边界和多层高度
|
||
var dedupedGridPath = new List<GridPoint2D>();
|
||
var dedupedPoints = new List<PathPoint>();
|
||
for (int i = 0; i < gridPath.Count; i++)
|
||
{
|
||
// 🔥 关键修复：即使网格坐标相同，如果限速不同或高度不同也不能去重
|
||
// 2D场景：hasHeightChange为false，不影响原有逻辑
|
||
// 3D场景：保留同网格不同高度的点
|
||
bool hasGridChange = i == 0 || !gridPath[i].Equals(gridPath[i-1]);
|
||
bool hasSpeedChange = i > 0 && HasSpeedLimitChange(points[i-1], points[i]);
|
||
bool hasHeightChange = i > 0 && Math.Abs(points[i].Position.Z - points[i-1].Position.Z) > 1e-6;
|
||
|
||
bool shouldKeep = hasGridChange || hasSpeedChange || hasHeightChange;
|
||
|
||
if (shouldKeep)
|
||
{
|
||
dedupedGridPath.Add(gridPath[i]);
|
||
dedupedPoints.Add(points[i]);
|
||
}
|
||
}
|
||
LogManager.Info($"[路径优化] 去重完成：{gridPath.Count} -> {dedupedPoints.Count} 个点");
|
||
|
||
// 步骤2：基于去重后的路径进行方向优化
|
||
// 🔥 关键修复：不使用网格坐标，直接使用索引来跟踪保留的点
|
||
// 这样可以避免同一网格坐标的不同高度点被混淆
|
||
var simplifiedIndices = new List<int> { 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)
|
||
);
|
||
|
||
// 简化路径：只保留转弯点、高度变化点和限速变化点
|
||
bool forceKeepNext = false;
|
||
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)
|
||
);
|
||
|
||
// 检查高度变化（2D场景下始终为false，不影响原有逻辑）
|
||
bool hasHeightChange = HasHeightChange(
|
||
dedupedPoints[i-2],
|
||
dedupedPoints[i-1],
|
||
dedupedPoints[i]
|
||
);
|
||
|
||
// 检查限速变化
|
||
bool hasSpeedLimitChange = HasSpeedLimitChange(
|
||
dedupedPoints[i-2],
|
||
dedupedPoints[i-1],
|
||
dedupedPoints[i]
|
||
);
|
||
|
||
// 保留转折点、高度变化点或限速变化点
|
||
bool shouldKeep = !currentDirection.Equals(prevDirection) || hasHeightChange || hasSpeedLimitChange || forceKeepNext;
|
||
|
||
if (shouldKeep)
|
||
{
|
||
simplifiedIndices.Add(i - 1); // 保留索引而不是网格坐标
|
||
prevDirection = currentDirection;
|
||
}
|
||
|
||
// 🔥 如果当前检测到高度变化，下一个点也必须保留
|
||
forceKeepNext = hasHeightChange;
|
||
}
|
||
|
||
// 添加终点索引
|
||
simplifiedIndices.Add(dedupedGridPath.Count - 1);
|
||
|
||
LogManager.Info($"[网格路径优化] 简化完成：{dedupedGridPath.Count} -> {simplifiedIndices.Count} 个点");
|
||
|
||
// 步骤3：根据索引提取PathPoint
|
||
var simplifiedPoints = new List<PathPoint>();
|
||
foreach (int index in simplifiedIndices)
|
||
{
|
||
simplifiedPoints.Add(dedupedPoints[index]);
|
||
}
|
||
|
||
// 打印优化后的路径点详情
|
||
LogManager.Info($"[优化后路径详情] 共 {simplifiedPoints.Count} 个点：");
|
||
for (int i = 0; i < simplifiedPoints.Count; i++)
|
||
{
|
||
var pt = simplifiedPoints[i];
|
||
LogManager.Debug($" [优化后点{i}] 位置=({pt.Position.X:F2}, {pt.Position.Y:F2}, Z={pt.Position.Z:F2}), 限速={pt.SpeedLimit:F1}");
|
||
}
|
||
|
||
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]); // 添加起点
|
||
|
||
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);
|
||
|
||
// 🔥 关键修复：检查限速变化，即使共线也不能移除限速边界点
|
||
bool hasSpeedLimitChange = HasSpeedLimitChange(prevPoint, currPoint, nextPoint);
|
||
|
||
if (!isCollinear || hasSpeedLimitChange)
|
||
{
|
||
// 这是一个转折点或限速边界点，需要保留
|
||
simplified.Add(currPoint);
|
||
|
||
if (hasSpeedLimitChange)
|
||
{
|
||
LogManager.Debug($"[共线简化] 保留限速边界点 {i}: {prevPoint.SpeedLimit} -> {currPoint.SpeedLimit} -> {nextPoint.SpeedLimit}");
|
||
}
|
||
}
|
||
else
|
||
{
|
||
// 这是直线上的冗余点且无限速变化，跳过
|
||
removedCount++;
|
||
}
|
||
}
|
||
|
||
simplified.Add(points[points.Count - 1]); // 添加终点
|
||
|
||
LogManager.Info($"[共线简化] 简化完成，移除了 {removedCount} 个冗余点");
|
||
return simplified;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 检查两个路径点之间是否有显著的高度变化
|
||
/// 用于路径优化时判断是否需要保留中间点
|
||
/// </summary>
|
||
/// <param name="point1">第一个路径点</param>
|
||
/// <param name="point2">第二个路径点</param>
|
||
/// <returns>如果有高度变化返回true，需要保留中间点</returns>
|
||
private bool HasHeightChange(PathPoint point1, PathPoint point2)
|
||
{
|
||
const double heightTolerance = 1e-6; // 仅覆盖浮点数精度误差
|
||
return Math.Abs(point1.Position.Z - point2.Position.Z) > heightTolerance;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 检查三个点之间是否有高度变化（用于判断是否可以跳过中间点）
|
||
/// </summary>
|
||
/// <param name="p1">第一个点的3D坐标</param>
|
||
/// <param name="p2">中间点的3D坐标</param>
|
||
/// <param name="p3">第三个点的3D坐标</param>
|
||
/// <returns>如果有高度变化返回true，不能跳过中间点</returns>
|
||
private bool HasHeightChange(Point3D p1, Point3D p2, Point3D p3)
|
||
{
|
||
const double heightTolerance = 1e-6; // 仅覆盖浮点数精度误差
|
||
|
||
// 检查任意两点间是否有高度变化
|
||
if (Math.Abs(p1.Z - p2.Z) > heightTolerance ||
|
||
Math.Abs(p2.Z - p3.Z) > heightTolerance)
|
||
{
|
||
return true;
|
||
}
|
||
return false;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 检查三个点之间是否有高度变化（PathPoint版本）
|
||
/// </summary>
|
||
/// <param name="point1">第一个路径点</param>
|
||
/// <param name="point2">中间路径点</param>
|
||
/// <param name="point3">第三个路径点</param>
|
||
/// <returns>如果有高度变化返回true，不能跳过中间点</returns>
|
||
private bool HasHeightChange(PathPoint point1, PathPoint point2, PathPoint point3)
|
||
{
|
||
return HasHeightChange(point1.Position, point2.Position, point3.Position);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 检查两个路径点之间是否有限速变化
|
||
/// 用于路径优化时判断是否需要保留中间点
|
||
/// </summary>
|
||
/// <param name="point1">第一个路径点</param>
|
||
/// <param name="point2">第二个路径点</param>
|
||
/// <returns>如果有限速变化返回true，需要保留中间点</returns>
|
||
private bool HasSpeedLimitChange(PathPoint point1, PathPoint point2)
|
||
{
|
||
const double speedTolerance = 1e-6; // 仅覆盖浮点数精度误差
|
||
return Math.Abs(point1.SpeedLimit - point2.SpeedLimit) > speedTolerance;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 检查三个点之间是否有限速变化（用于判断是否可以跳过中间点）
|
||
/// </summary>
|
||
/// <param name="point1">第一个路径点</param>
|
||
/// <param name="point2">中间路径点</param>
|
||
/// <param name="point3">第三个路径点</param>
|
||
/// <returns>如果有限速变化返回true，不能跳过中间点</returns>
|
||
private bool HasSpeedLimitChange(PathPoint point1, PathPoint point2, PathPoint point3)
|
||
{
|
||
const double speedTolerance = 1e-6; // 仅覆盖浮点数精度误差
|
||
|
||
// 检查任意两点间是否有限速变化
|
||
if (Math.Abs(point1.SpeedLimit - point2.SpeedLimit) > speedTolerance ||
|
||
Math.Abs(point2.SpeedLimit - point3.SpeedLimit) > speedTolerance)
|
||
{
|
||
return true;
|
||
}
|
||
return false;
|
||
}
|
||
|
||
/// <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 = COLLINEAR_TOLERANCE_METERS; // 容差值，单位：模型单位
|
||
|
||
// 🔥 修复：使用统一的高度检查函数
|
||
if (HasHeightChange(p1, p2, p3))
|
||
{
|
||
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 = DUPLICATE_POINT_TOLERANCE_METERS; // 用很小的值检查真正的重复点
|
||
bool isSegment1Zero = Math.Abs(dx12) < duplicatePointTolerance && Math.Abs(dy12) < duplicatePointTolerance;
|
||
bool isSegment2Zero = Math.Abs(dx23) < duplicatePointTolerance && Math.Abs(dy23) < duplicatePointTolerance;
|
||
|
||
if (isSegment1Zero || isSegment2Zero)
|
||
{
|
||
// 🔥 在真正重复点时才认为共线
|
||
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)
|
||
{
|
||
// 检查水平方向是否一致（同向或反向都可以）
|
||
return (dx12 * dx23 > 0) || Math.Abs(dx12) < tolerance || Math.Abs(dx23) < tolerance;
|
||
}
|
||
else if (isSegment1Vertical && isSegment2Vertical)
|
||
{
|
||
// 检查垂直方向是否一致（同向或反向都可以）
|
||
return (dy12 * dy23 > 0) || Math.Abs(dy12) < tolerance || Math.Abs(dy23) < tolerance;
|
||
}
|
||
else
|
||
{
|
||
// 🔥 关键：不同类型的线段（一个水平一个垂直，或包含斜线）一律拒绝
|
||
string seg1Type = isSegment1Horizontal ? "水平" : (isSegment1Vertical ? "垂直" : "斜线");
|
||
string seg2Type = isSegment2Horizontal ? "水平" : (isSegment2Vertical ? "垂直" : "斜线");
|
||
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.RecalculateRoute("路径优化");
|
||
|
||
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;
|
||
}
|
||
|
||
#endregion
|
||
}
|
||
} |