tian/NavisworksTransport
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Fix rail rotation correction semantics

Browse Source
This commit is contained in:
tian 2026-03-23 00:11:49 +08:00
parent 93ad55c725
commit e9128282b5
5 changed files with 240 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

82
UnitTests/CoordinateSystem/CanonicalRailPoseBuilderTests.cs
View File

@ -1,5 +1,6 @@
using Microsoft.VisualStudio.TestTools.UnitTesting;
using NavisworksTransport.Utils.CoordinateSystem;
using System;
using System.Numerics;
namespace NavisworksTransport.UnitTests.CoordinateSystem
@ -82,6 +83,60 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem
Assert.AreEqual(0.0, Vector3.Dot(frame.Lateral, frame.Normal), 1e-6);
}
[TestMethod]
public void ZeroLocalUpRotation_ShouldMatchCurrentRailBaseline()
{
var convention = ModelAxisConvention.CreateRailAssetConvention();
Assert.IsTrue(CanonicalRailPoseBuilder.TryCreateQuaternion(
new Vector3(0, 0, 0),
new Vector3(1, 0, 0),
new Vector3(2, 0, 0),
Vector3.UnitZ,
convention,
out Quaternion baselineRotation));
Assert.IsTrue(CanonicalRailPoseBuilder.TryCreateQuaternion(
new Vector3(0, 0, 0),
new Vector3(1, 0, 0),
new Vector3(2, 0, 0),
Vector3.UnitZ,
convention,
0.0,
out Quaternion correctedRotation));
AssertQuaternionEquivalent(baselineRotation, correctedRotation);
}
[TestMethod]
public void LocalUpRotation_ShouldAlignCorrectedLocalForwardWithRailTangent()
{
var convention = ModelAxisConvention.CreateRailAssetConvention();
Assert.IsTrue(CanonicalRailPoseBuilder.TryCreateQuaternion(
new Vector3(0, 0, 0),
new Vector3(1, 0, 0),
new Vector3(2, 0, 0),
Vector3.UnitZ,
convention,
90.0,
out Quaternion rotation));
Quaternion localPreRotation = Quaternion.CreateFromAxisAngle(
Vector3.Normalize(convention.UpUnitVector),
(float)(90.0 * Math.PI / 180.0));
Vector3 correctedLocalForward = Vector3.Normalize(Vector3.Transform(convention.ForwardUnitVector, localPreRotation));
Vector3 correctedLocalUp = Vector3.Normalize(Vector3.Transform(convention.UpUnitVector, localPreRotation));
Matrix4x4 linear = Matrix4x4.CreateFromQuaternion(rotation);
Vector3 mappedForward = TransformLocalVector(linear, correctedLocalForward);
Vector3 mappedUp = TransformLocalVector(linear, correctedLocalUp);
AssertVector(mappedForward, 1, 0, 0);
AssertVector(mappedUp, 0, 0, 1);
}
private static void AssertColumn(Matrix4x4 matrix, int column, double x, double y, double z)
{
switch (column)
@ -106,5 +161,32 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem
break;
}
}
private static void AssertVector(Vector3 actual, double x, double y, double z)
{
Assert.AreEqual(x, actual.X, 1e-6);
Assert.AreEqual(y, actual.Y, 1e-6);
Assert.AreEqual(z, actual.Z, 1e-6);
}
private static void AssertQuaternionEquivalent(Quaternion expected, Quaternion actual)
{
double dot = Math.Abs(
expected.X * actual.X +
expected.Y * actual.Y +
expected.Z * actual.Z +
expected.W * actual.W);
Assert.AreEqual(1.0, dot, 1e-6);
}
private static Vector3 TransformLocalVector(Matrix4x4 linear, Vector3 localVector)
{
Vector3 world = new Vector3(
linear.M11 * localVector.X + linear.M12 * localVector.Y + linear.M13 * localVector.Z,
linear.M21 * localVector.X + linear.M22 * localVector.Y + linear.M23 * localVector.Z,
linear.M31 * localVector.X + linear.M32 * localVector.Y + linear.M33 * localVector.Z);
return Vector3.Normalize(world);
}
}
}

29
doc/working/current-engineering-state.md
View File

