diff --git a/Code/INTERNODES_large/SPAI.m b/Code/INTERNODES_large/SPAI.m
new file mode 100644
index 0000000..2625452
--- /dev/null
+++ b/Code/INTERNODES_large/SPAI.m
@@ -0,0 +1,94 @@
+function[M]=SPAI(A, S, varargin)
+
+% SPAI: Implements the SPAI algorithm
+% A: Matrix
+% S: Initial sparsity pattern
+
+n=size(A,1);
+e=speye(n);
+
+% Define default parameters
+% Maximum fill in per column of M per iteration
+Param.s=5;
+% Maximum number of iterations
+Param.maxit=20;
+% Tolerance on the norm of the residual
+Param.tol=0.2;
+
+if nargin > 2
+[Param]=updateFields(Param, varargin{1});
+end
+
+% Retrieve parameters
+s=Param.s;
+tol=Param.tol;
+maxit=Param.maxit;
+
+% Initialization
+V=zeros(maxit*s*n,1);
+In=zeros(maxit*s*n,2);
+count=0;
+
+
+for k=1:n
+    i=S{k};
+    J=false(n,1);
+    J(i)=true;
+    % Set of row indices
+    I=any(A(:,J), 2);
+    As=A(I,J);
+    % Economy-size QR decomposition
+    [Q, R]=qr(As, 0);
+    mk_hat=R\(Q'*e(I,k));
+    % Residual
+    r=A(:,J)*mk_hat-e(:,k);
+    norm_r=norm(r);
+    niter=0;
+    
+    while norm_r>tol && niter<maxit
+        % Set of column indices
+        l=any(A(r~=0,:), 1)';
+        J_tilde=l & ~J;
+        indx=find(J_tilde);
+        
+        At=A(:,J_tilde);
+        d1=(r'*At);
+        d2=vecnorm(At,2,1);
+        rho2=norm_r^2-(d1./d2).^2;
+        % First selection
+        crit1=rho2<=1/length(rho2)*sum(rho2);
+        rho2=rho2(crit1);
+        indx=indx(crit1);
+        % Second selection
+        [~,crit2]=mink(rho2, s);
+        
+        J_tilde(J_tilde)=0;
+        J_tilde(indx(crit2))=1;
+        
+        % Update column index set
+        J=J | J_tilde;
+        
+        % Set of row indices
+        I=any(A(:,J), 2);
+        As=A(I,J);
+        % Economy-size QR decomposition
+        [Q, R]=qr(As, 0);
+        mk_hat=R\(Q'*e(I,k));
+        % Residual
+        r=A(:,J)*mk_hat-e(:,k);
+        norm_r=norm(r);
+        niter=niter+1;
+             
+    end
+    indices=find(J);
+    count_old=count;
+    count_new=count+length(indices);
+    V(count_old+1:count_new)=mk_hat;
+    In(count_old+1:count_new,1)=indices;
+    In(count_old+1:count_new,2)=k;
+    count=count_new;
+end
+
+i=V~=0;
+M=sparse(In(i,1), In(i,2), V(i), n, n);
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_large/assembleInterpMat.m b/Code/INTERNODES_large/assembleInterpMat.m
new file mode 100644
index 0000000..f83e934
--- /dev/null
+++ b/Code/INTERNODES_large/assembleInterpMat.m
@@ -0,0 +1,28 @@
+function[Phi]=assembleInterpMat(coord_ref, coord_interp, radiuses)
+
+% assembleInterpMat: Assemblage of the interpolation matrices for the
+% localized radial basis functions in dimension d
+% INPUT:
+% coord_ref:    Coordinate matrix of the reference points (size m x d)
+% coord_interp: Coordinate matrix for the interpolation points (size n x d)
+% radiuses:     Vector of radiuses of the radial basis functions. It is a row
+%               vector (size 1 x m)
+% OUTPUT:
+% Phi:          Interpolation matrix
+
+m=size(coord_ref,1);
+n=size(coord_interp,1);
+I=[];
+V=[];
+
+% Proceed column after column
+for k=1:m
+    dist=vecnorm(coord_interp-coord_ref(k,:),2,2);
+    index=dist<=radiuses(k);
+    i=find(index);
+    I=[I; i, k*ones(length(i),1)];
+    V=[V; (1-dist(index)./radiuses(k)).^4.*(1+4*dist(index)./radiuses(k))];
+end
+
+Phi=sparse(I(:,1), I(:,2), V, n, m);
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_large/computeNormals.m b/Code/INTERNODES_large/computeNormals.m
new file mode 100644
index 0000000..9a30e45
--- /dev/null
+++ b/Code/INTERNODES_large/computeNormals.m
@@ -0,0 +1,77 @@
+function[Data]=computeNormals(Mesh, Data, Solution)
+
+% computeNormals: Computes the normal vectors for each interface
+% INPUT:
+% Mesh:     Structure containing all the mesh parameters
+% Data:     Structure containing all the data parameters
+% Solution: Structure containing the solution
+% OUTPUT:
+% Data:     Updated structure with the interface normals
+
+% Retrieve mesh data
+% Connectivity matrix
+connect=Mesh.connect_mat;
+
+% Local number of degrees of freedom (dimension in solid mechanics)
+d=Mesh.nb_dof_local;
+% Retrieve bodies
+body=Data.body;
+% Compute the normals based on the deformed structure
+def=Mesh.coord_mat+Solution.U_sort;
+
+nb_bodies=length(body);
+
+
+for k=1:nb_bodies
+    % Normal vectors are computed for all nodes in the initial potential 
+    % contact interface
+    connecti=body{k}.initial_interface_connect;
+    nodesi=body{k}.initial_nodes;
+    
+    % Reduced connectivity matrix
+    connect_red=connect(any(ismember(connect, nodesi), 2), :);
+    % Reduced set of body nodes
+    nodesb=setdiff(unique(connect_red), nodesi);  
+    n=length(nodesi);
+    m=size(connecti,1);
+    % Coordinates of the endpoints of the segments
+    coord1=def(connecti(:,1),:);
+    coord2=def(connecti(:,2),:);
+    % Tangent vectors
+    V=coord2-coord1;
+    % Segment lengths
+    L=vecnorm(V,2,2);
+    V=V./vecnorm(V,2,2);
+
+    % !! Only for a two-dimensional case !!
+    N=zeros(m, d);
+    N(:,1)=-V(:,2);
+    N(:,2)=V(:,1);
+    
+    % Initialization
+    normals=zeros(n,d);
+    % Step size
+    gamma=1e-3;
+    
+    for j=1:n
+        [id,~]=find(connecti==nodesi(j));
+        l=L(id);
+        % Weighted average of normal vectors
+        normals(j,:)=1/sum(l)*sum(N(id,:).*l,1);
+        % Current node
+        x=def(nodesi(j),:);
+        % Step in the direction of the gradient
+        y_plus=x+gamma*normals(j,:);
+        % Step in the opposite direction of the gradient
+        y_minus=x-gamma*normals(j,:);
+        min_plus=min(vecnorm(def(nodesb,:)-y_plus,2,2));
+        min_minus=min(vecnorm(def(nodesb,:)-y_minus,2,2));
+        if min_plus < min_minus
+            normals(j,:)=-normals(j,:);
+        end
+    end
+    
+    normals=normals./vecnorm(normals,2,2);
+    Data.body{k}.normal=normals;
+end
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_large/computeRHS.m b/Code/INTERNODES_large/computeRHS.m
new file mode 100644
index 0000000..b6f4112
--- /dev/null
+++ b/Code/INTERNODES_large/computeRHS.m
@@ -0,0 +1,223 @@
+function[Data]=computeRHS(Mesh, Data)
+
+% computeRHS: Function which assembles the right-hand side consisting of
+% body forces and surface tractions
+% INPUT:
+% Mesh:             Structure containing the mesh parameters
+% Data:             Structure containing the data parameters
+% OUTPUT:
+% Data:             Updated data structure
+
+% Assemble the global vector of body forces
+[Bs]=computeBody(Mesh, Data);
+% Assemble the global vector of surface tractions
+[Bn]=computeNeumann(Mesh, Data);
+
+% Sum the contribution from body forces and Neumann boundary
+% conditions
+B=Bs+Bn;
+% Update data
+Data.F=B;
+
+    function[B]=computeBody(Mesh, Data)
+        
+        % computeBody: Assembles the component for the body forces
+        % INPUT:
+        % Mesh:     Structure containing all the mesh parameters
+        % Data:     Structure containing all the data parameters
+        % OUTPUT:
+        % B:        Global vector of body forces
+        
+        % Retrieve mesh data
+        % Local number of degrees of freedom (dimension in solid mechanics)
+        d=Mesh.nb_dof_local;
+        % Total number of degrees of freedom
+        nb_dof=Mesh.nb_dof;
+        % Coordinate matrix
+        coord=Mesh.coord_mat;
+        % Connectivity matrix
+        connect=Mesh.connect_mat;
+        % Finite element order
+        order=Mesh.order;
+        % Number of elements
+        ne=Mesh.nb_elem;
+        % Number of nodes per element
+        nne=Mesh.nb_nodes_elem;
+        % Matrix of equation numbers linking an element to the degrees of freedom
+        % associated to it.
+        Eq=Mesh.Eq;
+        
+        % Retrieve data parameters
+        % Body force vector
+        f=Data.Coefficient.f.function;
+        % Retrieve the basis functions
+        [Functions]=getFunctions(order);
+        % Retrieve basis functions
+        phi=Functions.phi;
+        
+        
+        % Retrieve quadrature points and weights
+        switch order
+            case 1
+                [points, weights] = getQuadrature(3, 'bulk');
+            case 2
+                [points, weights] = getQuadrature(4, 'bulk');
+            case 3
+                [points, weights] = getQuadrature(6, 'bulk');
+            otherwise
+                error('Not yet implemented');
+        end
+        
+        % Number of quadrature points
+        nq=length(weights);
+        s=nne*d;
+        Id=eye(d);
+        % Initialization
+        X=zeros(ne, nq, 2);
+        Q=zeros(s, nq*d);
+        
+        
+        % Loop over the Gauss points
+        for i = 1:nq
+            Phi=phi(points(i,:));
+            Q(:, (i-1)*d+1:i*d)=kron(Phi, Id);
+        end
+        
+        I=Eq';
+        T=connect';
+        G=T(:);
+        
+        % Coordinates of the nodes of the elements
+        coord_nodes=coord(G,:);
+        % Vertices of the elements
+        a=coord_nodes(1:nne:end,:);
+        b=coord_nodes(2:nne:end,:);
+        c=coord_nodes(3:nne:end,:);
+        % Interpolated coordinates
+        P=a(:)+[b(:)-a(:) c(:)-a(:)]*points';
+        X(:,:,1)=P(1:ne,:);
+        X(:,:,2)=P(ne+1:end,:);
+        
+        % Determinants of Jacobian matrices
+        detJK=(b(:,1)-a(:,1)).*(c(:,2)-a(:,2))-(b(:,2)-a(:,2)).*(c(:,1)-a(:,1));
+        
+        % Evaluation at the Gauss points
+        F=f(X);
+        if size(F,1)>d
+            F=reshape(F, ne, d*nq);
+            F=reshape(F', d, nq, ne);
+        end
+        
+        X=reshape(abs(detJK), 1, 1, ne).*F.*weights;
+        X=reshape(X, d*nq, ne);
+        
+        B=Q*X;
+        
+        % Assemblage
+        B=accumarray(I(:), B(:), [nb_dof 1]);
+    end
+
+    function[B]=computeNeumann(Mesh, Data)
+        
+        % computeNeumann: Implements Neumann boundary conditions, returns a vector
+        % having for length the number of degrees of freedom.
+        % This vector is to be added to the RHS.
+        % INPUT:
+        % Mesh:     Structure containing all the mesh parameters
+        % Data:     Structure containing all the data parameters
+        % OUTPUT:
+        % B:        Global vector of surface tractions
+        
+        % Retrieve mesh parameters
+        % Total number of degrees of freedom
+        nb_dof=Mesh.nb_dof;
+        % Number of local degrees of freedom
+        d=Mesh.nb_dof_local;
+        % Coordinate matrix
+        coord=Mesh.coord_mat;
+        % Order
+        order=Mesh.order;
+        % Boundary equation numbers
+        Eq_BC=Mesh.Eq_BC;
+        % Retrive functions
+        [Functions]=getFunctions(order);
+        % Retrieve boundary basis functions
+        phi_boundary=Functions.phi_boundary;
+        % Get quadrature
+        [points, weights]=getQuadrature(3, 'boundary');
+        % Number of quadrature points
+        nq=length(weights);
+        
+        % Initialization
+        B=zeros(nb_dof,1);
+        s=order+1;
+        Id=eye(d);
+        % Initialization
+        Q=zeros(s*d, nq*d);
+        
+        % Loop over the Gauss points
+        for i = 1:nq
+            Phi=phi_boundary(points(i,:));
+            Q(:, (i-1)*d+1:i*d)=kron(Phi, Id);
+        end
+        
+        
+        % Neumann boundary conditions
+        N_BC=Data.N_BC;
+        % Number of boundaries where surface tractions are imposed
+        nb_bc=length(N_BC);
+        
+        for m=1:nb_bc
+            % Retrieve data
+            data=N_BC{m};
+            edges=data.edges;
+            
+            for flag=edges
+                % Surface traction function
+                f=data.function;
+                % Retriveve edges making up the boundary
+                Neum_edges=Mesh.BC_nodes(Mesh.BC_tag==flag,:);
+                % Equation numbers
+                Eq=Eq_BC(Mesh.BC_tag==flag,:);
+                % Number of Neumann boundary edges
+                ne=size(Neum_edges, 1);
+                
+                % Initialization
+                X=zeros(ne, nq, 2);
+                
+                C=Neum_edges';
+                G=C(:);
+                I=Eq';
+                
+                % Coordinates of the nodes of the elements
+                coord_nodes=coord(G,:);
+                % Endpoints of the edges
+                a=coord_nodes(1:s:end,:);
+                b=coord_nodes(2:s:end,:);
+                bk=b-a;
+                norm_bk=vecnorm(bk,2,2);
+                
+                % Interpolated coordinates
+                P=a(:)+(b(:)-a(:))*points';
+                X(:,:,1)=P(1:ne,:);
+                X(:,:,2)=P(ne+1:end,:);
+                
+                % Evaluation at the Gauss points
+                F=f(X);
+                if size(F,1)>d
+                    F=reshape(F, ne, d*nq);
+                    F=reshape(F', d, nq, ne);
+                end
+                
+                X=reshape(norm_bk, 1, 1, ne).*F.*weights';
+                X=reshape(X, d*nq, ne);
+                
+                Bl=Q*X;
+                
+                % Assemblage
+                B=B+accumarray(I(:), Bl(:), [nb_dof 1]);
+                
+            end
+        end
+    end
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_large/computeStiffness.m b/Code/INTERNODES_large/computeStiffness.m
new file mode 100644
index 0000000..6091151
--- /dev/null
+++ b/Code/INTERNODES_large/computeStiffness.m
@@ -0,0 +1,138 @@
+function[K]=computeStiffness(Mesh, Data)
+
+% computeStiffness: Function which assembles the global stiffness matrix
+% Assumes all elements are of the same type
+% INPUT:
+% Mesh:     Structure containing all the mesh parameters
+% Data:     Structure containing all the data parameters
+% OUTPUT:
+% K:        Global stiffness matrix
+
+% Mesh parameters
+% Local number of degrees of freedom (dimension in solid mechanics)
+d=Mesh.nb_dof_local;
+% Total number of degrees of freedom
+nb_dof=Mesh.nb_dof;
+% Coordinate matrix
+coord=Mesh.coord_mat;
+% Connectivity matrix
+connect=Mesh.connect_mat;
+% Finite element order
+order=Mesh.order;
+% Number of nodes per element
+nne=Mesh.nb_nodes_elem;
+% Total number of elements
+ne=Mesh.nb_elem;
+% Matrix linking an element to the degrees of freedom associated to it.
+Eq=Mesh.Eq;
+
+% Data parameters
+% Mu
+f1=Data.Coefficient.mu.function;
+% Lambda
+f2=Data.Coefficient.lambda.function;
+
+
+% Retrieve functions
+[Functions]=getFunctions(order);
+% Jacobian matrix
+J_phi=Functions.J_phi;
+% Define permutation matrix
+Sdd=zeros(d*d);
+Id=eye(d);
+for k=1:d
+    Sdd=Sdd+kron(Id(:,k)', kron(Id, Id(:,k)));
+end
+
+% Retrieve quadrature nodes and weights
+switch order
+    case 1
+        [points, weights] = getQuadrature(1, 'bulk');
+    case 2
+        [points, weights] = getQuadrature(2, 'bulk');
+    case 3
+        [points, weights] = getQuadrature(3, 'bulk');
+    otherwise
+        error('Not yet implemented');
+end
+
+% Initialization
+s=nne*d;
+nq=length(weights);
+Q=zeros(s^2, nq*d^4);
+
+% Loop over the Gauss points
+for i = 1:nq
+    L=kron(J_phi(points(i,:)), Id);
+    Q(:, (i-1)*d^4+1:i*d^4)=kron(L, L);
+end
+
+T=Eq';
+G=T(:);
+
+I=repmat(Eq, [1 nne*d])';
+J=repmat(G', [nne*d 1]);
+
+T=connect';
+G=T(:);
+
+% Coordinates of the nodes of the elements
+coord_nodes=coord(G,:);
+% Vertices of the elements
+a=coord_nodes(1:nne:end,:);
+b=coord_nodes(2:nne:end,:);
+c=coord_nodes(3:nne:end,:);
+% Interpolated coordinates
+P=a(:)+[b(:)-a(:) c(:)-a(:)]*points';
+X(:,:,1)=P(1:ne,:);
+X(:,:,2)=P(ne+1:end,:);
+
+% Determinants of Jacobian matrices
+detJK=(b(:,1)-a(:,1)).*(c(:,2)-a(:,2))-(b(:,2)-a(:,2)).*(c(:,1)-a(:,1));
+
+V=[(vecnorm(c-a,2,2).^2) -sum((c-a).*(b-a),2) -sum((c-a).*(b-a),2) (vecnorm(b-a,2,2).^2)];
+W=1./(detJK').^2.*(V');
+
+V=reshape(W,d,d*ne);
+
+JK_inv=zeros(d, d*ne);
+JK_inv(:,1:d:d*ne)=1./detJK'.*[c(:,2)-a(:,2) -(b(:,2)-a(:,2))]';
+JK_inv(:,2:d:d*ne)=1./detJK'.*[-(c(:,1)-a(:,1)) b(:,1)-a(:,1)]';
+
+JK_invT=zeros(d, d*ne);
+JK_invT(:,1:d:d*ne)=1./detJK'.*[c(:,2)-a(:,2) -(c(:,1)-a(:,1))]';
+JK_invT(:,2:d:d*ne)=1./detJK'.*[-(b(:,2)-a(:,2)) b(:,1)-a(:,1)]';
+
+vec_JK_invT=reshape(JK_invT, d^2, ne);
+
+[S1]=product(V , repmat(Id, 1, ne), d, ne);
+[S2]=product(JK_invT , JK_inv, d, ne);
+S2=Sdd*S2;
+
+A1=reshape(S1, d^4, ne);
+A2=reshape(S2, d^4, ne);
+A3=kr(vec_JK_invT, vec_JK_invT);
+
+
+Lambda1=(abs(detJK).*f1(X).*weights)';
+Lambda2=(abs(detJK).*f2(X).*weights)';
+
+X=kr(Lambda1, A1+A2)+kr(Lambda2, A3);
+K=Q*X;
+
+% Assemblage
+K=sparse(I(:), J(:), K(:), nb_dof, nb_dof);
+
+    function[M]=product(A,B,d,ne)
+        
+        % Blockwise Kronecker product of two matrices
+        % !! Only for a two-dimensional case !!
+        M=zeros(d^2, d^2*ne);
+        
+        M(:,1:d^2:end)=kr(A(:,1:d:end), B(:,1:d:end));
+        M(:,2:d^2:end)=kr(A(:,1:d:end), B(:,2:d:end));
+        M(:,3:d^2:end)=kr(A(:,2:d:end), B(:,1:d:end));
+        M(:,4:d^2:end)=kr(A(:,2:d:end), B(:,2:d:end));
+    end
+
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_large/constructInterpolants.m b/Code/INTERNODES_large/constructInterpolants.m
new file mode 100644
index 0000000..f252815
--- /dev/null
+++ b/Code/INTERNODES_large/constructInterpolants.m
@@ -0,0 +1,55 @@
+function[Data]=constructInterpolants(Data, radiuses1, radiuses2, coord1, coord2)
+
+% constructInterpolants: Assembles the interpolation matrices and
+% pre-computes sparse LU factorizations of Phi11 and Phi22
+% INPUT:
+% Data:         Structure containing all the data parameters
+% radiusesk:    Radiuses of support of the radial basis functions (k=1,2)
+% coordk:       Coordinates of the nodes on the potential contact interface
+%               of body k (k=1,2)
+% OUTPUT:
+% Data:         Updated data structure with RBF interpolants
+
+[Phi11] = assembleInterpMat(coord1, coord1, radiuses1);
+[Phi21] = assembleInterpMat(coord1, coord2, radiuses1);
+[Phi22] = assembleInterpMat(coord2, coord2, radiuses2);
+[Phi12] = assembleInterpMat(coord2, coord1, radiuses2);
+
+% Store sparse factorization of matrices
+[L1, U1, orl1, orc1]=lu(Phi11, 'vector');
+[L2, U2, orl2, orc2]=lu(Phi22, 'vector');
+
+n1=length(radiuses1);
+n2=length(radiuses2);
+
+y1=ones(n1,1);
+y2=ones(n2,1);
+
+s1=zeros(n2,1);
+s2=zeros(n1,1);
+
+s1(orc2)=U2\(L2\y2(orl2));
+s2(orc1)=U1\(L1\y1(orl1));
+
+s1=Phi12*s1;
+s2=Phi21*s2;
+
+% For future purposes
+Data.L1=L1;
+Data.U1=U1;
+Data.L2=L2;
+Data.U2=U2;
+Data.Phi11=Phi11;
+Data.Phi21=Phi21;
+Data.Phi12=Phi12;
+Data.Phi22=Phi22;
+
+Data.orl1=orl1;
+Data.orc1=orc1;
+
+Data.orl2=orl2;
+Data.orc2=orc2;
+
+Data.s1=s1;
+Data.s2=s2;
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_large/detectGaps.m b/Code/INTERNODES_large/detectGaps.m
new file mode 100644
index 0000000..190a7a6
--- /dev/null
+++ b/Code/INTERNODES_large/detectGaps.m
@@ -0,0 +1,67 @@
+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
+
+[Data]=constructInterpolants(Data, radiuses1, radiuses2, coord1, coord2);
+
+Diff1=multiplyR12(Data, coord2)-coord1;
+Diff2=multiplyR21(Data, 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
\ No newline at end of file
diff --git a/Code/INTERNODES_large/findNNZc.m b/Code/INTERNODES_large/findNNZc.m
new file mode 100644
index 0000000..2d9063b
--- /dev/null
+++ b/Code/INTERNODES_large/findNNZc.m
@@ -0,0 +1,17 @@
+function[I]=findNNZc(i,j)
+
+% findNNZc: Finds the smallest row indices for each column index
+% INPUT:
+% i:    Row indices
+% j:    Column indices
+% OUTPUT:
+% I:    Smallest row indices for each column index in j
+
+J=unique(j);
+n=length(J);
+I=zeros(n,1);
+
+for k=1:n
+    I(k)=min(i(j==J(k)));
+end
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_large/getFunctions.m b/Code/INTERNODES_large/getFunctions.m
new file mode 100644
index 0000000..ddff9aa
--- /dev/null
+++ b/Code/INTERNODES_large/getFunctions.m
@@ -0,0 +1,46 @@
+function[Functions]=getFunctions(order)
+
+% getFunctions: Returns the basis functions, their jacobian matrices and
+% the basis functions needed to integrate over the boundary
+% INPUT:
+% order:        Finite element order
+% OUTPUT:
+% Functions:    Structure containing all the functions
+
+
+switch order
+    case 1
+Functions.phi=@(x) [1-x(1)-x(2);...
+                    x(1);...
+                    x(2)];
+                
+Functions.J_phi=@(x) [-1 -1; ...
+                       1  0;...
+                       0  1]; 
+                   
+Functions.phi_boundary=@(x) [1-x;...
+                             x];
+    case 2
+Functions.phi=@(x) [(1-x(1)-x(2))*(1-2*x(1)-2*x(2));...
+                    x(1)*(2*x(1)-1);...
+                    x(2)*(2*x(2)-1);...
+                    4*x(1)*(1-x(1)-x(2));...
+                    4*x(1)*x(2);...
+                    4*x(2)*(1-x(1)-x(2))];
+                 
+                                                       
+Functions.J_phi=@(x) [-3+4*x(2)+4*x(1) -3+4*x(1)+4*x(2); ...
+                       -1+4*x(1)        0;...
+                       0                -1+4*x(2);...
+                       4-4*x(2)-8*x(1)  -4*x(1);...
+                       4*x(2)           4*x(1);...
+                       -4*x(2)          4-4*x(1)-8*x(2)];
+
+Functions.phi_boundary=@(x) [(2*x-1)*(x-1);...
+                            x*(2*x-1);...
+                            4*x*(1-x)];
+               
+    otherwise
+        error('Unimplemented!');
+end
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_large/getParameters.m b/Code/INTERNODES_large/getParameters.m
new file mode 100644
index 0000000..4a0812c
--- /dev/null
+++ b/Code/INTERNODES_large/getParameters.m
@@ -0,0 +1,72 @@
+function[Mesh]=getParameters(Mesh)
+
+% getParameters: Retrieves mesh parameters
+% INPUT:
+% Mesh: Structure containing the mesh geometry
+% OUTPUT:
+% Mesh: Updated structure containing all mesh parameters needed to run the code
+
+% Retrieve mesh parameters
+% Coordinate matrix
+coord=Mesh.coord_mat;
+% Connectivity matrix
+connect=Mesh.connect_mat;
+% Boundary nodes
+BC_nodes=Mesh.BC_nodes;
+
+% Geometry [m] 
+x_min=min(coord(:,1));
+x_max=max(coord(:,1));
+y_min=min(coord(:,2));
+y_max=max(coord(:,2));
+% Maximum dimension [m]
+L_max=max([abs(x_max-x_min) abs(y_max-y_min)]);
+% Number of elements
+nb_elem=size(connect,1);
+% Number of nodes per element
+nb_nodes_elem=size(connect,2);
+% Number of nodes
+nb_nodes=size(coord,1);
+% Local number of degrees of freedom
+nb_dof_local=size(coord,2);
+% Total number of degrees of freedom
+nb_dof=nb_dof_local*nb_nodes;
+% Number of boundary nodes
+nb_BC_nodes=size(BC_nodes,1);
+
+% Order of polynomials
+% It is assumed all elements of the mesh are of same type
+order_map=containers.Map([3 6 10], [1 2 3]);
+order=order_map(nb_nodes_elem);
+
+% Initialization
+% Matrix of equation numbers   
+Eq=zeros(nb_elem,nb_dof_local*nb_nodes_elem);
+% Matrix of equation number for boundary nodes;
+Eq_BC=zeros(nb_BC_nodes,nb_dof_local*order);
+% Matrix containing the sets of degrees of freedom for each node
+Dof_set=zeros(nb_dof_local, nb_nodes);
+
+% Iteration over the dimension
+for k=1:nb_dof_local
+    d1=k:nb_dof_local:nb_dof_local*nb_nodes_elem;
+    d2=k:nb_dof_local:nb_dof_local*(order+1);
+    Eq(:,d1)=nb_dof_local*connect-nb_dof_local+k;
+    Eq_BC(:,d2)=nb_dof_local*BC_nodes-nb_dof_local+k;
+    Dof_set(k,:)=nb_dof_local*(1:nb_nodes)-nb_dof_local+k;
+end
+
+% Update mesh structure
+Mesh.frame=[x_min y_min; x_max y_max];
+Mesh.L_max=L_max;
+Mesh.nb_elem=nb_elem;
+Mesh.nb_nodes_elem=nb_nodes_elem;
+Mesh.nb_nodes=nb_nodes;
+Mesh.nb_dof_local=nb_dof_local;
+Mesh.nb_dof=nb_dof;
+Mesh.order=order;
+Mesh.Eq=Eq;
+Mesh.Eq_BC=Eq_BC;
+Mesh.nb_BC_nodes=nb_BC_nodes;
+Mesh.Dof_set=Dof_set;
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_large/getQuadrature.m b/Code/INTERNODES_large/getQuadrature.m
new file mode 100755
index 0000000..706128e
--- /dev/null
+++ b/Code/INTERNODES_large/getQuadrature.m
@@ -0,0 +1,362 @@
+function[points, weights] = getQuadrature(id, object)
+
+% getQuadrature: Function which returns the integration points and
+% weights for a given quadrature ID
+% INPUT:
+% id:       Integration ID
+% OUTPUT:
+% points:   Integration points
+% weights:  Integration weights
+
+switch object
+    case 'bulk'
+        switch id
+            case 1
+                % Source: Wriggers, Computational contact mechanics (2006)
+                % Number of points: 1
+                % Degree of exactness: 1
+                
+                a1=1/3;                 w1=1;
+                
+                points=[a1 a1];
+                
+                weights=0.5*w1;
+                
+            case 2
+                % Source: Wriggers, Computational contact mechanics (2006)
+                % Number of points: 3
+                % Degree of exactness: 2
+                
+                a1=2/3;                 w1=1/3;
+                b1=1/6;
+                
+                points=[b1 b1;
+                        a1 b1;
+                        b1 a1];
+                
+                weights=0.5*[w1 w1 w1];
+                
+            case 3
+                % Source: Wriggers, Computational contact mechanics (2006)
+                % Number of points: 4
+                % Degree of exactness: 3
+                
+                a1=1/3;                 w1=-27/48;
+                a2=3/5;                 w2=25/48;
+                b2=1/5;
+                
+                points=[a1 a1;            
+                        b2 b2;
+                        a2 b2;
+                        b2 a2];
+                
+                weights=0.5*[w1 w2 w2 w2];
+                
+            case 4
+                % Source: Laursen and Gellert, Some criteria for
+                % numerically integrated matrices and quadrature formulas
+                % for triangles (1978)
+                % Number of points: 6
+                % Degree of exactness: 4
+                
+                a1=0.816847572980459;       w1=0.109951743655322;
+                b1=0.091576213509771;       w2=0.223381589678011;
+                a2=0.108103018168070;
+                b2=0.445948490915965;
+                
+                points=[b1 b1;
+                        a1 b1;
+                        b1 a1;
+                        b2 b2;
+                        a2 b2;
+                        b2 a2];
+                
+                weights=0.5*[w1 w1 w1 w2 w2 w2];
+                
+            case 5
+                % Source: Laursen and Gellert, Some criteria for
+                % numerically integrated matrices and quadrature formulas
+                % for triangles (1978)
+                % Number of points: 7
+                % Degree of exactness: 5
+                
+                a1=1/3;                     w1=0.225000000000000;
+                a2=0.797426985353087;       w2=0.125939180544827;
+                b2=0.101286507323456;       w3=0.132394152788506;
+                a3=0.059715871789770;
+                b3=0.470142064105115;
+                
+                points=[a1 a1;
+                        b2 b2;
+                        a2 b2;
+                        b2 a2;
+                        b3 b3;
+                        a3 b3;
+                        b3 a3];
+                    
+               weights=0.5*[w1 w2 w2 w2 w3 w3 w3];
+                    
+            case 6
+                % Source: Laursen and Gellert, Some criteria for
+                % numerically integrated matrices and quadrature formulas
+                % for triangles (1978)
+                % Number of points: 12
+                % Degree of exactness: 6
+                
+                a1=0.873821971016996;       w1=0.050844906370207;
+                b1=0.063089014491502;       w2=0.116786275726379;
+                a2=0.501426509658179;       w3=0.082851075618374;
+                b2=0.249286745170910;
+                a3=0.636502499121399;
+                b3=0.310352451033785;
+                c3=0.053145049844816;
+                
+                points=[b1 b1;
+                        b1 a1;
+                        a1 b1;
+                        b2 b2;
+                        b2 a2;
+                        a2 b2;
+                        a3 b3;
+                        a3 c3;
+                        b3 a3;
+                        b3 c3;
+                        c3 a3;
+                        c3 b3];
+                    
+               weights=0.5*[w1 w1 w1 w2 w2 w2 w3 w3 w3 w3 w3 w3];
+                 
+            case 7
+                % Source: Laursen and Gellert, Some criteria for
+                % numerically integrated matrices and quadrature formulas
+                % for triangles (1978)
+                % Number of points: 13
+                % Degree of exactness: 7
+                
+                a1=1/3;                     w1=-0.149570044467670;
+                a2=0.479308067841923;       w2= 0.175615257433204;           
+                b2=0.260345966079038;       w3= 0.053347235608839;
+                a3=0.869739794195568;       w4= 0.077113760890257;
+                b3=0.065130102902216;
+                a4=0.638444188569809;
+                b4=0.312865496004875;
+                c4=0.048690315425316;
+                
+                points=[a1 a1;
+                        b2 b2;
+                        b2 a2;
+                        a2 b2;
+                        b3 b3;
+                        b3 a3;
+                        a3 b3;
+                        a4 b4;
+                        a4 c4;
+                        b4 a4;
+                        b4 c4;
+                        c4 a4;
+                        c4 b4];
+                                                      
+                weights=0.5*[w1 w2 w2 w2 w3 w3 w3 w4 w4 w4 w4 w4 w4];
+
+            case 8
+                % Source: Laursen and Gellert, Some criteria for
+                % numerically integrated matrices and quadrature formulas
+                % for triangles (1978)
+                % Number of points: 16
+                % Degree of exactness: 8
+                
+                a1=1/3;                     w1=0.144315607677787;
+                a2=0.658861384496478;       w2=0.103217370534718;
+                b2=0.170569307751761;       w3=0.032458497623198;
+                a3=0.898905543365938;       w4=0.095091634267284;
+                b3=0.050547228317031;       w5=0.027230314174435;
+                a4=0.081414823414554;
+                b4=0.459292588292723;
+                a5=0.008394777409958;
+                b5=0.263112829634638;
+                c5=0.728492392955404;
+                
+                points=[a1 a1;
+                        b2 b2;
+                        b2 a2;
+                        a2 b2;
+                        b3 b3;
+                        b3 a3;
+                        a3 b3;
+                        b4 b4;
+                        b4 a4;
+                        a4 b4;
+                        a5 b5;
+                        a5 c5;
+                        b5 a5;
+                        b5 c5;
+                        c5 a5;
+                        c5 b5];
+                    
+                weights=0.5*[w1 w2 w2 w2 w3 w3 w3 w4 w4 w4 w5 w5 w5 w5 w5 w5];
+                
+            case 9
+                % Source: Laursen and Gellert, Some criteria for
+                % numerically integrated matrices and quadrature formulas
+                % for triangles (1978)
+                % Number of points: 19
+                % Degree of exactness: 9
+                
+                a1=1/3;                     w1=0.097135796282799;
+                a2=0.020634961602524;       w2=0.031334700227139;
+                b2=0.489682519198738;       w3=0.077827541004774;
+                a3=0.125820817014126;       w4=0.079647738927210;
+                b3=0.437089591492937;       w5=0.025577675658698;
+                a4=0.623592928761934;       w6=0.043283539377289;
+                b4=0.188203535619033;
+                a5=0.910540973211094;
+                b5=0.044729513394453;
+                a6=0.036838412054736;
+                b6=0.221962989160766;
+                c6=0.741198598784498;
+                
+                points=[a1 a1;
+                        b2 b2;
+                        b2 a2;
+                        a2 b2;
+                        b3 b3;
+                        b3 a3;
+                        a3 b3;
+                        b4 b4;
+                        b4 a4;
+                        a4 b4;
+                        b5 b5;
+                        b5 a5;
+                        a5 b5;
+                        a6 b6;
+                        a6 c6;
+                        b6 a6;
+                        b6 c6;
+                        c6 a6;
+                        c6 b6];
+              
+             weights=0.5*[w1 w2 w2 w2 w3 w3 w3 w4 w4 w4 w5 w5 w5 w6 w6 w6 w6 w6 w6];       
+                    
+            case 10
+                % Source: Laursen and Gellert, Some criteria for
+                % numerically integrated matrices and quadrature formulas
+                % for triangles (1978)
+                % Number of points: 25
+                % Degree of exactness: 10
+                  
+                a1=1/3;                     w1=0.081743329146286;
+                a2=0.715677797886872;       w2=0.045957963604745;
+                b2=0.142161101056564;       w3=0.013352968813150;
+                a3=0.935889253566112;       w4=0.063904906396424;
+                b3=0.032055373216944;       w5=0.034184648162959;
+                a4=0.148132885783821;       w6=0.025297757707288;
+                b4=0.321812995288835;
+                c4=0.530054118927344;
+                a5=0.029619889488730;
+                b5=0.369146781827811;
+                c5=0.601233328683459;
+                a6=0.028367665339938;
+                b6=0.163701733737182;
+                c6=0.807930600922880;
+                 
+                points=[a1 a1;
+                        b2 b2;
+                        a2 b2;
+                        b2 a2;
+                        b3 b3;
+                        a3 b3;
+                        b3 a3;
+                        a4 b4;
+                        a4 c4;
+                        b4 a4;
+                        b4 c4;
+                        c4 a4;
+                        c4 b4;
+                        a5 b5;
+                        a5 c5;
+                        b5 a5;
+                        b5 c5;
+                        c5 a5;
+                        c5 b5;
+                        a6 b6;
+                        a6 c6;
+                        b6 a6;
+                        b6 c6;
+                        c6 a6;
+                        c6 b6];
+                                    
+                weights=0.5*[w1 w2 w2 w2 w3 w3 w3 w4 w4 w4 w4 w4 w4 w5 w5 w5 w5 w5 w5 w6 w6 w6 w6 w6 w6];           
+                
+            otherwise
+                error('Unimplemented');
+        end
+        
+    case 'boundary'
+        
+        [QuadRule] = getGaussLegendre(0, 1, id);
+        
+        % Quadrature nodes
+        points=QuadRule.x;
+        % Quadrature weights
+        weights=QuadRule.w;
+        
+    case 'interpolation'
+        
+        e=1e-6;
+        
+        switch id
+            case 3
+                points=[e e
+                        1-e e
+                        e 1-e];
+                
+            case 6
+                points=[e e
+                        1-e e
+                        e 1-e
+                        1/3 1/3
+                        2/3 1/3
+                        1/3 2/3];
+                
+        end
+        
+        weights=[];
+end
+
+
+    function [QuadRule] = getGaussLegendre(a,b,n)
+        
+        % getGaussLegendre: 1D n-point Gauss-Legendre quadrature rule on the interval [a,b].
+        % Quadrature rules obtained from Gauss-Legendre are of order 2*n-1.
+        % INPUT:
+        % a,b:          End-points of the interval
+        % n:            Number of quadrature nodes in [a,b]
+        % OUTPUT:
+        % QuadRule:     Structure which contains the fields:
+        %    x:         N-by-1 matrix specifying the abscissae of the quadrature rule.
+        %    w:         N-by-1 matrix specifying the weights of the quadrature rule.
+        
+        if (n==1), x = 0; w = 2;
+        else
+            c = zeros(n-1,1);
+            for i=1:(n-1), c(i)=i/sqrt(4*i*i-1); end
+            J=diag(c,-1)+diag(c,1); [ev,ew]=eig(J);
+            x=diag(ew); w=(2*(ev(1,:).*ev(1,:)))';
+        end
+        
+        % The above rule computes a quadrature rule for the reference interval.
+        % The reference interval is always [-1,1], and if [a,b] != [-1,1] then we
+        % re-map the abscissas (x) and rescale the weights.
+        
+        % Midpoint
+        xm = (b+a)/2;
+        % Area of the requested interval / area of the reference interval
+        xl = (b-a)/2;
+        
+        x = xm + xl*x;
+        w = w * xl;
+        
+        QuadRule.w = w(:);
+        QuadRule.x = x(:);
+    end
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_large/getRadius.m b/Code/INTERNODES_large/getRadius.m
new file mode 100644
index 0000000..242ea5b
--- /dev/null
+++ b/Code/INTERNODES_large/getRadius.m
@@ -0,0 +1,101 @@
+function[r, nnzRMM, nnzCMM, nnzRNM, nnzCNM]=getRadius(Data, nodesM, nodesN, coordM, coordN)
+
+% getRadius: Computes the radiuses of the radial basis functions defined on 
+% the interfaces.
+% INPUT:
+% Data:         Structure containing all the data parameters
+% nodesM:       Set of interpolation nodes
+% nodesN:       Set of evaluation nodes
+% coordM:       Coordinate matrix for the interpolation nodes
+% coordN:       Coordinate matrix for the evaluation nodes
+% OUTPUT:
+% r:            Radiuses of radial basis functions
+% nnzRMM:       Number of off-diagonal nonzeros in each row of PhiMM
+% nnzCMM:       Number of off-diagonal nonzeros in each column of PhiMM
+% nnzRNM:       Number of nonzeros in each row of PhiNM
+% nnzCNM:       Number of nonzeros in each column of PhiNM
+
+M=length(nodesM);
+N=length(nodesN);
+r=zeros(M,1);
+
+% Parameters
+% gamma parameter: real number between ]0,1[
+% Large values of gamma result in small supports
+gamma=Data.Contact.gamma;
+% Gamma parameter: real number between ]0,1[ with Gamma>gamma
+% Used to ensure evaluation points are well within the
+% support of radial basis functions (far enough from the boundary)
+Gamma=Data.Contact.Gamma;
+% Tolerance for contact detection
+% If the tolerance is unknown, set it to some small value.
+d0=Data.Contact.d0;
+% Number of closest neighbors on current interface
+c=Data.Contact.c;
+
+% Number of off-diagonal nonzeros per row of PhiMM
+nnzRMM=zeros(M,1);
+% Number of off-diagonal nonzeros per column of PhiMM
+nnzCMM=zeros(M,1);
+% Number of nonzeros per row of PhiNM
+nnzRNM=zeros(N,1);
+% Number of nonzeros per column of PhiNM
+nnzCNM=zeros(M,1);
+
+maxS=inf;
+f=0;
+niter=0;
+maxiter=10;
+
+% Version 2
+while maxS>f && niter<maxiter
+    % Maximum number of supports to which an evaluation point may belong.
+    % Condition for the matrix PhiMM to be diagonally dominant.
+    f=floor(1./((1-gamma).^4.*(1+4*gamma)));
+    
+    for k=1:M
+        point=coordM(k,:);
+        neighbors=coordM;
+        neighbors(k,:)=inf;
+        distMM=vecnorm(neighbors-point,2,2);
+        distMN=vecnorm(coordN-point,2,2);
+        
+        rMM=mink(distMM,c);
+        rNM=sqrt(d0^2+0.25*rMM(end)^2);
+        radius=max([rMM(end) rNM]);
+        
+        % Make sure the closest neighbor is at a distance >= gamma*radius.
+        % If the point is too close (with respect to the radius), diagonal
+        % dominance will be lost very quickly.
+        if radius>rMM(1)/gamma
+            radius=rMM(1)/gamma;
+        end
+        
+        s1=distMM<radius;
+        s2=distMN<Gamma*radius;
+        
+        nnzRMM(s1)=nnzRMM(s1)+1;
+        nnzRNM(s2)=nnzRNM(s2)+1;
+        nnzCMM(k)=sum(s1);
+        nnzCNM(k)=sum(s2);
+        
+        r(k)=radius;
+    end
+    % Compute the maximum number of supports to which a point belongs
+    maxS=max(nnzRMM);
+    if maxS>f
+        % Increase gamma (sequence converges to 1)
+        gamma=0.5*(1+gamma);
+        % Reinitialize counters
+        nnzRMM=zeros(M,1);
+        nnzCMM=zeros(M,1);
+        nnzRNM=zeros(N,1);
+        nnzCNM=zeros(M,1);
+        niter=niter+1;
+    end
+end
+
+if niter==maxiter && maxS>f
+    warning('The maximum number of rounds for radius computations was reached. Try decreasing d0')
+end
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_large/gmres_k.m b/Code/INTERNODES_large/gmres_k.m
new file mode 100644
index 0000000..a57c826
--- /dev/null
+++ b/Code/INTERNODES_large/gmres_k.m
@@ -0,0 +1,123 @@
+function[x, residual, res_in] = gmres_k(Mesh, Data, linearOp, b, x0, level)
+
+% gmres_k: Restarted GMRES algorithm
+% INPUT:
+% Mesh:             Structure containing all the mesh parameters
+% Data:             Structure containing all the data parameters
+% linearOp:         Linear Operator implementing matrix-vector multiplication 
+%                   and resolution of linear systems with the preconditioner
+% b:                Right-hand side
+% x0:               Starting vector
+% level:            Iteration level
+% OUTPUT:
+% x:                Approximate solution
+% residual:         Vector containing the norms of the residuals on the 
+%                   outer iterations
+% res_inner:        Cell array containing the norms of the residuals on the 
+%                   inner iterations
+
+% Maximum total number of iterations
+maxiter=Data.Solver.maxiter;
+% Number of iterations before restart
+riter=Data.Solver.riter;
+% Reorthogonalization tolerance
+reorth_tol=Data.Solver.reorth_tol;
+% Tolerance on the norm of the residual
+tol=Data.Solver.tol;
+
+if strcmp(level, 'level 2')
+    tol=1e-11;
+    riter=100;
+end
+
+% Initialization
+x=x0;
+res_error=1;
+budget=maxiter;
+residual=[];
+
+while res_error>tol && budget>0
+    [x, res1, ~, iter] = gmresPrec(b, x, min([riter, budget]), reorth_tol, tol);
+    residual=[residual; res1];
+    res_error=residual(end);
+    budget=budget-iter;
+end
+
+if res_error>tol && budget==0
+    disp(['The restarted GMRES method on ' level ' did not converge after ' num2str(maxiter-budget) ' iterations'])
+end
+
+
+
+    function [x, varargout] = gmresPrec(b, x0, riter, reorth_tol, tol)
+        
+        % gmresPrec: Right preconditioned GMRES
+        % INPUT:
+        % b:            Right-hand side
+        % x0:           Starting vector
+        % riter:        Maximum number of iterations before restart
+        % reorth_tol:   Reorthogonalization tolerance
+        % tol:          Tolerance on the norm of the residual
+        % OUTPUT:
+        % x:            Approximate solution
+        % res:          Vector containing the norms of the residuals on the 
+        %               outer iterations
+        % res_in:       Cell array containing the norms of the residuals on the 
+        %               inner iterations
+        % U:            Orthonormal Krylov basis
+        % H:            Upper Hessenberg matrix
+        % k:            Number of iterations
+        
+        r0=b-linearOp(Mesh, Data, x0, 'matvec');
+        U=zeros(length(r0), riter);
+        H=zeros(riter+1, riter);
+        res=zeros(riter+1,1);
+        res(1)=norm(r0);
+        U(:,1)=r0/res(1);
+        
+        
+        for k=1:riter
+            [w, res_in{k}]=linearOp(Mesh, Data, U(:,k), 'solve');
+            h=U(:,1:k)'*w;
+            u_tilde=w-U(:,1:k)*h;
+            
+            if norm(u_tilde)<reorth_tol*norm(w)
+                h_hat=U(:,1:k)'*u_tilde;
+                h=h+h_hat;
+                u_tilde=u_tilde-U(:,1:k)*h_hat;
+            end
+            
+            h_tilde=norm(u_tilde);
+            U(:,k+1)=u_tilde/h_tilde;
+            H(1:(k+1) ,k)=[h; h_tilde];
+            
+            % Be careful: parentheses are crucial
+            y=res(1)*(H(1:k+1,1:k)\eye(k+1,1));
+            res(k+1)=norm(res(1)*eye(k+1,1)-H(1:k+1,1:k)*y);
+            
+            if res(k+1)<tol
+                [v, res_in{k+1}]=linearOp(Mesh, Data, U(:,1:k)*y, 'solvePrec');
+                x=x0+v;
+                flag=1;
+                break
+            else
+                flag=0;
+            end
+            
+        end
+        
+        
+        if ~flag
+            [v, res_in{k+1}]=linearOp(Mesh, Data, U(:,1:k)*y, 'solvePrec');
+            x=x0+v;
+        end
+        
+        
+        varargout{1}=res(1:k+1);
+        varargout{2}=res_in;
+        varargout{3}=k;
+        varargout{4}=U(:,1:k);
+        varargout{5}=H(1:k,1:k);
+    end
+
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_large/initializeInterface.m b/Code/INTERNODES_large/initializeInterface.m
new file mode 100644
index 0000000..9b4e1bf
--- /dev/null
+++ b/Code/INTERNODES_large/initializeInterface.m
@@ -0,0 +1,107 @@
+function[Data]=initializeInterface(Data, Mesh, it)
+
+% initializeInterface: Initializes the interfaces properties
+% INPUT:
+% Mesh:     Structure containing all the mesh parameters
+% Data:     Structure containing all the data parameters
+% it:       Boundary tags for either a portion of the boundary or the entire boundary
+%           The first component is the tag for body 1
+%           The second component is the tag for body 2
+% OUTPUT:
+% Data:     Updated data structure with interface properties
+
+% Retrieve mesh parameters
+% Retrieve boundary tag
+BC_tag=Mesh.BC_tag;
+% Retrieve coordinate matrix
+coord=Mesh.coord_mat;
+% Define radius of intersection
+if isfield(Data.Contact, 'radius')
+    r=Data.Contact.radius;
+else
+    r=0.052;
+end
+
+% Boundary elements
+elements1=Mesh.BC_nodes(ismember(BC_tag,it{1}),:);
+elements2=Mesh.BC_nodes(ismember(BC_tag,it{2}),:);
+
+% Boundary nodes
+bc_nodes1=unique(elements1);
+bc_nodes2=unique(elements2);
+
+m1=length(bc_nodes1);
+m2=length(bc_nodes2);
+
+% Compute element lengths
+l1=vecnorm(coord(elements1(:,2),:)-coord(elements1(:,1),:),2,2);
+l2=vecnorm(coord(elements2(:,2),:)-coord(elements2(:,1),:),2,2);
+
+% Compute mesh sizes
+h1=min(l1);
+h2=min(l2);
+
+body_1.h=h1;
+body_2.h=h2;
+
+h=min([h1, h2]);
+Data.Contact.h=h;
+
+% Indicators
+ic1=zeros(m1,1);
+ic2=zeros(m2,1);
+
+% Create interfaces based on closest nodes from opposing interface
+for k=1:m1
+    node=bc_nodes1(k);
+    coord_node=coord(node,:);
+    dist=vecnorm(coord(bc_nodes2,:)-coord_node,2,2);
+    ic1(k)=any(dist<r);
+    
+end
+for k=1:m2
+    node=bc_nodes2(k);
+    coord_node=coord(node,:);
+    dist=vecnorm(coord(bc_nodes1,:)-coord_node,2,2);
+    ic2(k)=any(dist<r);
+end
+
+ic1=logical(ic1);
+ic2=logical(ic2);
+
+interface1=bc_nodes1(ic1);
+interface2=bc_nodes2(ic2);
+
+% Correct interface nodes to account for finite elements
+index1=all(ismember(Mesh.BC_nodes, interface1), 2);
+index2=all(ismember(Mesh.BC_nodes, interface2), 2);
+% Get interface connectivity matrices
+body_1.interface_connect=Mesh.BC_nodes(index1,:);
+body_2.interface_connect=Mesh.BC_nodes(index2,:);
+% Get interface nodes
+body_1.interface_nodes=unique(body_1.interface_connect);
+body_2.interface_nodes=unique(body_2.interface_connect);
+% Get interface degrees of freedom
+interface_dof_1=Mesh.Dof_set(:,body_1.interface_nodes);
+interface_dof_2=Mesh.Dof_set(:,body_2.interface_nodes);
+body_1.interface_dof=interface_dof_1(:);
+body_2.interface_dof=interface_dof_2(:);
+% Initial active set
+body_1.initial_nodes=body_1.interface_nodes;
+body_2.initial_nodes=body_2.interface_nodes;
+% Initial connectivity matrix
+body_1.initial_interface_connect=body_1.interface_connect;
+body_2.initial_interface_connect=body_2.interface_connect;
+% Active set
+body_1.active_set=ismember(body_1.initial_nodes, body_1.interface_nodes);
+body_2.active_set=ismember(body_2.initial_nodes, body_2.interface_nodes);
+
+% Initialization
+body=cell(2,1);
+% Create body entity
+body{1}=body_1;
+body{2}=body_2;
+Data.body=body;
+% Map global dof to local ones
+[Data]=mapGlobal2Local(Data);
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_large/initializeSystem.m b/Code/INTERNODES_large/initializeSystem.m
new file mode 100644
index 0000000..02615bd
--- /dev/null
+++ b/Code/INTERNODES_large/initializeSystem.m
@@ -0,0 +1,92 @@
+function[Data, Solution]=initializeSystem(Mesh, Data, it)
+
+% initializeSystem: Computes the sub-matrices of the system
+% INPUT:
+% Mesh:     Structure containing all the mesh parameters
+% Data:     Structure containing all the data parameters
+% Param:    Structure containing the optional parameters
+% bt:       Array containing the tags of the bodies
+% it:       Array containing the boundary tags of the interfaces
+% OUTPUT:
+% Data:     Updated structure containing all the data parameters
+% Solution: Initialized solution structure
+
+% Compute the Dirichlet BC
+[Solution]=computeBC(Data, Mesh);
+% Define the interface
+[Data]=initializeInterface(Data, Mesh, it);
+% Assemblage of the stiffness matrix
+[K]=computeStiffness(Mesh, Data);
+% Pre-processing
+[Data]=preProcess(Data, Solution, K);
+
+% Sub-assemblage
+[Data]=subAssemble(Mesh, Data, Solution, 'initialization');
+% Compute right-hand side vector
+[Data]=updateRHS(Mesh, Data);
+
+
+    function[Solution]=computeBC(Data, Mesh)
+        
+        % computeBC: Function which computes the displacement values at
+        % Dirichlet BC nodes
+        % INPUT:
+        % Data:     Structure containing all the data parameters
+        % Mesh:     Structure containing all the mesh parameters
+        % OUTPUT:
+        % Solution: Structure containing the solution, including displacements
+        %           evaluated at Dirichlet nodes
+        
+        % Retrieve mesh parameters
+        % Coordinate matrix
+        coord=Mesh.coord_mat;
+        % Matrix containing the sets of degrees of freedom for each node
+        Dof_set=Mesh.Dof_set;
+        % Total number of degrees of freedom
+        nb_dof=Mesh.nb_dof;
+        % Local number of degrees of freedom
+        nb_dof_local=Mesh.nb_dof_local;
+        
+        % Retrieve data
+        % Dirichlet boundary conditions
+        D_BC=Data.D_BC;
+        % Set of nodes associated to Dirichlet BC
+        set_D_nodes=Data.set_D_nodes;
+        % Number of boundaries with Dirichlet BC
+        n=length(D_BC);
+        
+        % Retrieve solutions
+        U=zeros(nb_dof,1);
+        
+        for i=1:n
+            data=D_BC{i};
+            % Retrieve function
+            g=data.function;
+            % Nodes making up the edges
+            nodes=set_D_nodes{i};
+            % Number of nodes on the edges
+            nb_nodes=length(nodes);
+            % Degrees of freedom
+            dofs=Dof_set(:, nodes);
+            % Coordinates
+            X=reshape(coord(nodes,:), nb_nodes, 1, nb_dof_local);
+            % Evaluation
+            G=g(X);
+            
+            if size(G,1)>nb_dof_local
+                G=reshape(G, nb_nodes, nb_dof_local)';
+                G=G(:);
+            else
+                G=repmat(G, nb_nodes, 1);
+            end
+            U(dofs(:))=G;
+            
+        end
+        
+        % Update solution
+        Solution.U=U;
+        % Reshape to an array of the same size as the coordinate matrix
+        Solution.U_sort=reshape(U, nb_dof_local, [])';
+    end
+
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_large/internodes1.m b/Code/INTERNODES_large/internodes1.m
new file mode 100644
index 0000000..edd86d4
--- /dev/null
+++ b/Code/INTERNODES_large/internodes1.m
@@ -0,0 +1,65 @@
+% INTERNODES method for contact mechanics
+% Case study: Hertzian contact problem with Dirichlet boundary conditions
+% applied to both bodies.
+
+% Limitations: 2D
+clc
+clear variables
+close all
+
+% Read GMSH mesh file (.msh)
+[Mesh] = readGMSH('../../GMSH/Meshes/contact2_p1_0_01.msh');
+
+%% Parameter prescription
+% Mesh parameters
+[Mesh]=getParameters(Mesh);
+% Initialize model
+[Data]=setModel('model', 'static_solid', 'submodel', 'contact', 'type', 'linear');
+
+%% Set material parameters
+% Elastic modulus [N\m^2]
+E=30e9;
+% Poisson ratio [-]
+nu=0.2;
+% Specific mass [kg/m^3]
+rho=2500;
+% Thickness [m]
+t=1;
+
+% Plane stress conditions
+lambda=E*nu/(1-nu^2);
+mu=E/(2*(1+nu));
+
+[Data]=setCoefficient(Data, 'rho', t*rho);
+[Data]=setCoefficient(Data, 'lambda', t*lambda);
+[Data]=setCoefficient(Data, 'mu', t*mu);
+[Data]=setCoefficient(Data, 'f', @(x) [0; 0]);
+
+%% Set boundary conditions
+
+% Used for testing with invertible stiffness matrix
+BC={{'Type', 'Dirichlet', 'Edges', 3, 'Function', @(x) [0; 0]},...
+    {'Type', 'Dirichlet', 'Edges', 10, 'Function', @(x) [0; -0.1]}};
+
+p2={'BC', BC};    
+
+[Data]=setBoundaryConditions(Mesh, Data, p2);
+%% Set numerical parameters
+% For more information, type 'help setNumericalParameters'
+
+[Data]=setNumericalParameters(Mesh, Data, 'Contact', 'rescaling', E, 'radius', 0.2);
+[Data]=setNumericalParameters(Mesh, Data, 'Solver', 'name', 'GMRES', 'riter', 100);
+[Data]=setNumericalParameters(Mesh, Data, 'PostProcessing', 'ampl_fact', 1, 'quantity', {'stress', 'strain'});
+
+%% Assemblage of the right-hand side
+[Data]=computeRHS(Mesh, Data);
+
+%% Solve the contact problem
+[Mesh, Data, Solution]=solve(Mesh, Data, {3:6, 7:10});
+
+%% Computation of the stresses and strains
+[Mesh, Solution]=postProcess(Mesh, Data, Solution);
+
+%% Visualization of the solution
+% For more information, type 'help plotSolution'
+plotSolution(Mesh, Data, Solution, 'disp', 'standard', 'stress', 'sigma_yy');
\ No newline at end of file
diff --git a/Code/INTERNODES_large/internodes2.m b/Code/INTERNODES_large/internodes2.m
new file mode 100644
index 0000000..716d741
--- /dev/null
+++ b/Code/INTERNODES_large/internodes2.m
@@ -0,0 +1,66 @@
+% INTERNODES method for contact mechanics
+% Case study: Hertzian contact problem with Dirichlet boundary conditions
+% applied to the lower body and Neumann boundary conditions applied to the
+% upper body.
+
+% Limitations: 2D
+clc
+clear variables
+close all
+
+% Read GMSH mesh file (.msh)
+[Mesh] = readGMSH('../../GMSH/Meshes/contact2_p1_0_01.msh');
+
+%% Parameter prescription
+% Mesh parameters
+[Mesh]=getParameters(Mesh);
+% Initialize model
+[Data]=setModel('model', 'static_solid', 'type', 'linear');
+
+%% Set material parameters
+% Elastic modulus [N\m^2]
+E=30e9;
+% Poisson ratio [-]
+nu=0.2;
+% Specific mass [kg/m^3]
+rho=2500;
+% Thickness [m]
+t=1;
+
+% Plane stress conditions
+lambda=E*nu/(1-nu^2);
+mu=E/(2*(1+nu));
+
+[Data]=setCoefficient(Data, 'rho', t*rho);
+[Data]=setCoefficient(Data, 'lambda', t*lambda);
+[Data]=setCoefficient(Data, 'mu', t*mu);
+[Data]=setCoefficient(Data, 'f', @(x) [0; 0]);
+
+%% Set boundary conditions
+
+% Used for testing with singular stiffness matrix
+BC={{'Type', 'Dirichlet', 'Edges', 3, 'Function', @(x) [0; 0]},...
+    {'Type', 'Neumann', 'Edges', 10, 'Function', @(x) [0; -1e9]}};
+
+p2={'BC', BC};    
+
+[Data]=setBoundaryConditions(Mesh, Data, p2);
+
+%% Set numerical parameters
+% For more information, type 'help setNumericalParameters'
+
+[Data]=setNumericalParameters(Mesh, Data, 'Contact', 'rescaling', E, 'maxiter', 30, 'radius', inf, 'tol', 0.9);
+[Data]=setNumericalParameters(Mesh, Data, 'PostProcessing', 'ampl_fact', 1, 'quantity', {'stress', 'strain'});
+
+%% Assemblage of the right-hand side
+[Data]=computeRHS(Mesh, Data);
+
+%% Solve the contact problem
+[Mesh, Data, Solution]=solve(Mesh, Data, {3:6, 7:10});
+
+%% Computation of the stresses and strains
+[Mesh, Solution]=postProcess(Mesh, Data, Solution);
+
+%% Visualization of the solution
+% For more information, type 'help plotSolution'
+plotSolution(Mesh, Data, Solution, 'disp', 'standard', 'stress', 'sigma_yy');
\ No newline at end of file
diff --git a/Code/INTERNODES_large/kr.m b/Code/INTERNODES_large/kr.m
new file mode 100644
index 0000000..874aea5
--- /dev/null
+++ b/Code/INTERNODES_large/kr.m
@@ -0,0 +1,31 @@
+function X = kr(U,varargin)
+%KR Khatri-Rao product.
+%   kr(A,B) returns the Khatri-Rao product of two matrices A and B, of 
+%   dimensions I-by-K and J-by-K respectively. The result is an I*J-by-K
+%   matrix formed by the matching columnwise Kronecker products, i.e.
+%   the k-th column of the Khatri-Rao product is defined as
+%   kron(A(:,k),B(:,k)).
+%
+%   kr(A,B,C,...) and kr({A B C ...}) compute a string of Khatri-Rao 
+%   products A o B o C o ..., where o denotes the Khatri-Rao product.
+%
+%   See also kron.
+
+%   Version: 21/10/10
+%   Authors: Laurent Sorber (Laurent.Sorber@cs.kuleuven.be)
+
+if ~iscell(U), U = [U varargin]; end
+K = size(U{1},2);
+if any(cellfun('size',U,2)-K)
+    error('kr:ColumnMismatch', ...
+          'Input matrices must have the same number of columns.');
+end
+J = size(U{end},1);
+X = reshape(U{end},[J 1 K]);
+for n = length(U)-1:-1:1
+    I = size(U{n},1);
+    A = reshape(U{n},[1 I K]);
+    X = reshape(bsxfun(@times,A,X),[I*J 1 K]);
+    J = I*J;
+end
+X = reshape(X,[size(X,1) K]);
diff --git a/Code/INTERNODES_large/linearOp1.m b/Code/INTERNODES_large/linearOp1.m
new file mode 100644
index 0000000..513ab8d
--- /dev/null
+++ b/Code/INTERNODES_large/linearOp1.m
@@ -0,0 +1,164 @@
+function[y, varargout]=linearOp1(Mesh, Data, x, type)
+
+% linearOp1: Wrap around function for operations related to the application
+% of linear operators in the outer iterations
+% INPUT:
+% Mesh:     Structure containing all the mesh parameters
+% Data:     Structure containing all the data parameters
+% x:        Vector
+% type:     Type of operation to perform
+% OUTPUT:
+% y:        Result of the operation
+% res:      Optional residual for the solution of a linear system
+
+switch type
+    case 'solve'
+        [y, res, Data]=solvePrec(Mesh, Data, x);
+        y=multiplyMatVec(Mesh, Data, y);
+        varargout{1}=res;
+        varargout{2}=Data;
+        
+    case 'solvePrec'
+        [y, res, Data]=solvePrec(Mesh, Data, x);
+        varargout{1}=res;
+        varargout{2}=Data;
+        
+    case 'matvec'
+        y=multiplyMatVec(Mesh, Data, x);
+end
+
+
+    function[y]=multiplyMatVec(Mesh, Data, x)
+        
+        % multiplyMatVec: Implements the mutiplication between the 
+        % INTERNODES matrix A and a vector x and stores the result in y
+        % INPUT:
+        % Mesh:     Structure containing all the mesh parameters
+        % Data:     Structure containing all the data parameters
+        % x:        Vector
+        % OUTPUT:
+        % y:        Result of the multiplication of A with x
+
+        % Retrieve data parameters
+        % Number of free degrees of freedom
+        nf=Data.nf;
+        % Number of constraints
+        nc=Data.nc;
+        
+        % Initialization
+        y=zeros(nf+nc,1);
+
+        y(1:nf)=Data.K*x(1:nf)+multiplyB(Mesh, Data, x(nf+1:nf+nc), 'vec');
+        y(nf+1:nf+nc)=multiplyB_tilde(Mesh, Data, x(1:nf), 'vec');
+    end
+
+    function[y, res, Data]=solvePrec(Mesh, Data, x)
+        
+        % solvePrec: Implements the resolution of a linear system with the 
+        % Liu preconditioner and a vector x and stores the result in y
+        % INPUT:
+        % Mesh:     Structure containing all the mesh parameters
+        % Data:     Structure containing all the data parameters
+        % x:        Vector
+        % OUTPUT:
+        % y:        Solution of the linear system
+        % res:      Vector containing the norms of the residuals on the inner iterations
+        
+        
+        % Retrive mesh parameters
+        d=Mesh.nb_dof_local;
+        % Parameter k
+        k=2;
+        
+        % Retrieve data parameters
+        % Cholesky factor of the reordered perturbed stiffness matrix
+        Lr=Data.Lr;
+        L22=Data.L22;
+%         L=Data.L;
+        % Ordering computed with Nested Dissection
+%         r=Data.p;
+        % Number of free degrees of freedom
+        nf=Data.nf;
+        % Number of constraints
+        nc=Data.nc;
+        % Small matrix G
+%         G=Data.G;
+%         V=Data.V;
+        % Perturbed reordering of Nested Dissection
+        or=Data.or;
+        % Size of block 1
+        b1=Data.b1;
+        
+        % Initialization
+        y=zeros(nf+nc,1);
+        w1=zeros(nf,1);
+        
+        p1=x(1:nf);
+        p2=x(nf+1:nf+nc);
+        
+        w2=reshape(p2, d, []);
+        w2=-Data.Contact.alpha*(w2/Data.body{1}.interface_mass);
+        
+        p1=p1-k*multiplyB(Mesh, Data, w2, 'mat');   
+        
+        % Method without L22
+%         p1=p1(or);
+%         v1=p1(1:b1);
+%         v2=p1(b1+1:end);
+%         v1=L11\v1;
+%         v2=-L21*v1+v2;
+%         v2=V\v2;
+%         v1=v1-L21'*v2;
+%         v1=L11'\v1;
+%         w1(or)=[v1; v2];
+        
+         % Method with L22
+%         p2=Lr\p1(or); 
+%         p2(b1+1:end)=L22'*(V\(L22*p2(b1+1:end)));
+%         w1(or)=Lr'\p2;
+        
+%         % Method for small problems
+        Lv=Data.Lv;
+        Uv=Data.Uv;
+
+        p2=Lr\p1(or); 
+        p3=p2(b1+1:end);
+        p3=L22*p3;
+        x0=zeros(length(p3),1);
+        [p3, res] = gmres_k(Mesh, Data, @linearOp2, p3, x0, 'level 2');
+%         p3(Data.orc)=Uv\(Lv\p3(Data.orl));
+%         res=0;
+        p2(b1+1:end)=L22'*p3;
+        w1(or)=Lr'\p2;
+
+% %         % Method for small problems
+%         L0=Data.L0;
+%         U0=Data.U0;
+%         Nk=Data.Nk;
+%         
+%         p2(Data.orl)=U0\(L0\p1(Data.orc)); 
+%         w1=Nk*p2;
+%         res=0;
+
+        % Test case
+%         L22=Data.L22;
+%         p2=Lr\p1(or); 
+%         p3=L22*p2(b1+1:end);
+%         [p3, res]=gmresRestart(Data.V, p3, zeros(length(p3),1), Data, Data.V_hat);
+%         p2(b1+1:end)=L22'*p3;
+%         w1(or)=Lr'\p2;
+        
+%         p1(r)=L'\(L\p1(r));
+%         p2=multiplyB_tilde(Mesh, Data, p1, 'vec');
+%         p2=Data.S\p2;
+%         p2=multiplyB(Mesh, Data, p2, 'vec');
+%         p2(r)=L'\(L\p2(r));
+%         w1=p1-p2;
+%         res=0;
+
+%         res=0;
+        y(1:nf)=w1;
+        y(nf+1:nf+nc)=w2;
+    end
+
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_large/linearOp2.m b/Code/INTERNODES_large/linearOp2.m
new file mode 100644
index 0000000..3e29d89
--- /dev/null
+++ b/Code/INTERNODES_large/linearOp2.m
@@ -0,0 +1,68 @@
+function[y, varargout]=linearOp2(Mesh, Data, x, type)
+
+% linearOp2: Wrap around function for operations related to the application
+% of linear operators in the inner iterations
+% INPUT:
+% Mesh:     Structure containing all the mesh parameters
+% Data:     Structure containing all the data parameters
+% x:        Vector
+% type:     Type of operation to perform
+% OUTPUT:
+% y:        Result of the operation
+% res:      Optional residual for the solution of a linear system
+
+switch type
+    case 'solve'         
+        [y, res]=solvePrec(Data, x);
+         y=multiplyMatVec(Mesh, Data, y);
+        varargout{1}=res;
+        
+    case 'solvePrec'
+        [y, res]=solvePrec(Data, x);
+        varargout{1}=res;
+        
+    case 'matvec'
+        y=multiplyMatVec(Mesh, Data, x);
+end
+
+    function[y]=multiplyMatVec(Mesh, Data, x)
+        
+        % multiplyMatVec: Implements the matrix-vector multiplication for
+        % the inner iterations
+        % INPUT:
+        % Mesh:     Structure containing all the mesh parameters
+        % Data:     Structure containing all the data parameters
+        % x:        Vector
+        % OUTPUT:
+        % y:        Result of the matrix-vector multiplication
+        
+        L22=Data.L22;
+        y=multiplyY2T(Mesh, Data, x);
+        y=multiplyY1(Mesh, Data, y);
+        y=L22*(L22'*x)-Data.Contact.alpha*y;
+    end
+
+    function[y, res]=solvePrec(Data, x)
+        
+        % solvePrec: Implements the resolution of a linear system with the 
+        % preconditioner and a vector x for the inner iterations
+        % Mesh:     Structure containing all the mesh parameters
+        % Data:     Structure containing all the data parameters
+        % x:        Vector
+        % OUTPUT:
+        % y:        Solution of the linear system
+        % res:      Norm of the residuals for the inner iterations 
+        
+        
+        % Retrieve data parameters
+          Lv=Data.Lv;
+          Uv=Data.Uv;
+          y=zeros(Data.b2,1);
+          
+          y(Data.orc)=Uv\(Lv\x(Data.orl));
+          res=0;
+    end
+
+
+
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_large/mapGlobal2Local.m b/Code/INTERNODES_large/mapGlobal2Local.m
new file mode 100644
index 0000000..6b14a3a
--- /dev/null
+++ b/Code/INTERNODES_large/mapGlobal2Local.m
@@ -0,0 +1,13 @@
+function[Data]=mapGlobal2Local(Data)
+
+% mapGlobal2Local: Function which maps the global degrees of freedom to the
+% local ones for the INTERNODES matrix
+% INPUT:
+% Data:     Structure containing all the data parameters
+% OUTPUT:
+% Data:     Updated structure with the local degrees of freedom
+
+Nf=Data.Nf;
+[~, Data.body{1}.indx_local, ~]=intersect(Nf, Data.body{1}.interface_dof);
+[~, Data.body{2}.indx_local, ~]=intersect(Nf, Data.body{2}.interface_dof);
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_large/multiplyB.m b/Code/INTERNODES_large/multiplyB.m
new file mode 100644
index 0000000..d1e08b7
--- /dev/null
+++ b/Code/INTERNODES_large/multiplyB.m
@@ -0,0 +1,25 @@
+function[y]=multiplyB(Mesh, Data, x, in)
+
+% multiplyB: Implements the mutiplication between B and a vector x and
+% stores the result in y
+% INPUT:
+% Mesh:     Structure containing all the mesh parameters
+% Data:     Structure containing all the data parameters
+% x:        Vector or matrix
+% in:       Parameter specifiying whether the input is a vector or a matrix
+% OUTPUT:
+% y:        Result of the multiplication of B with x
+
+if strcmp(in, 'vec')
+x=reshape(x, Mesh.nb_dof_local, []);
+end
+
+y=zeros(Data.nf,1);
+
+y1=-x*Data.body{1}.interface_mass;
+y2=multiplyR21(Data, x');
+y2=y2'*Data.body{2}.interface_mass;
+
+y(Data.body{1}.indx_local)=y1(:);
+y(Data.body{2}.indx_local)=y2(:);
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_large/multiplyB_tilde.m b/Code/INTERNODES_large/multiplyB_tilde.m
new file mode 100644
index 0000000..30b5678
--- /dev/null
+++ b/Code/INTERNODES_large/multiplyB_tilde.m
@@ -0,0 +1,27 @@
+function[y]=multiplyB_tilde(Mesh, Data, x, out)
+
+% multiplyB_tilde: Implements the mutiplication between B_tilde and a
+% vector x and stores the result in y
+% INPUT:
+% Mesh:     Structure containing all the mesh parameters
+% Data:     Structure containing all the data parameters
+% x:        Vector
+% out:      Parameter specifying whether the output should be a vector or
+%           a matrix
+% OUTPUT:
+% y:        Result of the multiplication of B_tilde with x
+
+% Retrive mesh parameters
+% Number of local degrees of freedom
+d=Mesh.nb_dof_local;
+
+w1=reshape(x(Data.body{1}.indx_local,:), d, []);
+w2=reshape(x(Data.body{2}.indx_local,:), d, []);
+w2=multiplyR12(Data, w2')';
+
+y=w1-w2;
+
+if strcmp(out, 'vec')
+    y=y(:);
+end
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_large/multiplyR12.m b/Code/INTERNODES_large/multiplyR12.m
new file mode 100644
index 0000000..e4d873c
--- /dev/null
+++ b/Code/INTERNODES_large/multiplyR12.m
@@ -0,0 +1,14 @@
+function[x]=multiplyR12(Data, x)
+
+% multiplyR12: Implements the multiplication between the interpolation matrix 
+% R12 and a vector x. The result is overwritten in x
+% INPUT:
+% Data: Structure containing all the data parameters
+% x:    Input matrix or vector
+% OUTPUT:
+% x:    Result of R12*x
+
+x(Data.orc2,:)=Data.U2\(Data.L2\x(Data.orl2,:));
+x=Data.Phi12*x;
+x=x./Data.s1;
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_large/multiplyR21.m b/Code/INTERNODES_large/multiplyR21.m
new file mode 100644
index 0000000..19c8d88
--- /dev/null
+++ b/Code/INTERNODES_large/multiplyR21.m
@@ -0,0 +1,14 @@
+function[x]=multiplyR21(Data, x)
+
+% multiplyR21: Implements the multiplication between the interpolation matrix 
+% R21 and a vector x. The result is overwritten in x
+% INPUT:
+% Data: Structure containing all the data parameters
+% x:    Input matrix or vector
+% OUTPUT:
+% x:    Result of R21*x
+
+x(Data.orc1,:)=Data.U1\(Data.L1\x(Data.orl1,:));
+x=Data.Phi21*x;
+x=x./Data.s2;
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_large/multiplyY1.m b/Code/INTERNODES_large/multiplyY1.m
new file mode 100644
index 0000000..208b790
--- /dev/null
+++ b/Code/INTERNODES_large/multiplyY1.m
@@ -0,0 +1,22 @@
+function[y]=multiplyY1(Mesh, Data, x)
+
+% multiplyY1: Implements the multiplication between the matrix Y1 and a
+% vector x and stores the result in y
+% INPUT:
+% Mesh:     Structure containing all the mesh parameters
+% Data:     Structure containing all the data parameters
+% x:        Vector
+% OUTPUT:
+% y:        Result of the multiplication of Y1 with x
+
+index=Data.index;
+y=zeros(Data.b2,1);
+
+w=reshape(x, Mesh.nb_dof_local, []);
+
+w=w/Data.body{1}.interface_mass;
+w=multiplyR21(Data, w')';
+w=w*Data.body{2}.interface_mass;
+
+y(index)=[x; -w(:)];
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_large/multiplyY2T.m b/Code/INTERNODES_large/multiplyY2T.m
new file mode 100644
index 0000000..84a6db5
--- /dev/null
+++ b/Code/INTERNODES_large/multiplyY2T.m
@@ -0,0 +1,21 @@
+function[y]=multiplyY2T(Mesh, Data, x)
+
+% multiplyY1: Implements the multiplication between the transpose of Y2 and 
+% a vector x and stores the result in y
+% INPUT:
+% Mesh:     Structure containing all the mesh parameters
+% Data:     Structure containing all the data parameters
+% x:        Vector
+% OUTPUT:
+% y:        Result of the multiplication of the transpose of Y2 with x
+
+x=x(Data.index);
+
+x1=x(1:Data.l1);
+x2=x(Data.l1+1:end);
+
+x2=reshape(x2, Mesh.nb_dof_local, []);
+x2=multiplyR12(Data, x2')';
+
+y=x1-x2(:);
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_large/plotSolution.m b/Code/INTERNODES_large/plotSolution.m
new file mode 100644
index 0000000..e30d799
--- /dev/null
+++ b/Code/INTERNODES_large/plotSolution.m
@@ -0,0 +1,811 @@
+function[]=plotSolution(Mesh, Data, Solution, varargin)
+
+% plotSolution: Function which plots the computed quantities
+% INPUT:
+% Mesh:     Structure containing the mesh parameters
+% Data:     Structure containing the data parameters
+% Solution: Structure containing the solution computed
+% OPTIONAL INPUT
+% snapshot: selected time-step at which the solution should be viewed
+%   1 -> none (default)
+%   any time step in the range defined
+% view_video: view the video of the solution
+%   1 -> yes 
+%   0 -> no (default)
+% save_video: choose whether to save the video or not
+%   1 -> yes
+%   0 -> no (default)
+% plot_arg: choose which solution to visualize
+%   dynamic_solid or static_solid module
+%       disp (default)
+%       stress
+%       strain
+%   transient_heat module
+%       temp (default)
+%       flux
+% plot_type: decide which type of metric should be used for visualization
+%   displacement metric
+%       standard -> structure with amplified displacements (default)
+%       norm
+%       absx
+%       absy
+%   temperature metric
+%       standard (default)
+%   stress metric
+%       von_mises (default)
+%       sigma_xx
+%       sigma_yy
+%       sigma_xy
+%       sigma_xx_abs
+%       sigma_yy_abs
+%       sigma_xy_abs
+%       frobenius
+%   strain metric
+%       epsilon_xx (default)
+%       epsilon_yy
+%       epsilon_xy
+%       epsilon_xx_abs
+%       epsilon_yy_abs
+%       epsilon_xy_abs
+%       frobenius
+%   flux metric
+%       standard (default)
+
+
+%% Initialize and ckeck Parameters
+
+if mod(length(varargin), 2)~=0
+    error('Missing a name or value')
+end
+labels_in=varargin(1:2:end);
+values_in=varargin(2:2:end);
+
+% Specify number of sub-intervals in time        
+if contains(Data.Model.name, 'static')
+    static=true;
+    N=0;
+else
+    static=false;
+    N=Data.Discretization.N;
+end
+
+switch Data.Model.name
+    case {'static_solid', 'dynamic_solid'}
+        plot_argv={'disp', 'stress', 'strain'};
+        n=length(plot_argv);
+        plot_typev=cell(n,1);
+        plot_typev{1}={'standard', 'norm', 'absx', 'absy'};
+        plot_typev{2}={'von_mises', 'sigma_xx', 'sigma_yy', 'sigma_xy', 'sigma_xx_abs', 'sigma_yy_abs', 'sigma_xy_abs', 'frobenius'};
+        plot_typev{3}={'epsilon_xx', 'epsilon_yy', 'epsilon_xy', 'epsilon_xx_abs', 'epsilon_yy_abs', 'epsilon_xy_abs', 'frobenius'};
+        
+    case 'transient_heat'
+        plot_argv={'temp', 'flux'};
+        n=length(plot_argv);
+        plot_typev=cell(n,1);
+        plot_typev{1}={'standard'};
+        plot_typev{2}={'standard'};
+        
+end
+
+% Set all default parameters
+% Default visualization parameters
+Param.visua_param.snapshot=1;
+Param.visua_param.view_video=false;
+Param.visua_param.write_video=false;
+% Default plot argument
+Param.plot_param.arg=plot_argv(1);
+% Default plot type
+Param.plot_param.type=plot_typev{1}(1);
+
+plot_arg={};
+plot_type={};
+nb_plot=0;
+
+for k=1:length(labels_in)
+    arg=lower(labels_in{k});
+    val=values_in{k};
+    
+    switch arg
+        case 'snapshot'
+            
+            if static
+                warning([arg ' parameter will be ignored because the simulation is in statics'])
+            elseif val < 0 || val > Data.Discretization.N+1
+                error(['Time snapshot invalid: it must be an integer between ' num2str(0) ' and ' num2str(Data.Discretization.N+1)])
+            else
+                Param.visua_param.snapshot=val;
+            end
+            
+        case {'view_video', 'write_video'}
+            
+            if static
+                warning([arg ' parameter will be ignored because the simulation is in statics'])
+            else
+                if isa(val, 'double')
+                    
+                    if val<0 || val>1
+                        error([arg ' parameter must be a boolean'])
+                    else
+                        Param.visua_param.view_video=logical(val);
+                    end
+                    
+                elseif isa(val, 'logical')
+                    Param.visua_param.view_video=val;
+                else
+                    error([arg ' parameter must be a boolean'])
+                end
+            end
+
+        case plot_argv
+            
+            % Check if desired quantities have been computed
+            switch arg
+                case 'stress'
+                    if ~isfield(Solution, 'sigma')
+                        error('Stresses have not been computed')
+                    end
+                    
+                case 'strain'
+                    if ~isfield(Solution, 'epsilon')
+                        error('Strains have not been computed')
+                    end
+                    
+                case 'flux'
+                    if ~isfield(Solution, 'Q')
+                        error('Fluxes have not been computed')
+                    end    
+            end
+            
+            plot_arg{nb_plot+1}=arg;
+            index=strcmp(arg, plot_argv);
+
+            if any(ismember(plot_typev{index}, val))
+                plot_type{nb_plot+1}=val;
+            else
+                error('Plot type invalid: see available plot types')
+            end
+                     
+            nb_plot=nb_plot+1;
+        otherwise
+            error('Unrecognized argument')
+    end
+    
+end
+
+if nb_plot>0
+    Param.plot_param.arg=plot_arg;
+    Param.plot_param.type=plot_type;
+end
+
+%% Plotting interface
+
+plot_arg=Param.plot_param.arg;
+plot_type=Param.plot_param.type;
+n=length(plot_arg);
+Plot=cell(n,1);
+Axis=cell(n,1);
+Colorbar=cell(n,1);
+Dynamic=cell(n,1);
+
+fig=figure;
+% fig.Units='normalized';
+% fig.Position=[0 0 1 1];
+
+% fig.Units='normalized';
+% fig.Position=[0 1 1/2 1/2];
+
+for k=1:n
+    switch plot_arg{k}
+        case {'disp', 'temp'}
+            [PlotParam, AxisParam, ColorbarParam, DynamicParam]=plotPatch(Mesh, Data, Solution, plot_type{k});
+            subplot(1,n,k)
+            axis=gca;
+            p=patch(axis);
+            c=colorbar(axis);
+            [p]=    setObject(p, PlotParam);
+            [axis]= setObject(axis, AxisParam);
+            [c]=    setObject(c, ColorbarParam);
+            
+            
+            
+        case {'strain', 'stress'}
+            [PlotParam, AxisParam, ColorbarParam, DynamicParam]=plotSurf(Mesh, Data, Solution, plot_arg{k}, plot_type{k});
+            subplot(1,n,k)
+            axis=gca;
+            p=patch(axis);
+            c=colorbar(axis);
+            [p]=    setObject(p, PlotParam);
+            [axis]= setObject(axis, AxisParam);
+            [c]=    setObject(c, ColorbarParam);
+            
+            
+            
+        case 'flux'
+            [PlotParam, AxisParam, ColorbarParam, DynamicParam]=plotQuiver(Mesh, Solution, plot_type{k});
+            subplot(1,n,k)
+            axis=gca;
+            p=quiver(axis, PlotParam{1,2}, PlotParam{2,2});
+            c=colorbar(axis);
+            [p]=    setObject(p, PlotParam);
+            [axis]= setObject(axis, AxisParam);
+            [c]=    setObject(c, ColorbarParam);
+            
+    end
+    
+    Plot{k}=p;
+    Axis{k}=axis;
+    Colorbar{k}=c;
+    Dynamic{k}=DynamicParam;
+end
+
+if Param.visua_param.view_video % View the solution over the entire simulation
+    
+    for k=1:n
+        [Plot{k}, Axis{k}]=updateObject(Plot{k}, Axis{k}, Dynamic{k}, 1, N);
+    end
+    
+    frames(1)=getframe(fig);
+    for step = 2:N+1
+        pause(0.05)
+        for k=1:n
+            [Plot{k}, Axis{k}]=updateObject(Plot{k}, Axis{k}, Dynamic{k}, step, N);
+            refreshdata(Axis{k});
+        end
+        frames(step)=getframe(fig);
+    end
+   
+else % View solution at a particular time snapshot
+    step=Param.visua_param.snapshot;
+        for k=1:n
+            [Plot{k}, Axis{k}]=updateObject(Plot{k}, Axis{k}, Dynamic{k}, step, N);
+        end
+end
+
+
+if Param.visua_param.write_video
+%     vid_duration=25; % seconds
+    vid_duration=10; % seconds
+    video=VideoWriter('Output.mp4', 'MPEG-4');
+    video.FrameRate=(N+1)/vid_duration;
+    open(video);
+    writeVideo(video,frames);
+    close(video);
+end
+
+%% Parameters for patch plots
+    function[PlotParam, AxisParam, ColorbarParam, DynamicParam]=plotPatch(Mesh, Data, Solution, type)
+        
+        
+        % Retrieve mesh parameters
+        % Coordinate matrix
+        coord=Mesh.coord_mat;
+        % Connectivity matrix
+        connect=Mesh.connect_mat;
+        % Total number of nodes
+        nb_nodes=Mesh.nb_nodes;
+        % Frame surrounding the structure
+        frame=Mesh.frame;
+        % Largest dimension
+        L_max=Mesh.L_max;
+        % Number of local degrees of freedom
+        nb_dof_local=Mesh.nb_dof_local;
+        % Total number of degrees of freedom
+        nb_dof=Mesh.nb_dof;
+        
+        % Retrieve solution
+        U=Solution.U;
+        
+        
+        switch Data.Model.name
+               
+            case {'static_solid', 'dynamic_solid'}
+                
+                % Initilization
+                Vertices=zeros(nb_nodes,2,N+1);
+                CData=cell(N+1,1);
+                
+                
+                switch type
+                    case 'standard'
+                        
+                        if strcmp(Data.PostProcessing.amplification, 'auto')
+                            % Maximum displacement
+                            disp_max=max(max(abs(U)));
+                            % Amplification factor
+                            fact_ampl=L_max/(2*disp_max);
+                        else
+                            fact_ampl=Data.PostProcessing.amplification;
+                        end
+                        % Retrieve degrees of freedom
+                        d1=1:nb_dof_local:nb_dof;
+                        d2=2:nb_dof_local:nb_dof;
+                        % Aplified displacements
+                        ux_ampl=fact_ampl*U(d1,:);
+                        uy_ampl=fact_ampl*U(d2,:);
+                        % Deformed state
+                        def_x=coord(:,1)+ux_ampl;
+                        def_y=coord(:,2)+uy_ampl;
+                        % Vertices in deformed state
+                        Vertices(:,1,:)=def_x;
+                        Vertices(:,2,:)=def_y;
+                        XLim=[min(def_x, [], 'all');
+                              max(def_x, [], 'all')];
+                        YLim=[min(def_y, [], 'all');
+                              max(def_y, [], 'all')];
+                        CLim=[0,1];
+                        Colormap=[];
+                        ColorbarStatus='off';
+                        ColorbarLabel='';
+                        
+%                         Title='Amplified displacements';
+                        Title='Displacements';
+                        EdgeColor='black';
+                        FaceColor='none';
+                        
+                    case 'norm'
+                        
+                        % Retrieve degrees of freedom
+                        d1=1:nb_dof_local:nb_dof;
+                        d2=2:nb_dof_local:nb_dof;
+                        Ux=U(d1,:);
+                        Uy=U(d2,:);
+                        x=coord(:,1)+Ux;
+                        y=coord(:,2)+Uy;
+                        
+                        U_norm=sqrt(Ux.^2+Uy.^2);
+
+                        XLim=[min(x, [], 'all');
+                              max(x, [], 'all')];
+                        YLim=[min(y, [], 'all');
+                              max(y, [], 'all')];
+                        CLim=[0 max(U_norm, [], 'all')];
+                        Colormap=jet;
+                        ColorbarStatus='on';
+                        ColorbarLabel='Displacement [m]';
+                        Title='Norm of displacements';
+                        EdgeColor='none';
+                        FaceColor='interp';
+                        
+                        for m=1:N+1
+                            Vertices(:,1,m)=x(:,m);
+                            Vertices(:,2,m)=y(:,m);
+                            CData{m}=U_norm(:,m);
+                        end
+                        
+                    case 'absx'
+                        
+                        % Retrieve degrees of freedom
+                        d1=1:nb_dof_local:nb_dof;
+                        d2=2:nb_dof_local:nb_dof;
+                        Ux=U(d1,:);
+                        Uy=U(d2,:);
+                        x=coord(:,1)+Ux;
+                        y=coord(:,2)+Uy;
+                        
+                        Ux_abs=abs(Ux);
+                        
+                        XLim=[min(x, [], 'all');
+                              max(x, [], 'all')];
+                        YLim=[min(y, [], 'all');
+                              max(y, [], 'all')];
+                        CLim=[0 max(Ux_abs, [], 'all')];
+                        Colormap=jet;
+                        ColorbarStatus='on';
+                        ColorbarLabel='Displacement [m]';
+                        Title='Displacements in absolute value along x';
+                        EdgeColor='none';
+                        FaceColor='interp';
+                        
+                        for m=1:N+1
+                            Vertices(:,1,m)=x(:,m);
+                            Vertices(:,2,m)=y(:,m);
+                            CData{m}=Ux_abs(:,m);
+                        end
+                        
+                    case 'absy'
+                        
+                        % Retrieve degrees of freedom
+                        d1=1:nb_dof_local:nb_dof;
+                        d2=2:nb_dof_local:nb_dof;
+                        Ux=U(d1,:);
+                        Uy=U(d2,:);
+                        x=coord(:,1)+Ux;
+                        y=coord(:,2)+Uy;
+                        Uy_abs=abs(Uy);
+                        
+                        XLim=[min(x, [], 'all');
+                              max(x, [], 'all')];
+                        YLim=[min(y, [], 'all');
+                              max(y, [], 'all')];
+                        CLim=[0 max(Uy_abs, [], 'all')];
+                        Colormap=jet;
+                        ColorbarStatus='on';
+                        ColorbarLabel='Displacement [m]';
+                        Title='Displacements in absolute value along y';
+                        EdgeColor='none';
+                        FaceColor='interp';
+                        
+                        for m=1:N+1
+                            Vertices(:,1,m)=x(:,m);
+                            Vertices(:,2,m)=y(:,m);
+                            CData{m}=Uy_abs(:,m);
+                        end
+                        
+                        
+                end
+                
+                
+                % Set static parameters
+                PlotParam = {'Vertices',        coord;...
+                             'Faces',           connect(:,1:3);...
+                             'FaceVertexCData', CData{1};...
+                             'FaceColor',       FaceColor;...
+                             'EdgeColor',       EdgeColor;...
+                             'LineWidth',       1};
+                
+                AxisParam = {'XLim',            XLim;...
+                             'YLim',            YLim;...
+                             'CLim',            CLim;...
+                             'Colormap',        Colormap;...
+                             'DataAspectRatio', [1 1 1];...
+                             'XLabel.String',          'x coordinate [m]';...
+                             'YLabel.String',          'y coordinate [m]'};
+                
+                ColorbarParam = {'Visible',         ColorbarStatus;...
+                                 'Label.String',    ColorbarLabel;...
+                                 'Label.FontSize',  11};
+                
+                % Set dynamic parameters
+                DynamicParam={'Vertices',   num2cell(Vertices, [1,2]);...
+                              'CData',      CData;...
+                              'Title',      Title};
+                          
+            case 'transient_heat'
+                
+                % Number of sub-intervals in time
+                N=Data.Discretization.N;
+                % Initilization
+                CData=cell(N+1,1);
+                
+                switch type
+                    case 'standard'
+                        
+                        XLim=frame(:,1);
+                        YLim=frame(:,2);
+                        CLim=[min(U, [], 'all') max(U, [], 'all')];
+                        Colormap=jet;
+                        ColorbarStatus='on';
+                        ColorbarLabel='Temperature [ºC]';
+                        % ColorbarLabel='Temperature [K]';
+                        Title='Temperature';
+                        EdgeColor='none';
+                        FaceColor='interp';
+                        
+                        for m=1:N+1
+                            CData{m}=U(:,m);
+                        end
+                        
+                end
+                
+                
+                % Set static parameters
+                PlotParam = {'Vertices',        coord;...
+                             'Faces',           connect(:,1:3);...
+                             'FaceVertexCData', CData{1};...
+                             'FaceColor',       FaceColor;...
+                             'EdgeColor',       EdgeColor;...
+                             'LineWidth',       1};
+                
+                AxisParam = {'XLim',            XLim;...
+                             'YLim',            YLim;...
+                             'CLim',            CLim;...
+                             'Colormap',        Colormap;...
+                             'DataAspectRatio', [1 1 1];...
+                             'XLabel.String',          'x coordinate [m]';...
+                             'YLabel.String',          'y coordinate [m]'};
+                
+                ColorbarParam = {'Visible',         ColorbarStatus;...
+                                 'Label.String',    ColorbarLabel;...
+                                 'Label.FontSize',  11};
+                
+                % Set dynamic parameters
+                DynamicParam={'CData',      CData;...
+                              'Title',      Title};
+                   
+        end
+        
+        
+    end
+
+%% Parameters for surf plots
+    function[PlotParam, AxisParam, ColorbarParam, DynamicParam]=plotSurf(Mesh, Data, Solution, arg, type)
+  
+        % Retrieve mesh parameters
+        % Coordinate matrix of interpolation nodes
+        coord_inter=Mesh.coord_inter;
+        
+        % Retrieve data parameters
+        % Number of evaluation points per element
+        nq=Data.PostProcessing.eval_points;
+        
+        % Total number of interpolation nodes
+        nb_nodes_inter=size(coord_inter,1);
+        
+        % Initilization
+        Vertices=zeros(nb_nodes_inter,2,N+1);
+        CData=cell(N+1,1);
+        
+        switch arg
+            
+            case 'stress'
+                
+                % Retrieve solution
+                sigma=permute(Solution.sigma, [2 1 3]);
+                
+                switch type
+                    case 'von_mises'
+                        
+                        % Von Mises stress
+                        select=sqrt(sigma(:,1,:).^2-sigma(:,1,:).*sigma(:,2,:)+sigma(:,2,:).^2+3*sigma(:,3,:).^2);
+                        ColorbarLabel='Stress [Pa]';
+                        Title='Von Mises stress field';
+                        
+                    case 'sigma_xx'
+                        
+                        % Value of sigma_xx
+                        select=sigma(:,1,:);
+                        ColorbarLabel='Stress [Pa]';
+                        Title='\sigma_{xx}';
+                        
+                    case 'sigma_yy'
+                        
+                        % Value of sigma_yy
+                        select=sigma(:,3,:);
+                        ColorbarLabel='Stress [Pa]';
+                        Title='\sigma_{yy}';
+                        
+                    case 'sigma_xy'
+                        
+                        % Value of sigma_xy
+                        select=sigma(:,2,:);
+                        ColorbarLabel='Stress [Pa]';
+                        Title='\sigma_{xy}';
+                        
+                    case 'sigma_xx_abs'
+                        
+                        % Abolute value of sigma_xx
+                        select=abs(sigma(:,1,:));
+                        ColorbarLabel='Stress [Pa]';
+                        Title='|\sigma_{xx}|';
+                        
+                    case 'sigma_yy_abs'
+                        
+                        % Absolute value of sigma_yy
+                        select=abs(sigma(:,3,:));
+                        ColorbarLabel='Stress [Pa]';
+                        Title='|\sigma_{yy}|';
+                        
+                    case 'sigma_xy_abs'
+                        
+                        % Absolute value of sigma_xy
+                        select=abs(sigma(:,2,:));
+                        ColorbarLabel='Stress [Pa]';
+                        Title='|\sigma_{xy}|';
+                        
+                    case 'frobenius'
+                        
+                        % Frobenius norm of sigma
+                        select=sqrt(sigma(:,2,:).^2+2*sigma(:,2,:).^2+sigma(:,3,:).^2);
+                        ColorbarLabel='Stress [Pa]';
+                        Title='Frobenius norm of \sigma';
+                        
+                end
+                
+            case 'strain'
+                
+                % Retrieve solution
+                epsilon=permute(Solution.epsilon, [2 1 3]);
+                
+                switch type
+                    case 'epsilon_xx'
+                        
+                        % Value of epsilon_xx
+                        select=epsilon(:,1,:);
+                        ColorbarLabel='Strain [-]';
+                        Title='\epsilon_{xx}';
+                        
+                    case 'epsilon_yy'
+                        
+                        % Value of epsilon_yy
+                        select=epsilon(:,3,:);
+                        ColorbarLabel='Strain [-]';
+                        Title='\epsilon_{yy}';
+                        
+                    case 'epsilon_xy'
+                        
+                        % Value of esilon_xy
+                        select=epsilon(:,2,:);
+                        ColorbarLabel='Strain [-]';
+                        Title='\epsilon_{xy}';
+                        
+                    case 'epsilon_xx_abs'
+                        
+                        % Abolute value of epsilon_xx
+                        select=abs(epsilon(:,1,:));
+                        ColorbarLabel='Strain [-]';
+                        Title='|\epsilon_{xx}|';
+                        
+                    case 'epsilon_yy_abs'
+                        
+                        % Absolute value of epsilon_yy
+                        select=abs(epsilon(:,3,:));
+                        ColorbarLabel='Strain [-]';
+                        Title='|\epsilon_{yy}|';
+                        
+                    case 'epsilon_xy_abs'
+                        
+                        % Absolute value of epsilon_xy
+                        select=abs(epsilon(:,2,:));
+                        ColorbarLabel='Strain [-]';
+                        Title='|\epsilon_{xy}|';
+                        
+                    case 'frobenius'
+                        
+                        % Frobenius norm of epsilon
+                        select=sqrt(epsilon(:,2,:).^2+2*epsilon(:,2,:).^2+epsilon(:,3,:).^2);
+                        ColorbarLabel='Strain [-]';
+                        Title='Frobenius norm of \epsilon';
+                        
+                end
+                
+        end
+        
+        % Common parameters
+        XLim=[min(coord_inter(:,1,:), [], 'all');
+              max(coord_inter(:,1,:), [], 'all')];
+        YLim=[min(coord_inter(:,2,:), [], 'all');
+              max(coord_inter(:,2,:), [], 'all')];
+        CLim=[min(select, [], 'all') max(select, [], 'all')];
+        Colormap=jet;
+        ColorbarStatus='on';
+        EdgeColor='none';
+        FaceColor='interp';
+        
+        for m=1:N+1
+            Vertices(:,1,m)=coord_inter(:,1,m);
+            Vertices(:,2,m)=coord_inter(:,2,m);
+            CData{m}=select(:,m);
+        end
+        
+
+        switch nq
+            case 3
+                % 3-point evaluation inside each element
+                I=1:3*Mesh.nb_elem;
+                I=reshape(I, 3, Mesh.nb_elem)';
+                
+            case 6
+                % 6-point evaluation inside each element
+                i1=[1 4 5 2 2 5 6 3 3 6 4 1];
+                i2=[4 5 6];
+                I1=i1+(0:Mesh.nb_elem-1)'*6;
+                I2=i2+(0:Mesh.nb_elem-1)'*6;
+                I1=reshape(I1', 4, 3*Mesh.nb_elem)';
+                I2=[I2 nan*zeros(size(I2,1),1)];
+                I=[I1; I2];
+        end
+        
+                % Set static parameters
+                PlotParam = {'Vertices',        coord_inter;...
+                             'Faces',           I;...
+                             'FaceVertexCData', CData{1};...
+                             'FaceColor',       FaceColor;...
+                             'EdgeColor',       EdgeColor;...
+                             'LineWidth',       1};
+                
+                AxisParam = {'XLim',            XLim;...
+                             'YLim',            YLim;...
+                             'CLim',            CLim;...
+                             'Colormap',        Colormap;...
+                             'DataAspectRatio', [1 1 1];...
+                             'XLabel.String',          'x coordinate [m]';...
+                             'YLabel.String',          'y coordinate [m]'};
+                
+                ColorbarParam = {'Visible',         ColorbarStatus;...
+                                 'Label.String',    ColorbarLabel;...
+                                 'Label.FontSize',  11};
+                
+                % Set dynamic parameters
+                DynamicParam={'Vertices',   num2cell(Vertices, [1,2]);...
+                              'CData',      CData;...
+                              'Title',      Title};
+        
+    end
+
+%% Parameters for quiver plots
+    function[PlotParam, AxisParam, ColorbarParam, DynamicParam]=plotQuiver(Mesh, Solution, type)
+        
+        % Retrieve mesh parameters
+        % Coordinate matrix
+        coord=Mesh.coord_mat;
+        % Frame surrounding the structure
+        frame=Mesh.frame;
+        
+        % Retrieve solution
+        Q_sort=Solution.Q_sort;
+        
+        % Initialization
+        U=cell(1,N+1);
+        V=cell(1,N+1);
+        
+        switch type
+            case 'standard'
+                % Standard visualization of fluxes
+                Title='Flux field';
+                
+                for m=1:N+1
+                    U{m}=Q_sort(:,2*m-1);
+                    V{m}=Q_sort(:,2*m);
+                end
+                
+        end
+        
+        
+        % Set static parameters
+        PlotParam = {'XData',        coord(:,1);...
+                     'YData',        coord(:,2);...
+                     'LineWidth',       1};
+        
+        AxisParam = {'XLim',            frame(:,1);...
+                     'YLim',            frame(:,2);...
+                     'DataAspectRatio', [1 1 1];...
+                     'XLabel.String',          'x coordinate [m]';...
+                     'YLabel.String',          'y coordinate [m]'};
+        
+        ColorbarParam = {'Visible',         'off';...
+                         'Label.String',    '';...
+                         'Label.FontSize',  11};
+        
+        % Set dynamic parameters
+        DynamicParam={'UData',      U;...
+                      'VData',      V;...
+                      'Title',      Title};
+        
+        
+    end
+
+
+
+
+
+
+%% Setting and updating graphic properties
+    function[object]=setObject(object, param)
+        
+        argument=param(:,1);
+        value=param(:,2);
+        
+        for i=1:length(argument)         
+            if contains(argument{i}, '.')
+                field=extractBetween(argument{i}, 1, '.');
+                subfield=extractAfter(argument{i}, '.');
+                object.(field{1}).(subfield)=value{i};
+            else 
+                object.(argument{i})=value{i};
+            end
+        end
+        
+    end
+
+    function[plot, axis]=updateObject(plot, axis, param, step, N)
+        
+        argument=param(:,1);
+        value=param(:,2);
+        
+        for i=1:length(argument)-1
+            plot.(argument{i})=value{i}{step};
+        end
+        
+        axis.Title.String=[value{end} ' at step ' num2str(step) ' out of ' num2str(N+1)];
+    end
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_large/postProcess.m b/Code/INTERNODES_large/postProcess.m
new file mode 100644
index 0000000..364859c
--- /dev/null
+++ b/Code/INTERNODES_large/postProcess.m
@@ -0,0 +1,18 @@
+function[Mesh, Solution]=postProcess(Mesh, Data, Solution)
+
+% postProcess: General post-processing function used to compute derived
+% quantities
+% INPUT:
+% Mesh:     Structure containing all the mesh parameters
+% Data:     Structure containing all the data parameters
+% Solution: Structure containing the solution
+% OUTPUT:
+% Mesh:     Updated mesh structure with coordinates of nodes where stresses
+%           and strains were evaluated
+% Solution: Updated solution structure with derived quantities
+
+
+if any(contains(Data.PostProcessing.quantity, {'stress', 'strain'}))
+    [Mesh, Solution]=postProcessDisp(Mesh, Data, Solution);  
+end
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_large/postProcessDisp.m b/Code/INTERNODES_large/postProcessDisp.m
new file mode 100644
index 0000000..f1f6613
--- /dev/null
+++ b/Code/INTERNODES_large/postProcessDisp.m
@@ -0,0 +1,147 @@
+function[Mesh, Solution]=postProcessDisp(Mesh, Data, Solution)
+
+% postProcessDisp: Function used to compute strains and stresses once the 
+% displacements are known.
+% INPUT:
+% Mesh:     Structure containing all mesh parameters
+% Data:     Structure containing all data parameters
+% Solution: Structure containing the computed solutions
+% OUTPUT:
+% Mesh:     Updated mesh structure with evaluation points coordinates
+% Solution: Updated solution structure including strains and stresses
+
+
+% Finite element order
+order=Mesh.order;
+% Number of local degrees of freedom
+d=Mesh.nb_dof_local;
+% Number of elements
+ne=Mesh.nb_elem;
+% Number of nodes per element
+nne=Mesh.nb_nodes_elem;
+% Coordinate matrix
+coord=Mesh.coord_mat;
+% Connectivity matrix
+connect=Mesh.connect_mat;
+% Retrieve basis functions
+[Functions]=getFunctions(order);
+% Retrieve Jacobian matrix
+J_phi=Functions.J_phi;
+% Number of evaluation points per element
+nq=Data.PostProcessing.eval_points;
+% Retrieve evaluation points
+[points, ~] = getQuadrature(nq, 'interpolation');
+% Number of independent strain and stress components
+Mesh.nb_comp_ind=1/2*d*(d+1);
+% Mu
+f1=Data.Coefficient.mu.function;
+% Lambda
+f2=Data.Coefficient.lambda.function;
+% Retrieve solution
+U_sort=Solution.U_sort;
+% Compute stresses and strains using current position
+coord=coord+U_sort;
+
+% Define permutation matrix
+Sdd=zeros(d*d);
+Id1=eye(d);
+Id2=eye(d^2);
+
+for k=1:d
+    Sdd=Sdd+kron(Id1(:,k)', kron(Id1, Id1(:,k)));
+end
+
+% Reduce size of matrices to avoid unnecessary computations
+% sigma_xx, sigma_xy, sigma_yy
+h=[1 2 4];
+
+I1=(Id2+Sdd);
+I2=Id1(:);
+
+% Truncate the matrices
+I1=I1(h,:);
+I2=I2(h);
+
+% Initialization
+Q=zeros(nq*d^2, nne*d);
+
+% Loop over the Gauss points
+for i = 1:nq
+    L=kron(J_phi(points(i,:))', Id1);
+    Q((i-1)*d^2+1:i*d^2, :)=L;
+end
+Q=sparse(Q);
+T=connect';
+G=T(:);
+
+% Coordinates of the nodes of the elements
+coord_nodes=coord(G,:);
+% Displacements of the nodes of the elements
+W=reshape(U_sort(G,:)', nne*d, ne);
+% Vertices of the elements
+a=coord_nodes(1:nne:end,:);
+b=coord_nodes(2:nne:end,:);
+c=coord_nodes(3:nne:end,:);
+% Interpolated coordinates
+P=a(:)+[b(:)-a(:) c(:)-a(:)]*points';
+X(:,:,1)=P(1:ne,:);
+X(:,:,2)=P(ne+1:end,:);
+coord_inter=reshape(P', nq*ne, d);
+% Update mesh
+Mesh.coord_inter=coord_inter;
+
+% Computation of geometric quantities
+% Determinants of Jacobian matrices
+detJK=(b(:,1)-a(:,1)).*(c(:,2)-a(:,2))-(b(:,2)-a(:,2)).*(c(:,1)-a(:,1));
+
+JK_invT=zeros(d, d*ne);
+JK_invT(:,1:d:d*ne)=1./detJK'.*[c(:,2)-a(:,2) -(c(:,1)-a(:,1))]';
+JK_invT(:,2:d:d*ne)=1./detJK'.*[-(b(:,2)-a(:,2)) b(:,1)-a(:,1)]';
+
+vec_JK_invT=reshape(JK_invT, 1, d^2*ne);
+
+% Coefficient evaluation
+F1=f1(X);
+F2=f2(X);
+if size(F1,1)>1
+F1=reshape(F1, 1, nq, ne);
+end
+if size(F2,1)>1
+F2=reshape(F2, 1, nq, ne);
+end
+
+% Matrix product computations
+W=reshape(Q*W, d^2, nq, ne);
+
+B1=reshape(product(JK_invT,repmat(Id1, 1, ne),d,ne), d^2, d^2, ne);
+B2=reshape(vec_JK_invT, 1, d^2, ne);
+
+% Computation of strains
+if any(contains(Data.PostProcessing.quantity, 'strain'))
+    E=pagemtimes(B1, W);
+    E=0.5*I1*reshape(E, d^2, nq*ne);
+    Solution.epsilon=E;
+end
+
+% Computation of stresses
+if any(contains(Data.PostProcessing.quantity, 'stress'))
+    S1=pagemtimes(B1, F1.*W);
+    S2=pagemtimes(B2, F2.*W);
+    S1=I1*reshape(S1, d^2, nq*ne);
+    S2=I2*reshape(S2, 1, nq*ne);
+    S=S1+S2;
+    Solution.sigma=S;
+end
+
+
+    function[M]=product(A,B,d,ne)
+        
+        % !! Only for a two-dimensional case !!
+        M=zeros(d^2, d^2*ne);
+        
+        M(:,1:d^2:end)=kr(A(:,1:d:end), B(:,1:d:end));
+        M(:,2:d^2:end)=kr(A(:,1:d:end), B(:,2:d:end));
+        M(:,3:d^2:end)=kr(A(:,2:d:end), B(:,1:d:end));
+        M(:,4:d^2:end)=kr(A(:,2:d:end), B(:,2:d:end));
+    end
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_large/preCompute.m b/Code/INTERNODES_large/preCompute.m
new file mode 100644
index 0000000..ecb4137
--- /dev/null
+++ b/Code/INTERNODES_large/preCompute.m
@@ -0,0 +1,103 @@
+function[Data]=preCompute(Mesh, Data)
+
+% preCompute: Function which pre-computes quantities needed in a single 
+% iteration of the contact algorithm
+% INPUT:
+% Mesh:     Structure containing all the mesh parameters
+% Data:     Structure containing all the data parameters
+% OUTPUT:
+% Data:     Updated data structure
+
+d=Mesh.nb_dof_local;
+
+% Retrieve bodies
+body=Data.body;
+% Retrieve local degrees of freedom
+indx1=body{1}.indx_local;
+indx2=body{2}.indx_local;
+l1=length(indx1);
+l2=length(indx2);
+
+Data.l1=l1;
+Data.l2=l2;
+
+b2=Data.b2;
+
+L22=Data.L22;
+
+i2_old=Data.i2;
+i2=[indx1; indx2];
+% Beware of the ordering in the linear operators
+[~, index, reor]=intersect(i2_old, i2, 'stable');
+Data.index=index;
+
+%% Modified LU
+
+Phi11=Data.Phi11;
+Phi21=Data.Phi21;
+Phi12=Data.Phi12;
+Phi22=Data.Phi22;
+
+r1=size(Phi11,1);
+r2=size(Phi22,1);
+
+q1=ones(r1,1);
+q2=ones(r2,1);
+
+s1=Phi12*(Phi22\q2);
+s2=Phi21*(Phi11\q1);
+
+% Define initial sparsity
+S1=num2cell(1:r1);
+S2=num2cell(1:r2);
+% Maximum fill in per column of M per iteration
+Param.s=5;
+Param.maxit=20;
+Param.tol=0.2;
+
+[Phi11ia]=SPAI(Phi11, S1, Param);
+[Phi22ia]=SPAI(Phi22, S2, Param);
+
+R12a=Phi12*Phi22ia;
+R21a=Phi21*Phi11ia;
+
+% Rescaling
+R12a=R12a./s1;
+R21a=R21a./s2;
+
+M1=kron(body{1}.interface_mass, speye(d));
+M2=kron(body{2}.interface_mass, speye(d));
+
+R12a=kron(R12a, speye(d));
+R21a=kron(R21a, speye(d));
+
+Q1a=-diag(M2).*R21a./(diag(M1)');
+Q2a=-R12a';
+
+Y1a=[eye(l1); Q1a];
+Y2a=[eye(l1); Q2a];
+
+Z1=sparse(b2, l1);
+Z2=sparse(b2, l1);
+
+% Z1(index,:)=-alpha*Y1a(reor,:);
+% Z2(index,:)=Y2a(reor,:);
+
+Z1(index,:)=Y1a(reor,:);
+Z2(index,:)=Y2a(reor,:);
+
+Lv=L22;
+Uv=L22;
+
+[i,j]=find(Z1);
+[I]=findNNZc(i,j);
+
+Lv(:,I)=Z1;
+Uv(:,I)=Z2;
+
+Data.Lv=Lv;
+Data.Uv=Uv';
+Data.orl=1:b2;
+Data.orc=1:b2;
+
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_large/preProcess.m b/Code/INTERNODES_large/preProcess.m
new file mode 100644
index 0000000..6547c56
--- /dev/null
+++ b/Code/INTERNODES_large/preProcess.m
@@ -0,0 +1,86 @@
+function[Data]=preProcess(Data, Solution, K)
+
+% preProcess: Pre-computes the incomplete Cholesky factor of the perturbed 
+% stiffness matrix and part of the right-hand side vector. These quantities 
+% do not change during the course of the contact algorithm
+% INPUT:
+% Data:     Structure containing all the data parameters
+% Solution: Structure containing the initialized solution
+% K:        Global stiffness matrix
+% OUTPUT:
+% Data:     Updated data structure with part of the right-hand side vector, 
+%           reordering indices, Cholesky factors and matrices
+
+
+% Retrieve data parameters
+% Free degrees of freedom
+Nf=Data.Nf;
+% Blocked degrees of freedom
+Nd=Data.Nd;
+% Retrieve bodies
+body=Data.body;
+% Rescaling parameter
+r=Data.Contact.rescaling;
+% Perturbation added to the stiffness matrix to make it SPD
+Data.Contact.epsilon=1e-8;
+% Right-hand side force vector
+f=Data.F;
+% Retrieve solution
+U=Solution.U;
+% Number of free degrees of freedom
+nf=length(Nf);
+% Retrieve local degrees of freedom
+indx1=body{1}.indx_local;
+indx2=body{2}.indx_local;
+
+l1=length(indx1);
+l2=length(indx2);
+
+% Rescaled stiffness matrix
+Kff=1/r*K(Nf, Nf);
+% Perturbed stiffness matrix
+K_tilde=Kff+Data.Contact.epsilon*speye(nf,nf);
+% Cholesky decomposition of the perturbed stiffness matrix
+p=dissect(K_tilde);
+
+% Block sizes for partitioning
+b1=nf-(l1+l2);
+b2=l1+l2;
+
+% Cholesky decomposition of the reordered perturbed stiffness matrix 
+i2=[indx1; indx2];
+or=[setdiff(p, i2, 'stable')'; i2]; 
+Kr=K_tilde(or, or);
+% diagcomp=1e-3;
+Options.type='ict';
+Options.droptol=1e-4;
+Options.michol='off';
+% Options.diagcomp=diagcomp;
+Lr=ichol(Kr, Options);
+L22=Lr(b1+1:end,b1+1:end);
+Data.L22=L22;
+Data.i2=i2;
+Data.i2c=i2;
+Data.or=or;
+Data.A=L22*L22';
+
+Data.bc=1/r*(f(Nf)-K(Nf,Nd)*U(Nd));
+
+% Block sizes for partitioning
+Data.b1=b1;
+Data.b2=b2;
+
+% Number of free degrees of freedom
+Data.nf=nf;
+% Parameter alpha
+if strcmp(Data.Contact.alpha, 'auto')
+    Data.Contact.alpha=-norm(Kff, Inf);
+end
+% Rescaled stiffness matrix
+Data.K=Kff;
+% Cholesky factor of the reordered perturbed stiffness matrix
+Data.Lr=Lr;
+% Initial sizes of the active sets
+Data.li1=l1;
+Data.li2=l2;
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_large/readGMSH.m b/Code/INTERNODES_large/readGMSH.m
new file mode 100755
index 0000000..21425ca
--- /dev/null
+++ b/Code/INTERNODES_large/readGMSH.m
@@ -0,0 +1,86 @@
+function [Mesh] = readGMSH(file_name)
+
+% readGMSH: Reads a GMSH mesh file (.msh) and constructs the coordinate,
+% connectivity, boundary nodes, material tag and boundary tag matrices
+% INPUT:
+% file_name: GMSH mesh file (.msh) 
+% OUTPUT:
+% Mesh: Structure containing all fields related to the geometry, boundary
+% conditions and material tags
+%   coord: Matrix of node coordinates
+%   connectivites: Connectivity matrix of the system
+%   boundary_nodes: Matrix containing the boundary nodes (both displacements
+%   and external forces)
+%   material_tag: Vector containing the material tag of the elements
+%   boundary_tag: Vector containing the boundary tags of the elements 
+%   forming boundaries (enables to make the distinction between Dirichlet
+%   and Neumann boundary conditions)
+
+fileID = fopen(file_name);
+text = textscan(fileID, '%s', 'delimiter', '\n');
+fclose(fileID);
+
+text=text{1};
+% Construction of the coordinate matrix
+nodes_start = findPos(text, '$Nodes') + 1;
+
+fileID = fopen(file_name);
+% Node coordinates
+coord = textscan(fileID, '%f %f %f %f', 'HeaderLines', nodes_start);
+% Elements
+connect = textscan(fileID, [repmat('%f', 1, 20) '%*[^\n]'], 'HeaderLines', 3, 'EndOfLine', '\n', 'CollectOutput', 1);
+fclose(fileID);
+
+% Coordinate matrix
+coord=cell2mat(coord);
+% Ignore z component
+coord=coord(:,2:end-1);
+
+% Connectivity matrix
+connect=cell2mat(connect);
+
+I=sum(~isnan(connect), 2);
+v=unique(I);
+
+% Boundary elements
+B=connect(I==v(1), 1:v(1));
+% Elements
+E=connect(I==v(2), 1:v(2));
+
+% Boundary tags
+B_tags=B(:,4);
+% Element tags
+E_tags=E(:,4);
+
+% Boundary elements connectivity matrix
+B_connect=B(:,6:end);
+% Elements connectivity matrix
+E_connect=E(:,6:end);
+
+% Initialize mesh structure
+Mesh.coord_mat=coord;
+Mesh.BC_nodes=B_connect;
+Mesh.connect_mat=E_connect;
+Mesh.BC_tag=B_tags;
+Mesh.material_tag=E_tags;
+
+% Identification of the type of element in GMSH
+% 2 -> T3
+% 9 -> T6
+% 21 -> T10
+% 1 -> line
+% 8 -> quadratic curve
+% 26 -> cubic curve
+end
+
+
+function[start]=findPos(text, string)
+lines = size(text, 1);
+start = 1;
+for i = 1:lines
+    if strcmp(text{i}, string)
+        start = i;
+        break
+    end 
+end
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_large/removeTraction.m b/Code/INTERNODES_large/removeTraction.m
new file mode 100644
index 0000000..a2cefb7
--- /dev/null
+++ b/Code/INTERNODES_large/removeTraction.m
@@ -0,0 +1,71 @@
+function[Data]=removeTraction(Mesh, Data, Solution, n)
+
+% removeTraction: Checks for convergence and updates the interface properties
+% INPUT:
+% Mesh:     Structure containing all the mesh parameters
+% Data:     Structure containing all the data parameters
+% Solution: Structure containing the solution
+% n:        Current iteration number
+% OUTPUT:
+% Data:     Updated data structure with updated interface properties
+
+% Compute the normals based on deformed structure
+% Normals for initial interface
+[Data]=computeNormals(Mesh, Data, Solution);
+
+normal1=Data.body{1}.normal;
+normal2=Data.body{2}.normal;
+
+% Retrieve Lagrange multipliers
+lambda_sort1=Solution.lambda_sort;
+
+% Project the lambdas on interface 2
+lambda_sort2=-multiplyR21(Data, lambda_sort1);
+
+% Computes the scalar products
+scalar1=sum(lambda_sort1.*normal1(Data.body{1}.active_set,:),2);
+scalar2=sum(lambda_sort2.*normal2(Data.body{2}.active_set,:),2);
+
+% Detect nodes to be removed from the interfaces
+dump_nodes1=Data.body{1}.interface_nodes(scalar1>0);
+dump_nodes2=Data.body{2}.interface_nodes(scalar2>0);
+
+% Update the interfaces
+% If only compression test penetration, otherwise remove nodes in traction
+if isempty(dump_nodes1) && isempty(dump_nodes2)
+    % Gap verification
+    [add_nodes1, add_nodes2]=detectGaps(Mesh, Data, Solution);
+    
+    [Data, nodediff1]=updateInterface(Mesh, Data, 1, add_nodes1, 'add');
+    [Data, nodediff2]=updateInterface(Mesh, Data, 2, add_nodes2, 'add');
+    
+else
+    % Skip gap verification
+    [Data, nodediff1]=updateInterface(Mesh, Data, 1, dump_nodes1, 'dump');
+    [Data, nodediff2]=updateInterface(Mesh, Data, 2, dump_nodes2, 'dump');
+end
+
+fprintf('Iteration %d: \n', n)
+fprintf('%+d nodes on interface 1 \n', nodediff1)
+fprintf('%+d nodes on interface 2 \n', nodediff2)
+fprintf('----------------------------------- \n')
+
+% Check for convergence failure
+if isempty(Data.body{1}.interface_nodes) || isempty(Data.body{2}.interface_nodes)
+    msg=['The contact algorithm stopped prematurely because at least one of ' ...
+         'the sets of nodes of the potential contact interface is empty. '...
+         'Potential causes for this error are:\n'];
+     
+    causes=['1) The boundary conditions are incorrect \n'...
+            '2) The potential contact interface was misidentified \n'...
+            '3) The mesh is too coarse \n'];
+    
+    error(sprintf([msg, causes]))
+end
+
+% Check for convergence
+if abs(nodediff1)+abs(nodediff2)==0 % Stop the iterations
+    Data.Contact.iterate=0;
+    disp('Successfully converged')
+end
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_large/setBoundaryConditions.m b/Code/INTERNODES_large/setBoundaryConditions.m
new file mode 100644
index 0000000..2d375dc
--- /dev/null
+++ b/Code/INTERNODES_large/setBoundaryConditions.m
@@ -0,0 +1,267 @@
+function[Data]=setBoundaryConditions(Mesh, Data, varargin)
+
+% setBoundaryConditions: Function which initializes the boundary data
+% INPUT:
+% Mesh:     Structure containing all the mesh parameters
+% Data:     Structure containing all the data parameters
+% OPTIONAL INPUT:
+% Any type of boundary condition. If no Dirichlet boundary conditions are 
+% specified, the program returns an error
+% Types of boundary conditions:
+%   Dirichlet
+%   Neumann
+% OUTPUT:
+% Data: Updated data structure with boundary conditions
+
+
+%% Set boundary conditions
+if mod(length(varargin{1}), 2)~=0
+    error('Missing a name or value')
+end
+
+% Retrieve boundary nodes
+BC_tags=unique(Mesh.BC_tag);
+
+% Dirichlet type
+D_types={'dirichlet'};
+% Neumann types
+N_types={'neumann'};
+% Available types of boundary conditions
+available_types=union(D_types, N_types);
+% Count to detect duplicate boundaries
+count=[];
+% Count number of Dirichlet and Neumann boundaries specified
+nb_D_BC=0;
+nb_N_BC=0;
+
+% Retrieve labels and values entered by the user
+labels_in=varargin{1}(1:2:end);
+values_in=varargin{1}(2:2:end);
+
+% Initilization
+Data.BC=[];
+
+% Checking for boundary conditions
+c1=strcmp(labels_in, 'BC');
+
+if any(c1)
+    BC=values_in{c1};
+    nb_BC=length(BC);
+    Data.BC=cell(nb_BC,1);
+    
+    for i=1:nb_BC
+        bc=BC{i};
+        l=length(bc);
+        
+        if mod(l,2)~=0
+            error('Missing a name or value')
+        end
+        
+        for j=1:2:l
+            arg=lower(bc{j});
+            val=bc{j+1};
+            
+            switch arg
+                
+                case 'type'
+                    if any(ismember(available_types, lower(val)))
+                        Data.BC{i}.(arg)=lower(val);
+                        
+                        if any(ismember(D_types, lower(val)))
+                            nb_D_BC=nb_D_BC+1;
+                        else
+                            nb_N_BC=nb_N_BC+1;
+                        end
+                    else
+                        error('Unrecognized boundary condition')
+                    end
+                    
+                case 'edges'
+                    
+                    edges=intersect(BC_tags, val);
+                    
+                    if isempty(edges)
+                        error('One of the edges specified does not exist')
+                    elseif ~isempty(intersect(count, val)) || length(unique(val))~=length(val)
+                        error('Duplicate edges detected')   
+                    end
+                    count=[count val];
+                    Data.BC{i}.(arg)=val;
+                    
+                case {'function'}
+                    
+                    if isa(val,'double')
+                        Data.BC{i}.label='const';
+                        Data.BC{i}.(arg)= @(x) val;
+                        
+                    elseif isa(val,'function_handle')
+                        Data.BC{i}.label=detectDependency(val);
+                        Data.BC{i}.(arg)=val;
+                    else
+                        error('Input parameters must be either constants or function handles')
+                    end
+                    
+                otherwise
+                    error('Unrecognized argument')
+            end
+            
+        end
+        
+        
+    end
+    
+end
+
+%% Post-Processing: Partitioning the structure BC into Dirichlet BC and Neumann BC and checking for missing data
+
+if nb_D_BC==0
+    error('Dirichlet boundary conditions must be prescribed')
+end
+
+BC=Data.BC;
+l=length(BC);
+
+% Initialize cells
+D_BC=cell(nb_D_BC,1);
+N_BC=cell(nb_N_BC,1);
+% Set of Dirichlet and Neumann tags
+D_tags=[];
+N_tags=[];
+% Initialize counters
+d=1;
+n=1;
+
+for i=1:l
+    bc=BC{i};
+    type=bc.type;
+    
+    if ~isfield(bc, 'edges')
+        error('The edges must be specified')
+    end
+    
+    if any(ismember(D_types, type))
+        D_tags=[D_tags bc.edges];
+        D_BC{d}.type=type; 
+        D_BC{d}.edges=bc.edges;
+        
+        if isfield(bc, 'function')
+            D_BC{d}.function=bc.function;
+            D_BC{d}.label=bc.label;
+        else
+            warning('A function has not been prescribed and will be assumed zero')
+            D_BC{d}.function=@(x) [0; 0];
+            D_BC{d}.label='const';
+        end
+        
+        d=d+1;
+        
+    elseif any(ismember(N_types, type))
+        N_tags=[N_tags bc.edges];
+        N_BC{n}.type=type;
+        N_BC{n}.edges=bc.edges;
+        
+        
+        if isfield(bc, 'function')
+            N_BC{n}.function=bc.function;
+            N_BC{n}.label=bc.label;
+        else
+            warning('A function has not been prescribed and will be assumed zero')
+            N_BC{n}.function=@(x) [0; 0];
+            N_BC{n}.label='const';
+        end
+        
+        n=n+1;
+    end
+end
+
+Data.D_BC=D_BC;
+Data.N_BC=N_BC;
+
+Data.D_tags=D_tags;
+Data.N_tags=N_tags;
+
+%% Sorting nodes
+% If there is a node belonging to conflicting boundary conditions, it is
+% set using the following priority list: Dirichlet, Neumann, Interior
+BC_nodes=Mesh.BC_nodes;
+BC_tag=Mesh.BC_tag;
+nb_nodes=Mesh.nb_nodes;
+Dof_set=Mesh.Dof_set;
+
+set_D_nodes=cell(nb_D_BC,1);
+set_N_nodes=cell(nb_N_BC,1);
+
+for k=1:nb_D_BC
+    nodes=BC_nodes(any(BC_tag==D_BC{k}.edges,2),:);
+    set_D_nodes{k}=unique(nodes);
+end
+for k=1:nb_N_BC
+    nodes=BC_nodes(any(BC_tag==N_BC{k}.edges,2),:);
+    set_N_nodes{k}=unique(nodes);
+end
+% Define the sets of nodes
+% Dirichlet nodes
+Nodes_D=unique(cell2mat(set_D_nodes));
+% Neumann nodes
+Nodes_N=setdiff(unique(BC_nodes), Nodes_D);
+% Interior nodes
+Nodes_I=setdiff(1:nb_nodes, union(Nodes_D, Nodes_N));
+% Free nodes
+Nodes_F=union(Nodes_I, Nodes_N);
+
+% Correct the set for Neumann nodes
+for k=1:nb_N_BC
+    set_N_nodes{k}=setdiff(set_N_nodes{k}, Nodes_D);
+end
+
+
+% Define the sets of degrees of freedom
+Nd=Dof_set(:,Nodes_D);
+Nn=Dof_set(:,Nodes_N);
+Ni=Dof_set(:,Nodes_I);
+
+Ni=Ni(:);
+Nd=Nd(:);
+Nn=Nn(:);
+Nf=union(Ni, Nn);
+
+% Update data
+Data.Nodes_I=Nodes_I;
+Data.Nodes_N=Nodes_N;
+Data.Nodes_D=Nodes_D;
+Data.Nodes_F=Nodes_F;
+
+Data.Ni=Ni;
+Data.Nd=Nd;
+Data.Nn=Nn;
+Data.Nf=Nf;
+
+Data.set_D_nodes=set_D_nodes;
+Data.set_N_nodes=set_N_nodes;
+
+%% Auxiliary function
+
+    function[label]=detectDependency(func)
+        
+        string=func2str(func);
+        
+        if ~strcmp(string(1:4), '@(x)')
+            error('Function handle must contain space variables')
+        end
+        string=string(5:end);
+        
+        if contains(string, 'x') && contains(string, 't')
+            label='space_time';
+        elseif contains(string, 'x')
+            label='space';
+        elseif contains(string, 't')
+            label='time';
+        elseif contains(string(1), '0')
+            label='zero';
+        else
+            label='const';
+        end
+        
+    end
+
+end
diff --git a/Code/INTERNODES_large/setCoefficient.m b/Code/INTERNODES_large/setCoefficient.m
new file mode 100644
index 0000000..d15a956
--- /dev/null
+++ b/Code/INTERNODES_large/setCoefficient.m
@@ -0,0 +1,103 @@
+function[Data]=setCoefficient(Data, varargin)
+
+% setCoefficient: Initializes the model coefficients
+% OPTIONAL INPUT:
+% rho:      Specific mass
+% mu:       Lamé constant
+% lambda:   Lamé constant
+% f:        Function "f" of the PDE
+% OUTPUT:
+% Data:     Data structure with initialized coefficients
+
+if mod(length(varargin), 2)~=0
+    error('Missing a name or value')
+end
+labels_in=varargin(1:2:end);
+values_in=varargin(2:2:end);
+
+available_coeff={'rho', 'lambda', 'mu', 'f'};
+
+for i=1:length(labels_in)
+    arg=lower(labels_in{i});
+    val=values_in{i};
+    
+    if any(ismember(available_coeff, arg))
+        
+        
+        switch arg
+            case {'rho', 'lambda', 'mu'} % Only space dependent
+                if isa(val,'double')
+                    
+                    if val ~= 0
+                        Data.Coefficient.(arg).label='const';
+                    else
+                        Data.Coefficient.(arg).label='zero';
+                    end
+                    Data.Coefficient.(arg).function=@(x) val;
+                    
+                elseif isa(val,'function_handle')
+                    Data.Coefficient.(arg).label=detectDependency1(val);
+                    Data.Coefficient.(arg).function=val;
+                else
+                    error('Input parameters must be either constants or function handles')
+                end
+                
+            case 'f'
+                if isa(val,'double') % Only space dependent
+                    Data.Coefficient.f.label='const';
+                    Data.Coefficient.f.function=@(x) val;
+                elseif isa(val,'function_handle')
+                    Data.Coefficient.f.label=detectDependency2(val);
+                    Data.Coefficient.f.function=@(x) val(x);
+                else
+                    error('Input parameters must be either constants or function handles')
+                end
+                
+        end
+        
+    else
+        error('Unrecognized coefficient')
+    end
+end
+
+
+    function[label]=detectDependency1(func)
+        
+        string=func2str(func);
+        
+        if ~strcmp(string(1:4), '@(x)')
+            error('Coefficient can only be constant or space dependent')
+        end
+        
+        if contains(string(5:end), 'x')
+            label='space';
+        elseif contains(string(5), '0')
+            label='zero';
+        else
+            label='const';
+        end
+    end
+
+    function[label]=detectDependency2(func)
+        
+        string=func2str(func);
+        
+        if ~strcmp(string(1:4), '@(x)')
+            error('Function handle must contain space variables')
+        end
+        string=string(5:end);
+        
+        if contains(string, 'x') && contains(string, 't')
+            label='space_time';
+        elseif contains(string, 'x')
+            label='space';
+        elseif contains(string, 't')
+            label='time';
+        elseif contains(string(1), '0')
+            label='zero';
+        else
+            label='const';
+        end
+        
+    end
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_large/setModel.m b/Code/INTERNODES_large/setModel.m
new file mode 100644
index 0000000..58c27a3
--- /dev/null
+++ b/Code/INTERNODES_large/setModel.m
@@ -0,0 +1,75 @@
+function[Data]=setModel(varargin)
+
+% setModel: Initializes the numerical model
+% OPTIONAL INPUT:
+% model:        Specifies which model should be used
+%               Default: transient_heat
+% submodel:     Specifies which submodel from the specified model should be
+%               used
+%               Default: standard
+% type:         Specifies whether the model is linear or nonlinear
+%               Default: linear
+% OUTPUT:
+% Data:         Data structure with initialized model
+
+if mod(length(varargin), 2)~=0
+    error('Missing a name or value')
+end
+labels_in=varargin(1:2:end);
+values_in=varargin(2:2:end);
+
+models={'transient_heat', 'static_solid', 'dynamic_solid'};
+n=length(models);
+submodels=cell(n,1);
+submodels{1}={'standard'};
+submodels{2}={'standard', 'contact'};
+submodels{3}={'standard'};
+types={'linear', 'nonlinear'};
+
+% Set default Parameters
+Data.Model.name=models{1};
+Data.Model.submodel=submodels{1};
+Data.Model.type=types{1};
+
+% Define constants
+% Stefan Boltzman constant W/(m^2*K^4)
+Data.Constants.sigma=5.670373e-8;
+
+for i=1:length(labels_in)
+    arg=lower(labels_in{i});
+    val=lower(values_in{i});
+    
+    switch arg
+        case 'model'
+            if any(ismember(models, val))
+                Data.Model.name=val;
+            else
+                error('Unrecognized model')
+            end
+            
+        case 'submodel'
+            
+            if isfield(Data.Model, 'name')
+                index=strcmp(Data.Model.name, models);
+            else
+                error('Submodel must be defined after the model')
+            end
+            if any(ismember(submodels{index}, val))
+                Data.Model.submodel=val;
+            else
+                error('Unrecognized submodel')
+            end
+            
+        case 'type'
+            if any(ismember(types, val))
+                Data.Model.type=val;
+            else
+                error('Unrecognized type')
+            end     
+            
+        otherwise
+            error('Unrecognized argument')
+    end
+    
+end
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_large/setNumericalParameters.m b/Code/INTERNODES_large/setNumericalParameters.m
new file mode 100644
index 0000000..3cbd970
--- /dev/null
+++ b/Code/INTERNODES_large/setNumericalParameters.m
@@ -0,0 +1,270 @@
+function[Data]=setNumericalParameters(Mesh, Data, varargin)
+
+% setNumericalParameters: Initializes the numerical parameters of the
+% solver
+% OPTIONAL INPUT:
+% Contact:
+%   iterate:            Parameter controlling whether iterations should 
+%                       take place (1) or not (0)
+%                       Default: 1
+%   maxiter:            Maximum number of iterations for solving the
+%                       contact problem
+%                       Default: 10
+%   rescaling:          Rescaling parameter for the stiffness matrix
+%                       Default: 1
+%   alpha:              Parameter for the preconditioner
+%                       Default: auto 
+%   c:                  Number of nearest neighbors for computing the
+%                       radius of support of the radial basis functions
+%                       Default: 1
+%   radius:             Intersection radius used to define the initial
+%                       potential contact interface. 
+%                       Default: infinity
+%   gamma:              Value of gamma for preventing quick loss of diagonal
+%                       dominance of the matrices PhiMM
+%                       Default: 0.5
+%   Gamma:              Value of Gamma to avoid very small nonzero entries
+%                       in the matrices PhiNM
+%                       Default: 0.95
+%   d0:                 Measure of tolerance for contact detection.
+%                       Default: 0.05
+%   tol:                Tolerance for gap detection
+%                       Default: 1e-1
+% Solver:
+%   name:               Name of the solver to be used.
+%                       Default: GMRES
+%   maxiter:            Total number of GMRES iterations
+%                       Default: 1000
+%   riter:              Number of iterations before GMRES restarts
+%                       Default: 20
+%   tol:                Tolerance (stopping creterion) for GMRES
+%                       Default: 1e-7
+%   reorth_tol:         Re-orthogonalization tolerance for GMRES
+%                       Default: 0.7
+% PostProcessing:
+%   quantity:           Additional quantities to be computed (for example
+%                       stresses, strains, fluxes,...)
+%                       Default: none
+%   ampl_fact:          Value of amplification factor to apply to
+%                       displacements
+%                       Default: auto
+%   eval_points:        Number of evaluation points used for computing
+%                       stresses or strains
+%                       Default: 3 (P1 finite elements)
+%                                6 (P2 or higher order finite elements)                
+
+if nargin>2
+if mod(length(varargin)-1, 2)~=0
+    error('Missing a name or value')
+end
+
+id=lower(varargin{1});
+labels_in=varargin(2:2:end);
+values_in=varargin(3:2:end);
+end
+
+% Set default Parameters
+if ~isfield(Data, 'Contact')
+    % Boolean indicator for contact algorithm iterations
+    Data.Contact.iterate=true;
+    % Maximum number of iterations of the contact algorithm
+    Data.Contact.maxiter=10;
+    % Rescaling parameter for the INTERNODES matrix
+    Data.Contact.rescaling=1;
+    % Parameter alpha of the preconditioner
+    Data.Contact.alpha='auto';
+    % Number of nearest neighbors
+    Data.Contact.c=1;
+    % Intersection radius
+    Data.Contact.radius=inf;
+    % gamma value
+    Data.Contact.gamma=0.5;
+    % Gamma value
+    Data.Contact.Gamma=0.95;
+    % Contact tolerance
+    Data.Contact.d0=0.05;
+    % Gap tolerance
+    Data.Contact.tol=1e-1;
+end
+if ~isfield(Data, 'Solver')  
+    % Solver name
+    Data.Solver.name='GMRES';
+    % GMRES parameters
+    % Total maximum number of iterations
+    Data.Solver.maxiter=1000;
+    % Maximum number of iterations before restart
+    Data.Solver.riter=20;
+    % Tolerance on the residual
+    Data.Solver.tol=1e-7;
+    % Reorthogonalization tolerance
+    Data.Solver.reorth_tol=0.7;
+end
+if ~isfield(Data, 'PostProcessing')   
+    % Additional quantities to be computed
+    Data.PostProcessing.quantity='none';
+    % Amplification factor
+    Data.PostProcessing.amplification='auto';
+    % Number of evaluation points per element for computing stresses/strains
+    eval_points={3,6};
+    
+    if Mesh.order==1
+        Data.PostProcessing.eval_points=eval_points{1};
+    else
+        Data.PostProcessing.eval_points=eval_points{2};
+    end
+    
+end
+
+% Override the defaults
+if exist('id', 'var')
+    switch id
+        case 'contact'
+            %% Contact algorithm specific parameters
+            
+            for i=1:length(labels_in)
+                arg=lower(labels_in{i});
+                val=lower(values_in{i});
+                
+                switch arg
+                    case 'iterate'
+                        if isa(val, 'double')
+                            
+                            if val<0 || val>1
+                                error([arg ' parameter must be a boolean'])
+                            else
+                                Data.Contact.(arg)=logical(val);
+                            end
+                            
+                        elseif isa(val, 'logical')
+                            Data.Contact.(arg)=val;
+                        else
+                            error([arg ' parameter must be a boolean'])
+                        end
+                        
+                    case {'maxiter', 'c', 'radius', 'd0'}
+                        if val<=0
+                            error(['Parameter ' arg ' must be strictly greater than zero'])
+                        else
+                            Data.Contact.(arg)=val;
+                        end
+                        
+                    case 'rescaling'
+                        if val==0
+                            error(['Parameter ' arg ' must be different from zero'])
+                        else
+                            Data.Contact.(arg)=val;
+                        end
+                        
+                    case {'gamma', 'Gamma', 'tol'}
+                        if val<=0 || val>=1
+                            error([arg ' must be strictly greater than 0 and strictly smaller than 1'])
+                        else
+                            Data.Contact.(arg)=val;
+                        end
+                        
+                    case 'alpha'
+                        if isa(val, 'char')
+                            if ~strcmp(val, 'auto')
+                                error('Invalid alpha parameter')
+                            else
+                                Data.Contact.(arg)=val;
+                            end
+                            
+                        elseif isa(val, 'double')
+                            if val == 0
+                                error('Invalid alpha parameter')
+                            else
+                                Data.Contact.(arg)=val;
+                            end
+                        else
+                            error('Invalid alpha parameter')
+                        end
+                        
+                    otherwise
+                        error('Unrecognized argument')
+                end
+            end
+            
+            
+        case 'solver'
+            %% Solver parameters
+            names={'GMRES'};
+            
+            for i=1:length(labels_in)
+                arg=lower(labels_in{i});
+                val=values_in{i};
+                
+                switch arg
+                    case 'name'
+                        if any(ismember(names, val))
+                            Data.Solver.(arg)=val;
+                        else
+                            error('Unrecognized solver name')
+                        end
+                        
+                    case {'maxiter', 'riter', 'tol', 'reorth_tol'}
+                        if val<=0
+                            error(['Parameter ' arg ' must be strictly positive'])
+                        else
+                            Data.Solver.(arg)=val;
+                        end
+                        
+                    otherwise
+                        error('Unrecognized argument')
+                end
+                
+            end
+            
+        case 'postprocessing'
+            %% Post-processing parameters
+            quantities={'none', 'flux', 'error', 'stress', 'strain'};
+            eval_points={3,6};
+            
+            for i=1:length(labels_in)
+                arg=lower(labels_in{i});
+                val=values_in{i};
+                
+                switch arg
+                    case 'quantity'
+                        if any(ismember(quantities, val))
+                            Data.PostProcessing.quantity=val;
+                        else
+                            error('Unrecognized quantity')
+                        end
+
+                    case 'eval_points'
+                        if any(ismember(eval_points, val))
+                            Data.PostProcessing.eval_points=val;
+                        else
+                            error('Invalid number of evaluation points')
+                        end
+                        
+                    case 'ampl_fact'
+                        
+                        if isa(val, 'char')
+                            if ~strcmp(val, 'auto')
+                                error('Invalid amplification factor')
+                            else
+                                Data.PostProcessing.amplification=val;
+                            end
+                            
+                        elseif isa(val, 'double')
+                            if val <= 0
+                                error('Invalid amplification factor')
+                            else
+                                Data.PostProcessing.amplification=val;
+                            end
+                        else
+                            error('Invalid amplification factor')
+                        end
+                        
+                    otherwise
+                        error('Unrecognized argument')
+                end
+            end
+            
+        otherwise
+            error('Unrecognized argument')
+    end
+end
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_large/solve.m b/Code/INTERNODES_large/solve.m
new file mode 100644
index 0000000..d6523e1
--- /dev/null
+++ b/Code/INTERNODES_large/solve.m
@@ -0,0 +1,94 @@
+function[Mesh, Data, Solution]=solve(Mesh, Data, it)
+
+% solve: Solves the contact problem
+% INPUT:
+% Mesh:     Structure containing all the mesh parameters
+% Data:     Structure containing all the data parameters
+% it:       Cell array containing the boundary tags of the interfaces (master
+%           interface and slave interface)
+% OUTPUT:
+% Mesh:     Updated structure containing all the mesh parameters
+% Data:     Updated structure containing all the data parameters
+% Solution: Structure containing the solution
+
+
+% Initialization of the system
+[Data, Solution]=initializeSystem(Mesh, Data, it);
+% Initialize count
+n=1;
+
+while Data.Contact.iterate && n<= Data.Contact.maxiter
+    
+    % Solve the system of equations
+    [Solution, Data]=solveSystem(Mesh, Data, Solution);
+    % Update the system
+    [Data]=updateSystem(Mesh, Data, Solution);
+    n=n+1;
+end
+
+if n > Data.Contact.maxiter && Data.Contact.iterate
+    warning('Failed to converge within the specified number of iterations')
+end
+
+    function[Solution, Data]=solveSystem(Mesh, Data, Solution)
+        
+        % Retrieve data parameters
+        % Free degrees of freedom
+        Nf=Data.Nf;
+        % Number of free degrees of freedom
+        nf=Data.nf;
+        % Number of constraints
+        nc=Data.nc;
+        
+        % Retrieve mesh parameters
+        d=Mesh.nb_dof_local;
+        
+        % Retrieve solution
+        U=Solution.U;
+        % Solve the linear system iteratively
+        [x, Data]=iterativeSolver(Mesh, Data);
+        % Retrieve the displacements
+        U(Nf)=x(1:nf);
+        
+        % Update solution
+        Solution.U=U;
+        Solution.U_sort=reshape(U, d, [])';
+        Solution.lambda=Data.Contact.rescaling*x(nf+1:nf+nc);
+        Solution.lambda_sort=reshape(Solution.lambda, d, [])';
+    end
+
+    function[x, Data]=iterativeSolver(Mesh, Data)
+        
+        b=Data.b;
+        [Data]=preCompute(Mesh, Data);
+        
+        if strcmp(Data.Solver.name,'GMRES')
+            x0=zeros(length(b),1);
+            [x, ~, ~] = gmres_k(Mesh, Data, @linearOp1, b, x0, 'level 1');
+            
+            
+        else
+            error('Solver unrecognized or not yet implemented')
+        end
+        
+    end
+        
+    function[Data]=updateSystem(Mesh, Data, Solution)
+        
+        % updateSystem: Updates all quantities which have changed
+        % INPUT:
+        % Mesh:     Structure containing all the mesh parameters
+        % Data:     Structure containing all the data parameters
+        % Solution: Solution structure with computed displacements
+        % OUPUT:
+        % Data:     Data structure with updated interface properties
+        
+        % Check for convergence
+        [Data]=removeTraction(Mesh, Data, Solution, n);
+        % Reassemble the interpolation matrices and update interface
+        [Data]=subAssemble(Mesh, Data, Solution, 'updating');
+        % Update right-hand side
+        [Data]=updateRHS(Mesh, Data);
+        
+    end
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_large/subAssemble.m b/Code/INTERNODES_large/subAssemble.m
new file mode 100644
index 0000000..11464ed
--- /dev/null
+++ b/Code/INTERNODES_large/subAssemble.m
@@ -0,0 +1,176 @@
+function[Data]=subAssemble(Mesh, Data, Solution, operation)
+
+% subAssemble: Assembles the part of the internodes matrix which is
+% changing from one iteration to the next
+% INPUT:
+% Mesh:         Structure containing all the mesh parameters
+% Data:         Structure containing all the data parameters
+% Solution:     Structure containing the solution
+% operation:    Specifies the type of operation (initialization or
+%               updating)
+% OUTPUT:
+% Data:         Updated structure containing all the data parameters
+
+if strcmp(operation, 'initialization')
+    [Data]=computeBoundaryMass(Mesh, Data, 'initialization');
+end
+
+% Assemble interpolation matrices
+[Data]=assembleRs(Mesh, Data, Solution);
+% Assemble boundary mass matrices
+[Data]=computeBoundaryMass(Mesh, Data, 'updating');
+
+    function[Data]=assembleRs(Mesh, Data, Solution)
+        
+        % assembleRs: Constructs the interpolation matrices
+        % INPUT:
+        % Mesh:     Structure containing all the mesh parameters
+        % Data:     Structure containing all the data parameters
+        % OUTPUT:
+        % Data:     Updated data structure with interpolation matrices
+        
+        % Retrieve nodes making up the interfaces
+        nodes1=Data.body{1}.interface_nodes;
+        nodes2=Data.body{2}.interface_nodes;
+        
+        % 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 distant bodies
+        if all(i1) || all(i2)
+            warning('One of the bodies will be translated because they are too far away from each other. Boundary conditions might be affected')
+            [Mesh, coord1, coord2]=translateBodies(Mesh, Data, radiuses1, radiuses2, nodes1, nodes2, coord1, coord2);
+            
+            % 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
+        
+        % Check for isolated nodes
+        while any(i1) || any(i2)
+            
+            dump_nodes1=nodes1(i1);
+            dump_nodes2=nodes2(i2);
+            
+            % Updating
+            [Data]=updateInterface(Mesh, Data, 1, dump_nodes1, 'dump');
+            [Data]=updateInterface(Mesh, Data, 2, dump_nodes2, 'dump');
+            
+            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
+        
+        [Data]=constructInterpolants(Data, radiuses1, radiuses2, coord1, coord2);
+    end
+
+    function[Data]=computeBoundaryMass(Mesh, Data, operation)
+        
+        % computeBoundaryMass: Assembles the boundary mass matrices
+        % INPUT:
+        % Mesh:         Structure containing all the mesh parameters
+        % Data:         Structure containing all the data parameters
+        % operation:    Specifies the type of operation (initialization or
+        %               updating)
+        % OUTPUT:
+        % Data:         Updated data structure with boundary mass matrices
+        
+        
+        % Retrieve mesh parameters
+        % Number of local degrees of freedom
+        nb_dof_local=Mesh.nb_dof_local;
+        % Coordinate matrix
+        coord=Mesh.coord_mat;
+        % Order
+        order=Mesh.order;
+        % Get quadrature nodes and weights
+        [points, weights]=getQuadrature(3, 'boundary');
+        % Number of quadrature points
+        nb_points=length(weights);
+        
+        % Retrive functions
+        [Functions]=getFunctions(order);
+        % Retrieve boundary basis functions
+        phi_boundary=Functions.phi_boundary;
+        
+        % Retrieve bodies
+        body=Data.body;
+        % Number of bodies
+        nb_bodies=length(body);
+        
+        if strcmp(operation, 'initialization')
+            
+            for n=1:nb_bodies
+                % Retrieve data
+                % Retriveve edges making up the interface
+                Edges=body{n}.interface_connect;
+                % Retrieve nodes making up the interface
+                nodes=body{n}.interface_nodes;
+                nb_nodes=length(nodes);
+                s=order+1;
+                
+                % Initialization
+                Phi=zeros(s, nb_points);
+                
+                % Map global nodes to local nodes
+                func=@(x) find(nodes==x);
+                connect=arrayfun(func, Edges);
+                T=connect';
+                G=T(:);
+                
+                I=repmat(connect, [1 s])';
+                J=repmat(G', [s 1]);
+                
+                % Loop over the Gauss points
+                for i = 1:nb_points
+                    Phi(:,i)=phi_boundary(points(i,:));
+                end
+                
+                Tr=Edges';
+                Gr=Tr(:);
+                % Coordinates of the nodes of the elements
+                coord_nodes=coord(Gr,:);
+                % Endpoints of the edges
+                a=coord_nodes(1:s:end,:);
+                b=coord_nodes(2:s:end,:);
+                bk=b-a;
+                norm_bk=vecnorm(bk,2,2);
+                
+                lambda=weights.*norm_bk';
+                
+                M=kr(Phi, Phi)*lambda;
+                M=sparse(I(:), J(:), M(:), nb_nodes, nb_nodes);
+                
+                Data.body{n}.iinterface_mass=M;
+                % Expansion to full size
+                M=kron(M, speye(nb_dof_local));
+                Data.body{n}.interface_mass=M;
+            end
+        else
+            for n=1:nb_bodies
+                active_set=Data.body{n}.active_set;                
+                Data.body{n}.interface_mass=Data.body{n}.iinterface_mass(active_set, active_set);
+            end
+        end
+    end
+
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_large/translateBodies.m b/Code/INTERNODES_large/translateBodies.m
new file mode 100644
index 0000000..f5a398f
--- /dev/null
+++ b/Code/INTERNODES_large/translateBodies.m
@@ -0,0 +1,60 @@
+function[Mesh, coord1, coord2]=translateBodies(Mesh, Data, radiuses1, radiuses2, nodes1, nodes2, coord1, coord2)
+
+% translateBodies: Performs a translation of one of the bodies in case the
+% initial potential contact interface is empty (because the bodies are too 
+% far away from each other)
+% INPUT:
+% Mesh:         Structure containing all the mesh parameters
+% Data:         Structure containing all the data parameters
+% radiusesk:    Radiuses of support of the radial basis functions (k=1,2)
+% nodesk:       Nodes on the potential contact interface of body k (k=1,2)
+% coordk:       Coordinates of the nodes on the potential contact interface 
+%               of body k (k=1,2)
+% OUTPUT:
+% Mesh:         Mesh structure with updated coordinate matrix
+% coordk:       New coordinates of the nodes on the potential contact 
+%               interface of body k (k=1,2)
+
+n1=length(radiuses1);
+
+dist=inf;
+node1=0;
+node2=0;
+
+for k=1:n1
+    
+    [d, i]=min(vecnorm(coord1(k,:)-coord2,2,2));
+    
+    if d<dist
+        dist=d;
+        node1=nodes1(k);
+        node2=nodes2(i);
+    end
+end
+
+% Check for the most restrictive
+i1=node1==nodes1;
+i2=node2==nodes2;
+
+r1=radiuses1(i1);
+r2=radiuses2(i2);
+
+shift=max([dist-r1, dist-r2]);
+
+% Add safety coefficient
+shift=(1+5e1*Data.Contact.h)*shift;
+
+r=shift/dist*(coord2(i2,:)-coord1(i1,:));
+
+% Change the coordinates of all nodes of body 1
+% !! Assumes body 1 has the smallest material tag !!
+coord=Mesh.coord_mat;
+mt=Mesh.material_tag;
+tags=unique(mt);
+N1=unique(Mesh.connect_mat(mt==tags(1),:));
+coord(N1,:)=coord(N1,:)+r;
+Mesh.coord_mat=coord;
+
+coord1=coord(nodes1,:);
+coord2=coord(nodes2,:);
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_large/updateFields.m b/Code/INTERNODES_large/updateFields.m
new file mode 100644
index 0000000..b0ae6a0
--- /dev/null
+++ b/Code/INTERNODES_large/updateFields.m
@@ -0,0 +1,23 @@
+function[Param]=updateFields(Param, param)
+
+% updateFields: Updates the fields of a structure based on the fields of
+% another structure
+% INPUT:
+% Param: Structure containing all the default parameters
+% param: Structure containing some overriden parameters
+% OUTPUT:
+% Param: Structure with updated fields
+
+fields_in=fieldnames(param);
+defaults=fieldnames(Param);
+
+for k=1:length(fields_in)
+    field=fields_in{k};
+    
+    if any(contains(defaults, field))
+        Param.(field)=param.(field);
+    else
+        error('Undefined parameter, please check spelling')
+    end
+end
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_large/updateInterface.m b/Code/INTERNODES_large/updateInterface.m
new file mode 100644
index 0000000..7ce3672
--- /dev/null
+++ b/Code/INTERNODES_large/updateInterface.m
@@ -0,0 +1,45 @@
+function[Data, nb_nodes]=updateInterface(Mesh, Data, body_tag, nodes, operation)
+
+% updateInterface: Updates the interface properties
+% INPUT:
+% Mesh:         Structure containing all the mesh parameters
+% Data:         Structure containing all the data parameters
+% body_tag:     Tag of the body to be updated
+% nodes:        Nodes to be added or removed from the potential contact interface
+% operation:    Specifies whether nodes should be added (add) or removed (dump)             
+% OUTPUT:
+% Data:         Updated data structure with interface properties
+% nb_nodes:     Number of nodes added or removed
+
+% Retrieve initial interface connectivity matrix
+iIc=Data.body{body_tag}.initial_interface_connect;
+% Retrieve initial set of nodes
+iIn=Data.body{body_tag}.initial_nodes;
+% Retrieve current set of nodes
+In=Data.body{body_tag}.interface_nodes;
+% Update interface connectivity matrix
+switch operation
+    case 'dump'
+        new_nodes=setdiff(In, nodes);
+        indx=all(ismember(iIc,new_nodes),2);
+        new_interface_connect=iIc(indx,:);
+    case 'add'
+        new_nodes=union(In, nodes);
+        indx=all(ismember(iIc,new_nodes),2);
+        new_interface_connect=iIc(indx,:);
+end
+% Update active set of nodes
+active_set=ismember(iIn, new_nodes);
+% Update interface degrees of freedom
+new_interface_dof=Mesh.Dof_set(:,new_nodes);
+new_interface_dof=new_interface_dof(:);
+% Update body properties
+Data.body{body_tag}.interface_nodes=new_nodes(:);
+Data.body{body_tag}.active_set=active_set;
+Data.body{body_tag}.interface_dof=new_interface_dof;
+Data.body{body_tag}.interface_connect=new_interface_connect;
+% Update mapping of degrees of freedom
+[Data]=mapGlobal2Local(Data);
+% Change in the number of nodes
+nb_nodes=length(new_nodes)-length(In);
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_large/updateRHS.m b/Code/INTERNODES_large/updateRHS.m
new file mode 100644
index 0000000..f7e2a20
--- /dev/null
+++ b/Code/INTERNODES_large/updateRHS.m
@@ -0,0 +1,37 @@
+function[Data]=updateRHS(Mesh, Data)
+
+% updateRHS: Updates the right-hand side vector for the next iteration of
+% the contact algorithm
+% INPUT:
+% Mesh:     Structure containing all the mesh parameters
+% Data:     Structure containing all the data parameters
+% OUTPUT:
+% Data:     Updated data structure
+
+% Retrive mesh parameters
+% Coordinate matrix
+coord=Mesh.coord_mat;
+
+% Retrieve data parameters
+% Number of free degrees of freedom
+nf=Data.nf;
+% Number of constraints
+nc=length(Data.body{1}.interface_dof);
+% Retrieve nodes making up the interfaces
+nodes1=Data.body{1}.interface_nodes;
+nodes2=Data.body{2}.interface_nodes;
+% Retrieve coordinates of nodes making up the interfaces
+coord1=coord(nodes1,:);
+coord2=coord(nodes2,:);
+
+Diff=multiplyR12(Data, coord2)-coord1;
+
+% Initialization
+b=zeros(nf+nc,1);
+b(1:nf)=Data.bc;
+b(nf+1:nf+nc)=reshape(Diff', [], 1);
+
+% Update data structure
+Data.b=b;
+Data.nc=nc;
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_medium/assembleInterpMat.m b/Code/INTERNODES_medium/assembleInterpMat.m
new file mode 100644
index 0000000..959d585
--- /dev/null
+++ b/Code/INTERNODES_medium/assembleInterpMat.m
@@ -0,0 +1,34 @@
+function [Phi] = assembleInterpMat(coord_ref, coord_interp, radiuses)
+
+% assembleInterpMat: Assemblage of the interpolation matrices for the
+% localized radial basis functions in dimension d
+% INPUT:
+% coord_ref:    Coordinate matrix of the reference points (size m x d)
+% coord_interp: Coordinate matrix for the interpolation points (size n x d)
+% radiuses:     Vector of radiuses of the radial basis functions. It is a row
+%               vector (size 1 x m)
+% OUTPUT:
+% Phi:          Interpolation matrix
+
+l=size(radiuses,1);
+if l>1
+    radiuses=radiuses';
+end
+
+m=size(coord_ref,1);
+n=size(coord_interp,1);
+dim=size(coord_ref,2);
+
+X_ref=reshape(coord_ref,1,m,dim);
+X_interp=reshape(coord_interp,n,1,dim);
+
+X=X_interp-X_ref;
+% 2-norm (euclidean norm) along the third dimension
+N=vecnorm(X,2,3);
+indx=N<=radiuses;
+Phi=zeros(n,m);
+
+% Wendland C^2 radial basis functions
+F=(1-N./radiuses).^4.*(1+4*N./radiuses);
+Phi(indx)=F(indx);
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_medium/computeNormals.m b/Code/INTERNODES_medium/computeNormals.m
new file mode 100644
index 0000000..9a30e45
--- /dev/null
+++ b/Code/INTERNODES_medium/computeNormals.m
@@ -0,0 +1,77 @@
+function[Data]=computeNormals(Mesh, Data, Solution)
+
+% computeNormals: Computes the normal vectors for each interface
+% INPUT:
+% Mesh:     Structure containing all the mesh parameters
+% Data:     Structure containing all the data parameters
+% Solution: Structure containing the solution
+% OUTPUT:
+% Data:     Updated structure with the interface normals
+
+% Retrieve mesh data
+% Connectivity matrix
+connect=Mesh.connect_mat;
+
+% Local number of degrees of freedom (dimension in solid mechanics)
+d=Mesh.nb_dof_local;
+% Retrieve bodies
+body=Data.body;
+% Compute the normals based on the deformed structure
+def=Mesh.coord_mat+Solution.U_sort;
+
+nb_bodies=length(body);
+
+
+for k=1:nb_bodies
+    % Normal vectors are computed for all nodes in the initial potential 
+    % contact interface
+    connecti=body{k}.initial_interface_connect;
+    nodesi=body{k}.initial_nodes;
+    
+    % Reduced connectivity matrix
+    connect_red=connect(any(ismember(connect, nodesi), 2), :);
+    % Reduced set of body nodes
+    nodesb=setdiff(unique(connect_red), nodesi);  
+    n=length(nodesi);
+    m=size(connecti,1);
+    % Coordinates of the endpoints of the segments
+    coord1=def(connecti(:,1),:);
+    coord2=def(connecti(:,2),:);
+    % Tangent vectors
+    V=coord2-coord1;
+    % Segment lengths
+    L=vecnorm(V,2,2);
+    V=V./vecnorm(V,2,2);
+
+    % !! Only for a two-dimensional case !!
+    N=zeros(m, d);
+    N(:,1)=-V(:,2);
+    N(:,2)=V(:,1);
+    
+    % Initialization
+    normals=zeros(n,d);
+    % Step size
+    gamma=1e-3;
+    
+    for j=1:n
+        [id,~]=find(connecti==nodesi(j));
+        l=L(id);
+        % Weighted average of normal vectors
+        normals(j,:)=1/sum(l)*sum(N(id,:).*l,1);
+        % Current node
+        x=def(nodesi(j),:);
+        % Step in the direction of the gradient
+        y_plus=x+gamma*normals(j,:);
+        % Step in the opposite direction of the gradient
+        y_minus=x-gamma*normals(j,:);
+        min_plus=min(vecnorm(def(nodesb,:)-y_plus,2,2));
+        min_minus=min(vecnorm(def(nodesb,:)-y_minus,2,2));
+        if min_plus < min_minus
+            normals(j,:)=-normals(j,:);
+        end
+    end
+    
+    normals=normals./vecnorm(normals,2,2);
+    Data.body{k}.normal=normals;
+end
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_medium/computeRHS.m b/Code/INTERNODES_medium/computeRHS.m
new file mode 100644
index 0000000..b6f4112
--- /dev/null
+++ b/Code/INTERNODES_medium/computeRHS.m
@@ -0,0 +1,223 @@
+function[Data]=computeRHS(Mesh, Data)
+
+% computeRHS: Function which assembles the right-hand side consisting of
+% body forces and surface tractions
+% INPUT:
+% Mesh:             Structure containing the mesh parameters
+% Data:             Structure containing the data parameters
+% OUTPUT:
+% Data:             Updated data structure
+
+% Assemble the global vector of body forces
+[Bs]=computeBody(Mesh, Data);
+% Assemble the global vector of surface tractions
+[Bn]=computeNeumann(Mesh, Data);
+
+% Sum the contribution from body forces and Neumann boundary
+% conditions
+B=Bs+Bn;
+% Update data
+Data.F=B;
+
+    function[B]=computeBody(Mesh, Data)
+        
+        % computeBody: Assembles the component for the body forces
+        % INPUT:
+        % Mesh:     Structure containing all the mesh parameters
+        % Data:     Structure containing all the data parameters
+        % OUTPUT:
+        % B:        Global vector of body forces
+        
+        % Retrieve mesh data
+        % Local number of degrees of freedom (dimension in solid mechanics)
+        d=Mesh.nb_dof_local;
+        % Total number of degrees of freedom
+        nb_dof=Mesh.nb_dof;
+        % Coordinate matrix
+        coord=Mesh.coord_mat;
+        % Connectivity matrix
+        connect=Mesh.connect_mat;
+        % Finite element order
+        order=Mesh.order;
+        % Number of elements
+        ne=Mesh.nb_elem;
+        % Number of nodes per element
+        nne=Mesh.nb_nodes_elem;
+        % Matrix of equation numbers linking an element to the degrees of freedom
+        % associated to it.
+        Eq=Mesh.Eq;
+        
+        % Retrieve data parameters
+        % Body force vector
+        f=Data.Coefficient.f.function;
+        % Retrieve the basis functions
+        [Functions]=getFunctions(order);
+        % Retrieve basis functions
+        phi=Functions.phi;
+        
+        
+        % Retrieve quadrature points and weights
+        switch order
+            case 1
+                [points, weights] = getQuadrature(3, 'bulk');
+            case 2
+                [points, weights] = getQuadrature(4, 'bulk');
+            case 3
+                [points, weights] = getQuadrature(6, 'bulk');
+            otherwise
+                error('Not yet implemented');
+        end
+        
+        % Number of quadrature points
+        nq=length(weights);
+        s=nne*d;
+        Id=eye(d);
+        % Initialization
+        X=zeros(ne, nq, 2);
+        Q=zeros(s, nq*d);
+        
+        
+        % Loop over the Gauss points
+        for i = 1:nq
+            Phi=phi(points(i,:));
+            Q(:, (i-1)*d+1:i*d)=kron(Phi, Id);
+        end
+        
+        I=Eq';
+        T=connect';
+        G=T(:);
+        
+        % Coordinates of the nodes of the elements
+        coord_nodes=coord(G,:);
+        % Vertices of the elements
+        a=coord_nodes(1:nne:end,:);
+        b=coord_nodes(2:nne:end,:);
+        c=coord_nodes(3:nne:end,:);
+        % Interpolated coordinates
+        P=a(:)+[b(:)-a(:) c(:)-a(:)]*points';
+        X(:,:,1)=P(1:ne,:);
+        X(:,:,2)=P(ne+1:end,:);
+        
+        % Determinants of Jacobian matrices
+        detJK=(b(:,1)-a(:,1)).*(c(:,2)-a(:,2))-(b(:,2)-a(:,2)).*(c(:,1)-a(:,1));
+        
+        % Evaluation at the Gauss points
+        F=f(X);
+        if size(F,1)>d
+            F=reshape(F, ne, d*nq);
+            F=reshape(F', d, nq, ne);
+        end
+        
+        X=reshape(abs(detJK), 1, 1, ne).*F.*weights;
+        X=reshape(X, d*nq, ne);
+        
+        B=Q*X;
+        
+        % Assemblage
+        B=accumarray(I(:), B(:), [nb_dof 1]);
+    end
+
+    function[B]=computeNeumann(Mesh, Data)
+        
+        % computeNeumann: Implements Neumann boundary conditions, returns a vector
+        % having for length the number of degrees of freedom.
+        % This vector is to be added to the RHS.
+        % INPUT:
+        % Mesh:     Structure containing all the mesh parameters
+        % Data:     Structure containing all the data parameters
+        % OUTPUT:
+        % B:        Global vector of surface tractions
+        
+        % Retrieve mesh parameters
+        % Total number of degrees of freedom
+        nb_dof=Mesh.nb_dof;
+        % Number of local degrees of freedom
+        d=Mesh.nb_dof_local;
+        % Coordinate matrix
+        coord=Mesh.coord_mat;
+        % Order
+        order=Mesh.order;
+        % Boundary equation numbers
+        Eq_BC=Mesh.Eq_BC;
+        % Retrive functions
+        [Functions]=getFunctions(order);
+        % Retrieve boundary basis functions
+        phi_boundary=Functions.phi_boundary;
+        % Get quadrature
+        [points, weights]=getQuadrature(3, 'boundary');
+        % Number of quadrature points
+        nq=length(weights);
+        
+        % Initialization
+        B=zeros(nb_dof,1);
+        s=order+1;
+        Id=eye(d);
+        % Initialization
+        Q=zeros(s*d, nq*d);
+        
+        % Loop over the Gauss points
+        for i = 1:nq
+            Phi=phi_boundary(points(i,:));
+            Q(:, (i-1)*d+1:i*d)=kron(Phi, Id);
+        end
+        
+        
+        % Neumann boundary conditions
+        N_BC=Data.N_BC;
+        % Number of boundaries where surface tractions are imposed
+        nb_bc=length(N_BC);
+        
+        for m=1:nb_bc
+            % Retrieve data
+            data=N_BC{m};
+            edges=data.edges;
+            
+            for flag=edges
+                % Surface traction function
+                f=data.function;
+                % Retriveve edges making up the boundary
+                Neum_edges=Mesh.BC_nodes(Mesh.BC_tag==flag,:);
+                % Equation numbers
+                Eq=Eq_BC(Mesh.BC_tag==flag,:);
+                % Number of Neumann boundary edges
+                ne=size(Neum_edges, 1);
+                
+                % Initialization
+                X=zeros(ne, nq, 2);
+                
+                C=Neum_edges';
+                G=C(:);
+                I=Eq';
+                
+                % Coordinates of the nodes of the elements
+                coord_nodes=coord(G,:);
+                % Endpoints of the edges
+                a=coord_nodes(1:s:end,:);
+                b=coord_nodes(2:s:end,:);
+                bk=b-a;
+                norm_bk=vecnorm(bk,2,2);
+                
+                % Interpolated coordinates
+                P=a(:)+(b(:)-a(:))*points';
+                X(:,:,1)=P(1:ne,:);
+                X(:,:,2)=P(ne+1:end,:);
+                
+                % Evaluation at the Gauss points
+                F=f(X);
+                if size(F,1)>d
+                    F=reshape(F, ne, d*nq);
+                    F=reshape(F', d, nq, ne);
+                end
+                
+                X=reshape(norm_bk, 1, 1, ne).*F.*weights';
+                X=reshape(X, d*nq, ne);
+                
+                Bl=Q*X;
+                
+                % Assemblage
+                B=B+accumarray(I(:), Bl(:), [nb_dof 1]);
+                
+            end
+        end
+    end
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_medium/computeStiffness.m b/Code/INTERNODES_medium/computeStiffness.m
new file mode 100644
index 0000000..6091151
--- /dev/null
+++ b/Code/INTERNODES_medium/computeStiffness.m
@@ -0,0 +1,138 @@
+function[K]=computeStiffness(Mesh, Data)
+
+% computeStiffness: Function which assembles the global stiffness matrix
+% Assumes all elements are of the same type
+% INPUT:
+% Mesh:     Structure containing all the mesh parameters
+% Data:     Structure containing all the data parameters
+% OUTPUT:
+% K:        Global stiffness matrix
+
+% Mesh parameters
+% Local number of degrees of freedom (dimension in solid mechanics)
+d=Mesh.nb_dof_local;
+% Total number of degrees of freedom
+nb_dof=Mesh.nb_dof;
+% Coordinate matrix
+coord=Mesh.coord_mat;
+% Connectivity matrix
+connect=Mesh.connect_mat;
+% Finite element order
+order=Mesh.order;
+% Number of nodes per element
+nne=Mesh.nb_nodes_elem;
+% Total number of elements
+ne=Mesh.nb_elem;
+% Matrix linking an element to the degrees of freedom associated to it.
+Eq=Mesh.Eq;
+
+% Data parameters
+% Mu
+f1=Data.Coefficient.mu.function;
+% Lambda
+f2=Data.Coefficient.lambda.function;
+
+
+% Retrieve functions
+[Functions]=getFunctions(order);
+% Jacobian matrix
+J_phi=Functions.J_phi;
+% Define permutation matrix
+Sdd=zeros(d*d);
+Id=eye(d);
+for k=1:d
+    Sdd=Sdd+kron(Id(:,k)', kron(Id, Id(:,k)));
+end
+
+% Retrieve quadrature nodes and weights
+switch order
+    case 1
+        [points, weights] = getQuadrature(1, 'bulk');
+    case 2
+        [points, weights] = getQuadrature(2, 'bulk');
+    case 3
+        [points, weights] = getQuadrature(3, 'bulk');
+    otherwise
+        error('Not yet implemented');
+end
+
+% Initialization
+s=nne*d;
+nq=length(weights);
+Q=zeros(s^2, nq*d^4);
+
+% Loop over the Gauss points
+for i = 1:nq
+    L=kron(J_phi(points(i,:)), Id);
+    Q(:, (i-1)*d^4+1:i*d^4)=kron(L, L);
+end
+
+T=Eq';
+G=T(:);
+
+I=repmat(Eq, [1 nne*d])';
+J=repmat(G', [nne*d 1]);
+
+T=connect';
+G=T(:);
+
+% Coordinates of the nodes of the elements
+coord_nodes=coord(G,:);
+% Vertices of the elements
+a=coord_nodes(1:nne:end,:);
+b=coord_nodes(2:nne:end,:);
+c=coord_nodes(3:nne:end,:);
+% Interpolated coordinates
+P=a(:)+[b(:)-a(:) c(:)-a(:)]*points';
+X(:,:,1)=P(1:ne,:);
+X(:,:,2)=P(ne+1:end,:);
+
+% Determinants of Jacobian matrices
+detJK=(b(:,1)-a(:,1)).*(c(:,2)-a(:,2))-(b(:,2)-a(:,2)).*(c(:,1)-a(:,1));
+
+V=[(vecnorm(c-a,2,2).^2) -sum((c-a).*(b-a),2) -sum((c-a).*(b-a),2) (vecnorm(b-a,2,2).^2)];
+W=1./(detJK').^2.*(V');
+
+V=reshape(W,d,d*ne);
+
+JK_inv=zeros(d, d*ne);
+JK_inv(:,1:d:d*ne)=1./detJK'.*[c(:,2)-a(:,2) -(b(:,2)-a(:,2))]';
+JK_inv(:,2:d:d*ne)=1./detJK'.*[-(c(:,1)-a(:,1)) b(:,1)-a(:,1)]';
+
+JK_invT=zeros(d, d*ne);
+JK_invT(:,1:d:d*ne)=1./detJK'.*[c(:,2)-a(:,2) -(c(:,1)-a(:,1))]';
+JK_invT(:,2:d:d*ne)=1./detJK'.*[-(b(:,2)-a(:,2)) b(:,1)-a(:,1)]';
+
+vec_JK_invT=reshape(JK_invT, d^2, ne);
+
+[S1]=product(V , repmat(Id, 1, ne), d, ne);
+[S2]=product(JK_invT , JK_inv, d, ne);
+S2=Sdd*S2;
+
+A1=reshape(S1, d^4, ne);
+A2=reshape(S2, d^4, ne);
+A3=kr(vec_JK_invT, vec_JK_invT);
+
+
+Lambda1=(abs(detJK).*f1(X).*weights)';
+Lambda2=(abs(detJK).*f2(X).*weights)';
+
+X=kr(Lambda1, A1+A2)+kr(Lambda2, A3);
+K=Q*X;
+
+% Assemblage
+K=sparse(I(:), J(:), K(:), nb_dof, nb_dof);
+
+    function[M]=product(A,B,d,ne)
+        
+        % Blockwise Kronecker product of two matrices
+        % !! Only for a two-dimensional case !!
+        M=zeros(d^2, d^2*ne);
+        
+        M(:,1:d^2:end)=kr(A(:,1:d:end), B(:,1:d:end));
+        M(:,2:d^2:end)=kr(A(:,1:d:end), B(:,2:d:end));
+        M(:,3:d^2:end)=kr(A(:,2:d:end), B(:,1:d:end));
+        M(:,4:d^2:end)=kr(A(:,2:d:end), B(:,2:d:end));
+    end
+
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_medium/detectGaps.m b/Code/INTERNODES_medium/detectGaps.m
new file mode 100644
index 0000000..ffe94c0
--- /dev/null
+++ b/Code/INTERNODES_medium/detectGaps.m
@@ -0,0 +1,81 @@
+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
\ No newline at end of file
diff --git a/Code/INTERNODES_medium/getFunctions.m b/Code/INTERNODES_medium/getFunctions.m
new file mode 100644
index 0000000..ddff9aa
--- /dev/null
+++ b/Code/INTERNODES_medium/getFunctions.m
@@ -0,0 +1,46 @@
+function[Functions]=getFunctions(order)
+
+% getFunctions: Returns the basis functions, their jacobian matrices and
+% the basis functions needed to integrate over the boundary
+% INPUT:
+% order:        Finite element order
+% OUTPUT:
+% Functions:    Structure containing all the functions
+
+
+switch order
+    case 1
+Functions.phi=@(x) [1-x(1)-x(2);...
+                    x(1);...
+                    x(2)];
+                
+Functions.J_phi=@(x) [-1 -1; ...
+                       1  0;...
+                       0  1]; 
+                   
+Functions.phi_boundary=@(x) [1-x;...
+                             x];
+    case 2
+Functions.phi=@(x) [(1-x(1)-x(2))*(1-2*x(1)-2*x(2));...
+                    x(1)*(2*x(1)-1);...
+                    x(2)*(2*x(2)-1);...
+                    4*x(1)*(1-x(1)-x(2));...
+                    4*x(1)*x(2);...
+                    4*x(2)*(1-x(1)-x(2))];
+                 
+                                                       
+Functions.J_phi=@(x) [-3+4*x(2)+4*x(1) -3+4*x(1)+4*x(2); ...
+                       -1+4*x(1)        0;...
+                       0                -1+4*x(2);...
+                       4-4*x(2)-8*x(1)  -4*x(1);...
+                       4*x(2)           4*x(1);...
+                       -4*x(2)          4-4*x(1)-8*x(2)];
+
+Functions.phi_boundary=@(x) [(2*x-1)*(x-1);...
+                            x*(2*x-1);...
+                            4*x*(1-x)];
+               
+    otherwise
+        error('Unimplemented!');
+end
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_medium/getParameters.m b/Code/INTERNODES_medium/getParameters.m
new file mode 100644
index 0000000..4a0812c
--- /dev/null
+++ b/Code/INTERNODES_medium/getParameters.m
@@ -0,0 +1,72 @@
+function[Mesh]=getParameters(Mesh)
+
+% getParameters: Retrieves mesh parameters
+% INPUT:
+% Mesh: Structure containing the mesh geometry
+% OUTPUT:
+% Mesh: Updated structure containing all mesh parameters needed to run the code
+
+% Retrieve mesh parameters
+% Coordinate matrix
+coord=Mesh.coord_mat;
+% Connectivity matrix
+connect=Mesh.connect_mat;
+% Boundary nodes
+BC_nodes=Mesh.BC_nodes;
+
+% Geometry [m] 
+x_min=min(coord(:,1));
+x_max=max(coord(:,1));
+y_min=min(coord(:,2));
+y_max=max(coord(:,2));
+% Maximum dimension [m]
+L_max=max([abs(x_max-x_min) abs(y_max-y_min)]);
+% Number of elements
+nb_elem=size(connect,1);
+% Number of nodes per element
+nb_nodes_elem=size(connect,2);
+% Number of nodes
+nb_nodes=size(coord,1);
+% Local number of degrees of freedom
+nb_dof_local=size(coord,2);
+% Total number of degrees of freedom
+nb_dof=nb_dof_local*nb_nodes;
+% Number of boundary nodes
+nb_BC_nodes=size(BC_nodes,1);
+
+% Order of polynomials
+% It is assumed all elements of the mesh are of same type
+order_map=containers.Map([3 6 10], [1 2 3]);
+order=order_map(nb_nodes_elem);
+
+% Initialization
+% Matrix of equation numbers   
+Eq=zeros(nb_elem,nb_dof_local*nb_nodes_elem);
+% Matrix of equation number for boundary nodes;
+Eq_BC=zeros(nb_BC_nodes,nb_dof_local*order);
+% Matrix containing the sets of degrees of freedom for each node
+Dof_set=zeros(nb_dof_local, nb_nodes);
+
+% Iteration over the dimension
+for k=1:nb_dof_local
+    d1=k:nb_dof_local:nb_dof_local*nb_nodes_elem;
+    d2=k:nb_dof_local:nb_dof_local*(order+1);
+    Eq(:,d1)=nb_dof_local*connect-nb_dof_local+k;
+    Eq_BC(:,d2)=nb_dof_local*BC_nodes-nb_dof_local+k;
+    Dof_set(k,:)=nb_dof_local*(1:nb_nodes)-nb_dof_local+k;
+end
+
+% Update mesh structure
+Mesh.frame=[x_min y_min; x_max y_max];
+Mesh.L_max=L_max;
+Mesh.nb_elem=nb_elem;
+Mesh.nb_nodes_elem=nb_nodes_elem;
+Mesh.nb_nodes=nb_nodes;
+Mesh.nb_dof_local=nb_dof_local;
+Mesh.nb_dof=nb_dof;
+Mesh.order=order;
+Mesh.Eq=Eq;
+Mesh.Eq_BC=Eq_BC;
+Mesh.nb_BC_nodes=nb_BC_nodes;
+Mesh.Dof_set=Dof_set;
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_medium/getQuadrature.m b/Code/INTERNODES_medium/getQuadrature.m
new file mode 100755
index 0000000..706128e
--- /dev/null
+++ b/Code/INTERNODES_medium/getQuadrature.m
@@ -0,0 +1,362 @@
+function[points, weights] = getQuadrature(id, object)
+
+% getQuadrature: Function which returns the integration points and
+% weights for a given quadrature ID
+% INPUT:
+% id:       Integration ID
+% OUTPUT:
+% points:   Integration points
+% weights:  Integration weights
+
+switch object
+    case 'bulk'
+        switch id
+            case 1
+                % Source: Wriggers, Computational contact mechanics (2006)
+                % Number of points: 1
+                % Degree of exactness: 1
+                
+                a1=1/3;                 w1=1;
+                
+                points=[a1 a1];
+                
+                weights=0.5*w1;
+                
+            case 2
+                % Source: Wriggers, Computational contact mechanics (2006)
+                % Number of points: 3
+                % Degree of exactness: 2
+                
+                a1=2/3;                 w1=1/3;
+                b1=1/6;
+                
+                points=[b1 b1;
+                        a1 b1;
+                        b1 a1];
+                
+                weights=0.5*[w1 w1 w1];
+                
+            case 3
+                % Source: Wriggers, Computational contact mechanics (2006)
+                % Number of points: 4
+                % Degree of exactness: 3
+                
+                a1=1/3;                 w1=-27/48;
+                a2=3/5;                 w2=25/48;
+                b2=1/5;
+                
+                points=[a1 a1;            
+                        b2 b2;
+                        a2 b2;
+                        b2 a2];
+                
+                weights=0.5*[w1 w2 w2 w2];
+                
+            case 4
+                % Source: Laursen and Gellert, Some criteria for
+                % numerically integrated matrices and quadrature formulas
+                % for triangles (1978)
+                % Number of points: 6
+                % Degree of exactness: 4
+                
+                a1=0.816847572980459;       w1=0.109951743655322;
+                b1=0.091576213509771;       w2=0.223381589678011;
+                a2=0.108103018168070;
+                b2=0.445948490915965;
+                
+                points=[b1 b1;
+                        a1 b1;
+                        b1 a1;
+                        b2 b2;
+                        a2 b2;
+                        b2 a2];
+                
+                weights=0.5*[w1 w1 w1 w2 w2 w2];
+                
+            case 5
+                % Source: Laursen and Gellert, Some criteria for
+                % numerically integrated matrices and quadrature formulas
+                % for triangles (1978)
+                % Number of points: 7
+                % Degree of exactness: 5
+                
+                a1=1/3;                     w1=0.225000000000000;
+                a2=0.797426985353087;       w2=0.125939180544827;
+                b2=0.101286507323456;       w3=0.132394152788506;
+                a3=0.059715871789770;
+                b3=0.470142064105115;
+                
+                points=[a1 a1;
+                        b2 b2;
+                        a2 b2;
+                        b2 a2;
+                        b3 b3;
+                        a3 b3;
+                        b3 a3];
+                    
+               weights=0.5*[w1 w2 w2 w2 w3 w3 w3];
+                    
+            case 6
+                % Source: Laursen and Gellert, Some criteria for
+                % numerically integrated matrices and quadrature formulas
+                % for triangles (1978)
+                % Number of points: 12
+                % Degree of exactness: 6
+                
+                a1=0.873821971016996;       w1=0.050844906370207;
+                b1=0.063089014491502;       w2=0.116786275726379;
+                a2=0.501426509658179;       w3=0.082851075618374;
+                b2=0.249286745170910;
+                a3=0.636502499121399;
+                b3=0.310352451033785;
+                c3=0.053145049844816;
+                
+                points=[b1 b1;
+                        b1 a1;
+                        a1 b1;
+                        b2 b2;
+                        b2 a2;
+                        a2 b2;
+                        a3 b3;
+                        a3 c3;
+                        b3 a3;
+                        b3 c3;
+                        c3 a3;
+                        c3 b3];
+                    
+               weights=0.5*[w1 w1 w1 w2 w2 w2 w3 w3 w3 w3 w3 w3];
+                 
+            case 7
+                % Source: Laursen and Gellert, Some criteria for
+                % numerically integrated matrices and quadrature formulas
+                % for triangles (1978)
+                % Number of points: 13
+                % Degree of exactness: 7
+                
+                a1=1/3;                     w1=-0.149570044467670;
+                a2=0.479308067841923;       w2= 0.175615257433204;           
+                b2=0.260345966079038;       w3= 0.053347235608839;
+                a3=0.869739794195568;       w4= 0.077113760890257;
+                b3=0.065130102902216;
+                a4=0.638444188569809;
+                b4=0.312865496004875;
+                c4=0.048690315425316;
+                
+                points=[a1 a1;
+                        b2 b2;
+                        b2 a2;
+                        a2 b2;
+                        b3 b3;
+                        b3 a3;
+                        a3 b3;
+                        a4 b4;
+                        a4 c4;
+                        b4 a4;
+                        b4 c4;
+                        c4 a4;
+                        c4 b4];
+                                                      
+                weights=0.5*[w1 w2 w2 w2 w3 w3 w3 w4 w4 w4 w4 w4 w4];
+
+            case 8
+                % Source: Laursen and Gellert, Some criteria for
+                % numerically integrated matrices and quadrature formulas
+                % for triangles (1978)
+                % Number of points: 16
+                % Degree of exactness: 8
+                
+                a1=1/3;                     w1=0.144315607677787;
+                a2=0.658861384496478;       w2=0.103217370534718;
+                b2=0.170569307751761;       w3=0.032458497623198;
+                a3=0.898905543365938;       w4=0.095091634267284;
+                b3=0.050547228317031;       w5=0.027230314174435;
+                a4=0.081414823414554;
+                b4=0.459292588292723;
+                a5=0.008394777409958;
+                b5=0.263112829634638;
+                c5=0.728492392955404;
+                
+                points=[a1 a1;
+                        b2 b2;
+                        b2 a2;
+                        a2 b2;
+                        b3 b3;
+                        b3 a3;
+                        a3 b3;
+                        b4 b4;
+                        b4 a4;
+                        a4 b4;
+                        a5 b5;
+                        a5 c5;
+                        b5 a5;
+                        b5 c5;
+                        c5 a5;
+                        c5 b5];
+                    
+                weights=0.5*[w1 w2 w2 w2 w3 w3 w3 w4 w4 w4 w5 w5 w5 w5 w5 w5];
+                
+            case 9
+                % Source: Laursen and Gellert, Some criteria for
+                % numerically integrated matrices and quadrature formulas
+                % for triangles (1978)
+                % Number of points: 19
+                % Degree of exactness: 9
+                
+                a1=1/3;                     w1=0.097135796282799;
+                a2=0.020634961602524;       w2=0.031334700227139;
+                b2=0.489682519198738;       w3=0.077827541004774;
+                a3=0.125820817014126;       w4=0.079647738927210;
+                b3=0.437089591492937;       w5=0.025577675658698;
+                a4=0.623592928761934;       w6=0.043283539377289;
+                b4=0.188203535619033;
+                a5=0.910540973211094;
+                b5=0.044729513394453;
+                a6=0.036838412054736;
+                b6=0.221962989160766;
+                c6=0.741198598784498;
+                
+                points=[a1 a1;
+                        b2 b2;
+                        b2 a2;
+                        a2 b2;
+                        b3 b3;
+                        b3 a3;
+                        a3 b3;
+                        b4 b4;
+                        b4 a4;
+                        a4 b4;
+                        b5 b5;
+                        b5 a5;
+                        a5 b5;
+                        a6 b6;
+                        a6 c6;
+                        b6 a6;
+                        b6 c6;
+                        c6 a6;
+                        c6 b6];
+              
+             weights=0.5*[w1 w2 w2 w2 w3 w3 w3 w4 w4 w4 w5 w5 w5 w6 w6 w6 w6 w6 w6];       
+                    
+            case 10
+                % Source: Laursen and Gellert, Some criteria for
+                % numerically integrated matrices and quadrature formulas
+                % for triangles (1978)
+                % Number of points: 25
+                % Degree of exactness: 10
+                  
+                a1=1/3;                     w1=0.081743329146286;
+                a2=0.715677797886872;       w2=0.045957963604745;
+                b2=0.142161101056564;       w3=0.013352968813150;
+                a3=0.935889253566112;       w4=0.063904906396424;
+                b3=0.032055373216944;       w5=0.034184648162959;
+                a4=0.148132885783821;       w6=0.025297757707288;
+                b4=0.321812995288835;
+                c4=0.530054118927344;
+                a5=0.029619889488730;
+                b5=0.369146781827811;
+                c5=0.601233328683459;
+                a6=0.028367665339938;
+                b6=0.163701733737182;
+                c6=0.807930600922880;
+                 
+                points=[a1 a1;
+                        b2 b2;
+                        a2 b2;
+                        b2 a2;
+                        b3 b3;
+                        a3 b3;
+                        b3 a3;
+                        a4 b4;
+                        a4 c4;
+                        b4 a4;
+                        b4 c4;
+                        c4 a4;
+                        c4 b4;
+                        a5 b5;
+                        a5 c5;
+                        b5 a5;
+                        b5 c5;
+                        c5 a5;
+                        c5 b5;
+                        a6 b6;
+                        a6 c6;
+                        b6 a6;
+                        b6 c6;
+                        c6 a6;
+                        c6 b6];
+                                    
+                weights=0.5*[w1 w2 w2 w2 w3 w3 w3 w4 w4 w4 w4 w4 w4 w5 w5 w5 w5 w5 w5 w6 w6 w6 w6 w6 w6];           
+                
+            otherwise
+                error('Unimplemented');
+        end
+        
+    case 'boundary'
+        
+        [QuadRule] = getGaussLegendre(0, 1, id);
+        
+        % Quadrature nodes
+        points=QuadRule.x;
+        % Quadrature weights
+        weights=QuadRule.w;
+        
+    case 'interpolation'
+        
+        e=1e-6;
+        
+        switch id
+            case 3
+                points=[e e
+                        1-e e
+                        e 1-e];
+                
+            case 6
+                points=[e e
+                        1-e e
+                        e 1-e
+                        1/3 1/3
+                        2/3 1/3
+                        1/3 2/3];
+                
+        end
+        
+        weights=[];
+end
+
+
+    function [QuadRule] = getGaussLegendre(a,b,n)
+        
+        % getGaussLegendre: 1D n-point Gauss-Legendre quadrature rule on the interval [a,b].
+        % Quadrature rules obtained from Gauss-Legendre are of order 2*n-1.
+        % INPUT:
+        % a,b:          End-points of the interval
+        % n:            Number of quadrature nodes in [a,b]
+        % OUTPUT:
+        % QuadRule:     Structure which contains the fields:
+        %    x:         N-by-1 matrix specifying the abscissae of the quadrature rule.
+        %    w:         N-by-1 matrix specifying the weights of the quadrature rule.
+        
+        if (n==1), x = 0; w = 2;
+        else
+            c = zeros(n-1,1);
+            for i=1:(n-1), c(i)=i/sqrt(4*i*i-1); end
+            J=diag(c,-1)+diag(c,1); [ev,ew]=eig(J);
+            x=diag(ew); w=(2*(ev(1,:).*ev(1,:)))';
+        end
+        
+        % The above rule computes a quadrature rule for the reference interval.
+        % The reference interval is always [-1,1], and if [a,b] != [-1,1] then we
+        % re-map the abscissas (x) and rescale the weights.
+        
+        % Midpoint
+        xm = (b+a)/2;
+        % Area of the requested interval / area of the reference interval
+        xl = (b-a)/2;
+        
+        x = xm + xl*x;
+        w = w * xl;
+        
+        QuadRule.w = w(:);
+        QuadRule.x = x(:);
+    end
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_medium/getRadius.m b/Code/INTERNODES_medium/getRadius.m
new file mode 100644
index 0000000..242ea5b
--- /dev/null
+++ b/Code/INTERNODES_medium/getRadius.m
@@ -0,0 +1,101 @@
+function[r, nnzRMM, nnzCMM, nnzRNM, nnzCNM]=getRadius(Data, nodesM, nodesN, coordM, coordN)
+
+% getRadius: Computes the radiuses of the radial basis functions defined on 
+% the interfaces.
+% INPUT:
+% Data:         Structure containing all the data parameters
+% nodesM:       Set of interpolation nodes
+% nodesN:       Set of evaluation nodes
+% coordM:       Coordinate matrix for the interpolation nodes
+% coordN:       Coordinate matrix for the evaluation nodes
+% OUTPUT:
+% r:            Radiuses of radial basis functions
+% nnzRMM:       Number of off-diagonal nonzeros in each row of PhiMM
+% nnzCMM:       Number of off-diagonal nonzeros in each column of PhiMM
+% nnzRNM:       Number of nonzeros in each row of PhiNM
+% nnzCNM:       Number of nonzeros in each column of PhiNM
+
+M=length(nodesM);
+N=length(nodesN);
+r=zeros(M,1);
+
+% Parameters
+% gamma parameter: real number between ]0,1[
+% Large values of gamma result in small supports
+gamma=Data.Contact.gamma;
+% Gamma parameter: real number between ]0,1[ with Gamma>gamma
+% Used to ensure evaluation points are well within the
+% support of radial basis functions (far enough from the boundary)
+Gamma=Data.Contact.Gamma;
+% Tolerance for contact detection
+% If the tolerance is unknown, set it to some small value.
+d0=Data.Contact.d0;
+% Number of closest neighbors on current interface
+c=Data.Contact.c;
+
+% Number of off-diagonal nonzeros per row of PhiMM
+nnzRMM=zeros(M,1);
+% Number of off-diagonal nonzeros per column of PhiMM
+nnzCMM=zeros(M,1);
+% Number of nonzeros per row of PhiNM
+nnzRNM=zeros(N,1);
+% Number of nonzeros per column of PhiNM
+nnzCNM=zeros(M,1);
+
+maxS=inf;
+f=0;
+niter=0;
+maxiter=10;
+
+% Version 2
+while maxS>f && niter<maxiter
+    % Maximum number of supports to which an evaluation point may belong.
+    % Condition for the matrix PhiMM to be diagonally dominant.
+    f=floor(1./((1-gamma).^4.*(1+4*gamma)));
+    
+    for k=1:M
+        point=coordM(k,:);
+        neighbors=coordM;
+        neighbors(k,:)=inf;
+        distMM=vecnorm(neighbors-point,2,2);
+        distMN=vecnorm(coordN-point,2,2);
+        
+        rMM=mink(distMM,c);
+        rNM=sqrt(d0^2+0.25*rMM(end)^2);
+        radius=max([rMM(end) rNM]);
+        
+        % Make sure the closest neighbor is at a distance >= gamma*radius.
+        % If the point is too close (with respect to the radius), diagonal
+        % dominance will be lost very quickly.
+        if radius>rMM(1)/gamma
+            radius=rMM(1)/gamma;
+        end
+        
+        s1=distMM<radius;
+        s2=distMN<Gamma*radius;
+        
+        nnzRMM(s1)=nnzRMM(s1)+1;
+        nnzRNM(s2)=nnzRNM(s2)+1;
+        nnzCMM(k)=sum(s1);
+        nnzCNM(k)=sum(s2);
+        
+        r(k)=radius;
+    end
+    % Compute the maximum number of supports to which a point belongs
+    maxS=max(nnzRMM);
+    if maxS>f
+        % Increase gamma (sequence converges to 1)
+        gamma=0.5*(1+gamma);
+        % Reinitialize counters
+        nnzRMM=zeros(M,1);
+        nnzCMM=zeros(M,1);
+        nnzRNM=zeros(N,1);
+        nnzCNM=zeros(M,1);
+        niter=niter+1;
+    end
+end
+
+if niter==maxiter && maxS>f
+    warning('The maximum number of rounds for radius computations was reached. Try decreasing d0')
+end
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_medium/gmres_k.m b/Code/INTERNODES_medium/gmres_k.m
new file mode 100644
index 0000000..793689d
--- /dev/null
+++ b/Code/INTERNODES_medium/gmres_k.m
@@ -0,0 +1,112 @@
+function[x, residual] = gmres_k(Mesh, Data, linearOp, b, x0)
+
+% gmres_k: Restarted GMRES algorithm
+% INPUT:
+% Mesh:     Structure containing all the mesh parameters
+% Data:     Structure containing all the data parameters
+% linearOp: Linear Operator implementing matrix-vector multiplication and
+%           resolution of linear systems with the preconditioner
+% b:        Right-hand side vector
+% x0:       Starting vector
+% OUTPUT:
+% x:        Approximate solution
+% residual: Vector containing the norms of the residuals
+
+
+% Maximum total number of iterations
+maxiter=Data.Solver.maxiter;
+% Number of iterations before restart
+riter=Data.Solver.riter;
+% Reorthogonalization tolerance
+reorth_tol=Data.Solver.reorth_tol;
+% Tolerance on the norm of the residual
+tol=Data.Solver.tol;
+
+% Initialization
+x=x0;
+res_error=1;
+budget=maxiter;
+residual=[];
+
+while res_error>tol && budget>0
+    [x, beta1, iter] = gmresPrec(b, x, min([riter, budget]), reorth_tol, tol);
+    residual=[residual; beta1];
+    res_error=residual(end);
+    budget=budget-iter;
+end
+
+if res_error>tol && budget==0
+    disp(['The restarted GMRES method did not converge after ' num2str(maxiter-budget) ' iterations'])
+end
+
+
+
+    function [x, varargout] = gmresPrec(b, x0, riter, reorth_tol, tol)
+        
+        % gmresPrec: Right preconditioned GMRES
+        % INPUT:
+        % b:            Right-hand side vector
+        % x0:           Starting vector
+        % riter:        Maximum number of iterations before restart
+        % reorth_tol:   Reorthogonalization tolerance
+        % tol:          Tolerance on the norm of the residual
+        % OUTPUT:
+        % x:            Approximate solution
+        % res:          Vector containing the norms of the residuals
+        % k:            Number of iterations
+        % U:            Orthonormal Krylov basis
+        % H:            Upper Hessenberg matrix
+        
+        
+        r0=b-linearOp(Mesh, Data, x0, 'matvec');
+        U=zeros(length(r0), riter);
+        H=zeros(riter+1, riter);
+        res=zeros(riter+1,1);
+        res(1)=norm(r0);
+        U(:,1)=r0/res(1);
+        
+        
+        for k=1:riter
+            w=linearOp(Mesh, Data, U(:,k), 'solve');
+            h=U(:,1:k)'*w;
+            u_tilde=w-U(:,1:k)*h;
+            
+            if norm(u_tilde)<reorth_tol*norm(w)
+                h_hat=U(:,1:k)'*u_tilde;
+                h=h+h_hat;
+                u_tilde=u_tilde-U(:,1:k)*h_hat;
+            end
+            
+            h_tilde=norm(u_tilde);
+            U(:,k+1)=u_tilde/h_tilde;
+            H(1:(k+1) ,k)=[h; h_tilde];
+            
+            % Be careful: parentheses are crucial
+            y=res(1)*(H(1:k+1,1:k)\eye(k+1,1));
+            res(k+1)=norm(res(1)*eye(k+1,1)-H(1:k+1,1:k)*y);
+            
+            if res(k+1)<tol
+                v=linearOp(Mesh, Data, U(:,1:k)*y, 'solvePrec');
+                x=x0+v;
+                flag=1;
+                break
+            else
+                flag=0;
+            end
+            
+        end
+        
+        
+        if ~flag
+            v=linearOp(Mesh, Data, U(:,1:k)*y, 'solvePrec');
+            x=x0+v;
+        end
+        
+        
+        varargout{1}=res(1:k+1);
+        varargout{2}=k;
+        varargout{3}=U(:,1:k);
+        varargout{4}=H(1:k,1:k);
+    end
+
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_medium/initializeInterface.m b/Code/INTERNODES_medium/initializeInterface.m
new file mode 100644
index 0000000..9b4e1bf
--- /dev/null
+++ b/Code/INTERNODES_medium/initializeInterface.m
@@ -0,0 +1,107 @@
+function[Data]=initializeInterface(Data, Mesh, it)
+
+% initializeInterface: Initializes the interfaces properties
+% INPUT:
+% Mesh:     Structure containing all the mesh parameters
+% Data:     Structure containing all the data parameters
+% it:       Boundary tags for either a portion of the boundary or the entire boundary
+%           The first component is the tag for body 1
+%           The second component is the tag for body 2
+% OUTPUT:
+% Data:     Updated data structure with interface properties
+
+% Retrieve mesh parameters
+% Retrieve boundary tag
+BC_tag=Mesh.BC_tag;
+% Retrieve coordinate matrix
+coord=Mesh.coord_mat;
+% Define radius of intersection
+if isfield(Data.Contact, 'radius')
+    r=Data.Contact.radius;
+else
+    r=0.052;
+end
+
+% Boundary elements
+elements1=Mesh.BC_nodes(ismember(BC_tag,it{1}),:);
+elements2=Mesh.BC_nodes(ismember(BC_tag,it{2}),:);
+
+% Boundary nodes
+bc_nodes1=unique(elements1);
+bc_nodes2=unique(elements2);
+
+m1=length(bc_nodes1);
+m2=length(bc_nodes2);
+
+% Compute element lengths
+l1=vecnorm(coord(elements1(:,2),:)-coord(elements1(:,1),:),2,2);
+l2=vecnorm(coord(elements2(:,2),:)-coord(elements2(:,1),:),2,2);
+
+% Compute mesh sizes
+h1=min(l1);
+h2=min(l2);
+
+body_1.h=h1;
+body_2.h=h2;
+
+h=min([h1, h2]);
+Data.Contact.h=h;
+
+% Indicators
+ic1=zeros(m1,1);
+ic2=zeros(m2,1);
+
+% Create interfaces based on closest nodes from opposing interface
+for k=1:m1
+    node=bc_nodes1(k);
+    coord_node=coord(node,:);
+    dist=vecnorm(coord(bc_nodes2,:)-coord_node,2,2);
+    ic1(k)=any(dist<r);
+    
+end
+for k=1:m2
+    node=bc_nodes2(k);
+    coord_node=coord(node,:);
+    dist=vecnorm(coord(bc_nodes1,:)-coord_node,2,2);
+    ic2(k)=any(dist<r);
+end
+
+ic1=logical(ic1);
+ic2=logical(ic2);
+
+interface1=bc_nodes1(ic1);
+interface2=bc_nodes2(ic2);
+
+% Correct interface nodes to account for finite elements
+index1=all(ismember(Mesh.BC_nodes, interface1), 2);
+index2=all(ismember(Mesh.BC_nodes, interface2), 2);
+% Get interface connectivity matrices
+body_1.interface_connect=Mesh.BC_nodes(index1,:);
+body_2.interface_connect=Mesh.BC_nodes(index2,:);
+% Get interface nodes
+body_1.interface_nodes=unique(body_1.interface_connect);
+body_2.interface_nodes=unique(body_2.interface_connect);
+% Get interface degrees of freedom
+interface_dof_1=Mesh.Dof_set(:,body_1.interface_nodes);
+interface_dof_2=Mesh.Dof_set(:,body_2.interface_nodes);
+body_1.interface_dof=interface_dof_1(:);
+body_2.interface_dof=interface_dof_2(:);
+% Initial active set
+body_1.initial_nodes=body_1.interface_nodes;
+body_2.initial_nodes=body_2.interface_nodes;
+% Initial connectivity matrix
+body_1.initial_interface_connect=body_1.interface_connect;
+body_2.initial_interface_connect=body_2.interface_connect;
+% Active set
+body_1.active_set=ismember(body_1.initial_nodes, body_1.interface_nodes);
+body_2.active_set=ismember(body_2.initial_nodes, body_2.interface_nodes);
+
+% Initialization
+body=cell(2,1);
+% Create body entity
+body{1}=body_1;
+body{2}=body_2;
+Data.body=body;
+% Map global dof to local ones
+[Data]=mapGlobal2Local(Data);
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_medium/initializeSystem.m b/Code/INTERNODES_medium/initializeSystem.m
new file mode 100644
index 0000000..1b03d71
--- /dev/null
+++ b/Code/INTERNODES_medium/initializeSystem.m
@@ -0,0 +1,91 @@
+function[Data, Solution]=initializeSystem(Mesh, Data, it)
+
+% initializeSystem: Initializes the interface and computes all quantities
+% which do not change during the course of the contact algorithm
+% INPUT:
+% Mesh:     Structure containing all the mesh parameters
+% Data:     Structure containing all the data parameters
+% it:       Cell array containing the boundary tags of the interfaces
+% OUTPUT:
+% Data:     Updated structure containing all the data parameters
+% Solution: Initialized solution structure
+
+% Compute the Dirichlet BC
+[Solution]=computeBC(Data, Mesh);
+% Define the interface
+[Data]=initializeInterface(Data, Mesh, it);
+% Assemblage of the stiffness matrix
+[K]=computeStiffness(Mesh, Data);
+% Pre-processing
+[Data]=preProcess(Data, Solution, K);
+
+% Sub-assemblage
+[Data]=subAssemble(Mesh, Data, Solution, 'initialization');
+% Compute right-hand side vector
+[Data]=updateRHS(Mesh, Data);
+
+
+    function[Solution]=computeBC(Data, Mesh)
+        
+        % computeBC: Function which computes the displacement values at
+        % Dirichlet BC nodes
+        % INPUT:
+        % Data:     Structure containing all the data parameters
+        % Mesh:     Structure containing all the mesh parameters
+        % OUTPUT:
+        % Solution: Structure containing the solution, including displacements
+        %           evaluated at Dirichlet nodes
+        
+        % Retrieve mesh parameters
+        % Coordinate matrix
+        coord=Mesh.coord_mat;
+        % Matrix containing the sets of degrees of freedom for each node
+        Dof_set=Mesh.Dof_set;
+        % Total number of degrees of freedom
+        nb_dof=Mesh.nb_dof;
+        % Local number of degrees of freedom
+        nb_dof_local=Mesh.nb_dof_local;
+        
+        % Retrieve data
+        % Dirichlet boundary conditions
+        D_BC=Data.D_BC;
+        % Set of nodes associated to Dirichlet BC
+        set_D_nodes=Data.set_D_nodes;
+        % Number of boundaries with Dirichlet BC
+        n=length(D_BC);
+        
+        % Retrieve solutions
+        U=zeros(nb_dof,1);
+        
+        for i=1:n
+            data=D_BC{i};
+            % Retrieve function
+            g=data.function;
+            % Nodes making up the edges
+            nodes=set_D_nodes{i};
+            % Number of nodes on the edges
+            nb_nodes=length(nodes);
+            % Degrees of freedom
+            dofs=Dof_set(:, nodes);
+            % Coordinates
+            X=reshape(coord(nodes,:), nb_nodes, 1, nb_dof_local);
+            % Evaluation
+            G=g(X);
+            
+            if size(G,1)>nb_dof_local
+                G=reshape(G, nb_nodes, nb_dof_local)';
+                G=G(:);
+            else
+                G=repmat(G, nb_nodes, 1);
+            end
+            U(dofs(:))=G;
+            
+        end
+        
+        % Update solution
+        Solution.U=U;
+        % Reshape to an array of the same size as the coordinate matrix
+        Solution.U_sort=reshape(U, nb_dof_local, [])';
+    end
+
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_medium/internodes1.m b/Code/INTERNODES_medium/internodes1.m
new file mode 100644
index 0000000..85268c7
--- /dev/null
+++ b/Code/INTERNODES_medium/internodes1.m
@@ -0,0 +1,65 @@
+% INTERNODES method for contact mechanics
+% Case study: Hertzian contact problem with Dirichlet boundary conditions
+% applied to both bodies.
+
+% Limitations: 2D
+clc
+clear variables
+close all
+
+% Read GMSH mesh file (.msh)
+[Mesh] = readGMSH('../../GMSH/Meshes/contact2_p1_0_05.msh');
+
+%% Parameter prescription
+% Mesh parameters
+[Mesh]=getParameters(Mesh);
+% Initialize model
+[Data]=setModel('model', 'static_solid', 'submodel', 'contact', 'type', 'linear');
+
+%% Set material parameters
+% Elastic modulus [N\m^2]
+E=30e9;
+% Poisson ratio [-]
+nu=0.2;
+% Specific mass [kg/m^3]
+rho=2500;
+% Thickness [m]
+t=1;
+
+% Plane stress conditions
+lambda=E*nu/(1-nu^2);
+mu=E/(2*(1+nu));
+
+[Data]=setCoefficient(Data, 'rho', t*rho);
+[Data]=setCoefficient(Data, 'lambda', t*lambda);
+[Data]=setCoefficient(Data, 'mu', t*mu);
+[Data]=setCoefficient(Data, 'f', @(x) [0; 0]);
+
+%% Set boundary conditions
+
+% Used for testing with invertible stiffness matrix
+BC={{'Type', 'Dirichlet', 'Edges', 3, 'Function', @(x) [0; 0]},...
+    {'Type', 'Dirichlet', 'Edges', 10, 'Function', @(x) [0; -0.1]}};
+
+p2={'BC', BC};    
+
+[Data]=setBoundaryConditions(Mesh, Data, p2);
+%% Set numerical parameters
+% For more information, type 'help setNumericalParameters'
+
+[Data]=setNumericalParameters(Mesh, Data, 'Contact', 'rescaling', E, 'radius', 0.2);
+[Data]=setNumericalParameters(Mesh, Data, 'Solver', 'name', 'GMRES', 'riter', 100);
+[Data]=setNumericalParameters(Mesh, Data, 'PostProcessing', 'ampl_fact', 1, 'quantity', {'stress', 'strain'});
+
+%% Assemblage of the right-hand side
+[Data]=computeRHS(Mesh, Data);
+
+%% Solve the contact problem
+[Mesh, Data, Solution]=solve(Mesh, Data, {3:6, 7:10});
+
+%% Computation of the stresses and strains
+[Mesh, Solution]=postProcess(Mesh, Data, Solution);
+
+%% Visualization of the solution
+% For more information, type 'help plotSolution'
+plotSolution(Mesh, Data, Solution, 'disp', 'standard', 'stress', 'sigma_yy');
\ No newline at end of file
diff --git a/Code/INTERNODES_medium/internodes2.m b/Code/INTERNODES_medium/internodes2.m
new file mode 100644
index 0000000..2447f2e
--- /dev/null
+++ b/Code/INTERNODES_medium/internodes2.m
@@ -0,0 +1,66 @@
+% INTERNODES method for contact mechanics
+% Case study: Hertzian contact problem with Dirichlet boundary conditions
+% applied to the lower body and Neumann boundary conditions applied to the
+% upper body.
+
+% Limitations: 2D
+clc
+clear variables
+close all
+
+% Read GMSH mesh file (.msh)
+[Mesh] = readGMSH('../../GMSH/Meshes/contact2_p1_0_05.msh');
+
+%% Parameter prescription
+% Mesh parameters
+[Mesh]=getParameters(Mesh);
+% Initialize model
+[Data]=setModel('model', 'static_solid', 'type', 'linear');
+
+%% Set material parameters
+% Elastic modulus [N\m^2]
+E=30e9;
+% Poisson ratio [-]
+nu=0.2;
+% Specific mass [kg/m^3]
+rho=2500;
+% Thickness [m]
+t=1;
+
+% Plane stress conditions
+lambda=E*nu/(1-nu^2);
+mu=E/(2*(1+nu));
+
+[Data]=setCoefficient(Data, 'rho', t*rho);
+[Data]=setCoefficient(Data, 'lambda', t*lambda);
+[Data]=setCoefficient(Data, 'mu', t*mu);
+[Data]=setCoefficient(Data, 'f', @(x) [0; 0]);
+
+%% Set boundary conditions
+
+% Used for testing with singular stiffness matrix
+BC={{'Type', 'Dirichlet', 'Edges', 3, 'Function', @(x) [0; 0]},...
+    {'Type', 'Neumann', 'Edges', 10, 'Function', @(x) [0; -1e9]}};
+
+p2={'BC', BC};    
+
+[Data]=setBoundaryConditions(Mesh, Data, p2);
+
+%% Set numerical parameters
+% For more information, type 'help setNumericalParameters'
+
+[Data]=setNumericalParameters(Mesh, Data, 'Contact', 'rescaling', E, 'maxiter', 30, 'radius', inf, 'tol', 0.9);
+[Data]=setNumericalParameters(Mesh, Data, 'PostProcessing', 'ampl_fact', 1, 'quantity', {'stress', 'strain'});
+
+%% Assemblage of the right-hand side
+[Data]=computeRHS(Mesh, Data);
+
+%% Solve the contact problem
+[Mesh, Data, Solution]=solve(Mesh, Data, {3:6, 7:10});
+
+%% Computation of the stresses and strains
+[Mesh, Solution]=postProcess(Mesh, Data, Solution);
+
+%% Visualization of the solution
+% For more information, type 'help plotSolution'
+plotSolution(Mesh, Data, Solution, 'disp', 'standard', 'stress', 'sigma_yy');
\ No newline at end of file
diff --git a/Code/INTERNODES_medium/kr.m b/Code/INTERNODES_medium/kr.m
new file mode 100644
index 0000000..874aea5
--- /dev/null
+++ b/Code/INTERNODES_medium/kr.m
@@ -0,0 +1,31 @@
+function X = kr(U,varargin)
+%KR Khatri-Rao product.
+%   kr(A,B) returns the Khatri-Rao product of two matrices A and B, of 
+%   dimensions I-by-K and J-by-K respectively. The result is an I*J-by-K
+%   matrix formed by the matching columnwise Kronecker products, i.e.
+%   the k-th column of the Khatri-Rao product is defined as
+%   kron(A(:,k),B(:,k)).
+%
+%   kr(A,B,C,...) and kr({A B C ...}) compute a string of Khatri-Rao 
+%   products A o B o C o ..., where o denotes the Khatri-Rao product.
+%
+%   See also kron.
+
+%   Version: 21/10/10
+%   Authors: Laurent Sorber (Laurent.Sorber@cs.kuleuven.be)
+
+if ~iscell(U), U = [U varargin]; end
+K = size(U{1},2);
+if any(cellfun('size',U,2)-K)
+    error('kr:ColumnMismatch', ...
+          'Input matrices must have the same number of columns.');
+end
+J = size(U{end},1);
+X = reshape(U{end},[J 1 K]);
+for n = length(U)-1:-1:1
+    I = size(U{n},1);
+    A = reshape(U{n},[1 I K]);
+    X = reshape(bsxfun(@times,A,X),[I*J 1 K]);
+    J = I*J;
+end
+X = reshape(X,[size(X,1) K]);
diff --git a/Code/INTERNODES_medium/linearOp1.m b/Code/INTERNODES_medium/linearOp1.m
new file mode 100644
index 0000000..7eabec9
--- /dev/null
+++ b/Code/INTERNODES_medium/linearOp1.m
@@ -0,0 +1,107 @@
+function[y]=linearOp1(Mesh, Data, x, type)
+
+% linearOp1: Wrap around function for operations related to the application
+% of linear operators
+% INPUT:
+% Mesh:     Structure containing all the mesh parameters
+% Data:     Structure containing all the data parameters
+% x:        Vector
+% type:     Type of operation to perform
+% OUTPUT:
+% y:        Result of the operation
+
+switch type
+    case 'solve'
+        y=solvePrec(Mesh, Data, x);
+        y=multiplyMatVec(Mesh, Data, y);
+        
+    case 'solvePrec'
+        y=solvePrec(Mesh, Data, x);
+        
+    case 'matvec'
+        y=multiplyMatVec(Mesh, Data, x);
+end
+
+
+    function[y]=multiplyMatVec(Mesh, Data, x)
+        
+        % multiplyMatVec: Implements the mutiplication between the 
+        % INTERNODES matrix A and a vector x and stores the result in y
+        % INPUT:
+        % Mesh:     Structure containing all the mesh parameters
+        % Data:     Structure containing all the data parameters
+        % x:        Vector
+        % OUTPUT:
+        % y:        Result of the multiplication of A with x
+
+        % Retrieve data parameters
+        % Number of free degrees of freedom
+        nf=Data.nf;
+        % Number of constraints
+        nc=Data.nc;
+        
+        % Initialization
+        y=zeros(nf+nc,1);
+
+        y(1:nf)=Data.K*x(1:nf)+multiplyB(Mesh, Data, x(nf+1:nf+nc), 'vec');
+        y(nf+1:nf+nc)=multiplyB_tilde(Mesh, Data, x(1:nf), 'vec');
+    end
+
+    function[y]=solvePrec(Mesh, Data, x)
+        
+        % solvePrec: Implements the resolution of a linear system with the 
+        % preconditioner and a right-hand side vector x and stores the result in y
+        % INPUT:
+        % Mesh:     Structure containing all the mesh parameters
+        % Data:     Structure containing all the data parameters
+        % x:        Right-hand side vector
+        % OUTPUT:
+        % y:        Solution of the linear system
+        
+        
+        % Retrive mesh parameters
+        d=Mesh.nb_dof_local;
+        % Parameter k
+        k=2;
+        
+        % Retrieve data parameters
+        % Cholesky factor of the reordered perturbed stiffness matrix
+        Lr=Data.Lr;
+        L22=Data.L22;
+        % Number of free degrees of freedom
+        nf=Data.nf;
+        % Number of constraints
+        nc=Data.nc;
+        % Perturbed reordering of Nested Dissection
+        or=Data.or;
+        % Size of block 1
+        b1=Data.b1;
+        
+        % Initialization
+        y=zeros(nf+nc,1);
+        w1=zeros(nf,1);
+        
+        p1=x(1:nf);
+        p2=x(nf+1:nf+nc);
+        
+        w2=reshape(p2, d, []);
+        w2=-Data.Contact.alpha*(w2/Data.body{1}.interface_mass);
+        
+        p1=p1-k*multiplyB(Mesh, Data, w2, 'mat');   
+
+        % Method for small problems
+        Lv=Data.Lv;
+        Uv=Data.Uv;
+
+        p2=Lr\p1(or); 
+        p3=p2(b1+1:end);
+        p3=L22*p3;
+        p3(Data.orc)=Uv\(Lv\p3(Data.orl));
+        p2(b1+1:end)=L22'*p3;
+        w1(or)=Lr'\p2;
+
+        y(1:nf)=w1;
+        y(nf+1:nf+nc)=w2;
+    end
+
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_medium/mapGlobal2Local.m b/Code/INTERNODES_medium/mapGlobal2Local.m
new file mode 100644
index 0000000..6b14a3a
--- /dev/null
+++ b/Code/INTERNODES_medium/mapGlobal2Local.m
@@ -0,0 +1,13 @@
+function[Data]=mapGlobal2Local(Data)
+
+% mapGlobal2Local: Function which maps the global degrees of freedom to the
+% local ones for the INTERNODES matrix
+% INPUT:
+% Data:     Structure containing all the data parameters
+% OUTPUT:
+% Data:     Updated structure with the local degrees of freedom
+
+Nf=Data.Nf;
+[~, Data.body{1}.indx_local, ~]=intersect(Nf, Data.body{1}.interface_dof);
+[~, Data.body{2}.indx_local, ~]=intersect(Nf, Data.body{2}.interface_dof);
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_medium/multiplyB.m b/Code/INTERNODES_medium/multiplyB.m
new file mode 100644
index 0000000..0a2ccd4
--- /dev/null
+++ b/Code/INTERNODES_medium/multiplyB.m
@@ -0,0 +1,24 @@
+function[y]=multiplyB(Mesh, Data, x, in)
+
+% multiplyB: Implements the mutiplication between B and a vector x and
+% stores the result in y
+% INPUT:
+% Mesh:     Structure containing all the mesh parameters
+% Data:     Structure containing all the data parameters
+% x:        Vector or matrix
+% in:       Parameter specifiying whether the input is a vector or a matrix
+% OUTPUT:
+% y:        Result of the multiplication of B with x
+
+if strcmp(in, 'vec')
+x=reshape(x, Mesh.nb_dof_local, []);
+end
+
+y=zeros(Data.nf,1);
+
+y1=-x*Data.body{1}.interface_mass;
+y2=(x*Data.R21')*Data.body{2}.interface_mass;
+
+y(Data.body{1}.indx_local)=y1(:);
+y(Data.body{2}.indx_local)=y2(:);
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_medium/multiplyB_tilde.m b/Code/INTERNODES_medium/multiplyB_tilde.m
new file mode 100644
index 0000000..c3d09ba
--- /dev/null
+++ b/Code/INTERNODES_medium/multiplyB_tilde.m
@@ -0,0 +1,26 @@
+function[y]=multiplyB_tilde(Mesh, Data, x, out)
+
+% multiplyB_tilde: Implements the mutiplication between B_tilde and a
+% vector x and stores the result in y
+% INPUT:
+% Mesh:     Structure containing all the mesh parameters
+% Data:     Structure containing all the data parameters
+% x:        Vector
+% out:      Parameter specifying whether the output should be a vector or
+%           a matrix
+% OUTPUT:
+% y:        Result of the multiplication of B_tilde with x
+
+% Retrieve mesh parameters
+% Number of local degrees of freedom
+d=Mesh.nb_dof_local;
+
+w1=reshape(x(Data.body{1}.indx_local,:), d, []);
+w2=reshape(x(Data.body{2}.indx_local,:), d, []);
+
+y=w1-w2*Data.R12';
+
+if strcmp(out, 'vec')
+    y=y(:);
+end
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_medium/plotSolution.m b/Code/INTERNODES_medium/plotSolution.m
new file mode 100644
index 0000000..47ba494
--- /dev/null
+++ b/Code/INTERNODES_medium/plotSolution.m
@@ -0,0 +1,813 @@
+function[]=plotSolution(Mesh, Data, Solution, varargin)
+
+% plotSolution: Function which plots the computed quantities
+% INPUT:
+% Mesh:     Structure containing the mesh parameters
+% Data:     Structure containing the data parameters
+% Solution: Structure containing the solution computed
+% OPTIONAL INPUT
+% snapshot: selected time-step at which the solution should be viewed
+%   1 -> none (default)
+%   any time step in the range defined
+% view_video: view the video of the solution
+%   1 -> yes 
+%   0 -> no (default)
+% save_video: choose whether to save the video or not
+%   1 -> yes
+%   0 -> no (default)
+% plot_arg: choose which solution to visualize
+%   dynamic_solid or static_solid module
+%       disp (default)
+%       stress
+%       strain
+%   transient_heat module
+%       temp (default)
+%       flux
+% plot_type: decide which type of metric should be used for visualization
+%   displacement metric
+%       standard -> structure with amplified displacements (default)
+%       norm
+%       absx
+%       absy
+%   temperature metric
+%       standard (default)
+%   stress metric
+%       von_mises (default)
+%       sigma_xx
+%       sigma_yy
+%       sigma_xy
+%       sigma_xx_abs
+%       sigma_yy_abs
+%       sigma_xy_abs
+%       frobenius
+%   strain metric
+%       epsilon_xx (default)
+%       epsilon_yy
+%       epsilon_xy
+%       epsilon_xx_abs
+%       epsilon_yy_abs
+%       epsilon_xy_abs
+%       frobenius
+%   flux metric
+%       standard (default)
+
+
+%% Initialize and ckeck Parameters
+
+if mod(length(varargin), 2)~=0
+    error('Missing a name or value')
+end
+labels_in=varargin(1:2:end);
+values_in=varargin(2:2:end);
+
+% Specify number of sub-intervals in time        
+if contains(Data.Model.name, 'static')
+    static=true;
+    N=0;
+else
+    static=false;
+    N=Data.Discretization.N;
+end
+
+switch Data.Model.name
+    case {'static_solid', 'dynamic_solid'}
+        plot_argv={'disp', 'stress', 'strain'};
+        n=length(plot_argv);
+        plot_typev=cell(n,1);
+        plot_typev{1}={'standard', 'norm', 'absx', 'absy'};
+        plot_typev{2}={'von_mises', 'sigma_xx', 'sigma_yy', 'sigma_xy', 'sigma_xx_abs', 'sigma_yy_abs', 'sigma_xy_abs', 'frobenius'};
+        plot_typev{3}={'epsilon_xx', 'epsilon_yy', 'epsilon_xy', 'epsilon_xx_abs', 'epsilon_yy_abs', 'epsilon_xy_abs', 'frobenius'};
+        
+    case 'transient_heat'
+        plot_argv={'temp', 'flux'};
+        n=length(plot_argv);
+        plot_typev=cell(n,1);
+        plot_typev{1}={'standard'};
+        plot_typev{2}={'standard'};
+        
+end
+
+% Set all default parameters
+% Default visualization parameters
+Param.visua_param.snapshot=1;
+Param.visua_param.view_video=false;
+Param.visua_param.write_video=false;
+% Default plot argument
+Param.plot_param.arg=plot_argv(1);
+% Default plot type
+Param.plot_param.type=plot_typev{1}(1);
+
+plot_arg={};
+plot_type={};
+nb_plot=0;
+
+for k=1:length(labels_in)
+    arg=lower(labels_in{k});
+    val=values_in{k};
+    
+    switch arg
+        case 'snapshot'
+            
+            if static
+                warning([arg ' parameter will be ignored because the simulation is in statics'])
+            elseif val < 0 || val > Data.Discretization.N+1
+                error(['Time snapshot invalid: it must be an integer between ' num2str(0) ' and ' num2str(Data.Discretization.N+1)])
+            else
+                Param.visua_param.snapshot=val;
+            end
+            
+        case {'view_video', 'write_video'}
+            
+            if static
+                warning([arg ' parameter will be ignored because the simulation is in statics'])
+            else
+                if isa(val, 'double')
+                    
+                    if val<0 || val>1
+                        error([arg ' parameter must be a boolean'])
+                    else
+                        Param.visua_param.view_video=logical(val);
+                    end
+                    
+                elseif isa(val, 'logical')
+                    Param.visua_param.view_video=val;
+                else
+                    error([arg ' parameter must be a boolean'])
+                end
+            end
+
+        case plot_argv
+            
+            % Check if desired quantities have been computed
+            switch arg
+                case 'stress'
+                    if ~isfield(Solution, 'sigma')
+                        error('Stresses have not been computed')
+                    end
+                    
+                case 'strain'
+                    if ~isfield(Solution, 'epsilon')
+                        error('Strains have not been computed')
+                    end
+                    
+                case 'flux'
+                    if ~isfield(Solution, 'Q')
+                        error('Fluxes have not been computed')
+                    end    
+            end
+            
+            plot_arg{nb_plot+1}=arg;
+            index=strcmp(arg, plot_argv);
+
+            if any(ismember(plot_typev{index}, val))
+                plot_type{nb_plot+1}=val;
+            else
+                error('Plot type invalid: see available plot types')
+            end
+                     
+            nb_plot=nb_plot+1;
+        otherwise
+            error('Unrecognized argument')
+    end
+    
+end
+
+if nb_plot>0
+    Param.plot_param.arg=plot_arg;
+    Param.plot_param.type=plot_type;
+end
+
+%% Plotting interface
+
+plot_arg=Param.plot_param.arg;
+plot_type=Param.plot_param.type;
+n=length(plot_arg);
+Plot=cell(n,1);
+Axis=cell(n,1);
+Colorbar=cell(n,1);
+Dynamic=cell(n,1);
+
+fig=figure;
+% fig.Units='normalized';
+% fig.Position=[0 0 1 1];
+
+% fig.Units='normalized';
+% fig.Position=[0 1 1/2 1/2];
+
+for k=1:n
+    switch plot_arg{k}
+        case {'disp', 'temp'}
+            [PlotParam, AxisParam, ColorbarParam, DynamicParam]=plotPatch(Mesh, Data, Solution, plot_type{k});
+            subplot(1,n,k)
+            axis=gca;
+            p=patch(axis);
+            c=colorbar(axis);
+            [p]=    setObject(p, PlotParam);
+            [axis]= setObject(axis, AxisParam);
+            [c]=    setObject(c, ColorbarParam);
+            
+            
+            
+        case {'strain', 'stress'}
+            [PlotParam, AxisParam, ColorbarParam, DynamicParam]=plotSurf(Mesh, Data, Solution, plot_arg{k}, plot_type{k});
+            subplot(1,n,k)
+            axis=gca;
+            p=patch(axis);
+            c=colorbar(axis);
+            [p]=    setObject(p, PlotParam);
+            [axis]= setObject(axis, AxisParam);
+            [c]=    setObject(c, ColorbarParam);
+            
+            
+            
+        case 'flux'
+            [PlotParam, AxisParam, ColorbarParam, DynamicParam]=plotQuiver(Mesh, Solution, plot_type{k});
+            subplot(1,n,k)
+            axis=gca;
+            p=quiver(axis, PlotParam{1,2}, PlotParam{2,2});
+            c=colorbar(axis);
+            [p]=    setObject(p, PlotParam);
+            [axis]= setObject(axis, AxisParam);
+            [c]=    setObject(c, ColorbarParam);
+            
+    end
+    
+    Plot{k}=p;
+    Axis{k}=axis;
+    Colorbar{k}=c;
+    Dynamic{k}=DynamicParam;
+end
+
+if Param.visua_param.view_video % View the solution over the entire simulation
+    
+    for k=1:n
+        [Plot{k}, Axis{k}]=updateObject(Plot{k}, Axis{k}, Dynamic{k}, 1, N);
+    end
+    
+    frames(1)=getframe(fig);
+    for step = 2:N+1
+        pause(0.05)
+        for k=1:n
+            [Plot{k}, Axis{k}]=updateObject(Plot{k}, Axis{k}, Dynamic{k}, step, N);
+            refreshdata(Axis{k});
+        end
+        frames(step)=getframe(fig);
+    end
+   
+else % View solution at a particular time snapshot
+    step=Param.visua_param.snapshot;
+        for k=1:n
+            [Plot{k}, Axis{k}]=updateObject(Plot{k}, Axis{k}, Dynamic{k}, step, N);
+        end
+end
+
+
+if Param.visua_param.write_video
+%     vid_duration=25; % seconds
+    vid_duration=10; % seconds
+    video=VideoWriter('Output.mp4', 'MPEG-4');
+    video.FrameRate=(N+1)/vid_duration;
+    open(video);
+    writeVideo(video,frames);
+    close(video);
+end
+
+%% Parameters for patch plots
+    function[PlotParam, AxisParam, ColorbarParam, DynamicParam]=plotPatch(Mesh, Data, Solution, type)
+        
+        
+        % Retrieve mesh parameters
+        % Coordinate matrix
+        coord=Mesh.coord_mat;
+        % Connectivity matrix
+        connect=Mesh.connect_mat;
+        % Total number of nodes
+        nb_nodes=Mesh.nb_nodes;
+        % Frame surrounding the structure
+        frame=Mesh.frame;
+        % Largest dimension
+        L_max=Mesh.L_max;
+        % Number of local degrees of freedom
+        nb_dof_local=Mesh.nb_dof_local;
+        % Total number of degrees of freedom
+        nb_dof=Mesh.nb_dof;
+        
+        % Retrieve solution
+        U=Solution.U;
+        
+        
+        switch Data.Model.name
+               
+            case {'static_solid', 'dynamic_solid'}
+                
+                % Initilization
+                Vertices=zeros(nb_nodes,2,N+1);
+                CData=cell(N+1,1);
+                
+                
+                switch type
+                    case 'standard'
+                        
+                        if strcmp(Data.PostProcessing.amplification, 'auto')
+                            % Maximum displacement
+                            disp_max=max(max(abs(U)));
+                            % Amplification factor
+                            fact_ampl=L_max/(2*disp_max);
+                        else
+                            fact_ampl=Data.PostProcessing.amplification;
+                        end
+                        % Retrieve degrees of freedom
+                        d1=1:nb_dof_local:nb_dof;
+                        d2=2:nb_dof_local:nb_dof;
+                        % Aplified displacements
+                        ux_ampl=fact_ampl*U(d1,:);
+                        uy_ampl=fact_ampl*U(d2,:);
+                        % Deformed state
+                        def_x=coord(:,1)+ux_ampl;
+                        def_y=coord(:,2)+uy_ampl;
+                        % Vertices in deformed state
+                        Vertices(:,1,:)=def_x;
+                        Vertices(:,2,:)=def_y;
+                        XLim=[min(def_x, [], 'all');
+                              max(def_x, [], 'all')];
+                        YLim=[min(def_y, [], 'all');
+                              max(def_y, [], 'all')];
+                        CLim=[0,1];
+                        Colormap=[];
+                        ColorbarStatus='off';
+                        ColorbarLabel='';
+                        
+%                         Title='Amplified displacements';
+                        Title='Displacements';
+                        EdgeColor='black';
+                        FaceColor='none';
+                        
+                    case 'norm'
+                        
+                        % Retrieve degrees of freedom
+                        d1=1:nb_dof_local:nb_dof;
+                        d2=2:nb_dof_local:nb_dof;
+                        Ux=U(d1,:);
+                        Uy=U(d2,:);
+                        x=coord(:,1)+Ux;
+                        y=coord(:,2)+Uy;
+                        
+                        U_norm=sqrt(Ux.^2+Uy.^2);
+
+                        XLim=[min(x, [], 'all');
+                              max(x, [], 'all')];
+                        YLim=[min(y, [], 'all');
+                              max(y, [], 'all')];
+                        CLim=[0 max(U_norm, [], 'all')];
+                        Colormap=jet;
+                        ColorbarStatus='on';
+                        ColorbarLabel='Displacement [m]';
+                        Title='Norm of displacements';
+                        EdgeColor='none';
+                        FaceColor='interp';
+                        
+                        for m=1:N+1
+                            Vertices(:,1,m)=x(:,m);
+                            Vertices(:,2,m)=y(:,m);
+                            CData{m}=U_norm(:,m);
+                        end
+                        
+                    case 'absx'
+                        
+                        % Retrieve degrees of freedom
+                        d1=1:nb_dof_local:nb_dof;
+                        d2=2:nb_dof_local:nb_dof;
+                        Ux=U(d1,:);
+                        Uy=U(d2,:);
+                        x=coord(:,1)+Ux;
+                        y=coord(:,2)+Uy;
+                        
+                        Ux_abs=abs(Ux);
+                        
+                        XLim=[min(x, [], 'all');
+                              max(x, [], 'all')];
+                        YLim=[min(y, [], 'all');
+                              max(y, [], 'all')];
+                        CLim=[0 max(Ux_abs, [], 'all')];
+                        Colormap=jet;
+                        ColorbarStatus='on';
+                        ColorbarLabel='Displacement [m]';
+                        Title='Displacements in absolute value along x';
+                        EdgeColor='none';
+                        FaceColor='interp';
+                        
+                        for m=1:N+1
+                            Vertices(:,1,m)=x(:,m);
+                            Vertices(:,2,m)=y(:,m);
+                            CData{m}=Ux_abs(:,m);
+                        end
+                        
+                    case 'absy'
+                        
+                        % Retrieve degrees of freedom
+                        d1=1:nb_dof_local:nb_dof;
+                        d2=2:nb_dof_local:nb_dof;
+                        Ux=U(d1,:);
+                        Uy=U(d2,:);
+                        x=coord(:,1)+Ux;
+                        y=coord(:,2)+Uy;
+                        Uy_abs=abs(Uy);
+                        
+                        XLim=[min(x, [], 'all');
+                              max(x, [], 'all')];
+                        YLim=[min(y, [], 'all');
+                              max(y, [], 'all')];
+                        CLim=[0 max(Uy_abs, [], 'all')];
+                        Colormap=jet;
+                        ColorbarStatus='on';
+                        ColorbarLabel='Displacement [m]';
+                        Title='Displacements in absolute value along y';
+                        EdgeColor='none';
+                        FaceColor='interp';
+                        
+                        for m=1:N+1
+                            Vertices(:,1,m)=x(:,m);
+                            Vertices(:,2,m)=y(:,m);
+                            CData{m}=Uy_abs(:,m);
+                        end
+                        
+                        
+                end
+                
+                
+                % Set static parameters
+                PlotParam = {'Vertices',        coord;...
+                             'Faces',           connect(:,1:3);...
+                             'FaceVertexCData', CData{1};...
+                             'FaceColor',       FaceColor;...
+                             'EdgeColor',       EdgeColor;...
+                             'LineWidth',       1};
+                
+                AxisParam = {'XLim',            XLim;...
+                             'YLim',            YLim;...
+                             'CLim',            CLim;...
+                             'Colormap',        Colormap;...
+                             'DataAspectRatio', [1 1 1];...
+                             'XLabel.String',          'x coordinate [m]';...
+                             'YLabel.String',          'y coordinate [m]'};
+                
+                ColorbarParam = {'Visible',         ColorbarStatus;...
+                                 'Label.String',    ColorbarLabel;...
+                                 'Label.FontSize',  11};
+                
+                % Set dynamic parameters
+                DynamicParam={'Vertices',   num2cell(Vertices, [1,2]);...
+                              'CData',      CData;...
+                              'Title',      Title};
+                          
+            case 'transient_heat'
+                
+                % Number of sub-intervals in time
+                N=Data.Discretization.N;
+                % Initilization
+                CData=cell(N+1,1);
+                
+                switch type
+                    case 'standard'
+                        
+                        XLim=frame(:,1);
+                        YLim=frame(:,2);
+                        CLim=[min(U, [], 'all') max(U, [], 'all')];
+                        Colormap=jet;
+                        ColorbarStatus='on';
+                        ColorbarLabel='Temperature [ºC]';
+                        % ColorbarLabel='Temperature [K]';
+                        Title='Temperature';
+                        EdgeColor='none';
+                        FaceColor='interp';
+                        
+                        for m=1:N+1
+                            CData{m}=U(:,m);
+                        end
+                        
+                end
+                
+                
+                % Set static parameters
+                PlotParam = {'Vertices',        coord;...
+                             'Faces',           connect(:,1:3);...
+                             'FaceVertexCData', CData{1};...
+                             'FaceColor',       FaceColor;...
+                             'EdgeColor',       EdgeColor;...
+                             'LineWidth',       1};
+                
+                AxisParam = {'XLim',            XLim;...
+                             'YLim',            YLim;...
+                             'CLim',            CLim;...
+                             'Colormap',        Colormap;...
+                             'DataAspectRatio', [1 1 1];...
+                             'XLabel.String',          'x coordinate [m]';...
+                             'YLabel.String',          'y coordinate [m]'};
+                
+                ColorbarParam = {'Visible',         ColorbarStatus;...
+                                 'Label.String',    ColorbarLabel;...
+                                 'Label.FontSize',  11};
+                
+                % Set dynamic parameters
+                DynamicParam={'CData',      CData;...
+                              'Title',      Title};
+                   
+        end
+        
+        
+    end
+
+%% Parameters for surf plots
+    function[PlotParam, AxisParam, ColorbarParam, DynamicParam]=plotSurf(Mesh, Data, Solution, arg, type)
+  
+        % Retrieve mesh parameters
+        % Coordinate matrix of interpolation nodes
+        coord_inter=Mesh.coord_inter;
+        % Frame surrounding the structure
+        frame=Mesh.frame;
+        
+        % Retrieve data parameters
+        % Number of evaluation points per element
+        nq=Data.PostProcessing.eval_points;
+        
+        % Total number of interpolation nodes
+        nb_nodes_inter=size(coord_inter,1);
+        
+        % Initilization
+        Vertices=zeros(nb_nodes_inter,2,N+1);
+        CData=cell(N+1,1);
+        
+        switch arg
+            
+            case 'stress'
+                
+                % Retrieve solution
+                sigma=permute(Solution.sigma, [2 1 3]);
+                
+                switch type
+                    case 'von_mises'
+                        
+                        % Von Mises stress
+                        select=sqrt(sigma(:,1,:).^2-sigma(:,1,:).*sigma(:,2,:)+sigma(:,2,:).^2+3*sigma(:,3,:).^2);
+                        ColorbarLabel='Stress [Pa]';
+                        Title='Von Mises stress field';
+                        
+                    case 'sigma_xx'
+                        
+                        % Value of sigma_xx
+                        select=sigma(:,1,:);
+                        ColorbarLabel='Stress [Pa]';
+                        Title='\sigma_{xx}';
+                        
+                    case 'sigma_yy'
+                        
+                        % Value of sigma_yy
+                        select=sigma(:,3,:);
+                        ColorbarLabel='Stress [Pa]';
+                        Title='\sigma_{yy}';
+                        
+                    case 'sigma_xy'
+                        
+                        % Value of sigma_xy
+                        select=sigma(:,2,:);
+                        ColorbarLabel='Stress [Pa]';
+                        Title='\sigma_{xy}';
+                        
+                    case 'sigma_xx_abs'
+                        
+                        % Abolute value of sigma_xx
+                        select=abs(sigma(:,1,:));
+                        ColorbarLabel='Stress [Pa]';
+                        Title='|\sigma_{xx}|';
+                        
+                    case 'sigma_yy_abs'
+                        
+                        % Absolute value of sigma_yy
+                        select=abs(sigma(:,3,:));
+                        ColorbarLabel='Stress [Pa]';
+                        Title='|\sigma_{yy}|';
+                        
+                    case 'sigma_xy_abs'
+                        
+                        % Absolute value of sigma_xy
+                        select=abs(sigma(:,2,:));
+                        ColorbarLabel='Stress [Pa]';
+                        Title='|\sigma_{xy}|';
+                        
+                    case 'frobenius'
+                        
+                        % Frobenius norm of sigma
+                        select=sqrt(sigma(:,2,:).^2+2*sigma(:,2,:).^2+sigma(:,3,:).^2);
+                        ColorbarLabel='Stress [Pa]';
+                        Title='Frobenius norm of \sigma';
+                        
+                end
+                
+            case 'strain'
+                
+                % Retrieve solution
+                epsilon=permute(Solution.epsilon, [2 1 3]);
+                
+                switch type
+                    case 'epsilon_xx'
+                        
+                        % Value of epsilon_xx
+                        select=epsilon(:,1,:);
+                        ColorbarLabel='Strain [-]';
+                        Title='\epsilon_{xx}';
+                        
+                    case 'epsilon_yy'
+                        
+                        % Value of epsilon_yy
+                        select=epsilon(:,3,:);
+                        ColorbarLabel='Strain [-]';
+                        Title='\epsilon_{yy}';
+                        
+                    case 'epsilon_xy'
+                        
+                        % Value of esilon_xy
+                        select=epsilon(:,2,:);
+                        ColorbarLabel='Strain [-]';
+                        Title='\epsilon_{xy}';
+                        
+                    case 'epsilon_xx_abs'
+                        
+                        % Abolute value of epsilon_xx
+                        select=abs(epsilon(:,1,:));
+                        ColorbarLabel='Strain [-]';
+                        Title='|\epsilon_{xx}|';
+                        
+                    case 'epsilon_yy_abs'
+                        
+                        % Absolute value of epsilon_yy
+                        select=abs(epsilon(:,3,:));
+                        ColorbarLabel='Strain [-]';
+                        Title='|\epsilon_{yy}|';
+                        
+                    case 'epsilon_xy_abs'
+                        
+                        % Absolute value of epsilon_xy
+                        select=abs(epsilon(:,2,:));
+                        ColorbarLabel='Strain [-]';
+                        Title='|\epsilon_{xy}|';
+                        
+                    case 'frobenius'
+                        
+                        % Frobenius norm of epsilon
+                        select=sqrt(epsilon(:,2,:).^2+2*epsilon(:,2,:).^2+epsilon(:,3,:).^2);
+                        ColorbarLabel='Strain [-]';
+                        Title='Frobenius norm of \epsilon';
+                        
+                end
+                
+        end
+        
+        % Common parameters
+        XLim=[min(coord_inter(:,1,:), [], 'all');
+              max(coord_inter(:,1,:), [], 'all')];
+        YLim=[min(coord_inter(:,2,:), [], 'all');
+              max(coord_inter(:,2,:), [], 'all')];
+        CLim=[min(select, [], 'all') max(select, [], 'all')];
+        Colormap=jet;
+        ColorbarStatus='on';
+        EdgeColor='none';
+        FaceColor='interp';
+        
+        for m=1:N+1
+            Vertices(:,1,m)=coord_inter(:,1,m);
+            Vertices(:,2,m)=coord_inter(:,2,m);
+            CData{m}=select(:,m);
+        end
+        
+
+        switch nq
+            case 3
+                % 3-point evaluation inside each element
+                I=1:3*Mesh.nb_elem;
+                I=reshape(I, 3, Mesh.nb_elem)';
+                
+            case 6
+                % 6-point evaluation inside each element
+                i1=[1 4 5 2 2 5 6 3 3 6 4 1];
+                i2=[4 5 6];
+                I1=i1+(0:Mesh.nb_elem-1)'*6;
+                I2=i2+(0:Mesh.nb_elem-1)'*6;
+                I1=reshape(I1', 4, 3*Mesh.nb_elem)';
+                I2=[I2 nan*zeros(size(I2,1),1)];
+                I=[I1; I2];
+        end
+        
+                % Set static parameters
+                PlotParam = {'Vertices',        coord_inter;...
+                             'Faces',           I;...
+                             'FaceVertexCData', CData{1};...
+                             'FaceColor',       FaceColor;...
+                             'EdgeColor',       EdgeColor;...
+                             'LineWidth',       1};
+                
+                AxisParam = {'XLim',            XLim;...
+                             'YLim',            YLim;...
+                             'CLim',            CLim;...
+                             'Colormap',        Colormap;...
+                             'DataAspectRatio', [1 1 1];...
+                             'XLabel.String',          'x coordinate [m]';...
+                             'YLabel.String',          'y coordinate [m]'};
+                
+                ColorbarParam = {'Visible',         ColorbarStatus;...
+                                 'Label.String',    ColorbarLabel;...
+                                 'Label.FontSize',  11};
+                
+                % Set dynamic parameters
+                DynamicParam={'Vertices',   num2cell(Vertices, [1,2]);...
+                              'CData',      CData;...
+                              'Title',      Title};
+        
+    end
+
+%% Parameters for quiver plots
+    function[PlotParam, AxisParam, ColorbarParam, DynamicParam]=plotQuiver(Mesh, Solution, type)
+        
+        % Retrieve mesh parameters
+        % Coordinate matrix
+        coord=Mesh.coord_mat;
+        % Frame surrounding the structure
+        frame=Mesh.frame;
+        
+        % Retrieve solution
+        Q_sort=Solution.Q_sort;
+        
+        % Initialization
+        U=cell(1,N+1);
+        V=cell(1,N+1);
+        
+        switch type
+            case 'standard'
+                % Standard visualization of fluxes
+                Title='Flux field';
+                
+                for m=1:N+1
+                    U{m}=Q_sort(:,2*m-1);
+                    V{m}=Q_sort(:,2*m);
+                end
+                
+        end
+        
+        
+        % Set static parameters
+        PlotParam = {'XData',        coord(:,1);...
+                     'YData',        coord(:,2);...
+                     'LineWidth',       1};
+        
+        AxisParam = {'XLim',            frame(:,1);...
+                     'YLim',            frame(:,2);...
+                     'DataAspectRatio', [1 1 1];...
+                     'XLabel.String',          'x coordinate [m]';...
+                     'YLabel.String',          'y coordinate [m]'};
+        
+        ColorbarParam = {'Visible',         'off';...
+                         'Label.String',    '';...
+                         'Label.FontSize',  11};
+        
+        % Set dynamic parameters
+        DynamicParam={'UData',      U;...
+                      'VData',      V;...
+                      'Title',      Title};
+        
+        
+    end
+
+
+
+
+
+
+%% Setting and updating graphic properties
+    function[object]=setObject(object, param)
+        
+        argument=param(:,1);
+        value=param(:,2);
+        
+        for i=1:length(argument)         
+            if contains(argument{i}, '.')
+                field=extractBetween(argument{i}, 1, '.');
+                subfield=extractAfter(argument{i}, '.');
+                object.(field{1}).(subfield)=value{i};
+            else 
+                object.(argument{i})=value{i};
+            end
+        end
+        
+    end
+
+    function[plot, axis]=updateObject(plot, axis, param, step, N)
+        
+        argument=param(:,1);
+        value=param(:,2);
+        
+        for i=1:length(argument)-1
+            plot.(argument{i})=value{i}{step};
+        end
+        
+        axis.Title.String=[value{end} ' at step ' num2str(step) ' out of ' num2str(N+1)];
+    end
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_medium/postProcess.m b/Code/INTERNODES_medium/postProcess.m
new file mode 100644
index 0000000..364859c
--- /dev/null
+++ b/Code/INTERNODES_medium/postProcess.m
@@ -0,0 +1,18 @@
+function[Mesh, Solution]=postProcess(Mesh, Data, Solution)
+
+% postProcess: General post-processing function used to compute derived
+% quantities
+% INPUT:
+% Mesh:     Structure containing all the mesh parameters
+% Data:     Structure containing all the data parameters
+% Solution: Structure containing the solution
+% OUTPUT:
+% Mesh:     Updated mesh structure with coordinates of nodes where stresses
+%           and strains were evaluated
+% Solution: Updated solution structure with derived quantities
+
+
+if any(contains(Data.PostProcessing.quantity, {'stress', 'strain'}))
+    [Mesh, Solution]=postProcessDisp(Mesh, Data, Solution);  
+end
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_medium/postProcessDisp.m b/Code/INTERNODES_medium/postProcessDisp.m
new file mode 100644
index 0000000..f1f6613
--- /dev/null
+++ b/Code/INTERNODES_medium/postProcessDisp.m
@@ -0,0 +1,147 @@
+function[Mesh, Solution]=postProcessDisp(Mesh, Data, Solution)
+
+% postProcessDisp: Function used to compute strains and stresses once the 
+% displacements are known.
+% INPUT:
+% Mesh:     Structure containing all mesh parameters
+% Data:     Structure containing all data parameters
+% Solution: Structure containing the computed solutions
+% OUTPUT:
+% Mesh:     Updated mesh structure with evaluation points coordinates
+% Solution: Updated solution structure including strains and stresses
+
+
+% Finite element order
+order=Mesh.order;
+% Number of local degrees of freedom
+d=Mesh.nb_dof_local;
+% Number of elements
+ne=Mesh.nb_elem;
+% Number of nodes per element
+nne=Mesh.nb_nodes_elem;
+% Coordinate matrix
+coord=Mesh.coord_mat;
+% Connectivity matrix
+connect=Mesh.connect_mat;
+% Retrieve basis functions
+[Functions]=getFunctions(order);
+% Retrieve Jacobian matrix
+J_phi=Functions.J_phi;
+% Number of evaluation points per element
+nq=Data.PostProcessing.eval_points;
+% Retrieve evaluation points
+[points, ~] = getQuadrature(nq, 'interpolation');
+% Number of independent strain and stress components
+Mesh.nb_comp_ind=1/2*d*(d+1);
+% Mu
+f1=Data.Coefficient.mu.function;
+% Lambda
+f2=Data.Coefficient.lambda.function;
+% Retrieve solution
+U_sort=Solution.U_sort;
+% Compute stresses and strains using current position
+coord=coord+U_sort;
+
+% Define permutation matrix
+Sdd=zeros(d*d);
+Id1=eye(d);
+Id2=eye(d^2);
+
+for k=1:d
+    Sdd=Sdd+kron(Id1(:,k)', kron(Id1, Id1(:,k)));
+end
+
+% Reduce size of matrices to avoid unnecessary computations
+% sigma_xx, sigma_xy, sigma_yy
+h=[1 2 4];
+
+I1=(Id2+Sdd);
+I2=Id1(:);
+
+% Truncate the matrices
+I1=I1(h,:);
+I2=I2(h);
+
+% Initialization
+Q=zeros(nq*d^2, nne*d);
+
+% Loop over the Gauss points
+for i = 1:nq
+    L=kron(J_phi(points(i,:))', Id1);
+    Q((i-1)*d^2+1:i*d^2, :)=L;
+end
+Q=sparse(Q);
+T=connect';
+G=T(:);
+
+% Coordinates of the nodes of the elements
+coord_nodes=coord(G,:);
+% Displacements of the nodes of the elements
+W=reshape(U_sort(G,:)', nne*d, ne);
+% Vertices of the elements
+a=coord_nodes(1:nne:end,:);
+b=coord_nodes(2:nne:end,:);
+c=coord_nodes(3:nne:end,:);
+% Interpolated coordinates
+P=a(:)+[b(:)-a(:) c(:)-a(:)]*points';
+X(:,:,1)=P(1:ne,:);
+X(:,:,2)=P(ne+1:end,:);
+coord_inter=reshape(P', nq*ne, d);
+% Update mesh
+Mesh.coord_inter=coord_inter;
+
+% Computation of geometric quantities
+% Determinants of Jacobian matrices
+detJK=(b(:,1)-a(:,1)).*(c(:,2)-a(:,2))-(b(:,2)-a(:,2)).*(c(:,1)-a(:,1));
+
+JK_invT=zeros(d, d*ne);
+JK_invT(:,1:d:d*ne)=1./detJK'.*[c(:,2)-a(:,2) -(c(:,1)-a(:,1))]';
+JK_invT(:,2:d:d*ne)=1./detJK'.*[-(b(:,2)-a(:,2)) b(:,1)-a(:,1)]';
+
+vec_JK_invT=reshape(JK_invT, 1, d^2*ne);
+
+% Coefficient evaluation
+F1=f1(X);
+F2=f2(X);
+if size(F1,1)>1
+F1=reshape(F1, 1, nq, ne);
+end
+if size(F2,1)>1
+F2=reshape(F2, 1, nq, ne);
+end
+
+% Matrix product computations
+W=reshape(Q*W, d^2, nq, ne);
+
+B1=reshape(product(JK_invT,repmat(Id1, 1, ne),d,ne), d^2, d^2, ne);
+B2=reshape(vec_JK_invT, 1, d^2, ne);
+
+% Computation of strains
+if any(contains(Data.PostProcessing.quantity, 'strain'))
+    E=pagemtimes(B1, W);
+    E=0.5*I1*reshape(E, d^2, nq*ne);
+    Solution.epsilon=E;
+end
+
+% Computation of stresses
+if any(contains(Data.PostProcessing.quantity, 'stress'))
+    S1=pagemtimes(B1, F1.*W);
+    S2=pagemtimes(B2, F2.*W);
+    S1=I1*reshape(S1, d^2, nq*ne);
+    S2=I2*reshape(S2, 1, nq*ne);
+    S=S1+S2;
+    Solution.sigma=S;
+end
+
+
+    function[M]=product(A,B,d,ne)
+        
+        % !! Only for a two-dimensional case !!
+        M=zeros(d^2, d^2*ne);
+        
+        M(:,1:d^2:end)=kr(A(:,1:d:end), B(:,1:d:end));
+        M(:,2:d^2:end)=kr(A(:,1:d:end), B(:,2:d:end));
+        M(:,3:d^2:end)=kr(A(:,2:d:end), B(:,1:d:end));
+        M(:,4:d^2:end)=kr(A(:,2:d:end), B(:,2:d:end));
+    end
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_medium/preCompute.m b/Code/INTERNODES_medium/preCompute.m
new file mode 100644
index 0000000..7003bd1
--- /dev/null
+++ b/Code/INTERNODES_medium/preCompute.m
@@ -0,0 +1,57 @@
+function[Data]=preCompute(Mesh, Data)
+
+% preCompute: Function which pre-computes quantities needed in a single 
+% iteration of the contact algorithm
+% INPUT:
+% Mesh:     Structure containing all the mesh parameters
+% Data:     Structure containing all the data parameters
+% OUTPUT:
+% Data:     Updated data structure
+
+% Number of local degrees of freedom
+d=Mesh.nb_dof_local;
+% Retrieve bodies
+body=Data.body;
+% Retrieve local degrees of freedom
+indx1=body{1}.indx_local;
+indx2=body{2}.indx_local;
+% alpha Parameter
+alpha=Data.Contact.alpha;
+
+l1=length(indx1);
+% Block size
+b2=Data.b2;
+
+i2i=Data.i2;
+i2=[indx1; indx2];
+% Beware of the ordering in the linear operators
+[~, index, reor]=intersect(i2i, i2, 'stable');
+
+%% Solution method for small systems
+
+M1=kron(body{1}.interface_mass, speye(d));
+M2=kron(body{2}.interface_mass, speye(d));
+
+R12=kron(Data.R12, speye(d));
+R21=kron(Data.R21, speye(d));
+
+Q1=-M2*(R21/M1);
+Q2=-R12';
+
+Y1=[eye(l1); Q1];
+Y2=[eye(l1); Q2];
+C=-Y1*Y2';
+
+Z=sparse(b2, b2);
+Z(index,index)=C(reor, reor);
+
+V=Data.A+alpha*Z;
+Data.V=V;
+
+[Lv, Uv, orl, orc]=lu(V, 'vector');
+
+Data.Lv=Lv;
+Data.Uv=Uv;
+Data.orl=orl;
+Data.orc=orc;
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_medium/preProcess.m b/Code/INTERNODES_medium/preProcess.m
new file mode 100644
index 0000000..9a757eb
--- /dev/null
+++ b/Code/INTERNODES_medium/preProcess.m
@@ -0,0 +1,81 @@
+function[Data]=preProcess(Data, Solution, K)
+
+% preProcess: Pre-computes the Cholesky factor of the perturbed stiffness
+% matrix and part of the right-hand side vector. These quantities do not
+% change during the course of the contact algorithm
+% INPUT:
+% Data:     Structure containing all the data parameters
+% Solution: Structure containing the initialized solution
+% K:        Global stiffness matrix
+% OUTPUT:
+% Data:     Updated data structure with part of the right-hand side vector, 
+%           reordering indices, Cholesky factors and matrices
+
+% Retrieve data parameters
+% Free degrees of freedom
+Nf=Data.Nf;
+% Blocked degrees of freedom
+Nd=Data.Nd;
+% Retrieve bodies
+body=Data.body;
+% Rescaling parameter
+r=Data.Contact.rescaling;
+% Perturbation added to the stiffness matrix to make it SPD
+Data.Contact.epsilon=1e-8;
+% Right-hand side force vector
+f=Data.F;
+% Number of free degrees of freedom
+nf=length(Nf);
+% Retrieve local degrees of freedom
+indx1=body{1}.indx_local;
+indx2=body{2}.indx_local;
+
+l1=length(indx1);
+l2=length(indx2);
+
+% Retrieve solution
+U=Solution.U;
+
+% Rescaled stiffness matrix
+Kff=1/r*K(Nf, Nf);
+% Perturbed stiffness matrix
+K_tilde=Kff+Data.Contact.epsilon*speye(nf,nf);
+% Cholesky decomposition of the perturbed stiffness matrix
+p=dissect(K_tilde);
+
+% Block sizes for partitioning
+b1=nf-(l1+l2);
+b2=l1+l2;
+
+% Cholesky decomposition of the reordered perturbed stiffness matrix 
+i2=[indx1; indx2];
+or=[setdiff(p, i2, 'stable')'; i2]; 
+Kr=K_tilde(or, or);
+Lr=chol(Kr, 'lower');
+L22=Lr(b1+1:end,b1+1:end);
+Data.L22=L22;
+Data.i2=i2;
+Data.or=or;
+Data.A=L22*L22';
+
+% Right-hand side
+bc=1/r*(f(Nf)-K(Nf,Nd)*U(Nd));
+
+% Block sizes for partitioning
+Data.b1=b1;
+Data.b2=b2;
+
+% Number of free degrees of freedom
+Data.nf=nf;
+
+% Parameter alpha
+if strcmp(Data.Contact.alpha, 'auto')
+    Data.Contact.alpha=-norm(Kff, Inf);
+end
+% Rescaled stiffness matrix
+Data.K=Kff;
+% Cholesky factor of the reordered perturbed stiffness matrix
+Data.Lr=Lr;
+% First vector component of the RHS vector
+Data.bc=bc;
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_medium/readGMSH.m b/Code/INTERNODES_medium/readGMSH.m
new file mode 100755
index 0000000..21425ca
--- /dev/null
+++ b/Code/INTERNODES_medium/readGMSH.m
@@ -0,0 +1,86 @@
+function [Mesh] = readGMSH(file_name)
+
+% readGMSH: Reads a GMSH mesh file (.msh) and constructs the coordinate,
+% connectivity, boundary nodes, material tag and boundary tag matrices
+% INPUT:
+% file_name: GMSH mesh file (.msh) 
+% OUTPUT:
+% Mesh: Structure containing all fields related to the geometry, boundary
+% conditions and material tags
+%   coord: Matrix of node coordinates
+%   connectivites: Connectivity matrix of the system
+%   boundary_nodes: Matrix containing the boundary nodes (both displacements
+%   and external forces)
+%   material_tag: Vector containing the material tag of the elements
+%   boundary_tag: Vector containing the boundary tags of the elements 
+%   forming boundaries (enables to make the distinction between Dirichlet
+%   and Neumann boundary conditions)
+
+fileID = fopen(file_name);
+text = textscan(fileID, '%s', 'delimiter', '\n');
+fclose(fileID);
+
+text=text{1};
+% Construction of the coordinate matrix
+nodes_start = findPos(text, '$Nodes') + 1;
+
+fileID = fopen(file_name);
+% Node coordinates
+coord = textscan(fileID, '%f %f %f %f', 'HeaderLines', nodes_start);
+% Elements
+connect = textscan(fileID, [repmat('%f', 1, 20) '%*[^\n]'], 'HeaderLines', 3, 'EndOfLine', '\n', 'CollectOutput', 1);
+fclose(fileID);
+
+% Coordinate matrix
+coord=cell2mat(coord);
+% Ignore z component
+coord=coord(:,2:end-1);
+
+% Connectivity matrix
+connect=cell2mat(connect);
+
+I=sum(~isnan(connect), 2);
+v=unique(I);
+
+% Boundary elements
+B=connect(I==v(1), 1:v(1));
+% Elements
+E=connect(I==v(2), 1:v(2));
+
+% Boundary tags
+B_tags=B(:,4);
+% Element tags
+E_tags=E(:,4);
+
+% Boundary elements connectivity matrix
+B_connect=B(:,6:end);
+% Elements connectivity matrix
+E_connect=E(:,6:end);
+
+% Initialize mesh structure
+Mesh.coord_mat=coord;
+Mesh.BC_nodes=B_connect;
+Mesh.connect_mat=E_connect;
+Mesh.BC_tag=B_tags;
+Mesh.material_tag=E_tags;
+
+% Identification of the type of element in GMSH
+% 2 -> T3
+% 9 -> T6
+% 21 -> T10
+% 1 -> line
+% 8 -> quadratic curve
+% 26 -> cubic curve
+end
+
+
+function[start]=findPos(text, string)
+lines = size(text, 1);
+start = 1;
+for i = 1:lines
+    if strcmp(text{i}, string)
+        start = i;
+        break
+    end 
+end
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_medium/removeTraction.m b/Code/INTERNODES_medium/removeTraction.m
new file mode 100644
index 0000000..b96c60f
--- /dev/null
+++ b/Code/INTERNODES_medium/removeTraction.m
@@ -0,0 +1,71 @@
+function[Data]=removeTraction(Mesh, Data, Solution, n)
+
+% removeTraction: Checks for convergence and updates the interface properties
+% INPUT:
+% Mesh:     Structure containing all the mesh parameters
+% Data:     Structure containing all the data parameters
+% Solution: Structure containing the solution
+% n:        Current iteration number
+% OUTPUT:
+% Data:     Updated data structure with updated interface properties
+
+% Compute the normals based on deformed structure
+% Normals for initial interface
+[Data]=computeNormals(Mesh, Data, Solution);
+
+normal1=Data.body{1}.normal;
+normal2=Data.body{2}.normal;
+
+% Retrieve Lagrange multipliers
+lambda_sort1=Solution.lambda_sort;
+
+% Project the lambdas on interface 2
+lambda_sort2=-Data.R21*lambda_sort1;
+
+% Computes the scalar products
+scalar1=sum(lambda_sort1.*normal1(Data.body{1}.active_set,:),2);
+scalar2=sum(lambda_sort2.*normal2(Data.body{2}.active_set,:),2);
+
+% Detect nodes to be removed from the interfaces
+dump_nodes1=Data.body{1}.interface_nodes(scalar1>0);
+dump_nodes2=Data.body{2}.interface_nodes(scalar2>0);
+
+% Update the interfaces
+% If only compression test penetration, otherwise remove nodes in traction
+if isempty(dump_nodes1) && isempty(dump_nodes2)
+    % Gap verification
+    [add_nodes1, add_nodes2]=detectGaps(Mesh, Data, Solution);
+    
+    [Data, nodediff1]=updateInterface(Mesh, Data, 1, add_nodes1, 'add');
+    [Data, nodediff2]=updateInterface(Mesh, Data, 2, add_nodes2, 'add');
+    
+else
+    % Skip gap verification
+    [Data, nodediff1]=updateInterface(Mesh, Data, 1, dump_nodes1, 'dump');
+    [Data, nodediff2]=updateInterface(Mesh, Data, 2, dump_nodes2, 'dump');
+end
+
+fprintf('Iteration %d: \n', n)
+fprintf('%+d nodes on interface 1 \n', nodediff1)
+fprintf('%+d nodes on interface 2 \n', nodediff2)
+fprintf('----------------------------------- \n')
+
+% Check for convergence failure
+if isempty(Data.body{1}.interface_nodes) || isempty(Data.body{2}.interface_nodes)
+    msg=['The contact algorithm stopped prematurely because at least one of ' ...
+         'the sets of nodes of the potential contact interface is empty. '...
+         'Potential causes for this error are:\n'];
+     
+    causes=['1) The boundary conditions are incorrect \n'...
+            '2) The potential contact interface was misidentified \n'...
+            '3) The mesh is too coarse \n'];
+    
+    error(sprintf([msg, causes]))
+end
+
+% Check for convergence
+if abs(nodediff1)+abs(nodediff2)==0 % Stop the iterations
+    Data.Contact.iterate=0;
+    disp('Successfully converged')
+end
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_medium/setBoundaryConditions.m b/Code/INTERNODES_medium/setBoundaryConditions.m
new file mode 100644
index 0000000..2d375dc
--- /dev/null
+++ b/Code/INTERNODES_medium/setBoundaryConditions.m
@@ -0,0 +1,267 @@
+function[Data]=setBoundaryConditions(Mesh, Data, varargin)
+
+% setBoundaryConditions: Function which initializes the boundary data
+% INPUT:
+% Mesh:     Structure containing all the mesh parameters
+% Data:     Structure containing all the data parameters
+% OPTIONAL INPUT:
+% Any type of boundary condition. If no Dirichlet boundary conditions are 
+% specified, the program returns an error
+% Types of boundary conditions:
+%   Dirichlet
+%   Neumann
+% OUTPUT:
+% Data: Updated data structure with boundary conditions
+
+
+%% Set boundary conditions
+if mod(length(varargin{1}), 2)~=0
+    error('Missing a name or value')
+end
+
+% Retrieve boundary nodes
+BC_tags=unique(Mesh.BC_tag);
+
+% Dirichlet type
+D_types={'dirichlet'};
+% Neumann types
+N_types={'neumann'};
+% Available types of boundary conditions
+available_types=union(D_types, N_types);
+% Count to detect duplicate boundaries
+count=[];
+% Count number of Dirichlet and Neumann boundaries specified
+nb_D_BC=0;
+nb_N_BC=0;
+
+% Retrieve labels and values entered by the user
+labels_in=varargin{1}(1:2:end);
+values_in=varargin{1}(2:2:end);
+
+% Initilization
+Data.BC=[];
+
+% Checking for boundary conditions
+c1=strcmp(labels_in, 'BC');
+
+if any(c1)
+    BC=values_in{c1};
+    nb_BC=length(BC);
+    Data.BC=cell(nb_BC,1);
+    
+    for i=1:nb_BC
+        bc=BC{i};
+        l=length(bc);
+        
+        if mod(l,2)~=0
+            error('Missing a name or value')
+        end
+        
+        for j=1:2:l
+            arg=lower(bc{j});
+            val=bc{j+1};
+            
+            switch arg
+                
+                case 'type'
+                    if any(ismember(available_types, lower(val)))
+                        Data.BC{i}.(arg)=lower(val);
+                        
+                        if any(ismember(D_types, lower(val)))
+                            nb_D_BC=nb_D_BC+1;
+                        else
+                            nb_N_BC=nb_N_BC+1;
+                        end
+                    else
+                        error('Unrecognized boundary condition')
+                    end
+                    
+                case 'edges'
+                    
+                    edges=intersect(BC_tags, val);
+                    
+                    if isempty(edges)
+                        error('One of the edges specified does not exist')
+                    elseif ~isempty(intersect(count, val)) || length(unique(val))~=length(val)
+                        error('Duplicate edges detected')   
+                    end
+                    count=[count val];
+                    Data.BC{i}.(arg)=val;
+                    
+                case {'function'}
+                    
+                    if isa(val,'double')
+                        Data.BC{i}.label='const';
+                        Data.BC{i}.(arg)= @(x) val;
+                        
+                    elseif isa(val,'function_handle')
+                        Data.BC{i}.label=detectDependency(val);
+                        Data.BC{i}.(arg)=val;
+                    else
+                        error('Input parameters must be either constants or function handles')
+                    end
+                    
+                otherwise
+                    error('Unrecognized argument')
+            end
+            
+        end
+        
+        
+    end
+    
+end
+
+%% Post-Processing: Partitioning the structure BC into Dirichlet BC and Neumann BC and checking for missing data
+
+if nb_D_BC==0
+    error('Dirichlet boundary conditions must be prescribed')
+end
+
+BC=Data.BC;
+l=length(BC);
+
+% Initialize cells
+D_BC=cell(nb_D_BC,1);
+N_BC=cell(nb_N_BC,1);
+% Set of Dirichlet and Neumann tags
+D_tags=[];
+N_tags=[];
+% Initialize counters
+d=1;
+n=1;
+
+for i=1:l
+    bc=BC{i};
+    type=bc.type;
+    
+    if ~isfield(bc, 'edges')
+        error('The edges must be specified')
+    end
+    
+    if any(ismember(D_types, type))
+        D_tags=[D_tags bc.edges];
+        D_BC{d}.type=type; 
+        D_BC{d}.edges=bc.edges;
+        
+        if isfield(bc, 'function')
+            D_BC{d}.function=bc.function;
+            D_BC{d}.label=bc.label;
+        else
+            warning('A function has not been prescribed and will be assumed zero')
+            D_BC{d}.function=@(x) [0; 0];
+            D_BC{d}.label='const';
+        end
+        
+        d=d+1;
+        
+    elseif any(ismember(N_types, type))
+        N_tags=[N_tags bc.edges];
+        N_BC{n}.type=type;
+        N_BC{n}.edges=bc.edges;
+        
+        
+        if isfield(bc, 'function')
+            N_BC{n}.function=bc.function;
+            N_BC{n}.label=bc.label;
+        else
+            warning('A function has not been prescribed and will be assumed zero')
+            N_BC{n}.function=@(x) [0; 0];
+            N_BC{n}.label='const';
+        end
+        
+        n=n+1;
+    end
+end
+
+Data.D_BC=D_BC;
+Data.N_BC=N_BC;
+
+Data.D_tags=D_tags;
+Data.N_tags=N_tags;
+
+%% Sorting nodes
+% If there is a node belonging to conflicting boundary conditions, it is
+% set using the following priority list: Dirichlet, Neumann, Interior
+BC_nodes=Mesh.BC_nodes;
+BC_tag=Mesh.BC_tag;
+nb_nodes=Mesh.nb_nodes;
+Dof_set=Mesh.Dof_set;
+
+set_D_nodes=cell(nb_D_BC,1);
+set_N_nodes=cell(nb_N_BC,1);
+
+for k=1:nb_D_BC
+    nodes=BC_nodes(any(BC_tag==D_BC{k}.edges,2),:);
+    set_D_nodes{k}=unique(nodes);
+end
+for k=1:nb_N_BC
+    nodes=BC_nodes(any(BC_tag==N_BC{k}.edges,2),:);
+    set_N_nodes{k}=unique(nodes);
+end
+% Define the sets of nodes
+% Dirichlet nodes
+Nodes_D=unique(cell2mat(set_D_nodes));
+% Neumann nodes
+Nodes_N=setdiff(unique(BC_nodes), Nodes_D);
+% Interior nodes
+Nodes_I=setdiff(1:nb_nodes, union(Nodes_D, Nodes_N));
+% Free nodes
+Nodes_F=union(Nodes_I, Nodes_N);
+
+% Correct the set for Neumann nodes
+for k=1:nb_N_BC
+    set_N_nodes{k}=setdiff(set_N_nodes{k}, Nodes_D);
+end
+
+
+% Define the sets of degrees of freedom
+Nd=Dof_set(:,Nodes_D);
+Nn=Dof_set(:,Nodes_N);
+Ni=Dof_set(:,Nodes_I);
+
+Ni=Ni(:);
+Nd=Nd(:);
+Nn=Nn(:);
+Nf=union(Ni, Nn);
+
+% Update data
+Data.Nodes_I=Nodes_I;
+Data.Nodes_N=Nodes_N;
+Data.Nodes_D=Nodes_D;
+Data.Nodes_F=Nodes_F;
+
+Data.Ni=Ni;
+Data.Nd=Nd;
+Data.Nn=Nn;
+Data.Nf=Nf;
+
+Data.set_D_nodes=set_D_nodes;
+Data.set_N_nodes=set_N_nodes;
+
+%% Auxiliary function
+
+    function[label]=detectDependency(func)
+        
+        string=func2str(func);
+        
+        if ~strcmp(string(1:4), '@(x)')
+            error('Function handle must contain space variables')
+        end
+        string=string(5:end);
+        
+        if contains(string, 'x') && contains(string, 't')
+            label='space_time';
+        elseif contains(string, 'x')
+            label='space';
+        elseif contains(string, 't')
+            label='time';
+        elseif contains(string(1), '0')
+            label='zero';
+        else
+            label='const';
+        end
+        
+    end
+
+end
diff --git a/Code/INTERNODES_medium/setCoefficient.m b/Code/INTERNODES_medium/setCoefficient.m
new file mode 100644
index 0000000..d15a956
--- /dev/null
+++ b/Code/INTERNODES_medium/setCoefficient.m
@@ -0,0 +1,103 @@
+function[Data]=setCoefficient(Data, varargin)
+
+% setCoefficient: Initializes the model coefficients
+% OPTIONAL INPUT:
+% rho:      Specific mass
+% mu:       Lamé constant
+% lambda:   Lamé constant
+% f:        Function "f" of the PDE
+% OUTPUT:
+% Data:     Data structure with initialized coefficients
+
+if mod(length(varargin), 2)~=0
+    error('Missing a name or value')
+end
+labels_in=varargin(1:2:end);
+values_in=varargin(2:2:end);
+
+available_coeff={'rho', 'lambda', 'mu', 'f'};
+
+for i=1:length(labels_in)
+    arg=lower(labels_in{i});
+    val=values_in{i};
+    
+    if any(ismember(available_coeff, arg))
+        
+        
+        switch arg
+            case {'rho', 'lambda', 'mu'} % Only space dependent
+                if isa(val,'double')
+                    
+                    if val ~= 0
+                        Data.Coefficient.(arg).label='const';
+                    else
+                        Data.Coefficient.(arg).label='zero';
+                    end
+                    Data.Coefficient.(arg).function=@(x) val;
+                    
+                elseif isa(val,'function_handle')
+                    Data.Coefficient.(arg).label=detectDependency1(val);
+                    Data.Coefficient.(arg).function=val;
+                else
+                    error('Input parameters must be either constants or function handles')
+                end
+                
+            case 'f'
+                if isa(val,'double') % Only space dependent
+                    Data.Coefficient.f.label='const';
+                    Data.Coefficient.f.function=@(x) val;
+                elseif isa(val,'function_handle')
+                    Data.Coefficient.f.label=detectDependency2(val);
+                    Data.Coefficient.f.function=@(x) val(x);
+                else
+                    error('Input parameters must be either constants or function handles')
+                end
+                
+        end
+        
+    else
+        error('Unrecognized coefficient')
+    end
+end
+
+
+    function[label]=detectDependency1(func)
+        
+        string=func2str(func);
+        
+        if ~strcmp(string(1:4), '@(x)')
+            error('Coefficient can only be constant or space dependent')
+        end
+        
+        if contains(string(5:end), 'x')
+            label='space';
+        elseif contains(string(5), '0')
+            label='zero';
+        else
+            label='const';
+        end
+    end
+
+    function[label]=detectDependency2(func)
+        
+        string=func2str(func);
+        
+        if ~strcmp(string(1:4), '@(x)')
+            error('Function handle must contain space variables')
+        end
+        string=string(5:end);
+        
+        if contains(string, 'x') && contains(string, 't')
+            label='space_time';
+        elseif contains(string, 'x')
+            label='space';
+        elseif contains(string, 't')
+            label='time';
+        elseif contains(string(1), '0')
+            label='zero';
+        else
+            label='const';
+        end
+        
+    end
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_medium/setModel.m b/Code/INTERNODES_medium/setModel.m
new file mode 100644
index 0000000..58c27a3
--- /dev/null
+++ b/Code/INTERNODES_medium/setModel.m
@@ -0,0 +1,75 @@
+function[Data]=setModel(varargin)
+
+% setModel: Initializes the numerical model
+% OPTIONAL INPUT:
+% model:        Specifies which model should be used
+%               Default: transient_heat
+% submodel:     Specifies which submodel from the specified model should be
+%               used
+%               Default: standard
+% type:         Specifies whether the model is linear or nonlinear
+%               Default: linear
+% OUTPUT:
+% Data:         Data structure with initialized model
+
+if mod(length(varargin), 2)~=0
+    error('Missing a name or value')
+end
+labels_in=varargin(1:2:end);
+values_in=varargin(2:2:end);
+
+models={'transient_heat', 'static_solid', 'dynamic_solid'};
+n=length(models);
+submodels=cell(n,1);
+submodels{1}={'standard'};
+submodels{2}={'standard', 'contact'};
+submodels{3}={'standard'};
+types={'linear', 'nonlinear'};
+
+% Set default Parameters
+Data.Model.name=models{1};
+Data.Model.submodel=submodels{1};
+Data.Model.type=types{1};
+
+% Define constants
+% Stefan Boltzman constant W/(m^2*K^4)
+Data.Constants.sigma=5.670373e-8;
+
+for i=1:length(labels_in)
+    arg=lower(labels_in{i});
+    val=lower(values_in{i});
+    
+    switch arg
+        case 'model'
+            if any(ismember(models, val))
+                Data.Model.name=val;
+            else
+                error('Unrecognized model')
+            end
+            
+        case 'submodel'
+            
+            if isfield(Data.Model, 'name')
+                index=strcmp(Data.Model.name, models);
+            else
+                error('Submodel must be defined after the model')
+            end
+            if any(ismember(submodels{index}, val))
+                Data.Model.submodel=val;
+            else
+                error('Unrecognized submodel')
+            end
+            
+        case 'type'
+            if any(ismember(types, val))
+                Data.Model.type=val;
+            else
+                error('Unrecognized type')
+            end     
+            
+        otherwise
+            error('Unrecognized argument')
+    end
+    
+end
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_medium/setNumericalParameters.m b/Code/INTERNODES_medium/setNumericalParameters.m
new file mode 100644
index 0000000..3cbd970
--- /dev/null
+++ b/Code/INTERNODES_medium/setNumericalParameters.m
@@ -0,0 +1,270 @@
+function[Data]=setNumericalParameters(Mesh, Data, varargin)
+
+% setNumericalParameters: Initializes the numerical parameters of the
+% solver
+% OPTIONAL INPUT:
+% Contact:
+%   iterate:            Parameter controlling whether iterations should 
+%                       take place (1) or not (0)
+%                       Default: 1
+%   maxiter:            Maximum number of iterations for solving the
+%                       contact problem
+%                       Default: 10
+%   rescaling:          Rescaling parameter for the stiffness matrix
+%                       Default: 1
+%   alpha:              Parameter for the preconditioner
+%                       Default: auto 
+%   c:                  Number of nearest neighbors for computing the
+%                       radius of support of the radial basis functions
+%                       Default: 1
+%   radius:             Intersection radius used to define the initial
+%                       potential contact interface. 
+%                       Default: infinity
+%   gamma:              Value of gamma for preventing quick loss of diagonal
+%                       dominance of the matrices PhiMM
+%                       Default: 0.5
+%   Gamma:              Value of Gamma to avoid very small nonzero entries
+%                       in the matrices PhiNM
+%                       Default: 0.95
+%   d0:                 Measure of tolerance for contact detection.
+%                       Default: 0.05
+%   tol:                Tolerance for gap detection
+%                       Default: 1e-1
+% Solver:
+%   name:               Name of the solver to be used.
+%                       Default: GMRES
+%   maxiter:            Total number of GMRES iterations
+%                       Default: 1000
+%   riter:              Number of iterations before GMRES restarts
+%                       Default: 20
+%   tol:                Tolerance (stopping creterion) for GMRES
+%                       Default: 1e-7
+%   reorth_tol:         Re-orthogonalization tolerance for GMRES
+%                       Default: 0.7
+% PostProcessing:
+%   quantity:           Additional quantities to be computed (for example
+%                       stresses, strains, fluxes,...)
+%                       Default: none
+%   ampl_fact:          Value of amplification factor to apply to
+%                       displacements
+%                       Default: auto
+%   eval_points:        Number of evaluation points used for computing
+%                       stresses or strains
+%                       Default: 3 (P1 finite elements)
+%                                6 (P2 or higher order finite elements)                
+
+if nargin>2
+if mod(length(varargin)-1, 2)~=0
+    error('Missing a name or value')
+end
+
+id=lower(varargin{1});
+labels_in=varargin(2:2:end);
+values_in=varargin(3:2:end);
+end
+
+% Set default Parameters
+if ~isfield(Data, 'Contact')
+    % Boolean indicator for contact algorithm iterations
+    Data.Contact.iterate=true;
+    % Maximum number of iterations of the contact algorithm
+    Data.Contact.maxiter=10;
+    % Rescaling parameter for the INTERNODES matrix
+    Data.Contact.rescaling=1;
+    % Parameter alpha of the preconditioner
+    Data.Contact.alpha='auto';
+    % Number of nearest neighbors
+    Data.Contact.c=1;
+    % Intersection radius
+    Data.Contact.radius=inf;
+    % gamma value
+    Data.Contact.gamma=0.5;
+    % Gamma value
+    Data.Contact.Gamma=0.95;
+    % Contact tolerance
+    Data.Contact.d0=0.05;
+    % Gap tolerance
+    Data.Contact.tol=1e-1;
+end
+if ~isfield(Data, 'Solver')  
+    % Solver name
+    Data.Solver.name='GMRES';
+    % GMRES parameters
+    % Total maximum number of iterations
+    Data.Solver.maxiter=1000;
+    % Maximum number of iterations before restart
+    Data.Solver.riter=20;
+    % Tolerance on the residual
+    Data.Solver.tol=1e-7;
+    % Reorthogonalization tolerance
+    Data.Solver.reorth_tol=0.7;
+end
+if ~isfield(Data, 'PostProcessing')   
+    % Additional quantities to be computed
+    Data.PostProcessing.quantity='none';
+    % Amplification factor
+    Data.PostProcessing.amplification='auto';
+    % Number of evaluation points per element for computing stresses/strains
+    eval_points={3,6};
+    
+    if Mesh.order==1
+        Data.PostProcessing.eval_points=eval_points{1};
+    else
+        Data.PostProcessing.eval_points=eval_points{2};
+    end
+    
+end
+
+% Override the defaults
+if exist('id', 'var')
+    switch id
+        case 'contact'
+            %% Contact algorithm specific parameters
+            
+            for i=1:length(labels_in)
+                arg=lower(labels_in{i});
+                val=lower(values_in{i});
+                
+                switch arg
+                    case 'iterate'
+                        if isa(val, 'double')
+                            
+                            if val<0 || val>1
+                                error([arg ' parameter must be a boolean'])
+                            else
+                                Data.Contact.(arg)=logical(val);
+                            end
+                            
+                        elseif isa(val, 'logical')
+                            Data.Contact.(arg)=val;
+                        else
+                            error([arg ' parameter must be a boolean'])
+                        end
+                        
+                    case {'maxiter', 'c', 'radius', 'd0'}
+                        if val<=0
+                            error(['Parameter ' arg ' must be strictly greater than zero'])
+                        else
+                            Data.Contact.(arg)=val;
+                        end
+                        
+                    case 'rescaling'
+                        if val==0
+                            error(['Parameter ' arg ' must be different from zero'])
+                        else
+                            Data.Contact.(arg)=val;
+                        end
+                        
+                    case {'gamma', 'Gamma', 'tol'}
+                        if val<=0 || val>=1
+                            error([arg ' must be strictly greater than 0 and strictly smaller than 1'])
+                        else
+                            Data.Contact.(arg)=val;
+                        end
+                        
+                    case 'alpha'
+                        if isa(val, 'char')
+                            if ~strcmp(val, 'auto')
+                                error('Invalid alpha parameter')
+                            else
+                                Data.Contact.(arg)=val;
+                            end
+                            
+                        elseif isa(val, 'double')
+                            if val == 0
+                                error('Invalid alpha parameter')
+                            else
+                                Data.Contact.(arg)=val;
+                            end
+                        else
+                            error('Invalid alpha parameter')
+                        end
+                        
+                    otherwise
+                        error('Unrecognized argument')
+                end
+            end
+            
+            
+        case 'solver'
+            %% Solver parameters
+            names={'GMRES'};
+            
+            for i=1:length(labels_in)
+                arg=lower(labels_in{i});
+                val=values_in{i};
+                
+                switch arg
+                    case 'name'
+                        if any(ismember(names, val))
+                            Data.Solver.(arg)=val;
+                        else
+                            error('Unrecognized solver name')
+                        end
+                        
+                    case {'maxiter', 'riter', 'tol', 'reorth_tol'}
+                        if val<=0
+                            error(['Parameter ' arg ' must be strictly positive'])
+                        else
+                            Data.Solver.(arg)=val;
+                        end
+                        
+                    otherwise
+                        error('Unrecognized argument')
+                end
+                
+            end
+            
+        case 'postprocessing'
+            %% Post-processing parameters
+            quantities={'none', 'flux', 'error', 'stress', 'strain'};
+            eval_points={3,6};
+            
+            for i=1:length(labels_in)
+                arg=lower(labels_in{i});
+                val=values_in{i};
+                
+                switch arg
+                    case 'quantity'
+                        if any(ismember(quantities, val))
+                            Data.PostProcessing.quantity=val;
+                        else
+                            error('Unrecognized quantity')
+                        end
+
+                    case 'eval_points'
+                        if any(ismember(eval_points, val))
+                            Data.PostProcessing.eval_points=val;
+                        else
+                            error('Invalid number of evaluation points')
+                        end
+                        
+                    case 'ampl_fact'
+                        
+                        if isa(val, 'char')
+                            if ~strcmp(val, 'auto')
+                                error('Invalid amplification factor')
+                            else
+                                Data.PostProcessing.amplification=val;
+                            end
+                            
+                        elseif isa(val, 'double')
+                            if val <= 0
+                                error('Invalid amplification factor')
+                            else
+                                Data.PostProcessing.amplification=val;
+                            end
+                        else
+                            error('Invalid amplification factor')
+                        end
+                        
+                    otherwise
+                        error('Unrecognized argument')
+                end
+            end
+            
+        otherwise
+            error('Unrecognized argument')
+    end
+end
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_medium/solve.m b/Code/INTERNODES_medium/solve.m
new file mode 100644
index 0000000..8eff1ba
--- /dev/null
+++ b/Code/INTERNODES_medium/solve.m
@@ -0,0 +1,94 @@
+function[Mesh, Data, Solution]=solve(Mesh, Data, it)
+
+% solve: Solves the contact problem
+% INPUT:
+% Mesh:     Structure containing all the mesh parameters
+% Data:     Structure containing all the data parameters
+% it:       Cell array containing the boundary tags of the interfaces (master
+%           interface and slave interface)
+% OUTPUT:
+% Mesh:     Updated structure containing all the mesh parameters
+% Data:     Updated structure containing all the data parameters
+% Solution: Structure containing the solution
+
+
+% Initialization of the system
+[Data, Solution]=initializeSystem(Mesh, Data, it);
+% Initialize count
+n=1;
+
+while Data.Contact.iterate && n<= Data.Contact.maxiter
+    
+    % Solve the system of equations
+    [Solution, Data]=solveSystem(Mesh, Data, Solution);
+    % Update the system
+    [Data]=updateSystem(Mesh, Data, Solution);
+    n=n+1;
+end
+
+if n > Data.Contact.maxiter && Data.Contact.iterate
+    warning('Failed to converge within the specified number of iterations')
+end
+
+    function[Solution, Data]=solveSystem(Mesh, Data, Solution)
+        
+        % Retrieve data parameters
+        % Free degrees of freedom
+        Nf=Data.Nf;
+        % Number of free degrees of freedom
+        nf=Data.nf;
+        % Number of constraints
+        nc=Data.nc;
+        
+        % Retrieve mesh parameters
+        d=Mesh.nb_dof_local;
+        
+        % Retrieve solution
+        U=Solution.U;
+        % Solve the linear system iteratively
+        [x, Data]=iterativeSolver(Mesh, Data);
+        % Retrieve the displacements
+        U(Nf)=x(1:nf);
+        
+        % Update solution
+        Solution.U=U;
+        Solution.U_sort=reshape(U, d, [])';
+        Solution.lambda=Data.Contact.rescaling*x(nf+1:nf+nc);
+        Solution.lambda_sort=reshape(Solution.lambda, d, [])';
+        
+    end
+
+    function[x, Data]=iterativeSolver(Mesh, Data)
+        
+        b=Data.b;
+        [Data]=preCompute(Mesh, Data);
+        
+        if strcmp(Data.Solver.name,'GMRES')
+            x0=zeros(length(b),1);
+            [x, ~] = gmres_k(Mesh, Data, @linearOp1, b, x0);
+            
+        else
+            error('Solver unrecognized or not yet implemented')
+        end
+        
+    end
+        
+    function[Data]=updateSystem(Mesh, Data, Solution)
+        
+        % updateSystem: Updates all quantities which have changed
+        % INPUT:
+        % Mesh:     Structure containing all the mesh parameters
+        % Data:     Structure containing all the data parameters
+        % Solution: Solution structure with computed displacements
+        % OUPUT:
+        % Data:     Data structure with updated interface properties
+        
+        % Check for convergence
+        [Data]=removeTraction(Mesh, Data, Solution, n);
+        % Reassemble the interpolation matrices and update interface
+        [Data]=subAssemble(Mesh, Data, Solution, 'updating');
+        % Update right-hand side
+        [Data]=updateRHS(Mesh, Data);
+        
+    end
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_medium/subAssemble.m b/Code/INTERNODES_medium/subAssemble.m
new file mode 100644
index 0000000..f58cc86
--- /dev/null
+++ b/Code/INTERNODES_medium/subAssemble.m
@@ -0,0 +1,204 @@
+function[Data]=subAssemble(Mesh, Data, Solution, operation)
+
+% subAssemble: Assembles the part of the internodes matrix which is
+% changing from one iteration to the next
+% INPUT:
+% Mesh:         Structure containing all the mesh parameters
+% Data:         Structure containing all the data parameters
+% Solution:     Structure containing the solution
+% operation:    Specifies the type of operation (initialization or
+%               updating)
+% OUTPUT:
+% Data:         Updated structure containing all the data parameters
+
+if strcmp(operation, 'initialization')
+    [Data]=computeBoundaryMass(Mesh, Data, 'initialization');
+end
+
+% Assemble interpolation matrices
+[Data]=assembleRs(Mesh, Data, Solution);
+% Assemble boundary mass matrices
+[Data]=computeBoundaryMass(Mesh, Data, 'updating');
+
+    function[Data]=assembleRs(Mesh, Data, Solution)
+        
+        % assembleRs: Constructs the interpolation matrices
+        % INPUT:
+        % Mesh:     Structure containing all the mesh parameters
+        % Data:     Structure containing all the data parameters
+        % Solution: Structure containing the solution
+        % OUTPUT:
+        % Data:     Updated data structure with interpolation matrices
+        
+        % Retrieve nodes making up the interfaces
+        nodes1=Data.body{1}.interface_nodes;
+        nodes2=Data.body{2}.interface_nodes;
+        
+        % 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 distant bodies
+        if all(i1) || all(i2)
+            warning('One of the bodies will be translated because they are too far away from each other. Boundary conditions might be affected')
+            [Mesh, coord1, coord2]=translateBodies(Mesh, Data, radiuses1, radiuses2, nodes1, nodes2, coord1, coord2);
+            
+            % 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
+        
+        % Check for isolated nodes
+        while any(i1) || any(i2)
+            
+            dump_nodes1=nodes1(i1);
+            dump_nodes2=nodes2(i2);
+            
+            % Updating
+            [Data]=updateInterface(Mesh, Data, 1, dump_nodes1, 'dump');
+            [Data]=updateInterface(Mesh, Data, 2, dump_nodes2, 'dump');
+            
+            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;
+        
+        Data.R12=R12;
+        Data.R21=R21; 
+        
+        % For future purposes
+        Data.Phi11=Phi11;
+        Data.Phi21=Phi21;
+        Data.Phi12=Phi12;
+        Data.Phi22=Phi22;
+        
+        Data.s1=s1;
+        Data.s2=s2;
+        
+    end
+
+    function[Data]=computeBoundaryMass(Mesh, Data, operation)
+        
+        % computeBoundaryMass: Assembles the boundary mass matrices
+        % INPUT:
+        % Mesh:         Structure containing all the mesh parameters
+        % Data:         Structure containing all the data parameters
+        % operation:    Specifies the type of operation (initialization or
+        %               updating)
+        % OUTPUT:
+        % Data:         Updated data structure with boundary mass matrices
+        
+        
+        % Retrieve mesh parameters
+        % Number of local degrees of freedom
+        nb_dof_local=Mesh.nb_dof_local;
+        % Coordinate matrix
+        coord=Mesh.coord_mat;
+        % Order
+        order=Mesh.order;
+        % Get quadrature nodes and weights
+        [points, weights]=getQuadrature(3, 'boundary');
+        % Number of quadrature points
+        nb_points=length(weights);
+        
+        % Retrive functions
+        [Functions]=getFunctions(order);
+        % Retrieve boundary basis functions
+        phi_boundary=Functions.phi_boundary;
+        
+        % Retrieve bodies
+        body=Data.body;
+        % Number of bodies
+        nb_bodies=length(body);
+        
+        if strcmp(operation, 'initialization')
+            
+            for n=1:nb_bodies
+                % Retrieve data
+                % Retriveve edges making up the interface
+                Edges=body{n}.interface_connect;
+                % Retrieve nodes making up the interface
+                nodes=body{n}.interface_nodes;
+                nb_nodes=length(nodes);
+                s=order+1;
+                
+                % Initialization
+                Phi=zeros(s, nb_points);
+                
+                % Map global nodes to local nodes
+                func=@(x) find(nodes==x);
+                connect=arrayfun(func, Edges);
+                T=connect';
+                G=T(:);
+                
+                I=repmat(connect, [1 s])';
+                J=repmat(G', [s 1]);
+                
+                % Loop over the Gauss points
+                for i = 1:nb_points
+                    Phi(:,i)=phi_boundary(points(i,:));
+                end
+                
+                Tr=Edges';
+                Gr=Tr(:);
+                % Coordinates of the nodes of the elements
+                coord_nodes=coord(Gr,:);
+                % Endpoints of the edges
+                a=coord_nodes(1:s:end,:);
+                b=coord_nodes(2:s:end,:);
+                bk=b-a;
+                norm_bk=vecnorm(bk,2,2);
+                
+                lambda=weights.*norm_bk';
+                
+                M=kr(Phi, Phi)*lambda;
+                M=sparse(I(:), J(:), M(:), nb_nodes, nb_nodes);
+                
+                Data.body{n}.iinterface_mass=M;
+                % Expansion to full size
+                M=kron(M, speye(nb_dof_local));
+                Data.body{n}.interface_mass=M;
+            end
+        else
+            for n=1:nb_bodies
+                active_set=Data.body{n}.active_set;                
+                Data.body{n}.interface_mass=Data.body{n}.iinterface_mass(active_set, active_set);
+            end
+        end
+    end
+
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_medium/translateBodies.m b/Code/INTERNODES_medium/translateBodies.m
new file mode 100644
index 0000000..f5a398f
--- /dev/null
+++ b/Code/INTERNODES_medium/translateBodies.m
@@ -0,0 +1,60 @@
+function[Mesh, coord1, coord2]=translateBodies(Mesh, Data, radiuses1, radiuses2, nodes1, nodes2, coord1, coord2)
+
+% translateBodies: Performs a translation of one of the bodies in case the
+% initial potential contact interface is empty (because the bodies are too 
+% far away from each other)
+% INPUT:
+% Mesh:         Structure containing all the mesh parameters
+% Data:         Structure containing all the data parameters
+% radiusesk:    Radiuses of support of the radial basis functions (k=1,2)
+% nodesk:       Nodes on the potential contact interface of body k (k=1,2)
+% coordk:       Coordinates of the nodes on the potential contact interface 
+%               of body k (k=1,2)
+% OUTPUT:
+% Mesh:         Mesh structure with updated coordinate matrix
+% coordk:       New coordinates of the nodes on the potential contact 
+%               interface of body k (k=1,2)
+
+n1=length(radiuses1);
+
+dist=inf;
+node1=0;
+node2=0;
+
+for k=1:n1
+    
+    [d, i]=min(vecnorm(coord1(k,:)-coord2,2,2));
+    
+    if d<dist
+        dist=d;
+        node1=nodes1(k);
+        node2=nodes2(i);
+    end
+end
+
+% Check for the most restrictive
+i1=node1==nodes1;
+i2=node2==nodes2;
+
+r1=radiuses1(i1);
+r2=radiuses2(i2);
+
+shift=max([dist-r1, dist-r2]);
+
+% Add safety coefficient
+shift=(1+5e1*Data.Contact.h)*shift;
+
+r=shift/dist*(coord2(i2,:)-coord1(i1,:));
+
+% Change the coordinates of all nodes of body 1
+% !! Assumes body 1 has the smallest material tag !!
+coord=Mesh.coord_mat;
+mt=Mesh.material_tag;
+tags=unique(mt);
+N1=unique(Mesh.connect_mat(mt==tags(1),:));
+coord(N1,:)=coord(N1,:)+r;
+Mesh.coord_mat=coord;
+
+coord1=coord(nodes1,:);
+coord2=coord(nodes2,:);
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_medium/updateInterface.m b/Code/INTERNODES_medium/updateInterface.m
new file mode 100644
index 0000000..eb45955
--- /dev/null
+++ b/Code/INTERNODES_medium/updateInterface.m
@@ -0,0 +1,45 @@
+function[Data, nb_nodes]=updateInterface(Mesh, Data, body_tag, nodes, operation)
+
+% updateInterface: Updates the interface properties
+% INPUT:
+% Mesh:         Structure containing all the mesh parameters
+% Data:         Structure containing all the data parameters
+% body_tag:     Tag of the body to be updated
+% nodes:        Nodes to be added or removed from the potential contact interface
+% operation:    Specifies whether nodes should be added (add) or removed (dump)            
+% OUTPUT:
+% Data:         Updated data structure with interface properties
+% nb_nodes:     Number of nodes added or removed
+
+% Retrieve initial interface connectivity matrix
+iIc=Data.body{body_tag}.initial_interface_connect;
+% Retrieve initial set of nodes
+iIn=Data.body{body_tag}.initial_nodes;
+% Retrieve current set of nodes
+In=Data.body{body_tag}.interface_nodes;
+% Update interface connectivity matrix
+switch operation
+    case 'dump'
+        new_nodes=setdiff(In, nodes);
+        indx=all(ismember(iIc,new_nodes),2);
+        new_interface_connect=iIc(indx,:);
+    case 'add'
+        new_nodes=union(In, nodes);
+        indx=all(ismember(iIc,new_nodes),2);
+        new_interface_connect=iIc(indx,:);
+end
+% Update active set of nodes
+active_set=ismember(iIn, new_nodes);
+% Update interface degrees of freedom
+new_interface_dof=Mesh.Dof_set(:,new_nodes);
+new_interface_dof=new_interface_dof(:);
+% Update body properties
+Data.body{body_tag}.interface_nodes=new_nodes(:);
+Data.body{body_tag}.active_set=active_set;
+Data.body{body_tag}.interface_dof=new_interface_dof;
+Data.body{body_tag}.interface_connect=new_interface_connect;
+% Update mapping of degrees of freedom
+[Data]=mapGlobal2Local(Data);
+% Change in the number of nodes
+nb_nodes=length(new_nodes)-length(In);
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_medium/updateRHS.m b/Code/INTERNODES_medium/updateRHS.m
new file mode 100644
index 0000000..a3c0daf
--- /dev/null
+++ b/Code/INTERNODES_medium/updateRHS.m
@@ -0,0 +1,37 @@
+function[Data]=updateRHS(Mesh, Data)
+
+% updateRHS: Updates the right-hand side vector for the next iteration of
+% the contact algorithm
+% INPUT:
+% Mesh:     Structure containing all the mesh parameters
+% Data:     Structure containing all the data parameters
+% OUTPUT:
+% Data:     Updated data structure
+
+% Retrieve mesh parameters
+% Coordinate matrix
+coord=Mesh.coord_mat;
+
+% Retrieve data parameters
+% Number of free degrees of freedom
+nf=Data.nf;
+% Number of constraints
+nc=length(Data.body{1}.interface_dof);
+% Retrieve nodes making up the interfaces
+nodes1=Data.body{1}.interface_nodes;
+nodes2=Data.body{2}.interface_nodes;
+% Retrieve coordinates of nodes making up the interfaces
+coord1=coord(nodes1,:);
+coord2=coord(nodes2,:);
+
+Diff=Data.R12*coord2-coord1;
+
+% Initialization
+b=zeros(nf+nc,1);
+b(1:nf)=Data.bc;
+b(nf+1:nf+nc)=reshape(Diff', [], 1);
+
+% Update data structure
+Data.b=b;
+Data.nc=nc;
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_small/assembleInternodesMat.m b/Code/INTERNODES_small/assembleInternodesMat.m
new file mode 100644
index 0000000..bacf225
--- /dev/null
+++ b/Code/INTERNODES_small/assembleInternodesMat.m
@@ -0,0 +1,68 @@
+function[A, b]=assembleInternodesMat(Mesh, Data, Solution, K)
+
+% assembleInternodesMat: Assembles the entire INTERNODES matrix
+% INPUT:
+% Mesh:     Structure containing all the mesh parameters
+% Data:     Structure containing all the data parameters
+% Solution: Structure containing the solution
+% K:        Global stiffness matrix
+% OUTPUT:
+% A:        Global INTERNODES matrix
+% b:        Global right-hand side
+
+% Retrieve mesh parameters
+% Coordinate matrix
+coord=Mesh.coord_mat;
+
+% Retrieve data parameters
+% Free degrees of freedom
+Nf=Data.Nf;
+% Blocked degrees of freedom
+Nd=Data.Nd;
+% Retrieve bodies
+body=Data.body;
+% Right-hand side force vector
+f=Data.F;
+% Rescaling parameter
+r=Data.Contact.rescaling;
+
+% Retrive solution
+U=Solution.U;
+
+% Number of free degrees of freedom
+nf=length(Nf);
+% Number of constraints
+nc=length(body{1}.interface_dof);
+% Retrieve local degrees of freedom
+indx1=body{1}.indx_local;
+indx2=body{2}.indx_local;
+% Retrieve nodes making up the interfaces
+nodes1=Data.body{1}.interface_nodes;
+nodes2=Data.body{2}.interface_nodes;
+% Retrieve coordinates of nodes making up the interfaces
+coord1=coord(nodes1,:);
+coord2=coord(nodes2,:);
+
+% Initialization
+b=zeros(nf+nc,1);
+B=sparse(nf,nc);
+B_tilde=sparse(nc,nf);
+C=sparse(nc,nc);
+
+Kff=1/r*K(Nf, Nf);
+Diff=Data.R12_normal*coord2-coord1;
+
+% Assemblage
+% Right-hand side vector
+b(1:nf)=1/r*(f(Nf)-K(Nf,Nd)*U(Nd));
+b(nf+1:nf+nc)=reshape(Diff', [],1);
+% B and B_tilde
+B(indx1,:)=-body{1}.interface_mass;
+B(indx2,:)=body{2}.interface_mass*Data.R21;
+B_tilde(:,indx1)=speye(nc,nc);
+B_tilde(:,indx2)=-Data.R12;
+% INTERNODES matrix
+A1=horzcat(Kff, B);
+A2=horzcat(B_tilde, C);
+A=vertcat(A1, A2);
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_small/assembleInterpMat.m b/Code/INTERNODES_small/assembleInterpMat.m
new file mode 100644
index 0000000..959d585
--- /dev/null
+++ b/Code/INTERNODES_small/assembleInterpMat.m
@@ -0,0 +1,34 @@
+function [Phi] = assembleInterpMat(coord_ref, coord_interp, radiuses)
+
+% assembleInterpMat: Assemblage of the interpolation matrices for the
+% localized radial basis functions in dimension d
+% INPUT:
+% coord_ref:    Coordinate matrix of the reference points (size m x d)
+% coord_interp: Coordinate matrix for the interpolation points (size n x d)
+% radiuses:     Vector of radiuses of the radial basis functions. It is a row
+%               vector (size 1 x m)
+% OUTPUT:
+% Phi:          Interpolation matrix
+
+l=size(radiuses,1);
+if l>1
+    radiuses=radiuses';
+end
+
+m=size(coord_ref,1);
+n=size(coord_interp,1);
+dim=size(coord_ref,2);
+
+X_ref=reshape(coord_ref,1,m,dim);
+X_interp=reshape(coord_interp,n,1,dim);
+
+X=X_interp-X_ref;
+% 2-norm (euclidean norm) along the third dimension
+N=vecnorm(X,2,3);
+indx=N<=radiuses;
+Phi=zeros(n,m);
+
+% Wendland C^2 radial basis functions
+F=(1-N./radiuses).^4.*(1+4*N./radiuses);
+Phi(indx)=F(indx);
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_small/computeNormals.m b/Code/INTERNODES_small/computeNormals.m
new file mode 100644
index 0000000..9a30e45
--- /dev/null
+++ b/Code/INTERNODES_small/computeNormals.m
@@ -0,0 +1,77 @@
+function[Data]=computeNormals(Mesh, Data, Solution)
+
+% computeNormals: Computes the normal vectors for each interface
+% INPUT:
+% Mesh:     Structure containing all the mesh parameters
+% Data:     Structure containing all the data parameters
+% Solution: Structure containing the solution
+% OUTPUT:
+% Data:     Updated structure with the interface normals
+
+% Retrieve mesh data
+% Connectivity matrix
+connect=Mesh.connect_mat;
+
+% Local number of degrees of freedom (dimension in solid mechanics)
+d=Mesh.nb_dof_local;
+% Retrieve bodies
+body=Data.body;
+% Compute the normals based on the deformed structure
+def=Mesh.coord_mat+Solution.U_sort;
+
+nb_bodies=length(body);
+
+
+for k=1:nb_bodies
+    % Normal vectors are computed for all nodes in the initial potential 
+    % contact interface
+    connecti=body{k}.initial_interface_connect;
+    nodesi=body{k}.initial_nodes;
+    
+    % Reduced connectivity matrix
+    connect_red=connect(any(ismember(connect, nodesi), 2), :);
+    % Reduced set of body nodes
+    nodesb=setdiff(unique(connect_red), nodesi);  
+    n=length(nodesi);
+    m=size(connecti,1);
+    % Coordinates of the endpoints of the segments
+    coord1=def(connecti(:,1),:);
+    coord2=def(connecti(:,2),:);
+    % Tangent vectors
+    V=coord2-coord1;
+    % Segment lengths
+    L=vecnorm(V,2,2);
+    V=V./vecnorm(V,2,2);
+
+    % !! Only for a two-dimensional case !!
+    N=zeros(m, d);
+    N(:,1)=-V(:,2);
+    N(:,2)=V(:,1);
+    
+    % Initialization
+    normals=zeros(n,d);
+    % Step size
+    gamma=1e-3;
+    
+    for j=1:n
+        [id,~]=find(connecti==nodesi(j));
+        l=L(id);
+        % Weighted average of normal vectors
+        normals(j,:)=1/sum(l)*sum(N(id,:).*l,1);
+        % Current node
+        x=def(nodesi(j),:);
+        % Step in the direction of the gradient
+        y_plus=x+gamma*normals(j,:);
+        % Step in the opposite direction of the gradient
+        y_minus=x-gamma*normals(j,:);
+        min_plus=min(vecnorm(def(nodesb,:)-y_plus,2,2));
+        min_minus=min(vecnorm(def(nodesb,:)-y_minus,2,2));
+        if min_plus < min_minus
+            normals(j,:)=-normals(j,:);
+        end
+    end
+    
+    normals=normals./vecnorm(normals,2,2);
+    Data.body{k}.normal=normals;
+end
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_small/computeRHS.m b/Code/INTERNODES_small/computeRHS.m
new file mode 100644
index 0000000..b6f4112
--- /dev/null
+++ b/Code/INTERNODES_small/computeRHS.m
@@ -0,0 +1,223 @@
+function[Data]=computeRHS(Mesh, Data)
+
+% computeRHS: Function which assembles the right-hand side consisting of
+% body forces and surface tractions
+% INPUT:
+% Mesh:             Structure containing the mesh parameters
+% Data:             Structure containing the data parameters
+% OUTPUT:
+% Data:             Updated data structure
+
+% Assemble the global vector of body forces
+[Bs]=computeBody(Mesh, Data);
+% Assemble the global vector of surface tractions
+[Bn]=computeNeumann(Mesh, Data);
+
+% Sum the contribution from body forces and Neumann boundary
+% conditions
+B=Bs+Bn;
+% Update data
+Data.F=B;
+
+    function[B]=computeBody(Mesh, Data)
+        
+        % computeBody: Assembles the component for the body forces
+        % INPUT:
+        % Mesh:     Structure containing all the mesh parameters
+        % Data:     Structure containing all the data parameters
+        % OUTPUT:
+        % B:        Global vector of body forces
+        
+        % Retrieve mesh data
+        % Local number of degrees of freedom (dimension in solid mechanics)
+        d=Mesh.nb_dof_local;
+        % Total number of degrees of freedom
+        nb_dof=Mesh.nb_dof;
+        % Coordinate matrix
+        coord=Mesh.coord_mat;
+        % Connectivity matrix
+        connect=Mesh.connect_mat;
+        % Finite element order
+        order=Mesh.order;
+        % Number of elements
+        ne=Mesh.nb_elem;
+        % Number of nodes per element
+        nne=Mesh.nb_nodes_elem;
+        % Matrix of equation numbers linking an element to the degrees of freedom
+        % associated to it.
+        Eq=Mesh.Eq;
+        
+        % Retrieve data parameters
+        % Body force vector
+        f=Data.Coefficient.f.function;
+        % Retrieve the basis functions
+        [Functions]=getFunctions(order);
+        % Retrieve basis functions
+        phi=Functions.phi;
+        
+        
+        % Retrieve quadrature points and weights
+        switch order
+            case 1
+                [points, weights] = getQuadrature(3, 'bulk');
+            case 2
+                [points, weights] = getQuadrature(4, 'bulk');
+            case 3
+                [points, weights] = getQuadrature(6, 'bulk');
+            otherwise
+                error('Not yet implemented');
+        end
+        
+        % Number of quadrature points
+        nq=length(weights);
+        s=nne*d;
+        Id=eye(d);
+        % Initialization
+        X=zeros(ne, nq, 2);
+        Q=zeros(s, nq*d);
+        
+        
+        % Loop over the Gauss points
+        for i = 1:nq
+            Phi=phi(points(i,:));
+            Q(:, (i-1)*d+1:i*d)=kron(Phi, Id);
+        end
+        
+        I=Eq';
+        T=connect';
+        G=T(:);
+        
+        % Coordinates of the nodes of the elements
+        coord_nodes=coord(G,:);
+        % Vertices of the elements
+        a=coord_nodes(1:nne:end,:);
+        b=coord_nodes(2:nne:end,:);
+        c=coord_nodes(3:nne:end,:);
+        % Interpolated coordinates
+        P=a(:)+[b(:)-a(:) c(:)-a(:)]*points';
+        X(:,:,1)=P(1:ne,:);
+        X(:,:,2)=P(ne+1:end,:);
+        
+        % Determinants of Jacobian matrices
+        detJK=(b(:,1)-a(:,1)).*(c(:,2)-a(:,2))-(b(:,2)-a(:,2)).*(c(:,1)-a(:,1));
+        
+        % Evaluation at the Gauss points
+        F=f(X);
+        if size(F,1)>d
+            F=reshape(F, ne, d*nq);
+            F=reshape(F', d, nq, ne);
+        end
+        
+        X=reshape(abs(detJK), 1, 1, ne).*F.*weights;
+        X=reshape(X, d*nq, ne);
+        
+        B=Q*X;
+        
+        % Assemblage
+        B=accumarray(I(:), B(:), [nb_dof 1]);
+    end
+
+    function[B]=computeNeumann(Mesh, Data)
+        
+        % computeNeumann: Implements Neumann boundary conditions, returns a vector
+        % having for length the number of degrees of freedom.
+        % This vector is to be added to the RHS.
+        % INPUT:
+        % Mesh:     Structure containing all the mesh parameters
+        % Data:     Structure containing all the data parameters
+        % OUTPUT:
+        % B:        Global vector of surface tractions
+        
+        % Retrieve mesh parameters
+        % Total number of degrees of freedom
+        nb_dof=Mesh.nb_dof;
+        % Number of local degrees of freedom
+        d=Mesh.nb_dof_local;
+        % Coordinate matrix
+        coord=Mesh.coord_mat;
+        % Order
+        order=Mesh.order;
+        % Boundary equation numbers
+        Eq_BC=Mesh.Eq_BC;
+        % Retrive functions
+        [Functions]=getFunctions(order);
+        % Retrieve boundary basis functions
+        phi_boundary=Functions.phi_boundary;
+        % Get quadrature
+        [points, weights]=getQuadrature(3, 'boundary');
+        % Number of quadrature points
+        nq=length(weights);
+        
+        % Initialization
+        B=zeros(nb_dof,1);
+        s=order+1;
+        Id=eye(d);
+        % Initialization
+        Q=zeros(s*d, nq*d);
+        
+        % Loop over the Gauss points
+        for i = 1:nq
+            Phi=phi_boundary(points(i,:));
+            Q(:, (i-1)*d+1:i*d)=kron(Phi, Id);
+        end
+        
+        
+        % Neumann boundary conditions
+        N_BC=Data.N_BC;
+        % Number of boundaries where surface tractions are imposed
+        nb_bc=length(N_BC);
+        
+        for m=1:nb_bc
+            % Retrieve data
+            data=N_BC{m};
+            edges=data.edges;
+            
+            for flag=edges
+                % Surface traction function
+                f=data.function;
+                % Retriveve edges making up the boundary
+                Neum_edges=Mesh.BC_nodes(Mesh.BC_tag==flag,:);
+                % Equation numbers
+                Eq=Eq_BC(Mesh.BC_tag==flag,:);
+                % Number of Neumann boundary edges
+                ne=size(Neum_edges, 1);
+                
+                % Initialization
+                X=zeros(ne, nq, 2);
+                
+                C=Neum_edges';
+                G=C(:);
+                I=Eq';
+                
+                % Coordinates of the nodes of the elements
+                coord_nodes=coord(G,:);
+                % Endpoints of the edges
+                a=coord_nodes(1:s:end,:);
+                b=coord_nodes(2:s:end,:);
+                bk=b-a;
+                norm_bk=vecnorm(bk,2,2);
+                
+                % Interpolated coordinates
+                P=a(:)+(b(:)-a(:))*points';
+                X(:,:,1)=P(1:ne,:);
+                X(:,:,2)=P(ne+1:end,:);
+                
+                % Evaluation at the Gauss points
+                F=f(X);
+                if size(F,1)>d
+                    F=reshape(F, ne, d*nq);
+                    F=reshape(F', d, nq, ne);
+                end
+                
+                X=reshape(norm_bk, 1, 1, ne).*F.*weights';
+                X=reshape(X, d*nq, ne);
+                
+                Bl=Q*X;
+                
+                % Assemblage
+                B=B+accumarray(I(:), Bl(:), [nb_dof 1]);
+                
+            end
+        end
+    end
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_small/computeStiffness.m b/Code/INTERNODES_small/computeStiffness.m
new file mode 100644
index 0000000..6091151
--- /dev/null
+++ b/Code/INTERNODES_small/computeStiffness.m
@@ -0,0 +1,138 @@
+function[K]=computeStiffness(Mesh, Data)
+
+% computeStiffness: Function which assembles the global stiffness matrix
+% Assumes all elements are of the same type
+% INPUT:
+% Mesh:     Structure containing all the mesh parameters
+% Data:     Structure containing all the data parameters
+% OUTPUT:
+% K:        Global stiffness matrix
+
+% Mesh parameters
+% Local number of degrees of freedom (dimension in solid mechanics)
+d=Mesh.nb_dof_local;
+% Total number of degrees of freedom
+nb_dof=Mesh.nb_dof;
+% Coordinate matrix
+coord=Mesh.coord_mat;
+% Connectivity matrix
+connect=Mesh.connect_mat;
+% Finite element order
+order=Mesh.order;
+% Number of nodes per element
+nne=Mesh.nb_nodes_elem;
+% Total number of elements
+ne=Mesh.nb_elem;
+% Matrix linking an element to the degrees of freedom associated to it.
+Eq=Mesh.Eq;
+
+% Data parameters
+% Mu
+f1=Data.Coefficient.mu.function;
+% Lambda
+f2=Data.Coefficient.lambda.function;
+
+
+% Retrieve functions
+[Functions]=getFunctions(order);
+% Jacobian matrix
+J_phi=Functions.J_phi;
+% Define permutation matrix
+Sdd=zeros(d*d);
+Id=eye(d);
+for k=1:d
+    Sdd=Sdd+kron(Id(:,k)', kron(Id, Id(:,k)));
+end
+
+% Retrieve quadrature nodes and weights
+switch order
+    case 1
+        [points, weights] = getQuadrature(1, 'bulk');
+    case 2
+        [points, weights] = getQuadrature(2, 'bulk');
+    case 3
+        [points, weights] = getQuadrature(3, 'bulk');
+    otherwise
+        error('Not yet implemented');
+end
+
+% Initialization
+s=nne*d;
+nq=length(weights);
+Q=zeros(s^2, nq*d^4);
+
+% Loop over the Gauss points
+for i = 1:nq
+    L=kron(J_phi(points(i,:)), Id);
+    Q(:, (i-1)*d^4+1:i*d^4)=kron(L, L);
+end
+
+T=Eq';
+G=T(:);
+
+I=repmat(Eq, [1 nne*d])';
+J=repmat(G', [nne*d 1]);
+
+T=connect';
+G=T(:);
+
+% Coordinates of the nodes of the elements
+coord_nodes=coord(G,:);
+% Vertices of the elements
+a=coord_nodes(1:nne:end,:);
+b=coord_nodes(2:nne:end,:);
+c=coord_nodes(3:nne:end,:);
+% Interpolated coordinates
+P=a(:)+[b(:)-a(:) c(:)-a(:)]*points';
+X(:,:,1)=P(1:ne,:);
+X(:,:,2)=P(ne+1:end,:);
+
+% Determinants of Jacobian matrices
+detJK=(b(:,1)-a(:,1)).*(c(:,2)-a(:,2))-(b(:,2)-a(:,2)).*(c(:,1)-a(:,1));
+
+V=[(vecnorm(c-a,2,2).^2) -sum((c-a).*(b-a),2) -sum((c-a).*(b-a),2) (vecnorm(b-a,2,2).^2)];
+W=1./(detJK').^2.*(V');
+
+V=reshape(W,d,d*ne);
+
+JK_inv=zeros(d, d*ne);
+JK_inv(:,1:d:d*ne)=1./detJK'.*[c(:,2)-a(:,2) -(b(:,2)-a(:,2))]';
+JK_inv(:,2:d:d*ne)=1./detJK'.*[-(c(:,1)-a(:,1)) b(:,1)-a(:,1)]';
+
+JK_invT=zeros(d, d*ne);
+JK_invT(:,1:d:d*ne)=1./detJK'.*[c(:,2)-a(:,2) -(c(:,1)-a(:,1))]';
+JK_invT(:,2:d:d*ne)=1./detJK'.*[-(b(:,2)-a(:,2)) b(:,1)-a(:,1)]';
+
+vec_JK_invT=reshape(JK_invT, d^2, ne);
+
+[S1]=product(V , repmat(Id, 1, ne), d, ne);
+[S2]=product(JK_invT , JK_inv, d, ne);
+S2=Sdd*S2;
+
+A1=reshape(S1, d^4, ne);
+A2=reshape(S2, d^4, ne);
+A3=kr(vec_JK_invT, vec_JK_invT);
+
+
+Lambda1=(abs(detJK).*f1(X).*weights)';
+Lambda2=(abs(detJK).*f2(X).*weights)';
+
+X=kr(Lambda1, A1+A2)+kr(Lambda2, A3);
+K=Q*X;
+
+% Assemblage
+K=sparse(I(:), J(:), K(:), nb_dof, nb_dof);
+
+    function[M]=product(A,B,d,ne)
+        
+        % Blockwise Kronecker product of two matrices
+        % !! Only for a two-dimensional case !!
+        M=zeros(d^2, d^2*ne);
+        
+        M(:,1:d^2:end)=kr(A(:,1:d:end), B(:,1:d:end));
+        M(:,2:d^2:end)=kr(A(:,1:d:end), B(:,2:d:end));
+        M(:,3:d^2:end)=kr(A(:,2:d:end), B(:,1:d:end));
+        M(:,4:d^2:end)=kr(A(:,2:d:end), B(:,2:d:end));
+    end
+
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_small/conforming1.m b/Code/INTERNODES_small/conforming1.m
new file mode 100644
index 0000000..ef735e7
--- /dev/null
+++ b/Code/INTERNODES_small/conforming1.m
@@ -0,0 +1,64 @@
+% INTERNODES method for contact mechanics
+% Case study: Hertzian contact problem with Dirichlet boundary conditions
+% applied to both bodies. Conforming meshes.
+
+% Limitations: 2D
+clc
+clear variables
+close all
+
+% Read GMSH mesh file (.msh)
+[Mesh] = readGMSH('../../GMSH/Meshes/contact3_p1_0_1.msh');
+
+%% Parameter prescription
+% Mesh parameters
+[Mesh]=getParameters(Mesh);
+% Initialize model
+[Data]=setModel('model', 'static_solid', 'submodel', 'contact', 'type', 'linear');
+
+%% Set material parameters
+% Elastic modulus [N\m^2]
+E=30e9;
+% Poisson ratio [-]
+nu=0.2;
+% Specific mass [kg/m^3]
+rho=2500;
+% Thickness [m]
+t=1;
+
+% Plane stress conditions
+lambda=E*nu/(1-nu^2);
+mu=E/(2*(1+nu));
+
+[Data]=setCoefficient(Data, 'rho', t*rho);
+[Data]=setCoefficient(Data, 'lambda', t*lambda);
+[Data]=setCoefficient(Data, 'mu', t*mu);
+[Data]=setCoefficient(Data, 'f', @(x) [0; 0]);
+
+%% Set boundary conditions
+
+% Used for testing with invertible stiffness matrix
+BC={{'Type', 'Dirichlet', 'Edges', 3, 'Function', @(x) [0; 0]},...
+    {'Type', 'Dirichlet', 'Edges', 10, 'Function', @(x) [0; -0.1]}};
+
+p2={'BC', BC};    
+
+[Data]=setBoundaryConditions(Mesh, Data, p2);
+%% Set numerical parameters
+% For more information, type 'help setNumericalParameters'
+
+[Data]=setNumericalParameters(Mesh, Data, 'Contact', 'rescaling', E, 'maxiter', 30);
+[Data]=setNumericalParameters(Mesh, Data, 'PostProcessing', 'ampl_fact', 1, 'quantity', {'stress', 'strain'});
+
+%% Assemblage of the right-hand side
+[Data]=computeRHS(Mesh, Data);
+
+%% Solve the contact problem
+[Mesh, Data, Solution, A, b]=solve(Mesh, Data, {3:6, 7:10});
+
+%% Computation of the stresses and strains
+[Mesh, Solution]=postProcess(Mesh, Data, Solution);
+
+%% Visualization of the solution
+% For more information, type 'help plotSolution'
+plotSolution(Mesh, Data, Solution, 'disp', 'standard', 'stress', 'sigma_yy');
\ No newline at end of file
diff --git a/Code/INTERNODES_small/conforming2.m b/Code/INTERNODES_small/conforming2.m
new file mode 100644
index 0000000..50d85a8
--- /dev/null
+++ b/Code/INTERNODES_small/conforming2.m
@@ -0,0 +1,66 @@
+% INTERNODES method for contact mechanics
+% Case study: Hertzian contact problem with Dirichlet boundary conditions
+% applied to the lower body and Neumann boundary conditions applied to the
+% upper body. Conforming meshes.
+
+% Limitations: 2D
+clc
+clear variables
+close all
+
+% Read GMSH mesh file (.msh)
+[Mesh] = readGMSH('../../GMSH/Meshes/contact3_p1_0_1.msh');
+
+%% Parameter prescription
+% Mesh parameters
+[Mesh]=getParameters(Mesh);
+% Initialize model
+[Data]=setModel('model', 'static_solid', 'type', 'linear');
+
+%% Set material parameters
+% Elastic modulus [N\m^2]
+E=30e9;
+% Poisson ratio [-]
+nu=0.2;
+% Specific mass [kg/m^3]
+rho=2500;
+% Thickness [m]
+t=1;
+
+% Plane stress conditions
+lambda=E*nu/(1-nu^2);
+mu=E/(2*(1+nu));
+
+[Data]=setCoefficient(Data, 'rho', t*rho);
+[Data]=setCoefficient(Data, 'lambda', t*lambda);
+[Data]=setCoefficient(Data, 'mu', t*mu);
+[Data]=setCoefficient(Data, 'f', @(x) [0; 0]);
+
+%% Set boundary conditions
+
+% Used for testing with singular stiffness matrix
+BC={{'Type', 'Dirichlet', 'Edges', 3, 'Function', @(x) [0; 0]},...
+    {'Type', 'Neumann', 'Edges', 10, 'Function', @(x) [0; -1e10]}};
+
+p2={'BC', BC};    
+
+[Data]=setBoundaryConditions(Mesh, Data, p2);
+
+%% Set numerical parameters
+
+% For more information, type 'help setNumericalParameters'
+[Data]=setNumericalParameters(Mesh, Data, 'Contact', 'rescaling', E);
+[Data]=setNumericalParameters(Mesh, Data, 'PostProcessing', 'ampl_fact', 1, 'quantity', {'stress', 'strain'});
+
+%% Assemblage of the right-hand side
+[Data]=computeRHS(Mesh, Data);
+
+%% Solve the contact problem
+[Mesh, Data, Solution, A, b]=solve(Mesh, Data, {3:6, 7:10});
+
+%% Computation of the stresses and strains
+[Mesh, Solution]=postProcess(Mesh, Data, Solution);
+
+%% Visualization of the solution
+% For more information, type 'help plotSolution'
+plotSolution(Mesh, Data, Solution, 'disp', 'standard', 'stress', 'sigma_yy');
\ No newline at end of file
diff --git a/Code/INTERNODES_small/detectGaps.m b/Code/INTERNODES_small/detectGaps.m
new file mode 100644
index 0000000..ffe94c0
--- /dev/null
+++ b/Code/INTERNODES_small/detectGaps.m
@@ -0,0 +1,81 @@
+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
\ No newline at end of file
diff --git a/Code/INTERNODES_small/getFunctions.m b/Code/INTERNODES_small/getFunctions.m
new file mode 100644
index 0000000..ddff9aa
--- /dev/null
+++ b/Code/INTERNODES_small/getFunctions.m
@@ -0,0 +1,46 @@
+function[Functions]=getFunctions(order)
+
+% getFunctions: Returns the basis functions, their jacobian matrices and
+% the basis functions needed to integrate over the boundary
+% INPUT:
+% order:        Finite element order
+% OUTPUT:
+% Functions:    Structure containing all the functions
+
+
+switch order
+    case 1
+Functions.phi=@(x) [1-x(1)-x(2);...
+                    x(1);...
+                    x(2)];
+                
+Functions.J_phi=@(x) [-1 -1; ...
+                       1  0;...
+                       0  1]; 
+                   
+Functions.phi_boundary=@(x) [1-x;...
+                             x];
+    case 2
+Functions.phi=@(x) [(1-x(1)-x(2))*(1-2*x(1)-2*x(2));...
+                    x(1)*(2*x(1)-1);...
+                    x(2)*(2*x(2)-1);...
+                    4*x(1)*(1-x(1)-x(2));...
+                    4*x(1)*x(2);...
+                    4*x(2)*(1-x(1)-x(2))];
+                 
+                                                       
+Functions.J_phi=@(x) [-3+4*x(2)+4*x(1) -3+4*x(1)+4*x(2); ...
+                       -1+4*x(1)        0;...
+                       0                -1+4*x(2);...
+                       4-4*x(2)-8*x(1)  -4*x(1);...
+                       4*x(2)           4*x(1);...
+                       -4*x(2)          4-4*x(1)-8*x(2)];
+
+Functions.phi_boundary=@(x) [(2*x-1)*(x-1);...
+                            x*(2*x-1);...
+                            4*x*(1-x)];
+               
+    otherwise
+        error('Unimplemented!');
+end
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_small/getParameters.m b/Code/INTERNODES_small/getParameters.m
new file mode 100644
index 0000000..4a0812c
--- /dev/null
+++ b/Code/INTERNODES_small/getParameters.m
@@ -0,0 +1,72 @@
+function[Mesh]=getParameters(Mesh)
+
+% getParameters: Retrieves mesh parameters
+% INPUT:
+% Mesh: Structure containing the mesh geometry
+% OUTPUT:
+% Mesh: Updated structure containing all mesh parameters needed to run the code
+
+% Retrieve mesh parameters
+% Coordinate matrix
+coord=Mesh.coord_mat;
+% Connectivity matrix
+connect=Mesh.connect_mat;
+% Boundary nodes
+BC_nodes=Mesh.BC_nodes;
+
+% Geometry [m] 
+x_min=min(coord(:,1));
+x_max=max(coord(:,1));
+y_min=min(coord(:,2));
+y_max=max(coord(:,2));
+% Maximum dimension [m]
+L_max=max([abs(x_max-x_min) abs(y_max-y_min)]);
+% Number of elements
+nb_elem=size(connect,1);
+% Number of nodes per element
+nb_nodes_elem=size(connect,2);
+% Number of nodes
+nb_nodes=size(coord,1);
+% Local number of degrees of freedom
+nb_dof_local=size(coord,2);
+% Total number of degrees of freedom
+nb_dof=nb_dof_local*nb_nodes;
+% Number of boundary nodes
+nb_BC_nodes=size(BC_nodes,1);
+
+% Order of polynomials
+% It is assumed all elements of the mesh are of same type
+order_map=containers.Map([3 6 10], [1 2 3]);
+order=order_map(nb_nodes_elem);
+
+% Initialization
+% Matrix of equation numbers   
+Eq=zeros(nb_elem,nb_dof_local*nb_nodes_elem);
+% Matrix of equation number for boundary nodes;
+Eq_BC=zeros(nb_BC_nodes,nb_dof_local*order);
+% Matrix containing the sets of degrees of freedom for each node
+Dof_set=zeros(nb_dof_local, nb_nodes);
+
+% Iteration over the dimension
+for k=1:nb_dof_local
+    d1=k:nb_dof_local:nb_dof_local*nb_nodes_elem;
+    d2=k:nb_dof_local:nb_dof_local*(order+1);
+    Eq(:,d1)=nb_dof_local*connect-nb_dof_local+k;
+    Eq_BC(:,d2)=nb_dof_local*BC_nodes-nb_dof_local+k;
+    Dof_set(k,:)=nb_dof_local*(1:nb_nodes)-nb_dof_local+k;
+end
+
+% Update mesh structure
+Mesh.frame=[x_min y_min; x_max y_max];
+Mesh.L_max=L_max;
+Mesh.nb_elem=nb_elem;
+Mesh.nb_nodes_elem=nb_nodes_elem;
+Mesh.nb_nodes=nb_nodes;
+Mesh.nb_dof_local=nb_dof_local;
+Mesh.nb_dof=nb_dof;
+Mesh.order=order;
+Mesh.Eq=Eq;
+Mesh.Eq_BC=Eq_BC;
+Mesh.nb_BC_nodes=nb_BC_nodes;
+Mesh.Dof_set=Dof_set;
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_small/getQuadrature.m b/Code/INTERNODES_small/getQuadrature.m
new file mode 100755
index 0000000..706128e
--- /dev/null
+++ b/Code/INTERNODES_small/getQuadrature.m
@@ -0,0 +1,362 @@
+function[points, weights] = getQuadrature(id, object)
+
+% getQuadrature: Function which returns the integration points and
+% weights for a given quadrature ID
+% INPUT:
+% id:       Integration ID
+% OUTPUT:
+% points:   Integration points
+% weights:  Integration weights
+
+switch object
+    case 'bulk'
+        switch id
+            case 1
+                % Source: Wriggers, Computational contact mechanics (2006)
+                % Number of points: 1
+                % Degree of exactness: 1
+                
+                a1=1/3;                 w1=1;
+                
+                points=[a1 a1];
+                
+                weights=0.5*w1;
+                
+            case 2
+                % Source: Wriggers, Computational contact mechanics (2006)
+                % Number of points: 3
+                % Degree of exactness: 2
+                
+                a1=2/3;                 w1=1/3;
+                b1=1/6;
+                
+                points=[b1 b1;
+                        a1 b1;
+                        b1 a1];
+                
+                weights=0.5*[w1 w1 w1];
+                
+            case 3
+                % Source: Wriggers, Computational contact mechanics (2006)
+                % Number of points: 4
+                % Degree of exactness: 3
+                
+                a1=1/3;                 w1=-27/48;
+                a2=3/5;                 w2=25/48;
+                b2=1/5;
+                
+                points=[a1 a1;            
+                        b2 b2;
+                        a2 b2;
+                        b2 a2];
+                
+                weights=0.5*[w1 w2 w2 w2];
+                
+            case 4
+                % Source: Laursen and Gellert, Some criteria for
+                % numerically integrated matrices and quadrature formulas
+                % for triangles (1978)
+                % Number of points: 6
+                % Degree of exactness: 4
+                
+                a1=0.816847572980459;       w1=0.109951743655322;
+                b1=0.091576213509771;       w2=0.223381589678011;
+                a2=0.108103018168070;
+                b2=0.445948490915965;
+                
+                points=[b1 b1;
+                        a1 b1;
+                        b1 a1;
+                        b2 b2;
+                        a2 b2;
+                        b2 a2];
+                
+                weights=0.5*[w1 w1 w1 w2 w2 w2];
+                
+            case 5
+                % Source: Laursen and Gellert, Some criteria for
+                % numerically integrated matrices and quadrature formulas
+                % for triangles (1978)
+                % Number of points: 7
+                % Degree of exactness: 5
+                
+                a1=1/3;                     w1=0.225000000000000;
+                a2=0.797426985353087;       w2=0.125939180544827;
+                b2=0.101286507323456;       w3=0.132394152788506;
+                a3=0.059715871789770;
+                b3=0.470142064105115;
+                
+                points=[a1 a1;
+                        b2 b2;
+                        a2 b2;
+                        b2 a2;
+                        b3 b3;
+                        a3 b3;
+                        b3 a3];
+                    
+               weights=0.5*[w1 w2 w2 w2 w3 w3 w3];
+                    
+            case 6
+                % Source: Laursen and Gellert, Some criteria for
+                % numerically integrated matrices and quadrature formulas
+                % for triangles (1978)
+                % Number of points: 12
+                % Degree of exactness: 6
+                
+                a1=0.873821971016996;       w1=0.050844906370207;
+                b1=0.063089014491502;       w2=0.116786275726379;
+                a2=0.501426509658179;       w3=0.082851075618374;
+                b2=0.249286745170910;
+                a3=0.636502499121399;
+                b3=0.310352451033785;
+                c3=0.053145049844816;
+                
+                points=[b1 b1;
+                        b1 a1;
+                        a1 b1;
+                        b2 b2;
+                        b2 a2;
+                        a2 b2;
+                        a3 b3;
+                        a3 c3;
+                        b3 a3;
+                        b3 c3;
+                        c3 a3;
+                        c3 b3];
+                    
+               weights=0.5*[w1 w1 w1 w2 w2 w2 w3 w3 w3 w3 w3 w3];
+                 
+            case 7
+                % Source: Laursen and Gellert, Some criteria for
+                % numerically integrated matrices and quadrature formulas
+                % for triangles (1978)
+                % Number of points: 13
+                % Degree of exactness: 7
+                
+                a1=1/3;                     w1=-0.149570044467670;
+                a2=0.479308067841923;       w2= 0.175615257433204;           
+                b2=0.260345966079038;       w3= 0.053347235608839;
+                a3=0.869739794195568;       w4= 0.077113760890257;
+                b3=0.065130102902216;
+                a4=0.638444188569809;
+                b4=0.312865496004875;
+                c4=0.048690315425316;
+                
+                points=[a1 a1;
+                        b2 b2;
+                        b2 a2;
+                        a2 b2;
+                        b3 b3;
+                        b3 a3;
+                        a3 b3;
+                        a4 b4;
+                        a4 c4;
+                        b4 a4;
+                        b4 c4;
+                        c4 a4;
+                        c4 b4];
+                                                      
+                weights=0.5*[w1 w2 w2 w2 w3 w3 w3 w4 w4 w4 w4 w4 w4];
+
+            case 8
+                % Source: Laursen and Gellert, Some criteria for
+                % numerically integrated matrices and quadrature formulas
+                % for triangles (1978)
+                % Number of points: 16
+                % Degree of exactness: 8
+                
+                a1=1/3;                     w1=0.144315607677787;
+                a2=0.658861384496478;       w2=0.103217370534718;
+                b2=0.170569307751761;       w3=0.032458497623198;
+                a3=0.898905543365938;       w4=0.095091634267284;
+                b3=0.050547228317031;       w5=0.027230314174435;
+                a4=0.081414823414554;
+                b4=0.459292588292723;
+                a5=0.008394777409958;
+                b5=0.263112829634638;
+                c5=0.728492392955404;
+                
+                points=[a1 a1;
+                        b2 b2;
+                        b2 a2;
+                        a2 b2;
+                        b3 b3;
+                        b3 a3;
+                        a3 b3;
+                        b4 b4;
+                        b4 a4;
+                        a4 b4;
+                        a5 b5;
+                        a5 c5;
+                        b5 a5;
+                        b5 c5;
+                        c5 a5;
+                        c5 b5];
+                    
+                weights=0.5*[w1 w2 w2 w2 w3 w3 w3 w4 w4 w4 w5 w5 w5 w5 w5 w5];
+                
+            case 9
+                % Source: Laursen and Gellert, Some criteria for
+                % numerically integrated matrices and quadrature formulas
+                % for triangles (1978)
+                % Number of points: 19
+                % Degree of exactness: 9
+                
+                a1=1/3;                     w1=0.097135796282799;
+                a2=0.020634961602524;       w2=0.031334700227139;
+                b2=0.489682519198738;       w3=0.077827541004774;
+                a3=0.125820817014126;       w4=0.079647738927210;
+                b3=0.437089591492937;       w5=0.025577675658698;
+                a4=0.623592928761934;       w6=0.043283539377289;
+                b4=0.188203535619033;
+                a5=0.910540973211094;
+                b5=0.044729513394453;
+                a6=0.036838412054736;
+                b6=0.221962989160766;
+                c6=0.741198598784498;
+                
+                points=[a1 a1;
+                        b2 b2;
+                        b2 a2;
+                        a2 b2;
+                        b3 b3;
+                        b3 a3;
+                        a3 b3;
+                        b4 b4;
+                        b4 a4;
+                        a4 b4;
+                        b5 b5;
+                        b5 a5;
+                        a5 b5;
+                        a6 b6;
+                        a6 c6;
+                        b6 a6;
+                        b6 c6;
+                        c6 a6;
+                        c6 b6];
+              
+             weights=0.5*[w1 w2 w2 w2 w3 w3 w3 w4 w4 w4 w5 w5 w5 w6 w6 w6 w6 w6 w6];       
+                    
+            case 10
+                % Source: Laursen and Gellert, Some criteria for
+                % numerically integrated matrices and quadrature formulas
+                % for triangles (1978)
+                % Number of points: 25
+                % Degree of exactness: 10
+                  
+                a1=1/3;                     w1=0.081743329146286;
+                a2=0.715677797886872;       w2=0.045957963604745;
+                b2=0.142161101056564;       w3=0.013352968813150;
+                a3=0.935889253566112;       w4=0.063904906396424;
+                b3=0.032055373216944;       w5=0.034184648162959;
+                a4=0.148132885783821;       w6=0.025297757707288;
+                b4=0.321812995288835;
+                c4=0.530054118927344;
+                a5=0.029619889488730;
+                b5=0.369146781827811;
+                c5=0.601233328683459;
+                a6=0.028367665339938;
+                b6=0.163701733737182;
+                c6=0.807930600922880;
+                 
+                points=[a1 a1;
+                        b2 b2;
+                        a2 b2;
+                        b2 a2;
+                        b3 b3;
+                        a3 b3;
+                        b3 a3;
+                        a4 b4;
+                        a4 c4;
+                        b4 a4;
+                        b4 c4;
+                        c4 a4;
+                        c4 b4;
+                        a5 b5;
+                        a5 c5;
+                        b5 a5;
+                        b5 c5;
+                        c5 a5;
+                        c5 b5;
+                        a6 b6;
+                        a6 c6;
+                        b6 a6;
+                        b6 c6;
+                        c6 a6;
+                        c6 b6];
+                                    
+                weights=0.5*[w1 w2 w2 w2 w3 w3 w3 w4 w4 w4 w4 w4 w4 w5 w5 w5 w5 w5 w5 w6 w6 w6 w6 w6 w6];           
+                
+            otherwise
+                error('Unimplemented');
+        end
+        
+    case 'boundary'
+        
+        [QuadRule] = getGaussLegendre(0, 1, id);
+        
+        % Quadrature nodes
+        points=QuadRule.x;
+        % Quadrature weights
+        weights=QuadRule.w;
+        
+    case 'interpolation'
+        
+        e=1e-6;
+        
+        switch id
+            case 3
+                points=[e e
+                        1-e e
+                        e 1-e];
+                
+            case 6
+                points=[e e
+                        1-e e
+                        e 1-e
+                        1/3 1/3
+                        2/3 1/3
+                        1/3 2/3];
+                
+        end
+        
+        weights=[];
+end
+
+
+    function [QuadRule] = getGaussLegendre(a,b,n)
+        
+        % getGaussLegendre: 1D n-point Gauss-Legendre quadrature rule on the interval [a,b].
+        % Quadrature rules obtained from Gauss-Legendre are of order 2*n-1.
+        % INPUT:
+        % a,b:          End-points of the interval
+        % n:            Number of quadrature nodes in [a,b]
+        % OUTPUT:
+        % QuadRule:     Structure which contains the fields:
+        %    x:         N-by-1 matrix specifying the abscissae of the quadrature rule.
+        %    w:         N-by-1 matrix specifying the weights of the quadrature rule.
+        
+        if (n==1), x = 0; w = 2;
+        else
+            c = zeros(n-1,1);
+            for i=1:(n-1), c(i)=i/sqrt(4*i*i-1); end
+            J=diag(c,-1)+diag(c,1); [ev,ew]=eig(J);
+            x=diag(ew); w=(2*(ev(1,:).*ev(1,:)))';
+        end
+        
+        % The above rule computes a quadrature rule for the reference interval.
+        % The reference interval is always [-1,1], and if [a,b] != [-1,1] then we
+        % re-map the abscissas (x) and rescale the weights.
+        
+        % Midpoint
+        xm = (b+a)/2;
+        % Area of the requested interval / area of the reference interval
+        xl = (b-a)/2;
+        
+        x = xm + xl*x;
+        w = w * xl;
+        
+        QuadRule.w = w(:);
+        QuadRule.x = x(:);
+    end
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_small/getRadius.m b/Code/INTERNODES_small/getRadius.m
new file mode 100644
index 0000000..242ea5b
--- /dev/null
+++ b/Code/INTERNODES_small/getRadius.m
@@ -0,0 +1,101 @@
+function[r, nnzRMM, nnzCMM, nnzRNM, nnzCNM]=getRadius(Data, nodesM, nodesN, coordM, coordN)
+
+% getRadius: Computes the radiuses of the radial basis functions defined on 
+% the interfaces.
+% INPUT:
+% Data:         Structure containing all the data parameters
+% nodesM:       Set of interpolation nodes
+% nodesN:       Set of evaluation nodes
+% coordM:       Coordinate matrix for the interpolation nodes
+% coordN:       Coordinate matrix for the evaluation nodes
+% OUTPUT:
+% r:            Radiuses of radial basis functions
+% nnzRMM:       Number of off-diagonal nonzeros in each row of PhiMM
+% nnzCMM:       Number of off-diagonal nonzeros in each column of PhiMM
+% nnzRNM:       Number of nonzeros in each row of PhiNM
+% nnzCNM:       Number of nonzeros in each column of PhiNM
+
+M=length(nodesM);
+N=length(nodesN);
+r=zeros(M,1);
+
+% Parameters
+% gamma parameter: real number between ]0,1[
+% Large values of gamma result in small supports
+gamma=Data.Contact.gamma;
+% Gamma parameter: real number between ]0,1[ with Gamma>gamma
+% Used to ensure evaluation points are well within the
+% support of radial basis functions (far enough from the boundary)
+Gamma=Data.Contact.Gamma;
+% Tolerance for contact detection
+% If the tolerance is unknown, set it to some small value.
+d0=Data.Contact.d0;
+% Number of closest neighbors on current interface
+c=Data.Contact.c;
+
+% Number of off-diagonal nonzeros per row of PhiMM
+nnzRMM=zeros(M,1);
+% Number of off-diagonal nonzeros per column of PhiMM
+nnzCMM=zeros(M,1);
+% Number of nonzeros per row of PhiNM
+nnzRNM=zeros(N,1);
+% Number of nonzeros per column of PhiNM
+nnzCNM=zeros(M,1);
+
+maxS=inf;
+f=0;
+niter=0;
+maxiter=10;
+
+% Version 2
+while maxS>f && niter<maxiter
+    % Maximum number of supports to which an evaluation point may belong.
+    % Condition for the matrix PhiMM to be diagonally dominant.
+    f=floor(1./((1-gamma).^4.*(1+4*gamma)));
+    
+    for k=1:M
+        point=coordM(k,:);
+        neighbors=coordM;
+        neighbors(k,:)=inf;
+        distMM=vecnorm(neighbors-point,2,2);
+        distMN=vecnorm(coordN-point,2,2);
+        
+        rMM=mink(distMM,c);
+        rNM=sqrt(d0^2+0.25*rMM(end)^2);
+        radius=max([rMM(end) rNM]);
+        
+        % Make sure the closest neighbor is at a distance >= gamma*radius.
+        % If the point is too close (with respect to the radius), diagonal
+        % dominance will be lost very quickly.
+        if radius>rMM(1)/gamma
+            radius=rMM(1)/gamma;
+        end
+        
+        s1=distMM<radius;
+        s2=distMN<Gamma*radius;
+        
+        nnzRMM(s1)=nnzRMM(s1)+1;
+        nnzRNM(s2)=nnzRNM(s2)+1;
+        nnzCMM(k)=sum(s1);
+        nnzCNM(k)=sum(s2);
+        
+        r(k)=radius;
+    end
+    % Compute the maximum number of supports to which a point belongs
+    maxS=max(nnzRMM);
+    if maxS>f
+        % Increase gamma (sequence converges to 1)
+        gamma=0.5*(1+gamma);
+        % Reinitialize counters
+        nnzRMM=zeros(M,1);
+        nnzCMM=zeros(M,1);
+        nnzRNM=zeros(N,1);
+        nnzCNM=zeros(M,1);
+        niter=niter+1;
+    end
+end
+
+if niter==maxiter && maxS>f
+    warning('The maximum number of rounds for radius computations was reached. Try decreasing d0')
+end
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_small/initializeInterface.m b/Code/INTERNODES_small/initializeInterface.m
new file mode 100644
index 0000000..9f57511
--- /dev/null
+++ b/Code/INTERNODES_small/initializeInterface.m
@@ -0,0 +1,107 @@
+function[Data]=initializeInterface(Mesh, Data, it)
+
+% initializeInterface: Initializes the interfaces properties
+% INPUT:
+% Mesh:     Structure containing all the mesh parameters
+% Data:     Structure containing all the data parameters
+% it:       Boundary tags for either a portion of the boundary or the entire boundary
+%           The first component is the tag for body 1
+%           The second component is the tag for body 2
+% OUTPUT:
+% Data:     Updated data structure with interface properties
+
+% Retrieve mesh parameters
+% Retrieve boundary tag
+BC_tag=Mesh.BC_tag;
+% Retrieve coordinate matrix
+coord=Mesh.coord_mat;
+% Define radius of intersection
+if isfield(Data.Contact, 'radius')
+    r=Data.Contact.radius;
+else
+    r=0.052;
+end
+
+% Boundary elements
+elements1=Mesh.BC_nodes(ismember(BC_tag,it{1}),:);
+elements2=Mesh.BC_nodes(ismember(BC_tag,it{2}),:);
+
+% Boundary nodes
+bc_nodes1=unique(elements1);
+bc_nodes2=unique(elements2);
+
+m1=length(bc_nodes1);
+m2=length(bc_nodes2);
+
+% Compute element lengths
+l1=vecnorm(coord(elements1(:,2),:)-coord(elements1(:,1),:),2,2);
+l2=vecnorm(coord(elements2(:,2),:)-coord(elements2(:,1),:),2,2);
+
+% Compute mesh sizes
+h1=min(l1);
+h2=min(l2);
+
+body_1.h=h1;
+body_2.h=h2;
+
+h=min([h1, h2]);
+Data.Contact.h=h;
+
+% Indicators
+ic1=zeros(m1,1);
+ic2=zeros(m2,1);
+
+% Create interfaces based on closest nodes from opposing interface
+for k=1:m1
+    node=bc_nodes1(k);
+    coord_node=coord(node,:);
+    dist=vecnorm(coord(bc_nodes2,:)-coord_node,2,2);
+    ic1(k)=any(dist<r);
+    
+end
+for k=1:m2
+    node=bc_nodes2(k);
+    coord_node=coord(node,:);
+    dist=vecnorm(coord(bc_nodes1,:)-coord_node,2,2);
+    ic2(k)=any(dist<r);
+end
+
+ic1=logical(ic1);
+ic2=logical(ic2);
+
+interface1=bc_nodes1(ic1);
+interface2=bc_nodes2(ic2);
+
+% Correct interface nodes to account for finite elements
+index1=all(ismember(Mesh.BC_nodes, interface1), 2);
+index2=all(ismember(Mesh.BC_nodes, interface2), 2);
+% Get interface connectivity matrices
+body_1.interface_connect=Mesh.BC_nodes(index1,:);
+body_2.interface_connect=Mesh.BC_nodes(index2,:);
+% Get interface nodes
+body_1.interface_nodes=unique(body_1.interface_connect);
+body_2.interface_nodes=unique(body_2.interface_connect);
+% Get interface degrees of freedom
+interface_dof_1=Mesh.Dof_set(:,body_1.interface_nodes);
+interface_dof_2=Mesh.Dof_set(:,body_2.interface_nodes);
+body_1.interface_dof=interface_dof_1(:);
+body_2.interface_dof=interface_dof_2(:);
+% Initial active set
+body_1.initial_nodes=body_1.interface_nodes;
+body_2.initial_nodes=body_2.interface_nodes;
+% Initial connectivity matrix
+body_1.initial_interface_connect=body_1.interface_connect;
+body_2.initial_interface_connect=body_2.interface_connect;
+% Active set
+body_1.active_set=ismember(body_1.initial_nodes, body_1.interface_nodes);
+body_2.active_set=ismember(body_2.initial_nodes, body_2.interface_nodes);
+
+% Initialization
+body=cell(2,1);
+% Create body entity
+body{1}=body_1;
+body{2}=body_2;
+Data.body=body;
+% Map global dof to local ones
+[Data]=mapGlobal2Local(Data);
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_small/initializeSystem.m b/Code/INTERNODES_small/initializeSystem.m
new file mode 100644
index 0000000..d1b85f9
--- /dev/null
+++ b/Code/INTERNODES_small/initializeSystem.m
@@ -0,0 +1,90 @@
+function[Data, Solution, A, b, K]=initializeSystem(Mesh, Data, it)
+
+% initializeSystem: Assembles the INTERNODES matrix for iteration 0
+% INPUT:
+% Mesh:     Structure containing all the mesh parameters
+% Data:     Structure containing all the data parameters
+% it:       Cell array containing the boundary tags of the interfaces
+% OUTPUT:
+% Data:     Updated structure containing all the data parameters
+% Solution: Initialized solution structure with the Dirichlet BC
+% A:        INTERNODES matrix stored as a sparse matrix
+% b:        Right-hand side vector
+% K:        Global stiffness matrix
+
+% Compute the Dirichlet BC
+[Solution]=computeBC(Mesh, Data);
+% Define the interface
+[Data]=initializeInterface(Mesh, Data, it);
+
+% Sub-assemblage
+[Data]=subAssemble(Mesh, Data, Solution, 'initialization');
+% Assemblage of the stiffness matrix
+[K]=computeStiffness(Mesh, Data);
+% Assemblage of the INTERNODES matrix
+[A,b]=assembleInternodesMat(Mesh, Data, Solution, K);
+
+    function[Solution]=computeBC(Mesh, Data)
+        
+        % computeBC: Function which computes the displacement values at
+        % Dirichlet BC nodes
+        % INPUT:
+        % Mesh:     Structure containing all the mesh parameters
+        % Data:     Structure containing all the data parameters
+        % OUTPUT:
+        % Solution: Structure containing the solution, including displacements
+        %           evaluated at Dirichlet nodes
+        
+        % Retrieve mesh parameters
+        % Coordinate matrix
+        coord=Mesh.coord_mat;
+        % Matrix containing the sets of degrees of freedom for each node
+        Dof_set=Mesh.Dof_set;
+        % Total number of degrees of freedom
+        nb_dof=Mesh.nb_dof;
+        % Local number of degrees of freedom
+        nb_dof_local=Mesh.nb_dof_local;
+        
+        % Retrieve data
+        % Dirichlet boundary conditions
+        D_BC=Data.D_BC;
+        % Set of nodes associated to Dirichlet BC
+        set_D_nodes=Data.set_D_nodes;
+        % Number of boundaries with Dirichlet BC
+        n=length(D_BC);
+        
+        % Retrieve solutions
+        U=zeros(nb_dof,1);
+        
+        for i=1:n
+            data=D_BC{i};
+            % Retrieve function
+            g=data.function;
+            % Nodes making up the edges
+            nodes=set_D_nodes{i};
+            % Number of nodes on the edges
+            nb_nodes=length(nodes);
+            % Degrees of freedom
+            dofs=Dof_set(:, nodes);
+            % Coordinates
+            X=reshape(coord(nodes,:), nb_nodes, 1, nb_dof_local);
+            % Evaluation
+            G=g(X);
+            
+            if size(G,1)>nb_dof_local
+                G=reshape(G, nb_nodes, nb_dof_local)';
+                G=G(:);
+            else
+                G=repmat(G, nb_nodes, 1);
+            end
+            U(dofs(:))=G;
+            
+        end
+        
+        % Create solution structure
+        Solution.U=U;
+        % Reshape to an array of the same size as the coordinate matrix
+        Solution.U_sort=reshape(U, nb_dof_local, [])';
+    end
+
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_small/internodes1.m b/Code/INTERNODES_small/internodes1.m
new file mode 100644
index 0000000..8a9f5d2
--- /dev/null
+++ b/Code/INTERNODES_small/internodes1.m
@@ -0,0 +1,64 @@
+% INTERNODES method for contact mechanics
+% Case study: Hertzian contact problem with Dirichlet boundary conditions
+% applied to both bodies.
+
+% Limitations: 2D
+clc
+clear variables
+close all
+
+% Read GMSH mesh file (.msh)
+[Mesh] = readGMSH('../../GMSH/Meshes/contact2_p1_0_05.msh');
+
+%% Parameter prescription
+% Mesh parameters
+[Mesh]=getParameters(Mesh);
+% Initialize model
+[Data]=setModel('model', 'static_solid', 'submodel', 'contact', 'type', 'linear');
+
+%% Set material parameters
+% Elastic modulus [N\m^2]
+E=30e9;
+% Poisson ratio [-]
+nu=0.2;
+% Specific mass [kg/m^3]
+rho=2500;
+% Thickness [m]
+t=1;
+
+% Plane stress conditions
+lambda=E*nu/(1-nu^2);
+mu=E/(2*(1+nu));
+
+[Data]=setCoefficient(Data, 'rho', t*rho);
+[Data]=setCoefficient(Data, 'lambda', t*lambda);
+[Data]=setCoefficient(Data, 'mu', t*mu);
+[Data]=setCoefficient(Data, 'f', @(x) [0; 0]);
+
+%% Set boundary conditions
+
+% Used for testing with invertible stiffness matrix
+BC={{'Type', 'Dirichlet', 'Edges', 3, 'Function', @(x) [0; 0]},...
+    {'Type', 'Dirichlet', 'Edges', 10, 'Function', @(x) [0; -0.1]}};
+
+p2={'BC', BC};    
+
+[Data]=setBoundaryConditions(Mesh, Data, p2);
+%% Set numerical parameters
+% For more information, type 'help setNumericalParameters'
+
+[Data]=setNumericalParameters(Mesh, Data, 'Contact', 'rescaling', E, 'maxiter', 10, 'radius', 0.2);
+[Data]=setNumericalParameters(Mesh, Data, 'PostProcessing', 'ampl_fact', 1, 'quantity', {'stress', 'strain'});
+
+%% Assemblage of the right-hand side
+[Data]=computeRHS(Mesh, Data);
+
+%% Solve the contact problem
+[Mesh, Data, Solution, A, b]=solve(Mesh, Data, {3:6, 7:10});
+
+%% Computation of the stresses and strains
+[Mesh, Solution]=postProcess(Mesh, Data, Solution);
+
+%% Visualization of the solution
+% For more information, type 'help plotSolution'
+plotSolution(Mesh, Data, Solution, 'disp', 'standard', 'stress', 'sigma_yy');
\ No newline at end of file
diff --git a/Code/INTERNODES_small/internodes2.m b/Code/INTERNODES_small/internodes2.m
new file mode 100644
index 0000000..124d693
--- /dev/null
+++ b/Code/INTERNODES_small/internodes2.m
@@ -0,0 +1,66 @@
+% INTERNODES method for contact mechanics
+% Case study: Hertzian contact problem with Dirichlet boundary conditions
+% applied to the lower body and Neumann boundary conditions applied to the
+% upper body.
+
+% Limitations: 2D
+clc
+clear variables
+close all
+
+% Read GMSH mesh file (.msh)
+[Mesh] = readGMSH('../../GMSH/Meshes/contact2_p1_0_05.msh');
+
+%% Parameter prescription
+% Mesh parameters
+[Mesh]=getParameters(Mesh);
+% Initialize model
+[Data]=setModel('model', 'static_solid', 'type', 'linear');
+
+%% Set material parameters
+% Elastic modulus [N\m^2]
+E=30e9;
+% Poisson ratio [-]
+nu=0.2;
+% Specific mass [kg/m^3]
+rho=2500;
+% Thickness [m]
+t=1;
+
+% Plane stress conditions
+lambda=E*nu/(1-nu^2);
+mu=E/(2*(1+nu));
+
+[Data]=setCoefficient(Data, 'rho', t*rho);
+[Data]=setCoefficient(Data, 'lambda', t*lambda);
+[Data]=setCoefficient(Data, 'mu', t*mu);
+[Data]=setCoefficient(Data, 'f', @(x) [0; 0]);
+
+%% Set boundary conditions
+
+% Used for testing with singular stiffness matrix
+BC={{'Type', 'Dirichlet', 'Edges', 3, 'Function', @(x) [0; 0]},...
+    {'Type', 'Neumann', 'Edges', 10, 'Function', @(x) [0; -1e9]}};
+
+p2={'BC', BC};    
+
+[Data]=setBoundaryConditions(Mesh, Data, p2);
+
+%% Set numerical parameters
+% For more information, type 'help setNumericalParameters'
+
+[Data]=setNumericalParameters(Mesh, Data, 'Contact', 'rescaling', E, 'maxiter', 30, 'radius', inf, 'tol', 0.9);
+[Data]=setNumericalParameters(Mesh, Data, 'PostProcessing', 'ampl_fact', 1, 'quantity', {'stress', 'strain'});
+
+%% Assemblage of the right-hand side
+[Data]=computeRHS(Mesh, Data);
+
+%% Solve the contact problem
+[Mesh, Data, Solution, A, b]=solve(Mesh, Data, {3:6, 7:10});
+
+%% Computation of the stresses and strains
+[Mesh, Solution]=postProcess(Mesh, Data, Solution);
+
+%% Visualization of the solution
+% For more information, type 'help plotSolution'
+plotSolution(Mesh, Data, Solution, 'disp', 'standard', 'stress', 'sigma_yy');
\ No newline at end of file
diff --git a/Code/INTERNODES_small/internodes3.m b/Code/INTERNODES_small/internodes3.m
new file mode 100644
index 0000000..aa9c582
--- /dev/null
+++ b/Code/INTERNODES_small/internodes3.m
@@ -0,0 +1,64 @@
+% INTERNODES method for contact mechanics
+% Case study: contact between two rectangular blocks with Dirichlet 
+% boundary conditions applied to both bodies.
+
+% Limitations: 2D
+clc
+clear variables
+close all
+
+% Read GMSH mesh file (.msh)
+[Mesh] = readGMSH('../../GMSH/Meshes/contact6_p1_0_05.msh');
+
+%% Parameter prescription
+% Mesh parameters
+[Mesh]=getParameters(Mesh);
+% Initialize model
+[Data]=setModel('model', 'static_solid', 'submodel', 'contact', 'type', 'linear');
+
+%% Set material parameters
+% Elastic modulus [N\m^2]
+E=30e9;
+% Poisson ratio [-]
+nu=0.2;
+% Specific mass [kg/m^3]
+rho=2500;
+% Thickness [m]
+t=1;
+
+% Plane stress conditions
+lambda=E*nu/(1-nu^2);
+mu=E/(2*(1+nu));
+
+[Data]=setCoefficient(Data, 'rho', t*rho);
+[Data]=setCoefficient(Data, 'lambda', t*lambda);
+[Data]=setCoefficient(Data, 'mu', t*mu);
+[Data]=setCoefficient(Data, 'f', @(x) [0; 0]);
+
+%% Set boundary conditions
+
+% Used for testing with invertible stiffness matrix
+BC={{'Type', 'Dirichlet', 'Edges', 11, 'Function', @(x) [0; 0]},...
+    {'Type', 'Dirichlet', 'Edges', 17, 'Function', @(x) [0; -0.1]}};
+
+p2={'BC', BC};    
+
+[Data]=setBoundaryConditions(Mesh, Data, p2);
+%% Set numerical parameters
+% For more information, type 'help setNumericalParameters'
+
+[Data]=setNumericalParameters(Mesh, Data, 'Contact', 'rescaling', E, 'radius', 0.3, 'maxiter', 30);
+[Data]=setNumericalParameters(Mesh, Data, 'PostProcessing', 'ampl_fact', 1, 'quantity', {'stress', 'strain'});
+
+%% Assemblage of the right-hand side
+[Data]=computeRHS(Mesh, Data);
+
+%% Solve the contact problem
+[Mesh, Data, Solution, A, b]=solve(Mesh, Data, {11:14, 15:18});
+
+%% Computation of the stresses and strains
+[Mesh, Solution]=postProcess(Mesh, Data, Solution);
+
+%% Visualization of the solution
+% For more information, type 'help plotSolution'
+plotSolution(Mesh, Data, Solution, 'disp', 'standard', 'stress', 'sigma_yy');
\ No newline at end of file
diff --git a/Code/INTERNODES_small/internodes4.m b/Code/INTERNODES_small/internodes4.m
new file mode 100644
index 0000000..92a47c1
--- /dev/null
+++ b/Code/INTERNODES_small/internodes4.m
@@ -0,0 +1,81 @@
+% INTERNODES method for contact mechanics
+% Case study: Hertzian contact problem with Dirichlet boundary conditions
+% applied to both bodies. Elastic contact between a sphere and a half
+% space.
+
+% Limitations: 2D
+clc
+clear variables
+close all
+
+% Read GMSH mesh file (.msh)
+[Mesh] = readGMSH('../../GMSH/Meshes/contact10_p2_0_05.msh');
+
+%% Parameter prescription
+% Mesh parameters
+[Mesh]=getParameters(Mesh);
+% Initialize model
+[Data]=setModel('model', 'static_solid', 'submodel', 'contact', 'type', 'linear');
+
+%% Set material parameters
+% Elastic modulus [N\m^2]
+E=30e9;
+% Poisson ratio [-]
+nu=0.2;
+% Specific mass [kg/m^3]
+rho=2500;
+% Thickness [m]
+t=1;
+% Amplification factor for the stiffness of the upper body
+f=10;
+% Radius of the upper sphere
+R=0.5;
+% Prescribed displacement
+d=0.15;
+
+% Plane stress conditions
+lambda=E*nu/(1-nu^2);
+mu=E/(2*(1+nu));
+
+[Data]=setCoefficient(Data, 'rho', t*rho);
+[Data]=setCoefficient(Data, 'lambda', @(x) t*(lambda*(x(2)<=2)+f*lambda*(x(2)>2)));
+[Data]=setCoefficient(Data, 'mu', @(x) t*(mu*(x(2)<=2)+f*mu*(x(2)>2)));
+[Data]=setCoefficient(Data, 'f', @(x) [0; 0]);
+
+%% Set boundary conditions
+
+% Used for testing with invertible stiffness matrix
+BC={{'Type', 'Dirichlet', 'Edges', 9, 'Function', @(x) [0; 0]},...
+    {'Type', 'Dirichlet', 'Edges', 14, 'Function', @(x) [0; -d]}};
+
+p2={'BC', BC};    
+
+[Data]=setBoundaryConditions(Mesh, Data, p2);
+%% Set numerical parameters
+% For more information, type 'help setNumericalParameters'
+
+[Data]=setNumericalParameters(Mesh, Data, 'Contact', 'rescaling', E, 'radius', 0.2, 'maxiter', 30);
+[Data]=setNumericalParameters(Mesh, Data, 'PostProcessing', 'ampl_fact', 1, 'quantity', {'stress', 'strain'});
+
+%% Assemblage of the right-hand side
+[Data]=computeRHS(Mesh, Data);
+
+%% Solve the contact problem
+[Mesh, Data, Solution, A, b]=solve(Mesh, Data, {9:12, 13:14});
+
+%% Computation of the stresses and strains
+[Mesh, Solution]=postProcess(Mesh, Data, Solution);
+
+%% Visualization of the solution
+% For more information, type 'help plotSolution'
+plotSolution(Mesh, Data, Solution, 'disp', 'standard', 'stress', 'sigma_yy');
+
+%% Comparison between numerical and analytical solutions
+
+E_star=(1/E*(1-nu^2)+1/(10*E)*(1-nu^2))^(-1);
+F=4/3*E_star*sqrt(R)*d^(3/2);
+% Maximum pressure
+p0=1/pi*(6*F*E_star^2/R^2)^(1/3);
+
+lambda=Solution.lambda;
+lambda_max=max(abs(lambda));
\ No newline at end of file
diff --git a/Code/INTERNODES_small/internodes5.m b/Code/INTERNODES_small/internodes5.m
new file mode 100644
index 0000000..7b3a260
--- /dev/null
+++ b/Code/INTERNODES_small/internodes5.m
@@ -0,0 +1,65 @@
+% INTERNODES method for contact mechanics
+% Case study: Hertzian contact problem with Dirichlet boundary conditions
+% applied to both bodies. Structure with asperities and multiple contact
+% points.
+
+% Limitations: 2D
+clc
+clear variables
+close all
+
+% Read GMSH mesh file (.msh)
+[Mesh] = readGMSH('../../GMSH/Meshes/contact4_p1_0_05.msh');
+
+%% Parameter prescription
+% Mesh parameters
+[Mesh]=getParameters(Mesh);
+% Initialize model
+[Data]=setModel('model', 'static_solid', 'submodel', 'contact', 'type', 'linear');
+
+%% Set material parameters
+% Elastic modulus [N\m^2]
+E=30e9;
+% Poisson ratio [-]
+nu=0.2;
+% Specific mass [kg/m^3]
+rho=2500;
+% Thickness [m]
+t=1;
+
+% Plane stress conditions
+lambda=E*nu/(1-nu^2);
+mu=E/(2*(1+nu));
+
+[Data]=setCoefficient(Data, 'rho', t*rho);
+[Data]=setCoefficient(Data, 'lambda', t*lambda);
+[Data]=setCoefficient(Data, 'mu', t*mu);
+[Data]=setCoefficient(Data, 'f', @(x) [0; 0]);
+
+%% Set boundary conditions
+
+% Used for testing with invertible stiffness matrix
+BC={{'Type', 'Dirichlet', 'Edges', 25, 'Function', @(x) [0; 0]},...
+    {'Type', 'Neumann', 'Edges', 28, 'Function', @(x) [0; -1e9]}};
+
+p2={'BC', BC};    
+
+[Data]=setBoundaryConditions(Mesh, Data, p2);
+%% Set numerical parameters
+% For more information, type 'help setNumericalParameters'
+
+[Data]=setNumericalParameters(Mesh, Data, 'Contact', 'rescaling', E, 'radius', 0.2, 'maxiter', 30);
+[Data]=setNumericalParameters(Mesh, Data, 'PostProcessing', 'ampl_fact', 1, 'quantity', {'stress', 'strain'});
+
+%% Assemblage of the right-hand side
+[Data]=computeRHS(Mesh, Data);
+
+%% Solve the contact problem
+[Mesh, Data, Solution, A, b]=solve(Mesh, Data, {[23 25:27], [24 28:30]});
+
+%% Computation of the stresses and strains
+[Mesh, Solution]=postProcess(Mesh, Data, Solution);
+
+%% Visualization of the solution
+% For more information, type 'help plotSolution'
+plotSolution(Mesh, Data, Solution, 'disp', 'standard', 'stress', 'sigma_yy');
\ No newline at end of file
diff --git a/Code/INTERNODES_small/kr.m b/Code/INTERNODES_small/kr.m
new file mode 100644
index 0000000..874aea5
--- /dev/null
+++ b/Code/INTERNODES_small/kr.m
@@ -0,0 +1,31 @@
+function X = kr(U,varargin)
+%KR Khatri-Rao product.
+%   kr(A,B) returns the Khatri-Rao product of two matrices A and B, of 
+%   dimensions I-by-K and J-by-K respectively. The result is an I*J-by-K
+%   matrix formed by the matching columnwise Kronecker products, i.e.
+%   the k-th column of the Khatri-Rao product is defined as
+%   kron(A(:,k),B(:,k)).
+%
+%   kr(A,B,C,...) and kr({A B C ...}) compute a string of Khatri-Rao 
+%   products A o B o C o ..., where o denotes the Khatri-Rao product.
+%
+%   See also kron.
+
+%   Version: 21/10/10
+%   Authors: Laurent Sorber (Laurent.Sorber@cs.kuleuven.be)
+
+if ~iscell(U), U = [U varargin]; end
+K = size(U{1},2);
+if any(cellfun('size',U,2)-K)
+    error('kr:ColumnMismatch', ...
+          'Input matrices must have the same number of columns.');
+end
+J = size(U{end},1);
+X = reshape(U{end},[J 1 K]);
+for n = length(U)-1:-1:1
+    I = size(U{n},1);
+    A = reshape(U{n},[1 I K]);
+    X = reshape(bsxfun(@times,A,X),[I*J 1 K]);
+    J = I*J;
+end
+X = reshape(X,[size(X,1) K]);
diff --git a/Code/INTERNODES_small/mapGlobal2Local.m b/Code/INTERNODES_small/mapGlobal2Local.m
new file mode 100644
index 0000000..6b14a3a
--- /dev/null
+++ b/Code/INTERNODES_small/mapGlobal2Local.m
@@ -0,0 +1,13 @@
+function[Data]=mapGlobal2Local(Data)
+
+% mapGlobal2Local: Function which maps the global degrees of freedom to the
+% local ones for the INTERNODES matrix
+% INPUT:
+% Data:     Structure containing all the data parameters
+% OUTPUT:
+% Data:     Updated structure with the local degrees of freedom
+
+Nf=Data.Nf;
+[~, Data.body{1}.indx_local, ~]=intersect(Nf, Data.body{1}.interface_dof);
+[~, Data.body{2}.indx_local, ~]=intersect(Nf, Data.body{2}.interface_dof);
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_small/plotSolution.m b/Code/INTERNODES_small/plotSolution.m
new file mode 100644
index 0000000..47ba494
--- /dev/null
+++ b/Code/INTERNODES_small/plotSolution.m
@@ -0,0 +1,813 @@
+function[]=plotSolution(Mesh, Data, Solution, varargin)
+
+% plotSolution: Function which plots the computed quantities
+% INPUT:
+% Mesh:     Structure containing the mesh parameters
+% Data:     Structure containing the data parameters
+% Solution: Structure containing the solution computed
+% OPTIONAL INPUT
+% snapshot: selected time-step at which the solution should be viewed
+%   1 -> none (default)
+%   any time step in the range defined
+% view_video: view the video of the solution
+%   1 -> yes 
+%   0 -> no (default)
+% save_video: choose whether to save the video or not
+%   1 -> yes
+%   0 -> no (default)
+% plot_arg: choose which solution to visualize
+%   dynamic_solid or static_solid module
+%       disp (default)
+%       stress
+%       strain
+%   transient_heat module
+%       temp (default)
+%       flux
+% plot_type: decide which type of metric should be used for visualization
+%   displacement metric
+%       standard -> structure with amplified displacements (default)
+%       norm
+%       absx
+%       absy
+%   temperature metric
+%       standard (default)
+%   stress metric
+%       von_mises (default)
+%       sigma_xx
+%       sigma_yy
+%       sigma_xy
+%       sigma_xx_abs
+%       sigma_yy_abs
+%       sigma_xy_abs
+%       frobenius
+%   strain metric
+%       epsilon_xx (default)
+%       epsilon_yy
+%       epsilon_xy
+%       epsilon_xx_abs
+%       epsilon_yy_abs
+%       epsilon_xy_abs
+%       frobenius
+%   flux metric
+%       standard (default)
+
+
+%% Initialize and ckeck Parameters
+
+if mod(length(varargin), 2)~=0
+    error('Missing a name or value')
+end
+labels_in=varargin(1:2:end);
+values_in=varargin(2:2:end);
+
+% Specify number of sub-intervals in time        
+if contains(Data.Model.name, 'static')
+    static=true;
+    N=0;
+else
+    static=false;
+    N=Data.Discretization.N;
+end
+
+switch Data.Model.name
+    case {'static_solid', 'dynamic_solid'}
+        plot_argv={'disp', 'stress', 'strain'};
+        n=length(plot_argv);
+        plot_typev=cell(n,1);
+        plot_typev{1}={'standard', 'norm', 'absx', 'absy'};
+        plot_typev{2}={'von_mises', 'sigma_xx', 'sigma_yy', 'sigma_xy', 'sigma_xx_abs', 'sigma_yy_abs', 'sigma_xy_abs', 'frobenius'};
+        plot_typev{3}={'epsilon_xx', 'epsilon_yy', 'epsilon_xy', 'epsilon_xx_abs', 'epsilon_yy_abs', 'epsilon_xy_abs', 'frobenius'};
+        
+    case 'transient_heat'
+        plot_argv={'temp', 'flux'};
+        n=length(plot_argv);
+        plot_typev=cell(n,1);
+        plot_typev{1}={'standard'};
+        plot_typev{2}={'standard'};
+        
+end
+
+% Set all default parameters
+% Default visualization parameters
+Param.visua_param.snapshot=1;
+Param.visua_param.view_video=false;
+Param.visua_param.write_video=false;
+% Default plot argument
+Param.plot_param.arg=plot_argv(1);
+% Default plot type
+Param.plot_param.type=plot_typev{1}(1);
+
+plot_arg={};
+plot_type={};
+nb_plot=0;
+
+for k=1:length(labels_in)
+    arg=lower(labels_in{k});
+    val=values_in{k};
+    
+    switch arg
+        case 'snapshot'
+            
+            if static
+                warning([arg ' parameter will be ignored because the simulation is in statics'])
+            elseif val < 0 || val > Data.Discretization.N+1
+                error(['Time snapshot invalid: it must be an integer between ' num2str(0) ' and ' num2str(Data.Discretization.N+1)])
+            else
+                Param.visua_param.snapshot=val;
+            end
+            
+        case {'view_video', 'write_video'}
+            
+            if static
+                warning([arg ' parameter will be ignored because the simulation is in statics'])
+            else
+                if isa(val, 'double')
+                    
+                    if val<0 || val>1
+                        error([arg ' parameter must be a boolean'])
+                    else
+                        Param.visua_param.view_video=logical(val);
+                    end
+                    
+                elseif isa(val, 'logical')
+                    Param.visua_param.view_video=val;
+                else
+                    error([arg ' parameter must be a boolean'])
+                end
+            end
+
+        case plot_argv
+            
+            % Check if desired quantities have been computed
+            switch arg
+                case 'stress'
+                    if ~isfield(Solution, 'sigma')
+                        error('Stresses have not been computed')
+                    end
+                    
+                case 'strain'
+                    if ~isfield(Solution, 'epsilon')
+                        error('Strains have not been computed')
+                    end
+                    
+                case 'flux'
+                    if ~isfield(Solution, 'Q')
+                        error('Fluxes have not been computed')
+                    end    
+            end
+            
+            plot_arg{nb_plot+1}=arg;
+            index=strcmp(arg, plot_argv);
+
+            if any(ismember(plot_typev{index}, val))
+                plot_type{nb_plot+1}=val;
+            else
+                error('Plot type invalid: see available plot types')
+            end
+                     
+            nb_plot=nb_plot+1;
+        otherwise
+            error('Unrecognized argument')
+    end
+    
+end
+
+if nb_plot>0
+    Param.plot_param.arg=plot_arg;
+    Param.plot_param.type=plot_type;
+end
+
+%% Plotting interface
+
+plot_arg=Param.plot_param.arg;
+plot_type=Param.plot_param.type;
+n=length(plot_arg);
+Plot=cell(n,1);
+Axis=cell(n,1);
+Colorbar=cell(n,1);
+Dynamic=cell(n,1);
+
+fig=figure;
+% fig.Units='normalized';
+% fig.Position=[0 0 1 1];
+
+% fig.Units='normalized';
+% fig.Position=[0 1 1/2 1/2];
+
+for k=1:n
+    switch plot_arg{k}
+        case {'disp', 'temp'}
+            [PlotParam, AxisParam, ColorbarParam, DynamicParam]=plotPatch(Mesh, Data, Solution, plot_type{k});
+            subplot(1,n,k)
+            axis=gca;
+            p=patch(axis);
+            c=colorbar(axis);
+            [p]=    setObject(p, PlotParam);
+            [axis]= setObject(axis, AxisParam);
+            [c]=    setObject(c, ColorbarParam);
+            
+            
+            
+        case {'strain', 'stress'}
+            [PlotParam, AxisParam, ColorbarParam, DynamicParam]=plotSurf(Mesh, Data, Solution, plot_arg{k}, plot_type{k});
+            subplot(1,n,k)
+            axis=gca;
+            p=patch(axis);
+            c=colorbar(axis);
+            [p]=    setObject(p, PlotParam);
+            [axis]= setObject(axis, AxisParam);
+            [c]=    setObject(c, ColorbarParam);
+            
+            
+            
+        case 'flux'
+            [PlotParam, AxisParam, ColorbarParam, DynamicParam]=plotQuiver(Mesh, Solution, plot_type{k});
+            subplot(1,n,k)
+            axis=gca;
+            p=quiver(axis, PlotParam{1,2}, PlotParam{2,2});
+            c=colorbar(axis);
+            [p]=    setObject(p, PlotParam);
+            [axis]= setObject(axis, AxisParam);
+            [c]=    setObject(c, ColorbarParam);
+            
+    end
+    
+    Plot{k}=p;
+    Axis{k}=axis;
+    Colorbar{k}=c;
+    Dynamic{k}=DynamicParam;
+end
+
+if Param.visua_param.view_video % View the solution over the entire simulation
+    
+    for k=1:n
+        [Plot{k}, Axis{k}]=updateObject(Plot{k}, Axis{k}, Dynamic{k}, 1, N);
+    end
+    
+    frames(1)=getframe(fig);
+    for step = 2:N+1
+        pause(0.05)
+        for k=1:n
+            [Plot{k}, Axis{k}]=updateObject(Plot{k}, Axis{k}, Dynamic{k}, step, N);
+            refreshdata(Axis{k});
+        end
+        frames(step)=getframe(fig);
+    end
+   
+else % View solution at a particular time snapshot
+    step=Param.visua_param.snapshot;
+        for k=1:n
+            [Plot{k}, Axis{k}]=updateObject(Plot{k}, Axis{k}, Dynamic{k}, step, N);
+        end
+end
+
+
+if Param.visua_param.write_video
+%     vid_duration=25; % seconds
+    vid_duration=10; % seconds
+    video=VideoWriter('Output.mp4', 'MPEG-4');
+    video.FrameRate=(N+1)/vid_duration;
+    open(video);
+    writeVideo(video,frames);
+    close(video);
+end
+
+%% Parameters for patch plots
+    function[PlotParam, AxisParam, ColorbarParam, DynamicParam]=plotPatch(Mesh, Data, Solution, type)
+        
+        
+        % Retrieve mesh parameters
+        % Coordinate matrix
+        coord=Mesh.coord_mat;
+        % Connectivity matrix
+        connect=Mesh.connect_mat;
+        % Total number of nodes
+        nb_nodes=Mesh.nb_nodes;
+        % Frame surrounding the structure
+        frame=Mesh.frame;
+        % Largest dimension
+        L_max=Mesh.L_max;
+        % Number of local degrees of freedom
+        nb_dof_local=Mesh.nb_dof_local;
+        % Total number of degrees of freedom
+        nb_dof=Mesh.nb_dof;
+        
+        % Retrieve solution
+        U=Solution.U;
+        
+        
+        switch Data.Model.name
+               
+            case {'static_solid', 'dynamic_solid'}
+                
+                % Initilization
+                Vertices=zeros(nb_nodes,2,N+1);
+                CData=cell(N+1,1);
+                
+                
+                switch type
+                    case 'standard'
+                        
+                        if strcmp(Data.PostProcessing.amplification, 'auto')
+                            % Maximum displacement
+                            disp_max=max(max(abs(U)));
+                            % Amplification factor
+                            fact_ampl=L_max/(2*disp_max);
+                        else
+                            fact_ampl=Data.PostProcessing.amplification;
+                        end
+                        % Retrieve degrees of freedom
+                        d1=1:nb_dof_local:nb_dof;
+                        d2=2:nb_dof_local:nb_dof;
+                        % Aplified displacements
+                        ux_ampl=fact_ampl*U(d1,:);
+                        uy_ampl=fact_ampl*U(d2,:);
+                        % Deformed state
+                        def_x=coord(:,1)+ux_ampl;
+                        def_y=coord(:,2)+uy_ampl;
+                        % Vertices in deformed state
+                        Vertices(:,1,:)=def_x;
+                        Vertices(:,2,:)=def_y;
+                        XLim=[min(def_x, [], 'all');
+                              max(def_x, [], 'all')];
+                        YLim=[min(def_y, [], 'all');
+                              max(def_y, [], 'all')];
+                        CLim=[0,1];
+                        Colormap=[];
+                        ColorbarStatus='off';
+                        ColorbarLabel='';
+                        
+%                         Title='Amplified displacements';
+                        Title='Displacements';
+                        EdgeColor='black';
+                        FaceColor='none';
+                        
+                    case 'norm'
+                        
+                        % Retrieve degrees of freedom
+                        d1=1:nb_dof_local:nb_dof;
+                        d2=2:nb_dof_local:nb_dof;
+                        Ux=U(d1,:);
+                        Uy=U(d2,:);
+                        x=coord(:,1)+Ux;
+                        y=coord(:,2)+Uy;
+                        
+                        U_norm=sqrt(Ux.^2+Uy.^2);
+
+                        XLim=[min(x, [], 'all');
+                              max(x, [], 'all')];
+                        YLim=[min(y, [], 'all');
+                              max(y, [], 'all')];
+                        CLim=[0 max(U_norm, [], 'all')];
+                        Colormap=jet;
+                        ColorbarStatus='on';
+                        ColorbarLabel='Displacement [m]';
+                        Title='Norm of displacements';
+                        EdgeColor='none';
+                        FaceColor='interp';
+                        
+                        for m=1:N+1
+                            Vertices(:,1,m)=x(:,m);
+                            Vertices(:,2,m)=y(:,m);
+                            CData{m}=U_norm(:,m);
+                        end
+                        
+                    case 'absx'
+                        
+                        % Retrieve degrees of freedom
+                        d1=1:nb_dof_local:nb_dof;
+                        d2=2:nb_dof_local:nb_dof;
+                        Ux=U(d1,:);
+                        Uy=U(d2,:);
+                        x=coord(:,1)+Ux;
+                        y=coord(:,2)+Uy;
+                        
+                        Ux_abs=abs(Ux);
+                        
+                        XLim=[min(x, [], 'all');
+                              max(x, [], 'all')];
+                        YLim=[min(y, [], 'all');
+                              max(y, [], 'all')];
+                        CLim=[0 max(Ux_abs, [], 'all')];
+                        Colormap=jet;
+                        ColorbarStatus='on';
+                        ColorbarLabel='Displacement [m]';
+                        Title='Displacements in absolute value along x';
+                        EdgeColor='none';
+                        FaceColor='interp';
+                        
+                        for m=1:N+1
+                            Vertices(:,1,m)=x(:,m);
+                            Vertices(:,2,m)=y(:,m);
+                            CData{m}=Ux_abs(:,m);
+                        end
+                        
+                    case 'absy'
+                        
+                        % Retrieve degrees of freedom
+                        d1=1:nb_dof_local:nb_dof;
+                        d2=2:nb_dof_local:nb_dof;
+                        Ux=U(d1,:);
+                        Uy=U(d2,:);
+                        x=coord(:,1)+Ux;
+                        y=coord(:,2)+Uy;
+                        Uy_abs=abs(Uy);
+                        
+                        XLim=[min(x, [], 'all');
+                              max(x, [], 'all')];
+                        YLim=[min(y, [], 'all');
+                              max(y, [], 'all')];
+                        CLim=[0 max(Uy_abs, [], 'all')];
+                        Colormap=jet;
+                        ColorbarStatus='on';
+                        ColorbarLabel='Displacement [m]';
+                        Title='Displacements in absolute value along y';
+                        EdgeColor='none';
+                        FaceColor='interp';
+                        
+                        for m=1:N+1
+                            Vertices(:,1,m)=x(:,m);
+                            Vertices(:,2,m)=y(:,m);
+                            CData{m}=Uy_abs(:,m);
+                        end
+                        
+                        
+                end
+                
+                
+                % Set static parameters
+                PlotParam = {'Vertices',        coord;...
+                             'Faces',           connect(:,1:3);...
+                             'FaceVertexCData', CData{1};...
+                             'FaceColor',       FaceColor;...
+                             'EdgeColor',       EdgeColor;...
+                             'LineWidth',       1};
+                
+                AxisParam = {'XLim',            XLim;...
+                             'YLim',            YLim;...
+                             'CLim',            CLim;...
+                             'Colormap',        Colormap;...
+                             'DataAspectRatio', [1 1 1];...
+                             'XLabel.String',          'x coordinate [m]';...
+                             'YLabel.String',          'y coordinate [m]'};
+                
+                ColorbarParam = {'Visible',         ColorbarStatus;...
+                                 'Label.String',    ColorbarLabel;...
+                                 'Label.FontSize',  11};
+                
+                % Set dynamic parameters
+                DynamicParam={'Vertices',   num2cell(Vertices, [1,2]);...
+                              'CData',      CData;...
+                              'Title',      Title};
+                          
+            case 'transient_heat'
+                
+                % Number of sub-intervals in time
+                N=Data.Discretization.N;
+                % Initilization
+                CData=cell(N+1,1);
+                
+                switch type
+                    case 'standard'
+                        
+                        XLim=frame(:,1);
+                        YLim=frame(:,2);
+                        CLim=[min(U, [], 'all') max(U, [], 'all')];
+                        Colormap=jet;
+                        ColorbarStatus='on';
+                        ColorbarLabel='Temperature [ºC]';
+                        % ColorbarLabel='Temperature [K]';
+                        Title='Temperature';
+                        EdgeColor='none';
+                        FaceColor='interp';
+                        
+                        for m=1:N+1
+                            CData{m}=U(:,m);
+                        end
+                        
+                end
+                
+                
+                % Set static parameters
+                PlotParam = {'Vertices',        coord;...
+                             'Faces',           connect(:,1:3);...
+                             'FaceVertexCData', CData{1};...
+                             'FaceColor',       FaceColor;...
+                             'EdgeColor',       EdgeColor;...
+                             'LineWidth',       1};
+                
+                AxisParam = {'XLim',            XLim;...
+                             'YLim',            YLim;...
+                             'CLim',            CLim;...
+                             'Colormap',        Colormap;...
+                             'DataAspectRatio', [1 1 1];...
+                             'XLabel.String',          'x coordinate [m]';...
+                             'YLabel.String',          'y coordinate [m]'};
+                
+                ColorbarParam = {'Visible',         ColorbarStatus;...
+                                 'Label.String',    ColorbarLabel;...
+                                 'Label.FontSize',  11};
+                
+                % Set dynamic parameters
+                DynamicParam={'CData',      CData;...
+                              'Title',      Title};
+                   
+        end
+        
+        
+    end
+
+%% Parameters for surf plots
+    function[PlotParam, AxisParam, ColorbarParam, DynamicParam]=plotSurf(Mesh, Data, Solution, arg, type)
+  
+        % Retrieve mesh parameters
+        % Coordinate matrix of interpolation nodes
+        coord_inter=Mesh.coord_inter;
+        % Frame surrounding the structure
+        frame=Mesh.frame;
+        
+        % Retrieve data parameters
+        % Number of evaluation points per element
+        nq=Data.PostProcessing.eval_points;
+        
+        % Total number of interpolation nodes
+        nb_nodes_inter=size(coord_inter,1);
+        
+        % Initilization
+        Vertices=zeros(nb_nodes_inter,2,N+1);
+        CData=cell(N+1,1);
+        
+        switch arg
+            
+            case 'stress'
+                
+                % Retrieve solution
+                sigma=permute(Solution.sigma, [2 1 3]);
+                
+                switch type
+                    case 'von_mises'
+                        
+                        % Von Mises stress
+                        select=sqrt(sigma(:,1,:).^2-sigma(:,1,:).*sigma(:,2,:)+sigma(:,2,:).^2+3*sigma(:,3,:).^2);
+                        ColorbarLabel='Stress [Pa]';
+                        Title='Von Mises stress field';
+                        
+                    case 'sigma_xx'
+                        
+                        % Value of sigma_xx
+                        select=sigma(:,1,:);
+                        ColorbarLabel='Stress [Pa]';
+                        Title='\sigma_{xx}';
+                        
+                    case 'sigma_yy'
+                        
+                        % Value of sigma_yy
+                        select=sigma(:,3,:);
+                        ColorbarLabel='Stress [Pa]';
+                        Title='\sigma_{yy}';
+                        
+                    case 'sigma_xy'
+                        
+                        % Value of sigma_xy
+                        select=sigma(:,2,:);
+                        ColorbarLabel='Stress [Pa]';
+                        Title='\sigma_{xy}';
+                        
+                    case 'sigma_xx_abs'
+                        
+                        % Abolute value of sigma_xx
+                        select=abs(sigma(:,1,:));
+                        ColorbarLabel='Stress [Pa]';
+                        Title='|\sigma_{xx}|';
+                        
+                    case 'sigma_yy_abs'
+                        
+                        % Absolute value of sigma_yy
+                        select=abs(sigma(:,3,:));
+                        ColorbarLabel='Stress [Pa]';
+                        Title='|\sigma_{yy}|';
+                        
+                    case 'sigma_xy_abs'
+                        
+                        % Absolute value of sigma_xy
+                        select=abs(sigma(:,2,:));
+                        ColorbarLabel='Stress [Pa]';
+                        Title='|\sigma_{xy}|';
+                        
+                    case 'frobenius'
+                        
+                        % Frobenius norm of sigma
+                        select=sqrt(sigma(:,2,:).^2+2*sigma(:,2,:).^2+sigma(:,3,:).^2);
+                        ColorbarLabel='Stress [Pa]';
+                        Title='Frobenius norm of \sigma';
+                        
+                end
+                
+            case 'strain'
+                
+                % Retrieve solution
+                epsilon=permute(Solution.epsilon, [2 1 3]);
+                
+                switch type
+                    case 'epsilon_xx'
+                        
+                        % Value of epsilon_xx
+                        select=epsilon(:,1,:);
+                        ColorbarLabel='Strain [-]';
+                        Title='\epsilon_{xx}';
+                        
+                    case 'epsilon_yy'
+                        
+                        % Value of epsilon_yy
+                        select=epsilon(:,3,:);
+                        ColorbarLabel='Strain [-]';
+                        Title='\epsilon_{yy}';
+                        
+                    case 'epsilon_xy'
+                        
+                        % Value of esilon_xy
+                        select=epsilon(:,2,:);
+                        ColorbarLabel='Strain [-]';
+                        Title='\epsilon_{xy}';
+                        
+                    case 'epsilon_xx_abs'
+                        
+                        % Abolute value of epsilon_xx
+                        select=abs(epsilon(:,1,:));
+                        ColorbarLabel='Strain [-]';
+                        Title='|\epsilon_{xx}|';
+                        
+                    case 'epsilon_yy_abs'
+                        
+                        % Absolute value of epsilon_yy
+                        select=abs(epsilon(:,3,:));
+                        ColorbarLabel='Strain [-]';
+                        Title='|\epsilon_{yy}|';
+                        
+                    case 'epsilon_xy_abs'
+                        
+                        % Absolute value of epsilon_xy
+                        select=abs(epsilon(:,2,:));
+                        ColorbarLabel='Strain [-]';
+                        Title='|\epsilon_{xy}|';
+                        
+                    case 'frobenius'
+                        
+                        % Frobenius norm of epsilon
+                        select=sqrt(epsilon(:,2,:).^2+2*epsilon(:,2,:).^2+epsilon(:,3,:).^2);
+                        ColorbarLabel='Strain [-]';
+                        Title='Frobenius norm of \epsilon';
+                        
+                end
+                
+        end
+        
+        % Common parameters
+        XLim=[min(coord_inter(:,1,:), [], 'all');
+              max(coord_inter(:,1,:), [], 'all')];
+        YLim=[min(coord_inter(:,2,:), [], 'all');
+              max(coord_inter(:,2,:), [], 'all')];
+        CLim=[min(select, [], 'all') max(select, [], 'all')];
+        Colormap=jet;
+        ColorbarStatus='on';
+        EdgeColor='none';
+        FaceColor='interp';
+        
+        for m=1:N+1
+            Vertices(:,1,m)=coord_inter(:,1,m);
+            Vertices(:,2,m)=coord_inter(:,2,m);
+            CData{m}=select(:,m);
+        end
+        
+
+        switch nq
+            case 3
+                % 3-point evaluation inside each element
+                I=1:3*Mesh.nb_elem;
+                I=reshape(I, 3, Mesh.nb_elem)';
+                
+            case 6
+                % 6-point evaluation inside each element
+                i1=[1 4 5 2 2 5 6 3 3 6 4 1];
+                i2=[4 5 6];
+                I1=i1+(0:Mesh.nb_elem-1)'*6;
+                I2=i2+(0:Mesh.nb_elem-1)'*6;
+                I1=reshape(I1', 4, 3*Mesh.nb_elem)';
+                I2=[I2 nan*zeros(size(I2,1),1)];
+                I=[I1; I2];
+        end
+        
+                % Set static parameters
+                PlotParam = {'Vertices',        coord_inter;...
+                             'Faces',           I;...
+                             'FaceVertexCData', CData{1};...
+                             'FaceColor',       FaceColor;...
+                             'EdgeColor',       EdgeColor;...
+                             'LineWidth',       1};
+                
+                AxisParam = {'XLim',            XLim;...
+                             'YLim',            YLim;...
+                             'CLim',            CLim;...
+                             'Colormap',        Colormap;...
+                             'DataAspectRatio', [1 1 1];...
+                             'XLabel.String',          'x coordinate [m]';...
+                             'YLabel.String',          'y coordinate [m]'};
+                
+                ColorbarParam = {'Visible',         ColorbarStatus;...
+                                 'Label.String',    ColorbarLabel;...
+                                 'Label.FontSize',  11};
+                
+                % Set dynamic parameters
+                DynamicParam={'Vertices',   num2cell(Vertices, [1,2]);...
+                              'CData',      CData;...
+                              'Title',      Title};
+        
+    end
+
+%% Parameters for quiver plots
+    function[PlotParam, AxisParam, ColorbarParam, DynamicParam]=plotQuiver(Mesh, Solution, type)
+        
+        % Retrieve mesh parameters
+        % Coordinate matrix
+        coord=Mesh.coord_mat;
+        % Frame surrounding the structure
+        frame=Mesh.frame;
+        
+        % Retrieve solution
+        Q_sort=Solution.Q_sort;
+        
+        % Initialization
+        U=cell(1,N+1);
+        V=cell(1,N+1);
+        
+        switch type
+            case 'standard'
+                % Standard visualization of fluxes
+                Title='Flux field';
+                
+                for m=1:N+1
+                    U{m}=Q_sort(:,2*m-1);
+                    V{m}=Q_sort(:,2*m);
+                end
+                
+        end
+        
+        
+        % Set static parameters
+        PlotParam = {'XData',        coord(:,1);...
+                     'YData',        coord(:,2);...
+                     'LineWidth',       1};
+        
+        AxisParam = {'XLim',            frame(:,1);...
+                     'YLim',            frame(:,2);...
+                     'DataAspectRatio', [1 1 1];...
+                     'XLabel.String',          'x coordinate [m]';...
+                     'YLabel.String',          'y coordinate [m]'};
+        
+        ColorbarParam = {'Visible',         'off';...
+                         'Label.String',    '';...
+                         'Label.FontSize',  11};
+        
+        % Set dynamic parameters
+        DynamicParam={'UData',      U;...
+                      'VData',      V;...
+                      'Title',      Title};
+        
+        
+    end
+
+
+
+
+
+
+%% Setting and updating graphic properties
+    function[object]=setObject(object, param)
+        
+        argument=param(:,1);
+        value=param(:,2);
+        
+        for i=1:length(argument)         
+            if contains(argument{i}, '.')
+                field=extractBetween(argument{i}, 1, '.');
+                subfield=extractAfter(argument{i}, '.');
+                object.(field{1}).(subfield)=value{i};
+            else 
+                object.(argument{i})=value{i};
+            end
+        end
+        
+    end
+
+    function[plot, axis]=updateObject(plot, axis, param, step, N)
+        
+        argument=param(:,1);
+        value=param(:,2);
+        
+        for i=1:length(argument)-1
+            plot.(argument{i})=value{i}{step};
+        end
+        
+        axis.Title.String=[value{end} ' at step ' num2str(step) ' out of ' num2str(N+1)];
+    end
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_small/postProcess.m b/Code/INTERNODES_small/postProcess.m
new file mode 100644
index 0000000..364859c
--- /dev/null
+++ b/Code/INTERNODES_small/postProcess.m
@@ -0,0 +1,18 @@
+function[Mesh, Solution]=postProcess(Mesh, Data, Solution)
+
+% postProcess: General post-processing function used to compute derived
+% quantities
+% INPUT:
+% Mesh:     Structure containing all the mesh parameters
+% Data:     Structure containing all the data parameters
+% Solution: Structure containing the solution
+% OUTPUT:
+% Mesh:     Updated mesh structure with coordinates of nodes where stresses
+%           and strains were evaluated
+% Solution: Updated solution structure with derived quantities
+
+
+if any(contains(Data.PostProcessing.quantity, {'stress', 'strain'}))
+    [Mesh, Solution]=postProcessDisp(Mesh, Data, Solution);  
+end
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_small/postProcessDisp.m b/Code/INTERNODES_small/postProcessDisp.m
new file mode 100644
index 0000000..f1f6613
--- /dev/null
+++ b/Code/INTERNODES_small/postProcessDisp.m
@@ -0,0 +1,147 @@
+function[Mesh, Solution]=postProcessDisp(Mesh, Data, Solution)
+
+% postProcessDisp: Function used to compute strains and stresses once the 
+% displacements are known.
+% INPUT:
+% Mesh:     Structure containing all mesh parameters
+% Data:     Structure containing all data parameters
+% Solution: Structure containing the computed solutions
+% OUTPUT:
+% Mesh:     Updated mesh structure with evaluation points coordinates
+% Solution: Updated solution structure including strains and stresses
+
+
+% Finite element order
+order=Mesh.order;
+% Number of local degrees of freedom
+d=Mesh.nb_dof_local;
+% Number of elements
+ne=Mesh.nb_elem;
+% Number of nodes per element
+nne=Mesh.nb_nodes_elem;
+% Coordinate matrix
+coord=Mesh.coord_mat;
+% Connectivity matrix
+connect=Mesh.connect_mat;
+% Retrieve basis functions
+[Functions]=getFunctions(order);
+% Retrieve Jacobian matrix
+J_phi=Functions.J_phi;
+% Number of evaluation points per element
+nq=Data.PostProcessing.eval_points;
+% Retrieve evaluation points
+[points, ~] = getQuadrature(nq, 'interpolation');
+% Number of independent strain and stress components
+Mesh.nb_comp_ind=1/2*d*(d+1);
+% Mu
+f1=Data.Coefficient.mu.function;
+% Lambda
+f2=Data.Coefficient.lambda.function;
+% Retrieve solution
+U_sort=Solution.U_sort;
+% Compute stresses and strains using current position
+coord=coord+U_sort;
+
+% Define permutation matrix
+Sdd=zeros(d*d);
+Id1=eye(d);
+Id2=eye(d^2);
+
+for k=1:d
+    Sdd=Sdd+kron(Id1(:,k)', kron(Id1, Id1(:,k)));
+end
+
+% Reduce size of matrices to avoid unnecessary computations
+% sigma_xx, sigma_xy, sigma_yy
+h=[1 2 4];
+
+I1=(Id2+Sdd);
+I2=Id1(:);
+
+% Truncate the matrices
+I1=I1(h,:);
+I2=I2(h);
+
+% Initialization
+Q=zeros(nq*d^2, nne*d);
+
+% Loop over the Gauss points
+for i = 1:nq
+    L=kron(J_phi(points(i,:))', Id1);
+    Q((i-1)*d^2+1:i*d^2, :)=L;
+end
+Q=sparse(Q);
+T=connect';
+G=T(:);
+
+% Coordinates of the nodes of the elements
+coord_nodes=coord(G,:);
+% Displacements of the nodes of the elements
+W=reshape(U_sort(G,:)', nne*d, ne);
+% Vertices of the elements
+a=coord_nodes(1:nne:end,:);
+b=coord_nodes(2:nne:end,:);
+c=coord_nodes(3:nne:end,:);
+% Interpolated coordinates
+P=a(:)+[b(:)-a(:) c(:)-a(:)]*points';
+X(:,:,1)=P(1:ne,:);
+X(:,:,2)=P(ne+1:end,:);
+coord_inter=reshape(P', nq*ne, d);
+% Update mesh
+Mesh.coord_inter=coord_inter;
+
+% Computation of geometric quantities
+% Determinants of Jacobian matrices
+detJK=(b(:,1)-a(:,1)).*(c(:,2)-a(:,2))-(b(:,2)-a(:,2)).*(c(:,1)-a(:,1));
+
+JK_invT=zeros(d, d*ne);
+JK_invT(:,1:d:d*ne)=1./detJK'.*[c(:,2)-a(:,2) -(c(:,1)-a(:,1))]';
+JK_invT(:,2:d:d*ne)=1./detJK'.*[-(b(:,2)-a(:,2)) b(:,1)-a(:,1)]';
+
+vec_JK_invT=reshape(JK_invT, 1, d^2*ne);
+
+% Coefficient evaluation
+F1=f1(X);
+F2=f2(X);
+if size(F1,1)>1
+F1=reshape(F1, 1, nq, ne);
+end
+if size(F2,1)>1
+F2=reshape(F2, 1, nq, ne);
+end
+
+% Matrix product computations
+W=reshape(Q*W, d^2, nq, ne);
+
+B1=reshape(product(JK_invT,repmat(Id1, 1, ne),d,ne), d^2, d^2, ne);
+B2=reshape(vec_JK_invT, 1, d^2, ne);
+
+% Computation of strains
+if any(contains(Data.PostProcessing.quantity, 'strain'))
+    E=pagemtimes(B1, W);
+    E=0.5*I1*reshape(E, d^2, nq*ne);
+    Solution.epsilon=E;
+end
+
+% Computation of stresses
+if any(contains(Data.PostProcessing.quantity, 'stress'))
+    S1=pagemtimes(B1, F1.*W);
+    S2=pagemtimes(B2, F2.*W);
+    S1=I1*reshape(S1, d^2, nq*ne);
+    S2=I2*reshape(S2, 1, nq*ne);
+    S=S1+S2;
+    Solution.sigma=S;
+end
+
+
+    function[M]=product(A,B,d,ne)
+        
+        % !! Only for a two-dimensional case !!
+        M=zeros(d^2, d^2*ne);
+        
+        M(:,1:d^2:end)=kr(A(:,1:d:end), B(:,1:d:end));
+        M(:,2:d^2:end)=kr(A(:,1:d:end), B(:,2:d:end));
+        M(:,3:d^2:end)=kr(A(:,2:d:end), B(:,1:d:end));
+        M(:,4:d^2:end)=kr(A(:,2:d:end), B(:,2:d:end));
+    end
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_small/readGMSH.m b/Code/INTERNODES_small/readGMSH.m
new file mode 100755
index 0000000..21425ca
--- /dev/null
+++ b/Code/INTERNODES_small/readGMSH.m
@@ -0,0 +1,86 @@
+function [Mesh] = readGMSH(file_name)
+
+% readGMSH: Reads a GMSH mesh file (.msh) and constructs the coordinate,
+% connectivity, boundary nodes, material tag and boundary tag matrices
+% INPUT:
+% file_name: GMSH mesh file (.msh) 
+% OUTPUT:
+% Mesh: Structure containing all fields related to the geometry, boundary
+% conditions and material tags
+%   coord: Matrix of node coordinates
+%   connectivites: Connectivity matrix of the system
+%   boundary_nodes: Matrix containing the boundary nodes (both displacements
+%   and external forces)
+%   material_tag: Vector containing the material tag of the elements
+%   boundary_tag: Vector containing the boundary tags of the elements 
+%   forming boundaries (enables to make the distinction between Dirichlet
+%   and Neumann boundary conditions)
+
+fileID = fopen(file_name);
+text = textscan(fileID, '%s', 'delimiter', '\n');
+fclose(fileID);
+
+text=text{1};
+% Construction of the coordinate matrix
+nodes_start = findPos(text, '$Nodes') + 1;
+
+fileID = fopen(file_name);
+% Node coordinates
+coord = textscan(fileID, '%f %f %f %f', 'HeaderLines', nodes_start);
+% Elements
+connect = textscan(fileID, [repmat('%f', 1, 20) '%*[^\n]'], 'HeaderLines', 3, 'EndOfLine', '\n', 'CollectOutput', 1);
+fclose(fileID);
+
+% Coordinate matrix
+coord=cell2mat(coord);
+% Ignore z component
+coord=coord(:,2:end-1);
+
+% Connectivity matrix
+connect=cell2mat(connect);
+
+I=sum(~isnan(connect), 2);
+v=unique(I);
+
+% Boundary elements
+B=connect(I==v(1), 1:v(1));
+% Elements
+E=connect(I==v(2), 1:v(2));
+
+% Boundary tags
+B_tags=B(:,4);
+% Element tags
+E_tags=E(:,4);
+
+% Boundary elements connectivity matrix
+B_connect=B(:,6:end);
+% Elements connectivity matrix
+E_connect=E(:,6:end);
+
+% Initialize mesh structure
+Mesh.coord_mat=coord;
+Mesh.BC_nodes=B_connect;
+Mesh.connect_mat=E_connect;
+Mesh.BC_tag=B_tags;
+Mesh.material_tag=E_tags;
+
+% Identification of the type of element in GMSH
+% 2 -> T3
+% 9 -> T6
+% 21 -> T10
+% 1 -> line
+% 8 -> quadratic curve
+% 26 -> cubic curve
+end
+
+
+function[start]=findPos(text, string)
+lines = size(text, 1);
+start = 1;
+for i = 1:lines
+    if strcmp(text{i}, string)
+        start = i;
+        break
+    end 
+end
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_small/removeTraction.m b/Code/INTERNODES_small/removeTraction.m
new file mode 100644
index 0000000..e4c5c16
--- /dev/null
+++ b/Code/INTERNODES_small/removeTraction.m
@@ -0,0 +1,73 @@
+function[Data]=removeTraction(Mesh, Data, Solution, n)
+
+% removeTraction: Checks for convergence and updates the interface properties
+% INPUT:
+% Mesh:     Structure containing all the mesh parameters
+% Data:     Structure containing all the data parameters
+% Solution: Structure containing the solution
+% n:        Current iteration number
+% OUTPUT:
+% Data:     Updated data structure with updated interface properties
+
+% Compute the normals based on deformed structure
+% Normals for initial interface
+[Data]=computeNormals(Mesh, Data, Solution);
+
+normal1=Data.body{1}.normal;
+normal2=Data.body{2}.normal;
+
+% Retrieve Lagrange multipliers
+lambda1=Solution.lambda;
+lambda_sort1=Solution.lambda_sort;
+
+% Project the lambdas on interface 2
+lambda2=-Data.R21*lambda1;
+lambda_sort2=reshape(lambda2, Mesh.nb_dof_local, [])';
+
+% Computes the scalar products
+scalar1=sum(lambda_sort1.*normal1(Data.body{1}.active_set,:),2);
+scalar2=sum(lambda_sort2.*normal2(Data.body{2}.active_set,:),2);
+
+% Detect nodes to be removed from the interfaces
+dump_nodes1=Data.body{1}.interface_nodes(scalar1>0);
+dump_nodes2=Data.body{2}.interface_nodes(scalar2>0);
+
+% Update the interfaces
+% If only compression test penetration, otherwise remove nodes in traction
+if isempty(dump_nodes1) && isempty(dump_nodes2)
+    % Gap verification
+    [add_nodes1, add_nodes2]=detectGaps(Mesh, Data, Solution);
+    
+    [Data, nodediff1]=updateInterface(Mesh, Data, 1, add_nodes1, 'add');
+    [Data, nodediff2]=updateInterface(Mesh, Data, 2, add_nodes2, 'add');
+    
+else
+    % Skip gap verification
+    [Data, nodediff1]=updateInterface(Mesh, Data, 1, dump_nodes1, 'dump');
+    [Data, nodediff2]=updateInterface(Mesh, Data, 2, dump_nodes2, 'dump');
+end
+
+fprintf('Iteration %d: \n', n)
+fprintf('%+d nodes on interface 1 \n', nodediff1)
+fprintf('%+d nodes on interface 2 \n', nodediff2)
+fprintf('----------------------------------- \n')
+
+% Check for convergence failure
+if isempty(Data.body{1}.interface_nodes) || isempty(Data.body{2}.interface_nodes)
+    msg=['The contact algorithm stopped prematurely because at least one of ' ...
+         'the sets of nodes of the potential contact interface is empty. '...
+         'Potential causes for this error are:\n'];
+     
+    causes=['1) The boundary conditions are incorrect \n'...
+            '2) The potential contact interface was misidentified \n'...
+            '3) The mesh is too coarse \n'];
+    
+    error(sprintf([msg, causes]))
+end
+
+% Check for convergence
+if abs(nodediff1)+abs(nodediff2)==0 % Stop the iterations
+    Data.Contact.iterate=0;
+    disp('Successfully converged')
+end
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_small/setBoundaryConditions.m b/Code/INTERNODES_small/setBoundaryConditions.m
new file mode 100644
index 0000000..2d375dc
--- /dev/null
+++ b/Code/INTERNODES_small/setBoundaryConditions.m
@@ -0,0 +1,267 @@
+function[Data]=setBoundaryConditions(Mesh, Data, varargin)
+
+% setBoundaryConditions: Function which initializes the boundary data
+% INPUT:
+% Mesh:     Structure containing all the mesh parameters
+% Data:     Structure containing all the data parameters
+% OPTIONAL INPUT:
+% Any type of boundary condition. If no Dirichlet boundary conditions are 
+% specified, the program returns an error
+% Types of boundary conditions:
+%   Dirichlet
+%   Neumann
+% OUTPUT:
+% Data: Updated data structure with boundary conditions
+
+
+%% Set boundary conditions
+if mod(length(varargin{1}), 2)~=0
+    error('Missing a name or value')
+end
+
+% Retrieve boundary nodes
+BC_tags=unique(Mesh.BC_tag);
+
+% Dirichlet type
+D_types={'dirichlet'};
+% Neumann types
+N_types={'neumann'};
+% Available types of boundary conditions
+available_types=union(D_types, N_types);
+% Count to detect duplicate boundaries
+count=[];
+% Count number of Dirichlet and Neumann boundaries specified
+nb_D_BC=0;
+nb_N_BC=0;
+
+% Retrieve labels and values entered by the user
+labels_in=varargin{1}(1:2:end);
+values_in=varargin{1}(2:2:end);
+
+% Initilization
+Data.BC=[];
+
+% Checking for boundary conditions
+c1=strcmp(labels_in, 'BC');
+
+if any(c1)
+    BC=values_in{c1};
+    nb_BC=length(BC);
+    Data.BC=cell(nb_BC,1);
+    
+    for i=1:nb_BC
+        bc=BC{i};
+        l=length(bc);
+        
+        if mod(l,2)~=0
+            error('Missing a name or value')
+        end
+        
+        for j=1:2:l
+            arg=lower(bc{j});
+            val=bc{j+1};
+            
+            switch arg
+                
+                case 'type'
+                    if any(ismember(available_types, lower(val)))
+                        Data.BC{i}.(arg)=lower(val);
+                        
+                        if any(ismember(D_types, lower(val)))
+                            nb_D_BC=nb_D_BC+1;
+                        else
+                            nb_N_BC=nb_N_BC+1;
+                        end
+                    else
+                        error('Unrecognized boundary condition')
+                    end
+                    
+                case 'edges'
+                    
+                    edges=intersect(BC_tags, val);
+                    
+                    if isempty(edges)
+                        error('One of the edges specified does not exist')
+                    elseif ~isempty(intersect(count, val)) || length(unique(val))~=length(val)
+                        error('Duplicate edges detected')   
+                    end
+                    count=[count val];
+                    Data.BC{i}.(arg)=val;
+                    
+                case {'function'}
+                    
+                    if isa(val,'double')
+                        Data.BC{i}.label='const';
+                        Data.BC{i}.(arg)= @(x) val;
+                        
+                    elseif isa(val,'function_handle')
+                        Data.BC{i}.label=detectDependency(val);
+                        Data.BC{i}.(arg)=val;
+                    else
+                        error('Input parameters must be either constants or function handles')
+                    end
+                    
+                otherwise
+                    error('Unrecognized argument')
+            end
+            
+        end
+        
+        
+    end
+    
+end
+
+%% Post-Processing: Partitioning the structure BC into Dirichlet BC and Neumann BC and checking for missing data
+
+if nb_D_BC==0
+    error('Dirichlet boundary conditions must be prescribed')
+end
+
+BC=Data.BC;
+l=length(BC);
+
+% Initialize cells
+D_BC=cell(nb_D_BC,1);
+N_BC=cell(nb_N_BC,1);
+% Set of Dirichlet and Neumann tags
+D_tags=[];
+N_tags=[];
+% Initialize counters
+d=1;
+n=1;
+
+for i=1:l
+    bc=BC{i};
+    type=bc.type;
+    
+    if ~isfield(bc, 'edges')
+        error('The edges must be specified')
+    end
+    
+    if any(ismember(D_types, type))
+        D_tags=[D_tags bc.edges];
+        D_BC{d}.type=type; 
+        D_BC{d}.edges=bc.edges;
+        
+        if isfield(bc, 'function')
+            D_BC{d}.function=bc.function;
+            D_BC{d}.label=bc.label;
+        else
+            warning('A function has not been prescribed and will be assumed zero')
+            D_BC{d}.function=@(x) [0; 0];
+            D_BC{d}.label='const';
+        end
+        
+        d=d+1;
+        
+    elseif any(ismember(N_types, type))
+        N_tags=[N_tags bc.edges];
+        N_BC{n}.type=type;
+        N_BC{n}.edges=bc.edges;
+        
+        
+        if isfield(bc, 'function')
+            N_BC{n}.function=bc.function;
+            N_BC{n}.label=bc.label;
+        else
+            warning('A function has not been prescribed and will be assumed zero')
+            N_BC{n}.function=@(x) [0; 0];
+            N_BC{n}.label='const';
+        end
+        
+        n=n+1;
+    end
+end
+
+Data.D_BC=D_BC;
+Data.N_BC=N_BC;
+
+Data.D_tags=D_tags;
+Data.N_tags=N_tags;
+
+%% Sorting nodes
+% If there is a node belonging to conflicting boundary conditions, it is
+% set using the following priority list: Dirichlet, Neumann, Interior
+BC_nodes=Mesh.BC_nodes;
+BC_tag=Mesh.BC_tag;
+nb_nodes=Mesh.nb_nodes;
+Dof_set=Mesh.Dof_set;
+
+set_D_nodes=cell(nb_D_BC,1);
+set_N_nodes=cell(nb_N_BC,1);
+
+for k=1:nb_D_BC
+    nodes=BC_nodes(any(BC_tag==D_BC{k}.edges,2),:);
+    set_D_nodes{k}=unique(nodes);
+end
+for k=1:nb_N_BC
+    nodes=BC_nodes(any(BC_tag==N_BC{k}.edges,2),:);
+    set_N_nodes{k}=unique(nodes);
+end
+% Define the sets of nodes
+% Dirichlet nodes
+Nodes_D=unique(cell2mat(set_D_nodes));
+% Neumann nodes
+Nodes_N=setdiff(unique(BC_nodes), Nodes_D);
+% Interior nodes
+Nodes_I=setdiff(1:nb_nodes, union(Nodes_D, Nodes_N));
+% Free nodes
+Nodes_F=union(Nodes_I, Nodes_N);
+
+% Correct the set for Neumann nodes
+for k=1:nb_N_BC
+    set_N_nodes{k}=setdiff(set_N_nodes{k}, Nodes_D);
+end
+
+
+% Define the sets of degrees of freedom
+Nd=Dof_set(:,Nodes_D);
+Nn=Dof_set(:,Nodes_N);
+Ni=Dof_set(:,Nodes_I);
+
+Ni=Ni(:);
+Nd=Nd(:);
+Nn=Nn(:);
+Nf=union(Ni, Nn);
+
+% Update data
+Data.Nodes_I=Nodes_I;
+Data.Nodes_N=Nodes_N;
+Data.Nodes_D=Nodes_D;
+Data.Nodes_F=Nodes_F;
+
+Data.Ni=Ni;
+Data.Nd=Nd;
+Data.Nn=Nn;
+Data.Nf=Nf;
+
+Data.set_D_nodes=set_D_nodes;
+Data.set_N_nodes=set_N_nodes;
+
+%% Auxiliary function
+
+    function[label]=detectDependency(func)
+        
+        string=func2str(func);
+        
+        if ~strcmp(string(1:4), '@(x)')
+            error('Function handle must contain space variables')
+        end
+        string=string(5:end);
+        
+        if contains(string, 'x') && contains(string, 't')
+            label='space_time';
+        elseif contains(string, 'x')
+            label='space';
+        elseif contains(string, 't')
+            label='time';
+        elseif contains(string(1), '0')
+            label='zero';
+        else
+            label='const';
+        end
+        
+    end
+
+end
diff --git a/Code/INTERNODES_small/setCoefficient.m b/Code/INTERNODES_small/setCoefficient.m
new file mode 100644
index 0000000..d15a956
--- /dev/null
+++ b/Code/INTERNODES_small/setCoefficient.m
@@ -0,0 +1,103 @@
+function[Data]=setCoefficient(Data, varargin)
+
+% setCoefficient: Initializes the model coefficients
+% OPTIONAL INPUT:
+% rho:      Specific mass
+% mu:       Lamé constant
+% lambda:   Lamé constant
+% f:        Function "f" of the PDE
+% OUTPUT:
+% Data:     Data structure with initialized coefficients
+
+if mod(length(varargin), 2)~=0
+    error('Missing a name or value')
+end
+labels_in=varargin(1:2:end);
+values_in=varargin(2:2:end);
+
+available_coeff={'rho', 'lambda', 'mu', 'f'};
+
+for i=1:length(labels_in)
+    arg=lower(labels_in{i});
+    val=values_in{i};
+    
+    if any(ismember(available_coeff, arg))
+        
+        
+        switch arg
+            case {'rho', 'lambda', 'mu'} % Only space dependent
+                if isa(val,'double')
+                    
+                    if val ~= 0
+                        Data.Coefficient.(arg).label='const';
+                    else
+                        Data.Coefficient.(arg).label='zero';
+                    end
+                    Data.Coefficient.(arg).function=@(x) val;
+                    
+                elseif isa(val,'function_handle')
+                    Data.Coefficient.(arg).label=detectDependency1(val);
+                    Data.Coefficient.(arg).function=val;
+                else
+                    error('Input parameters must be either constants or function handles')
+                end
+                
+            case 'f'
+                if isa(val,'double') % Only space dependent
+                    Data.Coefficient.f.label='const';
+                    Data.Coefficient.f.function=@(x) val;
+                elseif isa(val,'function_handle')
+                    Data.Coefficient.f.label=detectDependency2(val);
+                    Data.Coefficient.f.function=@(x) val(x);
+                else
+                    error('Input parameters must be either constants or function handles')
+                end
+                
+        end
+        
+    else
+        error('Unrecognized coefficient')
+    end
+end
+
+
+    function[label]=detectDependency1(func)
+        
+        string=func2str(func);
+        
+        if ~strcmp(string(1:4), '@(x)')
+            error('Coefficient can only be constant or space dependent')
+        end
+        
+        if contains(string(5:end), 'x')
+            label='space';
+        elseif contains(string(5), '0')
+            label='zero';
+        else
+            label='const';
+        end
+    end
+
+    function[label]=detectDependency2(func)
+        
+        string=func2str(func);
+        
+        if ~strcmp(string(1:4), '@(x)')
+            error('Function handle must contain space variables')
+        end
+        string=string(5:end);
+        
+        if contains(string, 'x') && contains(string, 't')
+            label='space_time';
+        elseif contains(string, 'x')
+            label='space';
+        elseif contains(string, 't')
+            label='time';
+        elseif contains(string(1), '0')
+            label='zero';
+        else
+            label='const';
+        end
+        
+    end
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_small/setModel.m b/Code/INTERNODES_small/setModel.m
new file mode 100644
index 0000000..58c27a3
--- /dev/null
+++ b/Code/INTERNODES_small/setModel.m
@@ -0,0 +1,75 @@
+function[Data]=setModel(varargin)
+
+% setModel: Initializes the numerical model
+% OPTIONAL INPUT:
+% model:        Specifies which model should be used
+%               Default: transient_heat
+% submodel:     Specifies which submodel from the specified model should be
+%               used
+%               Default: standard
+% type:         Specifies whether the model is linear or nonlinear
+%               Default: linear
+% OUTPUT:
+% Data:         Data structure with initialized model
+
+if mod(length(varargin), 2)~=0
+    error('Missing a name or value')
+end
+labels_in=varargin(1:2:end);
+values_in=varargin(2:2:end);
+
+models={'transient_heat', 'static_solid', 'dynamic_solid'};
+n=length(models);
+submodels=cell(n,1);
+submodels{1}={'standard'};
+submodels{2}={'standard', 'contact'};
+submodels{3}={'standard'};
+types={'linear', 'nonlinear'};
+
+% Set default Parameters
+Data.Model.name=models{1};
+Data.Model.submodel=submodels{1};
+Data.Model.type=types{1};
+
+% Define constants
+% Stefan Boltzman constant W/(m^2*K^4)
+Data.Constants.sigma=5.670373e-8;
+
+for i=1:length(labels_in)
+    arg=lower(labels_in{i});
+    val=lower(values_in{i});
+    
+    switch arg
+        case 'model'
+            if any(ismember(models, val))
+                Data.Model.name=val;
+            else
+                error('Unrecognized model')
+            end
+            
+        case 'submodel'
+            
+            if isfield(Data.Model, 'name')
+                index=strcmp(Data.Model.name, models);
+            else
+                error('Submodel must be defined after the model')
+            end
+            if any(ismember(submodels{index}, val))
+                Data.Model.submodel=val;
+            else
+                error('Unrecognized submodel')
+            end
+            
+        case 'type'
+            if any(ismember(types, val))
+                Data.Model.type=val;
+            else
+                error('Unrecognized type')
+            end     
+            
+        otherwise
+            error('Unrecognized argument')
+    end
+    
+end
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_small/setNumericalParameters.m b/Code/INTERNODES_small/setNumericalParameters.m
new file mode 100644
index 0000000..9e15a42
--- /dev/null
+++ b/Code/INTERNODES_small/setNumericalParameters.m
@@ -0,0 +1,223 @@
+function[Data]=setNumericalParameters(Mesh, Data, varargin)
+
+% setNumericalParameters: Initializes the numerical parameters of the
+% solver
+% OPTIONAL INPUT:
+% Contact:
+%   iterate:            Parameter controlling whether iterations should 
+%                       take place (1) or not (0)
+%                       Default: 1
+%   maxiter:            Maximum number of iterations for solving the
+%                       contact problem
+%                       Default: 10
+%   rescaling:          Rescaling parameter for the stiffness matrix
+%                       Default: 1
+%   c:                  Number of nearest neighbors for computing the
+%                       radius of support of the radial basis functions
+%                       Default: 1
+%   radius:             Intersection radius used to define the initial
+%                       potential contact interface. 
+%                       Default: infinity
+%   gamma:              Value of gamma for preventing quick loss of diagonal
+%                       dominance of the matrices PhiMM
+%                       Default: 0.5
+%   Gamma:              Value of Gamma to avoid very small nonzero entries
+%                       in the matrices PhiNM
+%                       Default: 0.95
+%   d0:                 Measure of tolerance for contact detection.
+%                       Default: 0.05
+%   tol:                Tolerance for gap detection
+%                       Default: 1e-1
+% Solver:
+%   name:               Name of the solver to be used.
+%                       Default: direct
+% PostProcessing:
+%   quantity:           Additional quantities to be computed (for example
+%                       stresses, strains, fluxes,...)
+%                       Default: none
+%   ampl_fact:          Value of amplification factor to apply to
+%                       displacements
+%                       Default: auto
+%   eval_points:        Number of evaluation points used for computing
+%                       stresses or strains
+%                       Default: 3 (P1 finite elements)
+%                                6 (P2 or higher order finite elements)                
+
+if nargin>2
+if mod(length(varargin)-1, 2)~=0
+    error('Missing a name or value')
+end
+
+id=lower(varargin{1});
+labels_in=varargin(2:2:end);
+values_in=varargin(3:2:end);
+end
+
+% Set default Parameters
+if ~isfield(Data, 'Contact')
+    % Boolean indicator for contact algorithm iterations
+    Data.Contact.iterate=true;
+    % Maximum number of iterations of the contact algorithm
+    Data.Contact.maxiter=10;
+    % Rescaling parameter for the INTERNODES matrix
+    Data.Contact.rescaling=1;
+    % Number of nearest neighbors
+    Data.Contact.c=1;
+    % Intersection radius
+    Data.Contact.radius=inf;
+    % gamma value
+    Data.Contact.gamma=0.5;
+    % Gamma value
+    Data.Contact.Gamma=0.95;
+    % Contact tolerance
+    Data.Contact.d0=0.05;
+    % Gap tolerance
+    Data.Contact.tol=1e-1;
+end
+if ~isfield(Data, 'Solver')  
+    % Solver name
+    Data.Solver.name='direct';
+end
+if ~isfield(Data, 'PostProcessing')   
+    % Additional quantities to be computed
+    Data.PostProcessing.quantity='none';
+    % Amplification factor
+    Data.PostProcessing.amplification='auto';
+    % Number of evaluation points per element for computing stresses/strains
+    eval_points={3,6};
+    
+    if Mesh.order==1
+        Data.PostProcessing.eval_points=eval_points{1};
+    else
+        Data.PostProcessing.eval_points=eval_points{2};
+    end
+    
+end
+
+% Override the defaults
+if exist('id', 'var')
+    switch id
+        case 'contact'
+            %% Contact algorithm specific parameters
+            
+            for i=1:length(labels_in)
+                arg=lower(labels_in{i});
+                val=lower(values_in{i});
+                
+                switch arg
+                    case 'iterate'
+                        if isa(val, 'double')
+                            
+                            if val<0 || val>1
+                                error([arg ' parameter must be a boolean'])
+                            else
+                                Data.Contact.(arg)=logical(val);
+                            end
+                            
+                        elseif isa(val, 'logical')
+                            Data.Contact.(arg)=val;
+                        else
+                            error([arg ' parameter must be a boolean'])
+                        end
+                        
+                    case {'maxiter', 'c', 'radius', 'd0'}
+                        if val<=0
+                            error(['Parameter ' arg ' must be strictly greater than zero'])
+                        else
+                            Data.Contact.(arg)=val;
+                        end
+                        
+                    case 'rescaling'
+                        if val==0
+                            error(['Parameter ' arg ' must be different from zero'])
+                        else
+                            Data.Contact.(arg)=val;
+                        end
+                        
+                    case {'gamma', 'Gamma', 'tol'}
+                        if val<=0 || val>=1
+                            error([arg ' must be strictly greater than 0 and strictly smaller than 1'])
+                        else
+                            Data.Contact.(arg)=val;
+                        end
+                        
+                    otherwise
+                        error('Unrecognized argument')
+                end
+            end
+                
+        case 'solver'
+            %% Solver parameters
+            names={'direct'};
+            
+            for i=1:length(labels_in)
+                arg=lower(labels_in{i});
+                val=values_in{i};
+                
+                switch arg
+                    case 'name'
+                        if any(ismember(names, val))
+                            Data.Solver.(arg)=val;
+                        else
+                            error('Unrecognized solver name')
+                        end
+                        
+                    otherwise
+                        error('Unrecognized argument')
+                end
+                
+            end
+            
+        case 'postprocessing'
+            %% Post-processing parameters
+            quantities={'none', 'flux', 'error', 'stress', 'strain'};
+            eval_points={3,6};
+            
+            for i=1:length(labels_in)
+                arg=lower(labels_in{i});
+                val=values_in{i};
+                
+                switch arg
+                    case 'quantity'
+                        if any(ismember(quantities, val))
+                            Data.PostProcessing.quantity=val;
+                        else
+                            error('Unrecognized quantity')
+                        end
+
+                    case 'eval_points'
+                        if any(ismember(eval_points, val))
+                            Data.PostProcessing.eval_points=val;
+                        else
+                            error('Invalid number of evaluation points')
+                        end
+                        
+                    case 'ampl_fact'
+                        
+                        if isa(val, 'char')
+                            if ~strcmp(val, 'auto')
+                                error('Invalid amplification factor')
+                            else
+                                Data.PostProcessing.amplification=val;
+                            end
+                            
+                        elseif isa(val, 'double')
+                            if val <= 0
+                                error('Invalid amplification factor')
+                            else
+                                Data.PostProcessing.amplification=val;
+                            end
+                        else
+                            error('Invalid amplification factor')
+                        end
+                        
+                    otherwise
+                        error('Unrecognized argument')
+                end
+            end
+            
+        otherwise
+            error('Unrecognized argument')
+    end
+end
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_small/solve.m b/Code/INTERNODES_small/solve.m
new file mode 100644
index 0000000..12638e4
--- /dev/null
+++ b/Code/INTERNODES_small/solve.m
@@ -0,0 +1,93 @@
+function[Mesh, Data, Solution, A, b]=solve(Mesh, Data, it)
+
+% solve: Solves the contact problem
+% INPUT:
+% Mesh:     Structure containing all the mesh parameters
+% Data:     Structure containing all the data parameters
+% it:       Cell array containing the boundary tags of the interfaces (master
+%           interface and slave interface)
+% OUTPUT:
+% Mesh:     Updated structure containing all the mesh parameters
+% Data:     Updated structure containing all the data parameters
+% Solution: Structure containing the solution
+% A:        Global INTERNODES matrix
+% b:        Global right-hand side
+
+
+% Initialization of the system
+[Data, Solution, A, b, K]=initializeSystem(Mesh, Data, it);
+% Initialize count
+n=1;
+
+while Data.Contact.iterate && n<= Data.Contact.maxiter
+    
+    % Solve the system of equations
+    [Solution]=solveSystem(Mesh, Data, Solution, A, b);
+    % Update the system
+    [Data, A, b]=updateSystem(Mesh, Data, Solution);
+    n=n+1;
+end
+
+if n > Data.Contact.maxiter && Data.Contact.iterate
+    warning('Failed to converge within the specified number of iterations')
+end
+
+    function[Solution]=solveSystem(Mesh, Data, Solution, A, b)
+        
+        % solveSystem: Solves the linear system of equations
+        % INPUT:
+        % Mesh:     Structure containing all the mesh parameters
+        % Data:     Structure containing all the data parameters
+        % Solution: Initialized solution structure with Dirichlet BC
+        % A:        INTERNODES matrix
+        % b:        Right-hand side vector
+        % OUTPUT:
+        % Solution: Updated solution structure with computed displacements
+        %           and Lagrange multipliers
+        
+        
+        % Retrieve data parameters
+        % Free degrees of freedom
+        Nf=Data.Nf;
+        % Number of free degrees of freedom
+        nf=length(Nf);
+        % Number of constraints
+        nc=length(Data.body{1}.interface_dof);
+        
+        % Retrieve mesh parameters
+        d=Mesh.nb_dof_local;
+        % Retrieve solution
+        U=Solution.U;
+        % Direct method
+        x=A\b;
+        % Retrieve the displacements
+        U(Nf)=x(1:nf);
+        
+        % Update solution
+        Solution.U=U;
+        Solution.U_sort=reshape(U, d, [])';
+        Solution.lambda=Data.Contact.rescaling*x(nf+1:nf+nc);
+        Solution.lambda_sort=reshape(Solution.lambda, d, [])';
+    end
+
+    function[Data, A, b]=updateSystem(Mesh, Data, Solution)
+        
+        % updateSystem: Updates all quantities which have changed
+        % INPUT:
+        % Mesh:     Structure containing all the mesh parameters
+        % Data:     Structure containing all the data parameters
+        % Solution: Solution structure with computed displacements
+        % OUPUT:
+        % Data:     Data structure with updated interface properties
+        % A:        New INTERNODES matrix
+        % b:        New right-hand side
+        
+        
+        % Check for convergence
+        [Data]=removeTraction(Mesh, Data, Solution, n);
+        % Reassemble the blocks which have changed
+        [Data]=subAssemble(Mesh, Data, Solution, 'updating');
+        % Reassemble the entire internodes matrix
+        [A,b]=assembleInternodesMat(Mesh, Data, Solution, K);
+    end
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_small/subAssemble.m b/Code/INTERNODES_small/subAssemble.m
new file mode 100644
index 0000000..26e78d1
--- /dev/null
+++ b/Code/INTERNODES_small/subAssemble.m
@@ -0,0 +1,202 @@
+function[Data]=subAssemble(Mesh, Data, Solution, operation)
+
+% subAssemble: Assembles the part of the internodes matrix which is
+% changing from one iteration to the next
+% INPUT:
+% Mesh:         Structure containing all the mesh parameters
+% Data:         Structure containing all the data parameters
+% Solution:     Structure containing the solution
+% operation:    Specifies the type of operation (initialization or
+%               updating)
+% OUTPUT:
+% Data:         Updated structure containing all the data parameters
+
+if strcmp(operation, 'initialization')
+    [Data]=computeBoundaryMass(Mesh, Data, 'initialization');
+end
+
+% Assemble interpolation matrices
+[Data]=assembleRs(Mesh, Data, Solution);
+% Assemble boundary mass matrices
+[Data]=computeBoundaryMass(Mesh, Data, 'updating');
+
+    function[Data]=assembleRs(Mesh, Data, Solution)
+        
+        % assembleRs: Constructs the interpolation matrices
+        % INPUT:
+        % Mesh:     Structure containing all the mesh parameters
+        % Data:     Structure containing all the data parameters
+        % Solution: Structure containing the solution
+        % OUTPUT:
+        % Data:     Updated data structure with interpolation matrices
+        
+        % Retrieve mesh parameters
+        nb_dof_local=Mesh.nb_dof_local;
+        
+        % Retrieve nodes making up the interfaces
+        nodes1=Data.body{1}.interface_nodes;
+        nodes2=Data.body{2}.interface_nodes;
+        
+        % 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 distant bodies
+        if all(i1) || all(i2)
+            warning('One of the bodies will be translated because they are too far away from each other. Boundary conditions might be affected')
+            [Mesh, coord1, coord2]=translateBodies(Mesh, Data, radiuses1, radiuses2, nodes1, nodes2, coord1, coord2);
+            
+            % 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
+        
+        % Check for isolated nodes
+        while any(i1) || any(i2)
+            
+            dump_nodes1=nodes1(i1);
+            dump_nodes2=nodes2(i2);
+            
+            % Updating
+            [Data]=updateInterface(Mesh, Data, 1, dump_nodes1, 'dump');
+            [Data]=updateInterface(Mesh, Data, 2, dump_nodes2, 'dump');
+            
+            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;
+        
+        Data.R12_normal=R12;
+        Data.R21_normal=R21;
+        
+        % Expansion to full size
+        Data.R12=kron(R12, speye(nb_dof_local));
+        Data.R21=kron(R21, speye(nb_dof_local));
+        
+    end
+
+    function[Data]=computeBoundaryMass(Mesh, Data, operation)
+        
+        % computeBoundaryMass: Assembles the boundary mass matrices
+        % INPUT:
+        % Mesh:         Structure containing all the mesh parameters
+        % Data:         Structure containing all the data parameters
+        % operation:    Specifies the type of operation (initialization or
+        %               updating)
+        % OUTPUT:
+        % Data:         Updated data structure with boundary mass matrices
+        
+        
+        % Retrieve mesh parameters
+        % Number of local degrees of freedom
+        nb_dof_local=Mesh.nb_dof_local;
+        % Coordinate matrix
+        coord=Mesh.coord_mat;
+        % Order
+        order=Mesh.order;
+        % Get quadrature nodes and weights
+        [points, weights]=getQuadrature(3, 'boundary');
+        % Number of quadrature points
+        nb_points=length(weights);
+        
+        % Retrive functions
+        [Functions]=getFunctions(order);
+        % Retrieve boundary basis functions
+        phi_boundary=Functions.phi_boundary;
+        
+        % Retrieve bodies
+        body=Data.body;
+        % Number of bodies
+        nb_bodies=length(body);
+        
+        if strcmp(operation, 'initialization')
+            
+            for n=1:nb_bodies
+                % Retrieve data
+                % Retriveve edges making up the interface
+                Edges=body{n}.interface_connect;
+                % Retrieve nodes making up the interface
+                nodes=body{n}.interface_nodes;
+                nb_nodes=length(nodes);
+                s=order+1;
+                
+                % Initialization
+                Phi=zeros(s, nb_points);
+                
+                % Map global nodes to local nodes
+                func=@(x) find(nodes==x);
+                connect=arrayfun(func, Edges);
+                T=connect';
+                G=T(:);
+                
+                I=repmat(connect, [1 s])';
+                J=repmat(G', [s 1]);
+                
+                % Loop over the Gauss points
+                for i = 1:nb_points
+                    Phi(:,i)=phi_boundary(points(i,:));
+                end
+                
+                Tr=Edges';
+                Gr=Tr(:);
+                % Coordinates of the nodes of the elements
+                coord_nodes=coord(Gr,:);
+                % Endpoints of the edges
+                a=coord_nodes(1:s:end,:);
+                b=coord_nodes(2:s:end,:);
+                bk=b-a;
+                norm_bk=vecnorm(bk,2,2);
+                
+                lambda=weights.*norm_bk';
+                
+                M=kr(Phi, Phi)*lambda;
+                M=sparse(I(:), J(:), M(:), nb_nodes, nb_nodes);
+                
+                Data.body{n}.iinterface_mass=M;
+                % Expansion to full size
+                M=kron(M, speye(nb_dof_local));
+                Data.body{n}.interface_mass=M;
+            end
+        else
+            for n=1:nb_bodies
+                active_set=Data.body{n}.active_set;                
+                Data.body{n}.interface_mass=kron(Data.body{n}.iinterface_mass(active_set, active_set), speye(nb_dof_local));
+            end
+        end
+    end
+
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_small/translateBodies.m b/Code/INTERNODES_small/translateBodies.m
new file mode 100644
index 0000000..f5a398f
--- /dev/null
+++ b/Code/INTERNODES_small/translateBodies.m
@@ -0,0 +1,60 @@
+function[Mesh, coord1, coord2]=translateBodies(Mesh, Data, radiuses1, radiuses2, nodes1, nodes2, coord1, coord2)
+
+% translateBodies: Performs a translation of one of the bodies in case the
+% initial potential contact interface is empty (because the bodies are too 
+% far away from each other)
+% INPUT:
+% Mesh:         Structure containing all the mesh parameters
+% Data:         Structure containing all the data parameters
+% radiusesk:    Radiuses of support of the radial basis functions (k=1,2)
+% nodesk:       Nodes on the potential contact interface of body k (k=1,2)
+% coordk:       Coordinates of the nodes on the potential contact interface 
+%               of body k (k=1,2)
+% OUTPUT:
+% Mesh:         Mesh structure with updated coordinate matrix
+% coordk:       New coordinates of the nodes on the potential contact 
+%               interface of body k (k=1,2)
+
+n1=length(radiuses1);
+
+dist=inf;
+node1=0;
+node2=0;
+
+for k=1:n1
+    
+    [d, i]=min(vecnorm(coord1(k,:)-coord2,2,2));
+    
+    if d<dist
+        dist=d;
+        node1=nodes1(k);
+        node2=nodes2(i);
+    end
+end
+
+% Check for the most restrictive
+i1=node1==nodes1;
+i2=node2==nodes2;
+
+r1=radiuses1(i1);
+r2=radiuses2(i2);
+
+shift=max([dist-r1, dist-r2]);
+
+% Add safety coefficient
+shift=(1+5e1*Data.Contact.h)*shift;
+
+r=shift/dist*(coord2(i2,:)-coord1(i1,:));
+
+% Change the coordinates of all nodes of body 1
+% !! Assumes body 1 has the smallest material tag !!
+coord=Mesh.coord_mat;
+mt=Mesh.material_tag;
+tags=unique(mt);
+N1=unique(Mesh.connect_mat(mt==tags(1),:));
+coord(N1,:)=coord(N1,:)+r;
+Mesh.coord_mat=coord;
+
+coord1=coord(nodes1,:);
+coord2=coord(nodes2,:);
+end
\ No newline at end of file
diff --git a/Code/INTERNODES_small/updateInterface.m b/Code/INTERNODES_small/updateInterface.m
new file mode 100644
index 0000000..eb45955
--- /dev/null
+++ b/Code/INTERNODES_small/updateInterface.m
@@ -0,0 +1,45 @@
+function[Data, nb_nodes]=updateInterface(Mesh, Data, body_tag, nodes, operation)
+
+% updateInterface: Updates the interface properties
+% INPUT:
+% Mesh:         Structure containing all the mesh parameters
+% Data:         Structure containing all the data parameters
+% body_tag:     Tag of the body to be updated
+% nodes:        Nodes to be added or removed from the potential contact interface
+% operation:    Specifies whether nodes should be added (add) or removed (dump)            
+% OUTPUT:
+% Data:         Updated data structure with interface properties
+% nb_nodes:     Number of nodes added or removed
+
+% Retrieve initial interface connectivity matrix
+iIc=Data.body{body_tag}.initial_interface_connect;
+% Retrieve initial set of nodes
+iIn=Data.body{body_tag}.initial_nodes;
+% Retrieve current set of nodes
+In=Data.body{body_tag}.interface_nodes;
+% Update interface connectivity matrix
+switch operation
+    case 'dump'
+        new_nodes=setdiff(In, nodes);
+        indx=all(ismember(iIc,new_nodes),2);
+        new_interface_connect=iIc(indx,:);
+    case 'add'
+        new_nodes=union(In, nodes);
+        indx=all(ismember(iIc,new_nodes),2);
+        new_interface_connect=iIc(indx,:);
+end
+% Update active set of nodes
+active_set=ismember(iIn, new_nodes);
+% Update interface degrees of freedom
+new_interface_dof=Mesh.Dof_set(:,new_nodes);
+new_interface_dof=new_interface_dof(:);
+% Update body properties
+Data.body{body_tag}.interface_nodes=new_nodes(:);
+Data.body{body_tag}.active_set=active_set;
+Data.body{body_tag}.interface_dof=new_interface_dof;
+Data.body{body_tag}.interface_connect=new_interface_connect;
+% Update mapping of degrees of freedom
+[Data]=mapGlobal2Local(Data);
+% Change in the number of nodes
+nb_nodes=length(new_nodes)-length(In);
+end
\ No newline at end of file
diff --git a/Code/Westergaard/assembleInternodesMat.m b/Code/Westergaard/assembleInternodesMat.m
new file mode 100644
index 0000000..bacf225
--- /dev/null
+++ b/Code/Westergaard/assembleInternodesMat.m
@@ -0,0 +1,68 @@
+function[A, b]=assembleInternodesMat(Mesh, Data, Solution, K)
+
+% assembleInternodesMat: Assembles the entire INTERNODES matrix
+% INPUT:
+% Mesh:     Structure containing all the mesh parameters
+% Data:     Structure containing all the data parameters
+% Solution: Structure containing the solution
+% K:        Global stiffness matrix
+% OUTPUT:
+% A:        Global INTERNODES matrix
+% b:        Global right-hand side
+
+% Retrieve mesh parameters
+% Coordinate matrix
+coord=Mesh.coord_mat;
+
+% Retrieve data parameters
+% Free degrees of freedom
+Nf=Data.Nf;
+% Blocked degrees of freedom
+Nd=Data.Nd;
+% Retrieve bodies
+body=Data.body;
+% Right-hand side force vector
+f=Data.F;
+% Rescaling parameter
+r=Data.Contact.rescaling;
+
+% Retrive solution
+U=Solution.U;
+
+% Number of free degrees of freedom
+nf=length(Nf);
+% Number of constraints
+nc=length(body{1}.interface_dof);
+% Retrieve local degrees of freedom
+indx1=body{1}.indx_local;
+indx2=body{2}.indx_local;
+% Retrieve nodes making up the interfaces
+nodes1=Data.body{1}.interface_nodes;
+nodes2=Data.body{2}.interface_nodes;
+% Retrieve coordinates of nodes making up the interfaces
+coord1=coord(nodes1,:);
+coord2=coord(nodes2,:);
+
+% Initialization
+b=zeros(nf+nc,1);
+B=sparse(nf,nc);
+B_tilde=sparse(nc,nf);
+C=sparse(nc,nc);
+
+Kff=1/r*K(Nf, Nf);
+Diff=Data.R12_normal*coord2-coord1;
+
+% Assemblage
+% Right-hand side vector
+b(1:nf)=1/r*(f(Nf)-K(Nf,Nd)*U(Nd));
+b(nf+1:nf+nc)=reshape(Diff', [],1);
+% B and B_tilde
+B(indx1,:)=-body{1}.interface_mass;
+B(indx2,:)=body{2}.interface_mass*Data.R21;
+B_tilde(:,indx1)=speye(nc,nc);
+B_tilde(:,indx2)=-Data.R12;
+% INTERNODES matrix
+A1=horzcat(Kff, B);
+A2=horzcat(B_tilde, C);
+A=vertcat(A1, A2);
+end
\ No newline at end of file
diff --git a/Code/Westergaard/assembleInterpMat.m b/Code/Westergaard/assembleInterpMat.m
new file mode 100644
index 0000000..959d585
--- /dev/null
+++ b/Code/Westergaard/assembleInterpMat.m
@@ -0,0 +1,34 @@
+function [Phi] = assembleInterpMat(coord_ref, coord_interp, radiuses)
+
+% assembleInterpMat: Assemblage of the interpolation matrices for the
+% localized radial basis functions in dimension d
+% INPUT:
+% coord_ref:    Coordinate matrix of the reference points (size m x d)
+% coord_interp: Coordinate matrix for the interpolation points (size n x d)
+% radiuses:     Vector of radiuses of the radial basis functions. It is a row
+%               vector (size 1 x m)
+% OUTPUT:
+% Phi:          Interpolation matrix
+
+l=size(radiuses,1);
+if l>1
+    radiuses=radiuses';
+end
+
+m=size(coord_ref,1);
+n=size(coord_interp,1);
+dim=size(coord_ref,2);
+
+X_ref=reshape(coord_ref,1,m,dim);
+X_interp=reshape(coord_interp,n,1,dim);
+
+X=X_interp-X_ref;
+% 2-norm (euclidean norm) along the third dimension
+N=vecnorm(X,2,3);
+indx=N<=radiuses;
+Phi=zeros(n,m);
+
+% Wendland C^2 radial basis functions
+F=(1-N./radiuses).^4.*(1+4*N./radiuses);
+Phi(indx)=F(indx);
+end
\ No newline at end of file
diff --git a/Code/Westergaard/computeNormals.m b/Code/Westergaard/computeNormals.m
new file mode 100644
index 0000000..9a7a340
--- /dev/null
+++ b/Code/Westergaard/computeNormals.m
@@ -0,0 +1,83 @@
+function[Data]=computeNormals(Mesh, Data, Solution)
+
+% computeNormals: Computes the normal vectors for each interface
+% INPUT:
+% Mesh:     Structure containing all the mesh parameters
+% Data:     Structure containing all the data parameters
+% Solution: Structure containing the solution
+% OUTPUT:
+% Data:     Updated structure with the interface normals
+
+% Retrieve mesh data
+% Connectivity matrix
+connect=Mesh.connect_mat;
+
+% Local number of degrees of freedom (dimension in solid mechanics)
+d=Mesh.nb_dof_local;
+% Retrieve bodies
+body=Data.body;
+% Compute the normals based on the deformed structure
+def=Mesh.coord_mat+Solution.U_sort;
+
+nb_bodies=length(body);
+
+
+for k=1:nb_bodies
+    % Normal vectors are computed for all nodes in the initial potential 
+    % contact interface
+    connecti=body{k}.initial_interface_connect;
+    nodesi=body{k}.initial_nodes;
+    
+    % Reduced connectivity matrix
+    connect_red=connect(any(ismember(connect, nodesi), 2), :);
+    % Reduced set of body nodes
+    nodesb=setdiff(unique(connect_red), nodesi);  
+    n=length(nodesi);
+    m=size(connecti,1);
+    % Coordinates of the endpoints of the segments
+    coord1=def(connecti(:,1),:);
+    coord2=def(connecti(:,2),:);
+    % Tangent vectors
+    V=coord2-coord1;
+    if Data.Contact.average
+        % Segment lengths
+        V=V./vecnorm(V,2,2);
+        L=vecnorm(V,2,2);
+    else
+        % Segment lengths
+        L=vecnorm(V,2,2);
+        V=V./vecnorm(V,2,2);
+    end
+
+    % !! Only for a two-dimensional case !!
+    N=zeros(m, d);
+    N(:,1)=-V(:,2);
+    N(:,2)=V(:,1);
+    
+    % Initialization
+    normals=zeros(n,d);
+    % Step size
+    gamma=1e-3;
+    
+    for j=1:n
+        [id,~]=find(connecti==nodesi(j));
+        l=L(id);
+        % Weighted average of normal vectors
+        normals(j,:)=1/sum(l)*sum(N(id,:).*l,1);
+        % Current node
+        x=def(nodesi(j),:);
+        % Step in the direction of the gradient
+        y_plus=x+gamma*normals(j,:);
+        % Step in the opposite direction of the gradient
+        y_minus=x-gamma*normals(j,:);
+        min_plus=min(vecnorm(def(nodesb,:)-y_plus,2,2));
+        min_minus=min(vecnorm(def(nodesb,:)-y_minus,2,2));
+        if min_plus < min_minus
+            normals(j,:)=-normals(j,:);
+        end
+    end
+    
+    normals=normals./vecnorm(normals,2,2);
+    Data.body{k}.normal=normals;
+end
+end
\ No newline at end of file
diff --git a/Code/Westergaard/computeRHS.m b/Code/Westergaard/computeRHS.m
new file mode 100644
index 0000000..b6f4112
--- /dev/null
+++ b/Code/Westergaard/computeRHS.m
@@ -0,0 +1,223 @@
+function[Data]=computeRHS(Mesh, Data)
+
+% computeRHS: Function which assembles the right-hand side consisting of
+% body forces and surface tractions
+% INPUT:
+% Mesh:             Structure containing the mesh parameters
+% Data:             Structure containing the data parameters
+% OUTPUT:
+% Data:             Updated data structure
+
+% Assemble the global vector of body forces
+[Bs]=computeBody(Mesh, Data);
+% Assemble the global vector of surface tractions
+[Bn]=computeNeumann(Mesh, Data);
+
+% Sum the contribution from body forces and Neumann boundary
+% conditions
+B=Bs+Bn;
+% Update data
+Data.F=B;
+
+    function[B]=computeBody(Mesh, Data)
+        
+        % computeBody: Assembles the component for the body forces
+        % INPUT:
+        % Mesh:     Structure containing all the mesh parameters
+        % Data:     Structure containing all the data parameters
+        % OUTPUT:
+        % B:        Global vector of body forces
+        
+        % Retrieve mesh data
+        % Local number of degrees of freedom (dimension in solid mechanics)
+        d=Mesh.nb_dof_local;
+        % Total number of degrees of freedom
+        nb_dof=Mesh.nb_dof;
+        % Coordinate matrix
+        coord=Mesh.coord_mat;
+        % Connectivity matrix
+        connect=Mesh.connect_mat;
+        % Finite element order
+        order=Mesh.order;
+        % Number of elements
+        ne=Mesh.nb_elem;
+        % Number of nodes per element
+        nne=Mesh.nb_nodes_elem;
+        % Matrix of equation numbers linking an element to the degrees of freedom
+        % associated to it.
+        Eq=Mesh.Eq;
+        
+        % Retrieve data parameters
+        % Body force vector
+        f=Data.Coefficient.f.function;
+        % Retrieve the basis functions
+        [Functions]=getFunctions(order);
+        % Retrieve basis functions
+        phi=Functions.phi;
+        
+        
+        % Retrieve quadrature points and weights
+        switch order
+            case 1
+                [points, weights] = getQuadrature(3, 'bulk');
+            case 2
+                [points, weights] = getQuadrature(4, 'bulk');
+            case 3
+                [points, weights] = getQuadrature(6, 'bulk');
+            otherwise
+                error('Not yet implemented');
+        end
+        
+        % Number of quadrature points
+        nq=length(weights);
+        s=nne*d;
+        Id=eye(d);
+        % Initialization
+        X=zeros(ne, nq, 2);
+        Q=zeros(s, nq*d);
+        
+        
+        % Loop over the Gauss points
+        for i = 1:nq
+            Phi=phi(points(i,:));
+            Q(:, (i-1)*d+1:i*d)=kron(Phi, Id);
+        end
+        
+        I=Eq';
+        T=connect';
+        G=T(:);
+        
+        % Coordinates of the nodes of the elements
+        coord_nodes=coord(G,:);
+        % Vertices of the elements
+        a=coord_nodes(1:nne:end,:);
+        b=coord_nodes(2:nne:end,:);
+        c=coord_nodes(3:nne:end,:);
+        % Interpolated coordinates
+        P=a(:)+[b(:)-a(:) c(:)-a(:)]*points';
+        X(:,:,1)=P(1:ne,:);
+        X(:,:,2)=P(ne+1:end,:);
+        
+        % Determinants of Jacobian matrices
+        detJK=(b(:,1)-a(:,1)).*(c(:,2)-a(:,2))-(b(:,2)-a(:,2)).*(c(:,1)-a(:,1));
+        
+        % Evaluation at the Gauss points
+        F=f(X);
+        if size(F,1)>d
+            F=reshape(F, ne, d*nq);
+            F=reshape(F', d, nq, ne);
+        end
+        
+        X=reshape(abs(detJK), 1, 1, ne).*F.*weights;
+        X=reshape(X, d*nq, ne);
+        
+        B=Q*X;
+        
+        % Assemblage
+        B=accumarray(I(:), B(:), [nb_dof 1]);
+    end
+
+    function[B]=computeNeumann(Mesh, Data)
+        
+        % computeNeumann: Implements Neumann boundary conditions, returns a vector
+        % having for length the number of degrees of freedom.
+        % This vector is to be added to the RHS.
+        % INPUT:
+        % Mesh:     Structure containing all the mesh parameters
+        % Data:     Structure containing all the data parameters
+        % OUTPUT:
+        % B:        Global vector of surface tractions
+        
+        % Retrieve mesh parameters
+        % Total number of degrees of freedom
+        nb_dof=Mesh.nb_dof;
+        % Number of local degrees of freedom
+        d=Mesh.nb_dof_local;
+        % Coordinate matrix
+        coord=Mesh.coord_mat;
+        % Order
+        order=Mesh.order;
+        % Boundary equation numbers
+        Eq_BC=Mesh.Eq_BC;
+        % Retrive functions
+        [Functions]=getFunctions(order);
+        % Retrieve boundary basis functions
+        phi_boundary=Functions.phi_boundary;
+        % Get quadrature
+        [points, weights]=getQuadrature(3, 'boundary');
+        % Number of quadrature points
+        nq=length(weights);
+        
+        % Initialization
+        B=zeros(nb_dof,1);
+        s=order+1;
+        Id=eye(d);
+        % Initialization
+        Q=zeros(s*d, nq*d);
+        
+        % Loop over the Gauss points
+        for i = 1:nq
+            Phi=phi_boundary(points(i,:));
+            Q(:, (i-1)*d+1:i*d)=kron(Phi, Id);
+        end
+        
+        
+        % Neumann boundary conditions
+        N_BC=Data.N_BC;
+        % Number of boundaries where surface tractions are imposed
+        nb_bc=length(N_BC);
+        
+        for m=1:nb_bc
+            % Retrieve data
+            data=N_BC{m};
+            edges=data.edges;
+            
+            for flag=edges
+                % Surface traction function
+                f=data.function;
+                % Retriveve edges making up the boundary
+                Neum_edges=Mesh.BC_nodes(Mesh.BC_tag==flag,:);
+                % Equation numbers
+                Eq=Eq_BC(Mesh.BC_tag==flag,:);
+                % Number of Neumann boundary edges
+                ne=size(Neum_edges, 1);
+                
+                % Initialization
+                X=zeros(ne, nq, 2);
+                
+                C=Neum_edges';
+                G=C(:);
+                I=Eq';
+                
+                % Coordinates of the nodes of the elements
+                coord_nodes=coord(G,:);
+                % Endpoints of the edges
+                a=coord_nodes(1:s:end,:);
+                b=coord_nodes(2:s:end,:);
+                bk=b-a;
+                norm_bk=vecnorm(bk,2,2);
+                
+                % Interpolated coordinates
+                P=a(:)+(b(:)-a(:))*points';
+                X(:,:,1)=P(1:ne,:);
+                X(:,:,2)=P(ne+1:end,:);
+                
+                % Evaluation at the Gauss points
+                F=f(X);
+                if size(F,1)>d
+                    F=reshape(F, ne, d*nq);
+                    F=reshape(F', d, nq, ne);
+                end
+                
+                X=reshape(norm_bk, 1, 1, ne).*F.*weights';
+                X=reshape(X, d*nq, ne);
+                
+                Bl=Q*X;
+                
+                % Assemblage
+                B=B+accumarray(I(:), Bl(:), [nb_dof 1]);
+                
+            end
+        end
+    end
+end
\ No newline at end of file
diff --git a/Code/Westergaard/computeStiffness.m b/Code/Westergaard/computeStiffness.m
new file mode 100644
index 0000000..6091151
--- /dev/null
+++ b/Code/Westergaard/computeStiffness.m
@@ -0,0 +1,138 @@
+function[K]=computeStiffness(Mesh, Data)
+
+% computeStiffness: Function which assembles the global stiffness matrix
+% Assumes all elements are of the same type
+% INPUT:
+% Mesh:     Structure containing all the mesh parameters
+% Data:     Structure containing all the data parameters
+% OUTPUT:
+% K:        Global stiffness matrix
+
+% Mesh parameters
+% Local number of degrees of freedom (dimension in solid mechanics)
+d=Mesh.nb_dof_local;
+% Total number of degrees of freedom
+nb_dof=Mesh.nb_dof;
+% Coordinate matrix
+coord=Mesh.coord_mat;
+% Connectivity matrix
+connect=Mesh.connect_mat;
+% Finite element order
+order=Mesh.order;
+% Number of nodes per element
+nne=Mesh.nb_nodes_elem;
+% Total number of elements
+ne=Mesh.nb_elem;
+% Matrix linking an element to the degrees of freedom associated to it.
+Eq=Mesh.Eq;
+
+% Data parameters
+% Mu
+f1=Data.Coefficient.mu.function;
+% Lambda
+f2=Data.Coefficient.lambda.function;
+
+
+% Retrieve functions
+[Functions]=getFunctions(order);
+% Jacobian matrix
+J_phi=Functions.J_phi;
+% Define permutation matrix
+Sdd=zeros(d*d);
+Id=eye(d);
+for k=1:d
+    Sdd=Sdd+kron(Id(:,k)', kron(Id, Id(:,k)));
+end
+
+% Retrieve quadrature nodes and weights
+switch order
+    case 1
+        [points, weights] = getQuadrature(1, 'bulk');
+    case 2
+        [points, weights] = getQuadrature(2, 'bulk');
+    case 3
+        [points, weights] = getQuadrature(3, 'bulk');
+    otherwise
+        error('Not yet implemented');
+end
+
+% Initialization
+s=nne*d;
+nq=length(weights);
+Q=zeros(s^2, nq*d^4);
+
+% Loop over the Gauss points
+for i = 1:nq
+    L=kron(J_phi(points(i,:)), Id);
+    Q(:, (i-1)*d^4+1:i*d^4)=kron(L, L);
+end
+
+T=Eq';
+G=T(:);
+
+I=repmat(Eq, [1 nne*d])';
+J=repmat(G', [nne*d 1]);
+
+T=connect';
+G=T(:);
+
+% Coordinates of the nodes of the elements
+coord_nodes=coord(G,:);
+% Vertices of the elements
+a=coord_nodes(1:nne:end,:);
+b=coord_nodes(2:nne:end,:);
+c=coord_nodes(3:nne:end,:);
+% Interpolated coordinates
+P=a(:)+[b(:)-a(:) c(:)-a(:)]*points';
+X(:,:,1)=P(1:ne,:);
+X(:,:,2)=P(ne+1:end,:);
+
+% Determinants of Jacobian matrices
+detJK=(b(:,1)-a(:,1)).*(c(:,2)-a(:,2))-(b(:,2)-a(:,2)).*(c(:,1)-a(:,1));
+
+V=[(vecnorm(c-a,2,2).^2) -sum((c-a).*(b-a),2) -sum((c-a).*(b-a),2) (vecnorm(b-a,2,2).^2)];
+W=1./(detJK').^2.*(V');
+
+V=reshape(W,d,d*ne);
+
+JK_inv=zeros(d, d*ne);
+JK_inv(:,1:d:d*ne)=1./detJK'.*[c(:,2)-a(:,2) -(b(:,2)-a(:,2))]';
+JK_inv(:,2:d:d*ne)=1./detJK'.*[-(c(:,1)-a(:,1)) b(:,1)-a(:,1)]';
+
+JK_invT=zeros(d, d*ne);
+JK_invT(:,1:d:d*ne)=1./detJK'.*[c(:,2)-a(:,2) -(c(:,1)-a(:,1))]';
+JK_invT(:,2:d:d*ne)=1./detJK'.*[-(b(:,2)-a(:,2)) b(:,1)-a(:,1)]';
+
+vec_JK_invT=reshape(JK_invT, d^2, ne);
+
+[S1]=product(V , repmat(Id, 1, ne), d, ne);
+[S2]=product(JK_invT , JK_inv, d, ne);
+S2=Sdd*S2;
+
+A1=reshape(S1, d^4, ne);
+A2=reshape(S2, d^4, ne);
+A3=kr(vec_JK_invT, vec_JK_invT);
+
+
+Lambda1=(abs(detJK).*f1(X).*weights)';
+Lambda2=(abs(detJK).*f2(X).*weights)';
+
+X=kr(Lambda1, A1+A2)+kr(Lambda2, A3);
+K=Q*X;
+
+% Assemblage
+K=sparse(I(:), J(:), K(:), nb_dof, nb_dof);
+
+    function[M]=product(A,B,d,ne)
+        
+        % Blockwise Kronecker product of two matrices
+        % !! Only for a two-dimensional case !!
+        M=zeros(d^2, d^2*ne);
+        
+        M(:,1:d^2:end)=kr(A(:,1:d:end), B(:,1:d:end));
+        M(:,2:d^2:end)=kr(A(:,1:d:end), B(:,2:d:end));
+        M(:,3:d^2:end)=kr(A(:,2:d:end), B(:,1:d:end));
+        M(:,4:d^2:end)=kr(A(:,2:d:end), B(:,2:d:end));
+    end
+
+end
\ No newline at end of file
diff --git a/Code/Westergaard/detectGaps.m b/Code/Westergaard/detectGaps.m
new file mode 100644
index 0000000..ffe94c0
--- /dev/null
+++ b/Code/Westergaard/detectGaps.m
@@ -0,0 +1,81 @@
+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
\ No newline at end of file
diff --git a/Code/Westergaard/getFunctions.m b/Code/Westergaard/getFunctions.m
new file mode 100644
index 0000000..ddff9aa
--- /dev/null
+++ b/Code/Westergaard/getFunctions.m
@@ -0,0 +1,46 @@
+function[Functions]=getFunctions(order)
+
+% getFunctions: Returns the basis functions, their jacobian matrices and
+% the basis functions needed to integrate over the boundary
+% INPUT:
+% order:        Finite element order
+% OUTPUT:
+% Functions:    Structure containing all the functions
+
+
+switch order
+    case 1
+Functions.phi=@(x) [1-x(1)-x(2);...
+                    x(1);...
+                    x(2)];
+                
+Functions.J_phi=@(x) [-1 -1; ...
+                       1  0;...
+                       0  1]; 
+                   
+Functions.phi_boundary=@(x) [1-x;...
+                             x];
+    case 2
+Functions.phi=@(x) [(1-x(1)-x(2))*(1-2*x(1)-2*x(2));...
+                    x(1)*(2*x(1)-1);...
+                    x(2)*(2*x(2)-1);...
+                    4*x(1)*(1-x(1)-x(2));...
+                    4*x(1)*x(2);...
+                    4*x(2)*(1-x(1)-x(2))];
+                 
+                                                       
+Functions.J_phi=@(x) [-3+4*x(2)+4*x(1) -3+4*x(1)+4*x(2); ...
+                       -1+4*x(1)        0;...
+                       0                -1+4*x(2);...
+                       4-4*x(2)-8*x(1)  -4*x(1);...
+                       4*x(2)           4*x(1);...
+                       -4*x(2)          4-4*x(1)-8*x(2)];
+
+Functions.phi_boundary=@(x) [(2*x-1)*(x-1);...
+                            x*(2*x-1);...
+                            4*x*(1-x)];
+               
+    otherwise
+        error('Unimplemented!');
+end
+end
\ No newline at end of file
diff --git a/Code/Westergaard/getParameters.m b/Code/Westergaard/getParameters.m
new file mode 100644
index 0000000..4a0812c
--- /dev/null
+++ b/Code/Westergaard/getParameters.m
@@ -0,0 +1,72 @@
+function[Mesh]=getParameters(Mesh)
+
+% getParameters: Retrieves mesh parameters
+% INPUT:
+% Mesh: Structure containing the mesh geometry
+% OUTPUT:
+% Mesh: Updated structure containing all mesh parameters needed to run the code
+
+% Retrieve mesh parameters
+% Coordinate matrix
+coord=Mesh.coord_mat;
+% Connectivity matrix
+connect=Mesh.connect_mat;
+% Boundary nodes
+BC_nodes=Mesh.BC_nodes;
+
+% Geometry [m] 
+x_min=min(coord(:,1));
+x_max=max(coord(:,1));
+y_min=min(coord(:,2));
+y_max=max(coord(:,2));
+% Maximum dimension [m]
+L_max=max([abs(x_max-x_min) abs(y_max-y_min)]);
+% Number of elements
+nb_elem=size(connect,1);
+% Number of nodes per element
+nb_nodes_elem=size(connect,2);
+% Number of nodes
+nb_nodes=size(coord,1);
+% Local number of degrees of freedom
+nb_dof_local=size(coord,2);
+% Total number of degrees of freedom
+nb_dof=nb_dof_local*nb_nodes;
+% Number of boundary nodes
+nb_BC_nodes=size(BC_nodes,1);
+
+% Order of polynomials
+% It is assumed all elements of the mesh are of same type
+order_map=containers.Map([3 6 10], [1 2 3]);
+order=order_map(nb_nodes_elem);
+
+% Initialization
+% Matrix of equation numbers   
+Eq=zeros(nb_elem,nb_dof_local*nb_nodes_elem);
+% Matrix of equation number for boundary nodes;
+Eq_BC=zeros(nb_BC_nodes,nb_dof_local*order);
+% Matrix containing the sets of degrees of freedom for each node
+Dof_set=zeros(nb_dof_local, nb_nodes);
+
+% Iteration over the dimension
+for k=1:nb_dof_local
+    d1=k:nb_dof_local:nb_dof_local*nb_nodes_elem;
+    d2=k:nb_dof_local:nb_dof_local*(order+1);
+    Eq(:,d1)=nb_dof_local*connect-nb_dof_local+k;
+    Eq_BC(:,d2)=nb_dof_local*BC_nodes-nb_dof_local+k;
+    Dof_set(k,:)=nb_dof_local*(1:nb_nodes)-nb_dof_local+k;
+end
+
+% Update mesh structure
+Mesh.frame=[x_min y_min; x_max y_max];
+Mesh.L_max=L_max;
+Mesh.nb_elem=nb_elem;
+Mesh.nb_nodes_elem=nb_nodes_elem;
+Mesh.nb_nodes=nb_nodes;
+Mesh.nb_dof_local=nb_dof_local;
+Mesh.nb_dof=nb_dof;
+Mesh.order=order;
+Mesh.Eq=Eq;
+Mesh.Eq_BC=Eq_BC;
+Mesh.nb_BC_nodes=nb_BC_nodes;
+Mesh.Dof_set=Dof_set;
+end
\ No newline at end of file
diff --git a/Code/Westergaard/getQuadrature.m b/Code/Westergaard/getQuadrature.m
new file mode 100755
index 0000000..706128e
--- /dev/null
+++ b/Code/Westergaard/getQuadrature.m
@@ -0,0 +1,362 @@
+function[points, weights] = getQuadrature(id, object)
+
+% getQuadrature: Function which returns the integration points and
+% weights for a given quadrature ID
+% INPUT:
+% id:       Integration ID
+% OUTPUT:
+% points:   Integration points
+% weights:  Integration weights
+
+switch object
+    case 'bulk'
+        switch id
+            case 1
+                % Source: Wriggers, Computational contact mechanics (2006)
+                % Number of points: 1
+                % Degree of exactness: 1
+                
+                a1=1/3;                 w1=1;
+                
+                points=[a1 a1];
+                
+                weights=0.5*w1;
+                
+            case 2
+                % Source: Wriggers, Computational contact mechanics (2006)
+                % Number of points: 3
+                % Degree of exactness: 2
+                
+                a1=2/3;                 w1=1/3;
+                b1=1/6;
+                
+                points=[b1 b1;
+                        a1 b1;
+                        b1 a1];
+                
+                weights=0.5*[w1 w1 w1];
+                
+            case 3
+                % Source: Wriggers, Computational contact mechanics (2006)
+                % Number of points: 4
+                % Degree of exactness: 3
+                
+                a1=1/3;                 w1=-27/48;
+                a2=3/5;                 w2=25/48;
+                b2=1/5;
+                
+                points=[a1 a1;            
+                        b2 b2;
+                        a2 b2;
+                        b2 a2];
+                
+                weights=0.5*[w1 w2 w2 w2];
+                
+            case 4
+                % Source: Laursen and Gellert, Some criteria for
+                % numerically integrated matrices and quadrature formulas
+                % for triangles (1978)
+                % Number of points: 6
+                % Degree of exactness: 4
+                
+                a1=0.816847572980459;       w1=0.109951743655322;
+                b1=0.091576213509771;       w2=0.223381589678011;
+                a2=0.108103018168070;
+                b2=0.445948490915965;
+                
+                points=[b1 b1;
+                        a1 b1;
+                        b1 a1;
+                        b2 b2;
+                        a2 b2;
+                        b2 a2];
+                
+                weights=0.5*[w1 w1 w1 w2 w2 w2];
+                
+            case 5
+                % Source: Laursen and Gellert, Some criteria for
+                % numerically integrated matrices and quadrature formulas
+                % for triangles (1978)
+                % Number of points: 7
+                % Degree of exactness: 5
+                
+                a1=1/3;                     w1=0.225000000000000;
+                a2=0.797426985353087;       w2=0.125939180544827;
+                b2=0.101286507323456;       w3=0.132394152788506;
+                a3=0.059715871789770;
+                b3=0.470142064105115;
+                
+                points=[a1 a1;
+                        b2 b2;
+                        a2 b2;
+                        b2 a2;
+                        b3 b3;
+                        a3 b3;
+                        b3 a3];
+                    
+               weights=0.5*[w1 w2 w2 w2 w3 w3 w3];
+                    
+            case 6
+                % Source: Laursen and Gellert, Some criteria for
+                % numerically integrated matrices and quadrature formulas
+                % for triangles (1978)
+                % Number of points: 12
+                % Degree of exactness: 6
+                
+                a1=0.873821971016996;       w1=0.050844906370207;
+                b1=0.063089014491502;       w2=0.116786275726379;
+                a2=0.501426509658179;       w3=0.082851075618374;
+                b2=0.249286745170910;
+                a3=0.636502499121399;
+                b3=0.310352451033785;
+                c3=0.053145049844816;
+                
+                points=[b1 b1;
+                        b1 a1;
+                        a1 b1;
+                        b2 b2;
+                        b2 a2;
+                        a2 b2;
+                        a3 b3;
+                        a3 c3;
+                        b3 a3;
+                        b3 c3;
+                        c3 a3;
+                        c3 b3];
+                    
+               weights=0.5*[w1 w1 w1 w2 w2 w2 w3 w3 w3 w3 w3 w3];
+                 
+            case 7
+                % Source: Laursen and Gellert, Some criteria for
+                % numerically integrated matrices and quadrature formulas
+                % for triangles (1978)
+                % Number of points: 13
+                % Degree of exactness: 7
+                
+                a1=1/3;                     w1=-0.149570044467670;
+                a2=0.479308067841923;       w2= 0.175615257433204;           
+                b2=0.260345966079038;       w3= 0.053347235608839;
+                a3=0.869739794195568;       w4= 0.077113760890257;
+                b3=0.065130102902216;
+                a4=0.638444188569809;
+                b4=0.312865496004875;
+                c4=0.048690315425316;
+                
+                points=[a1 a1;
+                        b2 b2;
+                        b2 a2;
+                        a2 b2;
+                        b3 b3;
+                        b3 a3;
+                        a3 b3;
+                        a4 b4;
+                        a4 c4;
+                        b4 a4;
+                        b4 c4;
+                        c4 a4;
+                        c4 b4];
+                                                      
+                weights=0.5*[w1 w2 w2 w2 w3 w3 w3 w4 w4 w4 w4 w4 w4];
+
+            case 8
+                % Source: Laursen and Gellert, Some criteria for
+                % numerically integrated matrices and quadrature formulas
+                % for triangles (1978)
+                % Number of points: 16
+                % Degree of exactness: 8
+                
+                a1=1/3;                     w1=0.144315607677787;
+                a2=0.658861384496478;       w2=0.103217370534718;
+                b2=0.170569307751761;       w3=0.032458497623198;
+                a3=0.898905543365938;       w4=0.095091634267284;
+                b3=0.050547228317031;       w5=0.027230314174435;
+                a4=0.081414823414554;
+                b4=0.459292588292723;
+                a5=0.008394777409958;
+                b5=0.263112829634638;
+                c5=0.728492392955404;
+                
+                points=[a1 a1;
+                        b2 b2;
+                        b2 a2;
+                        a2 b2;
+                        b3 b3;
+                        b3 a3;
+                        a3 b3;
+                        b4 b4;
+                        b4 a4;
+                        a4 b4;
+                        a5 b5;
+                        a5 c5;
+                        b5 a5;
+                        b5 c5;
+                        c5 a5;
+                        c5 b5];
+                    
+                weights=0.5*[w1 w2 w2 w2 w3 w3 w3 w4 w4 w4 w5 w5 w5 w5 w5 w5];
+                
+            case 9
+                % Source: Laursen and Gellert, Some criteria for
+                % numerically integrated matrices and quadrature formulas
+                % for triangles (1978)
+                % Number of points: 19
+                % Degree of exactness: 9
+                
+                a1=1/3;                     w1=0.097135796282799;
+                a2=0.020634961602524;       w2=0.031334700227139;
+                b2=0.489682519198738;       w3=0.077827541004774;
+                a3=0.125820817014126;       w4=0.079647738927210;
+                b3=0.437089591492937;       w5=0.025577675658698;
+                a4=0.623592928761934;       w6=0.043283539377289;
+                b4=0.188203535619033;
+                a5=0.910540973211094;
+                b5=0.044729513394453;
+                a6=0.036838412054736;
+                b6=0.221962989160766;
+                c6=0.741198598784498;
+                
+                points=[a1 a1;
+                        b2 b2;
+                        b2 a2;
+                        a2 b2;
+                        b3 b3;
+                        b3 a3;
+                        a3 b3;
+                        b4 b4;
+                        b4 a4;
+                        a4 b4;
+                        b5 b5;
+                        b5 a5;
+                        a5 b5;
+                        a6 b6;
+                        a6 c6;
+                        b6 a6;
+                        b6 c6;
+                        c6 a6;
+                        c6 b6];
+              
+             weights=0.5*[w1 w2 w2 w2 w3 w3 w3 w4 w4 w4 w5 w5 w5 w6 w6 w6 w6 w6 w6];       
+                    
+            case 10
+                % Source: Laursen and Gellert, Some criteria for
+                % numerically integrated matrices and quadrature formulas
+                % for triangles (1978)
+                % Number of points: 25
+                % Degree of exactness: 10
+                  
+                a1=1/3;                     w1=0.081743329146286;
+                a2=0.715677797886872;       w2=0.045957963604745;
+                b2=0.142161101056564;       w3=0.013352968813150;
+                a3=0.935889253566112;       w4=0.063904906396424;
+                b3=0.032055373216944;       w5=0.034184648162959;
+                a4=0.148132885783821;       w6=0.025297757707288;
+                b4=0.321812995288835;
+                c4=0.530054118927344;
+                a5=0.029619889488730;
+                b5=0.369146781827811;
+                c5=0.601233328683459;
+                a6=0.028367665339938;
+                b6=0.163701733737182;
+                c6=0.807930600922880;
+                 
+                points=[a1 a1;
+                        b2 b2;
+                        a2 b2;
+                        b2 a2;
+                        b3 b3;
+                        a3 b3;
+                        b3 a3;
+                        a4 b4;
+                        a4 c4;
+                        b4 a4;
+                        b4 c4;
+                        c4 a4;
+                        c4 b4;
+                        a5 b5;
+                        a5 c5;
+                        b5 a5;
+                        b5 c5;
+                        c5 a5;
+                        c5 b5;
+                        a6 b6;
+                        a6 c6;
+                        b6 a6;
+                        b6 c6;
+                        c6 a6;
+                        c6 b6];
+                                    
+                weights=0.5*[w1 w2 w2 w2 w3 w3 w3 w4 w4 w4 w4 w4 w4 w5 w5 w5 w5 w5 w5 w6 w6 w6 w6 w6 w6];           
+                
+            otherwise
+                error('Unimplemented');
+        end
+        
+    case 'boundary'
+        
+        [QuadRule] = getGaussLegendre(0, 1, id);
+        
+        % Quadrature nodes
+        points=QuadRule.x;
+        % Quadrature weights
+        weights=QuadRule.w;
+        
+    case 'interpolation'
+        
+        e=1e-6;
+        
+        switch id
+            case 3
+                points=[e e
+                        1-e e
+                        e 1-e];
+                
+            case 6
+                points=[e e
+                        1-e e
+                        e 1-e
+                        1/3 1/3
+                        2/3 1/3
+                        1/3 2/3];
+                
+        end
+        
+        weights=[];
+end
+
+
+    function [QuadRule] = getGaussLegendre(a,b,n)
+        
+        % getGaussLegendre: 1D n-point Gauss-Legendre quadrature rule on the interval [a,b].
+        % Quadrature rules obtained from Gauss-Legendre are of order 2*n-1.
+        % INPUT:
+        % a,b:          End-points of the interval
+        % n:            Number of quadrature nodes in [a,b]
+        % OUTPUT:
+        % QuadRule:     Structure which contains the fields:
+        %    x:         N-by-1 matrix specifying the abscissae of the quadrature rule.
+        %    w:         N-by-1 matrix specifying the weights of the quadrature rule.
+        
+        if (n==1), x = 0; w = 2;
+        else
+            c = zeros(n-1,1);
+            for i=1:(n-1), c(i)=i/sqrt(4*i*i-1); end
+            J=diag(c,-1)+diag(c,1); [ev,ew]=eig(J);
+            x=diag(ew); w=(2*(ev(1,:).*ev(1,:)))';
+        end
+        
+        % The above rule computes a quadrature rule for the reference interval.
+        % The reference interval is always [-1,1], and if [a,b] != [-1,1] then we
+        % re-map the abscissas (x) and rescale the weights.
+        
+        % Midpoint
+        xm = (b+a)/2;
+        % Area of the requested interval / area of the reference interval
+        xl = (b-a)/2;
+        
+        x = xm + xl*x;
+        w = w * xl;
+        
+        QuadRule.w = w(:);
+        QuadRule.x = x(:);
+    end
+end
\ No newline at end of file
diff --git a/Code/Westergaard/getRadius.m b/Code/Westergaard/getRadius.m
new file mode 100644
index 0000000..570e9d0
--- /dev/null
+++ b/Code/Westergaard/getRadius.m
@@ -0,0 +1,101 @@
+function[r, nnzRMM, nnzCMM, nnzRNM, nnzCNM]=getRadius(Data, nodesM, nodesN, coordM, coordN)
+
+% getRadius: Computes the radiuses of the radial basis functions defined on 
+% the interfaces.
+% INPUT:
+% Data:         Structure containing all the data parameters
+% nodesM:       Set of interpolation nodes
+% nodesN:       Set of evaluation nodes
+% coordM:       Coordinate matrix for the interpolation nodes
+% coordN:       Coordinate matrix for the evaluation nodes
+% OUTPUT:
+% r:            Radiuses of radial basis functions
+% nnzRMM:       Number of off-diagonal nonzeros in each row of PhiMM
+% nnzCMM:       Number of off-diagonal nonzeros in each column of PhiMM
+% nnzRNM:       Number of nonzeros in each row of PhiNM
+% nnzCNM:       Number of nonzeros in each column of PhiNM
+
+M=length(nodesM);
+N=length(nodesN);
+r=zeros(M,1);
+
+% Parameters
+% gamma parameter: real number between ]0,1[
+% Large values of gamma result in small supports
+gamma=Data.Contact.gamma;
+% Gamma parameter: real number between ]0,1[ with Gamma>gamma
+% Used to ensure evaluation points are well within the
+% support of radial basis functions (far enough from the boundary)
+Gamma=Data.Contact.Gamma;
+% Tolerance for contact detection
+% If the tolerance is unknown, set it to some small value.
+d0=Data.Contact.d0;
+% Number of closest neighbors on current interface
+c=Data.Contact.c;
+
+% Number of off-diagonal nonzeros per row of PhiMM
+nnzRMM=zeros(M,1);
+% Number of off-diagonal nonzeros per column of PhiMM
+nnzCMM=zeros(M,1);
+% Number of nonzeros per row of PhiNM
+nnzRNM=zeros(N,1);
+% Number of nonzeros per column of PhiNM
+nnzCNM=zeros(M,1);
+
+maxS=inf;
+f=0;
+niter=0;
+maxiter=10;
+
+% Version 2
+while maxS>f && niter<maxiter
+    % Maximum number of supports to which an evaluation point may belong.
+    % Condition for the matrix PhiMM to be diagonally dominant.
+    f=floor(1./((1-gamma).^4.*(1+4*gamma)));
+    
+    for k=1:M
+        point=coordM(k,:);
+        neighbors=coordM;
+        neighbors(k,:)=inf;
+        distMM=vecnorm(neighbors-point,2,2);
+        distMN=vecnorm(coordN-point,2,2);
+        
+        rMM=mink(distMM,c);
+        rNM=sqrt(d0^2+0.5*rMM(end)^2);
+        radius=max([rMM(end) rNM]);
+        
+        % Make sure the closest neighbor is at a distance >= gamma*radius.
+        % If the point is too close (with respect to the radius), diagonal
+        % dominance will be lost very quickly.
+        if radius>rMM(1)/gamma
+            radius=rMM(1)/gamma;
+        end
+        
+        s1=distMM<radius;
+        s2=distMN<Gamma*radius;
+        
+        nnzRMM(s1)=nnzRMM(s1)+1;
+        nnzRNM(s2)=nnzRNM(s2)+1;
+        nnzCMM(k)=sum(s1);
+        nnzCNM(k)=sum(s2);
+        
+        r(k)=radius;
+    end
+    % Compute the maximum number of supports to which a point belongs
+    maxS=max(nnzRMM);
+    if maxS>f
+        % Increase gamma (sequence converges to 1)
+        gamma=0.5*(1+gamma);
+        % Reinitialize counters
+        nnzRMM=zeros(M,1);
+        nnzCMM=zeros(M,1);
+        nnzRNM=zeros(N,1);
+        nnzCNM=zeros(M,1);
+        niter=niter+1;
+    end
+end
+
+if niter==maxiter && maxS>f
+    warning('The maximum number of rounds for radius computations was reached. Try decreasing d0')
+end
+end
\ No newline at end of file
diff --git a/Code/Westergaard/initializeInterface.m b/Code/Westergaard/initializeInterface.m
new file mode 100644
index 0000000..9f57511
--- /dev/null
+++ b/Code/Westergaard/initializeInterface.m
@@ -0,0 +1,107 @@
+function[Data]=initializeInterface(Mesh, Data, it)
+
+% initializeInterface: Initializes the interfaces properties
+% INPUT:
+% Mesh:     Structure containing all the mesh parameters
+% Data:     Structure containing all the data parameters
+% it:       Boundary tags for either a portion of the boundary or the entire boundary
+%           The first component is the tag for body 1
+%           The second component is the tag for body 2
+% OUTPUT:
+% Data:     Updated data structure with interface properties
+
+% Retrieve mesh parameters
+% Retrieve boundary tag
+BC_tag=Mesh.BC_tag;
+% Retrieve coordinate matrix
+coord=Mesh.coord_mat;
+% Define radius of intersection
+if isfield(Data.Contact, 'radius')
+    r=Data.Contact.radius;
+else
+    r=0.052;
+end
+
+% Boundary elements
+elements1=Mesh.BC_nodes(ismember(BC_tag,it{1}),:);
+elements2=Mesh.BC_nodes(ismember(BC_tag,it{2}),:);
+
+% Boundary nodes
+bc_nodes1=unique(elements1);
+bc_nodes2=unique(elements2);
+
+m1=length(bc_nodes1);
+m2=length(bc_nodes2);
+
+% Compute element lengths
+l1=vecnorm(coord(elements1(:,2),:)-coord(elements1(:,1),:),2,2);
+l2=vecnorm(coord(elements2(:,2),:)-coord(elements2(:,1),:),2,2);
+
+% Compute mesh sizes
+h1=min(l1);
+h2=min(l2);
+
+body_1.h=h1;
+body_2.h=h2;
+
+h=min([h1, h2]);
+Data.Contact.h=h;
+
+% Indicators
+ic1=zeros(m1,1);
+ic2=zeros(m2,1);
+
+% Create interfaces based on closest nodes from opposing interface
+for k=1:m1
+    node=bc_nodes1(k);
+    coord_node=coord(node,:);
+    dist=vecnorm(coord(bc_nodes2,:)-coord_node,2,2);
+    ic1(k)=any(dist<r);
+    
+end
+for k=1:m2
+    node=bc_nodes2(k);
+    coord_node=coord(node,:);
+    dist=vecnorm(coord(bc_nodes1,:)-coord_node,2,2);
+    ic2(k)=any(dist<r);
+end
+
+ic1=logical(ic1);
+ic2=logical(ic2);
+
+interface1=bc_nodes1(ic1);
+interface2=bc_nodes2(ic2);
+
+% Correct interface nodes to account for finite elements
+index1=all(ismember(Mesh.BC_nodes, interface1), 2);
+index2=all(ismember(Mesh.BC_nodes, interface2), 2);
+% Get interface connectivity matrices
+body_1.interface_connect=Mesh.BC_nodes(index1,:);
+body_2.interface_connect=Mesh.BC_nodes(index2,:);
+% Get interface nodes
+body_1.interface_nodes=unique(body_1.interface_connect);
+body_2.interface_nodes=unique(body_2.interface_connect);
+% Get interface degrees of freedom
+interface_dof_1=Mesh.Dof_set(:,body_1.interface_nodes);
+interface_dof_2=Mesh.Dof_set(:,body_2.interface_nodes);
+body_1.interface_dof=interface_dof_1(:);
+body_2.interface_dof=interface_dof_2(:);
+% Initial active set
+body_1.initial_nodes=body_1.interface_nodes;
+body_2.initial_nodes=body_2.interface_nodes;
+% Initial connectivity matrix
+body_1.initial_interface_connect=body_1.interface_connect;
+body_2.initial_interface_connect=body_2.interface_connect;
+% Active set
+body_1.active_set=ismember(body_1.initial_nodes, body_1.interface_nodes);
+body_2.active_set=ismember(body_2.initial_nodes, body_2.interface_nodes);
+
+% Initialization
+body=cell(2,1);
+% Create body entity
+body{1}=body_1;
+body{2}=body_2;
+Data.body=body;
+% Map global dof to local ones
+[Data]=mapGlobal2Local(Data);
+end
\ No newline at end of file
diff --git a/Code/Westergaard/initializeSystem.m b/Code/Westergaard/initializeSystem.m
new file mode 100644
index 0000000..1082963
--- /dev/null
+++ b/Code/Westergaard/initializeSystem.m
@@ -0,0 +1,104 @@
+function[Data, Solution, A, b, K]=initializeSystem(Mesh, Data, it)
+
+% initializeSystem: Assembles the INTERNODES matrix for iteration 0
+% INPUT:
+% Mesh:     Structure containing all the mesh parameters
+% Data:     Structure containing all the data parameters
+% it:       Cell array containing the boundary tags of the interfaces
+% OUTPUT:
+% Data:     Updated structure containing all the data parameters
+% Solution: Initialized solution structure with the Dirichlet BC
+% A:        INTERNODES matrix stored as a sparse matrix
+% b:        Right-hand side vector
+% K:        Global stiffness matrix
+
+% Compute the Dirichlet BC
+[Solution]=computeBC(Mesh, Data);
+% Define the interface
+[Data]=initializeInterface(Mesh, Data, it);
+
+% Sub-assemblage
+[Data]=subAssemble(Mesh, Data, Solution, 'initialization');
+% Assemblage of the stiffness matrix
+[K]=computeStiffness(Mesh, Data);
+% Assemblage of the INTERNODES matrix
+[A,b]=assembleInternodesMat(Mesh, Data, Solution, K);
+
+    function[Solution]=computeBC(Mesh, Data)
+        
+        % computeBC: Function which computes the displacement values at
+        % Dirichlet BC nodes
+        % INPUT:
+        % Mesh:     Structure containing all the mesh parameters
+        % Data:     Structure containing all the data parameters
+        % OUTPUT:
+        % Solution: Structure containing the solution, including displacements
+        %           evaluated at Dirichlet nodes
+        
+        % Retrieve mesh parameters
+        % Coordinate matrix
+        coord=Mesh.coord_mat;
+        % Matrix containing the sets of degrees of freedom for each node
+        Dof_set=Mesh.Dof_set;
+        % Total number of degrees of freedom
+        nb_dof=Mesh.nb_dof;
+        % Local number of degrees of freedom
+        nb_dof_local=Mesh.nb_dof_local;
+        
+        % Retrieve data
+        % Dirichlet boundary conditions
+        D_BC=Data.D_BC;
+        % Set of nodes associated to Dirichlet BC
+        set_D_nodes=Data.set_D_nodes;
+        % Set of degrees of freedom
+        set_Nd=Data.set_Nd;
+        % Number of boundaries with Dirichlet BC
+        n=length(D_BC);
+        
+        % Retrieve solutions
+        U=zeros(nb_dof,1);
+        
+        for i=1:n
+            data=D_BC{i};
+            % Retrieve function
+            g=data.function;
+            % Nodes making up the edges
+            nodes=set_D_nodes{i};
+            % Number of nodes on the edges
+            nb_nodes=length(nodes);
+            
+            if strcmp(data.axis, 'all')
+                % Degrees of freedom
+                dofs=Dof_set(:, nodes);
+                % Coordinates
+                X=reshape(coord(nodes,:), nb_nodes, 1, nb_dof_local);
+                % Evaluation
+                G=g(X);
+                
+                if size(G,1)>nb_dof_local
+                    G=reshape(G, nb_nodes, nb_dof_local)';
+                    G=G(:);
+                else
+                    G=repmat(G, nb_nodes, 1);
+                end
+                U(dofs(:))=G;
+                
+            else
+                % Degrees of freedom
+                dofs=set_Nd{i};
+                % Coordinates
+                X=reshape(coord(nodes,:), nb_nodes, 1, nb_dof_local);
+                % Evaluation
+                G=g(X);
+                U(dofs)=G;
+            end
+            
+        end
+        
+        % Update solution
+        Solution.U=U;
+        % Reshape to an array of the same size as the coordinate matrix
+        Solution.U_sort=reshape(U, nb_dof_local, [])';
+    end
+
+end
\ No newline at end of file
diff --git a/Code/Westergaard/kr.m b/Code/Westergaard/kr.m
new file mode 100644
index 0000000..874aea5
--- /dev/null
+++ b/Code/Westergaard/kr.m
@@ -0,0 +1,31 @@
+function X = kr(U,varargin)
+%KR Khatri-Rao product.
+%   kr(A,B) returns the Khatri-Rao product of two matrices A and B, of 
+%   dimensions I-by-K and J-by-K respectively. The result is an I*J-by-K
+%   matrix formed by the matching columnwise Kronecker products, i.e.
+%   the k-th column of the Khatri-Rao product is defined as
+%   kron(A(:,k),B(:,k)).
+%
+%   kr(A,B,C,...) and kr({A B C ...}) compute a string of Khatri-Rao 
+%   products A o B o C o ..., where o denotes the Khatri-Rao product.
+%
+%   See also kron.
+
+%   Version: 21/10/10
+%   Authors: Laurent Sorber (Laurent.Sorber@cs.kuleuven.be)
+
+if ~iscell(U), U = [U varargin]; end
+K = size(U{1},2);
+if any(cellfun('size',U,2)-K)
+    error('kr:ColumnMismatch', ...
+          'Input matrices must have the same number of columns.');
+end
+J = size(U{end},1);
+X = reshape(U{end},[J 1 K]);
+for n = length(U)-1:-1:1
+    I = size(U{n},1);
+    A = reshape(U{n},[1 I K]);
+    X = reshape(bsxfun(@times,A,X),[I*J 1 K]);
+    J = I*J;
+end
+X = reshape(X,[size(X,1) K]);
diff --git a/Code/Westergaard/mapGlobal2Local.m b/Code/Westergaard/mapGlobal2Local.m
new file mode 100644
index 0000000..6b14a3a
--- /dev/null
+++ b/Code/Westergaard/mapGlobal2Local.m
@@ -0,0 +1,13 @@
+function[Data]=mapGlobal2Local(Data)
+
+% mapGlobal2Local: Function which maps the global degrees of freedom to the
+% local ones for the INTERNODES matrix
+% INPUT:
+% Data:     Structure containing all the data parameters
+% OUTPUT:
+% Data:     Updated structure with the local degrees of freedom
+
+Nf=Data.Nf;
+[~, Data.body{1}.indx_local, ~]=intersect(Nf, Data.body{1}.interface_dof);
+[~, Data.body{2}.indx_local, ~]=intersect(Nf, Data.body{2}.interface_dof);
+end
\ No newline at end of file
diff --git a/Code/Westergaard/plotSolution.m b/Code/Westergaard/plotSolution.m
new file mode 100644
index 0000000..47ba494
--- /dev/null
+++ b/Code/Westergaard/plotSolution.m
@@ -0,0 +1,813 @@
+function[]=plotSolution(Mesh, Data, Solution, varargin)
+
+% plotSolution: Function which plots the computed quantities
+% INPUT:
+% Mesh:     Structure containing the mesh parameters
+% Data:     Structure containing the data parameters
+% Solution: Structure containing the solution computed
+% OPTIONAL INPUT
+% snapshot: selected time-step at which the solution should be viewed
+%   1 -> none (default)
+%   any time step in the range defined
+% view_video: view the video of the solution
+%   1 -> yes 
+%   0 -> no (default)
+% save_video: choose whether to save the video or not
+%   1 -> yes
+%   0 -> no (default)
+% plot_arg: choose which solution to visualize
+%   dynamic_solid or static_solid module
+%       disp (default)
+%       stress
+%       strain
+%   transient_heat module
+%       temp (default)
+%       flux
+% plot_type: decide which type of metric should be used for visualization
+%   displacement metric
+%       standard -> structure with amplified displacements (default)
+%       norm
+%       absx
+%       absy
+%   temperature metric
+%       standard (default)
+%   stress metric
+%       von_mises (default)
+%       sigma_xx
+%       sigma_yy
+%       sigma_xy
+%       sigma_xx_abs
+%       sigma_yy_abs
+%       sigma_xy_abs
+%       frobenius
+%   strain metric
+%       epsilon_xx (default)
+%       epsilon_yy
+%       epsilon_xy
+%       epsilon_xx_abs
+%       epsilon_yy_abs
+%       epsilon_xy_abs
+%       frobenius
+%   flux metric
+%       standard (default)
+
+
+%% Initialize and ckeck Parameters
+
+if mod(length(varargin), 2)~=0
+    error('Missing a name or value')
+end
+labels_in=varargin(1:2:end);
+values_in=varargin(2:2:end);
+
+% Specify number of sub-intervals in time        
+if contains(Data.Model.name, 'static')
+    static=true;
+    N=0;
+else
+    static=false;
+    N=Data.Discretization.N;
+end
+
+switch Data.Model.name
+    case {'static_solid', 'dynamic_solid'}
+        plot_argv={'disp', 'stress', 'strain'};
+        n=length(plot_argv);
+        plot_typev=cell(n,1);
+        plot_typev{1}={'standard', 'norm', 'absx', 'absy'};
+        plot_typev{2}={'von_mises', 'sigma_xx', 'sigma_yy', 'sigma_xy', 'sigma_xx_abs', 'sigma_yy_abs', 'sigma_xy_abs', 'frobenius'};
+        plot_typev{3}={'epsilon_xx', 'epsilon_yy', 'epsilon_xy', 'epsilon_xx_abs', 'epsilon_yy_abs', 'epsilon_xy_abs', 'frobenius'};
+        
+    case 'transient_heat'
+        plot_argv={'temp', 'flux'};
+        n=length(plot_argv);
+        plot_typev=cell(n,1);
+        plot_typev{1}={'standard'};
+        plot_typev{2}={'standard'};
+        
+end
+
+% Set all default parameters
+% Default visualization parameters
+Param.visua_param.snapshot=1;
+Param.visua_param.view_video=false;
+Param.visua_param.write_video=false;
+% Default plot argument
+Param.plot_param.arg=plot_argv(1);
+% Default plot type
+Param.plot_param.type=plot_typev{1}(1);
+
+plot_arg={};
+plot_type={};
+nb_plot=0;
+
+for k=1:length(labels_in)
+    arg=lower(labels_in{k});
+    val=values_in{k};
+    
+    switch arg
+        case 'snapshot'
+            
+            if static
+                warning([arg ' parameter will be ignored because the simulation is in statics'])
+            elseif val < 0 || val > Data.Discretization.N+1
+                error(['Time snapshot invalid: it must be an integer between ' num2str(0) ' and ' num2str(Data.Discretization.N+1)])
+            else
+                Param.visua_param.snapshot=val;
+            end
+            
+        case {'view_video', 'write_video'}
+            
+            if static
+                warning([arg ' parameter will be ignored because the simulation is in statics'])
+            else
+                if isa(val, 'double')
+                    
+                    if val<0 || val>1
+                        error([arg ' parameter must be a boolean'])
+                    else
+                        Param.visua_param.view_video=logical(val);
+                    end
+                    
+                elseif isa(val, 'logical')
+                    Param.visua_param.view_video=val;
+                else
+                    error([arg ' parameter must be a boolean'])
+                end
+            end
+
+        case plot_argv
+            
+            % Check if desired quantities have been computed
+            switch arg
+                case 'stress'
+                    if ~isfield(Solution, 'sigma')
+                        error('Stresses have not been computed')
+                    end
+                    
+                case 'strain'
+                    if ~isfield(Solution, 'epsilon')
+                        error('Strains have not been computed')
+                    end
+                    
+                case 'flux'
+                    if ~isfield(Solution, 'Q')
+                        error('Fluxes have not been computed')
+                    end    
+            end
+            
+            plot_arg{nb_plot+1}=arg;
+            index=strcmp(arg, plot_argv);
+
+            if any(ismember(plot_typev{index}, val))
+                plot_type{nb_plot+1}=val;
+            else
+                error('Plot type invalid: see available plot types')
+            end
+                     
+            nb_plot=nb_plot+1;
+        otherwise
+            error('Unrecognized argument')
+    end
+    
+end
+
+if nb_plot>0
+    Param.plot_param.arg=plot_arg;
+    Param.plot_param.type=plot_type;
+end
+
+%% Plotting interface
+
+plot_arg=Param.plot_param.arg;
+plot_type=Param.plot_param.type;
+n=length(plot_arg);
+Plot=cell(n,1);
+Axis=cell(n,1);
+Colorbar=cell(n,1);
+Dynamic=cell(n,1);
+
+fig=figure;
+% fig.Units='normalized';
+% fig.Position=[0 0 1 1];
+
+% fig.Units='normalized';
+% fig.Position=[0 1 1/2 1/2];
+
+for k=1:n
+    switch plot_arg{k}
+        case {'disp', 'temp'}
+            [PlotParam, AxisParam, ColorbarParam, DynamicParam]=plotPatch(Mesh, Data, Solution, plot_type{k});
+            subplot(1,n,k)
+            axis=gca;
+            p=patch(axis);
+            c=colorbar(axis);
+            [p]=    setObject(p, PlotParam);
+            [axis]= setObject(axis, AxisParam);
+            [c]=    setObject(c, ColorbarParam);
+            
+            
+            
+        case {'strain', 'stress'}
+            [PlotParam, AxisParam, ColorbarParam, DynamicParam]=plotSurf(Mesh, Data, Solution, plot_arg{k}, plot_type{k});
+            subplot(1,n,k)
+            axis=gca;
+            p=patch(axis);
+            c=colorbar(axis);
+            [p]=    setObject(p, PlotParam);
+            [axis]= setObject(axis, AxisParam);
+            [c]=    setObject(c, ColorbarParam);
+            
+            
+            
+        case 'flux'
+            [PlotParam, AxisParam, ColorbarParam, DynamicParam]=plotQuiver(Mesh, Solution, plot_type{k});
+            subplot(1,n,k)
+            axis=gca;
+            p=quiver(axis, PlotParam{1,2}, PlotParam{2,2});
+            c=colorbar(axis);
+            [p]=    setObject(p, PlotParam);
+            [axis]= setObject(axis, AxisParam);
+            [c]=    setObject(c, ColorbarParam);
+            
+    end
+    
+    Plot{k}=p;
+    Axis{k}=axis;
+    Colorbar{k}=c;
+    Dynamic{k}=DynamicParam;
+end
+
+if Param.visua_param.view_video % View the solution over the entire simulation
+    
+    for k=1:n
+        [Plot{k}, Axis{k}]=updateObject(Plot{k}, Axis{k}, Dynamic{k}, 1, N);
+    end
+    
+    frames(1)=getframe(fig);
+    for step = 2:N+1
+        pause(0.05)
+        for k=1:n
+            [Plot{k}, Axis{k}]=updateObject(Plot{k}, Axis{k}, Dynamic{k}, step, N);
+            refreshdata(Axis{k});
+        end
+        frames(step)=getframe(fig);
+    end
+   
+else % View solution at a particular time snapshot
+    step=Param.visua_param.snapshot;
+        for k=1:n
+            [Plot{k}, Axis{k}]=updateObject(Plot{k}, Axis{k}, Dynamic{k}, step, N);
+        end
+end
+
+
+if Param.visua_param.write_video
+%     vid_duration=25; % seconds
+    vid_duration=10; % seconds
+    video=VideoWriter('Output.mp4', 'MPEG-4');
+    video.FrameRate=(N+1)/vid_duration;
+    open(video);
+    writeVideo(video,frames);
+    close(video);
+end
+
+%% Parameters for patch plots
+    function[PlotParam, AxisParam, ColorbarParam, DynamicParam]=plotPatch(Mesh, Data, Solution, type)
+        
+        
+        % Retrieve mesh parameters
+        % Coordinate matrix
+        coord=Mesh.coord_mat;
+        % Connectivity matrix
+        connect=Mesh.connect_mat;
+        % Total number of nodes
+        nb_nodes=Mesh.nb_nodes;
+        % Frame surrounding the structure
+        frame=Mesh.frame;
+        % Largest dimension
+        L_max=Mesh.L_max;
+        % Number of local degrees of freedom
+        nb_dof_local=Mesh.nb_dof_local;
+        % Total number of degrees of freedom
+        nb_dof=Mesh.nb_dof;
+        
+        % Retrieve solution
+        U=Solution.U;
+        
+        
+        switch Data.Model.name
+               
+            case {'static_solid', 'dynamic_solid'}
+                
+                % Initilization
+                Vertices=zeros(nb_nodes,2,N+1);
+                CData=cell(N+1,1);
+                
+                
+                switch type
+                    case 'standard'
+                        
+                        if strcmp(Data.PostProcessing.amplification, 'auto')
+                            % Maximum displacement
+                            disp_max=max(max(abs(U)));
+                            % Amplification factor
+                            fact_ampl=L_max/(2*disp_max);
+                        else
+                            fact_ampl=Data.PostProcessing.amplification;
+                        end
+                        % Retrieve degrees of freedom
+                        d1=1:nb_dof_local:nb_dof;
+                        d2=2:nb_dof_local:nb_dof;
+                        % Aplified displacements
+                        ux_ampl=fact_ampl*U(d1,:);
+                        uy_ampl=fact_ampl*U(d2,:);
+                        % Deformed state
+                        def_x=coord(:,1)+ux_ampl;
+                        def_y=coord(:,2)+uy_ampl;
+                        % Vertices in deformed state
+                        Vertices(:,1,:)=def_x;
+                        Vertices(:,2,:)=def_y;
+                        XLim=[min(def_x, [], 'all');
+                              max(def_x, [], 'all')];
+                        YLim=[min(def_y, [], 'all');
+                              max(def_y, [], 'all')];
+                        CLim=[0,1];
+                        Colormap=[];
+                        ColorbarStatus='off';
+                        ColorbarLabel='';
+                        
+%                         Title='Amplified displacements';
+                        Title='Displacements';
+                        EdgeColor='black';
+                        FaceColor='none';
+                        
+                    case 'norm'
+                        
+                        % Retrieve degrees of freedom
+                        d1=1:nb_dof_local:nb_dof;
+                        d2=2:nb_dof_local:nb_dof;
+                        Ux=U(d1,:);
+                        Uy=U(d2,:);
+                        x=coord(:,1)+Ux;
+                        y=coord(:,2)+Uy;
+                        
+                        U_norm=sqrt(Ux.^2+Uy.^2);
+
+                        XLim=[min(x, [], 'all');
+                              max(x, [], 'all')];
+                        YLim=[min(y, [], 'all');
+                              max(y, [], 'all')];
+                        CLim=[0 max(U_norm, [], 'all')];
+                        Colormap=jet;
+                        ColorbarStatus='on';
+                        ColorbarLabel='Displacement [m]';
+                        Title='Norm of displacements';
+                        EdgeColor='none';
+                        FaceColor='interp';
+                        
+                        for m=1:N+1
+                            Vertices(:,1,m)=x(:,m);
+                            Vertices(:,2,m)=y(:,m);
+                            CData{m}=U_norm(:,m);
+                        end
+                        
+                    case 'absx'
+                        
+                        % Retrieve degrees of freedom
+                        d1=1:nb_dof_local:nb_dof;
+                        d2=2:nb_dof_local:nb_dof;
+                        Ux=U(d1,:);
+                        Uy=U(d2,:);
+                        x=coord(:,1)+Ux;
+                        y=coord(:,2)+Uy;
+                        
+                        Ux_abs=abs(Ux);
+                        
+                        XLim=[min(x, [], 'all');
+                              max(x, [], 'all')];
+                        YLim=[min(y, [], 'all');
+                              max(y, [], 'all')];
+                        CLim=[0 max(Ux_abs, [], 'all')];
+                        Colormap=jet;
+                        ColorbarStatus='on';
+                        ColorbarLabel='Displacement [m]';
+                        Title='Displacements in absolute value along x';
+                        EdgeColor='none';
+                        FaceColor='interp';
+                        
+                        for m=1:N+1
+                            Vertices(:,1,m)=x(:,m);
+                            Vertices(:,2,m)=y(:,m);
+                            CData{m}=Ux_abs(:,m);
+                        end
+                        
+                    case 'absy'
+                        
+                        % Retrieve degrees of freedom
+                        d1=1:nb_dof_local:nb_dof;
+                        d2=2:nb_dof_local:nb_dof;
+                        Ux=U(d1,:);
+                        Uy=U(d2,:);
+                        x=coord(:,1)+Ux;
+                        y=coord(:,2)+Uy;
+                        Uy_abs=abs(Uy);
+                        
+                        XLim=[min(x, [], 'all');
+                              max(x, [], 'all')];
+                        YLim=[min(y, [], 'all');
+                              max(y, [], 'all')];
+                        CLim=[0 max(Uy_abs, [], 'all')];
+                        Colormap=jet;
+                        ColorbarStatus='on';
+                        ColorbarLabel='Displacement [m]';
+                        Title='Displacements in absolute value along y';
+                        EdgeColor='none';
+                        FaceColor='interp';
+                        
+                        for m=1:N+1
+                            Vertices(:,1,m)=x(:,m);
+                            Vertices(:,2,m)=y(:,m);
+                            CData{m}=Uy_abs(:,m);
+                        end
+                        
+                        
+                end
+                
+                
+                % Set static parameters
+                PlotParam = {'Vertices',        coord;...
+                             'Faces',           connect(:,1:3);...
+                             'FaceVertexCData', CData{1};...
+                             'FaceColor',       FaceColor;...
+                             'EdgeColor',       EdgeColor;...
+                             'LineWidth',       1};
+                
+                AxisParam = {'XLim',            XLim;...
+                             'YLim',            YLim;...
+                             'CLim',            CLim;...
+                             'Colormap',        Colormap;...
+                             'DataAspectRatio', [1 1 1];...
+                             'XLabel.String',          'x coordinate [m]';...
+                             'YLabel.String',          'y coordinate [m]'};
+                
+                ColorbarParam = {'Visible',         ColorbarStatus;...
+                                 'Label.String',    ColorbarLabel;...
+                                 'Label.FontSize',  11};
+                
+                % Set dynamic parameters
+                DynamicParam={'Vertices',   num2cell(Vertices, [1,2]);...
+                              'CData',      CData;...
+                              'Title',      Title};
+                          
+            case 'transient_heat'
+                
+                % Number of sub-intervals in time
+                N=Data.Discretization.N;
+                % Initilization
+                CData=cell(N+1,1);
+                
+                switch type
+                    case 'standard'
+                        
+                        XLim=frame(:,1);
+                        YLim=frame(:,2);
+                        CLim=[min(U, [], 'all') max(U, [], 'all')];
+                        Colormap=jet;
+                        ColorbarStatus='on';
+                        ColorbarLabel='Temperature [ºC]';
+                        % ColorbarLabel='Temperature [K]';
+                        Title='Temperature';
+                        EdgeColor='none';
+                        FaceColor='interp';
+                        
+                        for m=1:N+1
+                            CData{m}=U(:,m);
+                        end
+                        
+                end
+                
+                
+                % Set static parameters
+                PlotParam = {'Vertices',        coord;...
+                             'Faces',           connect(:,1:3);...
+                             'FaceVertexCData', CData{1};...
+                             'FaceColor',       FaceColor;...
+                             'EdgeColor',       EdgeColor;...
+                             'LineWidth',       1};
+                
+                AxisParam = {'XLim',            XLim;...
+                             'YLim',            YLim;...
+                             'CLim',            CLim;...
+                             'Colormap',        Colormap;...
+                             'DataAspectRatio', [1 1 1];...
+                             'XLabel.String',          'x coordinate [m]';...
+                             'YLabel.String',          'y coordinate [m]'};
+                
+                ColorbarParam = {'Visible',         ColorbarStatus;...
+                                 'Label.String',    ColorbarLabel;...
+                                 'Label.FontSize',  11};
+                
+                % Set dynamic parameters
+                DynamicParam={'CData',      CData;...
+                              'Title',      Title};
+                   
+        end
+        
+        
+    end
+
+%% Parameters for surf plots
+    function[PlotParam, AxisParam, ColorbarParam, DynamicParam]=plotSurf(Mesh, Data, Solution, arg, type)
+  
+        % Retrieve mesh parameters
+        % Coordinate matrix of interpolation nodes
+        coord_inter=Mesh.coord_inter;
+        % Frame surrounding the structure
+        frame=Mesh.frame;
+        
+        % Retrieve data parameters
+        % Number of evaluation points per element
+        nq=Data.PostProcessing.eval_points;
+        
+        % Total number of interpolation nodes
+        nb_nodes_inter=size(coord_inter,1);
+        
+        % Initilization
+        Vertices=zeros(nb_nodes_inter,2,N+1);
+        CData=cell(N+1,1);
+        
+        switch arg
+            
+            case 'stress'
+                
+                % Retrieve solution
+                sigma=permute(Solution.sigma, [2 1 3]);
+                
+                switch type
+                    case 'von_mises'
+                        
+                        % Von Mises stress
+                        select=sqrt(sigma(:,1,:).^2-sigma(:,1,:).*sigma(:,2,:)+sigma(:,2,:).^2+3*sigma(:,3,:).^2);
+                        ColorbarLabel='Stress [Pa]';
+                        Title='Von Mises stress field';
+                        
+                    case 'sigma_xx'
+                        
+                        % Value of sigma_xx
+                        select=sigma(:,1,:);
+                        ColorbarLabel='Stress [Pa]';
+                        Title='\sigma_{xx}';
+                        
+                    case 'sigma_yy'
+                        
+                        % Value of sigma_yy
+                        select=sigma(:,3,:);
+                        ColorbarLabel='Stress [Pa]';
+                        Title='\sigma_{yy}';
+                        
+                    case 'sigma_xy'
+                        
+                        % Value of sigma_xy
+                        select=sigma(:,2,:);
+                        ColorbarLabel='Stress [Pa]';
+                        Title='\sigma_{xy}';
+                        
+                    case 'sigma_xx_abs'
+                        
+                        % Abolute value of sigma_xx
+                        select=abs(sigma(:,1,:));
+                        ColorbarLabel='Stress [Pa]';
+                        Title='|\sigma_{xx}|';
+                        
+                    case 'sigma_yy_abs'
+                        
+                        % Absolute value of sigma_yy
+                        select=abs(sigma(:,3,:));
+                        ColorbarLabel='Stress [Pa]';
+                        Title='|\sigma_{yy}|';
+                        
+                    case 'sigma_xy_abs'
+                        
+                        % Absolute value of sigma_xy
+                        select=abs(sigma(:,2,:));
+                        ColorbarLabel='Stress [Pa]';
+                        Title='|\sigma_{xy}|';
+                        
+                    case 'frobenius'
+                        
+                        % Frobenius norm of sigma
+                        select=sqrt(sigma(:,2,:).^2+2*sigma(:,2,:).^2+sigma(:,3,:).^2);
+                        ColorbarLabel='Stress [Pa]';
+                        Title='Frobenius norm of \sigma';
+                        
+                end
+                
+            case 'strain'
+                
+                % Retrieve solution
+                epsilon=permute(Solution.epsilon, [2 1 3]);
+                
+                switch type
+                    case 'epsilon_xx'
+                        
+                        % Value of epsilon_xx
+                        select=epsilon(:,1,:);
+                        ColorbarLabel='Strain [-]';
+                        Title='\epsilon_{xx}';
+                        
+                    case 'epsilon_yy'
+                        
+                        % Value of epsilon_yy
+                        select=epsilon(:,3,:);
+                        ColorbarLabel='Strain [-]';
+                        Title='\epsilon_{yy}';
+                        
+                    case 'epsilon_xy'
+                        
+                        % Value of esilon_xy
+                        select=epsilon(:,2,:);
+                        ColorbarLabel='Strain [-]';
+                        Title='\epsilon_{xy}';
+                        
+                    case 'epsilon_xx_abs'
+                        
+                        % Abolute value of epsilon_xx
+                        select=abs(epsilon(:,1,:));
+                        ColorbarLabel='Strain [-]';
+                        Title='|\epsilon_{xx}|';
+                        
+                    case 'epsilon_yy_abs'
+                        
+                        % Absolute value of epsilon_yy
+                        select=abs(epsilon(:,3,:));
+                        ColorbarLabel='Strain [-]';
+                        Title='|\epsilon_{yy}|';
+                        
+                    case 'epsilon_xy_abs'
+                        
+                        % Absolute value of epsilon_xy
+                        select=abs(epsilon(:,2,:));
+                        ColorbarLabel='Strain [-]';
+                        Title='|\epsilon_{xy}|';
+                        
+                    case 'frobenius'
+                        
+                        % Frobenius norm of epsilon
+                        select=sqrt(epsilon(:,2,:).^2+2*epsilon(:,2,:).^2+epsilon(:,3,:).^2);
+                        ColorbarLabel='Strain [-]';
+                        Title='Frobenius norm of \epsilon';
+                        
+                end
+                
+        end
+        
+        % Common parameters
+        XLim=[min(coord_inter(:,1,:), [], 'all');
+              max(coord_inter(:,1,:), [], 'all')];
+        YLim=[min(coord_inter(:,2,:), [], 'all');
+              max(coord_inter(:,2,:), [], 'all')];
+        CLim=[min(select, [], 'all') max(select, [], 'all')];
+        Colormap=jet;
+        ColorbarStatus='on';
+        EdgeColor='none';
+        FaceColor='interp';
+        
+        for m=1:N+1
+            Vertices(:,1,m)=coord_inter(:,1,m);
+            Vertices(:,2,m)=coord_inter(:,2,m);
+            CData{m}=select(:,m);
+        end
+        
+
+        switch nq
+            case 3
+                % 3-point evaluation inside each element
+                I=1:3*Mesh.nb_elem;
+                I=reshape(I, 3, Mesh.nb_elem)';
+                
+            case 6
+                % 6-point evaluation inside each element
+                i1=[1 4 5 2 2 5 6 3 3 6 4 1];
+                i2=[4 5 6];
+                I1=i1+(0:Mesh.nb_elem-1)'*6;
+                I2=i2+(0:Mesh.nb_elem-1)'*6;
+                I1=reshape(I1', 4, 3*Mesh.nb_elem)';
+                I2=[I2 nan*zeros(size(I2,1),1)];
+                I=[I1; I2];
+        end
+        
+                % Set static parameters
+                PlotParam = {'Vertices',        coord_inter;...
+                             'Faces',           I;...
+                             'FaceVertexCData', CData{1};...
+                             'FaceColor',       FaceColor;...
+                             'EdgeColor',       EdgeColor;...
+                             'LineWidth',       1};
+                
+                AxisParam = {'XLim',            XLim;...
+                             'YLim',            YLim;...
+                             'CLim',            CLim;...
+                             'Colormap',        Colormap;...
+                             'DataAspectRatio', [1 1 1];...
+                             'XLabel.String',          'x coordinate [m]';...
+                             'YLabel.String',          'y coordinate [m]'};
+                
+                ColorbarParam = {'Visible',         ColorbarStatus;...
+                                 'Label.String',    ColorbarLabel;...
+                                 'Label.FontSize',  11};
+                
+                % Set dynamic parameters
+                DynamicParam={'Vertices',   num2cell(Vertices, [1,2]);...
+                              'CData',      CData;...
+                              'Title',      Title};
+        
+    end
+
+%% Parameters for quiver plots
+    function[PlotParam, AxisParam, ColorbarParam, DynamicParam]=plotQuiver(Mesh, Solution, type)
+        
+        % Retrieve mesh parameters
+        % Coordinate matrix
+        coord=Mesh.coord_mat;
+        % Frame surrounding the structure
+        frame=Mesh.frame;
+        
+        % Retrieve solution
+        Q_sort=Solution.Q_sort;
+        
+        % Initialization
+        U=cell(1,N+1);
+        V=cell(1,N+1);
+        
+        switch type
+            case 'standard'
+                % Standard visualization of fluxes
+                Title='Flux field';
+                
+                for m=1:N+1
+                    U{m}=Q_sort(:,2*m-1);
+                    V{m}=Q_sort(:,2*m);
+                end
+                
+        end
+        
+        
+        % Set static parameters
+        PlotParam = {'XData',        coord(:,1);...
+                     'YData',        coord(:,2);...
+                     'LineWidth',       1};
+        
+        AxisParam = {'XLim',            frame(:,1);...
+                     'YLim',            frame(:,2);...
+                     'DataAspectRatio', [1 1 1];...
+                     'XLabel.String',          'x coordinate [m]';...
+                     'YLabel.String',          'y coordinate [m]'};
+        
+        ColorbarParam = {'Visible',         'off';...
+                         'Label.String',    '';...
+                         'Label.FontSize',  11};
+        
+        % Set dynamic parameters
+        DynamicParam={'UData',      U;...
+                      'VData',      V;...
+                      'Title',      Title};
+        
+        
+    end
+
+
+
+
+
+
+%% Setting and updating graphic properties
+    function[object]=setObject(object, param)
+        
+        argument=param(:,1);
+        value=param(:,2);
+        
+        for i=1:length(argument)         
+            if contains(argument{i}, '.')
+                field=extractBetween(argument{i}, 1, '.');
+                subfield=extractAfter(argument{i}, '.');
+                object.(field{1}).(subfield)=value{i};
+            else 
+                object.(argument{i})=value{i};
+            end
+        end
+        
+    end
+
+    function[plot, axis]=updateObject(plot, axis, param, step, N)
+        
+        argument=param(:,1);
+        value=param(:,2);
+        
+        for i=1:length(argument)-1
+            plot.(argument{i})=value{i}{step};
+        end
+        
+        axis.Title.String=[value{end} ' at step ' num2str(step) ' out of ' num2str(N+1)];
+    end
+end
\ No newline at end of file
diff --git a/Code/Westergaard/postProcess.m b/Code/Westergaard/postProcess.m
new file mode 100644
index 0000000..364859c
--- /dev/null
+++ b/Code/Westergaard/postProcess.m
@@ -0,0 +1,18 @@
+function[Mesh, Solution]=postProcess(Mesh, Data, Solution)
+
+% postProcess: General post-processing function used to compute derived
+% quantities
+% INPUT:
+% Mesh:     Structure containing all the mesh parameters
+% Data:     Structure containing all the data parameters
+% Solution: Structure containing the solution
+% OUTPUT:
+% Mesh:     Updated mesh structure with coordinates of nodes where stresses
+%           and strains were evaluated
+% Solution: Updated solution structure with derived quantities
+
+
+if any(contains(Data.PostProcessing.quantity, {'stress', 'strain'}))
+    [Mesh, Solution]=postProcessDisp(Mesh, Data, Solution);  
+end
+end
\ No newline at end of file
diff --git a/Code/Westergaard/postProcessDisp.m b/Code/Westergaard/postProcessDisp.m
new file mode 100644
index 0000000..f1f6613
--- /dev/null
+++ b/Code/Westergaard/postProcessDisp.m
@@ -0,0 +1,147 @@
+function[Mesh, Solution]=postProcessDisp(Mesh, Data, Solution)
+
+% postProcessDisp: Function used to compute strains and stresses once the 
+% displacements are known.
+% INPUT:
+% Mesh:     Structure containing all mesh parameters
+% Data:     Structure containing all data parameters
+% Solution: Structure containing the computed solutions
+% OUTPUT:
+% Mesh:     Updated mesh structure with evaluation points coordinates
+% Solution: Updated solution structure including strains and stresses
+
+
+% Finite element order
+order=Mesh.order;
+% Number of local degrees of freedom
+d=Mesh.nb_dof_local;
+% Number of elements
+ne=Mesh.nb_elem;
+% Number of nodes per element
+nne=Mesh.nb_nodes_elem;
+% Coordinate matrix
+coord=Mesh.coord_mat;
+% Connectivity matrix
+connect=Mesh.connect_mat;
+% Retrieve basis functions
+[Functions]=getFunctions(order);
+% Retrieve Jacobian matrix
+J_phi=Functions.J_phi;
+% Number of evaluation points per element
+nq=Data.PostProcessing.eval_points;
+% Retrieve evaluation points
+[points, ~] = getQuadrature(nq, 'interpolation');
+% Number of independent strain and stress components
+Mesh.nb_comp_ind=1/2*d*(d+1);
+% Mu
+f1=Data.Coefficient.mu.function;
+% Lambda
+f2=Data.Coefficient.lambda.function;
+% Retrieve solution
+U_sort=Solution.U_sort;
+% Compute stresses and strains using current position
+coord=coord+U_sort;
+
+% Define permutation matrix
+Sdd=zeros(d*d);
+Id1=eye(d);
+Id2=eye(d^2);
+
+for k=1:d
+    Sdd=Sdd+kron(Id1(:,k)', kron(Id1, Id1(:,k)));
+end
+
+% Reduce size of matrices to avoid unnecessary computations
+% sigma_xx, sigma_xy, sigma_yy
+h=[1 2 4];
+
+I1=(Id2+Sdd);
+I2=Id1(:);
+
+% Truncate the matrices
+I1=I1(h,:);
+I2=I2(h);
+
+% Initialization
+Q=zeros(nq*d^2, nne*d);
+
+% Loop over the Gauss points
+for i = 1:nq
+    L=kron(J_phi(points(i,:))', Id1);
+    Q((i-1)*d^2+1:i*d^2, :)=L;
+end
+Q=sparse(Q);
+T=connect';
+G=T(:);
+
+% Coordinates of the nodes of the elements
+coord_nodes=coord(G,:);
+% Displacements of the nodes of the elements
+W=reshape(U_sort(G,:)', nne*d, ne);
+% Vertices of the elements
+a=coord_nodes(1:nne:end,:);
+b=coord_nodes(2:nne:end,:);
+c=coord_nodes(3:nne:end,:);
+% Interpolated coordinates
+P=a(:)+[b(:)-a(:) c(:)-a(:)]*points';
+X(:,:,1)=P(1:ne,:);
+X(:,:,2)=P(ne+1:end,:);
+coord_inter=reshape(P', nq*ne, d);
+% Update mesh
+Mesh.coord_inter=coord_inter;
+
+% Computation of geometric quantities
+% Determinants of Jacobian matrices
+detJK=(b(:,1)-a(:,1)).*(c(:,2)-a(:,2))-(b(:,2)-a(:,2)).*(c(:,1)-a(:,1));
+
+JK_invT=zeros(d, d*ne);
+JK_invT(:,1:d:d*ne)=1./detJK'.*[c(:,2)-a(:,2) -(c(:,1)-a(:,1))]';
+JK_invT(:,2:d:d*ne)=1./detJK'.*[-(b(:,2)-a(:,2)) b(:,1)-a(:,1)]';
+
+vec_JK_invT=reshape(JK_invT, 1, d^2*ne);
+
+% Coefficient evaluation
+F1=f1(X);
+F2=f2(X);
+if size(F1,1)>1
+F1=reshape(F1, 1, nq, ne);
+end
+if size(F2,1)>1
+F2=reshape(F2, 1, nq, ne);
+end
+
+% Matrix product computations
+W=reshape(Q*W, d^2, nq, ne);
+
+B1=reshape(product(JK_invT,repmat(Id1, 1, ne),d,ne), d^2, d^2, ne);
+B2=reshape(vec_JK_invT, 1, d^2, ne);
+
+% Computation of strains
+if any(contains(Data.PostProcessing.quantity, 'strain'))
+    E=pagemtimes(B1, W);
+    E=0.5*I1*reshape(E, d^2, nq*ne);
+    Solution.epsilon=E;
+end
+
+% Computation of stresses
+if any(contains(Data.PostProcessing.quantity, 'stress'))
+    S1=pagemtimes(B1, F1.*W);
+    S2=pagemtimes(B2, F2.*W);
+    S1=I1*reshape(S1, d^2, nq*ne);
+    S2=I2*reshape(S2, 1, nq*ne);
+    S=S1+S2;
+    Solution.sigma=S;
+end
+
+
+    function[M]=product(A,B,d,ne)
+        
+        % !! Only for a two-dimensional case !!
+        M=zeros(d^2, d^2*ne);
+        
+        M(:,1:d^2:end)=kr(A(:,1:d:end), B(:,1:d:end));
+        M(:,2:d^2:end)=kr(A(:,1:d:end), B(:,2:d:end));
+        M(:,3:d^2:end)=kr(A(:,2:d:end), B(:,1:d:end));
+        M(:,4:d^2:end)=kr(A(:,2:d:end), B(:,2:d:end));
+    end
+end
\ No newline at end of file
diff --git a/Code/Westergaard/readGMSH.m b/Code/Westergaard/readGMSH.m
new file mode 100755
index 0000000..21425ca
--- /dev/null
+++ b/Code/Westergaard/readGMSH.m
@@ -0,0 +1,86 @@
+function [Mesh] = readGMSH(file_name)
+
+% readGMSH: Reads a GMSH mesh file (.msh) and constructs the coordinate,
+% connectivity, boundary nodes, material tag and boundary tag matrices
+% INPUT:
+% file_name: GMSH mesh file (.msh) 
+% OUTPUT:
+% Mesh: Structure containing all fields related to the geometry, boundary
+% conditions and material tags
+%   coord: Matrix of node coordinates
+%   connectivites: Connectivity matrix of the system
+%   boundary_nodes: Matrix containing the boundary nodes (both displacements
+%   and external forces)
+%   material_tag: Vector containing the material tag of the elements
+%   boundary_tag: Vector containing the boundary tags of the elements 
+%   forming boundaries (enables to make the distinction between Dirichlet
+%   and Neumann boundary conditions)
+
+fileID = fopen(file_name);
+text = textscan(fileID, '%s', 'delimiter', '\n');
+fclose(fileID);
+
+text=text{1};
+% Construction of the coordinate matrix
+nodes_start = findPos(text, '$Nodes') + 1;
+
+fileID = fopen(file_name);
+% Node coordinates
+coord = textscan(fileID, '%f %f %f %f', 'HeaderLines', nodes_start);
+% Elements
+connect = textscan(fileID, [repmat('%f', 1, 20) '%*[^\n]'], 'HeaderLines', 3, 'EndOfLine', '\n', 'CollectOutput', 1);
+fclose(fileID);
+
+% Coordinate matrix
+coord=cell2mat(coord);
+% Ignore z component
+coord=coord(:,2:end-1);
+
+% Connectivity matrix
+connect=cell2mat(connect);
+
+I=sum(~isnan(connect), 2);
+v=unique(I);
+
+% Boundary elements
+B=connect(I==v(1), 1:v(1));
+% Elements
+E=connect(I==v(2), 1:v(2));
+
+% Boundary tags
+B_tags=B(:,4);
+% Element tags
+E_tags=E(:,4);
+
+% Boundary elements connectivity matrix
+B_connect=B(:,6:end);
+% Elements connectivity matrix
+E_connect=E(:,6:end);
+
+% Initialize mesh structure
+Mesh.coord_mat=coord;
+Mesh.BC_nodes=B_connect;
+Mesh.connect_mat=E_connect;
+Mesh.BC_tag=B_tags;
+Mesh.material_tag=E_tags;
+
+% Identification of the type of element in GMSH
+% 2 -> T3
+% 9 -> T6
+% 21 -> T10
+% 1 -> line
+% 8 -> quadratic curve
+% 26 -> cubic curve
+end
+
+
+function[start]=findPos(text, string)
+lines = size(text, 1);
+start = 1;
+for i = 1:lines
+    if strcmp(text{i}, string)
+        start = i;
+        break
+    end 
+end
+end
\ No newline at end of file
diff --git a/Code/Westergaard/removeTraction.m b/Code/Westergaard/removeTraction.m
new file mode 100644
index 0000000..e4c5c16
--- /dev/null
+++ b/Code/Westergaard/removeTraction.m
@@ -0,0 +1,73 @@
+function[Data]=removeTraction(Mesh, Data, Solution, n)
+
+% removeTraction: Checks for convergence and updates the interface properties
+% INPUT:
+% Mesh:     Structure containing all the mesh parameters
+% Data:     Structure containing all the data parameters
+% Solution: Structure containing the solution
+% n:        Current iteration number
+% OUTPUT:
+% Data:     Updated data structure with updated interface properties
+
+% Compute the normals based on deformed structure
+% Normals for initial interface
+[Data]=computeNormals(Mesh, Data, Solution);
+
+normal1=Data.body{1}.normal;
+normal2=Data.body{2}.normal;
+
+% Retrieve Lagrange multipliers
+lambda1=Solution.lambda;
+lambda_sort1=Solution.lambda_sort;
+
+% Project the lambdas on interface 2
+lambda2=-Data.R21*lambda1;
+lambda_sort2=reshape(lambda2, Mesh.nb_dof_local, [])';
+
+% Computes the scalar products
+scalar1=sum(lambda_sort1.*normal1(Data.body{1}.active_set,:),2);
+scalar2=sum(lambda_sort2.*normal2(Data.body{2}.active_set,:),2);
+
+% Detect nodes to be removed from the interfaces
+dump_nodes1=Data.body{1}.interface_nodes(scalar1>0);
+dump_nodes2=Data.body{2}.interface_nodes(scalar2>0);
+
+% Update the interfaces
+% If only compression test penetration, otherwise remove nodes in traction
+if isempty(dump_nodes1) && isempty(dump_nodes2)
+    % Gap verification
+    [add_nodes1, add_nodes2]=detectGaps(Mesh, Data, Solution);
+    
+    [Data, nodediff1]=updateInterface(Mesh, Data, 1, add_nodes1, 'add');
+    [Data, nodediff2]=updateInterface(Mesh, Data, 2, add_nodes2, 'add');
+    
+else
+    % Skip gap verification
+    [Data, nodediff1]=updateInterface(Mesh, Data, 1, dump_nodes1, 'dump');
+    [Data, nodediff2]=updateInterface(Mesh, Data, 2, dump_nodes2, 'dump');
+end
+
+fprintf('Iteration %d: \n', n)
+fprintf('%+d nodes on interface 1 \n', nodediff1)
+fprintf('%+d nodes on interface 2 \n', nodediff2)
+fprintf('----------------------------------- \n')
+
+% Check for convergence failure
+if isempty(Data.body{1}.interface_nodes) || isempty(Data.body{2}.interface_nodes)
+    msg=['The contact algorithm stopped prematurely because at least one of ' ...
+         'the sets of nodes of the potential contact interface is empty. '...
+         'Potential causes for this error are:\n'];
+     
+    causes=['1) The boundary conditions are incorrect \n'...
+            '2) The potential contact interface was misidentified \n'...
+            '3) The mesh is too coarse \n'];
+    
+    error(sprintf([msg, causes]))
+end
+
+% Check for convergence
+if abs(nodediff1)+abs(nodediff2)==0 % Stop the iterations
+    Data.Contact.iterate=0;
+    disp('Successfully converged')
+end
+end
\ No newline at end of file
diff --git a/Code/Westergaard/setBoundaryConditions.m b/Code/Westergaard/setBoundaryConditions.m
new file mode 100644
index 0000000..9b514dd
--- /dev/null
+++ b/Code/Westergaard/setBoundaryConditions.m
@@ -0,0 +1,324 @@
+function[Data]=setBoundaryConditions(Mesh, Data, varargin)
+
+% setBoundaryConditions: Function which initializes the boundary data
+% INPUT:
+% Mesh:     Structure containing all the mesh parameters
+% Data:     Structure containing all the data parameters
+% OPTIONAL INPUT:
+% Any type of boundary condition. If no Dirichlet boundary conditions are 
+% specified, the program returns an error
+% Types of boundary conditions:
+%   Dirichlet
+%   Neumann
+% OUTPUT:
+% Data: Updated data structure with boundary conditions
+
+
+%% Set boundary conditions
+if mod(length(varargin{1}), 2)~=0
+    error('Missing a name or value')
+end
+
+% Retrieve boundary nodes
+BC_tags=unique(Mesh.BC_tag);
+
+% Dirichlet type
+D_types={'dirichlet'};
+% Neumann types
+N_types={'neumann'};
+% Available types of boundary conditions
+available_types=union(D_types, N_types);
+% Count to detect duplicate boundaries
+count=[];
+% Count number of Dirichlet and Neumann boundaries specified
+nb_D_BC=0;
+nb_N_BC=0;
+
+% Retrieve labels and values entered by the user
+labels_in=varargin{1}(1:2:end);
+values_in=varargin{1}(2:2:end);
+
+% Initilization
+Data.BC=[];
+
+% Checking for boundary conditions
+c1=strcmp(labels_in, 'BC');
+
+if any(c1)
+    BC=values_in{c1};
+    nb_BC=length(BC);
+    Data.BC=cell(nb_BC,1);
+    
+    for i=1:nb_BC
+        bc=BC{i};
+        l=length(bc);
+        
+        if mod(l,2)~=0
+            error('Missing a name or value')
+        end
+        
+        for j=1:2:l
+            arg=lower(bc{j});
+            val=bc{j+1};
+            
+            switch arg
+                
+                case 'type'
+                    if any(ismember(available_types, lower(val)))
+                        Data.BC{i}.(arg)=lower(val);
+                        
+                        if any(ismember(D_types, lower(val)))
+                            nb_D_BC=nb_D_BC+1;
+                        else
+                            nb_N_BC=nb_N_BC+1;
+                        end
+                    else
+                        error('Unrecognized boundary condition')
+                    end
+                    
+                case 'edges'
+                    
+                    edges=intersect(BC_tags, val);
+                    
+                    if isempty(edges)
+                        error('One of the edges specified does not exist')
+                    elseif ~isempty(intersect(count, val)) || length(unique(val))~=length(val)
+                        error('Duplicate edges detected')   
+                    end
+                    count=[count val];
+                    Data.BC{i}.(arg)=val;
+                    
+                case {'function'}
+                    
+                    if isa(val,'double')
+                        Data.BC{i}.label='const';
+                        Data.BC{i}.(arg)= @(x) val;
+                        
+                    elseif isa(val,'function_handle')
+                        Data.BC{i}.label=detectDependency(val);
+                        Data.BC{i}.(arg)=val;
+                    else
+                        error('Input parameters must be either constants or function handles')
+                    end
+                                       
+                case 'axis'
+                    
+                    if strcmp(val, 'x') || strcmp(val, 'y') || strcmp(val, 'all')
+                        Data.BC{i}.(arg)=val;
+                    else
+                        error('Unrecognized axis')
+                    end
+                    
+                    
+                otherwise
+                    error('Unrecognized argument')
+            end
+            
+        end
+        
+        
+    end
+    
+end
+
+%% Post-Processing: Partitioning the structure BC into Dirichlet BC and Neumann BC and checking for missing data
+
+if nb_D_BC==0
+    error('Dirichlet boundary conditions must be prescribed')
+end
+
+BC=Data.BC;
+l=length(BC);
+
+% Initialize cells
+D_BC=cell(nb_D_BC,1);
+N_BC=cell(nb_N_BC,1);
+% Set of Dirichlet and Neumann tags
+D_tags=[];
+N_tags=[];
+% Initialize counters
+d=1;
+n=1;
+
+for i=1:l
+    bc=BC{i};
+    type=bc.type;
+    
+    if ~isfield(bc, 'edges')
+        error('The edges must be specified')
+    end
+    
+    if any(ismember(D_types, type))
+        D_tags=[D_tags bc.edges];
+        D_BC{d}.type=type; 
+        D_BC{d}.edges=bc.edges;
+        
+        if isfield(bc, 'function')
+            D_BC{d}.function=bc.function;
+            D_BC{d}.label=bc.label;
+        else
+            warning('A function has not been prescribed and will be assumed zero')
+            D_BC{d}.function=@(x) [0; 0];
+            D_BC{d}.label='const';
+        end
+        
+        if isfield(bc, 'axis')
+            D_BC{d}.axis=bc.axis;
+        else
+            D_BC{d}.axis='all';
+        end
+        
+        d=d+1;
+        
+    elseif any(ismember(N_types, type))
+        N_tags=[N_tags bc.edges];
+        N_BC{n}.type=type;
+        N_BC{n}.edges=bc.edges;
+        
+        
+        if isfield(bc, 'function')
+            N_BC{n}.function=bc.function;
+            N_BC{n}.label=bc.label;
+        else
+            warning('A function has not been prescribed and will be assumed zero')
+            N_BC{n}.function=@(x) [0; 0];
+            N_BC{n}.label='const';
+        end
+        
+        if isfield(bc, 'axis')
+            N_BC{n}.axis=bc.axis;
+        else
+            N_BC{n}.axis='all';
+        end
+        
+        n=n+1;
+    end
+end
+
+Data.D_BC=D_BC;
+Data.N_BC=N_BC;
+
+Data.D_tags=D_tags;
+Data.N_tags=N_tags;
+
+%% Sorting nodes
+% If there is a node belonging to conflicting boundary conditions, it is
+% set using the following priority list: Neumann, Dirichlet, Interior
+BC_nodes=Mesh.BC_nodes;
+BC_tag=Mesh.BC_tag;
+nb_nodes=Mesh.nb_nodes;
+Dof_set=Mesh.Dof_set;
+
+set_D_nodes=cell(nb_D_BC,1);
+set_N_nodes=cell(nb_N_BC,1);
+
+set_Nd=cell(nb_D_BC,1);
+set_Nn=cell(nb_N_BC,1);
+
+for k=1:nb_D_BC
+    nodes=BC_nodes(BC_tag==D_BC{k}.edges,:);
+    set_D_nodes{k}=unique(nodes);
+    
+    if strcmp(D_BC{k}.axis, 'x')
+        set_Nd{k}=Dof_set(1,set_D_nodes{k})';
+        
+    elseif strcmp(D_BC{k}.axis, 'y')
+        set_Nd{k}=Dof_set(2,set_D_nodes{k})';
+        
+    else
+        dof=Dof_set(:,set_D_nodes{k});
+        set_Nd{k}=dof(:); 
+    end
+end
+for k=1:nb_N_BC
+    nodes=BC_nodes(BC_tag==N_BC{k}.edges,:);
+    set_N_nodes{k}=unique(nodes);
+    
+    if strcmp(N_BC{k}.axis, 'x')
+        set_Nn{k}=Dof_set(1,set_N_nodes{k})';
+        
+    elseif strcmp(N_BC{k}.axis, 'y')
+        set_Nn{k}=Dof_set(2,set_N_nodes{k})';
+        
+    else
+        dof=Dof_set(:,set_N_nodes{k});
+        set_Nn{k}=dof(:);
+    end
+end
+% Define the sets of nodes
+bc_nodes=unique(BC_nodes);
+% Neumann nodes
+Nodes_N=unique(cell2mat(set_N_nodes));
+% Dirichlet nodes
+Nodes_D=unique(cell2mat(set_D_nodes));
+Nodes_D=setdiff(Nodes_D, Nodes_N);
+Nodes_N=setdiff(bc_nodes, Nodes_D);
+% Interior nodes
+Nodes_I=setdiff(1:nb_nodes, union(Nodes_D, Nodes_N));
+% Free nodes
+Nodes_F=union(Nodes_I, Nodes_N);
+
+% Define the sets of degrees of freedom
+% Neumann nodes
+Nn=unique(cell2mat(set_Nn));
+% Dirichlet nodes
+Nd=unique(cell2mat(set_Nd));
+Nd=setdiff(Nd, Nn);
+dof_n=Dof_set(:,bc_nodes);
+dof_n=dof_n(:);
+Nn=setdiff(dof_n, Nd);
+% Interior nodes
+Ni=setdiff(Dof_set(:), union(Nd, Nn));
+% Free nodes
+Nf=union(Ni, Nn);
+
+% Correct the set for Dirichlet nodes
+for k=1:nb_D_BC
+    set_D_nodes{k}=setdiff(set_D_nodes{k}, Nodes_N);
+    set_Nd{k}=setdiff(set_Nd{k}, Nn);
+end
+
+
+% Update data
+Data.Nodes_I=Nodes_I;
+Data.Nodes_N=Nodes_N;
+Data.Nodes_D=Nodes_D;
+Data.Nodes_F=Nodes_F;
+
+Data.Ni=Ni;
+Data.Nd=Nd;
+Data.Nn=Nn;
+Data.Nf=Nf;
+
+Data.set_D_nodes=set_D_nodes;
+Data.set_N_nodes=set_N_nodes;
+
+Data.set_Nd=set_Nd;
+Data.set_Nn=set_Nn;
+
+%% Auxiliary function
+
+    function[label]=detectDependency(func)
+        
+        string=func2str(func);
+        
+        if ~strcmp(string(1:4), '@(x)')
+            error('Function handle must contain space variables')
+        end
+        string=string(5:end);
+        
+        if contains(string, 'x') && contains(string, 't')
+            label='space_time';
+        elseif contains(string, 'x')
+            label='space';
+        elseif contains(string, 't')
+            label='time';
+        elseif contains(string(1), '0')
+            label='zero';
+        else
+            label='const';
+        end
+        
+    end
+
+end
diff --git a/Code/Westergaard/setCoefficient.m b/Code/Westergaard/setCoefficient.m
new file mode 100644
index 0000000..d15a956
--- /dev/null
+++ b/Code/Westergaard/setCoefficient.m
@@ -0,0 +1,103 @@
+function[Data]=setCoefficient(Data, varargin)
+
+% setCoefficient: Initializes the model coefficients
+% OPTIONAL INPUT:
+% rho:      Specific mass
+% mu:       Lamé constant
+% lambda:   Lamé constant
+% f:        Function "f" of the PDE
+% OUTPUT:
+% Data:     Data structure with initialized coefficients
+
+if mod(length(varargin), 2)~=0
+    error('Missing a name or value')
+end
+labels_in=varargin(1:2:end);
+values_in=varargin(2:2:end);
+
+available_coeff={'rho', 'lambda', 'mu', 'f'};
+
+for i=1:length(labels_in)
+    arg=lower(labels_in{i});
+    val=values_in{i};
+    
+    if any(ismember(available_coeff, arg))
+        
+        
+        switch arg
+            case {'rho', 'lambda', 'mu'} % Only space dependent
+                if isa(val,'double')
+                    
+                    if val ~= 0
+                        Data.Coefficient.(arg).label='const';
+                    else
+                        Data.Coefficient.(arg).label='zero';
+                    end
+                    Data.Coefficient.(arg).function=@(x) val;
+                    
+                elseif isa(val,'function_handle')
+                    Data.Coefficient.(arg).label=detectDependency1(val);
+                    Data.Coefficient.(arg).function=val;
+                else
+                    error('Input parameters must be either constants or function handles')
+                end
+                
+            case 'f'
+                if isa(val,'double') % Only space dependent
+                    Data.Coefficient.f.label='const';
+                    Data.Coefficient.f.function=@(x) val;
+                elseif isa(val,'function_handle')
+                    Data.Coefficient.f.label=detectDependency2(val);
+                    Data.Coefficient.f.function=@(x) val(x);
+                else
+                    error('Input parameters must be either constants or function handles')
+                end
+                
+        end
+        
+    else
+        error('Unrecognized coefficient')
+    end
+end
+
+
+    function[label]=detectDependency1(func)
+        
+        string=func2str(func);
+        
+        if ~strcmp(string(1:4), '@(x)')
+            error('Coefficient can only be constant or space dependent')
+        end
+        
+        if contains(string(5:end), 'x')
+            label='space';
+        elseif contains(string(5), '0')
+            label='zero';
+        else
+            label='const';
+        end
+    end
+
+    function[label]=detectDependency2(func)
+        
+        string=func2str(func);
+        
+        if ~strcmp(string(1:4), '@(x)')
+            error('Function handle must contain space variables')
+        end
+        string=string(5:end);
+        
+        if contains(string, 'x') && contains(string, 't')
+            label='space_time';
+        elseif contains(string, 'x')
+            label='space';
+        elseif contains(string, 't')
+            label='time';
+        elseif contains(string(1), '0')
+            label='zero';
+        else
+            label='const';
+        end
+        
+    end
+end
\ No newline at end of file
diff --git a/Code/Westergaard/setModel.m b/Code/Westergaard/setModel.m
new file mode 100644
index 0000000..58c27a3
--- /dev/null
+++ b/Code/Westergaard/setModel.m
@@ -0,0 +1,75 @@
+function[Data]=setModel(varargin)
+
+% setModel: Initializes the numerical model
+% OPTIONAL INPUT:
+% model:        Specifies which model should be used
+%               Default: transient_heat
+% submodel:     Specifies which submodel from the specified model should be
+%               used
+%               Default: standard
+% type:         Specifies whether the model is linear or nonlinear
+%               Default: linear
+% OUTPUT:
+% Data:         Data structure with initialized model
+
+if mod(length(varargin), 2)~=0
+    error('Missing a name or value')
+end
+labels_in=varargin(1:2:end);
+values_in=varargin(2:2:end);
+
+models={'transient_heat', 'static_solid', 'dynamic_solid'};
+n=length(models);
+submodels=cell(n,1);
+submodels{1}={'standard'};
+submodels{2}={'standard', 'contact'};
+submodels{3}={'standard'};
+types={'linear', 'nonlinear'};
+
+% Set default Parameters
+Data.Model.name=models{1};
+Data.Model.submodel=submodels{1};
+Data.Model.type=types{1};
+
+% Define constants
+% Stefan Boltzman constant W/(m^2*K^4)
+Data.Constants.sigma=5.670373e-8;
+
+for i=1:length(labels_in)
+    arg=lower(labels_in{i});
+    val=lower(values_in{i});
+    
+    switch arg
+        case 'model'
+            if any(ismember(models, val))
+                Data.Model.name=val;
+            else
+                error('Unrecognized model')
+            end
+            
+        case 'submodel'
+            
+            if isfield(Data.Model, 'name')
+                index=strcmp(Data.Model.name, models);
+            else
+                error('Submodel must be defined after the model')
+            end
+            if any(ismember(submodels{index}, val))
+                Data.Model.submodel=val;
+            else
+                error('Unrecognized submodel')
+            end
+            
+        case 'type'
+            if any(ismember(types, val))
+                Data.Model.type=val;
+            else
+                error('Unrecognized type')
+            end     
+            
+        otherwise
+            error('Unrecognized argument')
+    end
+    
+end
+end
\ No newline at end of file
diff --git a/Code/Westergaard/setNumericalParameters.m b/Code/Westergaard/setNumericalParameters.m
new file mode 100644
index 0000000..48a7fff
--- /dev/null
+++ b/Code/Westergaard/setNumericalParameters.m
@@ -0,0 +1,229 @@
+function[Data]=setNumericalParameters(Mesh, Data, varargin)
+
+% setNumericalParameters: Initializes the numerical parameters of the
+% solver
+% OPTIONAL INPUT:
+% Contact:
+%   iterate:            Parameter controlling whether iterations should 
+%                       take place (1) or not (0)
+%                       Default: 1
+%   maxiter:            Maximum number of iterations for solving the
+%                       contact problem
+%                       Default: 10
+%   rescaling:          Rescaling parameter for the stiffness matrix
+%                       Default: 1
+%   c:                  Number of nearest neighbors for computing the
+%                       radius of support of the radial basis functions
+%                       Default: 1
+%   radius:             Intersection radius used to define the initial
+%                       potential contact interface. 
+%                       Default: infinity
+%   gamma:              Value of gamma for preventing quick loss of diagonal
+%                       dominance of the matrices PhiMM
+%                       Default: 0.5
+%   Gamma:              Value of Gamma to avoid very small nonzero entries
+%                       in the matrices PhiNM
+%                       Default: 0.95
+%   d0:                 Measure of tolerance for contact detection.
+%                       Default: 0.05
+%   tol:                Tolerance for gap detection
+%                       Default: 1e-1
+% average:              Parameter controlling whether a simple average (1) 
+%                       or a weighted average (0) of the normal vectors 
+%                       should be used.
+%                       Default: 1                 
+% Solver:
+%   name:               Name of the solver to be used.
+%                       Default: direct
+% PostProcessing:
+%   quantity:           Additional quantities to be computed (for example
+%                       stresses, strains, fluxes,...)
+%                       Default: none
+%   ampl_fact:          Value of amplification factor to apply to
+%                       displacements
+%                       Default: auto
+%   eval_points:        Number of evaluation points used for computing
+%                       stresses or strains
+%                       Default: 3 (P1 finite elements)
+%                                6 (P2 or higher order finite elements)                
+
+if nargin>2
+if mod(length(varargin)-1, 2)~=0
+    error('Missing a name or value')
+end
+
+id=lower(varargin{1});
+labels_in=varargin(2:2:end);
+values_in=varargin(3:2:end);
+end
+
+% Set default Parameters
+if ~isfield(Data, 'Contact')
+    % Boolean indicator for contact algorithm iterations
+    Data.Contact.iterate=true;
+    % Maximum number of iterations of the contact algorithm
+    Data.Contact.maxiter=10;
+    % Rescaling parameter for the INTERNODES matrix
+    Data.Contact.rescaling=1;
+    % Number of nearest neighbors
+    Data.Contact.c=1;
+    % Intersection radius
+    Data.Contact.radius=inf;
+    % gamma value
+    Data.Contact.gamma=0.5;
+    % Gamma value
+    Data.Contact.Gamma=0.95;
+    % Contact tolerance
+    Data.Contact.d0=0.05;
+    % Gap tolerance
+    Data.Contact.tol=1e-1;
+    % Average for normal vectors
+    Data.Contact.average=1;
+end
+if ~isfield(Data, 'Solver')  
+    % Solver name
+    Data.Solver.name='direct';
+end
+if ~isfield(Data, 'PostProcessing')   
+    % Additional quantities to be computed
+    Data.PostProcessing.quantity='none';
+    % Amplification factor
+    Data.PostProcessing.amplification='auto';
+    % Number of evaluation points per element for computing stresses/strains
+    eval_points={3,6};
+    
+    if Mesh.order==1
+        Data.PostProcessing.eval_points=eval_points{1};
+    else
+        Data.PostProcessing.eval_points=eval_points{2};
+    end
+    
+end
+
+% Override the defaults
+if exist('id', 'var')
+    switch id
+        case 'contact'
+            %% Contact algorithm specific parameters
+            
+            for i=1:length(labels_in)
+                arg=lower(labels_in{i});
+                val=lower(values_in{i});
+                
+                switch arg
+                    case {'iterate', 'average'}
+                        if isa(val, 'double')
+                            
+                            if val<0 || val>1
+                                error([arg ' parameter must be a boolean'])
+                            else
+                                Data.Contact.(arg)=logical(val);
+                            end
+                            
+                        elseif isa(val, 'logical')
+                            Data.Contact.(arg)=val;
+                        else
+                            error([arg ' parameter must be a boolean'])
+                        end
+                        
+                    case {'maxiter', 'c', 'radius', 'd0'}
+                        if val<=0
+                            error(['Parameter ' arg ' must be strictly greater than zero'])
+                        else
+                            Data.Contact.(arg)=val;
+                        end
+                        
+                    case 'rescaling'
+                        if val==0
+                            error(['Parameter ' arg ' must be different from zero'])
+                        else
+                            Data.Contact.(arg)=val;
+                        end
+                        
+                    case {'gamma', 'Gamma', 'tol'}
+                        if val<=0 || val>=1
+                            error([arg ' must be strictly greater than 0 and strictly smaller than 1'])
+                        else
+                            Data.Contact.(arg)=val;
+                        end
+                        
+                    otherwise
+                        error('Unrecognized argument')
+                end
+            end
+                
+        case 'solver'
+            %% Solver parameters
+            names={'direct'};
+            
+            for i=1:length(labels_in)
+                arg=lower(labels_in{i});
+                val=values_in{i};
+                
+                switch arg
+                    case 'name'
+                        if any(ismember(names, val))
+                            Data.Solver.(arg)=val;
+                        else
+                            error('Unrecognized solver name')
+                        end
+                        
+                    otherwise
+                        error('Unrecognized argument')
+                end
+                
+            end
+            
+        case 'postprocessing'
+            %% Post-processing parameters
+            quantities={'none', 'flux', 'error', 'stress', 'strain'};
+            eval_points={3,6};
+            
+            for i=1:length(labels_in)
+                arg=lower(labels_in{i});
+                val=values_in{i};
+                
+                switch arg
+                    case 'quantity'
+                        if any(ismember(quantities, val))
+                            Data.PostProcessing.quantity=val;
+                        else
+                            error('Unrecognized quantity')
+                        end
+
+                    case 'eval_points'
+                        if any(ismember(eval_points, val))
+                            Data.PostProcessing.eval_points=val;
+                        else
+                            error('Invalid number of evaluation points')
+                        end
+                        
+                    case 'ampl_fact'
+                        
+                        if isa(val, 'char')
+                            if ~strcmp(val, 'auto')
+                                error('Invalid amplification factor')
+                            else
+                                Data.PostProcessing.amplification=val;
+                            end
+                            
+                        elseif isa(val, 'double')
+                            if val <= 0
+                                error('Invalid amplification factor')
+                            else
+                                Data.PostProcessing.amplification=val;
+                            end
+                        else
+                            error('Invalid amplification factor')
+                        end
+                        
+                    otherwise
+                        error('Unrecognized argument')
+                end
+            end
+            
+        otherwise
+            error('Unrecognized argument')
+    end
+end
+end
\ No newline at end of file
diff --git a/Code/Westergaard/solve.m b/Code/Westergaard/solve.m
new file mode 100644
index 0000000..12638e4
--- /dev/null
+++ b/Code/Westergaard/solve.m
@@ -0,0 +1,93 @@
+function[Mesh, Data, Solution, A, b]=solve(Mesh, Data, it)
+
+% solve: Solves the contact problem
+% INPUT:
+% Mesh:     Structure containing all the mesh parameters
+% Data:     Structure containing all the data parameters
+% it:       Cell array containing the boundary tags of the interfaces (master
+%           interface and slave interface)
+% OUTPUT:
+% Mesh:     Updated structure containing all the mesh parameters
+% Data:     Updated structure containing all the data parameters
+% Solution: Structure containing the solution
+% A:        Global INTERNODES matrix
+% b:        Global right-hand side
+
+
+% Initialization of the system
+[Data, Solution, A, b, K]=initializeSystem(Mesh, Data, it);
+% Initialize count
+n=1;
+
+while Data.Contact.iterate && n<= Data.Contact.maxiter
+    
+    % Solve the system of equations
+    [Solution]=solveSystem(Mesh, Data, Solution, A, b);
+    % Update the system
+    [Data, A, b]=updateSystem(Mesh, Data, Solution);
+    n=n+1;
+end
+
+if n > Data.Contact.maxiter && Data.Contact.iterate
+    warning('Failed to converge within the specified number of iterations')
+end
+
+    function[Solution]=solveSystem(Mesh, Data, Solution, A, b)
+        
+        % solveSystem: Solves the linear system of equations
+        % INPUT:
+        % Mesh:     Structure containing all the mesh parameters
+        % Data:     Structure containing all the data parameters
+        % Solution: Initialized solution structure with Dirichlet BC
+        % A:        INTERNODES matrix
+        % b:        Right-hand side vector
+        % OUTPUT:
+        % Solution: Updated solution structure with computed displacements
+        %           and Lagrange multipliers
+        
+        
+        % Retrieve data parameters
+        % Free degrees of freedom
+        Nf=Data.Nf;
+        % Number of free degrees of freedom
+        nf=length(Nf);
+        % Number of constraints
+        nc=length(Data.body{1}.interface_dof);
+        
+        % Retrieve mesh parameters
+        d=Mesh.nb_dof_local;
+        % Retrieve solution
+        U=Solution.U;
+        % Direct method
+        x=A\b;
+        % Retrieve the displacements
+        U(Nf)=x(1:nf);
+        
+        % Update solution
+        Solution.U=U;
+        Solution.U_sort=reshape(U, d, [])';
+        Solution.lambda=Data.Contact.rescaling*x(nf+1:nf+nc);
+        Solution.lambda_sort=reshape(Solution.lambda, d, [])';
+    end
+
+    function[Data, A, b]=updateSystem(Mesh, Data, Solution)
+        
+        % updateSystem: Updates all quantities which have changed
+        % INPUT:
+        % Mesh:     Structure containing all the mesh parameters
+        % Data:     Structure containing all the data parameters
+        % Solution: Solution structure with computed displacements
+        % OUPUT:
+        % Data:     Data structure with updated interface properties
+        % A:        New INTERNODES matrix
+        % b:        New right-hand side
+        
+        
+        % Check for convergence
+        [Data]=removeTraction(Mesh, Data, Solution, n);
+        % Reassemble the blocks which have changed
+        [Data]=subAssemble(Mesh, Data, Solution, 'updating');
+        % Reassemble the entire internodes matrix
+        [A,b]=assembleInternodesMat(Mesh, Data, Solution, K);
+    end
+end
\ No newline at end of file
diff --git a/Code/Westergaard/subAssemble.m b/Code/Westergaard/subAssemble.m
new file mode 100644
index 0000000..26e78d1
--- /dev/null
+++ b/Code/Westergaard/subAssemble.m
@@ -0,0 +1,202 @@
+function[Data]=subAssemble(Mesh, Data, Solution, operation)
+
+% subAssemble: Assembles the part of the internodes matrix which is
+% changing from one iteration to the next
+% INPUT:
+% Mesh:         Structure containing all the mesh parameters
+% Data:         Structure containing all the data parameters
+% Solution:     Structure containing the solution
+% operation:    Specifies the type of operation (initialization or
+%               updating)
+% OUTPUT:
+% Data:         Updated structure containing all the data parameters
+
+if strcmp(operation, 'initialization')
+    [Data]=computeBoundaryMass(Mesh, Data, 'initialization');
+end
+
+% Assemble interpolation matrices
+[Data]=assembleRs(Mesh, Data, Solution);
+% Assemble boundary mass matrices
+[Data]=computeBoundaryMass(Mesh, Data, 'updating');
+
+    function[Data]=assembleRs(Mesh, Data, Solution)
+        
+        % assembleRs: Constructs the interpolation matrices
+        % INPUT:
+        % Mesh:     Structure containing all the mesh parameters
+        % Data:     Structure containing all the data parameters
+        % Solution: Structure containing the solution
+        % OUTPUT:
+        % Data:     Updated data structure with interpolation matrices
+        
+        % Retrieve mesh parameters
+        nb_dof_local=Mesh.nb_dof_local;
+        
+        % Retrieve nodes making up the interfaces
+        nodes1=Data.body{1}.interface_nodes;
+        nodes2=Data.body{2}.interface_nodes;
+        
+        % 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 distant bodies
+        if all(i1) || all(i2)
+            warning('One of the bodies will be translated because they are too far away from each other. Boundary conditions might be affected')
+            [Mesh, coord1, coord2]=translateBodies(Mesh, Data, radiuses1, radiuses2, nodes1, nodes2, coord1, coord2);
+            
+            % 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
+        
+        % Check for isolated nodes
+        while any(i1) || any(i2)
+            
+            dump_nodes1=nodes1(i1);
+            dump_nodes2=nodes2(i2);
+            
+            % Updating
+            [Data]=updateInterface(Mesh, Data, 1, dump_nodes1, 'dump');
+            [Data]=updateInterface(Mesh, Data, 2, dump_nodes2, 'dump');
+            
+            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;
+        
+        Data.R12_normal=R12;
+        Data.R21_normal=R21;
+        
+        % Expansion to full size
+        Data.R12=kron(R12, speye(nb_dof_local));
+        Data.R21=kron(R21, speye(nb_dof_local));
+        
+    end
+
+    function[Data]=computeBoundaryMass(Mesh, Data, operation)
+        
+        % computeBoundaryMass: Assembles the boundary mass matrices
+        % INPUT:
+        % Mesh:         Structure containing all the mesh parameters
+        % Data:         Structure containing all the data parameters
+        % operation:    Specifies the type of operation (initialization or
+        %               updating)
+        % OUTPUT:
+        % Data:         Updated data structure with boundary mass matrices
+        
+        
+        % Retrieve mesh parameters
+        % Number of local degrees of freedom
+        nb_dof_local=Mesh.nb_dof_local;
+        % Coordinate matrix
+        coord=Mesh.coord_mat;
+        % Order
+        order=Mesh.order;
+        % Get quadrature nodes and weights
+        [points, weights]=getQuadrature(3, 'boundary');
+        % Number of quadrature points
+        nb_points=length(weights);
+        
+        % Retrive functions
+        [Functions]=getFunctions(order);
+        % Retrieve boundary basis functions
+        phi_boundary=Functions.phi_boundary;
+        
+        % Retrieve bodies
+        body=Data.body;
+        % Number of bodies
+        nb_bodies=length(body);
+        
+        if strcmp(operation, 'initialization')
+            
+            for n=1:nb_bodies
+                % Retrieve data
+                % Retriveve edges making up the interface
+                Edges=body{n}.interface_connect;
+                % Retrieve nodes making up the interface
+                nodes=body{n}.interface_nodes;
+                nb_nodes=length(nodes);
+                s=order+1;
+                
+                % Initialization
+                Phi=zeros(s, nb_points);
+                
+                % Map global nodes to local nodes
+                func=@(x) find(nodes==x);
+                connect=arrayfun(func, Edges);
+                T=connect';
+                G=T(:);
+                
+                I=repmat(connect, [1 s])';
+                J=repmat(G', [s 1]);
+                
+                % Loop over the Gauss points
+                for i = 1:nb_points
+                    Phi(:,i)=phi_boundary(points(i,:));
+                end
+                
+                Tr=Edges';
+                Gr=Tr(:);
+                % Coordinates of the nodes of the elements
+                coord_nodes=coord(Gr,:);
+                % Endpoints of the edges
+                a=coord_nodes(1:s:end,:);
+                b=coord_nodes(2:s:end,:);
+                bk=b-a;
+                norm_bk=vecnorm(bk,2,2);
+                
+                lambda=weights.*norm_bk';
+                
+                M=kr(Phi, Phi)*lambda;
+                M=sparse(I(:), J(:), M(:), nb_nodes, nb_nodes);
+                
+                Data.body{n}.iinterface_mass=M;
+                % Expansion to full size
+                M=kron(M, speye(nb_dof_local));
+                Data.body{n}.interface_mass=M;
+            end
+        else
+            for n=1:nb_bodies
+                active_set=Data.body{n}.active_set;                
+                Data.body{n}.interface_mass=kron(Data.body{n}.iinterface_mass(active_set, active_set), speye(nb_dof_local));
+            end
+        end
+    end
+
+end
\ No newline at end of file
diff --git a/Code/Westergaard/translateBodies.m b/Code/Westergaard/translateBodies.m
new file mode 100644
index 0000000..f5a398f
--- /dev/null
+++ b/Code/Westergaard/translateBodies.m
@@ -0,0 +1,60 @@
+function[Mesh, coord1, coord2]=translateBodies(Mesh, Data, radiuses1, radiuses2, nodes1, nodes2, coord1, coord2)
+
+% translateBodies: Performs a translation of one of the bodies in case the
+% initial potential contact interface is empty (because the bodies are too 
+% far away from each other)
+% INPUT:
+% Mesh:         Structure containing all the mesh parameters
+% Data:         Structure containing all the data parameters
+% radiusesk:    Radiuses of support of the radial basis functions (k=1,2)
+% nodesk:       Nodes on the potential contact interface of body k (k=1,2)
+% coordk:       Coordinates of the nodes on the potential contact interface 
+%               of body k (k=1,2)
+% OUTPUT:
+% Mesh:         Mesh structure with updated coordinate matrix
+% coordk:       New coordinates of the nodes on the potential contact 
+%               interface of body k (k=1,2)
+
+n1=length(radiuses1);
+
+dist=inf;
+node1=0;
+node2=0;
+
+for k=1:n1
+    
+    [d, i]=min(vecnorm(coord1(k,:)-coord2,2,2));
+    
+    if d<dist
+        dist=d;
+        node1=nodes1(k);
+        node2=nodes2(i);
+    end
+end
+
+% Check for the most restrictive
+i1=node1==nodes1;
+i2=node2==nodes2;
+
+r1=radiuses1(i1);
+r2=radiuses2(i2);
+
+shift=max([dist-r1, dist-r2]);
+
+% Add safety coefficient
+shift=(1+5e1*Data.Contact.h)*shift;
+
+r=shift/dist*(coord2(i2,:)-coord1(i1,:));
+
+% Change the coordinates of all nodes of body 1
+% !! Assumes body 1 has the smallest material tag !!
+coord=Mesh.coord_mat;
+mt=Mesh.material_tag;
+tags=unique(mt);
+N1=unique(Mesh.connect_mat(mt==tags(1),:));
+coord(N1,:)=coord(N1,:)+r;
+Mesh.coord_mat=coord;
+
+coord1=coord(nodes1,:);
+coord2=coord(nodes2,:);
+end
\ No newline at end of file
diff --git a/Code/Westergaard/updateInterface.m b/Code/Westergaard/updateInterface.m
new file mode 100644
index 0000000..eb45955
--- /dev/null
+++ b/Code/Westergaard/updateInterface.m
@@ -0,0 +1,45 @@
+function[Data, nb_nodes]=updateInterface(Mesh, Data, body_tag, nodes, operation)
+
+% updateInterface: Updates the interface properties
+% INPUT:
+% Mesh:         Structure containing all the mesh parameters
+% Data:         Structure containing all the data parameters
+% body_tag:     Tag of the body to be updated
+% nodes:        Nodes to be added or removed from the potential contact interface
+% operation:    Specifies whether nodes should be added (add) or removed (dump)            
+% OUTPUT:
+% Data:         Updated data structure with interface properties
+% nb_nodes:     Number of nodes added or removed
+
+% Retrieve initial interface connectivity matrix
+iIc=Data.body{body_tag}.initial_interface_connect;
+% Retrieve initial set of nodes
+iIn=Data.body{body_tag}.initial_nodes;
+% Retrieve current set of nodes
+In=Data.body{body_tag}.interface_nodes;
+% Update interface connectivity matrix
+switch operation
+    case 'dump'
+        new_nodes=setdiff(In, nodes);
+        indx=all(ismember(iIc,new_nodes),2);
+        new_interface_connect=iIc(indx,:);
+    case 'add'
+        new_nodes=union(In, nodes);
+        indx=all(ismember(iIc,new_nodes),2);
+        new_interface_connect=iIc(indx,:);
+end
+% Update active set of nodes
+active_set=ismember(iIn, new_nodes);
+% Update interface degrees of freedom
+new_interface_dof=Mesh.Dof_set(:,new_nodes);
+new_interface_dof=new_interface_dof(:);
+% Update body properties
+Data.body{body_tag}.interface_nodes=new_nodes(:);
+Data.body{body_tag}.active_set=active_set;
+Data.body{body_tag}.interface_dof=new_interface_dof;
+Data.body{body_tag}.interface_connect=new_interface_connect;
+% Update mapping of degrees of freedom
+[Data]=mapGlobal2Local(Data);
+% Change in the number of nodes
+nb_nodes=length(new_nodes)-length(In);
+end
\ No newline at end of file
diff --git a/Code/Westergaard/verifySolution.m b/Code/Westergaard/verifySolution.m
new file mode 100644
index 0000000..16b1dfb
--- /dev/null
+++ b/Code/Westergaard/verifySolution.m
@@ -0,0 +1,67 @@
+function[]=verifySolution(Mesh, Data, Solution, p, a)
+
+% verifySolution: Compares the contact pressure at the interface with the 
+% analytical solution and verifies the continuity of the normal stress
+% components.
+% INPUT:
+% Mesh:         Structure containing all the mesh parameters
+% Data:         Structure containing all the data parameters
+% Solution:     Structure containing the solution
+% p:            Analytical solution for the contact pressure
+% a:            Half area of contact [m]
+
+% Retrieve body properties at convergence
+normal1=Data.body{1}.normal;
+normal2=Data.body{2}.normal;
+
+nodes1=Data.body{1}.interface_nodes;
+nodes2=Data.body{2}.interface_nodes;
+
+% Current position
+x=Mesh.coord_mat+Solution.U_sort;
+
+% Retrieve Lagrange multipliers
+lambda1=Solution.lambda;
+lambda_sort1=Solution.lambda_sort;
+
+% Project the lambdas on interface 2
+lambda2=-Data.R21*lambda1;
+lambda_sort2=reshape(lambda2, Mesh.nb_dof_local, [])';
+
+% Compute the normal pressure for interface 1
+pressure1=sum(lambda_sort1.*normal1(Data.body{1}.active_set,:),2);
+
+% Compute the normal pressure for interface 2
+pressure2=sum(lambda_sort2.*normal2(Data.body{2}.active_set,:),2);
+
+x1=x(nodes1,1);
+x2=x(nodes2,1);
+x_fine=linspace(-a,a,1000);
+
+figure
+plot(x_fine, p(x_fine), '-r')
+hold on; grid on
+plot(x1, -pressure1, '-b')
+xlabel('Position [m]')
+ylabel('Pressure [Pa]')
+title('Contact pressure on interface 1')
+legend('Analytical solution', 'Numerical solution')
+
+figure
+plot(x_fine, p(x_fine), '-r')
+hold on; grid on
+plot(x2, -pressure2, '-b')
+xlabel('Position [m]')
+ylabel('Pressure [Pa]')
+title('Contact pressure on interface 2')
+legend('Analytical solution', 'Numerical solution')
+
+figure
+plot(x1, Data.body{1}.sigma_n, '-r')
+hold on; grid on;
+plot(x2, Data.body{2}.sigma_n, '-b')
+xlabel('Position [m]')
+ylabel('Normal stress [Pa]')
+title('Normal stress on interface')
+legend('Interface 1', 'Interface 2')
+end
\ No newline at end of file
diff --git a/Code/Westergaard/verifyStress.m b/Code/Westergaard/verifyStress.m
new file mode 100644
index 0000000..3b6f477
--- /dev/null
+++ b/Code/Westergaard/verifyStress.m
@@ -0,0 +1,56 @@
+function[Data]=verifyStress(Mesh, Data, Solution)
+
+% verifyStress: Computes the normal stresses at the interface of both
+% bodies. Continuity of the normal stress component across the interface
+% should be verified.
+% INPUT:
+% Mesh:         Structure containing all the mesh parameters
+% Data:         Structure containing all the data parameters
+% Solution:     Structure containing the solution
+% OUTPUT:
+% Data:         Updated data structure with the normal component of the
+%               stress vector at each node on the interface
+
+% Retrieve mesh parameters
+% Coordinate matrix
+coord=Mesh.coord_mat;
+% Coordinates of nodes used for the evalutation of stresses and strains
+coord_inter=Mesh.coord_inter;
+
+% Retrieve body
+body=Data.body;
+
+% Retrieve stresses
+sigma=Solution.sigma;
+
+for k=1:length(body)
+    % Retrieve body properties at convergence
+    normal=body{k}.normal;
+    nodes=body{k}.interface_nodes;
+    nodesi=body{k}.initial_nodes;
+    
+    n=length(nodes);
+    sigma_n=zeros(n,1);
+    
+    for j=1:n
+        node=nodes(j);
+        index=node==nodesi;
+        normal_loc=normal(index,:);
+        coord_node=coord(node,:);
+        % Find evaluation nodes closest to the interface node
+        [~, I]=mink(vecnorm(coord_inter-coord_node,2,2),3);
+        s=length(I);
+        stress=sigma(:,I);
+        
+        % !! Only for a two-dimensional case !!
+        tau1=normal_loc*stress([1 2],:);
+        tau2=normal_loc*stress([2 3],:);
+        % Simple average of the stresses
+        tau=1/s*[sum(tau1); sum(tau2)];
+        % Normal component of the stress vector
+        sigma_n(j)=normal_loc*tau;
+    end
+    
+    Data.body{k}.sigma_n=sigma_n;
+end
+end
\ No newline at end of file
diff --git a/Code/Westergaard/westergaard.m b/Code/Westergaard/westergaard.m
new file mode 100644
index 0000000..5890a27
--- /dev/null
+++ b/Code/Westergaard/westergaard.m
@@ -0,0 +1,88 @@
+% INTERNODES method for contact mechanics
+% Case study: Hertzian contact problem with Dirichlet boundary conditions
+% applied to both bodies.
+
+% Limitations: 2D
+clc
+clear variables
+close all
+
+% Read GMSH mesh file (.msh)
+[Mesh] = readGMSH('../../GMSH/Meshes/westergaard_nu_p1.msh');
+
+%% Parameter prescription
+% Mesh parameters
+[Mesh]=getParameters(Mesh);
+% Initialize model
+[Data]=setModel('model', 'static_solid', 'submodel', 'contact', 'type', 'linear');
+
+%% Set material parameters
+% Elastic modulus [N\m^2]
+E=30e9;
+% Poisson ratio [-]
+nu=0.2;
+% Specific mass [kg/m^3]
+rho=2500;
+% Thickness [m]
+t=1;
+% Wavelength [m]
+L=1;
+% Maximum gap [m]
+Delta=0.1;
+% Half area of contact [m]
+a=0.1;
+
+psi_a=pi*a/L;
+E_star=0.5*E/(1-nu^2);
+p_star=pi*E_star*Delta/L;
+p_bar=p_star*sin(psi_a)^2;
+p=@(x) 2*p_bar*cos(pi/L*x)/sin(psi_a)^2.*(sin(psi_a)^2-sin(pi/L*x).^2).^(1/2);
+
+mu=E/(2*(1+nu));
+% Plane strain conditions
+lambda=E*nu/(1+nu)*(1-2*nu);
+
+[Data]=setCoefficient(Data, 'rho', t*rho);
+[Data]=setCoefficient(Data, 'lambda', t*lambda);
+[Data]=setCoefficient(Data, 'mu', @(x) t*mu);
+[Data]=setCoefficient(Data, 'f', @(x) [0; 0]);
+
+%% Set boundary conditions
+
+BC={{'Type', 'Dirichlet', 'Edges', 1, 'Axis', 'x', 'Function', 0},...
+    {'Type', 'Dirichlet', 'Edges', 3, 'Axis', 'x', 'Function', 0},...
+    {'Type', 'Dirichlet', 'Edges', 6, 'Axis', 'x', 'Function', 0},...
+    {'Type', 'Dirichlet', 'Edges', 8, 'Axis', 'x', 'Function', 0},...
+    {'Type', 'Dirichlet', 'Edges', 2, 'Axis', 'y', 'Function', 0},...
+    {'Type', 'Neumann', 'Edges', 7, 'Function', @(x) [0; -p_bar]}};
+
+p2={'BC', BC};    
+
+[Data]=setBoundaryConditions(Mesh, Data, p2);
+%% Set numerical parameters
+% For more information, type 'help setNumericalParameters'
+
+[Data]=setNumericalParameters(Mesh, Data, 'Contact', 'rescaling', E, 'maxiter', 30, 'radius', 0.1, 'tol', 1e-2);
+[Data]=setNumericalParameters(Mesh, Data, 'PostProcessing', 'ampl_fact', 1, 'quantity', {'stress', 'strain'});
+
+%% Assemblage of the right-hand side
+[Data]=computeRHS(Mesh, Data);
+
+%% Solve the contact problem
+[Mesh, Data, Solution, A, b]=solve(Mesh, Data, {1:4, 5:8});
+
+%% Computation of the stresses and strains
+[Mesh, Solution]=postProcess(Mesh, Data, Solution);
+
+%% Visualization of the solution
+% For more information, type 'help plotSolution'
+plotSolution(Mesh, Data, Solution, 'disp', 'standard', 'stress', 'sigma_yy');
+
+%% Comparison between numerical and analytical solutions
+p_max=2*p_bar/sin(psi_a);
+
+[Data]=verifyStress(Mesh, Data, Solution);
+verifySolution(Mesh, Data, Solution, p, a)
+
+lambda=Solution.lambda;
+lambda_max=max(abs(lambda));
\ No newline at end of file
diff --git a/GMSH/Geometries/contact1.geo b/GMSH/Geometries/contact1.geo
new file mode 100644
index 0000000..efce507
--- /dev/null
+++ b/GMSH/Geometries/contact1.geo
@@ -0,0 +1,55 @@
+//+
+Point(1) = {0, 0, 0, 1.0};
+Point(2) = {1, 0, 0, 1.0};
+Point(3) = {1, 1, 0, 1.0};
+Point(4) = {0, 1, 0, 1.0};
+Point(5) = {0, 1.1, 0, 1.0};
+Point(6) = {1, 1.1, 0, 1.0};
+Point(7) = {1, 2, 0, 1.0};
+Point(8) = {0, 2, 0, 1.0};
+Point(9) = {0.5, 2.5, 0, 1.0};
+
+//+
+Line(1) = {1, 2};
+//+
+Line(2) = {2, 3};
+//+
+Line(3) = {3, 4};
+//+
+Line(4) = {4, 1};
+//+
+Circle(5) = {5, 9, 6};
+//+
+Line(6) = {6, 7};
+//+
+Line(7) = {7, 8};
+//+
+Line(8) = {8, 5};
+//+
+Curve Loop(1) = {4, 1, 2, 3};
+//+
+Plane Surface(1) = {1};
+//+
+Curve Loop(2) = {8, 5, 6, 7};
+//+
+Plane Surface(2) = {2};
+//+
+Physical Surface("body_lower") = {1};
+//+
+Physical Surface("body_upper") = {2};
+//+
+Physical Curve("lower_bottom") = {1};
+//+
+Physical Curve("lower_right") = {2};
+//+
+Physical Curve("lower_left") = {4};
+//+
+Physical Curve("lower_top") = {3};
+//+
+Physical Curve("upper_bottom") = {5};
+//+
+Physical Curve("upper_right") = {6};
+//+
+Physical Curve("upper_left") = {8};
+//+
+Physical Curve("upper_top") = {7};
diff --git a/GMSH/Geometries/contact10.geo b/GMSH/Geometries/contact10.geo
new file mode 100644
index 0000000..2001024
--- /dev/null
+++ b/GMSH/Geometries/contact10.geo
@@ -0,0 +1,54 @@
+SetFactory("OpenCASCADE");
+// Parameters
+// Mesh parameter at point
+lc=1.0;
+// Radius of the sphere
+R=0.5;
+// Length
+L=2;
+// Height
+H=2;
+
+
+
+Point(1) = {-L, 0, 0, lc};
+Point(2) = {L, 0, 0, lc};
+Point(3) = {L, H, 0, lc};
+Point(4) = {-L, H, 0, lc};
+Point(5) = {0, H+R, 0, lc};
+Point(6) = {-R, H+R, 0, 1.0};
+Point(7) = {R, H+R, 0, 1.0};
+
+Line(1) = {1, 2};
+Line(2) = {2, 3};
+Line(3) = {3, 4};
+Line(4) = {4, 1};
+Line(6) = {6, 7};
+
+
+Circle(5) = {7, 5, 6};
+
+//+
+Curve Loop(1) = {4, 1, 2, 3};
+//+
+Plane Surface(1) = {1};
+//+
+Curve Loop(2) = {5, 6};
+//+
+Plane Surface(2) = {2};
+//+
+Physical Surface("lower_body", 7) = {1};
+//+
+Physical Surface("upper_body", 8) = {2};
+//+
+Physical Curve("bottom_edge_lower_body", 9) = {1};
+//+
+Physical Curve("right_edge_lower_body", 10) = {2};
+//+
+Physical Curve("top_edge_lower_body", 11) = {3};
+//+
+Physical Curve("left_edge_lower_body", 12) = {4};
+//+
+Physical Curve("bottom_edge_upper_body", 13) = {5};
+//+
+Physical Curve("top_edge_upper_body", 14) = {6};
diff --git a/GMSH/Geometries/contact11.geo b/GMSH/Geometries/contact11.geo
new file mode 100644
index 0000000..d2307f4
--- /dev/null
+++ b/GMSH/Geometries/contact11.geo
@@ -0,0 +1,55 @@
+//+
+Point(1) = {0, 0, 0, 1.0};
+Point(2) = {1, 0, 0, 1.0};
+Point(3) = {1, 1, 0, 1.0};
+Point(4) = {0, 1, 0, 1.0};
+Point(5) = {0, 1.1, 0, 1.0};
+Point(6) = {1, 1.1, 0, 1.0};
+Point(7) = {1, 2, 0, 1.0};
+Point(8) = {0, 2, 0, 1.0};
+Point(9) = {0.5, 2.3, 0, 1.0};
+
+//+
+Line(1) = {1, 2};
+//+
+Line(2) = {2, 3};
+//+
+Line(3) = {3, 4};
+//+
+Line(4) = {4, 1};
+//+
+Circle(5) = {5, 9, 6};
+//+
+Line(6) = {6, 7};
+//+
+Line(7) = {7, 8};
+//+
+Line(8) = {8, 5};
+//+
+Curve Loop(1) = {4, 1, 2, 3};
+//+
+Plane Surface(1) = {1};
+//+
+Curve Loop(2) = {8, 5, 6, 7};
+//+
+Plane Surface(2) = {2};
+//+
+Physical Surface("body_lower") = {1};
+//+
+Physical Surface("body_upper") = {2};
+//+
+Physical Curve("lower_bottom") = {1};
+//+
+Physical Curve("lower_right") = {2};
+//+
+Physical Curve("lower_left") = {4};
+//+
+Physical Curve("lower_top") = {3};
+//+
+Physical Curve("upper_bottom") = {5};
+//+
+Physical Curve("upper_right") = {6};
+//+
+Physical Curve("upper_left") = {8};
+//+
+Physical Curve("upper_top") = {7};
diff --git a/GMSH/Geometries/contact2.geo b/GMSH/Geometries/contact2.geo
new file mode 100644
index 0000000..6074e8a
--- /dev/null
+++ b/GMSH/Geometries/contact2.geo
@@ -0,0 +1,55 @@
+//+
+Point(1) = {0, 0, 0, 1.0};
+Point(2) = {1, 0, 0, 1.0};
+Point(3) = {1, 1, 0, 1.0};
+Point(4) = {0, 1, 0, 1.0};
+Point(5) = {0, 1.1, 0, 1.0};
+Point(6) = {1, 1.1, 0, 1.0};
+Point(7) = {1, 2, 0, 1.0};
+Point(8) = {0, 2, 0, 1.0};
+Point(9) = {0.5, 2.1, 0, 1.0};
+
+//+
+Line(1) = {1, 2};
+//+
+Line(2) = {2, 3};
+//+
+Line(3) = {3, 4};
+//+
+Line(4) = {4, 1};
+//+
+Circle(5) = {5, 9, 6};
+//+
+Line(6) = {6, 7};
+//+
+Line(7) = {7, 8};
+//+
+Line(8) = {8, 5};
+//+
+Curve Loop(1) = {4, 1, 2, 3};
+//+
+Plane Surface(1) = {1};
+//+
+Curve Loop(2) = {8, 5, 6, 7};
+//+
+Plane Surface(2) = {2};
+//+
+Physical Surface("body_lower") = {1};
+//+
+Physical Surface("body_upper") = {2};
+//+
+Physical Curve("lower_bottom") = {1};
+//+
+Physical Curve("lower_right") = {2};
+//+
+Physical Curve("lower_left") = {4};
+//+
+Physical Curve("lower_top") = {3};
+//+
+Physical Curve("upper_bottom") = {5};
+//+
+Physical Curve("upper_right") = {6};
+//+
+Physical Curve("upper_left") = {8};
+//+
+Physical Curve("upper_top") = {7};
diff --git a/GMSH/Geometries/contact3.geo b/GMSH/Geometries/contact3.geo
new file mode 100644
index 0000000..e212346
--- /dev/null
+++ b/GMSH/Geometries/contact3.geo
@@ -0,0 +1,54 @@
+//+
+Point(1) = {0, 0, 0, 1.0};
+Point(2) = {1, 0, 0, 1.0};
+Point(3) = {1, 1, 0, 1.0};
+Point(4) = {0, 1, 0, 1.0};
+Point(5) = {0, 1, 0, 1.0};
+Point(6) = {1, 1, 0, 1.0};
+Point(7) = {1, 2, 0, 1.0};
+Point(8) = {0, 2, 0, 1.0};
+
+//+
+Line(1) = {1, 2};
+//+
+Line(2) = {2, 3};
+//+
+Line(3) = {3, 4};
+//+
+Line(4) = {4, 1};
+//+
+Line(5) = {5, 6};
+//+
+Line(6) = {6, 7};
+//+
+Line(7) = {7, 8};
+//+
+Line(8) = {8, 5};
+//+
+Curve Loop(1) = {4, 1, 2, 3};
+//+
+Plane Surface(1) = {1};
+//+
+Curve Loop(2) = {8, 5, 6, 7};
+//+
+Plane Surface(2) = {2};
+//+
+Physical Surface("body_lower") = {1};
+//+
+Physical Surface("body_upper") = {2};
+//+
+Physical Curve("lower_bottom") = {1};
+//+
+Physical Curve("lower_right") = {2};
+//+
+Physical Curve("lower_left") = {4};
+//+
+Physical Curve("lower_top") = {3};
+//+
+Physical Curve("upper_bottom") = {5};
+//+
+Physical Curve("upper_right") = {6};
+//+
+Physical Curve("upper_left") = {8};
+//+
+Physical Curve("upper_top") = {7};
diff --git a/GMSH/Geometries/contact4.geo b/GMSH/Geometries/contact4.geo
new file mode 100644
index 0000000..65e760d
--- /dev/null
+++ b/GMSH/Geometries/contact4.geo
@@ -0,0 +1,117 @@
+// Radius
+r=0.5;
+// Height
+h=1.5;
+// Length
+l=14*r;
+// Spacing
+s=1.1;
+
+// Lower body
+Point(1) = {0, 0, 0, 1.0};
+Point(2) = {l, 0, 0, 1.0};
+Point(3) = {l, h, 0, 1.0};
+Point(4) = {0, h, 0, 1.0};
+
+// Upper body
+Point(5) = {0, h+s, 0, 1.0};
+Point(6) = {l, h+s, 0, 1.0};
+Point(7) = {l, 2*h+s, 0, 1.0};
+Point(8) = {0, 2*h+s, 0, 1.0};
+
+// Midpoints lower body
+Point(9) = {r, h, 0, 1.0};
+Point(10) = {2*r, h, 0, 1.0};
+Point(11) = {3*r, h, 0, 1.0};
+Point(12) = {4*r, h, 0, 1.0};
+Point(13) = {5*r, h, 0, 1.0};
+Point(14) = {6*r, h, 0, 1.0};
+Point(15) = {7*r, h, 0, 1.0};
+Point(16) = {8*r, h, 0, 1.0};
+Point(17) = {9*r, h, 0, 1.0};
+Point(18) = {10*r, h, 0, 1.0};
+Point(19) = {11*r, h, 0, 1.0};
+Point(20) = {12*r, h, 0, 1.0};
+Point(21) = {13*r, h, 0, 1.0};
+
+// Midpoints upper body
+Point(22) = {r, h+s, 0, 1.0};
+Point(23) = {2*r, h+s, 0, 1.0};
+Point(24) = {3*r, h+s, 0, 1.0};
+Point(25) = {4*r, h+s, 0, 1.0};
+Point(26) = {5*r, h+s, 0, 1.0};
+Point(27) = {6*r, h+s, 0, 1.0};
+Point(28) = {7*r, h+s, 0, 1.0};
+Point(29) = {8*r, h+s, 0, 1.0};
+Point(30) = {9*r, h+s, 0, 1.0};
+Point(31) = {10*r, h+s, 0, 1.0};
+Point(32) = {11*r, h+s, 0, 1.0};
+Point(33) = {12*r, h+s, 0, 1.0};
+Point(34) = {13*r, h+s, 0, 1.0};//+
+Line(1) = {1, 2};
+//+
+Line(2) = {2, 3};
+//+
+Line(3) = {6, 7};
+//+
+Line(4) = {7, 8};
+//+
+Line(5) = {8, 5};
+//+
+Line(6) = {4, 1};
+//+
+Circle(7) = {10, 9, 4};
+//+
+Circle(8) = {14, 13, 12};
+//+
+Circle(9) = {18, 17, 16};
+//+
+Circle(10) = {3, 21, 20};
+//+
+Circle(11) = {10, 11, 12};
+//+
+Circle(12) = {14, 15, 16};
+//+
+Circle(13) = {18, 19, 20};
+//+
+Circle(14) = {5, 22, 23};
+//+
+Circle(15) = {25, 26, 27};
+//+
+Circle(16) = {29, 30, 31};
+//+
+Circle(17) = {33, 34, 6};
+//+
+Circle(18) = {33, 32, 31};
+//+
+Circle(19) = {29, 28, 27};
+//+
+Circle(20) = {25, 24, 23};
+//+
+Curve Loop(1) = {1, 2, 10, -13, 9, -12, 8, -11, 7, 6};
+//+
+Plane Surface(1) = {1};
+//+
+Curve Loop(2) = {19, -15, 20, -14, -5, -4, -3, -17, 18, -16};
+//+
+Plane Surface(2) = {2};
+//+
+Physical Surface("lower_body", 21) = {1};
+//+
+Physical Surface("upper_body", 22) = {2};
+//+
+Physical Curve("top_edge_lower_body", 23) = {7, 11, 8, 12, 9, 13, 10};
+//+
+Physical Curve("bottom_edge_upper_body", 24) = {14, 20, 15, 19, 16, 18, 17};
+//+
+Physical Curve("bottom_edge_lower_body", 25) = {1};
+//+
+Physical Curve("right_edge_lower_body", 26) = {2};
+//+
+Physical Curve("left_edge_lower_body", 27) = {6};
+//+
+Physical Curve("top_edge_upper_body", 28) = {4};
+//+
+Physical Curve("right_edge_upper_body", 29) = {3};
+//+
+Physical Curve("left_edge_upper_body", 30) = {5};
diff --git a/GMSH/Geometries/contact5.geo b/GMSH/Geometries/contact5.geo
new file mode 100644
index 0000000..c569d08
--- /dev/null
+++ b/GMSH/Geometries/contact5.geo
@@ -0,0 +1,117 @@
+// Radius
+r=0.5;
+// Height
+h=1.5;
+// Length
+l=14*r;
+// Spacing
+s=0.85;
+
+// Lower body
+Point(1) = {0, 0, 0, 1.0};
+Point(2) = {l, 0, 0, 1.0};
+Point(3) = {l, h, 0, 1.0};
+Point(4) = {0, h, 0, 1.0};
+
+// Upper body
+Point(5) = {0, h+s, 0, 1.0};
+Point(6) = {l, h+s, 0, 1.0};
+Point(7) = {l, 2*h+s, 0, 1.0};
+Point(8) = {0, 2*h+s, 0, 1.0};
+
+// Midpoints lower body
+Point(9) = {r, h, 0, 1.0};
+Point(10) = {2*r, h, 0, 1.0};
+Point(11) = {3*r, h, 0, 1.0};
+Point(12) = {4*r, h, 0, 1.0};
+Point(13) = {5*r, h, 0, 1.0};
+Point(14) = {6*r, h, 0, 1.0};
+Point(15) = {7*r, h, 0, 1.0};
+Point(16) = {8*r, h, 0, 1.0};
+Point(17) = {9*r, h, 0, 1.0};
+Point(18) = {10*r, h, 0, 1.0};
+Point(19) = {11*r, h, 0, 1.0};
+Point(20) = {12*r, h, 0, 1.0};
+Point(21) = {13*r, h, 0, 1.0};
+
+// Midpoints upper body
+Point(22) = {r, h+s, 0, 1.0};
+Point(23) = {2*r, h+s, 0, 1.0};
+Point(24) = {3*r, h+s, 0, 1.0};
+Point(25) = {4*r, h+s, 0, 1.0};
+Point(26) = {5*r, h+s, 0, 1.0};
+Point(27) = {6*r, h+s, 0, 1.0};
+Point(28) = {7*r, h+s, 0, 1.0};
+Point(29) = {8*r, h+s, 0, 1.0};
+Point(30) = {9*r, h+s, 0, 1.0};
+Point(31) = {10*r, h+s, 0, 1.0};
+Point(32) = {11*r, h+s, 0, 1.0};
+Point(33) = {12*r, h+s, 0, 1.0};
+Point(34) = {13*r, h+s, 0, 1.0};//+
+Line(1) = {1, 2};
+//+
+Line(2) = {2, 3};
+//+
+Line(3) = {6, 7};
+//+
+Line(4) = {7, 8};
+//+
+Line(5) = {8, 5};
+//+
+Line(6) = {4, 1};
+//+
+Circle(7) = {10, 9, 4};
+//+
+Circle(8) = {14, 13, 12};
+//+
+Circle(9) = {18, 17, 16};
+//+
+Circle(10) = {3, 21, 20};
+//+
+Circle(11) = {10, 11, 12};
+//+
+Circle(12) = {14, 15, 16};
+//+
+Circle(13) = {18, 19, 20};
+//+
+Circle(14) = {5, 22, 23};
+//+
+Circle(15) = {25, 26, 27};
+//+
+Circle(16) = {29, 30, 31};
+//+
+Circle(17) = {33, 34, 6};
+//+
+Circle(18) = {33, 32, 31};
+//+
+Circle(19) = {29, 28, 27};
+//+
+Circle(20) = {25, 24, 23};
+//+
+Curve Loop(1) = {1, 2, 10, -13, 9, -12, 8, -11, 7, 6};
+//+
+Plane Surface(1) = {1};
+//+
+Curve Loop(2) = {19, -15, 20, -14, -5, -4, -3, -17, 18, -16};
+//+
+Plane Surface(2) = {2};
+//+
+Physical Surface("lower_body", 21) = {1};
+//+
+Physical Surface("upper_body", 22) = {2};
+//+
+Physical Curve("top_edge_lower_body", 23) = {7, 11, 8, 12, 9, 13, 10};
+//+
+Physical Curve("bottom_edge_upper_body", 24) = {14, 20, 15, 19, 16, 18, 17};
+//+
+Physical Curve("bottom_edge_lower_body", 25) = {1};
+//+
+Physical Curve("right_edge_lower_body", 26) = {2};
+//+
+Physical Curve("left_edge_lower_body", 27) = {6};
+//+
+Physical Curve("top_edge_upper_body", 28) = {4};
+//+
+Physical Curve("right_edge_upper_body", 29) = {3};
+//+
+Physical Curve("left_edge_upper_body", 30) = {5};
diff --git a/GMSH/Geometries/contact6.geo b/GMSH/Geometries/contact6.geo
new file mode 100644
index 0000000..01e3072
--- /dev/null
+++ b/GMSH/Geometries/contact6.geo
@@ -0,0 +1,45 @@
+// Parameters
+l1=1.0;
+l2=0.01;
+
+// Points lower body
+Point(1) = {-2, 0, 0, l1};
+Point(2) = {2, 0, 0, l1};
+Point(3) = {2, 1, 0, l1};
+Point(4) = {-2, 1, 0, l1};
+
+// Points upper body
+Point(5) = {-1, 1.05, 0, l2};
+Point(6) = {1, 1.05, 0, l2};
+Point(7) = {1, 2.0, 0, l2};
+Point(8) = {-1, 2.0, 0, l2};
+
+// Lines
+Line(1) = {1, 2};
+Line(2) = {2, 3};
+Line(3) = {3, 4};
+Line(4) = {4, 1};
+Line(5) = {5, 6};
+Line(6) = {6, 7};
+Line(7) = {7, 8};
+Line(8) = {8, 5};
+
+// Loops
+Curve Loop(1) = {3, 4, 1, 2};
+Curve Loop(2) = {8, 5, 6, 7};
+
+// Surfaces
+Plane Surface(1) = {1};
+Plane Surface(2) = {2};
+
+// Physical groups
+Physical Surface("lower_body", 9) = {1};
+Physical Surface("upper_body", 10) = {2};
+Physical Curve("lower_body_bottom_edge", 11) = {1};
+Physical Curve("lower_body_right_edge", 12) = {2};
+Physical Curve("lower_body_top_edge", 13) = {3};
+Physical Curve("lower_body_left_edge", 14) = {4};
+Physical Curve("top_body_bottom_edge", 15) = {5};
+Physical Curve("top_body_right_edge", 16) = {6};
+Physical Curve("top_body_top_edge", 17) = {7};
+Physical Curve("top_body_left_edge", 18) = {8};
diff --git a/GMSH/Geometries/contact7.geo b/GMSH/Geometries/contact7.geo
new file mode 100644
index 0000000..5f074eb
--- /dev/null
+++ b/GMSH/Geometries/contact7.geo
@@ -0,0 +1,56 @@
+// Parameters
+lc1 = 0.1;
+lc2 = 0.05;
+
+// Points lower body
+Point(1) = {-2, 0, 0, lc1};
+Point(2) = {2, 0, 0, lc1};
+Point(3) = {2, 1, 0, lc1};
+Point(4) = {-2, 1, 0, lc1};
+
+// Points upper body
+Point(5) = {-1, 1.05, 0, lc2};
+Point(6) = {1, 1.05, 0, lc2};
+Point(7) = {1, 2.0, 0, lc2};
+Point(8) = {-1, 2.0, 0, lc2};
+
+// Lines
+Line(1) = {1, 2};
+Line(2) = {2, 3};
+Line(3) = {3, 4};
+Line(4) = {4, 1};
+Line(5) = {5, 6};
+Line(6) = {6, 7};
+Line(7) = {7, 8};
+Line(8) = {8, 5};
+
+// Loops
+Curve Loop(1) = {3, 4, 1, 2};
+Curve Loop(2) = {8, 5, 6, 7};
+
+// Surfaces
+Plane Surface(1) = {1};
+Plane Surface(2) = {2};
+
+// Physical groups
+Physical Surface("lower_body", 9) = {1};
+Physical Surface("upper_body", 10) = {2};
+Physical Curve("lower_body_bottom_edge", 11) = {1};
+Physical Curve("lower_body_right_edge", 12) = {2};
+Physical Curve("lower_body_top_edge", 13) = {3};
+Physical Curve("lower_body_left_edge", 14) = {4};
+Physical Curve("top_body_bottom_edge", 15) = {5};
+Physical Curve("top_body_right_edge", 16) = {6};
+Physical Curve("top_body_top_edge", 17) = {7};
+Physical Curve("top_body_left_edge", 18) = {8};
+
+// Mesh sizes
+MeshSize {4, 1, 2, 3} = lc1;
+MeshSize {5, 6, 7, 8} = lc2;
+//+
+Transfinite Curve {3} = 200 Using Progression 1;
+Transfinite Curve {5} = 500 Using Progression 1;
+Transfinite Curve {6} = 50 Using Progression 1;
+Transfinite Curve {8} = 50 Using Progression 1;
+Transfinite Curve {4} = 20 Using Progression 1;
+Transfinite Curve {2} = 20 Using Progression 1;
diff --git a/GMSH/Geometries/contact8.geo b/GMSH/Geometries/contact8.geo
new file mode 100644
index 0000000..3c54fb7
--- /dev/null
+++ b/GMSH/Geometries/contact8.geo
@@ -0,0 +1,67 @@
+//+
+Point(1) = {0, 0, 0, 1.0};
+Point(2) = {1, 0, 0, 1.0};
+Point(3) = {1, 1, 0, 1.0};
+Point(4) = {0, 1, 0, 1.0};
+Point(5) = {0, 1.1, 0, 1.0};
+Point(6) = {1, 1.1, 0, 1.0};
+Point(7) = {1, 2, 0, 1.0};
+Point(8) = {0, 2, 0, 1.0};
+Point(9) = {0.5, 2.1, 0, 1.0};
+
+//+
+Line(1) = {1, 2};
+//+
+Line(2) = {2, 3};
+//+
+Line(3) = {3, 4};
+//+
+Line(4) = {4, 1};
+//+
+Circle(5) = {5, 9, 6};
+//+
+Line(6) = {6, 7};
+//+
+Line(7) = {7, 8};
+//+
+Line(8) = {8, 5};
+//+
+Curve Loop(1) = {4, 1, 2, 3};
+//+
+Plane Surface(1) = {1};
+//+
+Curve Loop(2) = {8, 5, 6, 7};
+//+
+Plane Surface(2) = {2};
+//+
+Physical Surface("body_lower") = {1};
+//+
+Physical Surface("body_upper") = {2};
+//+
+Physical Curve("lower_bottom") = {1};
+//+
+Physical Curve("lower_right") = {2};
+//+
+Physical Curve("lower_left") = {4};
+//+
+Physical Curve("lower_top") = {3};
+//+
+Physical Curve("upper_bottom") = {5};
+//+
+Physical Curve("upper_right") = {6};
+//+
+Physical Curve("upper_left") = {8};
+//+
+Physical Curve("upper_top") = {7};
+//+
+Transfinite Curve {3} = 1000 Using Progression 1;
+//+
+Transfinite Curve {5} = 1000 Using Progression 1;
+//+
+Transfinite Curve {4} = 100 Using Progression 1;
+//+
+Transfinite Curve {2} = 100 Using Progression 1;
+//+
+Transfinite Curve {6} = 100 Using Progression 1;
+//+
+Transfinite Curve {8} = 100 Using Progression 1;
diff --git a/GMSH/Geometries/contact9.geo b/GMSH/Geometries/contact9.geo
new file mode 100644
index 0000000..80fc2c6
--- /dev/null
+++ b/GMSH/Geometries/contact9.geo
@@ -0,0 +1,50 @@
+SetFactory("OpenCASCADE");
+// Parameters
+// Mesh parameter at point
+lc=1.0;
+// Radius of the sphere
+R=0.5;
+// Length
+L=2;
+// Height
+H=2;
+
+
+
+Point(1) = {-L, 0, 0, lc};
+Point(2) = {L, 0, 0, lc};
+Point(3) = {L, H, 0, lc};
+Point(4) = {-L, H, 0, lc};
+Point(5) = {0, H+R, 0, lc};
+//+
+Line(1) = {1, 2};
+//+
+Line(2) = {2, 3};
+//+
+Line(3) = {3, 4};
+//+
+Line(4) = {4, 1};
+//+
+Circle(5) = {-0, 2.5, 0, 0.5, 0, 2*Pi};
+//+
+Curve Loop(1) = {4, 1, 2, 3};
+//+
+Plane Surface(1) = {1};
+//+
+Curve Loop(2) = {5};
+//+
+Plane Surface(2) = {2};
+//+
+Physical Surface("lower_body", 6) = {1};
+//+
+Physical Surface("upper_body", 7) = {2};
+//+
+Physical Curve("bottom_edge_lower_body", 8) = {1};
+//+
+Physical Curve("right_edge_lower_body", 9) = {2};
+//+
+Physical Curve("top_edge_lower_body", 10) = {3};
+//+
+Physical Curve("left_edge_lower_body", 11) = {4};
+//+
+Physical Curve("boundary_upper_body", 12) = {5};
diff --git a/GMSH/Geometries/piece1.geo b/GMSH/Geometries/piece1.geo
new file mode 100644
index 0000000..95cd247
--- /dev/null
+++ b/GMSH/Geometries/piece1.geo
@@ -0,0 +1,117 @@
+
+
+// Define parameters
+lc=1;
+// Cicle radius
+l=1;
+// Gap
+d=0.1;
+// Angle width
+a=1;
+// Step
+s=Sqrt(2)/2*d;
+// Midpoint
+m=0.5*a;
+// Frame height
+h=1;
+
+
+Point(1)={0, -(l+a), 0, lc};
+Point(2)={l, -(l+a), 0, lc};
+Point(3)={l+s, -(l+a)+s, 0, lc};
+Point(4)={l+m, -(l+m), 0, lc};
+Point(5)={l+a-s, -(l+s), 0, lc};
+Point(6)={l+a, -l, 0, lc};
+Point(7)={l+a, 0, 0, lc};
+Point(8)={l+a, l, 0, lc};
+Point(9)={l+a-s, l+s, 0, lc};
+Point(10)={l+m, l+m, 0, lc};
+Point(11)={l+s, l+a-s, 0, lc};
+Point(12)={l, l+a, 0, lc};
+Point(13)={0, l+a, 0, lc};
+Point(14)={-l, l+a, 0, lc};
+Point(15)={-(l+s), l+a-s, 0, lc};
+Point(16)={-(l+m), l+m, 0, lc};
+Point(17)={-(l+a)+s, l+s, 0, lc};
+Point(18)={-(l+a), l, 0, lc};
+Point(19)={-(l+a), 0, 0, lc};
+Point(20)={-(l+a), -l, 0, lc};
+Point(21)={-(l+a)+s, -(l+s), 0, lc};
+Point(22)={-(l+m), -(l+m), 0, lc};
+Point(23)={-(l+s), -(l+a)+s, 0, lc};
+Point(24)={-l, -(l+a), 0, lc};
+
+// Outer frame
+Point(25)={-(l+a+h), -(l+a+h), 0, lc};
+Point(26)={(l+a+h), -(l+a+h), 0, lc};
+Point(27)={(l+a+h), (l+a+h), 0, lc};
+Point(28)={-(l+a+h), (l+a+h), 0, lc};
+
+
+
+//+
+Circle(1) = {2, 1, 24};
+//+
+Circle(2) = {3, 1, 23};
+//+
+Circle(3) = {3, 4, 5};
+//+
+Circle(4) = {2, 4, 6};
+//+
+Circle(5) = {8, 7, 6};
+//+
+Circle(6) = {9, 7, 5};
+//+
+Circle(7) = {8, 10, 12};
+//+
+Circle(8) = {9, 10, 11};
+//+
+Circle(9) = {14, 13, 12};
+//+
+Circle(10) = {15, 13, 11};
+//+
+Circle(11) = {15, 16, 17};
+//+
+Circle(12) = {14, 16, 18};
+//+
+Circle(13) = {20, 19, 18};
+//+
+Circle(14) = {21, 19, 17};
+//+
+Circle(15) = {21, 22, 23};
+//+
+Circle(16) = {20, 22, 24};
+//+
+Line(17) = {25, 26};
+//+
+Line(18) = {26, 27};
+//+
+Line(19) = {27, 28};
+//+
+Line(20) = {28, 25};
+//+
+Curve Loop(1) = {14, -11, 10, -8, 6, -3, 2, -15};
+//+
+Plane Surface(1) = {1};
+//+
+Curve Loop(2) = {17, 18, 19, 20};
+//+
+Curve Loop(3) = {16, -1, 4, -5, 7, -9, 12, -13};
+//+
+Plane Surface(2) = {2, 3};
+//+
+Physical Curve("inner_boundary", 21) = {2, 15, 14, 11, 10, 8, 6, 3};
+//+
+Physical Curve("outer_boundary", 22) = {13, 12, 9, 7, 5, 4, 1, 16};
+//+
+Physical Curve("bottom_edge", 23) = {17};
+//+
+Physical Curve("right_edge", 24) = {18};
+//+
+Physical Curve("top_edge", 25) = {19};
+//+
+Physical Curve("left_edge", 26) = {20};
+//+
+Physical Surface("outer_body", 27) = {2};
+//+
+Physical Surface("inner_body", 28) = {1};
diff --git a/GMSH/Geometries/piece2.geo b/GMSH/Geometries/piece2.geo
new file mode 100644
index 0000000..8ed9b72
--- /dev/null
+++ b/GMSH/Geometries/piece2.geo
@@ -0,0 +1,117 @@
+
+
+// Define parameters
+lc=1;
+// Cicle radius
+l=1;
+// Gap
+d=0.1;
+// Angle width
+a=1.5;
+// Step
+s=Sqrt(2)/2*d;
+// Midpoint
+m=0.5*a;
+// Frame height
+h=1;
+
+
+Point(1)={0, -(l+a), 0, lc};
+Point(2)={l, -(l+a), 0, lc};
+Point(3)={l+s, -(l+a)+s, 0, lc};
+Point(4)={l+m, -(l+m), 0, lc};
+Point(5)={l+a-s, -(l+s), 0, lc};
+Point(6)={l+a, -l, 0, lc};
+Point(7)={l+a, 0, 0, lc};
+Point(8)={l+a, l, 0, lc};
+Point(9)={l+a-s, l+s, 0, lc};
+Point(10)={l+m, l+m, 0, lc};
+Point(11)={l+s, l+a-s, 0, lc};
+Point(12)={l, l+a, 0, lc};
+Point(13)={0, l+a, 0, lc};
+Point(14)={-l, l+a, 0, lc};
+Point(15)={-(l+s), l+a-s, 0, lc};
+Point(16)={-(l+m), l+m, 0, lc};
+Point(17)={-(l+a)+s, l+s, 0, lc};
+Point(18)={-(l+a), l, 0, lc};
+Point(19)={-(l+a), 0, 0, lc};
+Point(20)={-(l+a), -l, 0, lc};
+Point(21)={-(l+a)+s, -(l+s), 0, lc};
+Point(22)={-(l+m), -(l+m), 0, lc};
+Point(23)={-(l+s), -(l+a)+s, 0, lc};
+Point(24)={-l, -(l+a), 0, lc};
+
+// Outer frame
+Point(25)={-(l+a+h), -(l+a+h), 0, lc};
+Point(26)={(l+a+h), -(l+a+h), 0, lc};
+Point(27)={(l+a+h), (l+a+h), 0, lc};
+Point(28)={-(l+a+h), (l+a+h), 0, lc};
+
+
+
+//+
+Circle(1) = {2, 1, 24};
+//+
+Circle(2) = {3, 1, 23};
+//+
+Circle(3) = {3, 4, 5};
+//+
+Circle(4) = {2, 4, 6};
+//+
+Circle(5) = {8, 7, 6};
+//+
+Circle(6) = {9, 7, 5};
+//+
+Circle(7) = {8, 10, 12};
+//+
+Circle(8) = {9, 10, 11};
+//+
+Circle(9) = {14, 13, 12};
+//+
+Circle(10) = {15, 13, 11};
+//+
+Circle(11) = {15, 16, 17};
+//+
+Circle(12) = {14, 16, 18};
+//+
+Circle(13) = {20, 19, 18};
+//+
+Circle(14) = {21, 19, 17};
+//+
+Circle(15) = {21, 22, 23};
+//+
+Circle(16) = {20, 22, 24};
+//+
+Line(17) = {25, 26};
+//+
+Line(18) = {26, 27};
+//+
+Line(19) = {27, 28};
+//+
+Line(20) = {28, 25};
+//+
+Curve Loop(1) = {14, -11, 10, -8, 6, -3, 2, -15};
+//+
+Plane Surface(1) = {1};
+//+
+Curve Loop(2) = {17, 18, 19, 20};
+//+
+Curve Loop(3) = {16, -1, 4, -5, 7, -9, 12, -13};
+//+
+Plane Surface(2) = {2, 3};
+//+
+Physical Curve("inner_boundary", 21) = {2, 15, 14, 11, 10, 8, 6, 3};
+//+
+Physical Curve("outer_boundary", 22) = {13, 12, 9, 7, 5, 4, 1, 16};
+//+
+Physical Curve("bottom_edge", 23) = {17};
+//+
+Physical Curve("right_edge", 24) = {18};
+//+
+Physical Curve("top_edge", 25) = {19};
+//+
+Physical Curve("left_edge", 26) = {20};
+//+
+Physical Surface("outer_body", 27) = {2};
+//+
+Physical Surface("inner_body", 28) = {1};
diff --git a/GMSH/Meshes/.gitattributes b/GMSH/Meshes/.gitattributes
new file mode 100644
index 0000000..a173be0
--- /dev/null
+++ b/GMSH/Meshes/.gitattributes
@@ -0,0 +1,38 @@
+contact2_p1_0_05.msh filter=lfs diff=lfs merge=lfs -text
+contact2_p2_0_1.msh filter=lfs diff=lfs merge=lfs -text
+contact10_p1_0_05.msh filter=lfs diff=lfs merge=lfs -text
+contact4_p1_0_05.msh filter=lfs diff=lfs merge=lfs -text
+contact7_p1_0_05_u.msh filter=lfs diff=lfs merge=lfs -text
+piece2_p1_0_01.msh filter=lfs diff=lfs merge=lfs -text
+westergaard_nu_p2.msh filter=lfs diff=lfs merge=lfs -text
+contact1_p1_0_01.msh filter=lfs diff=lfs merge=lfs -text
+contact8_p1_0_05.msh filter=lfs diff=lfs merge=lfs -text
+contact9_p1_0_05.msh filter=lfs diff=lfs merge=lfs -text
+contact2_p2_0_05.msh filter=lfs diff=lfs merge=lfs -text
+contact5_p1_0_005.msh filter=lfs diff=lfs merge=lfs -text
+contact6_p2_0_05.msh filter=lfs diff=lfs merge=lfs -text
+contact7_p1_0_05.msh filter=lfs diff=lfs merge=lfs -text
+westergaard1.msh filter=lfs diff=lfs merge=lfs -text
+contact1_p1_0_005.msh filter=lfs diff=lfs merge=lfs -text
+contact1_p1_0_05.msh filter=lfs diff=lfs merge=lfs -text
+contact1_p2_0_1.msh filter=lfs diff=lfs merge=lfs -text
+westergaard_nu_p1.msh filter=lfs diff=lfs merge=lfs -text
+westergaard3.msh filter=lfs diff=lfs merge=lfs -text
+contact1_p2_0_05.msh filter=lfs diff=lfs merge=lfs -text
+contact2_p1_0_01.msh filter=lfs diff=lfs merge=lfs -text
+contact5_p1_0_05.msh filter=lfs diff=lfs merge=lfs -text
+contact6_p1_0_05.msh filter=lfs diff=lfs merge=lfs -text
+westergaard2.msh filter=lfs diff=lfs merge=lfs -text
+contact2_p1_0_005.msh filter=lfs diff=lfs merge=lfs -text
+contact2_p1_0_2.msh filter=lfs diff=lfs merge=lfs -text
+contact10_p1_0_01.msh filter=lfs diff=lfs merge=lfs -text
+contact10_p2_0_05.msh filter=lfs diff=lfs merge=lfs -text
+contact1_p1_0_1.msh filter=lfs diff=lfs merge=lfs -text
+contact3_p1_0_1.msh filter=lfs diff=lfs merge=lfs -text
+piece1_p1_0_05.msh filter=lfs diff=lfs merge=lfs -text
+westergaard_fine_u_p1.msh filter=lfs diff=lfs merge=lfs -text
+westergaard_u_p1.msh filter=lfs diff=lfs merge=lfs -text
+contact11_p1_0_005.msh filter=lfs diff=lfs merge=lfs -text
+contact2_p1_0_1.msh filter=lfs diff=lfs merge=lfs -text
+contact2_p1_0_5.msh filter=lfs diff=lfs merge=lfs -text
+*.msh filter=lfs diff=lfs merge=lfs -text
diff --git a/GMSH/Meshes/contact10_p1_0_01.msh b/GMSH/Meshes/contact10_p1_0_01.msh
new file mode 100644
index 0000000..2634002
--- /dev/null
+++ b/GMSH/Meshes/contact10_p1_0_01.msh
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:50e11d8b1017c9c4d45245ce767a890683047ca6a7a1b4500d4ce3ea9b024431
+size 10670314
diff --git a/GMSH/Meshes/contact10_p1_0_05.msh b/GMSH/Meshes/contact10_p1_0_05.msh
new file mode 100644
index 0000000..52cdcb1
--- /dev/null
+++ b/GMSH/Meshes/contact10_p1_0_05.msh
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:76b20c621f1373df3b4b8a657d6ddbaa9cd1255e8432c36cb7fb8d44806fd33f
+size 388364
diff --git a/GMSH/Meshes/contact10_p2_0_05.msh b/GMSH/Meshes/contact10_p2_0_05.msh
new file mode 100644
index 0000000..ce5f812
--- /dev/null
+++ b/GMSH/Meshes/contact10_p2_0_05.msh
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:bed92536a61ac9b09ac7f482698deeeebbd64586bdfebf67abb92479668d0bde
+size 1042827
diff --git a/GMSH/Meshes/contact11_p1_0_005.msh b/GMSH/Meshes/contact11_p1_0_005.msh
new file mode 100644
index 0000000..a27d08a
--- /dev/null
+++ b/GMSH/Meshes/contact11_p1_0_005.msh
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:247d82104e8ff3f3caedb786f9ed0a56fd33eb6394c50ca98c6741364c790d17
+size 10029969
diff --git a/GMSH/Meshes/contact1_p1_0_005.msh b/GMSH/Meshes/contact1_p1_0_005.msh
new file mode 100644
index 0000000..8897337
--- /dev/null
+++ b/GMSH/Meshes/contact1_p1_0_005.msh
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:abab8c67056f6093447252a1c0b2ccbd45bfd437e82aa7bc1f88061a181f1c82
+size 9973119
diff --git a/GMSH/Meshes/contact1_p1_0_01.msh b/GMSH/Meshes/contact1_p1_0_01.msh
new file mode 100644
index 0000000..1268113
--- /dev/null
+++ b/GMSH/Meshes/contact1_p1_0_01.msh
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:fbf83f93c9d46e32af29a607ed7194cce65f0ad86a9248554cb1d3798be6efe2
+size 2771589
diff --git a/GMSH/Meshes/contact1_p1_0_05.msh b/GMSH/Meshes/contact1_p1_0_05.msh
new file mode 100644
index 0000000..39bc573
--- /dev/null
+++ b/GMSH/Meshes/contact1_p1_0_05.msh
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:d40e48d331e3083fba27d7cb5a17398f9caedb39458f2999377d28f8a22e77a0
+size 98307
diff --git a/GMSH/Meshes/contact1_p1_0_1.msh b/GMSH/Meshes/contact1_p1_0_1.msh
new file mode 100644
index 0000000..547ec38
--- /dev/null
+++ b/GMSH/Meshes/contact1_p1_0_1.msh
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:8574489749ae5e585deb0b739df33a69e44084edb16840835e362d521464323f
+size 23897
diff --git a/GMSH/Meshes/contact1_p2_0_05.msh b/GMSH/Meshes/contact1_p2_0_05.msh
new file mode 100644
index 0000000..4bf7b79
--- /dev/null
+++ b/GMSH/Meshes/contact1_p2_0_05.msh
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:0993ba1522dbe7bfd8f08b496c85005df0f0ff69111da34c501f12b0c6d99e94
+size 267632
diff --git a/GMSH/Meshes/contact1_p2_0_1.msh b/GMSH/Meshes/contact1_p2_0_1.msh
new file mode 100644
index 0000000..eb2d715
--- /dev/null
+++ b/GMSH/Meshes/contact1_p2_0_1.msh
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:fde76b0a94a34e759ec351866b17f472b477cf5e9439263971760f89b576b047
+size 62617
diff --git a/GMSH/Meshes/contact2_p1_0_005.msh b/GMSH/Meshes/contact2_p1_0_005.msh
new file mode 100644
index 0000000..4693428
--- /dev/null
+++ b/GMSH/Meshes/contact2_p1_0_005.msh
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:fe5317b255c334459d50b2be4b2d8afeb77ce2206a415ce0188c09308dc11bf6
+size 10086844
diff --git a/GMSH/Meshes/contact2_p1_0_01.msh b/GMSH/Meshes/contact2_p1_0_01.msh
new file mode 100644
index 0000000..15702d7
--- /dev/null
+++ b/GMSH/Meshes/contact2_p1_0_01.msh
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:efcbe1d9aff15fde3a2bbb7f8cb54f12e867c97b7be6a9a1896a0e6bb313b655
+size 2462546
diff --git a/GMSH/Meshes/contact2_p1_0_05.msh b/GMSH/Meshes/contact2_p1_0_05.msh
new file mode 100644
index 0000000..b841b76
--- /dev/null
+++ b/GMSH/Meshes/contact2_p1_0_05.msh
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:e2a4a1d7f879da0b64a4395216148b02a1dccd0add147dd091f9108c59ec94fa
+size 90812
diff --git a/GMSH/Meshes/contact2_p1_0_1.msh b/GMSH/Meshes/contact2_p1_0_1.msh
new file mode 100644
index 0000000..ca979f0
--- /dev/null
+++ b/GMSH/Meshes/contact2_p1_0_1.msh
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:31443bfb8dcd99be0122a9ace07a525cd2f7e2972834a5fbba465e5ffdd1faa9
+size 24081
diff --git a/GMSH/Meshes/contact2_p1_0_2.msh b/GMSH/Meshes/contact2_p1_0_2.msh
new file mode 100644
index 0000000..c896ffd
--- /dev/null
+++ b/GMSH/Meshes/contact2_p1_0_2.msh
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:26cf9e5ea97e25629c0bc3711d317bd91c65c7d8c22651550b8060013025c17a
+size 4908
diff --git a/GMSH/Meshes/contact2_p1_0_5.msh b/GMSH/Meshes/contact2_p1_0_5.msh
new file mode 100644
index 0000000..7ad8649
--- /dev/null
+++ b/GMSH/Meshes/contact2_p1_0_5.msh
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:463e47d7c79d8a542e568a1e9b62dd07272475f0afb87d85d043daf09ae86c7c
+size 1868
diff --git a/GMSH/Meshes/contact2_p2_0_05.msh b/GMSH/Meshes/contact2_p2_0_05.msh
new file mode 100644
index 0000000..3a29e76
--- /dev/null
+++ b/GMSH/Meshes/contact2_p2_0_05.msh
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:dd536a4548a5f86d24fa48f009e8d487e332a1a073f1103d3e99961229289197
+size 273908
diff --git a/GMSH/Meshes/contact2_p2_0_1.msh b/GMSH/Meshes/contact2_p2_0_1.msh
new file mode 100644
index 0000000..c098b27
--- /dev/null
+++ b/GMSH/Meshes/contact2_p2_0_1.msh
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:e97ebbc41474dbb6cf0339bfcb8313ce583e8b11a1aaeb496b2a365da729550c
+size 63148
diff --git a/GMSH/Meshes/contact3_p1_0_1.msh b/GMSH/Meshes/contact3_p1_0_1.msh
new file mode 100644
index 0000000..a6b4d14
--- /dev/null
+++ b/GMSH/Meshes/contact3_p1_0_1.msh
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:6af32288cba1c011f414b9df9cbf8df38462d5c725da0b85d218cbb52d1dfc5a
+size 23546
diff --git a/GMSH/Meshes/contact4_p1_0_05.msh b/GMSH/Meshes/contact4_p1_0_05.msh
new file mode 100644
index 0000000..92881e0
--- /dev/null
+++ b/GMSH/Meshes/contact4_p1_0_05.msh
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:f7728d933fa7f5f670e9a48f2e72310e213625efd2eb7151c83efcbc703388e5
+size 1102844
diff --git a/GMSH/Meshes/contact5_p1_0_005.msh b/GMSH/Meshes/contact5_p1_0_005.msh
new file mode 100644
index 0000000..f4b446a
--- /dev/null
+++ b/GMSH/Meshes/contact5_p1_0_005.msh
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:e03c478e615bdeebd2b13ac175dd6798fc7faa28099406f71262fe81eccf1ac0
+size 121292622
diff --git a/GMSH/Meshes/contact5_p1_0_05.msh b/GMSH/Meshes/contact5_p1_0_05.msh
new file mode 100644
index 0000000..eda0bce
--- /dev/null
+++ b/GMSH/Meshes/contact5_p1_0_05.msh
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:231f7ab78a2ba18eac09b1e97cb7a854b7f6f17d0958311d2391e8d5d0ed4a3b
+size 1102660
diff --git a/GMSH/Meshes/contact6_p1_0_05.msh b/GMSH/Meshes/contact6_p1_0_05.msh
new file mode 100644
index 0000000..dc07e3b
--- /dev/null
+++ b/GMSH/Meshes/contact6_p1_0_05.msh
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:078e5065e89a3c5cd4e621515c39ca226a31c1e2fbf6329723f9d5907d5bf518
+size 281702
diff --git a/GMSH/Meshes/contact6_p2_0_05.msh b/GMSH/Meshes/contact6_p2_0_05.msh
new file mode 100644
index 0000000..b5dbcfd
--- /dev/null
+++ b/GMSH/Meshes/contact6_p2_0_05.msh
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:0adadc0536027e76c81594233a95dd169b43cbd291410a9e58a579500c009e0e
+size 744465
diff --git a/GMSH/Meshes/contact7_p1_0_05.msh b/GMSH/Meshes/contact7_p1_0_05.msh
new file mode 100644
index 0000000..5707991
--- /dev/null
+++ b/GMSH/Meshes/contact7_p1_0_05.msh
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:568e2d925e6b11da19d9c016ceff83b52f3a9b0e3c45076ca8d8e194dbc4ce2f
+size 1698928
diff --git a/GMSH/Meshes/contact7_p1_0_05_u.msh b/GMSH/Meshes/contact7_p1_0_05_u.msh
new file mode 100644
index 0000000..dc07e3b
--- /dev/null
+++ b/GMSH/Meshes/contact7_p1_0_05_u.msh
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:078e5065e89a3c5cd4e621515c39ca226a31c1e2fbf6329723f9d5907d5bf518
+size 281702
diff --git a/GMSH/Meshes/contact8_p1_0_05.msh b/GMSH/Meshes/contact8_p1_0_05.msh
new file mode 100644
index 0000000..a4c7a51
--- /dev/null
+++ b/GMSH/Meshes/contact8_p1_0_05.msh
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:dc6f586a6b761884407b258c06a469745f7dbae4a72397ce2fa96723ba5f9e82
+size 8472318
diff --git a/GMSH/Meshes/contact9_p1_0_05.msh b/GMSH/Meshes/contact9_p1_0_05.msh
new file mode 100644
index 0000000..b0a5648
--- /dev/null
+++ b/GMSH/Meshes/contact9_p1_0_05.msh
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:da8d1954ff59acce74f5a39b23ae386956435ae63d6eaf23e437d184b3193724
+size 407994
diff --git a/GMSH/Meshes/piece1_p1_0_05.msh b/GMSH/Meshes/piece1_p1_0_05.msh
new file mode 100644
index 0000000..a850d93
--- /dev/null
+++ b/GMSH/Meshes/piece1_p1_0_05.msh
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:26dba8a82556944eb01399528298d6e2e49d233b889c29397ca257d6a1b82fa0
+size 76371
diff --git a/GMSH/Meshes/piece2_p1_0_01.msh b/GMSH/Meshes/piece2_p1_0_01.msh
new file mode 100644
index 0000000..0fe0398
--- /dev/null
+++ b/GMSH/Meshes/piece2_p1_0_01.msh
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:4a8ed759df106cb2216dfd05ed42a0a184fb10f200e3c15ddde30bcb97007fb9
+size 2451335
diff --git a/GMSH/Meshes/westergaard1.msh b/GMSH/Meshes/westergaard1.msh
new file mode 100644
index 0000000..4466a99
--- /dev/null
+++ b/GMSH/Meshes/westergaard1.msh
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:5fcb1b38682d05367a23b5af01fc249881c9672713a4cc90efe5511f924d6bd3
+size 24727414
diff --git a/GMSH/Meshes/westergaard2.msh b/GMSH/Meshes/westergaard2.msh
new file mode 100644
index 0000000..6552e95
--- /dev/null
+++ b/GMSH/Meshes/westergaard2.msh
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:d5b3a5459a6107eb802c2ceb36082e8a658d1cbdbd2d6c863d2be3e27e895aec
+size 308156
diff --git a/GMSH/Meshes/westergaard3.msh b/GMSH/Meshes/westergaard3.msh
new file mode 100644
index 0000000..42e5977
--- /dev/null
+++ b/GMSH/Meshes/westergaard3.msh
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:fbd13572b1e8f8d7403c1051cbc91743316bd0e27f98a11a01cda5c5153fe9d1
+size 221765
diff --git a/GMSH/Meshes/westergaard_fine_u_p1.msh b/GMSH/Meshes/westergaard_fine_u_p1.msh
new file mode 100644
index 0000000..8398294
--- /dev/null
+++ b/GMSH/Meshes/westergaard_fine_u_p1.msh
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:60acca444c86487317564d2ed4169ef6f4ce300cda350bef114b99a186f04d7c
+size 3281675
diff --git a/GMSH/Meshes/westergaard_nu_p1.msh b/GMSH/Meshes/westergaard_nu_p1.msh
new file mode 100644
index 0000000..ecf3eeb
--- /dev/null
+++ b/GMSH/Meshes/westergaard_nu_p1.msh
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:795227e89de761e10f3377a2c6fea026ce6fd7c2e762aeefc5ebab164dbf37a1
+size 305515
diff --git a/GMSH/Meshes/westergaard_nu_p2.msh b/GMSH/Meshes/westergaard_nu_p2.msh
new file mode 100644
index 0000000..076eb5c
--- /dev/null
+++ b/GMSH/Meshes/westergaard_nu_p2.msh
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:27ff3506846393d32594a5ff1a0041bdfa744aa9e548e0b80645d7b54f4b3f92
+size 814308
diff --git a/GMSH/Meshes/westergaard_u_p1.msh b/GMSH/Meshes/westergaard_u_p1.msh
new file mode 100644
index 0000000..6076e31
--- /dev/null
+++ b/GMSH/Meshes/westergaard_u_p1.msh
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:d9adee6dfbfa6b0a8ef13163d72ad658f0c7f73f5cd8f22bf9bb1bf52adfe9d2
+size 296567
diff --git a/README.md b/README.md
new file mode 100644
index 0000000..21e75b7
--- /dev/null
+++ b/README.md
@@ -0,0 +1,30 @@
+
+# Warning regarding MATLAB version
+
+MATLAB R2021a or later is required to run the code with full capabilities.
+
+For an older version, the code still runs but the stresses and strains cannot be computed. In this case proceed as follows:
+
+  1) In the main file “internodesx”, (with x=1,2,3,…) in the function call to “setNumericalParameters(Mesh, Data, PostProcessing,…” remove the last argument-value pair “‘quantity’, {‘stress’, ‘strain’}”
+  2) In the main file “internodesx”, in the function call to “plotSolution(Mesh, Data, Solution,…” remove the last argument-value pair “‘stress’, ‘sigma_yy’”
+
+# List of folders
+
+- INTERNODES_small: Code for small size problems. Explicit assemblage of the INTERNODES matrix and solution to the linear systems using backslash (direct method).
+
+- INTERNODES_medium: Code for medium to large 2D applications. The linear systems are solved using right preconditioned GMRES. The algorithm relies on a single complete Cholesky factorization.
+
+- INTERNODES_large: Code for very large 2D and potentially 3D applications. The algorithm implements inexact solves with the preconditioning matrix based on an incomplete Cholesky factorization coupled with inner iterations. **Beware: the incomplete factorization might fail !**
+
+- Westergaard: Folder used to run the experiments for the Westergaard problem.
+
+# List of main files:
+
+- internodes1: Classical Hertzian contact problem with Dirichlet boundary conditions applied to both bodies.
+- internodes2: Classical Hertzian contact problem with Dirichlet boundary conditions applied to the lower body and Neumann boundary conditions applied to the upper body.
+- internodes3: Contact problem between two rectangular blocks with Dirichlet boundary conditions applied to both bodies.
+- internodes4: Elastic contact between a sphere and a half space. Dirichlet boundary conditions are applied to both bodies.
+- internodes5: Structure with asperities and multiple contact points. Dirichlet boundary conditions are applied to the lower body and Neumann boundary conditions are applied to the upper body.
+- conforming1: Rectangular blocks with conforming meshes. Dirichlet boundary conditions are applied to both bodies.
+- conforming2: Rectangular blocks with conforming meshes. Dirichlet boundary conditions are applied to the lower body and Neumann boundary conditions are applied to the upper body.
+- westergaard: Comparison of INTERNODES with the Westergaard solution.
\ No newline at end of file