NavisworksTransport/src/Utils/CoordinateSystem/ObjectPassageProjectionOptimizer.cs

using System;
using System.Collections.Generic;
using System.Numerics;

namespace NavisworksTransport.Utils.CoordinateSystem
{
    public sealed class ObjectPassageProjectionOptimizationRequest
    {
        public ObjectPassageProjectionOptimizationRequest(
            double sizeX,
            double sizeY,
            double sizeZ,
            Vector3 hostPathForward,
            Vector3 hostUp,
            double areaRelativeTieTolerance = 0.01,
            Quaternion? baselineHostRotation = null)
        {
            SizeX = sizeX;
            SizeY = sizeY;
            SizeZ = sizeZ;
            HostPathForward = hostPathForward;
            HostUp = hostUp;
            AreaRelativeTieTolerance = areaRelativeTieTolerance;
            BaselineHostRotation = Quaternion.Normalize(baselineHostRotation ?? Quaternion.Identity);
        }

        public double SizeX { get; }

        public double SizeY { get; }

        public double SizeZ { get; }

        public Vector3 HostPathForward { get; }

        public Vector3 HostUp { get; }

        public double AreaRelativeTieTolerance { get; }

        public Quaternion BaselineHostRotation { get; }
    }

    public sealed class ObjectPassageProjectionOptimizationResult
    {
        private ObjectPassageProjectionOptimizationResult(
            bool success,
            LocalEulerRotationCorrection correction,
            ObjectPassageProjectionScore score,
            string errorMessage)
        {
            Success = success;
            Correction = correction;
            Score = score;
            ErrorMessage = errorMessage;
        }

        public bool Success { get; }

        public LocalEulerRotationCorrection Correction { get; }

        public ObjectPassageProjectionScore Score { get; }

        public string ErrorMessage { get; }

        public static ObjectPassageProjectionOptimizationResult CreateSuccess(
            LocalEulerRotationCorrection correction,
            ObjectPassageProjectionScore score)
        {
            return new ObjectPassageProjectionOptimizationResult(true, correction, score, null);
        }

        public static ObjectPassageProjectionOptimizationResult CreateFailure(string errorMessage)
        {
            return new ObjectPassageProjectionOptimizationResult(
                false,
                LocalEulerRotationCorrection.Zero,
                ObjectPassageProjectionScore.Invalid,
                errorMessage);
        }
    }

    public struct ObjectPassageProjectionScore
    {
        public static readonly ObjectPassageProjectionScore Invalid =
            new ObjectPassageProjectionScore(double.PositiveInfinity, double.PositiveInfinity);

        public ObjectPassageProjectionScore(double widthAcrossPath, double heightAlongHostUp)
        {
            WidthAcrossPath = widthAcrossPath;
            HeightAlongHostUp = heightAlongHostUp;
            Area = widthAcrossPath * heightAlongHostUp;
        }

        public double WidthAcrossPath { get; }

        public double HeightAlongHostUp { get; }

        public double Area { get; }
    }

    public static class ObjectPassageProjectionOptimizer
    {
        private const double Epsilon = 1e-9;

        public static ObjectPassageProjectionOptimizationResult Optimize(
            ObjectPassageProjectionOptimizationRequest request)
        {
            if (!TryValidateRequest(request, out string errorMessage))
            {
                return ObjectPassageProjectionOptimizationResult.CreateFailure(errorMessage);
            }

            return OptimizeWithEvaluator(request, correction => Evaluate(request, correction));
        }

