XplorePlane/XP.Hardware.MotionControl/Services/GeometryCalculator.cs

using System;
using XP.Hardware.MotionControl.Abstractions;

namespace XP.Hardware.MotionControl.Services
{
    /// <summary>
    /// 几何计算器 | Geometry Calculator
    /// 负责 FOD/FDD/放大倍率的正算与反算，支持探测器摆动角度 | Forward and inverse calculation of FOD/FDD/Magnification with detector swing support
    /// </summary>
    public class GeometryCalculator
    {
        /// <summary>
        /// FOD 接近零的阈值（mm）| Threshold for FOD near zero (mm)
        /// </summary>
        private const double FodZeroThreshold = 0.001;

        /// <summary>
        /// 角度接近零的阈值（度）| Threshold for angle near zero (degrees)
        /// </summary>
        private const double AngleZeroThreshold = 0.001;

        #region 绝对坐标计算 | Absolute Coordinate Calculation

        /// <summary>
        /// 计算绝对坐标 | Calculate absolute coordinate
        /// 绝对坐标 = 当前位置 + Origin（配置）| Absolute = current position + Origin (config)
        /// </summary>
        /// <param name="currentPosition">当前位置（mm）| Current position (mm)</param>
        /// <param name="origin">原点偏移（mm）| Origin offset (mm)</param>
        /// <returns>绝对坐标（mm）| Absolute coordinate (mm)</returns>
        public double CalcAbsolutePosition(double currentPosition, double origin)
        {
            return currentPosition + origin;
        }

        #endregion

        #region 探测器中心坐标计算 | Detector Center Coordinate Calculation

        /// <summary>
        /// 计算探测器感光面中心的 X 坐标 | Calculate detector active area center X coordinate
        /// Dx = R × sin(θ)
        /// </summary>
        /// <param name="swingAngleDeg">探测器摆动角度（度）| Detector swing angle (degrees)</param>
        /// <param name="swingRadius">摆动半径（mm）| Swing radius (mm)</param>
        /// <returns>探测器中心 X 坐标（mm）| Detector center X (mm)</returns>
        public double CalcDetectorCenterX(double swingAngleDeg, double swingRadius)
        {
            if (swingRadius <= 0 || Math.Abs(swingAngleDeg) < AngleZeroThreshold)
                return 0.0;

            double thetaRad = swingAngleDeg * Math.PI / 180.0;
            return swingRadius * Math.Sin(thetaRad);
        }

        /// <summary>
        /// 计算探测器感光面中心的 Z 坐标 | Calculate detector active area center Z coordinate
        /// Dz = Pz - R × cos(θ)，其中 Pz = detectorZAbsolute + swingPivotOffset
        /// 探测器感光面在 Pivot 下方（朝向射线源）| Detector active area is below Pivot (toward source)
        /// </summary>
        /// <param name="detectorZAbsolute">探测器Z轴绝对坐标（mm）| Detector Z absolute (mm)</param>
        /// <param name="swingPivotOffset">Pivot 相对于 DetectorZ 绝对坐标的偏移（mm）| Pivot offset from DetectorZ absolute (mm)</param>
        /// <param name="swingAngleDeg">探测器摆动角度（度）| Detector swing angle (degrees)</param>
        /// <param name="swingRadius">摆动半径（mm）| Swing radius (mm)</param>
        /// <returns>探测器中心 Z 坐标（mm）| Detector center Z (mm)</returns>
        public double CalcDetectorCenterZ(double detectorZAbsolute, double swingPivotOffset,
            double swingAngleDeg, double swingRadius)
        {
            double pivotZ = detectorZAbsolute + swingPivotOffset;

            if (swingRadius <= 0)
                return detectorZAbsolute;

            double thetaRad = swingAngleDeg * Math.PI / 180.0;
            return pivotZ - swingRadius * Math.Cos(thetaRad);
        }

        #endregion

        #region 正算 | Forward Calculation

