level_model_slice

LEVEL_MODEL_SLICE - level 3D points for slicing at z=0
 
 XYZ = LEVEL_MODEL_SLICE(NODES, LEVEL)
 XYZ = LEVEL_MODEL_SLICE(NODES, LEVEL, SN)
 Transforms a list of 3D points such that level is a slice at z=0.
    NODES: n_nodes x 3 list of point coordinates 
    LEVEL: definition of the slice(s). Can be:
     - scalar: number of slices along the z axis ordered max to min
     - vector (N x 3): intercepts with the coordinate axis
     - struct:
       .centre (N x 3): centre point for transformation
        .normal_angle (1 x 4) [nx, ny, nz, theta]: normal vector and angle 
          of rotation around the axis defined by it (right hand rule).
         The model will be rotated such that the normal vector becomes 
         [0,0,1].         
        .rotation_matrix (3 x 3)
        An error is thrown if both normal_angle and rotation_matrix are present.
   SN     : slice number to use if level defines multiple slices (default: 1).
    XYZ     : NODES transformed such that level is a slice at z=0.

 MDL = LEVEL_MODEL_SLICE(MDL,__) 
 Modifies the nodes of an EIDORS fwd_model structure MDL. Can be used in
 place of NODES with every other syntax.

 {XYZ} = LEVEL_MODEL_SLICE(MDL, LEVEL, 'all') 
 {XYZ} = LEVEL_MODEL_SLICE(NODES, LEVEL, 'all') 
 Returns a cell array (one per slice) of transformed NODES (or MDL.nodes).

 N = LEVEL_MODEL_SLICE(LEVEL) returns the number of slices defined in LEVEL.

 [C,M] = LEVEL_MODEL_SLICE(LEVEL, SN) (discouraged) returns the center C and
 rotation matrix M of slice SN (optional, default 1). An error is thrown if
 level is scalar. Note that two outputs must always be requested for this 
 syntax.  

 [C,M] = LEVEL_MODEL_SLICE(NODES, LEVEL, SN) (discouraged) allows to obtain C
 and M when LEVEL is scalar. Note that two outputs must always be requested for
 this syntax.

