用最小夹角法修正空轨基线方向错误的问题

This commit is contained in:
tian 2026-01-13 10:29:19 +08:00
parent 99d2690800
commit b1da586b27
2 changed files with 20 additions and 37 deletions

View File

@ -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;

View File

@ -522,25 +522,4 @@ namespace NavisworksTransport.PathPlanning
Other
}
/// <summary>
/// 3D向量结构
/// </summary>
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})";
}
}
}
}