gtCrystRodriguesTestCore.m

function [Rout, goodrids, goodlines, Rdist, Redge] = ...
         gtCrystRodriguesTestCore(rline, rid, fzone_acc, fzone_ext, tol_Rdist)
% FUNCTION [Rout, goodrids, goodlines, Rdist, Redge] = ...
%          gtCrystRodriguesTestCore(rline, rid, fzone_acc, fzone_ext, tol)
% 
% Performs an orientation test in Rodrigues space on the specified plane
% normals. Gives which plane normals constitute a consistent crystal and 
% its fitted Rodrigues vector.
% Also determines how far this point is from the nearest boundary of the
% fundamental zone, and whether this distance is smaller than the specified
% limit.
%
% Note that this procedure is independent from how the Rodrigues vectors
% were calculated or their relation to crystal orientations. 
% 
%
% INPUT
%   rline        - lines in Rodrigues space (n,6)
%   rid          - reflection id-s of rlines; it is used to identify which 
%                  lines belong to the same plane normal, as one reflection
%                  can have multiple lines; its values can be any numbers, 
%                  as long as there is one unique number for one reflection;
%                  size (n,1)
%   fzone_acc    - the exact fundamental zone in Rodrigues space
%   fzone_ext    - extended fundamental zone to account for inaccuracy of
%                  data
%   tol_Rdist    - max. distance between intersecting lines in Rodrigues 
%                  space (accounts for data inaccuracy)
%
% OUTPUT
%   Rout         - fitted Rodrigues vector
%   goodrids     - list of unique reflection id-s that fit the solution
%   goodlines    - list of line indices that fit the solution
%   Rdist        - distance between Rout and the nearest fundamental zone 
%                  boundary plane in Rodrigues space
%   Redge        - true if Rdist is smaller than the specified tolerance
%   

tol_Rdist = tol_Rdist + 1e-15;

nof_lines = size(rline,1);

% No. of unique reflection id-s
nurid = length(unique(rid));
                                      
Rout      = [];
Rdist     = [];
Redge     = [];
goodrids  = [];
goodlines = [];


% Tolerance for two lines to be considered parallel in Rod. space. 
% A reasonable constant value has to be chosen dependent on 'tol_Rdist' 
% that scales with the expected angular error. Here it is used directly and
% it seems to work to find the parallel lines:
tol_cos = cos(tol_Rdist);
    

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Find line intersections -> base nodes
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% The distances, dot products and intersection points of all pair
% combinations of the input lines are calculated. 

Ld = zeros(nof_lines);   % distance of Rlines pairwise
Lc = zeros(nof_lines);   % angle of Rlines pairwise


% Store line indices for identification
[Lidb,Lida] = meshgrid(1:nof_lines);

% Reflection id-s as in the input
Rida = rid(:,ones(1,nof_lines));
Ridb = Rida';


% Determine intersections of all lines with one another in Rodr. space
for ii = 1:nof_lines-1
	% distance pair-wise
	Ld(ii,ii+1:end) = gtMathsLinesDists(rline(ii,:),rline(ii+1:end,:));  
	
	% dot product of the direction vectors pair-wise (=cosine of the angles)
	Lc(ii,ii+1:end) = sum(rline(ii(ones(nof_lines-ii,1)),4:6).*rline(ii+1:end,4:6),2);
end

% Which lines are identical pair-wise (small distance and angle) 
%   -> double detection of Friedel pairs or wrong indexing
Lp = (Ld <= tol_Rdist) & (abs(Lc) >= tol_cos);

% Make matrices symmetric
Ld = Ld + Ld';
Lp = Lp | Lp';


% Which lines are closer than the tolerance ? 
Lnear = (Ld <= tol_Rdist);
Lnear(sub2ind([nof_lines nof_lines], 1:nof_lines, 1:nof_lines)) = false;


% Intersections of the lines that are close but not identical (parallel) 
% constitute the base set:
baseset = Lnear & (~Lp);

% Line indices in the base set
LAbase = Lida(baseset);
LBbase = Lidb(baseset);

% Reflection id-s in the base set
RAbase = Rida(baseset);
RBbase = Ridb(baseset);


% Find intersection points of the base set -> Nodes coordinates Nbase
Nbase = zeros(length(LAbase),3);

LBbaseuni = unique(LBbase);

for ii = 1:length(LBbaseuni)
   
    % Do the identical indices at once
    inds = (LBbase == LBbaseuni(ii));
    
    Nbase(inds,:) = gtMathsLinePairsIntersections(rline(LBbaseuni(ii),:), ...
                    rline(LAbase(inds),:));
end


% Which nodes are in the (extended) fundamental zone ?
inzone = gtMathsIsPointInPolyhedron(Nbase,fzone_ext);

% Keep only base nodes in the fund. zone
Nbase  = Nbase(inzone,:);  % Base node coordinates
LAbase = LAbase(inzone);   % Line A id-s corresponding to base nodes
LBbase = LBbase(inzone);   % Line B id-s corresponding to base nodes
RAbase = RAbase(inzone);   % Reflection A id-s corresponding to base nodes
RBbase = RBbase(inzone);   % Reflection B id-s corresponding to base node


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Find neighbouring base nodes
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Check which base nodes are close to each other -> neighbouring nodes
told = tol_Rdist^2;

nof_Nbase = size(Nbase,1);        % no. of nodes
Nneib = false(nof_Nbase);         % neighbouring nodes


% !!! This could be a parfor loop 
for ii = 1:nof_Nbase-1
  
  dist = [Nbase(ii,1)-Nbase(:,1), ...
          Nbase(ii,2)-Nbase(:,2), ...
          Nbase(ii,3)-Nbase(:,3)];
  
  dist = sum(dist.*dist,2);
  
  distok = (dist <= told);
  
  Nneib(ii,:) = distok;