0001 function [out, out2, out3] = level_model_slice(varargin)
0002 %LEVEL_MODEL_SLICE - level 3D points for slicing at z=0
0003 %
0004 % XYZ = LEVEL_MODEL_SLICE(NODES, LEVEL)
0005 % XYZ = LEVEL_MODEL_SLICE(NODES, LEVEL, SN)
0006 % Transforms a list of 3D points such that level is a slice at z=0.
0007 %    NODES: n_nodes x 3 list of point coordinates
0008 %    LEVEL: definition of the slice(s). Can be:
0009 %     - scalar: number of slices along the z axis ordered max to min
0010 %     - vector (N x 3): intercepts with the coordinate axis
0011 %     - struct:
0012 %       .centre (N x 3): centre point for transformation
0013 %        .normal_angle (1 x 4) [nx, ny, nz, theta]: normal vector and angle
0014 %          of rotation around the axis defined by it (right hand rule).
0015 %         The model will be rotated such that the normal vector becomes
0016 %         [0,0,1].
0017 %        .rotation_matrix (3 x 3)
0018 %        An error is thrown if both normal_angle and rotation_matrix are present.
0019 %   SN     : slice number to use if level defines multiple slices (default: 1).
0020 %    XYZ     : NODES transformed such that level is a slice at z=0.
0021 %
0022 % MDL = LEVEL_MODEL_SLICE(MDL,__)
0023 % Modifies the nodes of an EIDORS fwd_model structure MDL. Can be used in
0024 % place of NODES with every other syntax.
0025 %
0026 % {XYZ} = LEVEL_MODEL_SLICE(MDL, LEVEL, 'all')
0027 % {XYZ} = LEVEL_MODEL_SLICE(NODES, LEVEL, 'all')
0028 % Returns a cell array (one per slice) of transformed NODES (or MDL.nodes).
0029 %
0030 % N = LEVEL_MODEL_SLICE(LEVEL) returns the number of slices defined in LEVEL.
0031 %
0032 % [C,M] = LEVEL_MODEL_SLICE(LEVEL, SN) (discouraged) returns the center C and
0033 % rotation matrix M of slice SN (optional, default 1). An error is thrown if
0034 % level is scalar. Note that two outputs must always be requested for this
0035 % syntax.
0036 %
0037 % [C,M] = LEVEL_MODEL_SLICE(NODES, LEVEL, SN) (discouraged) allows to obtain C
0038 % and M when LEVEL is scalar. Note that two outputs must always be requested for
0039 % this syntax.
0040 
0041 % (C) 2006-2022 Andy Adler and Bartek Grychtol
0042 % License: GPL version 2 or version 3
0043 % $Id: level_model_slice.m 6350 2022-04-25 20:40:24Z bgrychtol $
0044 
0045 if nargin==1 && ischar(varargin{1}) && strcmp(varargin{1}, 'UNIT_TEST')
0046   do_unit_test; return
0047 end
0048 
0049 if nargout == 1 || nargout == 3
0050     if nargin == 1
0051         % N = LEVEL_MODEL_SLICE(LEVEL) returns the number of slices defined in LEVEL
0052         out = get_number_of_slices(varargin{1});
0053     else
0054         % XYZ = LEVEL_MODEL_SLICE(NODES, LEVEL)
0055         % XYZ = LEVEL_MODEL_SLICE(NODES, LEVEL, SN)
0056         if nargin == 3 && ischar(varargin{3}) && strcmp(varargin{3},'all')
0057            n_slices = get_number_of_slices(varargin{2});
0058            if isstruct(varargin{1}), varargin{1}=varargin{1}.nodes; end
0059            for i = n_slices:-1:1
0060               out(i,1) = cell(1);
0061               [out{i,1}, C(i,:), M] = level_model_slice(varargin{1},varargin{2}, i);
0062            end
0063         else % nargin==3
0064             [C, M] = matrix_from_level(varargin{:});
0065             SHIFT = [C(1), C(2), C(3)] * M';
0066             SHIFT(3) = 0;
0067             % accept fwd_model as input
0068             if isstruct(varargin{1})
0069                out = varargin{1};
0070                % (nodes - C) * M' + SHIFT
0071                out.nodes = bsxfun(@plus, bsxfun(@minus, out.nodes, C ) * M', SHIFT);
0072             else
0073                % (nodes - C) * M' + SHIFT
0074                out = bsxfun(@plus, bsxfun(@minus, varargin{1}, C ) * M', SHIFT);
0075             end
0076             if nargout == 3
0077                 out2 = C;
0078                 out3 = M;
0079             end
0080         end
0081     end
0082 elseif nargout == 2
0083     if nargin < 3
0084         % [C,M] = LEVEL_MODEL_SLICE(LEVEL)
0085         % [C,M] = LEVEL_MODEL_SLICE(LEVEL, SN)
0086         [out, out2] = matrix_from_level(varargin{:});
0087     else
0088         % [C,M] = LEVEL_MODEL_SLICE(NODES, LEVEL, SN)
0089         [out, out2] = matrix_from_number(varargin{:});
0090     end
0091 end
0092 
0093 function N = get_number_of_slices(level)  
0094     if isnumeric(level) && isscalar(level)
0095         N = level;
0096     elseif isnumeric(level)
0097         N = size(level,1);
0098     elseif isstruct(level) && isscalar(level)
0099         N = size(level.centre, 1);
0100     else
0101         error('level must be numeric or a scalar struct');
0102     end
0103 
0104 function [C,M] = matrix_from_level(varargin)
0105     nodes = []; sn = 1;
0106     switch nargin
0107         case 3 % nodes, level, sn
0108             nodes = varargin{1}; level = varargin{2}; sn = varargin{3};
0109         case 2 % level & sn or nodes & level
0110             if isnumeric(varargin{2}) && isscalar(varargin{2}) 
0111                 level = varargin{1}; sn = varargin{2};
0112             else
0113                 nodes = varargin{1}; level = varargin{2};
0114             end
0115         case 1 % level
0116             level = varargin{1};
0117     end
0118 
0119     if isnumeric(level) && isscalar(level)
0120         if isempty(nodes)
0121             error('Need nodes to calculate with scalar numeric level');
0122         else
0123             [C, M] = matrix_from_number(nodes, level, sn);
0124         end
0125     elseif isnumeric(level)
0126         [C, M] = matrix_from_intercept(level, sn);
0127     elseif isstruct(level) && isscalar(level)
0128         [C, M] = matrix_from_struct(level, sn);
0129     else
0130         error('level must be numeric or a scalar struct');
0131     end
0132 
0133     
0134 function [C, M] = matrix_from_number(nodes, num_levels, sn)
0135     if isstruct(nodes)
0136         nodes = nodes.nodes;
0137     end
0138     M = diag([1,1,1]);
0139     
0140     zmax= max(nodes(:,3));
0141     zmin= min(nodes(:,3));
0142     levels = linspace(zmax,zmin, num_levels+2);
0143     levels = levels(2:end-1);
0144     
0145     C = (max(nodes) + min(nodes))/2;
0146     C(3) = levels(sn);
0147     
0148     
0149     
0150   
0151 function [C, M] = matrix_from_intercept(level, sn)   
0152    if nargin < 2
0153      sn = 1;
0154    end
0155    level = level(sn, :);
0156    R = eye(3);
0157    if length(level)==4
0158      theta = -level(4); % we rotate the model, user thinks of the image
0159      R = [cos(theta), -sin(theta), 0; sin(theta), cos(theta), 0; 0,0,1;];
0160    end
0161    
0162    % Infinities tend to cause issues -> replace with realmax
0163    % Don't need to worry about the sign of the inf
0164    level( isinf(level) | isnan(level) ) = realmax;
0165    level( level==0 ) =     1e-10; %eps;
0166 
0167    % Step 1: Choose a centre point in the plane
0168    %  Weight the point by it's inv axis coords
0169    invlev= 1./level;
0170    ctr= invlev / sum( invlev.^2 );
0171 
0172    % Step 2: Choose basis vectors in the plane
0173    %  First is the axis furthest from ctr
0174    [jnk, s_ax]= sort( - abs(level - ctr) );
0175    v1= [0,0,0]; v1(s_ax(1))= level(s_ax(1));
0176    v1= v1 - ctr;
0177    v1= v1 / norm(v1);
0178 
0179    % Step 3: Get off-plane vector, by cross product
0180    v2= [0,0,0]; v2(s_ax(2))= level(s_ax(2));
0181    v2= v2 - ctr;
0182    v2= v2 / norm(v2);
0183    v3= cross(v1,v2);
0184 
0185    % Step 4: Get orthonormal basis. Replace v2
0186    v2= cross(v1,v3);
0187 
0188    % Step 5: Get bases to point in 'positive directions'
0189    v1= v1 * (1-2*(sum(v1)<0));
0190    v2= v2 * (1-2*(sum(v2)<0));
0191    v3= v3 * (1-2*(sum(v3)<0));
0192    
0193    C = ctr;
0194    M = R * [v1;v2;v3];
0195 %   NODE= [v1;v2;v3] * (vtx' - ctr'*ones(1,nn) );
0196   
0197     
0198 function [C, M] = matrix_from_struct(level, sn)
0199   if nargin < 2
0200     sn = 1;
0201   end
0202   if ~isfield(level,'centre')
0203       error('Struct input ''level'' not understood.');
0204   end
0205   ctr = level.centre;
0206   if numel(ctr) == 3
0207       C = ctr; % make row
0208   elseif size(ctr,2) == 3 && size(ctr, 1) ~= 3
0209       C = ctr(:,sn);
0210   else
0211       C = ctr(sn,:);
0212   end
0213   C = C(:)'; % make row
0214   
0215   if isfield(level, 'rotation_matrix')
0216       M = level.rotation_matrix;
0217       return % no questions asked
0218   end
0219   
0220   if isfield(level, 'normal') && ~isfield(level,'normal_angle')
0221       level.normal_angle = level.normal; %quietly accept
0222   end
0223  
0224   if ~isfield(level,'normal_angle')
0225       error('Struct input ''level'' not understood.');
0226   end
0227   
0228   A = level.normal_angle(1:3);
0229   normA = norm(A);
0230   if normA == 0
0231     error('Normal vector has zero norm')
0232   end
0233   A = A(:) / normA; % normalize and make column
0234   B = [0,0,1]';
0235   R = eye(3);
0236   if length(level.normal_angle) == 4
0237     theta = level.normal_angle(4);
0238     R = [cos(theta), -sin(theta), 0; sin(theta), cos(theta), 0; 0,0,1;];
0239   end
0240    
0241   cross_AB = cross(A,B);
0242   if norm(cross_AB) == 0 % parallel vectors
0243     if A(3) > 0
0244       M = R*eye(3);
0245     elseif A(3) < 0
0246       M = R*diag([1,-1,-1]);
0247     end
0248     return
0249   end
0250   
0251   if 0
0252       % from https://math.stackexchange.com/questions/180418/
0253       % calculate-rotation-matrix-to-align-vector-a-to-vector-b-in-3d
0254       % works well, but leaves a rotation that's hard to control
0255       dot_AB = A(3);
0256       ssc = @(v) [0 -v(3) v(2); v(3) 0 -v(1); -v(2) v(1) 0];
0257       M = eye(3) + ssc(cross_AB) + ...
0258           ssc(cross_AB)^2*(1-dot_AB)/(norm(cross_AB)^2);
0259   else
0260       % here we define a nice coordinate system on the plane, and then
0261       % transform it to become the identity (ie. invert).
0262       
0263       % project each coordinate axis on the plane
0264       I = eye(3);
0265       for i = 1:3
0266           proj(:,i) = I(:,i) - (dot(I(:,i),A) / normA^2) * A;
0267       end
0268       norm_proj = vecnorm(proj);
0269       max_norm = max(norm_proj);
0270       
0271       % choose what happens based on which projection is longest
0272       M = zeros(3);
0273       M(:,3) = A;
0274       if norm_proj(3) == max_norm
0275           % projection of z becomes y
0276           M(:,2) = proj(:,3);
0277           M(:,1) = cross(M(:,2),M(:,3));
0278       elseif norm_proj(2) == max_norm
0279           % projection of y becomes y
0280           M(:,2) = proj(:,2);
0281           M(:,1) = cross(M(:,2),M(:,3));
0282       else
0283           % projection of x becomes x
0284           M(:,1) = proj(:,1);
0285           M(:,2) = cross(M(:,3),M(:,1));
0286       end
0287       M = inv(M);  
0288   end
0289   M = R*M ;  
0290   
0291 
0292   
0293 function do_unit_test
0294   unit_test_cmp('Scalar level', level_model_slice(5),5);
0295   unit_test_cmp('Vector level', level_model_slice([inf, inf, 3]),1);
0296   unit_test_cmp('Matrix level', level_model_slice([inf, inf, 3; inf, inf, 5]),2);
0297   LVL = [inf, inf, 3; inf, inf, 5];
0298   LVL(:,4) = rand(1,2);
0299   unit_test_cmp('Matrix level angle', level_model_slice(LVL),2);
0300   lvl.centre = [ 0,0,0];
0301   unit_test_cmp('Struct level', level_model_slice(lvl),1);
0302   lvl.centre(2,:) = [0,0,3];
0303   unit_test_cmp('Struct level', level_model_slice(lvl),2);
0304   lvl.normal_angle = rand(3,1)-.5;
0305   lvl.normal_angle = lvl.normal_angle / norm(lvl.normal_angle);
0306   lvl.normal_angle(4) = 2*pi * rand(1);
0307   [C, M] = level_model_slice(lvl);
0308   unit_test_cmp('Normal -> Matrix', M*lvl.normal_angle(1:3),[0,0,1]', 5*eps);
0309   [C, M] = level_model_slice([inf 5 inf]);
0310   lvl.normal_angle = [0,0,-1];
0311   [C, M] = level_model_slice(lvl);
0312   unit_test_cmp('Parallel normal', M,diag([1,-1,-1]));
0313   
0314   lvl.normal_angle = [0,0,0];
0315   try
0316     [C, M] = level_model_slice(lvl);
0317     fprintf('TEST: %20s = %4s \n','Expect error', 'Fail');  
0318   catch
0319     fprintf('TEST: %20s = %4s \n','Expect error', 'OK'); 
0320   end
0321   imdl = mk_common_model('n3r2',[16,2]); fmdl = imdl.fwd_model;
0322   nodes = level_model_slice(fmdl,[inf, inf, 1; inf, inf, 2],1);