fix: 四元数均匀采样替代欧拉角网格，模式搜索改为四元数邻域
- 网格搜索: S³ 均匀随机四元数 → canonical→host Euler，512→256 采样点 - 模式搜索: 7轴×2方向四元数乘法搜索，替代欧拉角 delta - 去掉 CandidateAngles/Euler 网格+PatternSearchDeltas/NormalizeDegrees - canonical 空间桥接保证 YUp/ZUp 兼容
This commit is contained in:
parent
51453df037
commit
b17df9aa31
@ -428,6 +428,7 @@ namespace NavisworksTransport.Core.Animation
|
||||
|
||||
/// <summary>
|
||||
/// 在 CAD 原位仅施加宿主 X/Y/Z 三轴旋转修正，不移动物体。
|
||||
/// <summary>
|
||||
/// 用于自动调整评测——每次评测在固定 CAD 位置，以 X 轴为统一参考前向。
|
||||
/// 调用前需确保物体处于 CAD 原位（已 ResetPermanentTransform）。
|
||||
/// </summary>
|
||||
|
||||
@ -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<LocalEulerRotationCorrection, ObjectPassageProjectionScore> 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user