function [intMatrix, intSurface] = SurfaceIntersection(surface1, surface2, varargin)
%SURFACEINTERSECTION intersection of 2 surfaces
% [intMatrix, intSurface] = SurfaceIntersection(surface1, surface2)
% calculates the intersection of surfaces 1 and 2. Code can either return
% just the matrix indicating which face of surface1 intersected with face
% of surface2, which is calculated using Tomas Moller algorithm, or can
% also return the actual line of intersection. In case when parts of the
% surface 1 and 2 lay on the same plane the intersection is a 2D area
% instead of 1D edge. In such a case the intersection area will be
% triangulated and intSurface.edges will hold the edges of the
% triangulation surface and intSurface.faces will hold the faces.
%
% INPUT:
% * surface1 & surface2 - two surfaces defined as structs or classes.
% Several inputs are possible:
% - struct with "faces" and "vertices" fields
% - 'triangulation' class (only the boundary surface will be used)
% - 'delaunayTriangulation' class
%
% OUTPUT:
% * intMatrix - sparse Matrix with n1 x n2 dimension where n1 and n2 are
% number of faces in surfaces
% * intSurface - a structure with following fields:
% intSurface.vertices - N x 3 array of unique points
% intSurface.edges - N x 2 array of edge vertex ID's
% intSurface.faces - N x 3 array of face vertex ID's
%
% ALGORITHM:
% Based on Triangle/triangle intersection test routine by Tomas Mller, 1997.
% See article "A Fast Triangle-Triangle Intersection Test",
% Journal of Graphics Tools, 2(2), 1997
% http://web.stanford.edu/class/cs277/resources/papers/Moller1997b.pdf
% http://fileadmin.cs.lth.se/cs/Personal/Tomas_Akenine-Moller/code/opttritri.txt
%% Get FACES and VERTICES inputs
if isa(surface1, 'triangulation')
[surface1.faces, surface1.vertices] = freeBoundary(surface1);
elseif isa(surface1, 'delaunayTriangulation')
S = surface1;
surface1 = [];
surface1.faces = S.ConnectivityList;
surface1.vertices = S.Points;
clear S
end
if isa(surface2, 'triangulation')
[surface2.faces, surface1.vertices] = freeBoundary(surface2);
elseif isa(surface2, 'delaunayTriangulation')
S = surface2;
surface2 = [];
surface2.faces = S.ConnectivityList;
surface2.vertices = S.Points;
clear S
end
ok1 = isstruct(surface1) && isfield(surface1, 'vertices') && isfield(surface1, 'faces');
ok2 = isstruct(surface2) && isfield(surface2, 'vertices') && isfield(surface2, 'faces');
assert(ok1, 'Surface #1 must be a struct with "faces" and "vertices" fields' );
assert(ok2, 'Surface #2 must be a struct with "faces" and "vertices" fields' );
%% Flip dimentions if necessery
if size(surface1.faces,1)==3 && size(surface1.faces,2)~=3
surface1.faces = surface1.faces';
end
if size(surface1.vertices,1)==3 && size(surface1.vertices,2)~=3
surface1.vertices = surface1.vertices';
end
if size(surface2.faces,1)==3 && size(surface2.faces,2)~=3
surface2.faces = surface2.faces';
end
if size(surface2.vertices,1)==3 && size(surface2.vertices,2)~=3
surface2.vertices = surface2.vertices';
end
%% Parse extra parameters
getIntersection = (nargout>1);
debug = true;
PointRoundingTol = 1e6;
algorithm = 'moller';
k=1;
nVarargs = length(varargin);
while (k<=nVarargs)
assert(ischar(varargin{k}), 'Incorrect input parameters')
switch lower(varargin{k})
case 'debug'
debug = varargin{k+1}~=0;
k = k+1;
case 'algorithm'
algorithm = lower(strtrim(varargin{k+1}));
k = k+1;
case 'pointroundingtol'
PointRoundingTol = varargin{k+1};
k = k+1;
end
k = k+1;
end
%% Initialize variables
epsilon = eps;
nFace1 = size(surface1.faces,1);
nFace2 = size(surface2.faces,1);
nVert1 = size(surface1.vertices,1);
nVert2 = size(surface2.vertices,1);
%% create strip down versions of MATLAB cross and dot function
cross_prod = @(a,b) [...
a(:,2).*b(:,3)-a(:,3).*b(:,2), ...
a(:,3).*b(:,1)-a(:,1).*b(:,3), ...
a(:,1).*b(:,2)-a(:,2).*b(:,1)];
dot_prod = @(a,b) a(:,1).*b(:,1)+a(:,2).*b(:,2)+a(:,3).*b(:,3);
normalize = @(V) bsxfun(@rdivide,V, sqrt(sum(V.^2,2)));
%% Initialize output variables
% intersect is a nFace1 x nFace2 matrix. Possible values: -2 (do not know),
% -1 (coplanar with unknown overlap), 0 (no intersections), 1 (intersects).
% Negative values are internal only.
intMatrix = zeros([nFace1,nFace2], 'int8')-2; % -2 indicates that there was no succesful test yet
intSurface.vertices = [];
intSurface.faces = [];
intSurface.edges = [];
% =======================================================================
%% === Stage 1 ==========================================================
% =======================================================================
% Each triangle is a subset of the plane it lies in, so for two triangles
% to intersect they must overlap along the line of intersection of their
% planes. Hence, a necessary condition for intersection is that each
% triangle must intersect the plane of the other.
% Mllers method begins by checking the mutual intersection of each
% triangle with the plane of the other. To do so, it determines for each
% triangle on which side of the other triangles supporting plane its
% vertices lie. Now, if all vertices of one triangle lie on the same side
% and no vertex is on the plane, the intersection is rejected.
%% compute plane equations for each triangle of the surface #1
% plane equation #1: N1.X-d1=0
V1 = surface1.vertices(surface1.faces(:,1),:);
V2 = surface1.vertices(surface1.faces(:,2),:);
V3 = surface1.vertices(surface1.faces(:,3),:);
N1 = cross_prod(V2-V1,V3-V1); % array size nFace1 x 3
N1 = normalize(N1);
d1 = dot_prod(N1,V1); % array size nFace1 x 1
%% Distance from surface #2 vertices to planes of surface #1
% Calculate signed distance from all vertices of surface #2 to each plane
% of of surface #1
du = zeros(nFace1,nVert2);
for iVert2 = 1:nVert2
p = surface2.vertices(iVert2,:);
du(:,iVert2) = N1(:,1)*p(1) + N1(:,2)*p(2) + N1(:,3)*p(3) - d1;
end
if debug
assert(all(size(du)==[nFace1,nVert2]), 'Incorrect array dimensions: dv')
end
du(abs(du)<epsilon)=0; % robustness check
% Distances from vertex 1, 2 & 3 of faces of surface #2 to planes of surface #1
du1 = du(:,surface2.faces(:,1));
du2 = du(:,surface2.faces(:,2));
du3 = du(:,surface2.faces(:,3));
if debug
assert(all(size(du1)==size(intMatrix)), 'Incorrect array dimensions: du1')
end
clear du
intMatrix(du1.*du2>0 & du1.*du3>0) = 0; % same sign on all of them & not equal 0
if(all(intMatrix==0)), return; end % no intersections
intMatrix(du1==0 & du2==0 & du3==0) = -1; % coplanar with unknown overlap
%% compute plane of triangle (U0,U1,U2)
% plane equation 2: N2.X-d2=0
U1 = surface2.vertices(surface2.faces(:,1),:);
U2 = surface2.vertices(surface2.faces(:,2),:);
U3 = surface2.vertices(surface2.faces(:,3),:);
N2 = cross_prod(U2-U1,U3-U1); % array size nFace1 x 3
N2 = normalize(N2);
d2 = dot_prod(N2,U1); % array size nFace1 x 1
%% Distance from surface #1 vertices to planes of surface #2
% Calculate signed distance from all vertices of surface #1 to each plane
% of of surface #2
dv = zeros(nFace2,nVert1);
for iVert1 = 1:nVert1
p = surface1.vertices(iVert1,:);
dv(:,iVert1) = N2(:,1)*p(1) + N2(:,2)*p(2) + N2(:,3)*p(3) - d2;
end
if debug
assert(all(size(dv)==[nFace2,nVert1]), 'Incorrect array dimensions: dv')
end
dv(abs(dv)<epsilon)=0; % robustness check
% Distances from vertex 1, 2 & 3 of faces of surface #1 to planes of surface #2
dv1 = dv(:,surface1.faces(:,1))';
dv2 = dv(:,surface1.faces(:,2))';
dv3 = dv(:,surface1.faces(:,3))';
if debug
assert(all(size(dv1)==size(intMatrix)), 'Incorrect array dimensions: dv1')
end
clear dv
intMatrix(dv1.*dv2>0 & dv1.*dv3>0) = 0; % same sign on all of them & not equal 0
if(all(intMatrix==0)), return; end % no intersections
intMatrix(dv1==0 & dv2==0 & dv3==0) = -1; % coplanar with unknown overlap
% =======================================================================
%% === Stage 2 ==========================================================
% =======================================================================
%% Process remaining (non-coplanar) triangle pairs
tMsk = (intMatrix==-2);
n = nnz(tMsk);
if n>0
[face1, face2] = find(tMsk);
switch lower(algorithm)
case 'moller'
if size(dv1(tMsk),1)==1
dv = [dv1(tMsk)', dv2(tMsk)', dv3(tMsk)'];
du = [du1(tMsk)', du2(tMsk)', du3(tMsk)'];
else
dv = [dv1(tMsk), dv2(tMsk), dv3(tMsk)];
du = [du1(tMsk), du2(tMsk), du3(tMsk)];
end
[intMatrix(tMsk), intSurface] = TriangleIntersection3D_Moller(...
V1(face1,:), V2(face1,:), V3(face1,:), N1(face1,:), d1(face1,:), dv, ...
U1(face2,:), U2(face2,:), U3(face2,:), N2(face2,:), d2(face2,:), du, ...
getIntersection, debug);
case 'rapid'
% Undocumented experimental feature. In some experiments I got
% identical results as with Moller algorithm, but others gave
% different results. Often faster tham Moller.
intMatrix(tMsk) = TriangleIntersection3D_Rapid( ...
V1(face1,:), V2(face1,:), V3(face1,:), ...
U1(face2,:), U2(face2,:), U3(face2,:), N1(face1,:), N2(face2,:) );
otherwise
error('Unknown algorithm name');
end
end % if
%% Process coplanar triangle pairs. Pass #1:
% compare the overlap of the bounding boxes
tMsk = (intMatrix==-1);
if nnz(tMsk)>0
[face1, face2] = find(tMsk);
overlap = true;
for idim = 1:3
v = [V1(face1,idim), V2(face1,idim), V3(face1,idim)];
u = [U1(face2,idim), U2(face2,idim), U3(face2,idim)];
t1 = min(v,[],2);
t2 = max(v,[],2);
s1 = min(u,[],2);
s2 = max(u,[],2);
overlap = overlap & (s1<=t2 & t1<=s2);
end
% if overlap intMatrix will remain "-1" otherwise it will change to "0"
intMatrix(tMsk) = -1*overlap;
clear v u t1 t2 s1 s2 overlap
end
%% Process coplanar triangle pairs. Pass #2:
% use edge-edge intersections
tMsk = (intMatrix==-1);
if nnz(tMsk)>0
[face1, face2] = find(tMsk);
% repack data prior to function call
V(:,:,1)=V1(face1,:); V(:,:,2)=V2(face1,:); V(:,:,3)=V3(face1,:);
U(:,:,1)=U1(face2,:); U(:,:,2)=U2(face2,:); U(:,:,3)=U3(face2,:);
[intMatrix(tMsk), intSurface2] = TriangleIntersection2D(V, U, ...
N1(face1,:), getIntersection, debug);
%% Merge surfaces
if getIntersection
np = size(intSurface.vertices,1);
intSurface.vertices = [intSurface.vertices; intSurface2.vertices];
intSurface.faces = [intSurface.faces; intSurface2.faces+np];
intSurface.edges = [intSurface.edges; intSurface2.edges+np];
if debug
np = size(intSurface.vertices,1);
assert(max(intSurface.faces(:))<=np, 'Bad surface definition')
assert(max(intSurface.edges(:))<=np, 'Bad surface definition')
end
end
end
%% Clean up the outputs
intMatrix = sparse(double(intMatrix));
if(getIntersection)
% make point array unique
P = round(intSurface.vertices*PointRoundingTol)/PointRoundingTol;
[~,ia,ic] = unique(P,'rows'); % V = P(ia,:) and P = V(ic,:).
intSurface.vertices = intSurface.vertices(ia,:);
intSurface.faces = ic(intSurface.faces);
intSurface.edges = ic(intSurface.edges);
end
end % function
%% ========================================================================
function [iMsk, intSurface] = TriangleIntersection3D_Moller(...
V1, V2, V3, N1, d1, dv, ...
U1, U2, U3, N2, d2, du, ...
getIntersection, debug)
%TriangleIntersection3D tests if 2 triangles defined in 3D intersect.
% This is a secondary test following Tomas Moller algorithm
%
% INPUTS:
% V1, V2, V3, - Nx3 array of surface 1 triangle vertex coordinates
% U1, U2, U3, - Nx3 array of surface 2 triangle vertex coordinates
% N1, d1 - Nx3 array of surface 1 triangle plane equations N1.X-d1=0
% N2, d2 - Nx3 array of surface 2 triangle plane equations N2.X-d2=0
% dv - Nx3 array of distances of surface 1 triangle vertices to surface 2 planes
% du - Nx3 array of distances of surface 2 triangle vertices to surface 1 planes
% getIntersection - do we need to output the intersecting surface?
% Algorithm is much simpler if we do not.
% debug - In the debugging mode much more extra "sanity check" test
% are performed.
%
% OUTPUT:
% iMsk - N x 1 intersection boolean mask marking which triangles overlap
% intSurface - intersection surface
%
% ALGORITHM:
% The input triangles are guaranteed to intersect the line of intersection
% of the two planes. Furthermore, these intersections form intervals on
% this line, and the triangles overlap iff these intervals overlap as well.
% Hence, the last part of the algorithm computes a parametric equation
% L(t) of the line of intersection of the two planes, finds the intervals
% (i.e. scalar intervals on L(t)) for which the line lies inside each
% triangle and performs a one-dimensional interval overlap test.
if debug
ok = size(N1,2)==3 && size(N2,2)==3 && size(dv,2)==3 && size(du,2)==3 && ...
size(V1,2)==3 && size(V2,2)==3 && size(V3,2)==3 && ...
size(U1,2)==3 && size(U2,2)==3 && size(U3,2)==3;
assert(ok, 'Incorrect array dimensions');
end
%% create strip down versions of MATLAB cross and dot function
cross_prod = @(a,b) [...
a(:,2).*b(:,3)-a(:,3).*b(:,2), ...
a(:,3).*b(:,1)-a(:,1).*b(:,3), ...
a(:,1).*b(:,2)-a(:,2).*b(:,1)];
dot_prod = @(a,b) a(:,1).*b(:,1)+a(:,2).*b(:,2)+a(:,3).*b(:,3);
normalize = @(V) bsxfun(@rdivide,V, sqrt(sum(V.^2,2)));
%% Find intervals of surface 1 and 2 triangles
% compute the scalar intervals on L(t) for which the line lies inside each
% triangle
% Plane creates two open half-spaces. Find the odd vertex, which:
% 1) if no or two vertices are on the plane than pick the vertex which is
% by itself in its half-space
% 2) if one vertex is on the plane and the other two occupy the same
% half-space than pick the vertex on the plane
% 3) if one vertex is on the plane and the other two occupy different
% half-spaces than pick one of the vertices off the plane
% Find vertex using a look-up table "lut" with key calculated based on
% sign of dv and du arrays
lut = [0;3;3;2;1;3;2;2;1;1;2;3;3;0;3;3;2;1;1;2;2;3;1;2;3;3;0];
n = numel(d1);
rows = (1:n)';
%% order surface 1 triangle vertices
a1 = lut(sign(dv)*[9; 3; 1] + 14); % calculate the key and call the look-up table
[b1, c1] = otherDim(a1);
if debug
assert(all(a1>0), 'Something Wrong: triangles are coplanar')
end
a1 = sub2ind([n,3],rows,a1); % convert row and column IDs to array indecies
b1 = sub2ind([n,3],rows,b1);
c1 = sub2ind([n,3],rows,c1);
%% order surface 2 triangle vertices
a2 = lut(sign(du)*[9; 3; 1] + 14); % calculate the key and call the look-up table
[b2, c2] = otherDim(a2);
if debug
assert(all(a2>0), 'Something Wrong: triangles are coplanar')
end
a2 = sub2ind([n,3],rows,a2);
b2 = sub2ind([n,3],rows,b2);
c2 = sub2ind([n,3],rows,c2);
%% compute direction of L the line of intersection of 2 planes
% containing 2 triangles. Line L parametric equation: t*D+O=0
D = cross_prod(N1,N2); % D must be perpendicular to both N1 and N2
[~, maxDim] = max(abs(D),[],2); % compute and index to the largest component of D
if(getIntersection)
D = normalize(D);
O = zeros(n,3);
d = [d1, d2, zeros(n,1)];
for r =1:n
N = [N1(r,:); N2(r,:); 0, 0, 0];
N(3,maxDim(r)) = 1;
dd = d(r,:)';
O(r,:) = (N\dd)'; %Solve systems of linear equations N*D3 = d for D3
end
clear N d dd
end
%% projection of triangle(V1,V2,V3) and triangle(U1,U2,U3) onto intersection line
% Vp and Up are Nx3 arrays with columns indicating corners of triangles 1 and 2
if(getIntersection)
Vp=[dot_prod(V1-O,D), dot_prod(V2-O,D), dot_prod(V3-O,D)];
Up=[dot_prod(U1-O,D), dot_prod(U2-O,D), dot_prod(U3-O,D)];
else
% Project on one of the axis (closest to the intersection line) instead.
% Simplified projection is faster and sufficient if we do not need
% intersection line
idx = sub2ind([n,3],rows,maxDim);
Vp = [V1(idx), V2(idx), V3(idx)];
Up = [U1(idx), U2(idx), U3(idx)];
end
clear V1 V2 V3 U1 U2 U3
%% Calculate surface 1 and 2 triangle intervals
% t1 and t2 are intersection points of surface 1 with the intersection line
% t*D+O=0, and s1 & s2 are intersection points of surface 2 with the same
% line. Tomas Moller algorithm made this section much more complicated
% trying to avoid divisions. However, I could not detect any speed-up.
% Operations (ADD: 12; MUL:4 ; DIV:4 )
t1 = Vp(a1) - (Vp(b1)-Vp(a1)).*dv(a1)./(dv(b1)-dv(a1));
t2 = Vp(a1) - (Vp(c1)-Vp(a1)).*dv(a1)./(dv(c1)-dv(a1));
s1 = Up(a2) - (Up(b2)-Up(a2)).*du(a2)./(du(b2)-du(a2));
s2 = Up(a2) - (Up(c2)-Up(a2)).*du(a2)./(du(c2)-du(a2));
%% Order the intervals as to t1<t2 and s1<s2
msk = t2<t1; % order t1 and t2 so t1<t2
t = t1(msk); t1(msk)=t2(msk); t2(msk)=t; % swap
msk = s2<s1; % order s1 and s2 so s1<s2
t = s1(msk); s1(msk)=s2(msk); s2(msk)=t; % swap
%% Perform THE final test we were preparying for.
% It test for the overlap of 2 1D intervals s1->s2 and t1->t2
iMsk = (s1<t2 & t1<s2);
%% calculate intersection segments
n = nnz(iMsk);
if(getIntersection && n>0)
% p1 = D*max(t1,s1) + O; p2 = D*min(t2,s2) + O
p1 = bsxfun(@times,D(iMsk,:),max(t1(iMsk),s1(iMsk))) + O(iMsk,:);
p2 = bsxfun(@times,D(iMsk,:),min(t2(iMsk),s2(iMsk))) + O(iMsk,:);
intSurface.vertices = [p1; p2];
intSurface.faces = [1:n; n+1:2*n; n+1:2*n]';
intSurface.edges = intSurface.faces(:,1:2);
else
intSurface.vertices = [];
intSurface.faces = [];
intSurface.edges = [];
end % if
end % function
%% ========================================================================
function [overlap, intSurface] = TriangleIntersection2D(V, U, N, ...
getIntersection, debug)
% Triangles V(V0,V1,V2) and U(U0,U1,U2) are are coplanar. Do they overlap?
% INPUTS:
% N - array(n,3) of surface normals where V(i,:,:) and U(i,:,:) are on the same plane
% V - array(n,3,3) (nFace x 3 dimensions x 3 vertices) of surface #1 vertices
% U - array(n,3,3) (nFace x 3 dimensions x 3 vertices) of surface #2 vertices
%
% OUTPUT:
% iMsk - N x 1 intersection boolean mask marking which triangles overlap
% intSurface - intersection surface
% * parameters: vertices of triangle 1: V0,V1,V2
% * vertices of triangle 2: U0,U1,U2
% * result : returns 1 if the triangles intersect, otherwise 0
%% Constants needed for creating a mesh based on 3 to 6 points in a circle
tri_mesh{6} = [1 2 6; 2 4 6; 2 3 4; 4 5 6];
tri_mesh{5} = [1 2 3; 1 3 4; 4 5 1];
tri_mesh{4} = [1 2 3; 1 3 4];
tri_mesh{3} = 1:3;
vertices = [];
faces = [];
pairs = []; % each row corresponds to pair of faces. match row number with face number
nVert = 0;
%% use edge-edge intersections
overlap = false(size(N,1),1);
i1Idx = [1 1 1 2 2 2 3 3 3];
i2Idx = [3 3 3 1 1 1 2 2 2];
j1Idx = [1 2 3 1 2 3 1 2 3];
j2Idx = [3 1 2 3 1 2 3 1 2];
for row = 1:size(N,1)
% When it is necesary to project 3D plane on 2D, dIdx will be the optimal
% dimensions to use.
[~, a] = max(abs(N(row,:)));
[b, c] = otherDim(a);
dIdx = [b, c];
order = [];
%% test all edges of triangle 1 against the edges of triangle 2
% triangles overlap if edges cross
[edgeMat, P] = EdgesIntersect3D(...
squeeze(V(row,:,i1Idx))',squeeze(V(row,:,i2Idx))', ...
squeeze(U(row,:,j1Idx))',squeeze(U(row,:,j2Idx))');
overlap(row) = any(edgeMat);
if ~getIntersection && overlap(row), continue; end
if ~overlap(row)
%% project onto an axis-aligned plane, that maximizes the area
% of the triangles, compute indices: dIdx which correspond to 2 smallest N1
% components.
V2d = [V(row,dIdx,1); V(row,dIdx,2); V(row,dIdx,3)]; % each row is a 2D vertex
U2d = [U(row,dIdx,1); U(row,dIdx,2); U(row,dIdx,3)];
%% test if tri1 is totally contained in tri2 or vice varsa
if PointInTriangle2D(V2d(1,:), U2d) % tri1 is totally contained in tri2
overlap(row) = true;
order = 1:3;
elseif PointInTriangle2D(U2d(1,:), V2d) % tri2 is totally contained in tri1
overlap(row) = true;
order = 4:6;
end
if overlap(row) && ~getIntersection, continue; end
clear V2d U2d
end
%% Build the intersection surface
if getIntersection && overlap(row)
%Assemble all the points which might be needed for desining
%intersection polygon: Intersection points and points from triangle 1
%and 2
points = [P(edgeMat,:); squeeze(V(row,:,:))'; squeeze(U(row,:,:))'];
if isempty(order) % when one tri is totally contained in the other tri then order is set
order = IntersectionPolygon(edgeMat>0, points, dIdx, debug);
if isempty(order), continue; end
end
nPoint = length(order); % how many points will be added?
nFace = nPoint-2; % how many faces will be added?
vertices = [vertices; points(order,:)]; %#ok<*AGROW>
faces = [faces; nVert+tri_mesh{nPoint} ];
pairs = [pairs; row+zeros(nFace,1)]; % each row corresponds to pair of faces. match row number with face number
nVert = nVert + nPoint;
if debug
assert(max(faces(:))<=size(vertices,1), 'Bad surface definition')
end
end
end % for
%% Prepare outputs
intSurface.vertices = vertices;
intSurface.faces = faces;
if isempty(faces)
intSurface.edges = [];
else
intSurface.edges = [faces(:,1:2); faces(:,2:3); faces(:,[1,3])];
end
end % function
%% ========================================================================
function polygon = IntersectionPolygon(edgeMat, points, dIdx, debug)
% edgeMat is an edge intersection matrix with 3 rows for edges between
% the points 1-3, 1-2, & 2-3 of the triangle 1 and 3 columns for the same
% edges of the triangle 2. If 2 edges intersect a point of intersection
% is calculated and stored in array "points" followed by points of the
% triangles 1 & 2. This function calculates the polygon of the intersection
% between 2 triangles.
persistent orderLUT verified
if isempty(orderLUT) || isempty(orderLUT{3})
% This pre-calculated look-up table is used to quickly look up the order of
% the vertices in array "points" which make up polygon of the intersection
% between 2 triangles. A unique key is calculated for each edgeMat using
% dot product between edgeMat(:) and [256 128 64 32 16 8 4 2 1], which is
% used to look up point order around the polygon. Negative numbers in the
% LUT indicate values which were not observed yet so they were not
% independently verified.
% reshape(sprintf('%09s',dec2base(key, 2)),3,3) will convert from the key
% to matrix.
OrderLUT = zeros(432,1);
OrderLUT(003) = 127;
OrderLUT(005) = 128;
OrderLUT(006) = 126;
OrderLUT(009) = 124;
OrderLUT(010) = 1427;
OrderLUT(012) = 1428;
OrderLUT(017) = 1427;
OrderLUT(018) = 124;
OrderLUT(020) = 1426;
OrderLUT(024) = 127;
OrderLUT(027) = 1243;
OrderLUT(029) = 12438;
OrderLUT(030) = 12034;
OrderLUT(033) = 1428;
OrderLUT(034) = 1426;
OrderLUT(036) = 124;
OrderLUT(040) = 128;
OrderLUT(043) = 21834;
OrderLUT(045) = 1243;
OrderLUT(046) = 21349;
OrderLUT(048) = 126;
OrderLUT(051) = 12340;
OrderLUT(053) = 12943;
OrderLUT(054) = 1243;
OrderLUT(065) = 125;
OrderLUT(066) = 1527;
OrderLUT(068) = 1825;
OrderLUT(072) = 123;
OrderLUT(080) = 1327;
OrderLUT(083) = 15234;
OrderLUT(085) = -15234;
OrderLUT(086) = -15243;
OrderLUT(090) = 13247;
OrderLUT(092) = -13247;
OrderLUT(096) = 1328;
OrderLUT(099) = 152834;
OrderLUT(101) = 15234;
OrderLUT(102) = 152349;
OrderLUT(106) = 132847;
OrderLUT(108) = 13247;
OrderLUT(114) = 102347;
OrderLUT(116) = -13247;
OrderLUT(129) = 1527;
OrderLUT(130) = 125;
OrderLUT(132) = 1526;
OrderLUT(136) = 1327;
OrderLUT(139) = 15243;
OrderLUT(141) = 152438;
OrderLUT(142) = 152034;
OrderLUT(144) = 123;
OrderLUT(153) = 12347;
OrderLUT(156) = 123047;
OrderLUT(160) = 1326;
OrderLUT(163) = -152043;
OrderLUT(165) = 13247;
OrderLUT(166) = 15234;
OrderLUT(169) = -182347;
OrderLUT(172) = 193247;
OrderLUT(177) = -132047;
OrderLUT(180) = 13247;
OrderLUT(192) = 127;
OrderLUT(195) = 1243;
OrderLUT(197) = 12438;
OrderLUT(198) = 12034;
OrderLUT(202) = 12364;
OrderLUT(204) = 123648;
OrderLUT(209) = 21364;
OrderLUT(212) = -21364;
OrderLUT(216) = 1243;
OrderLUT(225) = -124638;
OrderLUT(226) = 120364;
OrderLUT(232) = 12438;
OrderLUT(238) = 124356;
OrderLUT(240) = 12034;
OrderLUT(245) = -214356;
OrderLUT(257) = 1528;
OrderLUT(258) = 1526;
OrderLUT(260) = 125;
OrderLUT(264) = 1328;
OrderLUT(267) = -152438;
OrderLUT(269) = 15243;
OrderLUT(270) = -152943;
OrderLUT(272) = 1326;
OrderLUT(275) = 152340;
OrderLUT(277) = 152943;
OrderLUT(278) = 15243;
OrderLUT(281) = 182347;
OrderLUT(282) = -103247;
OrderLUT(288) = 123;
OrderLUT(297) = 12347;
OrderLUT(298) = -123947;
OrderLUT(305) = 123947;
OrderLUT(306) = 12347;
OrderLUT(320) = 128;
OrderLUT(323) = 21834;
OrderLUT(325) = 1243;
OrderLUT(326) = 21349;
OrderLUT(330) = -123648;
OrderLUT(332) = 12364;
OrderLUT(337) = 183642;
OrderLUT(340) = -129364;
OrderLUT(344) = 21834;
OrderLUT(350) = -124365;
OrderLUT(353) = 12463;
OrderLUT(354) = 136492;
OrderLUT(360) = 1243;
OrderLUT(368) = 12943;
OrderLUT(371) = 126543;
OrderLUT(384) = 126;
OrderLUT(387) = 12340;
OrderLUT(389) = 12943;
OrderLUT(390) = 1243;
OrderLUT(394) = -103642;
OrderLUT(396) = 129364;
OrderLUT(401) = 123640;
OrderLUT(404) = 12364;
OrderLUT(408) = 12340;
OrderLUT(413) = 215643;
OrderLUT(417) = -136492;
OrderLUT(418) = 12463;
OrderLUT(424) = 13492;
OrderLUT(427) = -213456;
OrderLUT(432) = 1342;
% Convert to more convinient format
orderLUT = cell(size(OrderLUT));
for i = 1:size(OrderLUT,1)
polygon = abs(OrderLUT(i));
if polygon>0
polygon = num2str(polygon)-48; % Convert from a single number to array of digits
polygon(polygon==0) = 10; % 0 stands for 10
orderLUT{i} = polygon;
end
end
% Negative numbers in the LUT indicate values which were not observed yet
% so they were not independently verified.
verified = OrderLUT>0;
clear OrderLUT
end
%% Calculate unique key for each edgeMat configuration
key = dot(1*edgeMat(:)', [256 128 64 32 16 8 4 2 1]);
assert(key<=432, 'Error: in IntersectionPolygon: key is out of bound');
%% Look up the point order around the polygon
polygon = orderLUT{key};
if (isempty(polygon))
return
end
%% in a rare case of 2 intersections there is ambiguity if one or two
% vertices of the triangle lay inside the other triangle. OrderLUT stores
% only the single vertex cases.
nx = nnz(edgeMat(:));
if nx==2
pList = polygon; % list of vertices to check
pList(pList<=nx) = []; % keep only the triangle points of the polygon
flip = false; % was there a flip from single vertex to vertices case?
for ip = 1:length(pList)
p = pList(ip); % point to check
t = floor((p-nx-1)/3); % does it belong to triangle 0 or 1 (actually 1 or 2)
tri = (1:3) + nx + 3*abs(1-t); % Points belonging to the other triangle
if ~PointInTriangle2D(points(p,dIdx), points(tri,dIdx))
d = nx+t*3; % offset
% "p-d" is vertex number of point just tested: 1, 2, or 3. "b, c" are
% the other 2 vertices
[b, c] = otherDim(p-d);
polygon = [polygon(polygon~=p), b+d, c+d]; % remove i2 and add i0 and i1
flip = true;
end
end
if flip
% if ther were any flips than use existing codes to figure out the
% order of the points around the polygon
DT = delaunayTriangulation(points(polygon,dIdx));
idx = freeBoundary(DT)';
idx(2,:) = [];
polygon = polygon(idx);
end
end
%% Check to duplicate points
tol = 1e6;
P = round(points(polygon,:)*tol)/tol;
[~,ia] = unique(P,'rows'); % V = P(ia,:) and P = V(ic,:).
polygon = polygon(sort(ia));
%% Test the results using more expensive function
doPlot = (~verified(key));
if debug && length(polygon)>3
DT = delaunayTriangulation(points(polygon,dIdx));
idx = freeBoundary(DT)';
idx(2,:) = [];
k = max(abs(diff(idx)));
%doPlot = (k>1 && k<(length(idx)-1)) || (~verified(key));
assert(k==1 || k==(length(idx)-1), 'Two triangle intersection polygon is not convex')
end
if debug && doPlot % plot the interesting cases
PlotTwoTriangles(points, polygon, 'm')
title(sprintf('key = %i', key));
end
end % function
%% ========================================================================
function PlotTwoTriangles(points, polygon, color)
% Plotting function used for debugging
nx = size(points,1)-6;
d = (max(points,[],1)-min(points,[],1))/200;
figure(2)
clf
hold on
line( points(nx+(1:2),1), points(nx+(1:2),2), points(nx+(1:2),3), 'Color', 'g');
line( points(nx+(2:3),1), points(nx+(2:3),2), points(nx+(2:3),3), 'Color', 'g');
line( points(nx+[1,3],1), points(nx+[1,3],2), points(nx+[1,3],3), 'Color', 'g');
line( points(nx+(4:5),1), points(nx+(4:5),2), points(nx+(4:5),3), 'Color', 'b');
line( points(nx+(5:6),1), points(nx+(5:6),2), points(nx+(5:6),3), 'Color', 'b');
line( points(nx+[4,6],1), points(nx+[4,6],2), points(nx+[4,6],3), 'Color', 'b');
plot3( points(:,1), points(:,2), points(:,3), 'm.');
if (length(polygon)>2)
idx = polygon([1:end, 1]);
plot3( points(idx,1), points(idx,2),points(idx,3), 'Color', color, 'LineWidth', 1);
end
for i = 1:nx+6
text(points(i,1)+d(1), points(i,2)+d(2), points(i,3), num2str(i))
end
end % function
%% ========================================================================
function [intersect, X] = EdgesIntersect3D(V1,V2, U1,U2)
%EdgesIntersectPoint3D calculates point of intersection of 2 coplanar
% segments in 3D
%
% INPUTS:
% V1,V2 - 1 x 3 coordinates of endpoints of edge 1
% U1,U2 - 1 x 3 coordinates of endpoints of edge 2
% OUTPUT:
% X - 1 x 3 coordinates of the intersection point
A = V2-V1;
B = U1-U2;
C = U1-V1;
%% Solve system of equations [A,B,1] * [d;e;0] = C for d and e
det3 = @(a,b) ... % determinant of a matrix with columns: [a, b, 1]
a(:,1).*b(:,2)-a(:,3).*b(:,2) + ...
a(:,2).*b(:,3)-a(:,2).*b(:,1) + ...
a(:,3).*b(:,1)-a(:,1).*b(:,3);
f=det3(A,B); % https://en.wikipedia.org/wiki/Cramer%27s_rule#Explicit_formulas_for_small_systems
t=det3(C,B)./f; % use Cramer's rule
s=det3(A,C)./f;
intersect = (t>=0 & t<=1 & s>=0 & s<=1);
X = V1 + bsxfun(@times,A,t);
end % function
%% ========================================================================
function inside = PointInTriangle2D(V1, U)
% check if V1 is inside triangle U (U1,U2,U3)
% Algorithm is checking on which side of the half-plane created by the
% edges the point is. It uses sign of determinant to calculate orientation
% of point triplets.
% INPUTS:
% V1 - 1 x 2 coordinates of a point
% U - 3 x 2 coordinates of endpoints of 3 edges of a triangle
% OUTPUT:
% inside - a boolean or boolean array
det2 = @(A,B,C) (A(:,1)-C(:,1))*(B(:,2)-C(:,2)) - (B(:,1)-C(:,1))*(A(:,2)-C(:,2));
b1 = (det2(U(1,:), U(2,:), V1) > 0);
b2 = (det2(U(2,:), U(3,:), V1) > 0);
b3 = (det2(U(3,:), U(1,:), V1) > 0);
inside = ((b1 == b2) & (b2 == b3)); % inside if same orientation for all 3 edges
end % function
%% ========================================================================
function [b, c] = otherDim(a)
% return set [1 2 3] without k
b = mod(a+1,3)+1; % b and c are vertices which are on the same side of the plane
c = 6-a-b; % a+b+c = 6
end
%% ========================================================================
function overlap = TriangleIntersection3D_Rapid( v1, v2, v3, u1, u2, u3, n1, n2 )
%TriangleIntersection3D tests if 2 triangles defined in 3D intersect.
%
% INPUTS:
% v1, v2, v3, - Nx3 array of surface 1 triangle vertex coordinates
% u1, u2, u3, - Nx3 array of surface 2 triangle vertex coordinates
% n1, n2 - Nx3 array of surface 1 & 2 triangle plane normals. Those
% are optional and if provided than the first 2 steps of the algorithm
% (which are equivalent to first 2 steps of Moller algorithm) will be
% skipped.
%
% OUTPUT:
% iMsk - N x 1 intersection boolean mask marking which triangles overlap
%
% ALGORITHM:
% translated from the UNC-CH V-Collide RAPID code
% https://wwwx.cs.unc.edu/~geom/papers/COLLISION/vcol.pdf
global V1 V2 V3 U1 U2 U3
cross_prod = @(a,b) [...
a(:,2).*b(:,3)-a(:,3).*b(:,2), ...
a(:,3).*b(:,1)-a(:,1).*b(:,3), ...
a(:,1).*b(:,2)-a(:,2).*b(:,1)];
%% shift t1 and t2 by p1#
V1 = zeros(size(v1));
V2 = v2-v1;
V3 = v3-v1;
U1 = u1-v1;
U2 = u2-v1;
U3 = u3-v1;
clear v1 v2 v3 u1 u2 u3
if(nargin<7)
%% now begin the series of tests
n1 = cross_prod( V2-V1, V3-V1 ); % face normals
n2 = cross_prod( U2-U1, U3-U1 ); % face normals
end
%% test the face normals
overlap = project6(n1) & project6(n2);
V1 = V1(overlap,:);
V2 = V2(overlap,:);
V3 = V3(overlap,:);
U1 = U1(overlap,:);
U2 = U2(overlap,:);
U3 = U3(overlap,:);
n1 = n1(overlap,:);
n2 = n2(overlap,:);
%% compute triangle edges
e1 = V2-V1;
e2 = V3-V2;
e3 = V1-V3;
f1 = U2-U1;
f2 = U3-U2;
f3 = U1-U3;
%% run more tests
overlap2 = project6(cross_prod(e1, f1));
overlap2 = project6(cross_prod(e1, f2)) & overlap2;
overlap2 = project6(cross_prod(e1, f3)) & overlap2;
overlap2 = project6(cross_prod(e2, f1)) & overlap2;
overlap2 = project6(cross_prod(e2, f2)) & overlap2;
overlap2 = project6(cross_prod(e2, f3)) & overlap2;
overlap2 = project6(cross_prod(e3, f1)) & overlap2;
overlap2 = project6(cross_prod(e3, f2)) & overlap2;
overlap2 = project6(cross_prod(e3, f3)) & overlap2;
overlap2 = project6(cross_prod(e1, n1)) & overlap2;
overlap2 = project6(cross_prod(e2, n1)) & overlap2;
overlap2 = project6(cross_prod(e3, n1)) & overlap2;
overlap2 = project6(cross_prod(f1, n2)) & overlap2;
overlap2 = project6(cross_prod(f2, n2)) & overlap2;
overlap2 = project6(cross_prod(f3, n2)) & overlap2;
overlap(overlap) = overlap2;
end
%% ========================================================================
function pass = project6( p )
% project all 6 vertices of both triangles onto vector p and check if two
% projections overlap
global V1 V2 V3 U1 U2 U3
dot_prod = @(a,b) a(:,1).*b(:,1)+a(:,2).*b(:,2)+a(:,3).*b(:,3);
%% Project vertices of triangle 1 and find the bounds min1 and max1
P = [dot_prod(p, V1), dot_prod(p, V2), dot_prod(p, V3)];
max1 = max(P,[],2);
min1 = min(P,[],2);
%% Project vertices of triangle 2 and find the bounds min1 and max1
P = [dot_prod(p, U1), dot_prod(p, U2), dot_prod(p, U3)];
max2 = max(P,[],2);
min2 = min(P,[],2);
%% Compare the bounds to see if they overlap
pass = (( min1 < max2 ) & ( min2 < max1 )) | ~dot_prod(p, p);
end
