find_edge2edge_intersections

FIND_EDGE2EDGE_INTERSECTIONS intersections between edges of two models
 Edges are considered intersecting if the minimum distance between them is
 less than epsilon and the closest point is not an endpoint.
 [pts,FE2CE,FE2pts,CE2pts] = find_edge2edge_intersections(FE,FN,CE,CN, epsilon)
 Inputs:
   FE      - Fine model edges [Nx2] as indices into FN
   FN      - Fine model nodes [Nx3]
   CE      - Coarse model edges [Mx2]
   CN      - Coarse model nodes [Mx3]
   epsilon - the minimum distance recognised as intersection
 Outputs:
   pts     - List of intersection points [Px3]
   FE2CE   - Boolean matrix indicating if two edges interesct [NxM]
   FE2pts  - Map between fine model edges and intersection points [NxP]
   CE2pts  - Map between coarse model edges and intersection points [MxP]

 This function is inspired by the simple Matlab conversion by Cristian Dima 
 of the C code posted by Paul Bourke at
 http://paulbourke.net/geometry/pointlineplane/lineline.c
 http://paulbourke.net/geometry/pointlineplane/linelineintersect.m

 See also: FIX_MODEL, MK_GRID_C2F

0001 function [pts,FE2CE,FE2pts,CE2pts] = find_edge2edge_intersections(FE,FN,CE,CN, epsilon)
0002 %FIND_EDGE2EDGE_INTERSECTIONS intersections between edges of two models
0003 % Edges are considered intersecting if the minimum distance between them is
0004 % less than epsilon and the closest point is not an endpoint.
0005 % [pts,FE2CE,FE2pts,CE2pts] = find_edge2edge_intersections(FE,FN,CE,CN, epsilon)
0006 % Inputs:
0007 %   FE      - Fine model edges [Nx2] as indices into FN
0008 %   FN      - Fine model nodes [Nx3]
0009 %   CE      - Coarse model edges [Mx2]
0010 %   CN      - Coarse model nodes [Mx3]
0011 %   epsilon - the minimum distance recognised as intersection
0012 % Outputs:
0013 %   pts     - List of intersection points [Px3]
0014 %   FE2CE   - Boolean matrix indicating if two edges interesct [NxM]
0015 %   FE2pts  - Map between fine model edges and intersection points [NxP]
0016 %   CE2pts  - Map between coarse model edges and intersection points [MxP]
0017 %
0018 % This function is inspired by the simple Matlab conversion by Cristian Dima
0019 % of the C code posted by Paul Bourke at
0020 % http://paulbourke.net/geometry/pointlineplane/lineline.c
0021 % http://paulbourke.net/geometry/pointlineplane/linelineintersect.m
0022 %
0023 % See also: FIX_MODEL, MK_GRID_C2F
0024 
0025 % (C) 2015 Bartlomiej Grychtol - all rights reserved by Swisstom AG
0026 % License: GPL version 2 or 3
0027 % $Id: find_edge2edge_intersections.m 4890 2015-04-23 14:09:55Z bgrychtol-ipa $
0028 
0029 % >> SWISSTOM CONTRIBUTION <<
0030 
0031 if size(CE,1) > size(FE,1)
0032     % flip the inputs if CE is longer than FE
0033     [pts,FE2CE,CE2pts,FE2pts] = find_edge2edge_intersections(CE,CN,FE,FN, epsilon);
0034     FE2CE = FE2CE';
0035     return
0036 end
0037 
0038 epsilon = epsilon.^2; % spares us doing sqrt to calculate distance
0039 
0040 % TODO: need a good heuristic to choose the faster approach
0041 try 
0042     % this is usually faster, but can still, exceptionally, run out of
0043     % memory
0044     [pts,FE2CE,FE2pts,CE2pts] = edge2edge_intersections_serial(FE,FN,CE,CN,epsilon);
0045 catch err
0046     if strcmp(err.identifier, 'MATLAB:nomem')
0047         % this should not run out of memory
0048         [pts,FE2CE,FE2pts,CE2pts] = edge2edge_intersections_wrapper(FE,FN,CE,CN,epsilon);
0049     else
0050         rethrow(err);
0051     end
0052 end
0053 
0054 
0055 %-------------------------------------------------------------------------%
0056 % Wrapper to divide the calculations into smaller chunks
0057 function [intpts, FE2CE, FE2pts, CE2pts] = edge2edge_intersections_wrapper(FE,FN,CE,CN, epsilon)
0058 % Doing this in parallel creates potentially huge matrices
0059 % Do it by parts to prevent out-of-memory errors
0060     sz = size(FE,1) * size(CE,1) * 8; % result in bytes
0061     desired_mem = 2*(1024^3 ); %
0062     % at least 9 variables of FExCE size are needed in the main function
0063     n_chunks = ceil(10*sz / desired_mem); 
0064 
0065     len_chnk = ceil(size(CE,1) / n_chunks);
0066     intpts = [];
0067     FE2CE = sparse(0,0);
0068     FE2pts = sparse(0,0);
0069     CE2pts = sparse(0,0);
0070     for c = 1:n_chunks
0071         eidors_msg('@@@: chunk %d of %d',c,n_chunks,2);
0072         start = 1 + (c-1)*len_chnk;
0073         stop  = min(1 + c*len_chnk, size(CE,1));
0074         rng   = start:stop;
0075         [ip, te2ve, te2ip, ve2ip] = edge2edge_intersections(FE,FN,CE(rng,:),CN, epsilon);
0076         len    = size(intpts,1);
0077         intpts = [intpts; ip];
0078         idx = te2ve>0;
0079         te2ve(idx) = len + te2ve(idx);
0080         FE2CE  = [FE2CE te2ve];
0081         FE2pts = [FE2pts te2ip];
0082         if size(ve2ip,1) < size(CE2pts,1)
0083             ve2ip(size(CE2pts,1),1) = 0;
0084         end
0085         CE2pts = [CE2pts ve2ip];
0086     end
0087 
0088     
0089 %-------------------------------------------------------------------------%
0090 % Calculate intersection points between two sets of edges
0091 function [intpts, FE2CE, FE2pts, CE2pts] = edge2edge_intersections_serial(FE,FN,CE,CN, epsilon)
0092 % Based on the simple Matlab conversion by Cristian Dima of the C code
0093 % posted by Paul Bourke at
0094 % http://paulbourke.net/geometry/pointlineplane/lineline.c
0095 % http://paulbourke.net/geometry/pointlineplane/linelineintersect.m
0096 
0097     progress_msg(0);
0098 
0099     P1 = FN(FE(:,1),:);
0100     P2 = FN(FE(:,2),:);
0101     P3 = CN(CE(:,1),:);
0102     P4 = CN(CE(:,2),:);
0103     
0104     C_bb = zeros(size(P3,1),6);
0105     C_bb(:,[1 3 5]) = min(P3,P4);
0106     C_bb(:,[2 4 6]) = max(P3,P4);
0107     
0108     F_bb = zeros(size(P1,1),6);
0109     F_bb(:,[1 3 5]) = min(P1,P2);
0110     F_bb(:,[2 4 6]) = max(P1,P2);
0111     
0112     
0113     excl =   bsxfun(@gt, F_bb(:,1), C_bb(:,2)') ...
0114            | bsxfun(@lt, F_bb(:,2), C_bb(:,1)') ...
0115            | bsxfun(@gt, F_bb(:,3), C_bb(:,4)') ...
0116            | bsxfun(@lt, F_bb(:,4), C_bb(:,3)') ...
0117            | bsxfun(@gt, F_bb(:,5), C_bb(:,6)') ...
0118            | bsxfun(@lt, F_bb(:,6), C_bb(:,5)');
0119     excl = ~excl;
0120     
0121     % these are too big to keep around
0122 %     p13.x = bsxfun(@minus, P1(:,1), P3(:,1)');
0123 %     p13.y = bsxfun(@minus, P1(:,2), P3(:,2)');
0124 %     p13.z = bsxfun(@minus, P1(:,3), P3(:,3)');
0125     
0126     p43 = P4 - P3;
0127     p21 = P2 - P1;
0128     
0129     d4343 = sum(p43.^2,2);
0130     d2121 = sum(p21.^2,2);
0131     
0132     % get rid of degenerate cases - slow, and superfluous
0133 %     JNK = bsxfun(@or,sparse(all(abs(p21)<eps,2)),sparse(all(abs(p43)<eps,2)'));
0134     IN  = false(size(P1,1),size(P3,1));
0135 
0136     intpts = [];
0137     
0138     todo = size(CE,1);
0139     id = 0;
0140     step = ceil(todo/100);
0141     
0142     pa = zeros(size(P1));
0143     
0144     for  v = 1:size(CE,1)
0145         id = id+1;
0146         if mod(id,step)==0, progress_msg(id/todo); end
0147         
0148         fid = excl(:,v);
0149         if ~any(fid), continue, end;
0150         
0151         d1343 = 0; d4321 = 0;  d1321 = 0;
0152         for i = 1:3
0153             p13 = P1(fid,i) - P3(v,i);
0154             d1343 = d1343 + p13*p43(v,i);
0155             d4321 = d4321 + p21(fid,i)*p43(v,i);
0156             d1321 = d1321 + p13 .* p21(fid,i);
0157         end
0158         
0159         
0160         
0161         denom = d2121(fid) * d4343(v) - d4321.^2;
0162         % this is slow, and should be taken care of by mua < 1
0163 %         JNK(:,v) = JNK(:,v) | abs(denom)<eps;
0164         numer = d1343 .* d4321 - d1321 * d4343(v);
0165         
0166         mua = numer./denom;
0167         mub = (d1343 + d4321.*mua) / d4343(v);
0168 
0169 
0170         D = 0; % distance
0171         for i = 1:3
0172             pa(fid,i) = P1(fid,i) + p21(fid,i).*mua;
0173             pb = P3(v,i) + p43(v,i) * mub;
0174             D = D + (pa(fid,i) - pb).^2;
0175         end
0176 %         D = sqrt(D);
0177 %         D(JNK(:,v)) = Inf;
0178         IN(fid,v) = ((mua>0) + (mua<1) + (mub>0) + (mub<1) + (D<=epsilon)) == 5;
0179         if any(IN(fid,v))
0180             intpts = [intpts; pa(IN(:,v),:)];
0181         end
0182     end
0183     [T, V] = find(IN);
0184     I = (1:size(intpts,1))';
0185     FE2CE = sparse(size(P1,1),size(P3,1));
0186     FE2CE(sub2ind(size(FE2CE),T,V)) = I;
0187     FE2pts = sparse(T,I,ones(size(I)),size(P1,1),size(I,1));
0188     CE2pts  = sparse(V,I,ones(size(I)),size(P3,1),size(I,1));
0189     
0190     if eidors_msg('log_level')>1
0191         progress_msg(Inf);
0192     end
0193     
0194     
0195 %-------------------------------------------------------------------------%
0196 % Calculate intersection points between two sets of edges
0197 function [intpts, FE2CE, FE2pts, CE2pts] = edge2edge_intersections(FE,FN,CE,CN, epsilon)
0198 % Based on the simple Matlab conversion by Cristian Dima of the C code
0199 % posted by Paul Bourke at
0200 % http://paulbourke.net/geometry/pointlineplane/lineline.c
0201 % http://paulbourke.net/geometry/pointlineplane/linelineintersect.m
0202 
0203 
0204 
0205     P1 = FN(FE(:,1),:);
0206     P2 = FN(FE(:,2),:);
0207     P3 = CN(CE(:,1),:);
0208     P4 = CN(CE(:,2),:);
0209     
0210     % these are too big to keep around
0211 %     p13.x = bsxfun(@minus, P1(:,1), P3(:,1)');
0212 %     p13.y = bsxfun(@minus, P1(:,2), P3(:,2)');
0213 %     p13.z = bsxfun(@minus, P1(:,3), P3(:,3)');
0214     
0215     p43 = P4 - P3;
0216     p21 = P2 - P1;
0217     
0218     % get rid of degenerate cases
0219     JNK = bsxfun(@or,sparse(all(abs(p21)<eps,2)),sparse(all(abs(p43)<eps,2)'));
0220     
0221     f = {'x','y','z'};
0222     d1343 = 0; d4321 = 0;  d1321 = 0;
0223 
0224     for i = 1:3;
0225         p13 = bsxfun(@minus, P1(:,i), P3(:,i)');
0226         d1343 = d1343 + bsxfun(@times,p13,p43(:,i)');
0227         d4321 = d4321 + bsxfun(@times,p21(:,i),p43(:,i)');
0228         d1321 = d1321 + bsxfun(@times,p13,p21(:,i));
0229     end
0230 
0231     clear p13
0232     d4343 = sum(p43.^2,2);
0233     d2121 = sum(p21.^2,2);
0234     
0235     denom = bsxfun(@times,d2121,d4343') - d4321.^2;
0236     JNK = JNK | abs(denom)<eps;
0237     numer = d1343 .* d4321 - bsxfun(@times,d1321,d4343');
0238     
0239     mua = numer./denom;
0240     mub = bsxfun(@rdivide, d1343 + d4321.*mua , d4343');
0241     
0242     clear d1343 d4321 d1321 d4343 d2121
0243     
0244     pa = struct; pb = struct;
0245     D = 0; % distance
0246     for i = 1:3
0247         pa.(f{i}) = bsxfun(@plus,P1(:,i),bsxfun(@times,p21(:,i),mua));
0248         pb = bsxfun(@plus,P3(:,i)',bsxfun(@times,p43(:,i)',mub));
0249         D = D + (pa.(f{i}) - pb).^2;
0250     end
0251 %     D = sqrt(D);
0252     D(JNK) = Inf;
0253     
0254 
0255     
0256     IN = mua>0 & mua <1 & mub>0 & mub<1 & D<=epsilon;
0257     nPts = nnz(IN);
0258     intpts = zeros(nPts,3);
0259     for i = 1:3
0260         intpts(:,i) = pa.(f{i})(IN);
0261     end
0262  
0263     [T, V] = find(IN);
0264     I = (1:nPts)';
0265     FE2CE = sparse(size(P1,1),size(P3,1));
0266     FE2CE(IN) = I;
0267    
0268     FE2pts = sparse(T,I,ones(size(I)),size(P1,1),size(I,1));
0269     CE2pts  = sparse(V,I,ones(size(I)),size(P3,1),size(I,1));
0270