% computeReferencePose
% :PROPERTIES:
% :header-args:matlab+: :tangle ../src/computeReferencePose.m
% :header-args:matlab+: :comments org :mkdirp yes
% :header-args:matlab+: :eval no :results none
% :END:
% <<sec:computeReferencePose>>

% This Matlab function is accessible [[file:src/computeReferencePose.m][here]].


function [WTr] = computeReferencePose(Dy, Ry, Rz, Dh)
% computeReferencePose - Compute the homogeneous transformation matrix corresponding to the wanted pose of the sample
%
% Syntax: [WTr] = computeReferencePose(Dy, Ry, Rz, Dh)
%
% Inputs:
%    - Dy, Ry, Rz, Dh -
%
% Outputs:
%    - WTr -

  %% Translation Stage
  Rty = [1 0 0 0;
         0 1 0 Dy;
         0 0 1 0;
         0 0 0 1];

  %% Tilt Stage - Pure rotating aligned with Ob
  Rry = [ cos(Ry) 0 sin(Ry) 0;
          0       1 0       0;
         -sin(Ry) 0 cos(Ry) 0;
          0       0 0       1];

  %% Spindle - Rotation along the Z axis
  Rrz = [cos(Rz) -sin(Rz) 0 0 ;
         sin(Rz)  cos(Rz) 0 0 ;
         0        0       1 0 ;
         0        0       0 1 ];


  %% Micro-Hexapod
  Rhx = [1 0           0;
         0 cos(Dh(4)) -sin(Dh(4));
         0 sin(Dh(4))  cos(Dh(4))];

  Rhy = [ cos(Dh(5)) 0 sin(Dh(5));
         0           1 0;
         -sin(Dh(5)) 0 cos(Dh(5))];

  Rhz = [cos(Dh(6)) -sin(Dh(6)) 0;
         sin(Dh(6))  cos(Dh(6)) 0;
         0           0          1];

  Rh = [1 0 0 Dh(1) ;
        0 1 0 Dh(2) ;
        0 0 1 Dh(3) ;
        0 0 0 1 ];

  Rh(1:3, 1:3) = Rhx*Rhy*Rhz;

  %% Total Homogeneous transformation
  WTr = Rty*Rry*Rrz*Rh;
end