@ -1,6 +1,6 @@
# 当前工程状态
更新时间：2026-03-22
更新时间：2026-03-23
## 1. 当前稳定状态
@ -136,8 +136,31 @@
- 那么 `YUp` 模型下聚焦就会跑到侧视图。
- 当前原则：
- 聚焦输入/输出使用宿主坐标
- 方向语义通过 `HostCoordinateAdapter` 明确适配
- 不要在视点辅助逻辑里直接假设宿主一定是 `ZUp`
- 方向语义通过 `HostCoordinateAdapter` 明确适配
- 不要在视点辅助逻辑里直接假设宿主一定是 `ZUp`
### 4.7 Rail 角度修正
- `Rail` 的“调整角度”和 `Ground / Hoisting` 不是同一种实现语义。
- `Ground / Hoisting` 当前语义：
- 在内部 `Canonical` 坐标系里，绕统一 `CanonicalUp` 做水平角度修正。
- `Rail` 当前正确语义：
- 角度修正必须先作用在构件局部轴约定上，等价于“CAD 原位局部预旋转”
- 然后再进入 `Rail` 的切向/法向姿态求解
- 不能在物体已经落到起点后，再在宿主空间对最终 pose 补一个世界轴旋转
- 当前实现链路已经收束到：
- `CanonicalRailPoseBuilder`
- `RailPathPoseHelper`
- `PathAnimationManager`
- 当前规则：
- `Rail 0°` 时，必须严格保持当前稳定基线，不允许改变既有 `Rail` 姿态
- `Rail` 有角度修正时，对齐到路径切向的是“修正后的 local forward”
- `Rail` 路径切向/法向框架本身不能因为角度修正被改坏
- `Rail` 通行空间和真实物体姿态必须共享同一套角度语义，不能一个改局部 footprint、一个改世界 pose
- 调试优先级：
1. 先验证 `0°` 基线是否保持不变
2. 再验证修正后 `forward` 是否仍沿 rail 切向
3. 最后再看 Navisworks 应用层是否正确按三步增量姿态落位
## 5. 当前保留的日志策略

2
src/Core/Animation/PathAnimationManager.cs
View File

@ -675,6 +675,7 @@ namespace NavisworksTransport.Core.Animation
_pathPoints[0],
nextPoint,
GetCurrentRailModelAxisConvention(),
_objectRotationCorrection,
out var railRotation))
{
var railLinearTransform = new Transform3D(railRotation).Linear;
@ -1037,6 +1038,7 @@ namespace NavisworksTransport.Core.Animation
framePosition,
nextFramePoint,
GetCurrentRailModelAxisConvention(),
_objectRotationCorrection,
out var railRotation))
{
frame.Rotation = railRotation;

79
src/Utils/CoordinateSystem/CanonicalRailPoseBuilder.cs
View File

@ -88,6 +88,25 @@ namespace NavisworksTransport.Utils.CoordinateSystem
Vector3 canonicalUp,
ModelAxisConvention convention,
out Quaternion rotation)
{
return TryCreateQuaternion(
previousPoint,
currentPoint,
nextPoint,
canonicalUp,
convention,
0.0,
out rotation);
}
public static bool TryCreateQuaternion(
Vector3 previousPoint,
Vector3 currentPoint,
Vector3 nextPoint,
Vector3 canonicalUp,
ModelAxisConvention convention,
double localUpRotationDegrees,
out Quaternion rotation)
{
rotation = Quaternion.Identity;
@ -101,9 +120,67 @@ namespace NavisworksTransport.Utils.CoordinateSystem
return false;
}
rotation = convention.CreateQuaternion(forward, up);
if (Math.Abs(localUpRotationDegrees) < 1e-9)
{
rotation = convention.CreateQuaternion(forward, up);
return true;
}
float correctionRadians = (float)(localUpRotationDegrees * Math.PI / 180.0);
Quaternion localPreRotation = Quaternion.CreateFromAxisAngle(
Vector3.Normalize(convention.UpUnitVector),
correctionRadians);
Vector3 correctedLocalForward = Vector3.Normalize(Vector3.Transform(convention.ForwardUnitVector, localPreRotation));
Vector3 correctedLocalUp = Vector3.Normalize(Vector3.Transform(convention.UpUnitVector, localPreRotation));
rotation = CreateQuaternionFromLocalAxes(correctedLocalForward, correctedLocalUp, forward, up);
return true;
}
private static Quaternion CreateQuaternionFromLocalAxes(
Vector3 localForward,
Vector3 localUp,
Vector3 worldForward,
Vector3 worldUp)
{
Vector3 normalizedLocalForward = Vector3.Normalize(localForward);
Vector3 normalizedLocalUp = Vector3.Normalize(localUp);
Vector3 normalizedWorldForward = Vector3.Normalize(worldForward);
Vector3 normalizedWorldUp = Vector3.Normalize(worldUp);
Vector3 localRemaining = Vector3.Normalize(Vector3.Cross(normalizedLocalForward, normalizedLocalUp));
Vector3 worldRemaining = Vector3.Normalize(Vector3.Cross(normalizedWorldForward, normalizedWorldUp));
Vector3 worldX = MapOriginalLocalAxis(Vector3.UnitX, normalizedLocalForward, normalizedLocalUp, localRemaining, normalizedWorldForward, normalizedWorldUp, worldRemaining);
Vector3 worldY = MapOriginalLocalAxis(Vector3.UnitY, normalizedLocalForward, normalizedLocalUp, localRemaining, normalizedWorldForward, normalizedWorldUp, worldRemaining);
Vector3 worldZ = MapOriginalLocalAxis(Vector3.UnitZ, normalizedLocalForward, normalizedLocalUp, localRemaining, normalizedWorldForward, normalizedWorldUp, worldRemaining);
Matrix4x4 linear = new Matrix4x4(
worldX.X, worldY.X, worldZ.X, 0f,
worldX.Y, worldY.Y, worldZ.Y, 0f,
worldX.Z, worldY.Z, worldZ.Z, 0f,
0f, 0f, 0f, 1f);
return Quaternion.CreateFromRotationMatrix(linear);
}
private static Vector3 MapOriginalLocalAxis(
Vector3 originalAxis,
Vector3 localForward,
Vector3 localUp,
Vector3 localRemaining,
Vector3 worldForward,
Vector3 worldUp,
Vector3 worldRemaining)
{
float forwardWeight = Vector3.Dot(originalAxis, localForward);
float upWeight = Vector3.Dot(originalAxis, localUp);
float remainingWeight = Vector3.Dot(originalAxis, localRemaining);
return Vector3.Normalize(
forwardWeight * worldForward +
upWeight * worldUp +
remainingWeight * worldRemaining);
}
}
}