        public static ObjectPassageProjectionOptimizationResult OptimizeWithEvaluator(
            ObjectPassageProjectionOptimizationRequest request,
            Func<LocalEulerRotationCorrection, ObjectPassageProjectionScore> evaluator)
        {
            if (!TryValidateRequest(request, out string errorMessage))
            {
                return ObjectPassageProjectionOptimizationResult.CreateFailure(errorMessage);
            }

            if (evaluator == null)
            {
                return ObjectPassageProjectionOptimizationResult.CreateFailure("自动调整评分器为空。");
            }

            LocalEulerRotationCorrection bestCorrection = LocalEulerRotationCorrection.Zero;
            ObjectPassageProjectionScore bestScore = ObjectPassageProjectionScore.Invalid;
            foreach (double xDegrees in CandidateAngles())
            {
                foreach (double yDegrees in CandidateAngles())
                {
                    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;
                        }
                    }
                }
            }

            RefineByPatternSearch(request, evaluator, ref bestCorrection, ref bestScore);

            if (double.IsInfinity(bestScore.Area))
            {
                return ObjectPassageProjectionOptimizationResult.CreateFailure("优化器未找到有效姿态。");
            }

            return ObjectPassageProjectionOptimizationResult.CreateSuccess(bestCorrection, bestScore);
        }

        public static ObjectPassageProjectionScore Evaluate(
            ObjectPassageProjectionOptimizationRequest request,
            LocalEulerRotationCorrection correction)
        {
            if (!TryCreateFrame(request, out Vector3 hostSide, out Vector3 hostUp))
            {
                throw new InvalidOperationException("路径方向退化，无法计算通行截面。");
            }

            Quaternion correctionRotation = correction.CreateQuaternion(Vector3.UnitX, Vector3.UnitY, Vector3.UnitZ);
            Quaternion rotation = Quaternion.Normalize(correctionRotation * request.BaselineHostRotation);
            Vector3 rotatedHostX = Vector3.Normalize(Vector3.Transform(Vector3.UnitX, rotation));
            Vector3 rotatedHostY = Vector3.Normalize(Vector3.Transform(Vector3.UnitY, rotation));
            Vector3 rotatedHostZ = Vector3.Normalize(Vector3.Transform(Vector3.UnitZ, rotation));

            double width = ProjectExtent(
                request.SizeX,
                request.SizeY,
                request.SizeZ,
                rotatedHostX,
                rotatedHostY,
                rotatedHostZ,
                hostSide);

            double height = ProjectExtent(
                request.SizeX,
                request.SizeY,
                request.SizeZ,
                rotatedHostX,
                rotatedHostY,
                rotatedHostZ,
                hostUp);

            return new ObjectPassageProjectionScore(width, height);
        }

        private static bool TryValidateRequest(
            ObjectPassageProjectionOptimizationRequest request,
            out string errorMessage)
        {
            if (request == null)
            {
                errorMessage = "自动调整请求为空。";
                return false;
            }

            if (request.SizeX <= Epsilon || request.SizeY <= Epsilon || request.SizeZ <= Epsilon)
            {
                errorMessage = "物体尺寸无效。";
                return false;
            }

            if (!TryCreateFrame(request, out _, out _))
            {
                errorMessage = "路径方向退化，无法计算通行截面。";
                return false;
            }

            errorMessage = null;
            return true;
        }

        private static bool TryCreateFrame(
            ObjectPassageProjectionOptimizationRequest request,
            out Vector3 hostSide,
            out Vector3 hostUp)
        {
            hostSide = Vector3.Zero;
            hostUp = Vector3.Zero;

            if (request == null || request.HostUp.LengthSquared() < Epsilon)
            {
                return false;
            }

            hostUp = Vector3.Normalize(request.HostUp);
            Vector3 hostPathForward = request.HostPathForward - hostUp * Vector3.Dot(request.HostPathForward, hostUp);
            if (hostPathForward.LengthSquared() < Epsilon)
            {
                return false;
            }

            hostPathForward = Vector3.Normalize(hostPathForward);
            hostSide = Vector3.Cross(hostPathForward, hostUp);
            if (hostSide.LengthSquared() < Epsilon)
            {
                return false;
            }

            hostSide = Vector3.Normalize(hostSide);
            return true;
        }

        private static bool IsBetter(
            ObjectPassageProjectionScore candidate,
            ObjectPassageProjectionScore currentBest,
            double areaRelativeTieTolerance)
        {
            if (double.IsInfinity(currentBest.Area))
            {
                return true;
            }

            double tolerance = Math.Max(0.0, areaRelativeTieTolerance) * Math.Max(1.0, currentBest.Area);
            if (candidate.Area < currentBest.Area - tolerance)
            {
                return true;
            }

            if (Math.Abs(candidate.Area - currentBest.Area) <= tolerance)
            {
                if (candidate.HeightAlongHostUp < currentBest.HeightAlongHostUp - Epsilon)
                {
                    return true;
                }

                if (Math.Abs(candidate.HeightAlongHostUp - currentBest.HeightAlongHostUp) <= Epsilon &&
                    candidate.WidthAcrossPath < currentBest.WidthAcrossPath - Epsilon)
                {
                    return true;
                }
            }

            return false;
        }

        private static double ProjectExtent(
            double sizeX,
            double sizeY,
            double sizeZ,
            Vector3 rotatedHostX,
            Vector3 rotatedHostY,
            Vector3 rotatedHostZ,
            Vector3 targetAxis)
        {
            return Math.Abs(Vector3.Dot(rotatedHostX, targetAxis)) * sizeX +
                   Math.Abs(Vector3.Dot(rotatedHostY, targetAxis)) * sizeY +
                   Math.Abs(Vector3.Dot(rotatedHostZ, targetAxis)) * sizeZ;
        }

        private static double[] CandidateAngles()
        {
            return new[]
            {
                0.0,
                90.0,
                180.0,
                270.0,
                45.0,
                135.0,
                225.0,
                315.0
            };
        }

        private static void RefineByPatternSearch(
            ObjectPassageProjectionOptimizationRequest request,
            Func<LocalEulerRotationCorrection, ObjectPassageProjectionScore> evaluator,
            ref LocalEulerRotationCorrection bestCorrection,
            ref ObjectPassageProjectionScore bestScore)
        {
            double stepDegrees = 22.5;
            while (stepDegrees >= 1.0)
            {
                bool improved = false;
                foreach (var delta in PatternSearchDeltas(stepDegrees))
                {
                    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))
                    {
                        bestCorrection = candidate;
                        bestScore = score;
                        improved = true;
                    }
                }

                if (!improved)
                {
                    stepDegrees *= 0.5;
                }
            }
        }

        private static (double X, double Y, double Z)[] PatternSearchDeltas(double stepDegrees)
        {
            return new[]
            {
                ( stepDegrees, 0.0, 0.0),
                (-stepDegrees, 0.0, 0.0),
                (0.0,  stepDegrees, 0.0),
                (0.0, -stepDegrees, 0.0),
                (0.0, 0.0,  stepDegrees),
                (0.0, 0.0, -stepDegrees),
                ( stepDegrees,  stepDegrees, 0.0),
                ( stepDegrees, -stepDegrees, 0.0),
                (-stepDegrees,  stepDegrees, 0.0),
                (-stepDegrees, -stepDegrees, 0.0),
                ( stepDegrees, 0.0,  stepDegrees),
                ( stepDegrees, 0.0, -stepDegrees),
                (-stepDegrees, 0.0,  stepDegrees),
                (-stepDegrees, 0.0, -stepDegrees),
                (0.0,  stepDegrees,  stepDegrees),
                (0.0,  stepDegrees, -stepDegrees),
                (0.0, -stepDegrees,  stepDegrees),
                (0.0, -stepDegrees, -stepDegrees)
            };
        }

        private static double NormalizeDegrees(double degrees)
        {
            double normalized = degrees % 360.0;
            if (normalized < 0.0)
            {
                normalized += 360.0;
            }

            return Math.Abs(normalized) < Epsilon ? 0.0 : normalized;
        }
    }
}