        /// <summary>
        /// 正算：计算 FDD（考虑探测器摆动）| Forward: Calculate FDD with detector swing
        /// FDD = sqrt(Dx² + (Dz - Sz)²)
        /// 当 swingRadius = 0 时退化为 |Sz - DetectorZ_abs| | Degrades to |Sz - DetectorZ_abs| when swingRadius = 0
        /// </summary>
        /// <param name="sourceZAbsolute">射线源Z轴绝对坐标（mm）| Source Z absolute (mm)</param>
        /// <param name="detectorZAbsolute">探测器Z轴绝对坐标（mm）| Detector Z absolute (mm)</param>
        /// <param name="swingPivotOffset">Pivot 偏移（mm）| Pivot offset (mm)</param>
        /// <param name="swingAngleDeg">摆动角度（度）| Swing angle (degrees)</param>
        /// <param name="swingRadius">摆动半径（mm）| Swing radius (mm)</param>
        /// <returns>FDD 值（mm）| FDD value (mm)</returns>
        public double CalcFDD(double sourceZAbsolute, double detectorZAbsolute,
            double swingPivotOffset, double swingAngleDeg, double swingRadius)
        {
            // 无摆动时退化为原始公式 | Degrade to original formula when no swing
            if (swingRadius <= 0 || Math.Abs(swingAngleDeg) < AngleZeroThreshold)
                return Math.Abs(sourceZAbsolute - detectorZAbsolute);

            double dx = CalcDetectorCenterX(swingAngleDeg, swingRadius);
            double dz = CalcDetectorCenterZ(detectorZAbsolute, swingPivotOffset, swingAngleDeg, swingRadius);

            return Math.Sqrt(dx * dx + (dz - sourceZAbsolute) * (dz - sourceZAbsolute));
        }

        /// <summary>
        /// 正算：计算 FOD（考虑探测器摆动）| Forward: Calculate FOD with detector swing
        /// FOD = 射线源到射线束与载物台平面交点的距离 | Distance from source to ray-beam/stage-plane intersection
        /// 当 swingRadius = 0 时退化为 |Sz - StageRotationCenterZ| | Degrades when swingRadius = 0
        /// </summary>
        /// <param name="sourceZAbsolute">射线源Z轴绝对坐标（mm）| Source Z absolute (mm)</param>
        /// <param name="stageRotationCenterZ">旋转中心Z坐标（mm）| Stage rotation center Z (mm)</param>
        /// <param name="detectorZAbsolute">探测器Z轴绝对坐标（mm）| Detector Z absolute (mm)</param>
        /// <param name="swingPivotOffset">Pivot 偏移（mm）| Pivot offset (mm)</param>
        /// <param name="swingAngleDeg">摆动角度（度）| Swing angle (degrees)</param>
        /// <param name="swingRadius">摆动半径（mm）| Swing radius (mm)</param>
        /// <returns>FOD 值（mm）| FOD value (mm)</returns>
        public double CalcFOD(double sourceZAbsolute, double stageRotationCenterZ,
            double detectorZAbsolute, double swingPivotOffset,
            double swingAngleDeg, double swingRadius)
        {
            // 无摆动时退化为原始公式 | Degrade to original formula when no swing
            if (swingRadius <= 0 || Math.Abs(swingAngleDeg) < AngleZeroThreshold)
                return Math.Abs(sourceZAbsolute - stageRotationCenterZ);

            double dx = CalcDetectorCenterX(swingAngleDeg, swingRadius);
            double dz = CalcDetectorCenterZ(detectorZAbsolute, swingPivotOffset, swingAngleDeg, swingRadius);

            double deltaZ_SD = dz - sourceZAbsolute;

            // 射线源和探测器中心在同一 Z 高度时，射线束水平，无法与载物台平面相交 | Horizontal beam, no intersection
            if (Math.Abs(deltaZ_SD) < FodZeroThreshold)
                return double.NaN;

            // 射线束与载物台平面交点参数 t_stage = (Oz - Sz) / (Dz - Sz) | Ray-plane intersection parameter
            double tStage = (stageRotationCenterZ - sourceZAbsolute) / deltaZ_SD;

            // 交点 X 坐标 | Intersection X coordinate
            double xIntersect = tStage * dx;

            // FOD = sqrt(x_intersect² + (Oz - Sz)²) | Euclidean distance from source to intersection
            double deltaZ_SO = stageRotationCenterZ - sourceZAbsolute;
            return Math.Sqrt(xIntersect * xIntersect + deltaZ_SO * deltaZ_SO);
        }

