function CNTELM(CNT,XYZ,CDISP,NUMNP,DYNF)
%******************************************************************************
% COMPUTE CONTACT STIFFNESS & FORCE AND ASSEMBLE TO GLOBAL STIFFNESS & FORCE
%******************************************************************************
%%
 global LTAN DISPTD FORCE GKF			                         % Global variables
 NCNT=size(CNT.CNODE,2); NTARG=size(CNT.LEC,1);          % No. of contact pairs
 NDOF=size(XYZ,2); NETARG=2^(NDOF-1);                              % Dimensions
 ELXY=zeros(NDOF,NTARG+1);                       % Coordinates of contact pairs
 ELXYP=zeros(NDOF,NTARG+1);                      % Coordinates at previous time
 for ICNT=1:NCNT                                       % Loop for contact nodes
  IDOF=NDOF*(CNT.CNODE(ICNT)-1)+(1:NDOF);                   % Contact node DOFs
  ELXY(:,1)=XYZ(CNT.CNODE(ICNT),:)+DISPTD(IDOF)';% Coordinates of contact nodes
  ELXYP(:,1)=XYZ(CNT.CNODE(ICNT),:)+CDISP(IDOF)';% Coordinates at previous time
  for ITARG=1:NTARG                                  % Loop for target segments
   if CNT.LEC(ITARG,1)>NUMNP                           % Target nodes are rigid
    for i=1:NETARG               % Loop for all nodes in current target segment
     ELXY(:,i+1)=XYZ(CNT.LEC(ITARG,i),:);         % Coordinates of target nodes
     ELXYP(:,i+1)=CNT.XYZP(CNT.LEC(ITARG,i)-NUMNP,:); % Coord. at previous time
    end
   else                                             % Target nodes are flexible
    for i=1:NETARG               % Loop for all nodes in current target segment
     JDOF=NDOF*(CNT.LEC(ITARG,i)-1)+(1:NDOF);    % DOF of flexible target nodes
     ELXY(:,i+1)=XYZ(CNT.LEC(ITARG,i),:)+DISPTD(JDOF)';   % Current coordinates
     ELXYP(:,i+1)=XYZ(CNT.LEC(ITARG,i),:)+CDISP(JDOF)';  % Previous coordinates
     IDOF=[IDOF JDOF];                     % Accumulated DOFs of flexible nodes
    end
   end
   NFLEX=size(IDOF,2);                    % No of flexible DOFs in contact pair
   if NDOF == 2
    [F,KC]=CNTELM2D(ELXY,ELXYP,CNT.PARM);      % 2D contact force and stiffness
   else
    [F,KC]=CNTELM3D(ELXY,ELXYP,CNT.PARM);      % 3D contact force and stiffness
   end
   if ~isempty(F)                    % F & KC are non-empty when contact occurs
    FORCE(IDOF)=FORCE(IDOF)+F(1:NFLEX);                % Assemble contact force
    if LTAN, GKF(IDOF,IDOF)=GKF(IDOF,IDOF)+KC(1:NFLEX,1:NFLEX)*DYNF; end % stiffness
    break;   % Stop checking other target segments. One target per contact node
   end
   IDOF=NDOF*(CNT.CNODE(ICNT)-1)+(1:NDOF);                  % Contact node DOFs
  end
 end
