function[add_nodes1, add_nodes2]=detectGaps(Mesh, Data, Solution)

% detectGaps: computes the gap between nodes of opposing interfaces and
% detects interpenetration
% INPUT:
% Mesh:         Structure containing all the mesh parameters
% Data:         Structure containing all the data parameters
% Solution:     Structure containing the solution
% OUTPUT:
% add_nodes1:   Interface nodes of body 1 to be added to the active set
% add_nodes2:   Interface nodes of body 2 to be added to the active set

% Initial nodes
inodes1=Data.body{1}.initial_nodes;
inodes2=Data.body{2}.initial_nodes;

nodes1=inodes1;
nodes2=inodes2;

% Retrieve coordinates of nodes making up the interfaces
coord1=Mesh.coord_mat(nodes1,:)+Solution.U_sort(nodes1,:);
coord2=Mesh.coord_mat(nodes2,:)+Solution.U_sort(nodes2,:);

[radiuses1, ~, ~, nnzR21, ~]=getRadius(Data, nodes1, nodes2, coord1, coord2);
[radiuses2, ~, ~, nnzR12, ~]=getRadius(Data, nodes2, nodes1, coord2, coord1);

i1=nnzR12==0;
i2=nnzR21==0;

% Check for isolated nodes
while any(i1) || any(i2)
    
    nodes1=nodes1(~i1);
    nodes2=nodes2(~i2);
    
    coord1=coord1(~i1,:);
    coord2=coord2(~i2,:);
    
    % Recompute radiuses
    [radiuses1, ~, ~, nnzR21, ~]=getRadius(Data, nodes1, nodes2, coord1, coord2);
    [radiuses2, ~, ~, nnzR12, ~]=getRadius(Data, nodes2, nodes1, coord2, coord1);
    
    i1=nnzR12==0;
    i2=nnzR21==0;
end

[Phi11] = assembleInterpMat(coord1, coord1, radiuses1);
[Phi21] = assembleInterpMat(coord1, coord2, radiuses1);
[Phi22] = assembleInterpMat(coord2, coord2, radiuses2);
[Phi12] = assembleInterpMat(coord2, coord1, radiuses2);

% Interpolation matrices
R12=Phi12/Phi22;
R21=Phi21/Phi11;

s1=sum(R12,2);
s2=sum(R21,2);

% Rescaling
R12=R12./s1;
R21=R21./s2;

Diff1=R12*coord2-coord1;
Diff2=R21*coord1-coord2;

i1=ismember(inodes1, nodes1);
i2=ismember(inodes2, nodes2);

% computes the scalar products
scalar1_g=sum(Diff1.*Data.body{1}.normal(i1,:),2);
scalar2_g=sum(Diff2.*Data.body{2}.normal(i2,:),2);

% Thresholds are a fraction of the mesh size and could be specific to each
% interface (consider using the mesh sizes of body 1 and 2)
threshold1=-Data.Contact.tol*Data.Contact.h;
threshold2=-Data.Contact.tol*Data.Contact.h;

% Detect interpenetration
add_nodes1=nodes1(scalar1_g<threshold1);
add_nodes2=nodes2(scalar2_g<threshold2);
end