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

Refine rail real-object pose handling

Browse Source
This commit is contained in:
tian 2026-03-28 22:22:18 +08:00
parent 7851e6affa
commit f4735b164e
5 changed files with 250 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

81
UnitTests/CoordinateSystem/RealObjectRailAxisConventionResolverTests.cs
View File

@ -14,22 +14,28 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem
Vector3 referenceAxisY = new Vector3(0.0f, 1.0f, 0.0f);
Vector3 referenceAxisZ = new Vector3(0.0f, 0.0f, -1.0f);
Vector3 desiredForward = Vector3.Normalize(new Vector3(-0.8987f, 0.0f, 0.4386f));
Vector3 desiredUp = Vector3.UnitY;
bool ok = RealObjectRailAxisConventionResolver.TryResolve(
referenceAxisX,
referenceAxisY,
referenceAxisZ,
LocalAxisDirection.PositiveY,
desiredForward,
CoordinateSystemType.YUp,
out ModelAxisConvention convention,
out LocalAxisDirection selectedForwardAxis,
out Vector3 selectedForwardWorldAxis);
out Vector3 selectedForwardWorldAxis,
out LocalAxisDirection selectedUpAxis,
out Vector3 selectedUpWorldAxis);
Assert.IsTrue(ok);
Assert.AreEqual(LocalAxisDirection.PositiveX, selectedForwardAxis);
Assert.AreEqual(LocalAxisDirection.PositiveX, convention.ForwardAxis);
Assert.AreEqual(LocalAxisDirection.PositiveY, convention.UpAxis);
Assert.AreEqual(LocalAxisDirection.PositiveY, selectedUpAxis);
AssertVector(selectedForwardWorldAxis, -1.0, 0.0, 0.0);
AssertVector(selectedUpWorldAxis, 0.0, 1.0, 0.0);
}
[TestMethod]
@ -39,21 +45,27 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem
Vector3 referenceAxisY = new Vector3(0.0f, 1.0f, 0.0f);
Vector3 referenceAxisZ = new Vector3(0.0f, 0.0f, 1.0f);
Vector3 desiredForward = Vector3.Normalize(new Vector3(-0.9f, 0.3f, 0.0f));
Vector3 desiredUp = Vector3.UnitZ;
bool ok = RealObjectRailAxisConventionResolver.TryResolve(
referenceAxisX,
referenceAxisY,
referenceAxisZ,
LocalAxisDirection.PositiveZ,
desiredForward,
CoordinateSystemType.ZUp,
out ModelAxisConvention convention,
out LocalAxisDirection selectedForwardAxis,
out _);
out _,
out LocalAxisDirection selectedUpAxis,
out Vector3 selectedUpWorldAxis);
Assert.IsTrue(ok);
Assert.AreEqual(LocalAxisDirection.PositiveX, selectedForwardAxis);
Assert.AreEqual(LocalAxisDirection.PositiveX, convention.ForwardAxis);
Assert.AreEqual(LocalAxisDirection.PositiveZ, convention.UpAxis);
Assert.AreEqual(LocalAxisDirection.PositiveZ, selectedUpAxis);
AssertVector(selectedUpWorldAxis, 0.0, 0.0, 1.0);
}
[TestMethod]
@ -63,15 +75,19 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem
Vector3 referenceAxisY = new Vector3(0.0f, 1.0f, 0.0f);
Vector3 referenceAxisZ = new Vector3(0.0f, 0.0f, 1.0f);
Vector3 desiredForward = Vector3.Normalize(new Vector3(0.0f, 1.0f, 0.01f));
Vector3 desiredUp = Vector3.UnitY;
bool ok = RealObjectRailAxisConventionResolver.TryResolve(
referenceAxisX,
referenceAxisY,
referenceAxisZ,
LocalAxisDirection.PositiveY,
desiredForward,
CoordinateSystemType.YUp,
out _,
out LocalAxisDirection selectedForwardAxis,
out _,
out _,
out _);
Assert.IsTrue(ok);
@ -79,6 +95,67 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem
Assert.AreNotEqual(LocalAxisDirection.NegativeY, selectedForwardAxis);
}
[TestMethod]
public void YUp_WhenRemainingZAxisBestMatchesDesiredUp_ShouldChooseZAsUp()
{
Vector3 referenceAxisX = new Vector3(-1.0f, 0.0f, 0.0f);
Vector3 referenceAxisY = new Vector3(0.0f, 0.0f, 1.0f);
Vector3 referenceAxisZ = new Vector3(0.0f, 1.0f, 0.0f);
Vector3 desiredForward = new Vector3(-1.0f, 0.0f, 0.0f);
Vector3 desiredUp = new Vector3(0.0f, 1.0f, 0.0f);
bool ok = RealObjectRailAxisConventionResolver.TryResolve(
referenceAxisX,
referenceAxisY,
referenceAxisZ,
LocalAxisDirection.PositiveZ,
desiredForward,
CoordinateSystemType.YUp,
out ModelAxisConvention convention,
out LocalAxisDirection selectedForwardAxis,
out Vector3 selectedForwardWorldAxis,
out LocalAxisDirection selectedUpAxis,
out Vector3 selectedUpWorldAxis);
Assert.IsTrue(ok);
Assert.AreEqual(LocalAxisDirection.PositiveX, selectedForwardAxis);
Assert.AreEqual(LocalAxisDirection.PositiveZ, selectedUpAxis);
Assert.AreEqual(LocalAxisDirection.PositiveX, convention.ForwardAxis);
Assert.AreEqual(LocalAxisDirection.PositiveZ, convention.UpAxis);
AssertVector(selectedForwardWorldAxis, -1.0, 0.0, 0.0);
AssertVector(selectedUpWorldAxis, 0.0, 1.0, 0.0);
}
[TestMethod]
public void YUp_ShouldUseMappedHostUpLocalAxis_AndSelectForwardFromRemainingAxes()
{
Vector3 referenceAxisX = new Vector3(0.0f, 0.0f, 1.0f);
Vector3 referenceAxisY = new Vector3(0.0f, 1.0f, 0.0f);
Vector3 referenceAxisZ = new Vector3(-1.0f, 0.0f, 0.0f);
Vector3 desiredForward = Vector3.Normalize(new Vector3(0.99f, -0.14f, -0.02f));
bool ok = RealObjectRailAxisConventionResolver.TryResolve(
referenceAxisX,
referenceAxisY,
referenceAxisZ,
LocalAxisDirection.PositiveZ,
desiredForward,
CoordinateSystemType.YUp,
out ModelAxisConvention convention,
out LocalAxisDirection selectedForwardAxis,
out Vector3 selectedForwardWorldAxis,
out LocalAxisDirection selectedUpAxis,
out Vector3 selectedUpWorldAxis);
Assert.IsTrue(ok);
Assert.AreEqual(LocalAxisDirection.PositiveZ, selectedUpAxis);
Assert.AreEqual(LocalAxisDirection.PositiveZ, convention.UpAxis);
Assert.AreEqual(LocalAxisDirection.NegativeY, selectedForwardAxis);
Assert.AreEqual(LocalAxisDirection.NegativeY, convention.ForwardAxis);
AssertVector(selectedUpWorldAxis, -1.0, 0.0, 0.0);
AssertVector(selectedForwardWorldAxis, 0.0, -1.0, 0.0);
}
private static void AssertVector(Vector3 actual, double x, double y, double z, double tolerance = 1e-6)
{
Assert.AreEqual(x, actual.X, tolerance);

21
UnitTests/CoordinateSystem/RealObjectRailExtentResolverTests.cs
View File

@ -21,11 +21,12 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem
referenceAxisX,
referenceAxisY,
referenceAxisZ,
LocalAxisDirection.PositiveY,
desiredForward,
CoordinateSystemType.YUp,
forwardSize: 6.0,
sideSize: 2.0,
upSize: 4.0,
6.0,
2.0,
4.0,
baseline,
baseline,
out ModelAxisConvention convention,
@ -56,11 +57,12 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem
referenceAxisX,
referenceAxisY,
referenceAxisZ,
LocalAxisDirection.PositiveY,
desiredForward,
CoordinateSystemType.YUp,
forwardSize: 6.0,
sideSize: 2.0,
upSize: 4.0,
6.0,
2.0,
4.0,
baseline,
final,
out _,
@ -95,11 +97,12 @@ namespace NavisworksTransport.UnitTests.CoordinateSystem
referenceAxisX,
referenceAxisY,
referenceAxisZ,
LocalAxisDirection.PositiveY,
desiredForward,
CoordinateSystemType.YUp,
forwardSize: 6.0,
sideSize: 2.0,
upSize: 4.0,
6.0,
2.0,
4.0,
baseline,
final,
out ModelAxisConvention convention,

135
src/Utils/CoordinateSystem/RealObjectRailAxisConventionResolver.cs
View File

@ -22,15 +22,22 @@ namespace NavisworksTransport.Utils.CoordinateSystem
Vector3 referenceAxisX,
Vector3 referenceAxisY,
Vector3 referenceAxisZ,
LocalAxisDirection hostUpLocalAxis,
Vector3 desiredForward,
CoordinateSystemType hostType,
out ModelAxisConvention convention,
out LocalAxisDirection selectedForwardAxis,
out Vector3 selectedForwardWorldAxis)
out Vector3 selectedForwardWorldAxis,
out LocalAxisDirection selectedUpAxis,
out Vector3 selectedUpWorldAxis)
{
convention = null;
selectedForwardAxis = LocalAxisDirection.PositiveX;
selectedForwardWorldAxis = Vector3.Zero;
selectedUpAxis = hostType == CoordinateSystemType.YUp
? LocalAxisDirection.PositiveY
: LocalAxisDirection.PositiveZ;
selectedUpWorldAxis = Vector3.Zero;
if (!TryNormalize(desiredForward, out Vector3 normalizedForward))
{
@ -44,29 +51,31 @@ namespace NavisworksTransport.Utils.CoordinateSystem
return false;
}
LocalAxisDirection semanticUpAxis =
hostType == CoordinateSystemType.YUp
? LocalAxisDirection.PositiveY
: LocalAxisDirection.PositiveZ;
Candidate best = Candidate.Invalid;
foreach (Candidate candidate in EnumerateForwardCandidates(axisX, axisY, axisZ, semanticUpAxis))
{
float score = Vector3.Dot(candidate.WorldAxis, normalizedForward);
if (best.IsInvalid || score > best.Score)
{
best = new Candidate(candidate.Axis, candidate.WorldAxis, score);
}
}
if (best.IsInvalid)
if (!TryResolveWorldAxisForDirection(hostUpLocalAxis, axisX, axisY, axisZ, out var fixedUpWorldAxis))
{
return false;
}
convention = new ModelAxisConvention(best.Axis, semanticUpAxis);
selectedForwardAxis = best.Axis;
selectedForwardWorldAxis = best.WorldAxis;
Candidate bestForward = Candidate.Invalid;
foreach (Candidate candidate in EnumerateForwardCandidates(axisX, axisY, axisZ, hostUpLocalAxis))
{
float score = Vector3.Dot(candidate.WorldAxis, normalizedForward);
if (bestForward.IsInvalid || score > bestForward.Score)
{
bestForward = new Candidate(candidate.Axis, candidate.WorldAxis, score);
}
}
if (bestForward.IsInvalid)
{
return false;
}
convention = new ModelAxisConvention(bestForward.Axis, hostUpLocalAxis);
selectedForwardAxis = bestForward.Axis;
selectedForwardWorldAxis = bestForward.WorldAxis;
selectedUpAxis = hostUpLocalAxis;
selectedUpWorldAxis = fixedUpWorldAxis;
return true;
}
@ -74,28 +83,78 @@ namespace NavisworksTransport.Utils.CoordinateSystem
Vector3 axisX,
Vector3 axisY,
Vector3 axisZ,
LocalAxisDirection semanticUpAxis)
LocalAxisDirection selectedUpAxis)
{
switch (semanticUpAxis)
{
case LocalAxisDirection.PositiveY:
return new[]
{
new Candidate(LocalAxisDirection.PositiveX, axisX, 0f),
new Candidate(LocalAxisDirection.NegativeX, -axisX, 0f),
new Candidate(LocalAxisDirection.PositiveZ, axisZ, 0f),
new Candidate(LocalAxisDirection.NegativeZ, -axisZ, 0f)
};
int upAxisIndex = GetAxisFamilyIndex(selectedUpAxis);
return Array.FindAll(
EnumerateAxisCandidates(axisX, axisY, axisZ),
candidate => GetAxisFamilyIndex(candidate.Axis) != upAxisIndex);
}
private static Candidate[] EnumerateAxisCandidates(
Vector3 axisX,
Vector3 axisY,
Vector3 axisZ)
{
return new[]
{
new Candidate(LocalAxisDirection.PositiveX, axisX, 0f),
new Candidate(LocalAxisDirection.NegativeX, -axisX, 0f),
new Candidate(LocalAxisDirection.PositiveY, axisY, 0f),
new Candidate(LocalAxisDirection.NegativeY, -axisY, 0f),
new Candidate(LocalAxisDirection.PositiveZ, axisZ, 0f),
new Candidate(LocalAxisDirection.NegativeZ, -axisZ, 0f)
};
}
private static bool TryResolveWorldAxisForDirection(
LocalAxisDirection axis,
Vector3 axisX,
Vector3 axisY,
Vector3 axisZ,
out Vector3 worldAxis)
{
switch (axis)
{
case LocalAxisDirection.PositiveX:
worldAxis = axisX;
return true;
case LocalAxisDirection.NegativeX:
worldAxis = -axisX;
return true;
case LocalAxisDirection.PositiveY:
worldAxis = axisY;
return true;
case LocalAxisDirection.NegativeY:
worldAxis = -axisY;
return true;
case LocalAxisDirection.PositiveZ:
worldAxis = axisZ;
return true;
case LocalAxisDirection.NegativeZ:
worldAxis = -axisZ;
return true;
default:
return new[]
{
new Candidate(LocalAxisDirection.PositiveX, axisX, 0f),
new Candidate(LocalAxisDirection.NegativeX, -axisX, 0f),
new Candidate(LocalAxisDirection.PositiveY, axisY, 0f),
new Candidate(LocalAxisDirection.NegativeY, -axisY, 0f)
};
worldAxis = Vector3.Zero;
return false;
}
}
private static int GetAxisFamilyIndex(LocalAxisDirection axis)
{
switch (axis)
{
case LocalAxisDirection.PositiveX:
case LocalAxisDirection.NegativeX:
return 0;
case LocalAxisDirection.PositiveY:
case LocalAxisDirection.NegativeY:
return 1;
case LocalAxisDirection.PositiveZ:
case LocalAxisDirection.NegativeZ:
return 2;
default:
throw new ArgumentOutOfRangeException(nameof(axis), axis, null);
}
}

4
src/Utils/CoordinateSystem/RealObjectRailExtentResolver.cs
View File

@ -20,6 +20,7 @@ namespace NavisworksTransport.Utils.CoordinateSystem
Vector3 referenceAxisX,
Vector3 referenceAxisY,
Vector3 referenceAxisZ,
LocalAxisDirection hostUpLocalAxis,
Vector3 desiredForward,
CoordinateSystemType hostType,
double forwardSize,
@ -37,10 +38,13 @@ namespace NavisworksTransport.Utils.CoordinateSystem
referenceAxisX,
referenceAxisY,
referenceAxisZ,
hostUpLocalAxis,
desiredForward,
hostType,
out convention,
out _,
out _,
out _,
out _))
{
return false;

58
src/Utils/ModelItemTransformHelper.cs
View File

@ -564,6 +564,8 @@ namespace NavisworksTransport.Utils
$"Z=({deltaLinear.Get(0, 2):F4},{deltaLinear.Get(1, 2):F4},{deltaLinear.Get(2, 2):F4}), " +
$"旋后当前点=({rotatedCurrentPosition.X:F3},{rotatedCurrentPosition.Y:F3},{rotatedCurrentPosition.Z:F3})");
LogGeometryLevelTransforms(item, "[模型增量姿态应用前][GeometryAPI]");
// 用显式三步法应用三维增量位姿：
// 1. 把当前锚点移到原点
// 2. 绕原点旋转到目标姿态
@ -619,6 +621,62 @@ namespace NavisworksTransport.Utils
}
}
public static bool TryGetCurrentOverrideRotation(ModelItem item, out Rotation3D rotation)
{
rotation = Rotation3D.Identity;
if (item == null)
{
return false;
}
try
{
ModelGeometry geometry = item.FindFirstGeometry() ?? item.Geometry;
if (geometry?.PermanentOverrideTransform == null)
{
return true;
}
rotation = geometry.PermanentOverrideTransform.Factor().Rotation;
return true;
}
catch (Exception ex)
{
LogManager.Warning($"[当前覆盖姿态] 读取 PermanentOverrideTransform 失败: {ex.Message}");
return false;
}
}
public static bool TryResolveOverrideRotationForFinalTarget(
ModelItem item,
Rotation3D finalTargetRotation,
out Rotation3D overrideRotation)
{
overrideRotation = Rotation3D.Identity;
if (item == null)
{
return false;
}
try
{
ModelGeometry geometry = item.FindFirstGeometry() ?? item.Geometry;
Transform3D originalTransform = geometry?.OriginalTransform ?? item.Transform;
Rotation3D originalRotation = originalTransform.Factor().Rotation;
var originalInverse = originalRotation.Invert();
var overrideTransform = Transform3D.Multiply(
new Transform3D(originalInverse),
new Transform3D(finalTargetRotation));
overrideRotation = overrideTransform.Factor().Rotation;
return true;
}
catch (Exception ex)
{
LogManager.Warning($"[覆盖姿态换算] 计算 override 姿态失败: {ex.Message}");
return false;
}
}
public static void LogCurrentGeometryTransformsForDebug(ModelItem item, string prefix)
{
LogGeometryLevelTransforms(item, prefix);