From b17df9aa31e5b63fabe4c5f96239d71ffdbbd9fa Mon Sep 17 00:00:00 2001 From: tian <11429339@qq.com> Date: Mon, 8 Jun 2026 03:35:33 +0800 Subject: [PATCH] =?UTF-8?q?fix:=20=E5=9B=9B=E5=85=83=E6=95=B0=E5=9D=87?= =?UTF-8?q?=E5=8C=80=E9=87=87=E6=A0=B7=E6=9B=BF=E4=BB=A3=E6=AC=A7=E6=8B=89?= =?UTF-8?q?=E8=A7=92=E7=BD=91=E6=A0=BC=EF=BC=8C=E6=A8=A1=E5=BC=8F=E6=90=9C?= =?UTF-8?q?=E7=B4=A2=E6=94=B9=E4=B8=BA=E5=9B=9B=E5=85=83=E6=95=B0=E9=82=BB?= =?UTF-8?q?=E5=9F=9F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - 网格搜索: S³ 均匀随机四元数 → canonical→host Euler，512→256 采样点 - 模式搜索: 7轴×2方向四元数乘法搜索，替代欧拉角 delta - 去掉 CandidateAngles/Euler 网格+PatternSearchDeltas/NormalizeDegrees - canonical 空间桥接保证 YUp/ZUp 兼容 --- src/Core/Animation/PathAnimationManager.cs | 1 + .../ObjectPassageProjectionOptimizer.cs | 136 +++++++++++++----- 2 files changed, 98 insertions(+), 39 deletions(-) diff --git a/src/Core/Animation/PathAnimationManager.cs b/src/Core/Animation/PathAnimationManager.cs index d6c07fe..c002ea8 100644 --- a/src/Core/Animation/PathAnimationManager.cs +++ b/src/Core/Animation/PathAnimationManager.cs @@ -428,6 +428,7 @@ namespace NavisworksTransport.Core.Animation /// /// 在 CAD 原位仅施加宿主 X/Y/Z 三轴旋转修正，不移动物体。 + /// /// 用于自动调整评测——每次评测在固定 CAD 位置，以 X 轴为统一参考前向。 /// 调用前需确保物体处于 CAD 原位（已 ResetPermanentTransform）。 /// diff --git a/src/Utils/CoordinateSystem/ObjectPassageProjectionOptimizer.cs b/src/Utils/CoordinateSystem/ObjectPassageProjectionOptimizer.cs index ee1b1ff..3d63b1f 100644 --- a/src/Utils/CoordinateSystem/ObjectPassageProjectionOptimizer.cs +++ b/src/Utils/CoordinateSystem/ObjectPassageProjectionOptimizer.cs @@ -128,6 +128,8 @@ namespace NavisworksTransport.Utils.CoordinateSystem public static class ObjectPassageProjectionOptimizer { private const double Epsilon = 1e-9; + private const int QuaternionSampleCount = 256; + private static readonly int QuaternionRandomSeed = 42; public static ObjectPassageProjectionOptimizationResult Optimize( ObjectPassageProjectionOptimizationRequest request) @@ -156,24 +158,21 @@ namespace NavisworksTransport.Utils.CoordinateSystem LocalEulerRotationCorrection bestCorrection = LocalEulerRotationCorrection.Zero; ObjectPassageProjectionScore bestScore = ObjectPassageProjectionScore.Invalid; - foreach (double xDegrees in CandidateAngles()) + Quaternion bestQuaternion = Quaternion.Identity; + + // S³ 均匀采样四元数 → 转 Euler → 覆盖 SO(3) 无万向节死锁 + foreach (var (correction, quat) in UniformQuaternionCorrections(QuaternionSampleCount, QuaternionRandomSeed)) { - foreach (double yDegrees in CandidateAngles()) + ObjectPassageProjectionScore score = evaluator(correction); + if (IsBetter(score, bestScore, request.AreaRelativeTieTolerance)) { - foreach (double zDegrees in CandidateAngles()) - { - var correction = new LocalEulerRotationCorrection(xDegrees, yDegrees, zDegrees); - ObjectPassageProjectionScore score = evaluator(correction); - if (IsBetter(score, bestScore, request.AreaRelativeTieTolerance)) - { - bestScore = score; - bestCorrection = correction; - } - } + bestScore = score; + bestCorrection = correction; + bestQuaternion = quat; } } - RefineByPatternSearch(request, evaluator, ref bestCorrection, ref bestScore); + RefineByQuaternionSearch(request, evaluator, ref bestCorrection, ref bestScore, ref bestQuaternion); if (double.IsInfinity(bestScore.Area)) { @@ -323,49 +322,108 @@ namespace NavisworksTransport.Utils.CoordinateSystem Math.Abs(Vector3.Dot(rotatedHostZ, targetAxis)) * sizeZ; } - private static double[] CandidateAngles() + private static IEnumerable<(LocalEulerRotationCorrection euler, Quaternion quat)> UniformQuaternionCorrections(int count, int seed) { - return new[] + var random = new Random(seed); + + for (int i = 0; i < count; i++) { - 0.0, - 90.0, - 180.0, - 270.0, - 45.0, - 135.0, - 225.0, - 315.0 - }; + double x = NextGaussian(random); + double y = NextGaussian(random); + double z = NextGaussian(random); + double w = NextGaussian(random); + + double norm = Math.Sqrt(x * x + y * y + z * z + w * w); + x /= norm; y /= norm; z /= norm; w /= norm; + + if (w < 0) { x = -x; y = -y; z = -z; w = -w; } + + var q = new Quaternion((float)x, (float)y, (float)z, (float)w); + yield return (CanonicalQuaternionToHostEulerCorrection(q), q); + } } - private static void RefineByPatternSearch( + private static LocalEulerRotationCorrection CanonicalQuaternionToHostEulerCorrection(Quaternion q) + { + double qw = q.W, qx = q.X, qy = q.Y, qz = q.Z; + + double sinr_cosp = 2.0 * (qw * qx + qy * qz); + double cosr_cosp = 1.0 - 2.0 * (qx * qx + qy * qy); + double roll = Math.Atan2(sinr_cosp, cosr_cosp); + + double sinp = 2.0 * (qw * qy - qz * qx); + sinp = Math.Max(-1.0, Math.Min(1.0, sinp)); + double pitch = Math.Asin(sinp); + + double siny_cosp = 2.0 * (qw * qz + qx * qy); + double cosy_cosp = 1.0 - 2.0 * (qy * qy + qz * qz); + double yaw_rad = Math.Atan2(siny_cosp, cosy_cosp); + + // Canonical (ZUp) → Host: 使用 CoordinateSystemManager 桥接 + var hostType = CoordinateSystemManager.Instance.ResolvedType; + if (hostType == CoordinateSystemType.ZUp) + { + return new LocalEulerRotationCorrection( + roll * 180.0 / Math.PI, + pitch * 180.0 / Math.PI, + yaw_rad * 180.0 / Math.PI); + } + // YUp: canonical X→host X, canonical Z→host Y(up), canonical -Y→host Z(nonUp) + return new LocalEulerRotationCorrection( + roll * 180.0 / Math.PI, + yaw_rad * 180.0 / Math.PI, + -pitch * 180.0 / Math.PI); + } + + private static double NextGaussian(Random random) + { + double u1 = 1.0 - random.NextDouble(); + double u2 = 1.0 - random.NextDouble(); + return Math.Sqrt(-2.0 * Math.Log(u1)) * Math.Sin(2.0 * Math.PI * u2); + } + + private static void RefineByQuaternionSearch( ObjectPassageProjectionOptimizationRequest request, Func evaluator, ref LocalEulerRotationCorrection bestCorrection, - ref ObjectPassageProjectionScore bestScore) + ref ObjectPassageProjectionScore bestScore, + ref Quaternion bestQuaternion) { - double stepDegrees = 22.5; - while (stepDegrees >= 1.0) + var axes = new[] + { + Vector3.UnitX, Vector3.UnitY, Vector3.UnitZ, + Vector3.Normalize(new Vector3(1, 1, 0)), + Vector3.Normalize(new Vector3(1, 0, 1)), + Vector3.Normalize(new Vector3(0, 1, 1)), + Vector3.Normalize(new Vector3(1, 1, 1)) + }; + + double stepRadians = 22.5 * Math.PI / 180.0; + while (stepRadians >= 1.0 * Math.PI / 180.0) { bool improved = false; - foreach (var delta in PatternSearchDeltas(stepDegrees)) + foreach (var axis in axes) { - var candidate = new LocalEulerRotationCorrection( - NormalizeDegrees(bestCorrection.XDegrees + delta.X), - NormalizeDegrees(bestCorrection.YDegrees + delta.Y), - NormalizeDegrees(bestCorrection.ZDegrees + delta.Z)); - ObjectPassageProjectionScore score = evaluator(candidate); - if (IsBetter(score, bestScore, request.AreaRelativeTieTolerance)) + for (int sign = -1; sign <= 1; sign += 2) { - bestCorrection = candidate; - bestScore = score; - improved = true; + double angle = sign * stepRadians; + var deltaQ = Quaternion.CreateFromAxisAngle(axis, (float)angle); + var candidateQ = Quaternion.Normalize(deltaQ * bestQuaternion); + var candidateEuler = CanonicalQuaternionToHostEulerCorrection(candidateQ); + ObjectPassageProjectionScore score = evaluator(candidateEuler); + if (IsBetter(score, bestScore, request.AreaRelativeTieTolerance)) + { + bestCorrection = candidateEuler; + bestScore = score; + bestQuaternion = candidateQ; + improved = true; + } } } if (!improved) { - stepDegrees *= 0.5; + stepRadians *= 0.5; } } }