用最小夹角法修正空轨基线方向错误的问题
This commit is contained in:
parent
99d2690800
commit
b1da586b27
@ -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;
|
||||
|
||||
@ -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})";
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user