采用obb包围盒计算空轨基准线
This commit is contained in:
parent
708bf533f1
commit
9ae4acdb03
@ -246,48 +246,11 @@ namespace NavisworksTransport.Core.Animation
|
||||
|
||||
/// <summary>
|
||||
/// 仅仅将物理模型同步到路径起点(不清理数据,不重置状态)
|
||||
/// 用于加载碰撞检测结果时
|
||||
/// </summary>
|
||||
public void SyncToPathStart(ModelItem item, List<Point3D> points)
|
||||
{
|
||||
try
|
||||
{
|
||||
if (item == null || points == null || points.Count == 0) return;
|
||||
|
||||
// 更新引用,确保后续 MoveVehicleToPathStart 使用正确物体
|
||||
_animatedObject = item;
|
||||
_pathPoints = new List<Point3D>(points);
|
||||
|
||||
// 记录原始位置(用于归位)
|
||||
_originalTransform = item.Transform;
|
||||
_currentYaw = ModelItemTransformHelper.GetYawFromTransform(_originalTransform);
|
||||
_originalCenter = item.BoundingBox().Center;
|
||||
_currentPosition = new Point3D(_originalCenter.X, _originalCenter.Y, item.BoundingBox().Min.Z);
|
||||
|
||||
if (points.Count >= 2)
|
||||
{
|
||||
double yaw = Math.Atan2(points[1].Y - points[0].Y, points[1].X - points[0].X);
|
||||
|
||||
// 🔥 根据路径类型调整起点位置
|
||||
Point3D startPosition = points[0];
|
||||
if (_route?.PathType == PathType.Rail)
|
||||
{
|
||||
double vehicleHeight = item.BoundingBox().Max.Z - item.BoundingBox().Min.Z;
|
||||
startPosition = new Point3D(startPosition.X, startPosition.Y, startPosition.Z - vehicleHeight);
|
||||
}
|
||||
|
||||
UpdateObjectPosition(startPosition, yaw);
|
||||
}
|
||||
else
|
||||
{
|
||||
UpdateObjectPosition(points[0]);
|
||||
}
|
||||
|
||||
LogManager.Debug($"[Sync] 物体 {item.DisplayName} 已移动到起点");
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
LogManager.Error($"[Sync] 同步到起点失败: {ex.Message}");
|
||||
}
|
||||
MoveVehicleToPathStart(item, points);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@ -408,28 +371,67 @@ namespace NavisworksTransport.Core.Animation
|
||||
/// <summary>
|
||||
/// 将车辆移动到路径起点
|
||||
/// </summary>
|
||||
private void MoveVehicleToPathStart()
|
||||
/// <param name="animatedObject">动画对象(可选,如果不提供则使用当前的_animatedObject)</param>
|
||||
/// <param name="pathPoints">路径点(可选,如果不提供则使用当前的_pathPoints)</param>
|
||||
public void MoveVehicleToPathStart(ModelItem animatedObject = null, List<Point3D> pathPoints = null)
|
||||
{
|
||||
try
|
||||
{
|
||||
if (_pathPoints.Count == 0 || _animationFrames == null || _animationFrames.Count == 0) return;
|
||||
|
||||
var firstFrame = _animationFrames[0];
|
||||
|
||||
// 🔥 根据路径类型调整起点位置
|
||||
Point3D startPosition = firstFrame.Position;
|
||||
if (_route.PathType == PathType.Rail)
|
||||
// 如果提供了参数,更新内部状态
|
||||
if (animatedObject != null)
|
||||
{
|
||||
// 空轨路径:路径点是空轨下表面,车辆顶面应该在这里
|
||||
_animatedObject = animatedObject;
|
||||
_originalTransform = animatedObject.Transform;
|
||||
_originalCenter = animatedObject.BoundingBox().Center;
|
||||
_currentPosition = new Point3D(_originalCenter.X, _originalCenter.Y, animatedObject.BoundingBox().Min.Z);
|
||||
|
||||
// 对于虚拟车辆,重置 _currentYaw 为 0,因为虚拟车辆刚创建时已经被重置为单位变换
|
||||
// 对于普通物体,保持原始朝向
|
||||
if (_isVirtualVehicle)
|
||||
{
|
||||
_currentYaw = 0.0;
|
||||
}
|
||||
else
|
||||
{
|
||||
_currentYaw = ModelItemTransformHelper.GetYawFromTransform(_originalTransform);
|
||||
}
|
||||
}
|
||||
|
||||
if (pathPoints != null)
|
||||
{
|
||||
_pathPoints = pathPoints;
|
||||
}
|
||||
|
||||
// 检查路径点
|
||||
if (_pathPoints == null || _pathPoints.Count < 2)
|
||||
{
|
||||
LogManager.Warning("[移动到起点] 没有可用的路径点");
|
||||
return;
|
||||
}
|
||||
|
||||
// 计算朝向(使用前两个路径点的方向)
|
||||
double yaw = Math.Atan2(_pathPoints[1].Y - _pathPoints[0].Y, _pathPoints[1].X - _pathPoints[0].X);
|
||||
|
||||
// 根据路径类型调整起点位置
|
||||
Point3D startPosition = _pathPoints[0];
|
||||
if (_route?.PathType == PathType.Rail)
|
||||
{
|
||||
// 空轨路径:points[0] 是空轨下表面位置,车辆顶面应该在这里
|
||||
// 所以车辆底面 = points[0] - 车辆高度
|
||||
double vehicleHeight = _animatedObject.BoundingBox().Max.Z - _animatedObject.BoundingBox().Min.Z;
|
||||
startPosition = new Point3D(startPosition.X, startPosition.Y, startPosition.Z - vehicleHeight);
|
||||
LogManager.Debug($"[移动到起点] 空轨路径调整: 空轨Z={_pathPoints[0].Z:F2}, 车辆底面Z={startPosition.Z:F2}, 车辆高度={vehicleHeight:F2}");
|
||||
}
|
||||
else
|
||||
{
|
||||
// 地面路径:points[0] 是地面位置,车辆底面应该在这里
|
||||
LogManager.Debug($"[移动到起点] 地面路径: pos=({startPosition.X:F2},{startPosition.Y:F2},{startPosition.Z:F2})");
|
||||
}
|
||||
|
||||
// 使用 UpdateObjectPosition 统一处理移动和旋转
|
||||
// 这将物体从当前位置(底面中心)移动到第一帧的位置,并旋转到第一帧的朝向
|
||||
UpdateObjectPosition(startPosition, firstFrame.YawRadians);
|
||||
UpdateObjectPosition(startPosition, yaw);
|
||||
|
||||
LogManager.Info($"物体已初始化到路径起点并对齐朝向: pos=({startPosition.X:F2},{startPosition.Y:F2},{startPosition.Z:F2}), yaw={firstFrame.YawRadians:F3}rad");
|
||||
LogManager.Info($"物体已初始化到路径起点并对齐朝向: pos=({startPosition.X:F2},{startPosition.Y:F2},{startPosition.Z:F2}), yaw={yaw:F3}rad, 路径类型={(_route?.PathType == PathType.Rail ? "空轨" : "地面")}");
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
|
||||
@ -347,7 +347,7 @@ namespace NavisworksTransport
|
||||
|
||||
// 使用 PathAnimationManager 将车辆移动到起点
|
||||
var pathAnimationManager = Core.Animation.PathAnimationManager.GetInstance();
|
||||
pathAnimationManager.SyncToPathStart(vehicleObject, new List<Point3D> { startPointPosition, startPointPosition });
|
||||
pathAnimationManager.MoveVehicleToPathStart(vehicleObject, new List<Point3D> { startPointPosition, startPointPosition });
|
||||
LogManager.Info($"[LoadClashDetectiveResultsFromDatabase] 虚拟车辆已移动到路径起点: ({startPointPosition.X:F2}, {startPointPosition.Y:F2}, {startPointPosition.Z:F2})");
|
||||
}
|
||||
}
|
||||
|
||||
@ -1089,25 +1089,11 @@ namespace NavisworksTransport
|
||||
BuildControlLines(visualization, sortedPoints);
|
||||
|
||||
// 地面路径使用曲线化后的路径(Edges)
|
||||
// 空轨路径直接使用控制点连线作为路径连线
|
||||
// 空轨路径只显示控制点连线,不显示路径连线
|
||||
if (visualization.PathRoute.PathType == NavisworksTransport.PathType.Rail)
|
||||
{
|
||||
// 空轨路径:将控制点连线复制到路径连线,使用不透明样式
|
||||
foreach (var controlLine in visualization.ControlLineMarkers)
|
||||
{
|
||||
var pathLineMarker = new LineMarker
|
||||
{
|
||||
StartPoint = controlLine.StartPoint,
|
||||
EndPoint = controlLine.EndPoint,
|
||||
Color = GetRenderStyle(RenderStyleName.Line).Color,
|
||||
Radius = GetLineRadius(),
|
||||
SegmentType = PathSegmentType.Straight,
|
||||
FromIndex = controlLine.FromIndex,
|
||||
ToIndex = controlLine.ToIndex
|
||||
};
|
||||
visualization.PathLineMarkers.Add(pathLineMarker);
|
||||
}
|
||||
LogManager.Debug($"[路径渲染] 空轨路径使用控制点连线,共 {visualization.PathLineMarkers.Count} 条");
|
||||
// 空轨路径:只显示控制点连线,不构建路径连线
|
||||
LogManager.Debug($"[路径渲染] 空轨路径只显示控制点连线,不显示路径连线");
|
||||
}
|
||||
else if (visualization.PathRoute.Edges != null && visualization.PathRoute.Edges.Count > 0)
|
||||
{
|
||||
|
||||
@ -7,6 +7,64 @@ using NavisworksTransport.Core;
|
||||
|
||||
namespace NavisworksTransport.PathPlanning
|
||||
{
|
||||
/// <summary>
|
||||
/// 定向包围盒(OBB)
|
||||
/// </summary>
|
||||
public class OrientedBoundingBox
|
||||
{
|
||||
/// <summary>
|
||||
/// 中心点
|
||||
/// </summary>
|
||||
public Point3D Center { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 主轴(3个正交向量)
|
||||
/// </summary>
|
||||
public Point3D[] Axes { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 半长(沿各轴的半长度)
|
||||
/// </summary>
|
||||
public Point3D Extents { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 投影范围的最小 Z 值(在主轴坐标系中)
|
||||
/// </summary>
|
||||
public double ProjectedMinZ { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 投影范围的最大 Z 值(在主轴坐标系中)
|
||||
/// </summary>
|
||||
public double ProjectedMaxZ { get; set; }
|
||||
/// <summary>
|
||||
/// 获取指定索引的轴
|
||||
/// </summary>
|
||||
public Point3D GetAxis(int index)
|
||||
{
|
||||
if (index < 0 || index >= Axes.Length)
|
||||
throw new ArgumentOutOfRangeException(nameof(index));
|
||||
return Axes[index];
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取指定轴的长度
|
||||
/// </summary>
|
||||
public double GetAxisLength(int index)
|
||||
{
|
||||
if (index < 0 || index >= Axes.Length)
|
||||
throw new ArgumentOutOfRangeException(nameof(index));
|
||||
|
||||
if (index == 0)
|
||||
return 2 * Extents.X;
|
||||
else if (index == 1)
|
||||
return 2 * Extents.Y;
|
||||
else if (index == 2)
|
||||
return 2 * Extents.Z;
|
||||
else
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 空轨几何体辅助工具
|
||||
/// 负责从空轨几何体提取下表面中心线,支持拐弯和倾斜
|
||||
@ -58,11 +116,6 @@ namespace NavisworksTransport.PathPlanning
|
||||
/// </summary>
|
||||
public double TotalLength { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 包围盒
|
||||
/// </summary>
|
||||
public BoundingBox3D Bounds { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 起点
|
||||
/// </summary>
|
||||
@ -132,32 +185,18 @@ namespace NavisworksTransport.PathPlanning
|
||||
}
|
||||
}
|
||||
|
||||
// 提取下表面信息
|
||||
var surfaceInfo = ExtractRailBottomSurface(railModel);
|
||||
if (surfaceInfo == null || surfaceInfo.BottomTriangles.Count == 0)
|
||||
{
|
||||
throw new InvalidOperationException($"无法从空轨模型提取下表面三角形: {railModel.DisplayName}");
|
||||
}
|
||||
|
||||
LogManager.Info($"[空轨] 提取到 {surfaceInfo.BottomTriangles.Count} 个下表面三角形");
|
||||
|
||||
// 提取骨架线
|
||||
var skeleton = ExtractSkeletonFromTriangles(surfaceInfo.BottomTriangles);
|
||||
LogManager.Info($"[空轨] 提取到 {skeleton.Count} 个骨架点");
|
||||
|
||||
// 沿骨架线生成采样点(完整路径)
|
||||
var pathPoints = GenerateSamplePointsAlongSkeleton(skeleton, samplingInterval);
|
||||
// 使用 OBB 方法提取基准路径
|
||||
var pathPoints = ExtractRailBottomCenterLineByOBB(railModel, samplingInterval);
|
||||
LogManager.Info($"[空轨] 生成 {pathPoints.Count} 个采样点");
|
||||
|
||||
// 计算路径长度
|
||||
double totalLength = CalculatePathLength(pathPoints);
|
||||
|
||||
|
||||
// 创建基准路径对象
|
||||
var baselinePath = new RailBaselinePath
|
||||
{
|
||||
PathPoints = pathPoints,
|
||||
TotalLength = totalLength,
|
||||
Bounds = surfaceInfo.Bounds,
|
||||
StartPoint = pathPoints.First(),
|
||||
EndPoint = pathPoints.Last(),
|
||||
RailModel = railModel,
|
||||
@ -420,6 +459,330 @@ namespace NavisworksTransport.PathPlanning
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 兼容方法:从空轨几何体提取下表面中心线(一步完成)
|
||||
/// </summary>
|
||||
/// <param name="railModel">空轨模型项</param>
|
||||
/// <param name="startPoint">起点(3D坐标)</param>
|
||||
/// <param name="endPoint">终点(3D坐标)</param>
|
||||
/// <param name="samplingInterval">采样间隔(模型单位),默认0.5</param>
|
||||
/// <returns>路径点列表(包含精确的Z坐标)</returns>
|
||||
|
||||
/// <summary>
|
||||
/// 使用 OBB 包围体提取空轨下表面中心线(新方法,支持倾斜)
|
||||
/// </summary>
|
||||
/// <param name="railModel">空轨模型项</param>
|
||||
/// <param name="samplingInterval">采样间隔(模型单位),默认0.5</param>
|
||||
/// <returns>路径点列表(下表面中心线)</returns>
|
||||
public static List<Point3D> ExtractRailBottomCenterLineByOBB(
|
||||
ModelItem railModel,
|
||||
double samplingInterval = 0.5)
|
||||
{
|
||||
try
|
||||
{
|
||||
LogManager.Info($"[空轨] 使用 OBB 方法提取下表面中心线: {railModel.DisplayName}");
|
||||
LogManager.Info($"[空轨] 采样间隔: {samplingInterval:F2}");
|
||||
|
||||
// 1. 计算 OBB 包围体
|
||||
var obb = ComputeOrientedBoundingBox(railModel);
|
||||
|
||||
// 2. 获取主轴(最长边,通常是空轨的长度方向)
|
||||
var principalAxis = obb.GetAxis(0);
|
||||
var axisLength = obb.GetAxisLength(0);
|
||||
var principalAxisVector = principalAxis.ToVector3D();
|
||||
|
||||
LogManager.Info($"[空轨] OBB 主轴长度: {axisLength:F2}(模型单位)");
|
||||
LogManager.Info($"[空轨] 主轴方向: ({principalAxisVector.X:F3}, {principalAxisVector.Y:F3}, {principalAxisVector.Z:F3})");
|
||||
|
||||
// 3. 计算中心线的起点和终点(下表面)
|
||||
var center = obb.Center;
|
||||
var direction = principalAxis.ToVector3D();
|
||||
|
||||
// 找到最接近"向下"方向的轴(Z 轴负方向)
|
||||
var downAxisIndex = 1; // 默认使用 Axis 1(次长轴)
|
||||
var minZComponent = 1.0; // Z 分量的最小值
|
||||
|
||||
for (int i = 1; i < 3; i++) // 检查 Axis 1 和 Axis 2
|
||||
{
|
||||
var axis = obb.GetAxis(i);
|
||||
var axisVector = axis.ToVector3D();
|
||||
var zComponent = axisVector.Z;
|
||||
|
||||
LogManager.Info($"[空轨] Axis {i} Z分量: {zComponent:F3}");
|
||||
|
||||
// 找到 Z 分量最小的轴(最向下,即最负)
|
||||
if (zComponent < minZComponent)
|
||||
{
|
||||
minZComponent = zComponent;
|
||||
downAxisIndex = i;
|
||||
}
|
||||
}
|
||||
|
||||
var downAxis = obb.GetAxis(downAxisIndex);
|
||||
var downAxisVector = downAxis.ToVector3D();
|
||||
|
||||
LogManager.Info($"[空轨] 下表面轴索引: {downAxisIndex}, 方向: ({downAxisVector.X:F3}, {downAxisVector.Y:F3}, {downAxisVector.Z:F3})");
|
||||
|
||||
// 计算下表面偏移(使用投影范围的 minZ)
|
||||
var downOffsetZ = obb.ProjectedMinZ;
|
||||
|
||||
LogManager.Info($"[空轨] 下表面偏移(投影 minZ): {downOffsetZ:F3}");
|
||||
|
||||
// 在主轴坐标系中,起点和终点的下表面位置
|
||||
// 起点:Axis0 = minX, Axis1 = 0, Axis2 = minZ
|
||||
// 终点:Axis0 = maxX, Axis1 = 0, Axis2 = minZ
|
||||
var axis0Min = -obb.GetAxisLength(0) / 2;
|
||||
var axis0Max = obb.GetAxisLength(0) / 2;
|
||||
|
||||
// 将主轴坐标系中的下表面点转换回世界坐标系
|
||||
var obbAxes = obb.Axes;
|
||||
|
||||
var startProjected = new Point3D(
|
||||
axis0Min,
|
||||
0,
|
||||
downOffsetZ
|
||||
);
|
||||
|
||||
var endProjected = new Point3D(
|
||||
axis0Max,
|
||||
0,
|
||||
downOffsetZ
|
||||
);
|
||||
|
||||
LogManager.Info($"[空轨] 主轴坐标系起点: ({startProjected.X:F2}, {startProjected.Y:F2}, {startProjected.Z:F2})");
|
||||
LogManager.Info($"[空轨] 主轴坐标系终点: ({endProjected.X:F2}, {endProjected.Y:F2}, {endProjected.Z:F2})");
|
||||
|
||||
var startCenter = new Point3D(
|
||||
center.X + startProjected.X * obbAxes[0].X + startProjected.Y * obbAxes[1].X + startProjected.Z * obbAxes[2].X,
|
||||
center.Y + startProjected.X * obbAxes[0].Y + startProjected.Y * obbAxes[1].Y + startProjected.Z * obbAxes[2].Y,
|
||||
center.Z + startProjected.X * obbAxes[0].Z + startProjected.Y * obbAxes[1].Z + startProjected.Z * obbAxes[2].Z
|
||||
);
|
||||
|
||||
var endCenter = new Point3D(
|
||||
center.X + endProjected.X * obbAxes[0].X + endProjected.Y * obbAxes[1].X + endProjected.Z * obbAxes[2].X,
|
||||
center.Y + endProjected.X * obbAxes[0].Y + endProjected.Y * obbAxes[1].Y + endProjected.Z * obbAxes[2].Y,
|
||||
center.Z + endProjected.X * obbAxes[0].Z + endProjected.Y * obbAxes[1].Z + endProjected.Z * obbAxes[2].Z
|
||||
);
|
||||
|
||||
LogManager.Info($"[空轨] 中心线起点: ({startCenter.X:F2}, {startCenter.Y:F2}, {startCenter.Z:F2})");
|
||||
LogManager.Info($"[空轨] 中心线终点: ({endCenter.X:F2}, {endCenter.Y:F2}, {endCenter.Z:F2})");
|
||||
|
||||
// 4. 沿中心线采样
|
||||
var baseline = SampleLine(startCenter, endCenter, samplingInterval);
|
||||
|
||||
LogManager.Info($"[空轨] OBB 方法完成,生成 {baseline.Count} 个采样点");
|
||||
|
||||
return baseline;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
LogManager.Error($"[空轨] OBB 方法提取下表面中心线失败: {ex.Message}");
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 计算 OBB 包围体(使用 PCA 方法)
|
||||
/// </summary>
|
||||
private static OrientedBoundingBox ComputeOrientedBoundingBox(ModelItem modelItem)
|
||||
{
|
||||
try
|
||||
{
|
||||
// 输出 ModelItem 的 AABB 包围盒信息
|
||||
var aabb = modelItem.BoundingBox();
|
||||
LogManager.Info($"[空轨] AABB包围盒: Min({aabb.Min.X:F2}, {aabb.Min.Y:F2}, {aabb.Min.Z:F2}), Max({aabb.Max.X:F2}, {aabb.Max.Y:F2}, {aabb.Max.Z:F2})");
|
||||
LogManager.Info($"[空轨] AABB尺寸: X={aabb.Max.X - aabb.Min.X:F2}, Y={aabb.Max.Y - aabb.Min.Y:F2}, Z={aabb.Max.Z - aabb.Min.Z:F2}");
|
||||
|
||||
// 使用 GeometryHelper 提取三角形
|
||||
var triangles = GeometryHelper.ExtractTriangles(new[] { modelItem });
|
||||
|
||||
// 从三角形中提取所有顶点(去重)
|
||||
var pointSet = new HashSet<Point3D>();
|
||||
foreach (var triangle in triangles)
|
||||
{
|
||||
pointSet.Add(triangle.Point1);
|
||||
pointSet.Add(triangle.Point2);
|
||||
pointSet.Add(triangle.Point3);
|
||||
}
|
||||
|
||||
var points = pointSet.ToList();
|
||||
|
||||
if (points.Count == 0)
|
||||
{
|
||||
throw new InvalidOperationException($"模型 {modelItem.DisplayName} 没有几何体顶点");
|
||||
}
|
||||
|
||||
LogManager.Info($"[空轨] 提取到 {points.Count} 个唯一顶点");
|
||||
|
||||
// 使用 PCA 计算 OBB
|
||||
return ComputeOBBByPCA(points);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
LogManager.Error($"[空轨] 计算 OBB 失败: {ex.Message}");
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 使用 PCA 计算 OBB
|
||||
/// </summary>
|
||||
private static OrientedBoundingBox ComputeOBBByPCA(List<Point3D> points)
|
||||
{
|
||||
// 1. 计算质心
|
||||
var centroid = new Point3D(
|
||||
points.Average(p => p.X),
|
||||
points.Average(p => p.Y),
|
||||
points.Average(p => p.Z)
|
||||
);
|
||||
|
||||
LogManager.Info($"[空轨] 质心: ({centroid.X:F2}, {centroid.Y:F2}, {centroid.Z:F2})");
|
||||
|
||||
// 2. 计算协方差矩阵
|
||||
double covXX = 0, covYY = 0, covZZ = 0;
|
||||
double covXY = 0, covXZ = 0, covYZ = 0;
|
||||
|
||||
foreach (var point in points)
|
||||
{
|
||||
double dx = point.X - centroid.X;
|
||||
double dy = point.Y - centroid.Y;
|
||||
double dz = point.Z - centroid.Z;
|
||||
|
||||
covXX += dx * dx;
|
||||
covYY += dy * dy;
|
||||
covZZ += dz * dz;
|
||||
covXY += dx * dy;
|
||||
covXZ += dx * dz;
|
||||
covYZ += dy * dz;
|
||||
}
|
||||
|
||||
covXX /= points.Count;
|
||||
covYY /= points.Count;
|
||||
covZZ /= points.Count;
|
||||
covXY /= points.Count;
|
||||
covXZ /= points.Count;
|
||||
covYZ /= points.Count;
|
||||
|
||||
// 3. 计算特征值和特征向量
|
||||
var eigenvalues = new double[3];
|
||||
var eigenvectors = new Point3D[3];
|
||||
ComputeEigen3x3(covXX, covYY, covZZ, covXY, covXZ, covYZ, eigenvalues, eigenvectors);
|
||||
|
||||
// 4. 按特征值降序排列(确保 Axis 0 是最长轴)
|
||||
var sortedIndices = new[] { 0, 1, 2 };
|
||||
Array.Sort(sortedIndices, (i, j) => eigenvalues[j].CompareTo(eigenvalues[i]));
|
||||
|
||||
var sortedEigenvalues = new double[3];
|
||||
var sortedEigenvectors = new Point3D[3];
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
sortedEigenvalues[i] = eigenvalues[sortedIndices[i]];
|
||||
sortedEigenvectors[i] = eigenvectors[sortedIndices[i]];
|
||||
}
|
||||
|
||||
LogManager.Info($"[空轨] OBB特征值(降序): {sortedEigenvalues[0]:F4}, {sortedEigenvalues[1]:F4}, {sortedEigenvalues[2]:F4}");
|
||||
|
||||
// 5. 将点转换到主轴坐标系
|
||||
var axes = sortedEigenvectors;
|
||||
var projectedPoints = new List<Point3D>();
|
||||
foreach (var point in points)
|
||||
{
|
||||
var dx = point.X - centroid.X;
|
||||
var dy = point.Y - centroid.Y;
|
||||
var dz = point.Z - centroid.Z;
|
||||
|
||||
var projected = new Point3D(
|
||||
dx * axes[0].X + dy * axes[0].Y + dz * axes[0].Z,
|
||||
dx * axes[1].X + dy * axes[1].Y + dz * axes[1].Z,
|
||||
dx * axes[2].X + dy * axes[2].Y + dz * axes[2].Z
|
||||
);
|
||||
projectedPoints.Add(projected);
|
||||
}
|
||||
|
||||
// 6. 计算 AABB(在主轴坐标系中)
|
||||
var minX = projectedPoints.Min(p => p.X);
|
||||
var maxX = projectedPoints.Max(p => p.X);
|
||||
var minY = projectedPoints.Min(p => p.Y);
|
||||
var maxY = projectedPoints.Max(p => p.Y);
|
||||
var minZ = projectedPoints.Min(p => p.Z);
|
||||
var maxZ = projectedPoints.Max(p => p.Z);
|
||||
|
||||
LogManager.Info($"[空轨] 投影范围: Axis0=[{minX:F2}, {maxX:F2}], Axis1=[{minY:F2}, {maxY:F2}], Axis2=[{minZ:F2}, {maxZ:F2}]");
|
||||
|
||||
// 7. 创建 OBB(使用投影范围的中点作为中心,而不是质心)
|
||||
var obbCenter = new Point3D(
|
||||
centroid.X + axes[0].X * (minX + maxX) / 2 + axes[1].X * (minY + maxY) / 2 + axes[2].X * (minZ + maxZ) / 2,
|
||||
centroid.Y + axes[0].Y * (minX + maxX) / 2 + axes[1].Y * (minY + maxY) / 2 + axes[2].Y * (minZ + maxZ) / 2,
|
||||
centroid.Z + axes[0].Z * (minX + maxX) / 2 + axes[1].Z * (minY + maxY) / 2 + axes[2].Z * (minZ + maxZ) / 2
|
||||
);
|
||||
|
||||
var obb = new OrientedBoundingBox
|
||||
{
|
||||
Center = obbCenter,
|
||||
Axes = axes,
|
||||
Extents = new Point3D(
|
||||
(maxX - minX) / 2,
|
||||
(maxY - minY) / 2,
|
||||
(maxZ - minZ) / 2
|
||||
)
|
||||
};
|
||||
|
||||
// 保存投影范围信息,用于计算下表面
|
||||
obb.ProjectedMinZ = minZ;
|
||||
obb.ProjectedMaxZ = maxZ;
|
||||
|
||||
LogManager.Info($"[空轨] OBB各轴长度(模型单位): {obb.GetAxisLength(0):F2}, {obb.GetAxisLength(1):F2}, {obb.GetAxisLength(2):F2}");
|
||||
|
||||
// 输出 OBB 中心和轴方向
|
||||
LogManager.Info($"[空轨] OBB中心: ({obb.Center.X:F2}, {obb.Center.Y:F2}, {obb.Center.Z:F2})");
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
var axis = obb.GetAxis(i);
|
||||
var axisVector = axis.ToVector3D();
|
||||
LogManager.Info($"[空轨] Axis {i} 方向: ({axisVector.X:F3}, {axisVector.Y:F3}, {axisVector.Z:F3}), 长度: {obb.GetAxisLength(i):F2}");
|
||||
}
|
||||
|
||||
return obb;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 沿直线采样
|
||||
/// </summary>
|
||||
private static List<Point3D> SampleLine(Point3D start, Point3D end, double interval)
|
||||
{
|
||||
var result = new List<Point3D>();
|
||||
|
||||
var direction = new Vector3D(end.X - start.X, end.Y - start.Y, end.Z - start.Z);
|
||||
var length = Math.Sqrt(direction.X * direction.X + direction.Y * direction.Y + direction.Z * direction.Z);
|
||||
|
||||
if (length < 0.001)
|
||||
{
|
||||
result.Add(start);
|
||||
return result;
|
||||
}
|
||||
|
||||
// 归一化方向
|
||||
direction.X /= length;
|
||||
direction.Y /= length;
|
||||
direction.Z /= length;
|
||||
|
||||
// 计算采样点数
|
||||
int numPoints = (int)Math.Ceiling(length / interval) + 1;
|
||||
|
||||
for (int i = 0; i < numPoints; i++)
|
||||
{
|
||||
var t = (double)i / (numPoints - 1);
|
||||
var point = new Point3D(
|
||||
start.X + direction.X * length * t,
|
||||
start.Y + direction.Y * length * t,
|
||||
start.Z + direction.Z * length * t
|
||||
);
|
||||
result.Add(point);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 兼容方法:从空轨几何体提取下表面中心线(一步完成)
|
||||
/// </summary>
|
||||
|
||||
@ -490,7 +490,7 @@ namespace NavisworksTransport.UI.WPF.ViewModels
|
||||
if (vModel != null && CurrentPathRoute != null && CurrentPathRoute.Points != null)
|
||||
{
|
||||
var points = CurrentPathRoute.Points.Select(p => new Point3D(p.X, p.Y, p.Z)).ToList();
|
||||
_pathAnimationManager?.SyncToPathStart(vModel, points);
|
||||
_pathAnimationManager?.MoveVehicleToPathStart(vModel, points);
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
@ -1211,7 +1211,7 @@ namespace NavisworksTransport.UI.WPF.ViewModels
|
||||
if (CurrentPathRoute != null && CurrentPathRoute.Points != null && _pathAnimationManager != null)
|
||||
{
|
||||
var points = CurrentPathRoute.Points.Select(p => new Point3D(p.X, p.Y, p.Z)).ToList();
|
||||
_pathAnimationManager.SyncToPathStart(newObject, points);
|
||||
_pathAnimationManager.MoveVehicleToPathStart(newObject, points);
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
@ -1255,6 +1255,18 @@ namespace NavisworksTransport.UI.WPF.ViewModels
|
||||
{
|
||||
CurrentPathRoute = pathRoute;
|
||||
|
||||
// 同步路径到 PathAnimationManager
|
||||
if (pathRoute != null && _pathAnimationManager != null)
|
||||
{
|
||||
// 从 PathPlanningManager 获取对应的 PathRoute 对象
|
||||
var pathPlanningManager = PathPlanningManager.Instance;
|
||||
var coreRoute = pathPlanningManager.GetAllRoutes().FirstOrDefault(r => r.Id == pathRoute.Id);
|
||||
if (coreRoute != null)
|
||||
{
|
||||
_pathAnimationManager.SetRoute(coreRoute);
|
||||
}
|
||||
}
|
||||
|
||||
// 添加调试日志
|
||||
if (pathRoute != null)
|
||||
{
|
||||
|
||||
@ -218,12 +218,6 @@ namespace NavisworksTransport.UI.WPF.ViewModels
|
||||
// 检查是否在编辑状态
|
||||
bool isInEditMode = _pathPlanningManager?.IsInEditableState ?? false;
|
||||
|
||||
// 如果选择的是空轨路径,自动提取并显示空轨基准线
|
||||
if (_selectedPathRoute?.PathType == PathType.Rail && !isInEditMode)
|
||||
{
|
||||
ExtractAndRenderRailBaselinePaths();
|
||||
}
|
||||
|
||||
// 1. 清理现有的路径可视化,但保留网格可视化和正在编辑的路径
|
||||
if (isInEditMode)
|
||||
{
|
||||
@ -3194,6 +3188,9 @@ namespace NavisworksTransport.UI.WPF.ViewModels
|
||||
string statusMessage = $"✅ 已从{e.LoadSource}加载 {e.RouteCount} 条历史路径";
|
||||
UpdateMainStatus(statusMessage);
|
||||
LogManager.Info($"*** 状态已更新: {statusMessage} ***");
|
||||
|
||||
// 自动提取并显示所有空轨模型的基准线
|
||||
ExtractAndRenderRailBaselinePaths();
|
||||
}, "处理路径加载完成事件");
|
||||
}
|
||||
catch (Exception ex)
|
||||
|
||||
Loading…
Reference in New Issue
Block a user