542 lines
21 KiB
C#
542 lines
21 KiB
C#
using System;
|
||
using System.Collections.Generic;
|
||
using System.Linq;
|
||
using Autodesk.Navisworks.Api;
|
||
using NavisworksTransport.Utils.CoordinateSystem;
|
||
|
||
namespace NavisworksTransport.PathPlanning
|
||
{
|
||
/// <summary>
|
||
/// 坡度分析器
|
||
/// 负责分析通道的坡度特征和高度变化规律
|
||
/// </summary>
|
||
public class SlopeAnalyzer
|
||
{
|
||
private readonly Dictionary<string, ChannelSlopeInfo> _slopeCache;
|
||
private readonly double _minSlopeAngle; // 最小坡度角度（弧度）
|
||
private readonly ICoordinateSystem _coordinateSystem;
|
||
|
||
/// <summary>
|
||
/// 构造函数
|
||
/// </summary>
|
||
/// <param name="minSlopeAngle">最小坡度角度（度），小于此角度认为是平面</param>
|
||
public SlopeAnalyzer(double minSlopeAngle = 1.0)
|
||
{
|
||
_slopeCache = new Dictionary<string, ChannelSlopeInfo>();
|
||
_minSlopeAngle = minSlopeAngle * Math.PI / 180.0; // 转换为弧度
|
||
_coordinateSystem = CoordinateSystemManager.Instance.Current;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 分析通道类型和坡度特征
|
||
/// </summary>
|
||
/// <param name="channel">通道模型项</param>
|
||
/// <returns>通道坡度信息</returns>
|
||
public ChannelSlopeInfo AnalyzeChannelSlope(ModelItem channel)
|
||
{
|
||
try
|
||
{
|
||
if (channel?.Geometry?.BoundingBox == null)
|
||
{
|
||
LogManager.Warning("[坡度分析] 通道没有有效的几何信息");
|
||
return CreateDefaultSlopeInfo();
|
||
}
|
||
|
||
LogManager.Debug($"[坡度分析] 开始分析通道坡度: {channel.DisplayName}");
|
||
|
||
// 检查缓存
|
||
var cacheKey = channel.InstanceGuid.ToString();
|
||
if (_slopeCache.TryGetValue(cacheKey, out var cachedInfo))
|
||
{
|
||
LogManager.Debug($"[坡度分析] 使用缓存的坡度信息: 角度={cachedInfo.SlopeAngle * 180 / Math.PI:F2}度, 类型={cachedInfo.Type}");
|
||
return cachedInfo;
|
||
}
|
||
|
||
// 分析通道几何特征
|
||
var slopeInfo = AnalyzeGeometrySlope(channel);
|
||
|
||
// 缓存结果
|
||
_slopeCache[cacheKey] = slopeInfo;
|
||
|
||
LogManager.Info($"[坡度分析] 通道坡度分析完成: 角度={slopeInfo.SlopeAngle * 180 / Math.PI:F2}度, 类型={slopeInfo.Type}, 方向=({slopeInfo.SlopeDirection.X:F2}, {slopeInfo.SlopeDirection.Y:F2}, {slopeInfo.SlopeDirection.Z:F2})");
|
||
return slopeInfo;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Error($"[坡度分析] 分析通道坡度时发生错误: {ex.Message}");
|
||
return CreateDefaultSlopeInfo();
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 根据坡度计算精确Z坐标
|
||
/// </summary>
|
||
/// <param name="position">目标位置</param>
|
||
/// <param name="slopeInfo">坡度信息</param>
|
||
/// <param name="baseHeight">基准高度</param>
|
||
/// <returns>精确的Z坐标</returns>
|
||
public double CalculateAccurateZCoordinate(Point3D position, ChannelSlopeInfo slopeInfo, double baseHeight)
|
||
{
|
||
try
|
||
{
|
||
LogManager.Debug($"[坡度分析] 计算位置 ({position.X:F2}, {position.Y:F2}) 的精确Z坐标");
|
||
|
||
// 对于平面通道，直接返回基准高度
|
||
if (slopeInfo.Type == SlopeType.Flat || Math.Abs(slopeInfo.SlopeAngle) < _minSlopeAngle)
|
||
{
|
||
return baseHeight;
|
||
}
|
||
|
||
// 根据坡度类型计算Z坐标
|
||
switch (slopeInfo.Type)
|
||
{
|
||
case SlopeType.Ramp:
|
||
return CalculateRampHeight(position, slopeInfo, baseHeight);
|
||
|
||
case SlopeType.Stairs:
|
||
return CalculateStairsHeight(position, slopeInfo, baseHeight);
|
||
|
||
case SlopeType.CurvedRamp:
|
||
return CalculateCurvedRampHeight(position, slopeInfo, baseHeight);
|
||
|
||
default:
|
||
return CalculateLinearSlopeHeight(position, slopeInfo, baseHeight);
|
||
}
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Warning($"[坡度分析] 计算精确Z坐标时发生错误: {ex.Message}，使用基准高度");
|
||
return baseHeight;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 分析几何体的坡度特征
|
||
/// </summary>
|
||
/// <param name="channel">通道模型项</param>
|
||
/// <returns>坡度信息</returns>
|
||
private ChannelSlopeInfo AnalyzeGeometrySlope(ModelItem channel)
|
||
{
|
||
var bounds = channel.Geometry.BoundingBox;
|
||
var slopeInfo = new ChannelSlopeInfo
|
||
{
|
||
Type = DetermineSlopeType(channel),
|
||
SlopePoints = new List<Point3D>()
|
||
};
|
||
|
||
// 添加边界点作为坡度采样点
|
||
AddBoundaryPoints(slopeInfo, bounds);
|
||
|
||
// 计算坡度角度和方向
|
||
CalculateSlopeAngleAndDirection(slopeInfo, bounds);
|
||
|
||
// 根据通道类型进行特殊处理
|
||
SpecializedSlopeAnalysis(channel, slopeInfo);
|
||
|
||
return slopeInfo;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 确定坡度类型
|
||
/// </summary>
|
||
/// <param name="channel">通道模型项</param>
|
||
/// <returns>坡度类型</returns>
|
||
private SlopeType DetermineSlopeType(ModelItem channel)
|
||
{
|
||
var displayName = channel.DisplayName?.ToLower() ?? "";
|
||
var className = channel.ClassDisplayName?.ToLower() ?? "";
|
||
|
||
if (displayName.Contains("楼梯") || displayName.Contains("stair") ||
|
||
className.Contains("楼梯") || className.Contains("stair"))
|
||
{
|
||
return SlopeType.Stairs;
|
||
}
|
||
|
||
if (displayName.Contains("坡道") || displayName.Contains("ramp") ||
|
||
className.Contains("坡道") || className.Contains("ramp"))
|
||
{
|
||
return SlopeType.Ramp;
|
||
}
|
||
|
||
// 根据几何特征判断
|
||
var bounds = channel.Geometry.BoundingBox;
|
||
var (minElevation, maxElevation) = _coordinateSystem.GetHeightRange(bounds);
|
||
var (minH1, maxH1, minH2, maxH2) = _coordinateSystem.GetHorizontalRange(bounds);
|
||
var heightDiff = maxElevation - minElevation;
|
||
var horizontalLength = Math.Max(maxH1 - minH1, maxH2 - minH2);
|
||
|
||
if (heightDiff > 0.1 && horizontalLength > 0.1) // 有明显高度差
|
||
{
|
||
var slopeRatio = heightDiff / horizontalLength;
|
||
|
||
if (slopeRatio > 0.5) // 坡度大于50%，可能是楼梯
|
||
{
|
||
return SlopeType.Stairs;
|
||
}
|
||
else if (slopeRatio > 0.05) // 坡度大于5%，可能是坡道
|
||
{
|
||
return SlopeType.Ramp;
|
||
}
|
||
}
|
||
|
||
return SlopeType.Flat; // 默认为平面
|
||
}
|
||
|
||
/// <summary>
|
||
/// 添加边界点
|
||
/// </summary>
|
||
/// <param name="slopeInfo">坡度信息</param>
|
||
/// <param name="bounds">边界框</param>
|
||
private void AddBoundaryPoints(ChannelSlopeInfo slopeInfo, BoundingBox3D bounds)
|
||
{
|
||
// 添加8个边界框顶点
|
||
var (minElevation, maxElevation) = _coordinateSystem.GetHeightRange(bounds);
|
||
var (minH1, maxH1, minH2, maxH2) = _coordinateSystem.GetHorizontalRange(bounds);
|
||
slopeInfo.SlopePoints.Add(_coordinateSystem.CreatePoint(minH1, minH2, minElevation));
|
||
slopeInfo.SlopePoints.Add(_coordinateSystem.CreatePoint(maxH1, minH2, minElevation));
|
||
slopeInfo.SlopePoints.Add(_coordinateSystem.CreatePoint(minH1, maxH2, minElevation));
|
||
slopeInfo.SlopePoints.Add(_coordinateSystem.CreatePoint(maxH1, maxH2, minElevation));
|
||
slopeInfo.SlopePoints.Add(_coordinateSystem.CreatePoint(minH1, minH2, maxElevation));
|
||
slopeInfo.SlopePoints.Add(_coordinateSystem.CreatePoint(maxH1, minH2, maxElevation));
|
||
slopeInfo.SlopePoints.Add(_coordinateSystem.CreatePoint(minH1, maxH2, maxElevation));
|
||
slopeInfo.SlopePoints.Add(_coordinateSystem.CreatePoint(maxH1, maxH2, maxElevation));
|
||
|
||
// 添加中心线上的采样点
|
||
int sampleCount = 5;
|
||
for (int i = 0; i < sampleCount; i++)
|
||
{
|
||
double ratio = (double)i / (sampleCount - 1);
|
||
var samplePoint = _coordinateSystem.CreatePoint(
|
||
minH1 + (maxH1 - minH1) * ratio,
|
||
minH2 + (maxH2 - minH2) * ratio,
|
||
minElevation + (maxElevation - minElevation) * ratio);
|
||
slopeInfo.SlopePoints.Add(samplePoint);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 计算坡度角度和方向
|
||
/// </summary>
|
||
/// <param name="slopeInfo">坡度信息</param>
|
||
/// <param name="bounds">边界框</param>
|
||
private void CalculateSlopeAngleAndDirection(ChannelSlopeInfo slopeInfo, BoundingBox3D bounds)
|
||
{
|
||
// 计算主要方向向量
|
||
var (minElevation, maxElevation) = _coordinateSystem.GetHeightRange(bounds);
|
||
var (minH1, maxH1, minH2, maxH2) = _coordinateSystem.GetHorizontalRange(bounds);
|
||
var directionH1 = maxH1 - minH1;
|
||
var directionH2 = maxH2 - minH2;
|
||
var directionElevation = maxElevation - minElevation;
|
||
|
||
// 选择主要的水平方向
|
||
var horizontalLength = Math.Max(Math.Abs(directionH1), Math.Abs(directionH2));
|
||
|
||
if (horizontalLength > 0.001)
|
||
{
|
||
// 计算坡度角度
|
||
slopeInfo.SlopeAngle = Math.Atan(Math.Abs(directionElevation) / horizontalLength);
|
||
|
||
// 计算坡度方向向量
|
||
var directionPoint = _coordinateSystem.CreatePoint(directionH1, directionH2, directionElevation);
|
||
var length = Math.Sqrt(directionPoint.X * directionPoint.X + directionPoint.Y * directionPoint.Y + directionPoint.Z * directionPoint.Z);
|
||
if (length > 0.001)
|
||
{
|
||
slopeInfo.SlopeDirection = new Vector3D(
|
||
directionPoint.X / length,
|
||
directionPoint.Y / length,
|
||
directionPoint.Z / length
|
||
);
|
||
}
|
||
else
|
||
{
|
||
slopeInfo.SlopeDirection = new Vector3D(0, 0, 1); // 默认向上
|
||
}
|
||
}
|
||
else
|
||
{
|
||
slopeInfo.SlopeAngle = 0;
|
||
var up = _coordinateSystem.UpVector;
|
||
slopeInfo.SlopeDirection = new Vector3D(up.X, up.Y, up.Z);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 特殊坡度分析
|
||
/// </summary>
|
||
/// <param name="channel">通道模型项</param>
|
||
/// <param name="slopeInfo">坡度信息</param>
|
||
private void SpecializedSlopeAnalysis(ModelItem channel, ChannelSlopeInfo slopeInfo)
|
||
{
|
||
switch (slopeInfo.Type)
|
||
{
|
||
case SlopeType.Stairs:
|
||
AnalyzeStairsDetails(channel, slopeInfo);
|
||
break;
|
||
|
||
case SlopeType.Ramp:
|
||
AnalyzeRampDetails(channel, slopeInfo);
|
||
break;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 分析楼梯详细信息
|
||
/// </summary>
|
||
/// <param name="channel">通道模型项</param>
|
||
/// <param name="slopeInfo">坡度信息</param>
|
||
private void AnalyzeStairsDetails(ModelItem channel, ChannelSlopeInfo slopeInfo)
|
||
{
|
||
try
|
||
{
|
||
var bounds = channel.Geometry.BoundingBox;
|
||
var (minElevation, maxElevation) = _coordinateSystem.GetHeightRange(bounds);
|
||
var (minH1, maxH1, minH2, maxH2) = _coordinateSystem.GetHorizontalRange(bounds);
|
||
var totalHeight = maxElevation - minElevation;
|
||
var totalLength = Math.Max(maxH1 - minH1, maxH2 - minH2);
|
||
|
||
// 估算楼梯级数（假设每级高度15-20cm）
|
||
var estimatedSteps = (int)(totalHeight / 0.175); // 平均17.5cm每级
|
||
if (estimatedSteps < 1) estimatedSteps = 1;
|
||
|
||
slopeInfo.StepCount = estimatedSteps;
|
||
slopeInfo.StepHeight = totalHeight / estimatedSteps;
|
||
slopeInfo.StepDepth = totalLength / estimatedSteps;
|
||
|
||
LogManager.Debug($"[坡度分析] 楼梯详细信息: 级数={estimatedSteps}, 每级高度={slopeInfo.StepHeight:F3}m, 每级深度={slopeInfo.StepDepth:F3}m");
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Warning($"[坡度分析] 分析楼梯详细信息时发生错误: {ex.Message}");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 分析坡道详细信息
|
||
/// </summary>
|
||
/// <param name="channel">通道模型项</param>
|
||
/// <param name="slopeInfo">坡度信息</param>
|
||
private void AnalyzeRampDetails(ModelItem channel, ChannelSlopeInfo slopeInfo)
|
||
{
|
||
try
|
||
{
|
||
var bounds = channel.Geometry.BoundingBox;
|
||
|
||
// 检查是否为弯曲坡道（通过长宽比判断）
|
||
var (minH1, maxH1, minH2, maxH2) = _coordinateSystem.GetHorizontalRange(bounds);
|
||
var width = Math.Min(maxH1 - minH1, maxH2 - minH2);
|
||
var length = Math.Max(maxH1 - minH1, maxH2 - minH2);
|
||
|
||
if (width > 0 && length / width > 3.0) // 长宽比大于3:1，可能是弯曲坡道
|
||
{
|
||
slopeInfo.Type = SlopeType.CurvedRamp;
|
||
LogManager.Debug($"[坡度分析] 检测到弯曲坡道: 长度={length:F2}m, 宽度={width:F2}m");
|
||
}
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
LogManager.Warning($"[坡度分析] 分析坡道详细信息时发生错误: {ex.Message}");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 计算坡道高度
|
||
/// </summary>
|
||
/// <param name="position">位置点</param>
|
||
/// <param name="slopeInfo">坡度信息</param>
|
||
/// <param name="baseHeight">基准高度</param>
|
||
/// <returns>计算得到的高度</returns>
|
||
private double CalculateRampHeight(Point3D position, ChannelSlopeInfo slopeInfo, double baseHeight)
|
||
{
|
||
return CalculateLinearSlopeHeight(position, slopeInfo, baseHeight);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 计算楼梯高度
|
||
/// </summary>
|
||
/// <param name="position">位置点</param>
|
||
/// <param name="slopeInfo">坡度信息</param>
|
||
/// <param name="baseHeight">基准高度</param>
|
||
/// <returns>计算得到的高度</returns>
|
||
private double CalculateStairsHeight(Point3D position, ChannelSlopeInfo slopeInfo, double baseHeight)
|
||
{
|
||
if (slopeInfo.StepCount <= 0 || slopeInfo.StepHeight <= 0)
|
||
{
|
||
return CalculateLinearSlopeHeight(position, slopeInfo, baseHeight);
|
||
}
|
||
|
||
// 找到楼梯的起点和终点
|
||
var startPoint = slopeInfo.SlopePoints.OrderBy(p => _coordinateSystem.GetElevation(p)).First();
|
||
var endPoint = slopeInfo.SlopePoints.OrderBy(p => _coordinateSystem.GetElevation(p)).Last();
|
||
|
||
// 计算位置在楼梯长度方向上的投影
|
||
var (startH1, startH2) = _coordinateSystem.GetHorizontalCoords(startPoint);
|
||
var (endH1, endH2) = _coordinateSystem.GetHorizontalCoords(endPoint);
|
||
var (positionH1, positionH2) = _coordinateSystem.GetHorizontalCoords(position);
|
||
var totalDistance = Math.Sqrt(
|
||
Math.Pow(endH1 - startH1, 2) +
|
||
Math.Pow(endH2 - startH2, 2)
|
||
);
|
||
|
||
var currentDistance = Math.Sqrt(
|
||
Math.Pow(positionH1 - startH1, 2) +
|
||
Math.Pow(positionH2 - startH2, 2)
|
||
);
|
||
|
||
if (totalDistance <= 0.001)
|
||
{
|
||
return baseHeight;
|
||
}
|
||
|
||
// 计算当前位置对应的楼梯级数
|
||
var ratio = currentDistance / totalDistance;
|
||
var currentStep = (int)(ratio * slopeInfo.StepCount);
|
||
|
||
// 楼梯的高度是阶梯状的
|
||
return baseHeight + currentStep * slopeInfo.StepHeight;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 计算弯曲坡道高度
|
||
/// </summary>
|
||
/// <param name="position">位置点</param>
|
||
/// <param name="slopeInfo">坡度信息</param>
|
||
/// <param name="baseHeight">基准高度</param>
|
||
/// <returns>计算得到的高度</returns>
|
||
private double CalculateCurvedRampHeight(Point3D position, ChannelSlopeInfo slopeInfo, double baseHeight)
|
||
{
|
||
// 弯曲坡道使用样条插值，这里先简化为线性插值
|
||
return CalculateLinearSlopeHeight(position, slopeInfo, baseHeight);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 计算线性坡度高度
|
||
/// </summary>
|
||
/// <param name="position">位置点</param>
|
||
/// <param name="slopeInfo">坡度信息</param>
|
||
/// <param name="baseHeight">基准高度</param>
|
||
/// <returns>计算得到的高度</returns>
|
||
private double CalculateLinearSlopeHeight(Point3D position, ChannelSlopeInfo slopeInfo, double baseHeight)
|
||
{
|
||
if (Math.Abs(slopeInfo.SlopeAngle) < _minSlopeAngle)
|
||
{
|
||
return baseHeight;
|
||
}
|
||
|
||
// 计算沿坡度方向的距离
|
||
var projectedDistance = position.X * slopeInfo.SlopeDirection.X +
|
||
position.Y * slopeInfo.SlopeDirection.Y;
|
||
|
||
// 根据坡度角度计算高度增量
|
||
var heightIncrement = projectedDistance * Math.Tan(slopeInfo.SlopeAngle);
|
||
|
||
return baseHeight + heightIncrement;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 创建默认坡度信息
|
||
/// </summary>
|
||
/// <returns>默认坡度信息</returns>
|
||
private ChannelSlopeInfo CreateDefaultSlopeInfo()
|
||
{
|
||
return new ChannelSlopeInfo
|
||
{
|
||
Type = SlopeType.Flat,
|
||
SlopeAngle = 0,
|
||
SlopeDirection = new Vector3D(0, 0, 1),
|
||
SlopePoints = new List<Point3D>()
|
||
};
|
||
}
|
||
|
||
/// <summary>
|
||
/// 清除坡度缓存
|
||
/// </summary>
|
||
public void ClearCache()
|
||
{
|
||
_slopeCache.Clear();
|
||
LogManager.Info("[坡度分析] 坡度缓存已清除");
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取缓存统计信息
|
||
/// </summary>
|
||
/// <returns>缓存项数量</returns>
|
||
public int GetCacheCount()
|
||
{
|
||
return _slopeCache.Count;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 通道坡度信息
|
||
/// </summary>
|
||
public class ChannelSlopeInfo
|
||
{
|
||
/// <summary>
|
||
/// 坡度角度（弧度）
|
||
/// </summary>
|
||
public double SlopeAngle { get; set; }
|
||
|
||
/// <summary>
|
||
/// 坡度类型
|
||
/// </summary>
|
||
public SlopeType Type { get; set; }
|
||
|
||
/// <summary>
|
||
/// 坡度方向向量（单位向量）
|
||
/// </summary>
|
||
public Vector3D SlopeDirection { get; set; }
|
||
|
||
/// <summary>
|
||
/// 坡度特征点集合
|
||
/// </summary>
|
||
public List<Point3D> SlopePoints { get; set; } = new List<Point3D>();
|
||
|
||
/// <summary>
|
||
/// 楼梯级数（仅对楼梯有效）
|
||
/// </summary>
|
||
public int StepCount { get; set; }
|
||
|
||
/// <summary>
|
||
/// 每级楼梯高度（仅对楼梯有效）
|
||
/// </summary>
|
||
public double StepHeight { get; set; }
|
||
|
||
/// <summary>
|
||
/// 每级楼梯深度（仅对楼梯有效）
|
||
/// </summary>
|
||
public double StepDepth { get; set; }
|
||
}
|
||
|
||
/// <summary>
|
||
/// 坡度类型枚举
|
||
/// </summary>
|
||
public enum SlopeType
|
||
{
|
||
/// <summary>
|
||
/// 平面
|
||
/// </summary>
|
||
Flat,
|
||
|
||
/// <summary>
|
||
/// 直线坡道
|
||
/// </summary>
|
||
Ramp,
|
||
|
||
/// <summary>
|
||
/// 楼梯
|
||
/// </summary>
|
||
Stairs,
|
||
|
||
/// <summary>
|
||
/// 弯曲坡道
|
||
/// </summary>
|
||
CurvedRamp,
|
||
|
||
/// <summary>
|
||
/// 其他类型
|
||
/// </summary>
|
||
Other
|
||
}
|
||
|
||
}
|