end

% Set diagonals to false
Nneib(sub2ind([nof_Nbase nof_Nbase],1:nof_Nbase,1:nof_Nbase)) = false;



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Find the highest concentration of neighbouring nodes
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Loop through neighbouring nodes to find the highest agglomeration

Nnurids = zeros(nof_Nbase,1); % number of planes belonging to the nodes

finish = false;

intsc.Rvec = [];

% 'Nneib' is a logical array, true where the two nodes are neighbours.
% Loop as long as there are nodes remaining to be checked

while any(Nneib(:))  % !!! check if this loop is needed; maybe can squeeze Nneib

    Nurids = cell(1,nof_Nbase);
    Nlis   = cell(1,nof_Nbase);

    % Loop through all nodes
    
	for ii = 1:nof_Nbase
		
        inb = Nneib(:,ii);            % indices of (remaining) neighbouring nodes

		urids = RAbase(inb);          % reflection A id-s of remaining neighbours
		urids = [urids; RBbase(inb)]; % add reflection B id-s of remaining neighbours
		urids = unique(urids);        % all reflections id-s that are close to node ii
		
        Nurids{ii}  = urids;          % ref id-s being close to this node
		Nnurids(ii) = length(urids);  % no. of those ref id-s

		linds = LAbase(inb);          % line A indices of remaining neighbours
		linds = [linds; LBbase(inb)]; % line B indices of remaining neighbours
		linds = unique(linds);        % all line indices close to node ii
		
        Nlis{ii} = linds;             % uniqued line indices close to node ii

		% if urids includes all plane normals
		if length(urids)==nurid
			
			% Rodrigues vector is the fitted intersection of those lines
			Rvec = gtMathsLinesIntersection(rline(linds,:))';
			
			% If Rvec is in the fund. zone, we found the solution
			if gtMathsIsPointInPolyhedron(Rvec,fzone_acc)
				finish = true;
				break  % for loop
			end
		end
		
	end

	% Solution found with all reflections
	if finish
		intsc.lines = linds;
		intsc.urids = urids;
		intsc.Rvec  = Rvec;
		break % while loop
	end
	
	
	% node index with most reflection id-s
	[~,ibn] = max(Nnurids); 

	% Rodrigues vector
	Rvec = gtMathsLinesIntersection(rline(Nlis{ibn},:))';

	
	% Test if Rvec is in the accurate fund. zone 
	if gtMathsIsPointInPolyhedron(Rvec,fzone_acc)
		intsc.lines = Nlis{ibn};
		intsc.urids = Nurids{ibn};
		intsc.Rvec  = Rvec;
		break % while loop
	end 

	
	% Test if Rvec is in the extended fund. zone 
	if gtMathsIsPointInPolyhedron(Rvec,fzone_ext)
		intsc.lines = Nlis{ibn};
		intsc.urids = Nurids{ibn};
		
		% Use its intersections with the accurate fund. zone, instead of Rvec
		Rsecs = gtMathsLinePolyhedronIntersections([0 0 0 Rvec],fzone_acc);
		
		% Distances between the two intersections and Rvec
		Rsecdist1 = Rvec - Rsecs(1,:);
        Rsecdist1 = sqrt(Rsecdist1*Rsecdist1');

        Rsecdist2 = Rvec - Rsecs(2,:);
        Rsecdist2 = sqrt(Rsecdist2*Rsecdist2');
       
		% Use the closer intersection
		if Rsecdist1 < Rsecdist2
			intsc.Rvec = Rsecs(1,:);
		else
			intsc.Rvec = Rsecs(2,:);
		end
			
		break % while loop
	end
	
	
	% The node with the most plane normals doesn't provide a solution in the 
	% extended fund. zone. Ignore best node in the neighbourhood matrix, and start the
	% loop again:
	Nneib(Nneib(:,ibn),:) = false;  
	Nneib(:,Nneib(ibn,:)) = false;
			
	Nneib(ibn,:) = false;
	Nneib(:,ibn) = false;
		
end
	

if isempty(intsc.Rvec)
	return
end


%%%%%%%%%%%%%%%%%%%%%%%
%% Finish up 
%%%%%%%%%%%%%%%%%%%%%%%

Rout = intsc.Rvec;
goodrids  = intsc.urids;
goodlines = intsc.lines;


% This probably should not happen, but for safety 
if length(goodlines) > length(goodrids)
    disp('WARNING! Multiple signed hkl-s found for some reflections.')
    [goodrids,ind] = unique(rid(goodlines));
    goodlines = goodlines(ind);
end
    

% Minimum distance from edge of exact fundamental zone:

% Fundamental zone face directions should be normalised before input
%fzonedirnorm = fzone_acc(:,4:6)./repmat(sqrt(sum(fzone_acc(:,4:6).*fzone_acc(:,4:6),2)),1,3);

% Difference vector between Rout and fund. zone face position vectors
diffRout = Rout(ones(size(fzone_acc,1),1),:) - fzone_acc(:,1:3);

% Signed distances between Rout point and fund. zone faces
Rdistall = sum(diffRout.*fzone_acc(:,4:6),2);

% Distance between Rout and nearest fund. zone face
Rdist = min(abs(Rdistall));

% Distance smaller than tolerance ?
Redge = Rdist <= tol_Rdist;

% If R vector is slightly out of the fundamental zone boundaries, set it
% back on the nearest boundary plane.
% if Rdistall(minind) > 0
% 	Rout  = Rout - Rdistall(minind)*fzone_acc(minind,4:6);
% 	Rdist = 0;
% end

	
end  % of function