XplorePlane/XP.Calibration/Core/Projection.cs

using XP.Calibration.Models;

namespace XP.Calibration.Core;

/// <summary>
/// TIGRE 投影模型 (角点法) | TIGRE projection model (corner-based, Siddon)
/// </summary>
public static class Projection
{
    private struct Point3D
    {
        public double X, Y, Z;
        public Point3D(double x, double y, double z) { X = x; Y = y; Z = z; }
    }

    /// <summary>
    /// 计算物体原点在探测器上的投影像素坐标
    /// </summary>
    /// <param name="scanAngle">扫描角 (rad)</param>
    /// <param name="ay">旋转轴倾斜角 (rad)</param>
    /// <param name="detX">探测器 dYaw/Rx (rad)</param>
    /// <param name="detY">探测器 dPitch/Ry (rad)</param>
    /// <param name="detZ">探测器 dRoll/Rz (rad)</param>
    /// <param name="offOrigX">物体 X 偏移 = -R (mm)</param>
    /// <param name="offDetecU">探测器 U 偏移 (mm)</param>
    /// <param name="offDetecV">探测器 V 偏移 (mm)</param>
    /// <param name="gp">几何参数</param>
    /// <param name="uPx">输出: U 像素坐标</param>
    /// <param name="vPx">输出: V 像素坐标</param>
    public static void ProjectPoint(
        double scanAngle, double ay,
        double detX, double detY, double detZ,
        double offOrigX, double offDetecU, double offDetecV,
        GeoParams gp,
        out double uPx, out double vPx)
    {
        double ODD = gp.DSD - gp.DSO;
        var S = new Point3D(gp.DSO, 0, 0);

        // 探测器角点初始化
        double py = gp.PixelSize * (0 - (double)gp.NDetecU / 2 + 0.5);
        double pz = gp.PixelSize * ((double)gp.NDetecV / 2 - 0.5 - 0);
        double puy = gp.PixelSize * (1 - (double)gp.NDetecU / 2 + 0.5);
        double pvz = gp.PixelSize * ((double)gp.NDetecV / 2 - 0.5 - 1);

        var P = new Point3D(0, py, pz);
        var Pu0 = new Point3D(0, puy, pz);
        var Pv0 = new Point3D(0, py, pvz);

        // Step 1: rollPitchYaw (探测器自身旋转)
        RollPitchYaw(detZ, detY, detX, ref P);
        RollPitchYaw(detZ, detY, detX, ref Pu0);
        RollPitchYaw(detZ, detY, detX, ref Pv0);

        // 平移回探测器位置
        P.X -= ODD; Pu0.X -= ODD; Pv0.X -= ODD;

        // Step 2: offDetecU/V 偏移
        P.Y += offDetecU; P.Z += offDetecV;
        Pu0.Y += offDetecU; Pu0.Z += offDetecV;
        Pv0.Y += offDetecU; Pv0.Z += offDetecV;

        // Step 3: eulerZYZ 扫描旋转
        EulerZYZ(scanAngle, ay, 0, ref P);
        EulerZYZ(scanAngle, ay, 0, ref Pu0);
        EulerZYZ(scanAngle, ay, 0, ref Pv0);
        EulerZYZ(scanAngle, ay, 0, ref S);

        // Step 4: offOrigin 偏移
        P.X -= offOrigX; Pu0.X -= offOrigX; Pv0.X -= offOrigX;
        S.X -= offOrigX;

        // deltaU, deltaV
        var deltaU = new Point3D(Pu0.X - P.X, Pu0.Y - P.Y, Pu0.Z - P.Z);
        var deltaV = new Point3D(Pv0.X - P.X, Pv0.Y - P.Y, Pv0.Z - P.Z);

        // 射线: S → 原点
        var ray = new Point3D(-S.X, -S.Y, -S.Z);

        // 探测器平面法线
        var normal = new Point3D(
            deltaU.Y * deltaV.Z - deltaU.Z * deltaV.Y,
            deltaU.Z * deltaV.X - deltaU.X * deltaV.Z,
            deltaU.X * deltaV.Y - deltaU.Y * deltaV.X);

        // 射线与探测器平面交点
        double pfsDotN = (P.X - S.X) * normal.X + (P.Y - S.Y) * normal.Y + (P.Z - S.Z) * normal.Z;
        double rayDotN = ray.X * normal.X + ray.Y * normal.Y + ray.Z * normal.Z;

        if (Math.Abs(rayDotN) < 1e-15)
        {
            uPx = vPx = -1;
            return;
        }

        double t = pfsDotN / rayDotN;
        var hit = new Point3D(S.X + t * ray.X, S.Y + t * ray.Y, S.Z + t * ray.Z);
        var hitP = new Point3D(hit.X - P.X, hit.Y - P.Y, hit.Z - P.Z);

        // 投影到像素坐标
        double dU2 = deltaU.X * deltaU.X + deltaU.Y * deltaU.Y + deltaU.Z * deltaU.Z;
        double dV2 = deltaV.X * deltaV.X + deltaV.Y * deltaV.Y + deltaV.Z * deltaV.Z;
        double pixelU = (hitP.X * deltaU.X + hitP.Y * deltaU.Y + hitP.Z * deltaU.Z) / dU2;
        double pixelV = (hitP.X * deltaV.X + hitP.Y * deltaV.Y + hitP.Z * deltaV.Z) / dV2;

        uPx = pixelU;
        vPx = (gp.NDetecV - 1) - pixelV; // TIGRE v 翻转
    }

    /// <summary>
    /// rollPitchYaw: Rz(dRoll) * Ry(dPitch) * Rx(dYaw)
    /// </summary>
    private static void RollPitchYaw(double dRoll, double dPitch, double dYaw, ref Point3D p)
    {
        double cr = Math.Cos(dRoll), sr = Math.Sin(dRoll);
        double cp = Math.Cos(dPitch), sp = Math.Sin(dPitch);
        double cy = Math.Cos(dYaw), sy = Math.Sin(dYaw);

        double x = p.X, y = p.Y, z = p.Z;
        p.X = cr * cp * x + (cr * sp * sy - sr * cy) * y + (cr * sp * cy + sr * sy) * z;
        p.Y = sr * cp * x + (sr * sp * sy + cr * cy) * y + (sr * sp * cy - cr * sy) * z;
        p.Z = -sp * x + cp * sy * y + cp * cy * z;
    }

    /// <summary>
    /// eulerZYZ: Rz(alpha) * Ry(theta) * Rz(psi)
    /// </summary>
    private static void EulerZYZ(double alpha, double theta, double psi, ref Point3D p)
    {
        double ca = Math.Cos(alpha), sa = Math.Sin(alpha);
        double ct = Math.Cos(theta), st = Math.Sin(theta);
        double cp = Math.Cos(psi), sp = Math.Sin(psi);

        double x = p.X, y = p.Y, z = p.Z;
        p.X = (ca * ct * cp - sa * sp) * x + (-ca * ct * sp - sa * cp) * y + ca * st * z;
        p.Y = (sa * ct * cp + ca * sp) * x + (-sa * ct * sp + ca * cp) * y + sa * st * z;
        p.Z = -st * cp * x + st * sp * y + ct * z;
    }
}