%Function: incorporate contact constraint to stiffness matrix based on
%          penaly method
function [K_Contact RC_force] = Contact_Formualtion(P1, P2, E1,E2,Penalty, dis_1, dis_2)    
    K_Contact=zeros(2*(size(P1,1)+size(P2,1))); %initialize the Output;    
    SD=2;%空间维度
    %slave接触面的节点及对应的全局索引
   [ P_s, patch_s, Global_DOF_ID_s]=  Contact_Geometry(0, dis_1, dis_2);  
   [ P_m, patch_m, Global_DOF_ID_m]=  Contact_Geometry( 1, dis_1, dis_2);  
    RC_force=zeros(size(P_s,1)+size(P_m,1),2);
	nnd_s=size(P_s,1);%从面节点数
    ne_s=nnd_s-1;%从面单元数（segment数量）
    nnd_m=size(P_m,1);%主面节点数
    ne_m=nnd_m-1;%主面单元数（segment数量）
    %对接触面进行搜索得到node-to-segement接触对
    for n_i=1:nnd_s %从面节点进行循环node
          ID_s=Global_DOF_ID_s(2*n_i-1:2*n_i);  
          for e_i=1:ne_m %主面segement进行循环
            ID_m1=  Global_DOF_ID_m(2*e_i-1:2*e_i);
            ID_m2=  Global_DOF_ID_m(2*(e_i+1)-1:2*(e_i+1));
            iDof=[ID_s,ID_m1,ID_m2];
            xs=P_s(n_i,1);
            ys=P_s(n_i,2);
            x1=P_m(e_i,1);
            x2=P_m(e_i+1,1);
            y1=P_m(e_i,2);
            y2=P_m(e_i+1,2);
            ELXY=[xs,x1,x2;ys,y1,y2];            
            [FORCE, STIFF] = cntelm2d(Penalty, ELXY);%返回接触刚度矩阵和接触力
         
            if isempty(STIFF)
                continue;
            end            
            for row=1:size(STIFF,1)
                for col=1:size(STIFF,2)
                    r=iDof(row);
                    c=iDof(col);                 
                    K_Contact(r,c)=K_Contact(r,c)+STIFF(row,col);
                end
            end
             RC_force(n_i,:)=FORCE(1:2);
             RC_force(5+e_i,:)=FORCE(3:4);
             RC_force(5+e_i+1,:)=FORCE(5:6);
          end
    end   
 end