        /// <summary>
        /// 正算：计算放大倍率 | Forward: Calculate Magnification
        /// M = FDD / FOD，FOD 接近零（&lt; 0.001mm）或 NaN 时返回 double.NaN
        /// </summary>
        /// <param name="fdd">焦点到探测器距离（mm）| Focus-to-detector distance (mm)</param>
        /// <param name="fod">焦点到旋转中心距离（mm）| Focus-to-object distance (mm)</param>
        /// <returns>放大倍率，FOD 接近零时返回 NaN | Magnification, NaN when FOD near zero</returns>
        public double CalcMagnification(double fdd, double fod)
        {
            if (double.IsNaN(fod) || fod < FodZeroThreshold)
                return double.NaN;

            return fdd / fod;
        }

        #endregion

        #region 反算 | Inverse Calculation

        /// <summary>
        /// 反算：由目标 FOD 和 FDD 计算轴目标位置（考虑探测器摆动）| Inverse: Calculate axis targets from FOD and FDD with swing
        /// 返回值为相对坐标（已减去 Origin）| Returns relative coordinates (Origin subtracted)
        /// </summary>
        /// <param name="targetFOD">目标 FOD（mm）| Target FOD (mm)</param>
        /// <param name="targetFDD">目标 FDD（mm）| Target FDD (mm)</param>
        /// <param name="stageRotationCenterZ">旋转中心绝对Z坐标（mm）| Stage rotation center Z (mm)</param>
        /// <param name="sourceZOrigin">射线源Z轴原点偏移（mm）| Source Z origin offset (mm)</param>
        /// <param name="detectorZOrigin">探测器Z轴原点偏移（mm）| Detector Z origin offset (mm)</param>
        /// <param name="swingPivotOffset">Pivot 偏移（mm）| Pivot offset (mm)</param>
        /// <param name="swingAngleDeg">摆动角度（度）| Swing angle (degrees)</param>
        /// <param name="swingRadius">摆动半径（mm）| Swing radius (mm)</param>
        /// <returns>
        /// sourceZTarget: 射线源Z轴相对目标位置（mm）| Source Z relative target (mm)
        /// detectorZTarget: 探测器Z轴相对目标位置（mm）| Detector Z relative target (mm)
        /// errorMessage: 错误信息，成功时为 null | Error message, null on success
        /// </returns>
        public (double sourceZTarget, double detectorZTarget, string errorMessage) CalcAxisTargets(
            double targetFOD, double targetFDD,
            double stageRotationCenterZ, double sourceZOrigin, double detectorZOrigin,
            double swingPivotOffset, double swingAngleDeg, double swingRadius)
        {
            // 无摆动时使用简化公式 | Use simplified formula when no swing
            if (swingRadius <= 0 || Math.Abs(swingAngleDeg) < AngleZeroThreshold)
            {
                double szAbs = stageRotationCenterZ - targetFOD;
                double dzAbs = szAbs + targetFDD;
                return (szAbs - sourceZOrigin, dzAbs - detectorZOrigin, null);
            }

            double thetaRad = swingAngleDeg * Math.PI / 180.0;
            double sinTheta = Math.Sin(thetaRad);
            double cosTheta = Math.Cos(thetaRad);

            // B = R × sin(θ)，探测器中心的 X 偏移 | Detector center X offset
            double bx = swingRadius * sinTheta;
            // A = SwingPivotOffset - R × cos(θ)，DetectorZ_abs 到探测器中心的 Z 偏移 | DetectorZ_abs to detector center Z offset
            double az = swingPivotOffset - swingRadius * cosTheta;

            // 由 targetFOD 求 SourceZ 绝对坐标 | Calculate SourceZ absolute from targetFOD
            // 射线源在载物台下方：Sz = Oz - targetFOD | Source below stage
            double sourceZAbsolute = stageRotationCenterZ - targetFOD;

            // 由 targetFDD 求 DetectorZ 绝对坐标 | Calculate DetectorZ absolute from targetFDD
            // FDD² = Bx² + (DetZ_abs + Az - Sz)²
            // (DetZ_abs + Az - Sz)² = FDD² - Bx²
            double fddSquared = targetFDD * targetFDD;
            double bxSquared = bx * bx;
            double discriminant = fddSquared - bxSquared;

            if (discriminant < 0)
            {
                return (0, 0,
                    $"目标 FDD={targetFDD:F3}mm 小于探测器 X 偏移={Math.Abs(bx):F3}mm，目标不可达 | " +
                    $"Target FDD={targetFDD:F3}mm less than detector X offset={Math.Abs(bx):F3}mm, target unreachable");
            }

            // DetZ_abs = Sz - Az + sqrt(FDD² - Bx²)（探测器在射线源上方，取正值）
            // DetZ_abs = Sz - Az + sqrt(discriminant)
            double detectorZAbsolute = sourceZAbsolute - az + Math.Sqrt(discriminant);

            // 转换为相对坐标 | Convert to relative coordinates
            double sourceZTarget = sourceZAbsolute - sourceZOrigin;
            double detectorZTarget = detectorZAbsolute - detectorZOrigin;

            return (sourceZTarget, detectorZTarget, null);
        }

