QI Mingxuan
304 lines
16 KiB
C#
304 lines
16 KiB
C#
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 接近零（< 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
|
||
}
|
||
}
|