kill(all);

/* ImagX parameters: alpha, TX, TY, TZ, PX, PY, PZ, AID, SAD, %beta[x], %beta[y], %beta[z] */

disp("************ ImagX matrices ************")$

disp("** Tube **")$
Ralpha: matrix([cos(alpha),  0, sin(alpha), 0],
               [0,           1, 0,          0],
               [-sin(alpha), 0, cos(alpha), 0],
               [0 ,          0, 0,          1])$
T: matrix([1, 0, 0, TX    ],
          [0, 1, 0, TY    ],
          [0, 0, 1, TZ+SAD],
          [0, 0, 0, 1     ])$
M: Ralpha.T$
Xprime: M.matrix([0],[0],[0],[1])$

disp("** Flat panel **")$
Rbetax: matrix([1, 0,             0,              0],
               [0, cos(%beta[x]), -sin(%beta[x]), 0],
               [0, sin(%beta[x]), cos(%beta[x]),  0],
               [0, 0 ,            0,              1])$
Rbetay: matrix([cos(%beta[y]),  0, sin(%beta[y]), 0],
               [0,              1, 0,             0],
               [-sin(%beta[y]), 0, cos(%beta[y]), 0],
               [0 ,             0, 0,             1])$
Rbetaz: matrix([cos(%beta[z]), -sin(%beta[z]), 0, 0],
               [sin(%beta[z]), cos(%beta[z]),  0, 0],
               [0,             0,              1, 0],
               [0 ,            0,              0, 1])$
TF: matrix([1, 0, 0, PX    ],
           [0, 1, 0, PY    ],
           [0, 0, 1, PZ+AID],
           [0, 0, 0, 1     ])$
/* ORIGINAL VERSION: MF: Ralpha.Rbetaz.TF.Rbetay.Rbetax$ */
/* Modification:*/   MF: Ralpha.TF.Rbetay.Rbetax.Rbetaz$

disp("** Projecting a point on the flat panel **")$
/* Compute projection matrix */
u:      MF.matrix([1],[0],[0],[1])-MF.matrix([0],[0],[0],[1])$
v:      MF.matrix([0],[1],[0],[1])-MF.matrix([0],[0],[0],[1])$
Fprime: MF.matrix([0],[0],[0],[1])$

P:matrix([x],[y],[z])$
Xprime: submatrix(4,Xprime)$
Fprime: submatrix(4,Fprime)$
t: P-Xprime$
u: submatrix(4,u)$
v: submatrix(4,v)$

D: u$
D: addcol(D,v,-1*t)$
D: determinant(D)$

/* Here, only the numerator of anum and bnum is computed */
anum:P-Fprime$
anum: addcol(anum, v,-1*t)$
anum: determinant(anum)$
anum: expand(anum)$

bnum:u$
bnum: addcol(bnum, P-Fprime,-1*t)$
bnum: determinant(bnum)$
bnum: expand(bnum)$

/* And this is the denominator */
D:   expand(D)$

disp("** Projecton matrix **")$
/* Hence the projection matrix */
Pimagx:matrix([coeff(anum,x),coeff(anum,y),coeff(anum,z),anum-coeff(anum,x)*x-coeff(anum,y)*y-coeff(anum,z)*z],
              [coeff(bnum,x),coeff(bnum,y),coeff(bnum,z),bnum-coeff(bnum,x)*x-coeff(bnum,y)*y-coeff(bnum,z)*z],
              [coeff(D,x),   coeff(D,y),   coeff(D,z),   D   -coeff(D,x)*x   -coeff(D,y)*y   -coeff(D,z)*z]);

disp("************ RTK rotations ************")$
GantryAngle: alpha+%beta[y];
OutOfPlaneAngle: %beta[x];
InPlaneAngle: %beta[z];

MRInPlane: matrix([cos(-InPlaneAngle), -sin(-InPlaneAngle), 0, 0],
                  [sin(-InPlaneAngle), cos(-InPlaneAngle),  0, 0],
                  [0,                  0,                   1, 0],
                  [0 ,                 0,                   0, 1])$
MRGantry: matrix([cos(-GantryAngle),  0, sin(-GantryAngle), 0],
                 [0,                  1, 0,                 0],
                 [-sin(-GantryAngle), 0, cos(-GantryAngle), 0],
                 [0 ,                 0, 0,                 1])$
MROutOfPlane: matrix([1, 0,                0,                 0],
                     [0, cos(-OutOfPlaneAngle), -sin(-OutOfPlaneAngle), 0],
                     [0, sin(-OutOfPlaneAngle), cos(-OutOfPlaneAngle),  0],
                     [0, 0 ,               0,                 1])$
MR: MRInPlane . MROutOfPlane . MRGantry$
MR: trigexpand(MR)$

disp("************ RTK translations ************")$
MRGantryAdjustment: matrix([cos(-%beta[y]),  0, sin(-%beta[y]), 0],
                           [0,               1, 0,              0],
                           [-sin(-%beta[y]), 0, cos(-%beta[y]), 0],
                           [0 ,              0, 0,              1])$
/* Source position */
Source: MRInPlane . MROutOfPlane . MRGantryAdjustment . matrix([TX], [TY], [TZ+SAD], [1]);
SourceOffsetX:              Source[1][1];
SourceOffsetY:              Source[2][1];
SourceToIsocenterDistance:  Source[3][1];
/* Compose MF with RTK rotations to have the detector origin */
MF:     MRInPlane . MROutOfPlane . MRGantryAdjustment . matrix([PX], [PY], [PZ+AID], [1]);
ProjectionOffsetX:        MF[1][1];
ProjectionOffsetY:        MF[2][1];
SourceToDetectorDistance: -MF[3][1] + SourceToIsocenterDistance;

/*** RTK projection matrix ***/
MP1: matrix([1, 0, SourceOffsetX-ProjectionOffsetX],
            [0, 1, SourceOffsetY-ProjectionOffsetY],
            [0, 0, 1])$
/* Everything has been multiplied by -1 which is equivalent in homogeneous coordinates but allows exact correspondence between matrices */
MP2: matrix([SourceToDetectorDistance,0,0,0],
            [0,SourceToDetectorDistance,0,0],
            [0,0,-1,SourceToIsocenterDistance]);
MP3: matrix([1, 0, 0, -SourceOffsetX],
            [0, 1, 0, -SourceOffsetY],
            [0, 0, 1, 0],
            [0, 0, 0, 1])$
MP: MP1.MP2.MP3.MR$

disp("************ Check correspondence of matrices ************")$
trigsimp(ratsimp(Pimagx-MP));

disp("************ Parameter correspondence ************")$
GantryAngle: alpha+%beta[y];
OutOfPlaneAngle: %beta[x];
InPlaneAngle: %beta[z];
SourceOffsetX;
SourceOffsetY;
SourceToIsocenterDistance;
ProjectionOffsetX;
ProjectionOffsetY;
SourceToDetectorDistance;