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