diff --git a/src/PathPlanning/RailGeometryHelper.cs b/src/PathPlanning/RailGeometryHelper.cs index 2eab161..3c9eb22 100644 --- a/src/PathPlanning/RailGeometryHelper.cs +++ b/src/PathPlanning/RailGeometryHelper.cs @@ -498,26 +498,28 @@ namespace NavisworksTransport.PathPlanning var center = obb.Center; var direction = principalAxis.ToVector3D(); - // 找到最接近"向下"方向的轴(Z 轴负方向) - var downAxisIndex = 1; // 默认使用 Axis 1(次长轴) - var minZComponent = 1.0; // Z 分量的最小值 + // 找到 Z 轴方向的轴(使用点积绝对值最大法) + var zAxisDirection = new Vector3D(0, 0, 1); // Z 轴向上方向 - for (int i = 1; i < 3; i++) // 检查 Axis 1 和 Axis 2 + // 计算所有轴与 Z 轴方向的点积绝对值 + var absDotProducts = new double[2]; + for (int i = 1; i < 3; i++) { var axis = obb.GetAxis(i); var axisVector = axis.ToVector3D(); - var zComponent = axisVector.Z; - LogManager.Info($"[空轨] Axis {i} Z分量: {zComponent:F3}"); + // 计算点积:axis · zAxisDirection + var dotProduct = axisVector.X * zAxisDirection.X + + axisVector.Y * zAxisDirection.Y + + axisVector.Z * zAxisDirection.Z; + absDotProducts[i - 1] = Math.Abs(dotProduct); - // 找到 Z 分量最小的轴(最向下,即最负) - if (zComponent < minZComponent) - { - minZComponent = zComponent; - downAxisIndex = i; - } + LogManager.Info($"[空轨] Axis {i} 与 Z 轴方向点积: {dotProduct:F3}, 绝对值: {absDotProducts[i - 1]:F3}"); } + // 选择点积绝对值最大的轴(最接近 Z 轴方向) + int downAxisIndex = absDotProducts[1] > absDotProducts[0] ? 2 : 1; + var downAxis = obb.GetAxis(downAxisIndex); var downAxisVector = downAxis.ToVector3D(); @@ -771,9 +773,11 @@ namespace NavisworksTransport.PathPlanning } // 归一化方向 - direction.X /= length; - direction.Y /= length; - direction.Z /= length; + direction = new Vector3D( + direction.X / length, + direction.Y / length, + direction.Z / length + ); // 计算采样点数 int numPoints = (int)Math.Ceiling(length / interval) + 1; diff --git a/src/PathPlanning/SlopeAnalyzer.cs b/src/PathPlanning/SlopeAnalyzer.cs index d041a13..afd9454 100644 --- a/src/PathPlanning/SlopeAnalyzer.cs +++ b/src/PathPlanning/SlopeAnalyzer.cs @@ -522,25 +522,4 @@ namespace NavisworksTransport.PathPlanning Other } - /// - /// 3D向量结构 - /// - public struct Vector3D - { - public double X { get; set; } - public double Y { get; set; } - public double Z { get; set; } - - public Vector3D(double x, double y, double z) - { - X = x; - Y = y; - Z = z; - } - - public override string ToString() - { - return $"({X:F3}, {Y:F3}, {Z:F3})"; - } - } -} \ No newline at end of file + } \ No newline at end of file