end
%
function [F,KC]=CNTELM2D(ELXY,ELXYP,PARM)
%******************************************************************************
% COMPUTE CONTACT STIFFNESS AND FORCE FOR 2D CONTACT PAIR
%******************************************************************************
%%
 F=[]; KC=[];                                     % Return null when no contact
 XT=ELXY(:,3)-ELXY(:,2); XLEN=norm(XT);                % Tangent target segment
 if XLEN < 1E-6, return; end               % Error in target segment definition
 XTP=ELXYP(:,3)-ELXYP(:,2); XLENP  = norm(XTP);      % Tangent at previous time
 XT=XT/XLEN;                                              % Unit tangent vector
 XTP=XTP/XLENP;                                 % Unit tangent at previous time
 XN=[-XT(2); XT(1)];                                       % Unit normal vector
 GN=(ELXY(:,1)-ELXY(:,2))'*XN;                     % Gap b/w contact and target
 ALPHA=(ELXY(:,1)-ELXY(:,2))'*XT/XLEN;                  % Parametric coordinate
 ALPHA0=((ELXYP(:,1)-ELXYP(:,2))'*XTP)/XLENP;  % Param. coord. at previous time
 if (ALPHA>1.05)||(ALPHA<-0.05)||(GN>=0)||(GN<=-XLEN), return; end
 XLAMBN=-PARM(1)*GN;                                     % Normal contact force
 GT=(ALPHA-ALPHA0)*XLENP;                                     % Tangential slip
 XLAMBT=-PARM(2)*GT;                         % Tangential force for stick cond.
 FRTOL=PARM(3)*XLAMBN;                     % Criterion for stick/slip condition
 NN=[XN; -(1-ALPHA)*XN; -ALPHA*XN];                   % Augmented normal vector
 TT=[XT; -(1-ALPHA)*XT; -ALPHA*XT];               % Augmented tangential vector
 if abs(XLAMBT)>FRTOL                                          % Slip condition
  XLAMBT=-FRTOL*sign(GT);                 % Tangential force for slip condition
  KC=PARM(1)*(NN*NN')-PARM(3)*PARM(1)*sign(GT)*(TT*NN');    % Contact stiffness
 else                                                         % Stick condition
  KC=PARM(1)*(NN*NN')+PARM(2)*(TT*TT');                     % Contact stiffness
 end                                                       % End of slip branch
 F= XLAMBN*NN+XLAMBT*TT;                                        % Contact force
end
%
function [F,KC]=CNTELM3D(ELXY,ELXYP,PARM)
%******************************************************************************
% COMPUTE CONTACT STIFFNESS AND FORCE FOR 3D CONTACT PAIR
%******************************************************************************
%%
 F=[]; KC=[];                                     % Return null when no contact
 [T1,T2,XC,XS,~]=CUTL(0,0,ELXY);        % Two tangent vectors and contact nodes
 XN=cross(T1,T2); XN=XN/norm(XN);            % Unit normal vector at the center
 XI=(XS'*T1)/(norm(T1)^2);                    % Parametric coord. along T1 dir.
 ETA=(XS'*T2)/(norm(T2)^2);                   % Parametric coord. along T2 dir.
 GN=XN'*XS;                                            % Gap in the normal dir.
 XX=(ELXY(:,1)-ELXY(:,2)+ELXY(:,3)-ELXY(:,4))/4;         % 2nd-derivative of Xc
 if((abs(XI)>1.2)||(abs(ETA)>1.2)||GN>0.1), return; end     % No contact occurs
 for ICOUNT=1:20                           % NR iteration to find contact point
  [T1,T2,XC,XS,AN] = CUTL(XI,ETA,ELXY); % Two tangent vectors and contact nodes
  A=[-T1'*T1, XS'*XX-T2'*T1; XS'*XX-T2'*T1, -T2'*T2];          % Jacobian of NR
  B=[-XS'*T1; -XS'*T2];                                        % Residual of NR
  DXI=A\B;                                 % Incremental parametric coordinates
  XI=XI+DXI(1); ETA=ETA+DXI(2);                 % Update parametric coordinates
  if(norm(DXI)<1.E-6), break; end                      % Converged NR iteration
 end                                                    % End of local N-R loop
 XN=cross(T1,T2); XN=XN/norm(XN);         % Unit normal vector at contact point
 GN=XN'*XS;                                            % Gap in the normal dir.
 if((abs(XI)>1.05)||(abs(ETA)>1.05)||GN>0), return; end     % No contact occurs
 XX=(ELXYP(:,1)-ELXYP(:,2)+ELXYP(:,3)-ELXYP(:,4))/4;     % 2nd-derivative of Xc
 XIP=XI; ETAP=ETA;                            % Previous parametric coordinates
 for ICOUNT=1:20                           % NR iteration to find contact point
  [T1P,T2P,~,XSP,~] = CUTL(XIP,ETAP,ELXYP);   % Tangent vectors & contact nodes
  A=[-T1P'*T1P, XSP'*XX-T2P'*T1P; XSP'*XX-T2P'*T1P, -T2P'*T2P];% Jacobian of NR
  B=[-XSP'*T1P; -XSP'*T2P];                                    % Residual of NR
  DXIP=A\B;                                % Incremental parametric coordinates
  XIP=XIP+DXIP(1); ETAP=ETAP+DXIP(2);           % Update parametric coordinates
  if(norm(DXIP)<1.E-6), break; end                     % Converged NR iteration
 end                                                    % End of local N-R loop
 [~,~,XCP,~,~] = CUTL(XIP,ETAP,ELXY);                  % Previous contact point
 GT=XC-XCP; XT=GT/norm(GT);                     % Tangential slip & unit vector
 if norm(GT)<1E-10, XT=(T1+T2)/norm(T1+T2); end % Tangent vector with zero slip
 NN=XN*AN; NN=reshape(NN,[],1);                       % Augmented normal vector
 TT=XT*AN; TT=reshape(TT,[],1);                   % Augmented tangential vector
 TT1=T1/norm(T1)*AN; TT1=reshape(TT1,[],1);        % Augmented tangent 1 vector
 TT2=T2/norm(T2)*AN; TT2=reshape(TT2,[],1);        % Augmented tangent 2 vector
 XLAMBN=-PARM(1)*GN;                                     % Normal contact force
 XLAMBT=-PARM(2)*norm(GT);                   % Tangential force for stick cond.
 if abs(XLAMBT)>PARM(3)*XLAMBN                                 % Slip condition
  XLAMBT=-PARM(3)*XLAMBN;                 % Tangential force for slip condition
  KC=PARM(1)*(NN*NN')-PARM(3)*PARM(1)*(TT*NN');             % Contact stiffness
 else                                                         % Stick condition
  KC=PARM(1)*(NN*NN')+PARM(2)*(TT1*TT1'+TT2*TT2');          % Contact stiffness
 end                                          % End of if XLAMBT>PARM(3)*XLAMBN
 F= XLAMBN*NN+XLAMBT*TT;                                        % Contact force
end
%
function [T1,T2,XC,XS,AN] = CUTL(XI,ETA,ELXY)
%******************************************************************************
% COMPUTE COORD. OF CENTEROID AND TWO TANGENT VECTORS
%******************************************************************************
%%
 XN=[-1 1 1 -1; -1 -1 1 1];                 % Parametric coordinates of 4 nodes
 T1=zeros(3,1); T2=T1; XC=T1; XS=T1;                    % Definition of vectors
 for J = 1:3                                             % Loop for coord. dir.
  T1(J)=sum(XN(1,:).*(1+ETA*XN(2,:)).*ELXY(J,2:5)./4);       % Tangent 1 vector
  T2(J)=sum(XN(2,:).*(1+XI *XN(1,:)).*ELXY(J,2:5)./4);       % Tangent 2 vector
  XC(J)=sum((1+XI*XN(1,:)).*(1+ETA*XN(2,:)).*ELXY(J,2:5)./4);   % Contact point
  XS(J)=ELXY(J,1)-XC(J);                       % Relative contact node location
 end                                                  % End of coord. dir. loop
 AN=[1 -(1+XI*XN(1,:)).*(1+ETA*XN(2,:))/4];   % Augmented shape function vector
end