55
src/Utils/RailPathPoseHelper.cs
View File

@ -182,6 +182,23 @@ namespace NavisworksTransport.Utils
Point3D nextPoint,
ModelAxisConvention axisConvention,
out Matrix3 linearTransform)
{
return TryCreateRailLinearTransform(
previousPoint,
currentPoint,
nextPoint,
axisConvention,
0.0,
out linearTransform);
}
public static bool TryCreateRailLinearTransform(
Point3D previousPoint,
Point3D currentPoint,
Point3D nextPoint,
ModelAxisConvention axisConvention,
double localUpRotationDegrees,
out Matrix3 linearTransform)
{
linearTransform = null;
@ -208,7 +225,16 @@ namespace NavisworksTransport.Utils
var canonicalForward = frame.Forward;
var canonicalLateral = frame.Lateral;
var canonicalUp = frame.Normal;
Quaternion canonicalRotation = axisConvention.CreateQuaternion(canonicalForward, canonicalUp);
Quaternion canonicalRotation = CanonicalRailPoseBuilder.TryCreateQuaternion(
canonicalPreviousPoint,
canonicalCurrentPoint,
canonicalNextPoint,
HostCoordinateAdapter.CanonicalUpVector3,
axisConvention,
localUpRotationDegrees,
out var correctedRotation)
? correctedRotation
: axisConvention.CreateQuaternion(canonicalForward, canonicalUp);
Matrix4x4 canonicalLinear = Matrix4x4.CreateFromQuaternion(canonicalRotation);
var hostXAxis = Normalize(adapter.FromCanonicalVector(ToNavVector(new Vector3(canonicalLinear.M11, canonicalLinear.M21, canonicalLinear.M31))));
@ -229,7 +255,7 @@ namespace NavisworksTransport.Utils
$"Canonical侧向=({canonicalLateral.X:F4},{canonicalLateral.Y:F4},{canonicalLateral.Z:F4}), " +
$"Canonical法向=({canonicalUp.X:F4},{canonicalUp.Y:F4},{canonicalUp.Z:F4}), " +
$"Host法向=({hostZAxis.X:F4},{hostZAxis.Y:F4},{hostZAxis.Z:F4}), " +
$"模型Forward={axisConvention.ForwardAxis}, 模型Up={axisConvention.UpAxis}");
$"模型Forward={axisConvention.ForwardAxis}, 模型Up={axisConvention.UpAxis}, 本地预旋转={localUpRotationDegrees:F1}°");
return true;
}
@ -257,11 +283,34 @@ namespace NavisworksTransport.Utils
Point3D nextPoint,
ModelAxisConvention axisConvention,
out Rotation3D rotation)
{
return TryCreateRailRotation(
previousPoint,
currentPoint,
nextPoint,
axisConvention,
0.0,
out rotation);
}
public static bool TryCreateRailRotation(
Point3D previousPoint,
Point3D currentPoint,
Point3D nextPoint,
ModelAxisConvention axisConvention,
double localUpRotationDegrees,
out Rotation3D rotation)
{
rotation = Rotation3D.Identity;
LogRotationConstructorConventionOnce();
if (!TryCreateRailLinearTransform(previousPoint, currentPoint, nextPoint, axisConvention, out var linearTransform))
if (!TryCreateRailLinearTransform(
previousPoint,
currentPoint,
nextPoint,
axisConvention,
localUpRotationDegrees,
out var linearTransform))
{
return false;
}