function []= contact_analytical_intersection(Rot_H2T,r_HH,CMDATA)

%{
Function for defining the intersection of the two spheres (HH and GF).
--------------------------------------------------------------------------
Syntax :
.
--------------------------------------------------------------------------
Function Description :
This function defines the conic section resulted from the intersection of
two spheres approximating the glenoid fossa and the humeral head. This
intersection curve is defined using the methods developed to address the
quadric surfaces intersection. The intersection of two spheres is
relatively easier than the general case of the intersection of two
arbitrary quadric surfaces. The resulted conic section is contaide in a
surface. Therefore, the intersection is called planar.

Ref: Joshua Zev Levin, 1978, Mathematical Models for Determining the
     Intersections of Quadric Surfaces.
--------------------------------------------------------------------------
%}
% define the spheres in the discriminant forms
    % discriminant matrix of GF sphere
Q_GF = [1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 -CMDATA.r_g^2]; 
    % discriminant matrix of GF sphere
Q_HH = [1 0 0 -r_HH(1); 0 1 0 -r_HH(2); 0 0 1 -r_HH(3);
        -r_HH(1) -r_HH(2) -r_HH(3) (norm(-r_HH)^2 - CMDATA.r^2)]; 
%-------------------------------------------------------------------------
% define the PARAs in the pencil of the two spheres.
% The general approach is construct the pencil and find the values alpha
% that makes the determinant of the pencil equal to zero.
syms alpha;

Pencil = Q_GF(1:3,1:3) + alpha*Q_HH(1:3,1:3);

alpha = solve(det(Pencil) == 0, alpha);
% As the subdiscriminant matrix of the two spheres are
% equal to I, we could have also used the eig command.
% alpha = -eig(Q_GF(1:3,1:3));

% define the discriminant of the parametrization surface (PS)
% note: the PS is one of the PARAs in the pencil that is more amendable to 
% parametrization. Here it makes no difference if one choses any of the
% three elements of alpha
Q_PS = Q_GF + alpha(1)*Q_HH; 
%-------------------------------------------------------------------------

% define the transformation that transforms the parametrization surface
% reulted above to the u-v surface of the u-v-w frame (the frame attached
% to the parametrization surface).
    % 1- rotation matrix
n = [2*Q_PS(4,1) 2*Q_PS(4,2) 2*Q_PS(4,3)]'; % normal vector of the parametrization surface    

w = n/norm(n); % the w vector of the frame fixed to the PS

if isequal(w,[0 0 1]')
    u = [1 0 0]';
    v = [0 1 0]';
elseif isequal(w,[0 0 -1]')
    u = [1 0 0]';
    v = [0 -1 0]';
else
u = cross(n, [0 0 1]'); u = u/norm(u); % the u vector of the frame fixed to the PS
                                       % defined regarding the Z axis of the thorax frame
v = cross(n, u); v = v/norm(v);        % the v vector of the frame fixed to the PS
                                       % defined regarding the Z axis of the thorax frame                                       
end

R_PS2T = [u v w]; % rotationmatrix from the PS frame to the thorax frame
                  % local -----> global
                  
    % 2- center of the PS frame
% the center is considered as the point of the intersection of the PS and
% the line goes through the origin of the glenoid frame in the direction of
% the normal vector of PS
t = -Q_PS(4,4)/(norm(n))^2; % t of the contact point

O_PS = [n(1)*t n(2)*t n(3)*t]'; % the origin of the PS fixed frame

% define the homogeneous transformation from PS to thorax (or maybe later on glenoid)
H_PS2T = [R_PS2T(1,:) O_PS(1);
          R_PS2T(2,:) O_PS(2);
          R_PS2T(3,:) O_PS(3);
          zeros(1,3)       1];
%-------------------------------------------------------------------------
      
% define the discriminant matrix of the PS in the u-v-w frame:
Q_PS_in_uvw = H_PS2T'*Q_PS*H_PS2T;

% transform any surface in the pencil to the u-v-w frame
% for the sake of simplicity Q_GF is choosen
Q_GF_in_uvw = H_PS2T'*Q_GF*H_PS2T;

% the QSIC is achieved by imposing the w coordinate of the quadric surface
% above equal to zero, meaning that we are only looking for those points on
% this quadric surface that are contained in the parametrization surface.
Q_CS_in_uvw = [Q_GF_in_uvw(1,[1:2,4]);
               Q_GF_in_uvw(2,[1:2,4]);
               Q_GF_in_uvw(4,[1:2,4])];% the discriminant matrix of the conic section in the u-v-w frame

% define the numerical values of the points of the resulted conic
% section in the u-v-w frame
% Note: we should have further transform the resulted conic section to its
% canonical form. But, given that the underquestion problem is far simpler
% than the general case, we expect to have it right away in the canonical
% form.
discritization_CS = 40;
t_PS = linspace(0,2*pi,discritization_CS); % the canonical parametrization variable from 0 to 2pi
CS_point_in_uvw = [sqrt(abs(Q_CS_in_uvw(3,3)))*cos(t_PS);
                   sqrt(abs(Q_CS_in_uvw(3,3)))*sin(t_PS);
                   zeros(1,discritization_CS);
                   ones(1,discritization_CS)];

% define the numerical values of the points of the resulted conic
% section in the thorax frame
CS_point_in_T = H_PS2T*CS_point_in_uvw; % intersection points in the thorax frame

% define the numerical values of the points of the resulted conic
% section in the humeral frame
Rot_T2H = Rot_H2T';
translation = -Rot_T2H*r_HH;
H_T2H = [Rot_T2H(1,:) translation(1);
         Rot_T2H(2,:) translation(2);
         Rot_T2H(3,:) translation(3);
         zeros(1,3)               1];

CS_point_in_H = H_T2H*CS_point_in_T; % intersection points in the humeral frame
%-------------------------------------------------------------------------
% define the cylindrical coordinates of the intersection curve
        % 1- phi (latitude)
y_max = max(CS_point_in_H(2,:)); % the lowest point
y_min = min(CS_point_in_H(2,:)); % the highest point

phi_min   = acos(y_max/CMDATA.r); % minimum latitude angle
phi_max   = acos(y_min/CMDATA.r); % maximum latitude angle

        % 2- theta (longtitude)
[x_max, flag_xmax] = max(CS_point_in_H(1,:)); % the most longtitudinal point
theta_max = atan2(CS_point_in_H(3,flag_xmax), x_max);

if (theta_max >= -pi & theta_max <= 0) % to fix the output of the atan2
    theta_max = 2*pi + theta_max;
end

[x_min, flag_xmin] = min(CS_point_in_H(1,:)); % the leat longtitudinal point
theta_min = atan2(CS_point_in_H(3,flag_xmin), x_min);

if (theta_min >= -pi & theta_min <= 0) % to fix the output of the atan2
    theta_min = 2*pi + theta_min;
end











h = CMDATA.h;           % receive h_i s in the frame {H}
a = CMDATA.a;           % receive associtaed surface area of the h_i s

for i = 1:size(h,1)-1          % associated with longtitude (theta)          
    for j = 2:size(h,2)-1      % associated with latitude (phi)   
        
        h_inT{i,j} = r_HH + Rot_H2T*h{i,j}; % hi in the {T} frame
        
        u(i,j) = norm(h_inT{i,j},2) - CMDATA.r_g; % the penetration of the point hi
                                                  % in the glenoid fossa