        #endregion

        #region 边界检查 | Boundary Validation

        /// <summary>
        /// 边界检查：验证目标位置是否在允许范围内 | Boundary validation: Check targets within allowed range
        /// </summary>
        /// <param name="sourceZTarget">射线源Z轴目标位置（mm）| Source Z target (mm)</param>
        /// <param name="detectorZTarget">探测器Z轴目标位置（mm）| Detector Z target (mm)</param>
        /// <param name="sourceZMin">射线源Z轴最小值（mm）| Source Z minimum (mm)</param>
        /// <param name="sourceZMax">射线源Z轴最大值（mm）| Source Z maximum (mm)</param>
        /// <param name="detectorZMin">探测器Z轴最小值（mm）| Detector Z minimum (mm)</param>
        /// <param name="detectorZMax">探测器Z轴最大值（mm）| Detector Z maximum (mm)</param>
        /// <returns>验证结果 | Validation result</returns>
        public MotionResult ValidateTargets(
            double sourceZTarget, double detectorZTarget,
            double sourceZMin, double sourceZMax,
            double detectorZMin, double detectorZMax)
        {
            if (sourceZTarget < sourceZMin || sourceZTarget > sourceZMax)
            {
                return MotionResult.Fail(
                    $"SourceZ 目标位置 {sourceZTarget:F3} 超出范围 [{sourceZMin:F3}, {sourceZMax:F3}] | " +
                    $"SourceZ target {sourceZTarget:F3} out of range [{sourceZMin:F3}, {sourceZMax:F3}]");
            }

            if (detectorZTarget < detectorZMin || detectorZTarget > detectorZMax)
            {
                return MotionResult.Fail(
                    $"DetectorZ 目标位置 {detectorZTarget:F3} 超出范围 [{detectorZMin:F3}, {detectorZMax:F3}] | " +
                    $"DetectorZ target {detectorZTarget:F3} out of range [{detectorZMin:F3}, {detectorZMax:F3}]");
            }

            return MotionResult.Ok();
        }

        #endregion

        #region 向后兼容的重载 | Backward-Compatible Overloads

        /// <summary>
        /// 正算 FOD（无摆动，向后兼容）| Forward FOD (no swing, backward compatible)
        /// </summary>
        public double CalcFOD(double sourceZAbsolute, double stageRotationCenterZ)
        {
            return Math.Abs(sourceZAbsolute - stageRotationCenterZ);
        }

        /// <summary>
        /// 正算 FDD（无摆动，向后兼容）| Forward FDD (no swing, backward compatible)
        /// </summary>
        public double CalcFDD(double sourceZAbsolute, double detectorZAbsolute)
        {
            return Math.Abs(sourceZAbsolute - detectorZAbsolute);
        }

        /// <summary>
        /// 反算轴目标（无摆动，向后兼容）| Inverse axis targets (no swing, backward compatible)
        /// </summary>
        public (double sourceZTarget, double detectorZTarget) CalcAxisTargetsSimple(
            double targetFOD, double targetFDD,
            double stageRotationCenterZ, double sourceZOrigin, double detectorZOrigin)
        {
            double sourceZAbsolute = stageRotationCenterZ - targetFOD;
            double detectorZAbsolute = sourceZAbsolute + targetFDD;
            return (sourceZAbsolute - sourceZOrigin, detectorZAbsolute - detectorZOrigin);
        }

        #endregion
    }
}