Add fragment up detection and real object pose probes
This commit is contained in:
parent
48de5aa921
commit
8d10f959b2
@ -22,6 +22,8 @@ Use this skill for any work involving coordinate-system transforms, object pose,
|
||||
- `src/Utils/CoordinateSystem/CanonicalPlanarPoseBuilder.cs`
|
||||
- `src/Utils/CoordinateSystem/CanonicalRailPoseBuilder.cs`
|
||||
- `src/Utils/CoordinateSystem/CanonicalTrackedPositionResolver.cs`
|
||||
- `src/Utils/CoordinateSystem/RealObjectPlanarPoseSolver.cs`
|
||||
- `src/Utils/CoordinateSystem/FragmentRepresentativePoseHelper.cs`
|
||||
- `src/Utils/ModelItemTransformHelper.cs`
|
||||
- `src/Core/Animation/PathAnimationManager.cs`
|
||||
- `src/Core/VirtualObjectManager.cs`
|
||||
@ -33,6 +35,8 @@ Use this skill for any work involving coordinate-system transforms, object pose,
|
||||
- UI, logs, and user inputs/output stay in host coordinates.
|
||||
- Internal pose solving stays in canonical space.
|
||||
- Asset coordinates only apply to plugin-owned assets such as the virtual object and the unit cylinder/reference rod.
|
||||
- Real-object planar pose solving must stay in host coordinates and should keep the reference-pose source injectable so fragment-derived representative pose can be wired in later.
|
||||
- Real-object planar pose solving should prefer fragment-derived representative pose when available; original `Transform` is only an explicit fallback, not the primary source.
|
||||
- Do not assume `BoundingBox.Center` is a stable pose anchor after rotation unless the flow explicitly proves it.
|
||||
- Do not add temporary force-sync or fallback logic unless it is removed after the root cause is fixed.
|
||||
- Do not mix virtual-object behavior into real-object behavior through shared mutable mode flags.
|
||||
@ -50,4 +54,3 @@ Use this skill for any work involving coordinate-system transforms, object pose,
|
||||
- Lock baseline pose, rotated pose, and restore behavior.
|
||||
- For passage-space or footprint changes, verify extents after rotation, not just orientation.
|
||||
- For virtual objects, verify scale preservation and CAD restore behavior.
|
||||
|
||||
|
||||
@ -59,9 +59,11 @@
|
||||
<Compile Include="UnitTests\CoordinateSystem\ObjectSpaceOrientationHelperTests.cs" />
|
||||
<Compile Include="UnitTests\CoordinateSystem\CanonicalRailOffsetResolverTests.cs" />
|
||||
<Compile Include="UnitTests\CoordinateSystem\CanonicalTrackedPositionResolverTests.cs" />
|
||||
<Compile Include="UnitTests\CoordinateSystem\FragmentRepresentativePoseHelperTests.cs" />
|
||||
<Compile Include="UnitTests\CoordinateSystem\HoistingCoordinateHelperTests.cs" />
|
||||
<Compile Include="UnitTests\CoordinateSystem\ModelAxisConventionTests.cs" />
|
||||
<Compile Include="UnitTests\CoordinateSystem\ProjectReferenceFrameTests.cs" />
|
||||
<Compile Include="UnitTests\CoordinateSystem\RealObjectPlanarPoseSolverTests.cs" />
|
||||
<Compile Include="UnitTests\CoordinateSystem\AssemblyEndFaceAnalyzerTests.cs" />
|
||||
<Compile Include="UnitTests\CoordinateSystem\AssemblyInstallationReferenceBuilderTests.cs" />
|
||||
<Compile Include="UnitTests\Properties\AssemblyInfo.cs" />
|
||||
|
||||
@ -340,6 +340,8 @@
|
||||
<Compile Include="src\Utils\CoordinateSystem\CanonicalRailOffsetResolver.cs" />
|
||||
<Compile Include="src\Utils\CoordinateSystem\CanonicalRailPoseBuilder.cs" />
|
||||
<Compile Include="src\Utils\CoordinateSystem\CanonicalTrackedPositionResolver.cs" />
|
||||
<Compile Include="src\Utils\CoordinateSystem\FragmentRepresentativePoseHelper.cs" />
|
||||
<Compile Include="src\Utils\CoordinateSystem\RealObjectPlanarPoseSolver.cs" />
|
||||
<Compile Include="src\Utils\CoordinateSystem\ObjectSpaceOrientationHelper.cs" />
|
||||
<Compile Include="src\Utils\CoordinateSystem\RotatedObjectExtentHelper.cs" />
|
||||
<Compile Include="src\Utils\CoordinateSystem\HostCoordinateAdapter.cs" />
|
||||
|
||||
@ -0,0 +1,123 @@
|
||||
using Microsoft.VisualStudio.TestTools.UnitTesting;
|
||||
using NavisworksTransport.Utils.CoordinateSystem;
|
||||
using System;
|
||||
using System.Numerics;
|
||||
|
||||
namespace NavisworksTransport.UnitTests.CoordinateSystem
|
||||
{
|
||||
[TestClass]
|
||||
public class FragmentRepresentativePoseHelperTests
|
||||
{
|
||||
[TestMethod]
|
||||
public void ShouldAverageFragmentMatrices_AndIgnoreTranslation()
|
||||
{
|
||||
Quaternion expectedRotation = Quaternion.Normalize(
|
||||
Quaternion.CreateFromAxisAngle(Vector3.UnitZ, (float)(Math.PI / 6.0)));
|
||||
|
||||
double[] fragmentMatrix1 = CreateFragmentMatrix(expectedRotation, new Vector3(10.0f, 20.0f, 30.0f));
|
||||
double[] fragmentMatrix2 = CreateFragmentMatrix(expectedRotation, new Vector3(-3.0f, 8.0f, 1.0f));
|
||||
|
||||
bool ok = FragmentRepresentativePoseHelper.TryGetRepresentativeRotation(
|
||||
new[] { fragmentMatrix1, fragmentMatrix2 },
|
||||
out Quaternion representativeRotation);
|
||||
|
||||
Assert.IsTrue(ok);
|
||||
AssertVector(
|
||||
Vector3.Transform(Vector3.UnitX, representativeRotation),
|
||||
Vector3.Transform(Vector3.UnitX, expectedRotation),
|
||||
1e-4);
|
||||
AssertVector(
|
||||
Vector3.Transform(Vector3.UnitY, representativeRotation),
|
||||
Vector3.Transform(Vector3.UnitY, expectedRotation),
|
||||
1e-4);
|
||||
AssertVector(
|
||||
Vector3.Transform(Vector3.UnitZ, representativeRotation),
|
||||
Vector3.Transform(Vector3.UnitZ, expectedRotation),
|
||||
1e-4);
|
||||
}
|
||||
|
||||
[TestMethod]
|
||||
public void ShouldIgnoreQuaternionSignWhenAveragingFragments()
|
||||
{
|
||||
Quaternion expectedRotation = Quaternion.Normalize(
|
||||
Quaternion.CreateFromAxisAngle(Vector3.UnitX, (float)(Math.PI / 4.0)));
|
||||
Quaternion oppositeSignRotation = new Quaternion(
|
||||
-expectedRotation.X,
|
||||
-expectedRotation.Y,
|
||||
-expectedRotation.Z,
|
||||
-expectedRotation.W);
|
||||
|
||||
bool ok = FragmentRepresentativePoseHelper.TryGetRepresentativeRotation(
|
||||
new[] { expectedRotation, oppositeSignRotation },
|
||||
out Quaternion representativeRotation);
|
||||
|
||||
Assert.IsTrue(ok);
|
||||
AssertVector(
|
||||
Vector3.Transform(Vector3.UnitY, representativeRotation),
|
||||
Vector3.Transform(Vector3.UnitY, expectedRotation),
|
||||
1e-4);
|
||||
AssertVector(
|
||||
Vector3.Transform(Vector3.UnitZ, representativeRotation),
|
||||
Vector3.Transform(Vector3.UnitZ, expectedRotation),
|
||||
1e-4);
|
||||
}
|
||||
|
||||
[TestMethod]
|
||||
public void ShouldFail_WhenNoFragmentRotationsAreAvailable()
|
||||
{
|
||||
bool ok = FragmentRepresentativePoseHelper.TryGetRepresentativeRotation(
|
||||
Array.Empty<double[]>(),
|
||||
out Quaternion representativeRotation);
|
||||
|
||||
Assert.IsFalse(ok);
|
||||
Assert.AreEqual(Quaternion.Identity, representativeRotation);
|
||||
}
|
||||
|
||||
[TestMethod]
|
||||
public void ShouldPreserveAxes_ForObservedYUpRealObjectFragmentBasis()
|
||||
{
|
||||
Vector3 axisX = new Vector3(1.0f, 0.0f, 0.0f);
|
||||
Vector3 axisY = new Vector3(0.0f, 0.0f, 1.0f);
|
||||
Vector3 axisZ = new Vector3(0.0f, -1.0f, 0.0f);
|
||||
|
||||
double[] fragmentMatrix =
|
||||
{
|
||||
axisX.X, axisX.Y, axisX.Z, 0.0,
|
||||
axisY.X, axisY.Y, axisY.Z, 0.0,
|
||||
axisZ.X, axisZ.Y, axisZ.Z, 0.0,
|
||||
0.0, 0.0, 0.0, 1.0
|
||||
};
|
||||
|
||||
bool ok = FragmentRepresentativePoseHelper.TryExtractRotationFromFragmentMatrix(
|
||||
fragmentMatrix,
|
||||
out Quaternion rotation);
|
||||
|
||||
Assert.IsTrue(ok);
|
||||
AssertVector(Vector3.Transform(Vector3.UnitX, rotation), axisX, 1e-4);
|
||||
AssertVector(Vector3.Transform(Vector3.UnitY, rotation), axisY, 1e-4);
|
||||
AssertVector(Vector3.Transform(Vector3.UnitZ, rotation), axisZ, 1e-4);
|
||||
}
|
||||
|
||||
private static double[] CreateFragmentMatrix(Quaternion rotation, Vector3 translation)
|
||||
{
|
||||
Vector3 axisX = Vector3.Transform(Vector3.UnitX, rotation);
|
||||
Vector3 axisY = Vector3.Transform(Vector3.UnitY, rotation);
|
||||
Vector3 axisZ = Vector3.Transform(Vector3.UnitZ, rotation);
|
||||
|
||||
return new[]
|
||||
{
|
||||
(double)axisX.X, (double)axisX.Y, (double)axisX.Z, 0.0,
|
||||
(double)axisY.X, (double)axisY.Y, (double)axisY.Z, 0.0,
|
||||
(double)axisZ.X, (double)axisZ.Y, (double)axisZ.Z, 0.0,
|
||||
(double)translation.X, (double)translation.Y, (double)translation.Z, 1.0
|
||||
};
|
||||
}
|
||||
|
||||
private static void AssertVector(Vector3 actual, Vector3 expected, double tolerance)
|
||||
{
|
||||
Assert.AreEqual(expected.X, actual.X, tolerance);
|
||||
Assert.AreEqual(expected.Y, actual.Y, tolerance);
|
||||
Assert.AreEqual(expected.Z, actual.Z, tolerance);
|
||||
}
|
||||
}
|
||||
}
|
||||
191
UnitTests/CoordinateSystem/RealObjectPlanarPoseSolverTests.cs
Normal file
191
UnitTests/CoordinateSystem/RealObjectPlanarPoseSolverTests.cs
Normal file
@ -0,0 +1,191 @@
|
||||
using Microsoft.VisualStudio.TestTools.UnitTesting;
|
||||
using NavisworksTransport.Utils.CoordinateSystem;
|
||||
using System;
|
||||
using System.Numerics;
|
||||
using Autodesk.Navisworks.Api;
|
||||
|
||||
namespace NavisworksTransport.UnitTests.CoordinateSystem
|
||||
{
|
||||
[TestClass]
|
||||
public class RealObjectPlanarPoseSolverTests
|
||||
{
|
||||
[TestMethod]
|
||||
public void ShouldChooseClosestCandidateAxis_FromRotatedReferencePose()
|
||||
{
|
||||
Quaternion referenceRotation = Quaternion.Normalize(
|
||||
Quaternion.CreateFromAxisAngle(Vector3.UnitZ, (float)(Math.PI / 6.0)));
|
||||
|
||||
Vector3 desiredForward = new Vector3(1.0f, 0.2f, 0.1f);
|
||||
Vector3 desiredUp = Vector3.UnitZ;
|
||||
|
||||
bool ok = RealObjectPlanarPoseSolver.TryCreatePlanarPoseFromReferencePose(
|
||||
referenceRotation,
|
||||
desiredForward,
|
||||
desiredUp,
|
||||
out RealObjectPlanarPoseSolution solution);
|
||||
|
||||
Assert.IsTrue(ok);
|
||||
AssertVector(solution.SelectedReferenceAxisLocal, 1.0, 0.0, 0.0);
|
||||
AssertVector(solution.ProjectedForward, 0.97590, 0.19518, 0.09759, 1e-4);
|
||||
|
||||
Vector3 transformedSelectedAxis = Vector3.Transform(solution.SelectedReferenceAxisLocal, solution.BaselineRotation);
|
||||
AssertVector(transformedSelectedAxis, solution.ProjectedForward.X, solution.ProjectedForward.Y, solution.ProjectedForward.Z, 1e-4);
|
||||
}
|
||||
|
||||
[TestMethod]
|
||||
public void ShouldPreferNegativeAxisWhenItMatchesForwardBest()
|
||||
{
|
||||
Quaternion referenceRotation = Quaternion.Identity;
|
||||
Vector3 desiredForward = new Vector3(-0.1f, -1.0f, 0.05f);
|
||||
Vector3 desiredUp = Vector3.UnitZ;
|
||||
|
||||
bool ok = RealObjectPlanarPoseSolver.TryCreatePlanarPoseFromReferencePose(
|
||||
referenceRotation,
|
||||
desiredForward,
|
||||
desiredUp,
|
||||
out RealObjectPlanarPoseSolution solution);
|
||||
|
||||
Assert.IsTrue(ok);
|
||||
AssertVector(solution.SelectedReferenceAxisLocal, 0.0, -1.0, 0.0);
|
||||
|
||||
Vector3 transformedSelectedAxis = Vector3.Transform(solution.SelectedReferenceAxisLocal, solution.BaselineRotation);
|
||||
AssertVector(transformedSelectedAxis, solution.ProjectedForward.X, solution.ProjectedForward.Y, solution.ProjectedForward.Z, 1e-4);
|
||||
}
|
||||
|
||||
[TestMethod]
|
||||
public void ShouldRotateWholeReferencePose_NotFlattenItToHostUpPlane()
|
||||
{
|
||||
Quaternion referenceRotation = Quaternion.Identity;
|
||||
Vector3 desiredForward = Vector3.Normalize(new Vector3(1.0f, 1.0f, 1.0f));
|
||||
|
||||
bool ok = RealObjectPlanarPoseSolver.TryCreatePlanarPoseFromReferencePose(
|
||||
referenceRotation,
|
||||
desiredForward,
|
||||
Vector3.UnitY,
|
||||
out RealObjectPlanarPoseSolution solution);
|
||||
|
||||
Assert.IsTrue(ok);
|
||||
AssertVector(solution.SelectedReferenceAxisLocal, 1.0, 0.0, 0.0);
|
||||
|
||||
Vector3 transformedSelectedAxis = Vector3.Transform(solution.SelectedReferenceAxisLocal, solution.BaselineRotation);
|
||||
AssertVector(transformedSelectedAxis, desiredForward.X, desiredForward.Y, desiredForward.Z, 1e-4);
|
||||
}
|
||||
|
||||
[TestMethod]
|
||||
public void ShouldPreserveReferenceOrthogonalityByApplyingSingleRotationDelta()
|
||||
{
|
||||
Quaternion referenceRotation = Quaternion.Normalize(
|
||||
Quaternion.CreateFromYawPitchRoll(
|
||||
(float)(Math.PI / 7.0),
|
||||
(float)(Math.PI / 9.0),
|
||||
(float)(Math.PI / 11.0)));
|
||||
|
||||
Vector3 desiredForward = Vector3.Normalize(new Vector3(-0.3f, 0.8f, 0.52f));
|
||||
|
||||
bool ok = RealObjectPlanarPoseSolver.TryCreatePlanarPoseFromReferencePose(
|
||||
referenceRotation,
|
||||
desiredForward,
|
||||
Vector3.UnitZ,
|
||||
out RealObjectPlanarPoseSolution solution);
|
||||
|
||||
Assert.IsTrue(ok);
|
||||
|
||||
Vector3 transformedSelectedAxis = Vector3.Transform(solution.SelectedReferenceAxisLocal, solution.BaselineRotation);
|
||||
AssertVector(transformedSelectedAxis, desiredForward.X, desiredForward.Y, desiredForward.Z, 1e-4);
|
||||
|
||||
Vector3 transformedReferenceUp = Vector3.Normalize(Vector3.Transform(Vector3.UnitZ, solution.BaselineRotation));
|
||||
Assert.AreEqual(1.0, transformedReferenceUp.Length(), 1e-4);
|
||||
Assert.AreEqual(0.0, Vector3.Dot(transformedSelectedAxis, transformedReferenceUp), 1e-4);
|
||||
}
|
||||
|
||||
[TestMethod]
|
||||
public void YUp_HorizontalRealObject_ToAnotherHorizontalDirection_ShouldKeepVerticalAxis()
|
||||
{
|
||||
Vector3 referenceX = new Vector3(1.0f, 0.0f, 0.0f);
|
||||
Vector3 referenceY = new Vector3(0.0f, 0.0f, 1.0f);
|
||||
Vector3 referenceZ = new Vector3(0.0f, -1.0f, 0.0f);
|
||||
Quaternion referenceRotation = CreateQuaternionFromBasis(referenceX, referenceY, referenceZ);
|
||||
|
||||
Vector3 desiredForward = Vector3.Normalize(new Vector3(-0.8987f, 0.0f, 0.4386f));
|
||||
Vector3 desiredUp = Vector3.UnitY;
|
||||
|
||||
bool ok = RealObjectPlanarPoseSolver.TryCreatePlanarPoseFromReferencePose(
|
||||
referenceRotation,
|
||||
desiredForward,
|
||||
desiredUp,
|
||||
out RealObjectPlanarPoseSolution solution);
|
||||
|
||||
Assert.IsTrue(ok);
|
||||
AssertVector(solution.SelectedReferenceAxisLocal, -1.0, 0.0, 0.0);
|
||||
|
||||
Vector3 transformedForward = Vector3.Normalize(
|
||||
Vector3.Transform(solution.SelectedReferenceAxisLocal, solution.BaselineRotation));
|
||||
Vector3 transformedVertical = Vector3.Normalize(
|
||||
Vector3.Transform(-Vector3.UnitZ, solution.BaselineRotation));
|
||||
|
||||
AssertVector(transformedForward, desiredForward.X, desiredForward.Y, desiredForward.Z, 1e-4);
|
||||
AssertVector(transformedVertical, 0.0, 1.0, 0.0, 1e-4);
|
||||
}
|
||||
|
||||
private static Quaternion CreateQuaternionFromBasis(Vector3 axisX, Vector3 axisY, Vector3 axisZ)
|
||||
{
|
||||
float m00 = axisX.X;
|
||||
float m01 = axisY.X;
|
||||
float m02 = axisZ.X;
|
||||
float m10 = axisX.Y;
|
||||
float m11 = axisY.Y;
|
||||
float m12 = axisZ.Y;
|
||||
float m20 = axisX.Z;
|
||||
float m21 = axisY.Z;
|
||||
float m22 = axisZ.Z;
|
||||
|
||||
float trace = m00 + m11 + m22;
|
||||
float qx;
|
||||
float qy;
|
||||
float qz;
|
||||
float qw;
|
||||
|
||||
if (trace > 0.0f)
|
||||
{
|
||||
float s = (float)Math.Sqrt(trace + 1.0f) * 2.0f;
|
||||
qw = 0.25f * s;
|
||||
qx = (m21 - m12) / s;
|
||||
qy = (m02 - m20) / s;
|
||||
qz = (m10 - m01) / s;
|
||||
}
|
||||
else if (m00 > m11 && m00 > m22)
|
||||
{
|
||||
float s = (float)Math.Sqrt(1.0f + m00 - m11 - m22) * 2.0f;
|
||||
qw = (m21 - m12) / s;
|
||||
qx = 0.25f * s;
|
||||
qy = (m01 + m10) / s;
|
||||
qz = (m02 + m20) / s;
|
||||
}
|
||||
else if (m11 > m22)
|
||||
{
|
||||
float s = (float)Math.Sqrt(1.0f + m11 - m00 - m22) * 2.0f;
|
||||
qw = (m02 - m20) / s;
|
||||
qx = (m01 + m10) / s;
|
||||
qy = 0.25f * s;
|
||||
qz = (m12 + m21) / s;
|
||||
}
|
||||
else
|
||||
{
|
||||
float s = (float)Math.Sqrt(1.0f + m22 - m00 - m11) * 2.0f;
|
||||
qw = (m10 - m01) / s;
|
||||
qx = (m02 + m20) / s;
|
||||
qy = (m12 + m21) / s;
|
||||
qz = 0.25f * s;
|
||||
}
|
||||
|
||||
return Quaternion.Normalize(new Quaternion(qx, qy, qz, qw));
|
||||
}
|
||||
|
||||
private static void AssertVector(Vector3 vector, double x, double y, double z, double tolerance = 1e-6)
|
||||
{
|
||||
Assert.AreEqual(x, vector.X, tolerance);
|
||||
Assert.AreEqual(y, vector.Y, tolerance);
|
||||
Assert.AreEqual(z, vector.Z, tolerance);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -65,16 +65,16 @@ speed_limit_meters_per_second = 0.8
|
||||
width_limit_meters = 3.0
|
||||
|
||||
[coordinate_system]
|
||||
# 坐标系类型
|
||||
# 可选值: "AutoDetect", "ZUp", "YUp"
|
||||
# AutoDetect: 自动检测(推荐)
|
||||
# ZUp: 强制使用 Z-Up 坐标系(Navisworks 默认)
|
||||
# YUp: 强制使用 Y-Up 坐标系(常见于 Revit 导出)
|
||||
type = "AutoDetect"
|
||||
|
||||
# 自定义物流类别
|
||||
# 用于扩展内置类别,只需填写类别名称
|
||||
#
|
||||
# 坐标系类型
|
||||
# 可选值: "AutoDetect", "ZUp", "YUp"
|
||||
# AutoDetect: 自动检测(推荐)
|
||||
# ZUp: 强制使用 Z-Up 坐标系(Navisworks 默认)
|
||||
# YUp: 强制使用 Y-Up 坐标系(常见于 Revit 导出)
|
||||
type = "AutoDetect"
|
||||
|
||||
# 自定义物流类别
|
||||
# 用于扩展内置类别,只需填写类别名称
|
||||
#
|
||||
# 示例:
|
||||
# [[custom_category]]
|
||||
# name = "狭窄通道"
|
||||
|
||||
@ -2,10 +2,13 @@ using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
using System.Numerics;
|
||||
using System.Runtime.InteropServices;
|
||||
using System.Text;
|
||||
using System.Windows;
|
||||
using System.Windows.Threading;
|
||||
using Autodesk.Navisworks.Api;
|
||||
using Autodesk.Navisworks.Api.Clash;
|
||||
using ComApiBridge = Autodesk.Navisworks.Api.ComApi.ComApiBridge;
|
||||
using NavisworksTransport.Core.Config;
|
||||
using NavisworksTransport.Core.Spatial;
|
||||
using NavisworksTransport.Utils;
|
||||
@ -107,6 +110,8 @@ namespace NavisworksTransport.Core.Animation
|
||||
private double _realObjectLength = 0; // 真实物体长度(模型单位,固定物理尺寸)
|
||||
private double _realObjectWidth = 0; // 真实物体宽度(模型单位,固定物理尺寸)
|
||||
private double _realObjectHeight = 0; // 真实物体高度(模型单位,固定物理尺寸)
|
||||
private Quaternion _realObjectReferenceRotation = Quaternion.Identity;
|
||||
private bool _hasRealObjectReferenceRotation = false;
|
||||
private List<Point3D> _pathPoints;
|
||||
private List<ModelItem> _manualCollisionTargets = new List<ModelItem>();
|
||||
private bool _manualCollisionOverrideEnabled = false;
|
||||
@ -474,6 +479,7 @@ namespace NavisworksTransport.Core.Animation
|
||||
_animatedObjectMode = animatedObject == null
|
||||
? AnimatedObjectMode.None
|
||||
: AnimatedObjectMode.RealObject;
|
||||
ResetRealObjectReferenceRotation();
|
||||
|
||||
if (animatedObject != null)
|
||||
{
|
||||
@ -483,6 +489,7 @@ namespace NavisworksTransport.Core.Animation
|
||||
_currentYaw = ModelItemTransformHelper.GetYawFromTransform(_originalTransform);
|
||||
_trackedRotation = _originalTransform.Factor().Rotation;
|
||||
_hasTrackedRotation = true;
|
||||
TryCaptureRealObjectReferenceRotation(animatedObject, out _);
|
||||
}
|
||||
}
|
||||
|
||||
@ -495,6 +502,10 @@ namespace NavisworksTransport.Core.Animation
|
||||
_animatedObjectMode = isVirtualObject
|
||||
? AnimatedObjectMode.VirtualObject
|
||||
: (_animatedObject != null ? AnimatedObjectMode.RealObject : AnimatedObjectMode.None);
|
||||
if (isVirtualObject)
|
||||
{
|
||||
ResetRealObjectReferenceRotation();
|
||||
}
|
||||
_virtualObjectLength = length; // 模型单位
|
||||
_virtualObjectWidth = width; // 模型单位
|
||||
_virtualObjectHeight = height; // 模型单位
|
||||
@ -636,6 +647,7 @@ namespace NavisworksTransport.Core.Animation
|
||||
_animatedObjectMode = isVirtualObject
|
||||
? AnimatedObjectMode.VirtualObject
|
||||
: AnimatedObjectMode.RealObject;
|
||||
ResetRealObjectReferenceRotation();
|
||||
_originalTransform = animatedObject.Transform;
|
||||
_originalCenter = animatedObject.BoundingBox().Center;
|
||||
_trackedPosition = GetTrackedObjectPosition(animatedObject);
|
||||
@ -645,6 +657,11 @@ namespace NavisworksTransport.Core.Animation
|
||||
_trackedRotation = _originalTransform.Factor().Rotation;
|
||||
_hasTrackedRotation = true;
|
||||
|
||||
if (!isVirtualObject)
|
||||
{
|
||||
TryCaptureRealObjectReferenceRotation(animatedObject, out _);
|
||||
}
|
||||
|
||||
LogManager.Info(
|
||||
$"[移动到起点] 更新动画对象内部状态: 对象={animatedObject.DisplayName}, 虚拟物体={IsVirtualObjectMode}");
|
||||
}
|
||||
@ -3571,6 +3588,17 @@ namespace NavisworksTransport.Core.Animation
|
||||
rotation = Rotation3D.Identity;
|
||||
|
||||
var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
|
||||
if (IsRealObjectMode)
|
||||
{
|
||||
Vector3 hostForward = ToNumerics(nextPoint) - ToNumerics(previousPoint);
|
||||
if (hostForward.LengthSquared() < 1e-6f)
|
||||
{
|
||||
hostForward = ToNumerics(nextPoint) - ToNumerics(currentPoint);
|
||||
}
|
||||
|
||||
return TryCreateRealObjectPlanarRotationFromHostForward(hostForward, out rotation);
|
||||
}
|
||||
|
||||
var convention = GetCurrentModelAxisConvention();
|
||||
Vector3 canonicalPrevious = ToNumerics(adapter.ToCanonicalPoint(previousPoint));
|
||||
Vector3 canonicalCurrent = ToNumerics(adapter.ToCanonicalPoint(currentPoint));
|
||||
@ -3582,24 +3610,6 @@ namespace NavisworksTransport.Core.Animation
|
||||
forward = canonicalNext - canonicalCurrent;
|
||||
}
|
||||
|
||||
if (IsRealObjectMode)
|
||||
{
|
||||
if (!CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForward(
|
||||
forward,
|
||||
HostCoordinateAdapter.CanonicalUpVector3,
|
||||
convention,
|
||||
out var baselineQuaternion))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
Matrix4x4 hostLinear = adapter.FromCanonicalLinearTransform(Matrix4x4.CreateFromQuaternion(baselineQuaternion));
|
||||
Quaternion hostBaselineQuaternion = Quaternion.CreateFromRotationMatrix(hostLinear);
|
||||
Quaternion hostComposedQuaternion = adapter.ComposeHostQuaternion(hostBaselineQuaternion, _objectRotationCorrection);
|
||||
rotation = adapter.FromHostQuaternion(hostComposedQuaternion);
|
||||
return true;
|
||||
}
|
||||
|
||||
Quaternion correctionQuaternion = adapter.CreateCanonicalRotationCorrection(_objectRotationCorrection);
|
||||
if (!CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForward(
|
||||
forward,
|
||||
@ -3620,26 +3630,15 @@ namespace NavisworksTransport.Core.Animation
|
||||
rotation = Rotation3D.Identity;
|
||||
|
||||
var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
|
||||
var convention = GetCurrentModelAxisConvention();
|
||||
Vector3 canonicalForward = adapter.ToCanonicalVector3(new Vector3((float)hostForward.X, (float)hostForward.Y, (float)hostForward.Z));
|
||||
if (IsRealObjectMode)
|
||||
{
|
||||
if (!CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForward(
|
||||
canonicalForward,
|
||||
HostCoordinateAdapter.CanonicalUpVector3,
|
||||
convention,
|
||||
out var baselineQuaternion))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
Matrix4x4 hostLinear = adapter.FromCanonicalLinearTransform(Matrix4x4.CreateFromQuaternion(baselineQuaternion));
|
||||
Quaternion hostBaselineQuaternion = Quaternion.CreateFromRotationMatrix(hostLinear);
|
||||
Quaternion hostComposedQuaternion = adapter.ComposeHostQuaternion(hostBaselineQuaternion, _objectRotationCorrection);
|
||||
rotation = adapter.FromHostQuaternion(hostComposedQuaternion);
|
||||
return true;
|
||||
return TryCreateRealObjectPlanarRotationFromHostForward(
|
||||
new Vector3((float)hostForward.X, (float)hostForward.Y, (float)hostForward.Z),
|
||||
out rotation);
|
||||
}
|
||||
|
||||
var convention = GetCurrentModelAxisConvention();
|
||||
Vector3 canonicalForward = adapter.ToCanonicalVector3(new Vector3((float)hostForward.X, (float)hostForward.Y, (float)hostForward.Z));
|
||||
Quaternion correctionQuaternion = adapter.CreateCanonicalRotationCorrection(_objectRotationCorrection);
|
||||
if (!CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForward(
|
||||
canonicalForward,
|
||||
@ -3655,6 +3654,174 @@ namespace NavisworksTransport.Core.Animation
|
||||
return true;
|
||||
}
|
||||
|
||||
private bool TryCreateRealObjectPlanarRotationFromHostForward(Vector3 hostForward, out Rotation3D rotation)
|
||||
{
|
||||
rotation = Rotation3D.Identity;
|
||||
|
||||
if (!TryGetRealObjectReferenceRotation(out var referenceRotation))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
|
||||
if (!RealObjectPlanarPoseSolver.TryCreatePlanarPoseFromReferencePose(
|
||||
referenceRotation,
|
||||
hostForward,
|
||||
adapter.HostUpVector3,
|
||||
out var solution))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
Quaternion hostComposedQuaternion = adapter.ComposeHostQuaternion(solution.BaselineRotation, _objectRotationCorrection);
|
||||
rotation = adapter.FromHostQuaternion(hostComposedQuaternion);
|
||||
|
||||
LogManager.Info(
|
||||
$"[真实物体起点姿态] 选中参考轴=({solution.SelectedReferenceAxisLocal.X:F3},{solution.SelectedReferenceAxisLocal.Y:F3},{solution.SelectedReferenceAxisLocal.Z:F3}), " +
|
||||
$"投影前进=({solution.ProjectedForward.X:F3},{solution.ProjectedForward.Y:F3},{solution.ProjectedForward.Z:F3}), " +
|
||||
$"基姿态已计算");
|
||||
return true;
|
||||
}
|
||||
|
||||
private bool TryGetRealObjectReferenceRotation(out Quaternion referenceRotation)
|
||||
{
|
||||
referenceRotation = Quaternion.Identity;
|
||||
|
||||
if (_hasRealObjectReferenceRotation)
|
||||
{
|
||||
referenceRotation = _realObjectReferenceRotation;
|
||||
return true;
|
||||
}
|
||||
|
||||
if (_originalTransform == null)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
if (TryCaptureRealObjectReferenceRotation(_animatedObject, out referenceRotation))
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
NotifyRealObjectReferencePoseFailure(_animatedObject);
|
||||
return false;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
LogManager.Error($"[真实物体参考姿态] 读取原始参考位姿失败: {ex.Message}");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
private void NotifyRealObjectReferencePoseFailure(ModelItem sourceObject)
|
||||
{
|
||||
string objectName = sourceObject?.DisplayName ?? "未命名对象";
|
||||
string message =
|
||||
$"无法从 fragment 提取真实物体参考姿态,已中止起点姿态计算。\n\n" +
|
||||
$"对象:{objectName}\n" +
|
||||
$"建议:\n" +
|
||||
$"1. 检查该真实物体是否存在可用的 fragment 几何。\n" +
|
||||
$"2. 重新选择该对象后再试。\n" +
|
||||
$"3. 如果是 Revit 导入件,请确认 fragment 结构没有异常简化或丢失。";
|
||||
|
||||
LogManager.Error($"[真实物体参考姿态] {objectName} fragment 代表姿态提取失败,已中止。");
|
||||
DialogHelper.ShowMessageBox(
|
||||
message,
|
||||
"fragment 代表姿态提取失败",
|
||||
MessageBoxButton.OK,
|
||||
MessageBoxImage.Error);
|
||||
}
|
||||
|
||||
private bool TryCaptureRealObjectReferenceRotation(ModelItem sourceObject, out Quaternion referenceRotation)
|
||||
{
|
||||
referenceRotation = Quaternion.Identity;
|
||||
|
||||
if (sourceObject == null)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
var selection = ComApiBridge.ToInwOpSelection(new ModelItemCollection { sourceObject });
|
||||
var fragmentInfos = GeometryHelper.GetAllFragments(selection);
|
||||
try
|
||||
{
|
||||
var fragmentMatrices = fragmentInfos
|
||||
.Where(fragmentInfo => fragmentInfo.TransformMatrix != null && fragmentInfo.TransformMatrix.Length == 16)
|
||||
.Select(fragmentInfo => fragmentInfo.TransformMatrix)
|
||||
.ToList();
|
||||
|
||||
if (fragmentMatrices.Count == 0)
|
||||
{
|
||||
LogManager.Warning($"[真实物体参考姿态] {sourceObject.DisplayName} 未提取到 fragment 代表姿态");
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!FragmentRepresentativePoseHelper.TryGetRepresentativeRotation(fragmentMatrices, out referenceRotation))
|
||||
{
|
||||
LogManager.Warning($"[真实物体参考姿态] {sourceObject.DisplayName} fragment 代表姿态求解失败");
|
||||
return false;
|
||||
}
|
||||
|
||||
_realObjectReferenceRotation = referenceRotation;
|
||||
_hasRealObjectReferenceRotation = true;
|
||||
|
||||
Matrix4x4 linear = Matrix4x4.CreateFromQuaternion(referenceRotation);
|
||||
LogManager.Info(
|
||||
$"[真实物体参考姿态] {sourceObject.DisplayName} 使用 fragment 代表姿态: " +
|
||||
$"X=({linear.M11:F4},{linear.M21:F4},{linear.M31:F4}), " +
|
||||
$"Y=({linear.M12:F4},{linear.M22:F4},{linear.M32:F4}), " +
|
||||
$"Z=({linear.M13:F4},{linear.M23:F4},{linear.M33:F4}), " +
|
||||
$"fragment数量={fragmentMatrices.Count}");
|
||||
return true;
|
||||
}
|
||||
finally
|
||||
{
|
||||
if (fragmentInfos != null)
|
||||
{
|
||||
foreach (var fragmentInfo in fragmentInfos)
|
||||
{
|
||||
if (fragmentInfo?.Fragment != null)
|
||||
{
|
||||
try
|
||||
{
|
||||
Marshal.ReleaseComObject(fragmentInfo.Fragment);
|
||||
}
|
||||
catch
|
||||
{
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (selection != null)
|
||||
{
|
||||
try
|
||||
{
|
||||
Marshal.ReleaseComObject(selection);
|
||||
}
|
||||
catch
|
||||
{
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
LogManager.Warning($"[真实物体参考姿态] 从 fragment 提取代表姿态失败: {ex.Message}");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
private void ResetRealObjectReferenceRotation()
|
||||
{
|
||||
_realObjectReferenceRotation = Quaternion.Identity;
|
||||
_hasRealObjectReferenceRotation = false;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 设置物体绕宿主 X/Y/Z 轴的角度修正
|
||||
/// </summary>
|
||||
|
||||
@ -278,5 +278,6 @@ namespace NavisworksTransport.Core.Config
|
||||
/// YUp: 强制使用 Y-Up 坐标系
|
||||
/// </summary>
|
||||
public string Type { get; set; } = "AutoDetect";
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@ -142,6 +142,22 @@ namespace NavisworksTransport
|
||||
/// 吊装路径样式(紫色)
|
||||
/// </summary>
|
||||
HoistingLine
|
||||
,
|
||||
|
||||
/// <summary>
|
||||
/// 真实物体原始位姿 X 轴样式(红色)
|
||||
/// </summary>
|
||||
TransformAxisX,
|
||||
|
||||
/// <summary>
|
||||
/// 真实物体原始位姿 Y 轴样式(绿色)
|
||||
/// </summary>
|
||||
TransformAxisY,
|
||||
|
||||
/// <summary>
|
||||
/// 真实物体原始位姿 Z 轴样式(蓝色)
|
||||
/// </summary>
|
||||
TransformAxisZ
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@ -2379,6 +2395,15 @@ namespace NavisworksTransport
|
||||
case RenderStyleName.HoistingLine:
|
||||
return new RenderStyle(Color.FromByteRGB(156, 39, 176), 0.8); // Material Purple吊装路径,20%透明
|
||||
|
||||
case RenderStyleName.TransformAxisX:
|
||||
return new RenderStyle(Color.FromByteRGB(244, 67, 54), 0.9); // Material Red
|
||||
|
||||
case RenderStyleName.TransformAxisY:
|
||||
return new RenderStyle(Color.FromByteRGB(76, 175, 80), 0.9); // Material Green
|
||||
|
||||
case RenderStyleName.TransformAxisZ:
|
||||
return new RenderStyle(Color.FromByteRGB(33, 150, 243), 0.9); // Material Blue
|
||||
|
||||
default:
|
||||
return new RenderStyle(Color.White, 1.0); // 默认白色,完全不透明
|
||||
}
|
||||
|
||||
@ -1,9 +1,15 @@
|
||||
using System;
|
||||
using System.Collections.ObjectModel;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
using System.Numerics;
|
||||
using System.Runtime.InteropServices;
|
||||
using System.Text;
|
||||
using System.Threading.Tasks;
|
||||
using System.Windows.Input;
|
||||
using System.Windows.Threading;
|
||||
using Autodesk.Navisworks.Api;
|
||||
using ComApiBridge = Autodesk.Navisworks.Api.ComApi.ComApiBridge;
|
||||
using NavisworksTransport.UI.WPF.Collections;
|
||||
using NavisworksTransport.Core;
|
||||
using NavisworksTransport.Core.Config;
|
||||
@ -21,6 +27,8 @@ namespace NavisworksTransport.UI.WPF.ViewModels
|
||||
#region 私有字段
|
||||
|
||||
private readonly UIStateManager _uiStateManager;
|
||||
private readonly Dictionary<string, string> _documentFragmentDefaultUpAxes = new Dictionary<string, string>(StringComparer.OrdinalIgnoreCase);
|
||||
private readonly HashSet<string> _documentRotationHintShown = new HashSet<string>(StringComparer.OrdinalIgnoreCase);
|
||||
|
||||
// 日志管理字段
|
||||
private ObservableCollection<string> _logLevels;
|
||||
@ -174,10 +182,13 @@ namespace NavisworksTransport.UI.WPF.ViewModels
|
||||
public ICommand TestVoxelPathFindingCommand { get; private set; }
|
||||
public ICommand ReadTransformTestCommand { get; private set; }
|
||||
public ICommand CoordinateSystemExplorerCommand { get; private set; }
|
||||
public ICommand CaptureRealObjectTransformCommand { get; private set; }
|
||||
public ICommand DetectFragmentDefaultUpCommand { get; private set; }
|
||||
|
||||
|
||||
// 坐标系设置
|
||||
public ObservableCollection<string> CoordinateSystemOptions { get; private set; }
|
||||
public ObservableCollection<string> FragmentDefaultUpAxisOptions { get; private set; }
|
||||
|
||||
private string _selectedCoordinateSystem = "AutoDetect";
|
||||
public string SelectedCoordinateSystem
|
||||
@ -199,6 +210,21 @@ namespace NavisworksTransport.UI.WPF.ViewModels
|
||||
set => SetProperty(ref _currentCoordinateSystemInfo, value);
|
||||
}
|
||||
|
||||
private string _selectedFragmentDefaultUpAxis = "Y";
|
||||
public string SelectedFragmentDefaultUpAxis
|
||||
{
|
||||
get => _selectedFragmentDefaultUpAxis;
|
||||
set
|
||||
{
|
||||
if (SetProperty(ref _selectedFragmentDefaultUpAxis, value))
|
||||
{
|
||||
SaveCurrentDocumentFragmentDefaultUpAxis(value);
|
||||
UpdateMainStatus($"当前文档 fragment 默认Up已设置为: {value}");
|
||||
LogManager.Info($"[fragment默认Up] 当前文档设置为: {value}");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region 构造函数
|
||||
@ -311,14 +337,18 @@ namespace NavisworksTransport.UI.WPF.ViewModels
|
||||
TestVoxelPathFindingCommand = new RelayCommand(() => ExecuteTestVoxelPathFinding());
|
||||
ReadTransformTestCommand = new RelayCommand(() => ExecuteReadTransformTest());
|
||||
CoordinateSystemExplorerCommand = new RelayCommand(() => ExecuteCoordinateSystemExplorer());
|
||||
CaptureRealObjectTransformCommand = new RelayCommand(() => ExecuteCaptureRealObjectTransform());
|
||||
DetectFragmentDefaultUpCommand = new RelayCommand(() => ExecuteDetectFragmentDefaultUp());
|
||||
|
||||
|
||||
// 初始化坐标系选项
|
||||
CoordinateSystemOptions = new ObservableCollection<string> { "AutoDetect", "ZUp", "YUp" };
|
||||
FragmentDefaultUpAxisOptions = new ObservableCollection<string> { "Y", "Z" };
|
||||
|
||||
// 从配置加载当前坐标系设置
|
||||
var configType = ConfigManager.Instance.Current.CoordinateSystem?.Type ?? "AutoDetect";
|
||||
_selectedCoordinateSystem = configType;
|
||||
_selectedFragmentDefaultUpAxis = GetCurrentDocumentFragmentDefaultUpAxis();
|
||||
|
||||
// 订阅文档事件(用于自动检测坐标系)
|
||||
SubscribeToDocumentEvents();
|
||||
@ -389,6 +419,7 @@ namespace NavisworksTransport.UI.WPF.ViewModels
|
||||
|
||||
// 更新显示信息
|
||||
UpdateCoordinateSystemInfo();
|
||||
ShowRootModelRotationHintIfNeeded();
|
||||
|
||||
LogManager.Info($"坐标系初始化完成: {configType}");
|
||||
}
|
||||
@ -436,6 +467,156 @@ namespace NavisworksTransport.UI.WPF.ViewModels
|
||||
{
|
||||
var info = CoordinateSystemManager.Instance.GetCurrentInfo();
|
||||
CurrentCoordinateSystemInfo = $"当前坐标系: {info}";
|
||||
RefreshFragmentDefaultUpAxisSelectionFromCurrentDocument();
|
||||
}
|
||||
|
||||
private void RefreshFragmentDefaultUpAxisSelectionFromCurrentDocument()
|
||||
{
|
||||
string resolvedAxis = GetCurrentDocumentFragmentDefaultUpAxis();
|
||||
if (_selectedFragmentDefaultUpAxis != resolvedAxis)
|
||||
{
|
||||
SetProperty(ref _selectedFragmentDefaultUpAxis, resolvedAxis, nameof(SelectedFragmentDefaultUpAxis));
|
||||
}
|
||||
}
|
||||
|
||||
private string GetCurrentDocumentFragmentDefaultUpAxis()
|
||||
{
|
||||
string documentKey = GetCurrentDocumentKey();
|
||||
if (!string.IsNullOrWhiteSpace(documentKey) &&
|
||||
_documentFragmentDefaultUpAxes.TryGetValue(documentKey, out string storedAxis) &&
|
||||
IsSupportedFragmentDefaultUpAxis(storedAxis))
|
||||
{
|
||||
return storedAxis;
|
||||
}
|
||||
|
||||
return GetCurrentHostUpAxis();
|
||||
}
|
||||
|
||||
private void SaveCurrentDocumentFragmentDefaultUpAxis(string axisName)
|
||||
{
|
||||
if (!IsSupportedFragmentDefaultUpAxis(axisName))
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
string documentKey = GetCurrentDocumentKey();
|
||||
if (string.IsNullOrWhiteSpace(documentKey))
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
_documentFragmentDefaultUpAxes[documentKey] = axisName;
|
||||
}
|
||||
|
||||
private string GetCurrentDocumentKey()
|
||||
{
|
||||
var doc = Autodesk.Navisworks.Api.Application.ActiveDocument;
|
||||
if (doc == null || doc.IsClear)
|
||||
{
|
||||
return string.Empty;
|
||||
}
|
||||
|
||||
if (!string.IsNullOrWhiteSpace(doc.FileName))
|
||||
{
|
||||
return doc.FileName;
|
||||
}
|
||||
|
||||
return doc.Title ?? string.Empty;
|
||||
}
|
||||
|
||||
private string GetCurrentHostUpAxis()
|
||||
{
|
||||
var doc = Autodesk.Navisworks.Api.Application.ActiveDocument;
|
||||
if (doc != null && !doc.IsClear)
|
||||
{
|
||||
UnitVector3D worldUp = doc.CurrentViewpoint.Value.WorldUpVector;
|
||||
if (!worldUp.IsZero)
|
||||
{
|
||||
return Math.Abs(worldUp.Y) >= Math.Abs(worldUp.Z) ? "Y" : "Z";
|
||||
}
|
||||
|
||||
Vector3D documentUp = doc.UpVector;
|
||||
if (!documentUp.IsZero)
|
||||
{
|
||||
return Math.Abs(documentUp.Y) >= Math.Abs(documentUp.Z) ? "Y" : "Z";
|
||||
}
|
||||
}
|
||||
|
||||
if (string.Equals(_selectedCoordinateSystem, "YUp", StringComparison.OrdinalIgnoreCase))
|
||||
{
|
||||
return "Y";
|
||||
}
|
||||
|
||||
return "Z";
|
||||
}
|
||||
|
||||
private static bool IsSupportedFragmentDefaultUpAxis(string axisName)
|
||||
{
|
||||
return string.Equals(axisName, "Y", StringComparison.OrdinalIgnoreCase) ||
|
||||
string.Equals(axisName, "Z", StringComparison.OrdinalIgnoreCase);
|
||||
}
|
||||
|
||||
private void ShowRootModelRotationHintIfNeeded()
|
||||
{
|
||||
string documentKey = GetCurrentDocumentKey();
|
||||
if (string.IsNullOrWhiteSpace(documentKey) || _documentRotationHintShown.Contains(documentKey))
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
var doc = Autodesk.Navisworks.Api.Application.ActiveDocument;
|
||||
if (doc == null || doc.IsClear || doc.Models == null || doc.Models.Count == 0)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if (!TryGetRootModelRotationHint(doc, out string hintMessage))
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
_documentRotationHintShown.Add(documentKey);
|
||||
LogManager.Info($"[模型整体旋转提示] {hintMessage}");
|
||||
System.Windows.MessageBox.Show(
|
||||
hintMessage,
|
||||
"坐标系提示",
|
||||
System.Windows.MessageBoxButton.OK,
|
||||
System.Windows.MessageBoxImage.Information);
|
||||
}
|
||||
|
||||
private static bool TryGetRootModelRotationHint(Document doc, out string hintMessage)
|
||||
{
|
||||
hintMessage = null;
|
||||
if (doc == null || doc.Models == null || doc.Models.Count == 0)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
const double minAngleDegrees = 1.0;
|
||||
|
||||
for (int i = 0; i < doc.Models.Count; i++)
|
||||
{
|
||||
Model model = doc.Models[i];
|
||||
ModelItem rootItem = model?.RootItem;
|
||||
if (rootItem == null)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
Transform3DComponents components = rootItem.Transform.Factor();
|
||||
var axisAngle = components.Rotation.ToAxisAndAngle();
|
||||
double angleDegrees = Math.Abs(axisAngle.Angle) * 180.0 / Math.PI;
|
||||
if (angleDegrees < minAngleDegrees)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
hintMessage =
|
||||
$"检测到根模型存在整体旋转(约 {angleDegrees:F1}°),模型可能经过坐标系转换;如后续真实物体姿态异常,请检查 Fragment默认Up 设置。";
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
#region 文档事件处理
|
||||
@ -566,6 +747,11 @@ namespace NavisworksTransport.UI.WPF.ViewModels
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
_ = _uiStateManager.ExecuteUIUpdateAsync(() =>
|
||||
{
|
||||
RefreshFragmentDefaultUpAxisSelectionFromCurrentDocument();
|
||||
});
|
||||
}
|
||||
|
||||
#endregion
|
||||
@ -1315,6 +1501,541 @@ namespace NavisworksTransport.UI.WPF.ViewModels
|
||||
}, "体素路径规划测试");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 捕获选中的真实物体,显示原始变换信息并可视化三轴。
|
||||
/// </summary>
|
||||
private void ExecuteCaptureRealObjectTransform()
|
||||
{
|
||||
SafeExecute(() =>
|
||||
{
|
||||
var doc = Autodesk.Navisworks.Api.Application.ActiveDocument;
|
||||
if (doc == null || doc.IsClear)
|
||||
{
|
||||
System.Windows.MessageBox.Show(
|
||||
"没有活动的文档!请先打开一个模型。",
|
||||
"错误",
|
||||
System.Windows.MessageBoxButton.OK,
|
||||
System.Windows.MessageBoxImage.Error);
|
||||
return;
|
||||
}
|
||||
|
||||
var selection = doc.CurrentSelection.SelectedItems;
|
||||
if (selection.Count == 0)
|
||||
{
|
||||
ClearCapturedRealObjectTransformVisualization();
|
||||
System.Windows.MessageBox.Show(
|
||||
"请先选择一个真实物体!",
|
||||
"提示",
|
||||
System.Windows.MessageBoxButton.OK,
|
||||
System.Windows.MessageBoxImage.Information);
|
||||
return;
|
||||
}
|
||||
|
||||
ModelItem item = selection[0];
|
||||
BoundingBox3D bounds = item.BoundingBox();
|
||||
Point3D boundsCenter = bounds.Center;
|
||||
TransformDiagnosticInfo selectedInfo = CreateTransformDiagnosticInfo(item, "选中节点");
|
||||
List<ModelItem> geometryDescendants = CollectGeometryDescendants(item);
|
||||
TransformDiagnosticInfo geometryInfo = geometryDescendants.Count > 0
|
||||
? CreateTransformDiagnosticInfo(geometryDescendants[0], "首个几何后代")
|
||||
: null;
|
||||
FragmentOrientationAnalysis fragmentAnalysis = AnalyzeFragmentOrientations(item);
|
||||
|
||||
Quaternion visualizationQuaternion = fragmentAnalysis != null && fragmentAnalysis.HasRepresentativeOrientation
|
||||
? fragmentAnalysis.RepresentativeRotation
|
||||
: geometryInfo?.RotationQuaternion ?? selectedInfo.RotationQuaternion;
|
||||
|
||||
double axisLength = GetTransformAxisVisualizationLength(bounds);
|
||||
RenderCapturedRealObjectTransformAxes(boundsCenter, visualizationQuaternion, axisLength);
|
||||
|
||||
var info = new StringBuilder();
|
||||
info.AppendLine("=== 真实物体原始变换信息 ===");
|
||||
info.AppendLine();
|
||||
info.AppendLine($"对象: {item.DisplayName}");
|
||||
info.AppendLine($"ClassName: {item.ClassName}");
|
||||
info.AppendLine($"HasGeometry: {item.HasGeometry}");
|
||||
info.AppendLine();
|
||||
info.AppendLine("【包围盒】");
|
||||
info.AppendLine($"Min = ({bounds.Min.X:F3}, {bounds.Min.Y:F3}, {bounds.Min.Z:F3})");
|
||||
info.AppendLine($"Max = ({bounds.Max.X:F3}, {bounds.Max.Y:F3}, {bounds.Max.Z:F3})");
|
||||
info.AppendLine($"Center = ({boundsCenter.X:F3}, {boundsCenter.Y:F3}, {boundsCenter.Z:F3})");
|
||||
info.AppendLine();
|
||||
|
||||
AppendTransformDiagnosticInfo(info, selectedInfo);
|
||||
|
||||
info.AppendLine("【几何后代】");
|
||||
info.AppendLine($"几何后代数量 = {geometryDescendants.Count}");
|
||||
if (geometryInfo != null)
|
||||
{
|
||||
AppendTransformDiagnosticInfo(info, geometryInfo);
|
||||
}
|
||||
else
|
||||
{
|
||||
info.AppendLine("未找到 HasGeometry=True 的后代节点");
|
||||
info.AppendLine();
|
||||
}
|
||||
|
||||
AppendFragmentAnalysis(info, fragmentAnalysis);
|
||||
info.AppendLine();
|
||||
info.AppendLine("【可视化说明】");
|
||||
info.AppendLine($"三轴原点使用包围盒中心: ({boundsCenter.X:F3}, {boundsCenter.Y:F3}, {boundsCenter.Z:F3})");
|
||||
info.AppendLine($"轴长度 = {axisLength:F3}");
|
||||
info.AppendLine($"三轴方向来源 = {(fragmentAnalysis != null && fragmentAnalysis.HasRepresentativeOrientation ? "fragment代表姿态" : geometryInfo != null ? "首个几何后代Transform" : "选中节点Transform")}");
|
||||
info.AppendLine("红色=X轴,绿色=Y轴,蓝色=Z轴");
|
||||
|
||||
string message = info.ToString();
|
||||
LogManager.Info("[真实物体原始位姿]\n" + message);
|
||||
|
||||
var resultDialog = new NavisworksTransport.UI.WPF.Views.CoordinateSystemResultDialog
|
||||
{
|
||||
Title = "真实物体原始变换信息",
|
||||
ResultText = message
|
||||
};
|
||||
DialogHelper.SetOwnerSafely(resultDialog);
|
||||
resultDialog.ShowDialog();
|
||||
|
||||
UpdateMainStatus($"已捕获真实物体原始位姿:{item.DisplayName}");
|
||||
}, "捕获真实物体原始位姿");
|
||||
}
|
||||
|
||||
private void ExecuteDetectFragmentDefaultUp()
|
||||
{
|
||||
SafeExecute(() =>
|
||||
{
|
||||
var doc = Autodesk.Navisworks.Api.Application.ActiveDocument;
|
||||
if (doc == null || doc.IsClear)
|
||||
{
|
||||
System.Windows.MessageBox.Show(
|
||||
"没有活动的文档!请先打开一个模型。",
|
||||
"错误",
|
||||
System.Windows.MessageBoxButton.OK,
|
||||
System.Windows.MessageBoxImage.Error);
|
||||
return;
|
||||
}
|
||||
|
||||
var selection = doc.CurrentSelection.SelectedItems;
|
||||
if (selection.Count == 0)
|
||||
{
|
||||
System.Windows.MessageBox.Show(
|
||||
"请先选择一个明显水平放置的真实物体,再执行 fragment 默认Up检测。",
|
||||
"提示",
|
||||
System.Windows.MessageBoxButton.OK,
|
||||
System.Windows.MessageBoxImage.Information);
|
||||
return;
|
||||
}
|
||||
|
||||
ModelItem item = selection[0];
|
||||
FragmentOrientationAnalysis fragmentAnalysis = AnalyzeFragmentOrientations(item);
|
||||
if (fragmentAnalysis == null || !fragmentAnalysis.HasRepresentativeOrientation)
|
||||
{
|
||||
System.Windows.MessageBox.Show(
|
||||
"未能读取选中物体的 fragment 代表姿态,无法检测默认Up。",
|
||||
"提示",
|
||||
System.Windows.MessageBoxButton.OK,
|
||||
System.Windows.MessageBoxImage.Warning);
|
||||
return;
|
||||
}
|
||||
|
||||
string hostUpAxis = GetCurrentHostUpAxis();
|
||||
Vector3 hostUpVector = hostUpAxis == "Y" ? Vector3.UnitY : Vector3.UnitZ;
|
||||
double yAlignment = Math.Abs(Vector3.Dot(fragmentAnalysis.RepresentativeYAxis, hostUpVector));
|
||||
double zAlignment = Math.Abs(Vector3.Dot(fragmentAnalysis.RepresentativeZAxis, hostUpVector));
|
||||
string detectedAxis = zAlignment > yAlignment ? "Z" : "Y";
|
||||
string currentAxis = SelectedFragmentDefaultUpAxis;
|
||||
LogManager.Info(
|
||||
$"[fragment默认Up检测] 对象={item.DisplayName}, 宿主Up={hostUpAxis}, 当前值={currentAxis}, 检测值={detectedAxis}, " +
|
||||
$"Y对齐度={yAlignment:F4}, Z对齐度={zAlignment:F4}");
|
||||
|
||||
if (string.Equals(detectedAxis, currentAxis, StringComparison.OrdinalIgnoreCase))
|
||||
{
|
||||
System.Windows.MessageBox.Show(
|
||||
$"检测完成:当前文档 Fragment默认Up 已是 {currentAxis}。",
|
||||
"Fragment默认Up检测",
|
||||
System.Windows.MessageBoxButton.OK,
|
||||
System.Windows.MessageBoxImage.Information);
|
||||
UpdateMainStatus($"Fragment默认Up检测完成:当前值 {currentAxis} 已匹配");
|
||||
return;
|
||||
}
|
||||
|
||||
SetProperty(ref _selectedFragmentDefaultUpAxis, detectedAxis, nameof(SelectedFragmentDefaultUpAxis));
|
||||
SaveCurrentDocumentFragmentDefaultUpAxis(detectedAxis);
|
||||
|
||||
System.Windows.MessageBox.Show(
|
||||
$"检测完成:当前文档 Fragment默认Up 已自动改为 {detectedAxis}。",
|
||||
"Fragment默认Up检测",
|
||||
System.Windows.MessageBoxButton.OK,
|
||||
System.Windows.MessageBoxImage.Information);
|
||||
|
||||
UpdateMainStatus($"当前文档 fragment 默认Up已自动更新为: {detectedAxis}");
|
||||
LogManager.Info($"[fragment默认Up] 检测后自动更新为: {detectedAxis}");
|
||||
}, "检测Fragment默认Up");
|
||||
}
|
||||
|
||||
private void ClearCapturedRealObjectTransformVisualization()
|
||||
{
|
||||
var renderPlugin = PathPointRenderPlugin.Instance;
|
||||
if (renderPlugin == null)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
renderPlugin.ClearRailBaseline("system_transform_axis_x");
|
||||
renderPlugin.ClearRailBaseline("system_transform_axis_y");
|
||||
renderPlugin.ClearRailBaseline("system_transform_axis_z");
|
||||
}
|
||||
|
||||
private void RenderCapturedRealObjectTransformAxes(Point3D origin, Quaternion rotationQuaternion, double axisLength)
|
||||
{
|
||||
var renderPlugin = PathPointRenderPlugin.Instance;
|
||||
if (renderPlugin == null)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
ClearCapturedRealObjectTransformVisualization();
|
||||
|
||||
RenderTransformAxis(renderPlugin, "system_transform_axis_x", origin, Vector3.Transform(Vector3.UnitX, rotationQuaternion), axisLength, RenderStyleName.TransformAxisX);
|
||||
RenderTransformAxis(renderPlugin, "system_transform_axis_y", origin, Vector3.Transform(Vector3.UnitY, rotationQuaternion), axisLength, RenderStyleName.TransformAxisY);
|
||||
RenderTransformAxis(renderPlugin, "system_transform_axis_z", origin, Vector3.Transform(Vector3.UnitZ, rotationQuaternion), axisLength, RenderStyleName.TransformAxisZ);
|
||||
}
|
||||
|
||||
private static void RenderTransformAxis(
|
||||
PathPointRenderPlugin renderPlugin,
|
||||
string pathId,
|
||||
Point3D origin,
|
||||
Vector3 direction,
|
||||
double axisLength,
|
||||
RenderStyleName renderStyleName)
|
||||
{
|
||||
if (direction.LengthSquared() < 1e-8f)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
Vector3 normalizedDirection = Vector3.Normalize(direction);
|
||||
Point3D endPoint = new Point3D(
|
||||
origin.X + normalizedDirection.X * (float)axisLength,
|
||||
origin.Y + normalizedDirection.Y * (float)axisLength,
|
||||
origin.Z + normalizedDirection.Z * (float)axisLength);
|
||||
|
||||
renderPlugin.RenderRailBaseline(
|
||||
pathId,
|
||||
new System.Collections.Generic.List<Point3D> { origin, endPoint },
|
||||
renderStyleName);
|
||||
}
|
||||
|
||||
private static double GetTransformAxisVisualizationLength(BoundingBox3D bounds)
|
||||
{
|
||||
double xSpan = bounds.Max.X - bounds.Min.X;
|
||||
double ySpan = bounds.Max.Y - bounds.Min.Y;
|
||||
double zSpan = bounds.Max.Z - bounds.Min.Z;
|
||||
double maxSpan = Math.Max(xSpan, Math.Max(ySpan, zSpan));
|
||||
double minLength = UnitsConverter.ConvertFromMeters(0.5);
|
||||
return Math.Max(minLength, maxSpan * 0.3);
|
||||
}
|
||||
|
||||
private static Quaternion ToQuaternion(Rotation3D rotation)
|
||||
{
|
||||
var axisAngle = rotation.ToAxisAndAngle();
|
||||
if (axisAngle.Axis.IsZero || Math.Abs(axisAngle.Angle) < 1e-9)
|
||||
{
|
||||
return Quaternion.Identity;
|
||||
}
|
||||
|
||||
return Quaternion.Normalize(Quaternion.CreateFromAxisAngle(
|
||||
new Vector3((float)axisAngle.Axis.X, (float)axisAngle.Axis.Y, (float)axisAngle.Axis.Z),
|
||||
(float)axisAngle.Angle));
|
||||
}
|
||||
|
||||
private static TransformDiagnosticInfo CreateTransformDiagnosticInfo(ModelItem item, string label)
|
||||
{
|
||||
Transform3DComponents components = item.Transform.Factor();
|
||||
var axisAngle = components.Rotation.ToAxisAndAngle();
|
||||
Quaternion rotationQuaternion = ToQuaternion(components.Rotation);
|
||||
|
||||
return new TransformDiagnosticInfo
|
||||
{
|
||||
Label = label,
|
||||
DisplayName = item.DisplayName,
|
||||
ClassName = item.ClassName,
|
||||
HasGeometry = item.HasGeometry,
|
||||
Translation = components.Translation,
|
||||
Scale = components.Scale,
|
||||
RotationAxis = axisAngle.Axis,
|
||||
RotationAngleRadians = axisAngle.Angle,
|
||||
RotationQuaternion = rotationQuaternion,
|
||||
HostXAxis = Vector3.Normalize(Vector3.Transform(Vector3.UnitX, rotationQuaternion)),
|
||||
HostYAxis = Vector3.Normalize(Vector3.Transform(Vector3.UnitY, rotationQuaternion)),
|
||||
HostZAxis = Vector3.Normalize(Vector3.Transform(Vector3.UnitZ, rotationQuaternion))
|
||||
};
|
||||
}
|
||||
|
||||
private static void AppendTransformDiagnosticInfo(StringBuilder info, TransformDiagnosticInfo diagnostic)
|
||||
{
|
||||
info.AppendLine($"【{diagnostic.Label}】");
|
||||
info.AppendLine($"对象 = {diagnostic.DisplayName}");
|
||||
info.AppendLine($"ClassName = {diagnostic.ClassName}");
|
||||
info.AppendLine($"HasGeometry = {diagnostic.HasGeometry}");
|
||||
info.AppendLine($"Translation = ({diagnostic.Translation.X:F3}, {diagnostic.Translation.Y:F3}, {diagnostic.Translation.Z:F3})");
|
||||
info.AppendLine($"Scale = ({diagnostic.Scale.X:F3}, {diagnostic.Scale.Y:F3}, {diagnostic.Scale.Z:F3})");
|
||||
info.AppendLine($"Rotation.Axis = ({diagnostic.RotationAxis.X:F4}, {diagnostic.RotationAxis.Y:F4}, {diagnostic.RotationAxis.Z:F4})");
|
||||
info.AppendLine($"Rotation.Angle = {diagnostic.RotationAngleRadians:F6} rad ({diagnostic.RotationAngleRadians * 180.0 / Math.PI:F2}°)");
|
||||
info.AppendLine($"X轴 = ({diagnostic.HostXAxis.X:F4}, {diagnostic.HostXAxis.Y:F4}, {diagnostic.HostXAxis.Z:F4})");
|
||||
info.AppendLine($"Y轴 = ({diagnostic.HostYAxis.X:F4}, {diagnostic.HostYAxis.Y:F4}, {diagnostic.HostYAxis.Z:F4})");
|
||||
info.AppendLine($"Z轴 = ({diagnostic.HostZAxis.X:F4}, {diagnostic.HostZAxis.Y:F4}, {diagnostic.HostZAxis.Z:F4})");
|
||||
info.AppendLine();
|
||||
}
|
||||
|
||||
private static List<ModelItem> CollectGeometryDescendants(ModelItem root)
|
||||
{
|
||||
var result = new List<ModelItem>();
|
||||
if (root == null)
|
||||
{
|
||||
return result;
|
||||
}
|
||||
|
||||
CollectGeometryDescendantsRecursive(root, result);
|
||||
return result;
|
||||
}
|
||||
|
||||
private static void CollectGeometryDescendantsRecursive(ModelItem item, List<ModelItem> result)
|
||||
{
|
||||
if (item == null)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
if (item.HasGeometry)
|
||||
{
|
||||
result.Add(item);
|
||||
}
|
||||
|
||||
foreach (ModelItem child in item.Children)
|
||||
{
|
||||
CollectGeometryDescendantsRecursive(child, result);
|
||||
}
|
||||
}
|
||||
|
||||
private static FragmentOrientationAnalysis AnalyzeFragmentOrientations(ModelItem item)
|
||||
{
|
||||
if (item == null)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
Autodesk.Navisworks.Api.ModelItemCollection collection = new Autodesk.Navisworks.Api.ModelItemCollection { item };
|
||||
var comSelection = ComApiBridge.ToInwOpSelection(collection);
|
||||
List<FragmentInfo> fragments = null;
|
||||
try
|
||||
{
|
||||
fragments = GeometryHelper.GetAllFragments(comSelection);
|
||||
var validMatrices = fragments
|
||||
.Where(f => f.TransformMatrix != null && f.TransformMatrix.Length == 16)
|
||||
.Select(f => f.TransformMatrix)
|
||||
.ToList();
|
||||
|
||||
var analysis = new FragmentOrientationAnalysis
|
||||
{
|
||||
FragmentCount = fragments.Count,
|
||||
ValidTransformCount = validMatrices.Count
|
||||
};
|
||||
|
||||
if (validMatrices.Count == 0)
|
||||
{
|
||||
return analysis;
|
||||
}
|
||||
|
||||
Vector3 referenceX = NormalizeOrUnitX(GetMatrixAxis(validMatrices[0], 0));
|
||||
Vector3 referenceY = NormalizeOrUnitY(GetMatrixAxis(validMatrices[0], 1));
|
||||
Vector3 referenceZ = NormalizeOrUnitZ(GetMatrixAxis(validMatrices[0], 2));
|
||||
|
||||
Vector3 sumX = referenceX;
|
||||
Vector3 sumY = referenceY;
|
||||
Vector3 sumZ = referenceZ;
|
||||
double xDotSum = 1.0;
|
||||
double yDotSum = 1.0;
|
||||
double zDotSum = 1.0;
|
||||
|
||||
for (int i = 1; i < validMatrices.Count; i++)
|
||||
{
|
||||
Vector3 axisX = NormalizeWithReference(GetMatrixAxis(validMatrices[i], 0), referenceX);
|
||||
Vector3 axisY = NormalizeWithReference(GetMatrixAxis(validMatrices[i], 1), referenceY);
|
||||
Vector3 axisZ = NormalizeWithReference(GetMatrixAxis(validMatrices[i], 2), referenceZ);
|
||||
|
||||
xDotSum += Math.Abs(Vector3.Dot(axisX, referenceX));
|
||||
yDotSum += Math.Abs(Vector3.Dot(axisY, referenceY));
|
||||
zDotSum += Math.Abs(Vector3.Dot(axisZ, referenceZ));
|
||||
|
||||
sumX += axisX;
|
||||
sumY += axisY;
|
||||
sumZ += axisZ;
|
||||
}
|
||||
|
||||
Vector3 representativeX = NormalizeOrUnitX(sumX);
|
||||
Vector3 representativeY = sumY - Vector3.Dot(sumY, representativeX) * representativeX;
|
||||
representativeY = NormalizeOrUnitY(representativeY);
|
||||
Vector3 representativeZ = Vector3.Normalize(Vector3.Cross(representativeX, representativeY));
|
||||
if (Vector3.Dot(representativeZ, NormalizeOrUnitZ(sumZ)) < 0)
|
||||
{
|
||||
representativeZ = -representativeZ;
|
||||
}
|
||||
representativeY = Vector3.Normalize(Vector3.Cross(representativeZ, representativeX));
|
||||
|
||||
Matrix4x4 linear = new Matrix4x4(
|
||||
representativeX.X, representativeY.X, representativeZ.X, 0f,
|
||||
representativeX.Y, representativeY.Y, representativeZ.Y, 0f,
|
||||
representativeX.Z, representativeY.Z, representativeZ.Z, 0f,
|
||||
0f, 0f, 0f, 1f);
|
||||
|
||||
analysis.HasRepresentativeOrientation = true;
|
||||
analysis.RepresentativeRotation = Quaternion.Normalize(Quaternion.CreateFromRotationMatrix(linear));
|
||||
analysis.RepresentativeXAxis = representativeX;
|
||||
analysis.RepresentativeYAxis = representativeY;
|
||||
analysis.RepresentativeZAxis = representativeZ;
|
||||
analysis.AverageXAxisConsistency = xDotSum / validMatrices.Count;
|
||||
analysis.AverageYAxisConsistency = yDotSum / validMatrices.Count;
|
||||
analysis.AverageZAxisConsistency = zDotSum / validMatrices.Count;
|
||||
analysis.SampleTranslation = GetMatrixTranslation(validMatrices[0]);
|
||||
return analysis;
|
||||
}
|
||||
finally
|
||||
{
|
||||
if (fragments != null)
|
||||
{
|
||||
foreach (var fragment in fragments)
|
||||
{
|
||||
if (fragment?.Fragment != null)
|
||||
{
|
||||
try
|
||||
{
|
||||
Marshal.ReleaseComObject(fragment.Fragment);
|
||||
}
|
||||
catch
|
||||
{
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (comSelection != null)
|
||||
{
|
||||
try
|
||||
{
|
||||
Marshal.ReleaseComObject(comSelection);
|
||||
}
|
||||
catch
|
||||
{
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private static void AppendFragmentAnalysis(StringBuilder info, FragmentOrientationAnalysis analysis)
|
||||
{
|
||||
info.AppendLine("【Fragment 姿态分析】");
|
||||
if (analysis == null)
|
||||
{
|
||||
info.AppendLine("未能执行 fragment 分析");
|
||||
info.AppendLine();
|
||||
return;
|
||||
}
|
||||
|
||||
info.AppendLine($"Fragment 总数 = {analysis.FragmentCount}");
|
||||
info.AppendLine($"有效变换矩阵数 = {analysis.ValidTransformCount}");
|
||||
if (!analysis.HasRepresentativeOrientation)
|
||||
{
|
||||
info.AppendLine("未得到可用的 fragment 代表姿态");
|
||||
info.AppendLine();
|
||||
return;
|
||||
}
|
||||
|
||||
info.AppendLine($"示例平移 = ({analysis.SampleTranslation.X:F3}, {analysis.SampleTranslation.Y:F3}, {analysis.SampleTranslation.Z:F3})");
|
||||
info.AppendLine($"代表X轴 = ({analysis.RepresentativeXAxis.X:F4}, {analysis.RepresentativeXAxis.Y:F4}, {analysis.RepresentativeXAxis.Z:F4})");
|
||||
info.AppendLine($"代表Y轴 = ({analysis.RepresentativeYAxis.X:F4}, {analysis.RepresentativeYAxis.Y:F4}, {analysis.RepresentativeYAxis.Z:F4})");
|
||||
info.AppendLine($"代表Z轴 = ({analysis.RepresentativeZAxis.X:F4}, {analysis.RepresentativeZAxis.Y:F4}, {analysis.RepresentativeZAxis.Z:F4})");
|
||||
info.AppendLine($"X轴一致性 = {analysis.AverageXAxisConsistency:F4}");
|
||||
info.AppendLine($"Y轴一致性 = {analysis.AverageYAxisConsistency:F4}");
|
||||
info.AppendLine($"Z轴一致性 = {analysis.AverageZAxisConsistency:F4}");
|
||||
info.AppendLine();
|
||||
}
|
||||
|
||||
private static Vector3 GetMatrixAxis(double[] matrix, int axisIndex)
|
||||
{
|
||||
switch (axisIndex)
|
||||
{
|
||||
case 0:
|
||||
return new Vector3((float)matrix[0], (float)matrix[1], (float)matrix[2]);
|
||||
case 1:
|
||||
return new Vector3((float)matrix[4], (float)matrix[5], (float)matrix[6]);
|
||||
case 2:
|
||||
return new Vector3((float)matrix[8], (float)matrix[9], (float)matrix[10]);
|
||||
default:
|
||||
throw new ArgumentOutOfRangeException(nameof(axisIndex));
|
||||
}
|
||||
}
|
||||
|
||||
private static Vector3 GetMatrixTranslation(double[] matrix)
|
||||
{
|
||||
return new Vector3((float)matrix[12], (float)matrix[13], (float)matrix[14]);
|
||||
}
|
||||
|
||||
private static Vector3 NormalizeWithReference(Vector3 axis, Vector3 reference)
|
||||
{
|
||||
Vector3 normalized = NormalizeOrFallback(axis, reference);
|
||||
return Vector3.Dot(normalized, reference) < 0 ? -normalized : normalized;
|
||||
}
|
||||
|
||||
private static Vector3 NormalizeOrUnitX(Vector3 value)
|
||||
{
|
||||
return NormalizeOrFallback(value, Vector3.UnitX);
|
||||
}
|
||||
|
||||
private static Vector3 NormalizeOrUnitY(Vector3 value)
|
||||
{
|
||||
return NormalizeOrFallback(value, Vector3.UnitY);
|
||||
}
|
||||
|
||||
private static Vector3 NormalizeOrUnitZ(Vector3 value)
|
||||
{
|
||||
return NormalizeOrFallback(value, Vector3.UnitZ);
|
||||
}
|
||||
|
||||
private static Vector3 NormalizeOrFallback(Vector3 value, Vector3 fallback)
|
||||
{
|
||||
return value.LengthSquared() < 1e-8f ? fallback : Vector3.Normalize(value);
|
||||
}
|
||||
|
||||
private sealed class TransformDiagnosticInfo
|
||||
{
|
||||
public string Label { get; set; }
|
||||
public string DisplayName { get; set; }
|
||||
public string ClassName { get; set; }
|
||||
public bool HasGeometry { get; set; }
|
||||
public Vector3D Translation { get; set; }
|
||||
public Vector3D Scale { get; set; }
|
||||
public Vector3D RotationAxis { get; set; }
|
||||
public double RotationAngleRadians { get; set; }
|
||||
public Quaternion RotationQuaternion { get; set; }
|
||||
public Vector3 HostXAxis { get; set; }
|
||||
public Vector3 HostYAxis { get; set; }
|
||||
public Vector3 HostZAxis { get; set; }
|
||||
}
|
||||
|
||||
private sealed class FragmentOrientationAnalysis
|
||||
{
|
||||
public int FragmentCount { get; set; }
|
||||
public int ValidTransformCount { get; set; }
|
||||
public bool HasRepresentativeOrientation { get; set; }
|
||||
public Quaternion RepresentativeRotation { get; set; }
|
||||
public Vector3 RepresentativeXAxis { get; set; }
|
||||
public Vector3 RepresentativeYAxis { get; set; }
|
||||
public Vector3 RepresentativeZAxis { get; set; }
|
||||
public double AverageXAxisConsistency { get; set; }
|
||||
public double AverageYAxisConsistency { get; set; }
|
||||
public double AverageZAxisConsistency { get; set; }
|
||||
public Vector3 SampleTranslation { get; set; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 读取选中对象的Transform信息,并测试旋转
|
||||
/// </summary>
|
||||
@ -1864,4 +2585,4 @@ namespace NavisworksTransport.UI.WPF.ViewModels
|
||||
|
||||
#endregion
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -178,6 +178,29 @@ NavisworksTransport 系统管理页签视图 - 采用与其他页签一致的Nav
|
||||
ToolTip="AutoDetect: 自动检测, ZUp: Z轴向上, YUp: Y轴向上"/>
|
||||
</Grid>
|
||||
|
||||
<StackPanel Orientation="Horizontal" Margin="0,0,0,8">
|
||||
<Button Content="坐标系检测"
|
||||
Command="{Binding CoordinateSystemExplorerCommand}"
|
||||
Style="{StaticResource ActionButtonStyle}"
|
||||
ToolTip="检测当前文档的坐标系信息(用于适配Y-up坐标系模型)"/>
|
||||
|
||||
<TextBlock Text="Fragment默认Up:"
|
||||
VerticalAlignment="Center"
|
||||
Margin="8,0,4,0"
|
||||
FontSize="10"/>
|
||||
|
||||
<ComboBox ItemsSource="{Binding FragmentDefaultUpAxisOptions}"
|
||||
SelectedItem="{Binding SelectedFragmentDefaultUpAxis}"
|
||||
Width="55"
|
||||
Margin="0,0,4,0"
|
||||
ToolTip="当前文档 fragment 默认Up轴族,仅用于当前文档运行时设置"/>
|
||||
|
||||
<Button Content="检测Fragment Up"
|
||||
Command="{Binding DetectFragmentDefaultUpCommand}"
|
||||
Style="{StaticResource ActionButtonStyle}"
|
||||
ToolTip="选一个明显水平放置的真实物体,检测 fragment 默认Up 更接近 Y 还是 Z;若与当前值不同会提示是否自动修改"/>
|
||||
</StackPanel>
|
||||
|
||||
<!-- 当前坐标系信息 -->
|
||||
<TextBlock Text="{Binding CurrentCoordinateSystemInfo}"
|
||||
FontSize="10"
|
||||
@ -261,11 +284,10 @@ NavisworksTransport 系统管理页签视图 - 采用与其他页签一致的Nav
|
||||
Style="{StaticResource ActionButtonStyle}"
|
||||
ToolTip="读取选中对象的Transform信息(包括旋转角度)"/>
|
||||
|
||||
<!-- 坐标系检测按钮 -->
|
||||
<Button Content="坐标系检测"
|
||||
Command="{Binding CoordinateSystemExplorerCommand}"
|
||||
<Button Content="捕获真实物体位姿"
|
||||
Command="{Binding CaptureRealObjectTransformCommand}"
|
||||
Style="{StaticResource ActionButtonStyle}"
|
||||
ToolTip="检测当前文档的坐标系信息(用于适配Y-up坐标系模型)"/>
|
||||
ToolTip="读取选中真实物体的原始Transform,并在3D视图中可视化X/Y/Z三轴"/>
|
||||
</StackPanel>
|
||||
</StackPanel>
|
||||
</Border>
|
||||
|
||||
251
src/Utils/CoordinateSystem/FragmentRepresentativePoseHelper.cs
Normal file
251
src/Utils/CoordinateSystem/FragmentRepresentativePoseHelper.cs
Normal file
@ -0,0 +1,251 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
using System.Numerics;
|
||||
using Autodesk.Navisworks.Api;
|
||||
using ComApi = Autodesk.Navisworks.Api.Interop.ComApi;
|
||||
using ComApiBridge = Autodesk.Navisworks.Api.ComApi.ComApiBridge;
|
||||
|
||||
namespace NavisworksTransport.Utils.CoordinateSystem
|
||||
{
|
||||
/// <summary>
|
||||
/// Fragment 代表姿态辅助工具。
|
||||
///
|
||||
/// 语义:
|
||||
/// - 从 fragment 的 world 矩阵中提取旋转
|
||||
/// - 对多个 fragment 旋转做同半球加权平均
|
||||
/// - 返回一个可作为真实物体参考位姿的代表旋转
|
||||
/// </summary>
|
||||
public static class FragmentRepresentativePoseHelper
|
||||
{
|
||||
private const float AxisEpsilon = 1e-6f;
|
||||
|
||||
/// <summary>
|
||||
/// 从 ModelItem 的 fragment 几何中提取代表旋转。
|
||||
/// </summary>
|
||||
public static bool TryGetRepresentativeRotation(ModelItem item, out Quaternion representativeRotation)
|
||||
{
|
||||
representativeRotation = Quaternion.Identity;
|
||||
|
||||
if (item == null)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
var selection = ComApiBridge.ToInwOpSelection(new ModelItemCollection { item });
|
||||
var fragmentInfos = GeometryHelper.GetAllFragments(selection);
|
||||
return TryGetRepresentativeRotation(
|
||||
fragmentInfos.Select(fragmentInfo => fragmentInfo.TransformMatrix),
|
||||
out representativeRotation);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
LogManager.Warning($"[fragment代表姿态] 从ModelItem提取失败: {ex.Message}");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从 fragment 矩阵集合中提取代表旋转。
|
||||
/// </summary>
|
||||
public static bool TryGetRepresentativeRotation(
|
||||
IEnumerable<double[]> fragmentMatrices,
|
||||
out Quaternion representativeRotation)
|
||||
{
|
||||
representativeRotation = Quaternion.Identity;
|
||||
|
||||
if (fragmentMatrices == null)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
var fragmentRotations = new List<Quaternion>();
|
||||
foreach (double[] fragmentMatrix in fragmentMatrices)
|
||||
{
|
||||
if (TryExtractRotationFromFragmentMatrix(fragmentMatrix, out Quaternion fragmentRotation))
|
||||
{
|
||||
fragmentRotations.Add(fragmentRotation);
|
||||
}
|
||||
}
|
||||
|
||||
return TryGetRepresentativeRotation(fragmentRotations, out representativeRotation);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从 fragment 旋转集合中提取代表旋转。
|
||||
/// </summary>
|
||||
public static bool TryGetRepresentativeRotation(
|
||||
IEnumerable<Quaternion> fragmentRotations,
|
||||
out Quaternion representativeRotation)
|
||||
{
|
||||
representativeRotation = Quaternion.Identity;
|
||||
|
||||
if (fragmentRotations == null)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
var rotations = fragmentRotations
|
||||
.Where(rotation => TryNormalize(rotation, out _))
|
||||
.Select(rotation => Quaternion.Normalize(rotation))
|
||||
.ToList();
|
||||
|
||||
if (rotations.Count == 0)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
Quaternion seed = rotations[0];
|
||||
Vector4 sum = Vector4.Zero;
|
||||
foreach (Quaternion rotation in rotations)
|
||||
{
|
||||
Quaternion aligned = Quaternion.Dot(seed, rotation) < 0.0f
|
||||
? new Quaternion(-rotation.X, -rotation.Y, -rotation.Z, -rotation.W)
|
||||
: rotation;
|
||||
|
||||
sum += new Vector4(aligned.X, aligned.Y, aligned.Z, aligned.W);
|
||||
}
|
||||
|
||||
if (sum.LengthSquared() < AxisEpsilon)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
representativeRotation = Quaternion.Normalize(new Quaternion(sum.X, sum.Y, sum.Z, sum.W));
|
||||
return true;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从 fragment 的 4x4 world matrix 中提取旋转。
|
||||
/// 仅使用旋转部分,忽略平移。
|
||||
/// </summary>
|
||||
public static bool TryExtractRotationFromFragmentMatrix(double[] fragmentMatrix, out Quaternion rotation)
|
||||
{
|
||||
rotation = Quaternion.Identity;
|
||||
|
||||
if (fragmentMatrix == null || fragmentMatrix.Length != 16)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
Vector3 axisX = new Vector3(
|
||||
(float)fragmentMatrix[0],
|
||||
(float)fragmentMatrix[1],
|
||||
(float)fragmentMatrix[2]);
|
||||
Vector3 axisY = new Vector3(
|
||||
(float)fragmentMatrix[4],
|
||||
(float)fragmentMatrix[5],
|
||||
(float)fragmentMatrix[6]);
|
||||
Vector3 axisZ = new Vector3(
|
||||
(float)fragmentMatrix[8],
|
||||
(float)fragmentMatrix[9],
|
||||
(float)fragmentMatrix[10]);
|
||||
|
||||
if (!TryNormalize(axisX, out axisX) ||
|
||||
!TryNormalize(axisY, out axisY) ||
|
||||
!TryNormalize(axisZ, out axisZ))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
// 先将三个轴正交化,避免 fragment 矩阵里存在轻微数值误差或尺度干扰。
|
||||
Vector3 orthogonalY = axisY - Vector3.Dot(axisY, axisX) * axisX;
|
||||
if (!TryNormalize(orthogonalY, out orthogonalY))
|
||||
{
|
||||
orthogonalY = Vector3.Normalize(Vector3.Cross(axisZ, axisX));
|
||||
}
|
||||
|
||||
Vector3 orthogonalZ = Vector3.Cross(axisX, orthogonalY);
|
||||
if (!TryNormalize(orthogonalZ, out orthogonalZ))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
orthogonalY = Vector3.Normalize(Vector3.Cross(orthogonalZ, axisX));
|
||||
|
||||
rotation = Quaternion.Normalize(CreateQuaternionFromBasis(axisX, orthogonalY, orthogonalZ));
|
||||
return true;
|
||||
}
|
||||
|
||||
private static Quaternion CreateQuaternionFromBasis(Vector3 axisX, Vector3 axisY, Vector3 axisZ)
|
||||
{
|
||||
float m00 = axisX.X;
|
||||
float m01 = axisY.X;
|
||||
float m02 = axisZ.X;
|
||||
float m10 = axisX.Y;
|
||||
float m11 = axisY.Y;
|
||||
float m12 = axisZ.Y;
|
||||
float m20 = axisX.Z;
|
||||
float m21 = axisY.Z;
|
||||
float m22 = axisZ.Z;
|
||||
|
||||
float trace = m00 + m11 + m22;
|
||||
float qx;
|
||||
float qy;
|
||||
float qz;
|
||||
float qw;
|
||||
|
||||
if (trace > 0.0f)
|
||||
{
|
||||
float s = (float)Math.Sqrt(trace + 1.0f) * 2.0f;
|
||||
qw = 0.25f * s;
|
||||
qx = (m21 - m12) / s;
|
||||
qy = (m02 - m20) / s;
|
||||
qz = (m10 - m01) / s;
|
||||
}
|
||||
else if (m00 > m11 && m00 > m22)
|
||||
{
|
||||
float s = (float)Math.Sqrt(1.0f + m00 - m11 - m22) * 2.0f;
|
||||
qw = (m21 - m12) / s;
|
||||
qx = 0.25f * s;
|
||||
qy = (m01 + m10) / s;
|
||||
qz = (m02 + m20) / s;
|
||||
}
|
||||
else if (m11 > m22)
|
||||
{
|
||||
float s = (float)Math.Sqrt(1.0f + m11 - m00 - m22) * 2.0f;
|
||||
qw = (m02 - m20) / s;
|
||||
qx = (m01 + m10) / s;
|
||||
qy = 0.25f * s;
|
||||
qz = (m12 + m21) / s;
|
||||
}
|
||||
else
|
||||
{
|
||||
float s = (float)Math.Sqrt(1.0f + m22 - m00 - m11) * 2.0f;
|
||||
qw = (m10 - m01) / s;
|
||||
qx = (m02 + m20) / s;
|
||||
qy = (m12 + m21) / s;
|
||||
qz = 0.25f * s;
|
||||
}
|
||||
|
||||
return new Quaternion(qx, qy, qz, qw);
|
||||
}
|
||||
|
||||
private static bool TryNormalize(Quaternion value, out Quaternion normalized)
|
||||
{
|
||||
float lengthSquared = value.X * value.X + value.Y * value.Y + value.Z * value.Z + value.W * value.W;
|
||||
if (lengthSquared < AxisEpsilon)
|
||||
{
|
||||
normalized = Quaternion.Identity;
|
||||
return false;
|
||||
}
|
||||
|
||||
normalized = Quaternion.Normalize(value);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryNormalize(Vector3 value, out Vector3 normalized)
|
||||
{
|
||||
if (value.LengthSquared() < AxisEpsilon)
|
||||
{
|
||||
normalized = Vector3.Zero;
|
||||
return false;
|
||||
}
|
||||
|
||||
normalized = Vector3.Normalize(value);
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -182,19 +182,18 @@ namespace NavisworksTransport.Utils.CoordinateSystem
|
||||
{
|
||||
Matrix4x4 canonicalLinear = Matrix4x4.CreateFromQuaternion(canonicalRotation);
|
||||
Matrix4x4 hostLinear = FromCanonicalLinearTransform(canonicalLinear);
|
||||
return CreateRotationFromLinearComponents(
|
||||
hostLinear.M11, hostLinear.M12, hostLinear.M13,
|
||||
hostLinear.M21, hostLinear.M22, hostLinear.M23,
|
||||
hostLinear.M31, hostLinear.M32, hostLinear.M33);
|
||||
Quaternion hostQuaternion = Quaternion.Normalize(Quaternion.CreateFromRotationMatrix(hostLinear));
|
||||
return new Rotation3D(hostQuaternion.X, hostQuaternion.Y, hostQuaternion.Z, hostQuaternion.W);
|
||||
}
|
||||
|
||||
public Rotation3D FromHostQuaternion(Quaternion hostRotation)
|
||||
{
|
||||
Matrix4x4 hostLinear = Matrix4x4.CreateFromQuaternion(hostRotation);
|
||||
return CreateRotationFromLinearComponents(
|
||||
hostLinear.M11, hostLinear.M12, hostLinear.M13,
|
||||
hostLinear.M21, hostLinear.M22, hostLinear.M23,
|
||||
hostLinear.M31, hostLinear.M32, hostLinear.M33);
|
||||
Quaternion normalizedHostRotation = Quaternion.Normalize(hostRotation);
|
||||
return new Rotation3D(
|
||||
normalizedHostRotation.X,
|
||||
normalizedHostRotation.Y,
|
||||
normalizedHostRotation.Z,
|
||||
normalizedHostRotation.W);
|
||||
}
|
||||
|
||||
public Quaternion CreateHostRotationCorrection(LocalEulerRotationCorrection correction)
|
||||
@ -237,53 +236,6 @@ namespace NavisworksTransport.Utils.CoordinateSystem
|
||||
return Quaternion.Normalize(qz * qy * qx);
|
||||
}
|
||||
|
||||
private static Rotation3D CreateRotationFromLinearComponents(
|
||||
double m00, double m01, double m02,
|
||||
double m10, double m11, double m12,
|
||||
double m20, double m21, double m22)
|
||||
{
|
||||
double qx;
|
||||
double qy;
|
||||
double qz;
|
||||
double qw;
|
||||
|
||||
double trace = m00 + m11 + m22;
|
||||
if (trace > 0.0)
|
||||
{
|
||||
double s = Math.Sqrt(trace + 1.0) * 2.0;
|
||||
qw = 0.25 * s;
|
||||
qx = (m21 - m12) / s;
|
||||
qy = (m02 - m20) / s;
|
||||
qz = (m10 - m01) / s;
|
||||
}
|
||||
else if (m00 > m11 && m00 > m22)
|
||||
{
|
||||
double s = Math.Sqrt(1.0 + m00 - m11 - m22) * 2.0;
|
||||
qw = (m21 - m12) / s;
|
||||
qx = 0.25 * s;
|
||||
qy = (m01 + m10) / s;
|
||||
qz = (m02 + m20) / s;
|
||||
}
|
||||
else if (m11 > m22)
|
||||
{
|
||||
double s = Math.Sqrt(1.0 + m11 - m00 - m22) * 2.0;
|
||||
qw = (m02 - m20) / s;
|
||||
qx = (m01 + m10) / s;
|
||||
qy = 0.25 * s;
|
||||
qz = (m12 + m21) / s;
|
||||
}
|
||||
else
|
||||
{
|
||||
double s = Math.Sqrt(1.0 + m22 - m00 - m11) * 2.0;
|
||||
qw = (m10 - m01) / s;
|
||||
qx = (m02 + m20) / s;
|
||||
qy = (m12 + m21) / s;
|
||||
qz = 0.25 * s;
|
||||
}
|
||||
|
||||
return new Rotation3D(qx, qy, qz, qw);
|
||||
}
|
||||
|
||||
public CanonicalBounds3 ToCanonicalBounds3(CanonicalBounds3 hostBounds)
|
||||
{
|
||||
return RebuildBounds3(hostBounds, ToCanonicalPoint3);
|
||||
|
||||
235
src/Utils/CoordinateSystem/RealObjectPlanarPoseSolver.cs
Normal file
235
src/Utils/CoordinateSystem/RealObjectPlanarPoseSolver.cs
Normal file
@ -0,0 +1,235 @@
|
||||
using System;
|
||||
using System.Numerics;
|
||||
|
||||
namespace NavisworksTransport.Utils.CoordinateSystem
|
||||
{
|
||||
/// <summary>
|
||||
/// 真实物体在地面/吊装平面路径下的起点姿态求解器。
|
||||
///
|
||||
/// 语义:
|
||||
/// - 输入和输出都保持在宿主坐标系语义下
|
||||
/// - 真实物体没有资产坐标系
|
||||
/// - 从原始参考位姿的 6 个候选轴里,选择与路径前进方向最接近的那个
|
||||
/// - 先求“原始前进轴 -> 路径前进方向”的旋转差,再作用到原始参考位姿上
|
||||
/// </summary>
|
||||
public static class RealObjectPlanarPoseSolver
|
||||
{
|
||||
private const float AxisEpsilon = 1e-6f;
|
||||
|
||||
public static bool TryCreatePlanarPoseFromReferencePose(
|
||||
Quaternion referenceRotation,
|
||||
Vector3 desiredForward,
|
||||
Vector3 desiredUp,
|
||||
out RealObjectPlanarPoseSolution solution)
|
||||
{
|
||||
solution = null;
|
||||
|
||||
if (!TryNormalize(referenceRotation, out Quaternion normalizedReferenceRotation))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!TryNormalize(desiredUp, out Vector3 normalizedUp))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!TryNormalize(desiredForward, out Vector3 normalizedForward))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
CandidateAxis bestCandidate = SelectBestCandidateAxis(
|
||||
normalizedReferenceRotation,
|
||||
normalizedForward);
|
||||
|
||||
if (bestCandidate.IsInvalid)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
Quaternion rotationDelta = CreateShortestArcQuaternion(
|
||||
bestCandidate.WorldAxis,
|
||||
normalizedForward,
|
||||
normalizedUp);
|
||||
|
||||
Quaternion baselineRotation = Quaternion.Normalize(rotationDelta * normalizedReferenceRotation);
|
||||
solution = new RealObjectPlanarPoseSolution(
|
||||
normalizedForward,
|
||||
normalizedUp,
|
||||
bestCandidate.LocalAxis,
|
||||
bestCandidate.WorldAxis,
|
||||
rotationDelta,
|
||||
baselineRotation);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static CandidateAxis SelectBestCandidateAxis(
|
||||
Quaternion referenceRotation,
|
||||
Vector3 desiredForward)
|
||||
{
|
||||
CandidateAxis bestCandidate = CandidateAxis.Invalid;
|
||||
|
||||
foreach (var candidate in EnumerateCandidateAxes(referenceRotation))
|
||||
{
|
||||
float score = Vector3.Dot(candidate.WorldAxis, desiredForward);
|
||||
if (bestCandidate.IsInvalid || score > bestCandidate.Score)
|
||||
{
|
||||
bestCandidate = new CandidateAxis(
|
||||
candidate.LocalAxis,
|
||||
candidate.WorldAxis,
|
||||
score);
|
||||
}
|
||||
}
|
||||
|
||||
return bestCandidate;
|
||||
}
|
||||
|
||||
private static CandidateAxis[] EnumerateCandidateAxes(Quaternion referenceRotation)
|
||||
{
|
||||
return new[]
|
||||
{
|
||||
CreateCandidateAxis(Vector3.UnitX, referenceRotation),
|
||||
CreateCandidateAxis(-Vector3.UnitX, referenceRotation),
|
||||
CreateCandidateAxis(Vector3.UnitY, referenceRotation),
|
||||
CreateCandidateAxis(-Vector3.UnitY, referenceRotation),
|
||||
CreateCandidateAxis(Vector3.UnitZ, referenceRotation),
|
||||
CreateCandidateAxis(-Vector3.UnitZ, referenceRotation)
|
||||
};
|
||||
}
|
||||
|
||||
private static CandidateAxis CreateCandidateAxis(
|
||||
Vector3 localAxis,
|
||||
Quaternion referenceRotation)
|
||||
{
|
||||
Vector3 worldAxis = Vector3.Normalize(Vector3.Transform(localAxis, referenceRotation));
|
||||
return new CandidateAxis(localAxis, worldAxis, float.MinValue);
|
||||
}
|
||||
|
||||
private static Quaternion CreateShortestArcQuaternion(
|
||||
Vector3 from,
|
||||
Vector3 to,
|
||||
Vector3 desiredUp)
|
||||
{
|
||||
Vector3 normalizedFrom = Vector3.Normalize(from);
|
||||
Vector3 normalizedTo = Vector3.Normalize(to);
|
||||
|
||||
float dot = Vector3.Dot(normalizedFrom, normalizedTo);
|
||||
dot = Math.Max(-1.0f, Math.Min(1.0f, dot));
|
||||
|
||||
if (dot > 1.0f - 1e-6f)
|
||||
{
|
||||
return Quaternion.Identity;
|
||||
}
|
||||
|
||||
if (dot < -1.0f + 1e-6f)
|
||||
{
|
||||
Vector3 axis = Vector3.Cross(normalizedFrom, desiredUp);
|
||||
if (axis.LengthSquared() < AxisEpsilon)
|
||||
{
|
||||
axis = Vector3.Cross(normalizedFrom, GetAnyOrthogonalVector(normalizedFrom));
|
||||
}
|
||||
|
||||
axis = Vector3.Normalize(axis);
|
||||
return Quaternion.Normalize(Quaternion.CreateFromAxisAngle(axis, (float)Math.PI));
|
||||
}
|
||||
|
||||
Vector3 cross = Vector3.Cross(normalizedFrom, normalizedTo);
|
||||
if (cross.LengthSquared() < AxisEpsilon)
|
||||
{
|
||||
return Quaternion.Identity;
|
||||
}
|
||||
|
||||
cross = Vector3.Normalize(cross);
|
||||
double angle = Math.Acos(dot);
|
||||
return Quaternion.Normalize(Quaternion.CreateFromAxisAngle(cross, (float)angle));
|
||||
}
|
||||
|
||||
private static Vector3 GetAnyOrthogonalVector(Vector3 source)
|
||||
{
|
||||
if (Math.Abs(source.X) < 0.9f)
|
||||
{
|
||||
return Vector3.UnitX;
|
||||
}
|
||||
|
||||
if (Math.Abs(source.Y) < 0.9f)
|
||||
{
|
||||
return Vector3.UnitY;
|
||||
}
|
||||
|
||||
return Vector3.UnitZ;
|
||||
}
|
||||
|
||||
private static bool TryNormalize(Quaternion value, out Quaternion normalized)
|
||||
{
|
||||
float lengthSquared = value.X * value.X + value.Y * value.Y + value.Z * value.Z + value.W * value.W;
|
||||
if (lengthSquared < AxisEpsilon)
|
||||
{
|
||||
normalized = Quaternion.Identity;
|
||||
return false;
|
||||
}
|
||||
|
||||
normalized = Quaternion.Normalize(value);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool TryNormalize(Vector3 value, out Vector3 normalized)
|
||||
{
|
||||
if (value.LengthSquared() < AxisEpsilon)
|
||||
{
|
||||
normalized = Vector3.Zero;
|
||||
return false;
|
||||
}
|
||||
|
||||
normalized = Vector3.Normalize(value);
|
||||
return true;
|
||||
}
|
||||
|
||||
private struct CandidateAxis
|
||||
{
|
||||
public static CandidateAxis Invalid => new CandidateAxis(Vector3.Zero, Vector3.Zero, float.MinValue);
|
||||
|
||||
public Vector3 LocalAxis { get; }
|
||||
public Vector3 WorldAxis { get; }
|
||||
public float Score { get; }
|
||||
|
||||
public bool IsInvalid => Score == float.MinValue;
|
||||
|
||||
public CandidateAxis(Vector3 localAxis, Vector3 worldAxis, float score)
|
||||
{
|
||||
LocalAxis = localAxis;
|
||||
WorldAxis = worldAxis;
|
||||
Score = score;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 真实物体平面起点姿态求解结果。
|
||||
/// </summary>
|
||||
public sealed class RealObjectPlanarPoseSolution
|
||||
{
|
||||
public Vector3 ProjectedForward { get; }
|
||||
public Vector3 DesiredUp { get; }
|
||||
public Vector3 SelectedReferenceAxisLocal { get; }
|
||||
public Vector3 SelectedReferenceAxisWorld { get; }
|
||||
public Quaternion RotationDelta { get; }
|
||||
public Quaternion BaselineRotation { get; }
|
||||
|
||||
public RealObjectPlanarPoseSolution(
|
||||
Vector3 projectedForward,
|
||||
Vector3 desiredUp,
|
||||
Vector3 selectedReferenceAxisLocal,
|
||||
Vector3 selectedReferenceAxisWorld,
|
||||
Quaternion rotationDelta,
|
||||
Quaternion baselineRotation)
|
||||
{
|
||||
ProjectedForward = projectedForward;
|
||||
DesiredUp = desiredUp;
|
||||
SelectedReferenceAxisLocal = selectedReferenceAxisLocal;
|
||||
SelectedReferenceAxisWorld = selectedReferenceAxisWorld;
|
||||
RotationDelta = rotationDelta;
|
||||
BaselineRotation = baselineRotation;
|
||||
}
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user