clc; clear
syms L1 L2
% 插值函数
P1 = [-1,1,1,-1; -1,-1,1,1];
P2 = [0,1,0,-1; -1,0,1,0];
N4 = 1/4*(1+P1(1,1:4)*L1).*(1+P1(2,1:4)*L2);

PP = [P1, P2];
N4fun = matlabFunction(N4, 'vars', {'L1', 'L2'});
%      for i = 1:size(PP,2)-4 % 确认插值函数的δ性
%         N4fun(PP(1,i), PP(2,i))
%      end
% 转化到L1和L2坐标
dN4dL1 = diff(N4, L1);
dN4dL2 = diff(N4, L2);
dN4dL  = [dN4dL1; dN4dL2];
dN4dL  = matlabFunction(dN4dL, 'vars', {'L1', 'L2'});
ndiv   = 1200;
[Nodes, Elements] = c2d4([1,1],[ndiv, ndiv]);
[W,HP] = GaussianPoints2D(2); % 获取高斯积分权系数和积分点值,4个点即可

NN    = size(Nodes,   1);  % 结点个数
NE    = size(Elements,1);  % 单元个数
DOF   = 3;                 % 单个结点自由度个数
ENN   = size(Elements,2);  % 单个单元结点个数
NT    = NN*DOF;            % 总的自由度个数
NNE   = (ENN*DOF)^2;       % 单个单元刚度矩阵大小
K_II  = zeros(NNE*NE,1);   % 稀疏矩阵行标记
K_JJ  = zeros(NNE*NE,1);   % 稀疏矩阵列标记
K_VAL = zeros(NNE*NE,1);   % 稀疏矩阵对应的值
M_VAL = zeros(NNE*NE,1);   % 稀疏矩阵对应的值
C_VAL = zeros(NNE*NE,1);   % 稀疏矩阵对应的值
Areas = zeros(NE,1);       % 每个单元的体积

dN4dLk_Arr = cell(size(W));
N4_Arr     = cell(size(W));
for k = 1:length(W)
    dN4dLk_Arr{k} = dN4dL(HP(k,1), HP(k,2)); % 计算每个积分点处的形函数导数值
    N4_Arr{k}     = N4fun(HP(k,1), HP(k,2));
end
% 对每个单元进行积分计算刚度矩阵稀疏形态
thickness     = 1;
density       = 1;
micro_inertia = 1;
emod = 1;
pr   = 0.2;
Lambda = pr*emod/(1-2*pr)/(1+pr);
G = emod/2/(1+pr);
coupling_number = 0.5;
Gc = coupling_number^2/(1-coupling_number^2)*G;
lc = 1;
De = [
    Lambda+2*G,      Lambda,     0,     0,          0,         0;
    Lambda ,     Lambda+2*G,     0,     0,          0,         0;
    0,                    0,  G+Gc,  G-Gc,          0,         0;
    0,                    0,  G-Gc,  G+Gc,          0,         0;
    0,                    0,     0,     0,   2*G*lc^2,         0;
    0,                    0,     0,     0,          0,  2*G*lc^2;
    ];
Rho_e = diag([density, density, density*micro_inertia]);

for i = 1:1:size(Elements,1)
    Me = zeros(DOF*ENN, DOF*ENN);
    Ke = zeros(DOF*ENN, DOF*ENN);
    enode = Nodes(Elements(i,:),:);
    Areas(i) = 0;
    B  = zeros(6, DOF*ENN);
    Bm = zeros(3, DOF*ENN);
    for k = 1:length(W)
        dN4dLk = dN4dLk_Arr{k};
        N4k = N4_Arr{k};
        J4 = dN4dLk*enode;
        detJ4=J4(2,2)*J4(1,1)-J4(2,1)*J4(1,2);
        invJ4=[J4(2,2),-J4(1,2);-J4(2,1),J4(1,1)]/detJ4;
        dN4dX=invJ4*dN4dLk;
        
        Bm(1,1:DOF:end) = N4k;
        Bm(2,2:DOF:end) = N4k;
        Bm(3,3:DOF:end) = N4k;
        B(1,1:DOF:end) = dN4dX(1,:);
        B(2,2:DOF:end) = dN4dX(2,:);
        B(3,2:DOF:end) = dN4dX(1,:);
        B(3,3:DOF:end) = -N4k;
        B(4,1:DOF:end) = dN4dX(2,:);
        B(4,3:DOF:end) = +N4k;
        B(5,3:DOF:end) = dN4dX(1,:);
        B(6,3:DOF:end) = dN4dX(2,:);
        
        Me = Me + W(k)*(Bm'*Rho_e*Bm)*thickness*(detJ4);
        Ke = Ke + W(k)*(B'*De*B)*thickness*(detJ4);
        Areas(i) = Areas(i) + W(k)*(detJ4);
    end
    PMatrix=repmat(DOF*(Elements(i,:)-1),DOF,1)+repmat((1:1:DOF)',1,ENN);
    GPS=reshape(PMatrix,DOF*ENN,1);
    
    [JJ, II] = meshgrid(GPS);
    POS = (i-1)*NNE+1:i*NNE;
    K_II(POS,1)  = II(:);
    K_JJ(POS,1)  = JJ(:);
    K_VAL(POS,1) = Ke(:);
    M_VAL(POS,1) = Me(:);
end
F = zeros(NT, 1);
K = sparse(K_II,K_JJ,K_VAL, NT, NT);       % 合成总体刚度矩阵
M = sparse(K_II,K_JJ,M_VAL, NT, NT);       % 合成总体质量矩阵

pset1 = find(Nodes(:,2)<=0);                                % downNode
pset2 = find(Nodes(:,1)<=0 & Nodes(:,2)>0 & Nodes(:,2)<1);    % leftNode
pset3 = find(Nodes(:,2)>=1);                                % upNode
pset4 = find(Nodes(:,1)>=1 & Nodes(:,2)>0 & Nodes(:,2)<1);    % rightNode
Nset  = Elements(:,5:end);
Nset  = unique(Nset(:));
DOFs  = setdiff(1:NT,Nset*3);
uc = [
    pset1, 1*ones(size(pset1)), zeros(size(pset1));
    pset1, 2*ones(size(pset1)), zeros(size(pset1));
    pset1, 3*ones(size(pset1)), zeros(size(pset1));
    pset3, 1*ones(size(pset3)), 0.1*ones(size(pset3));
    pset3, 2*ones(size(pset3)), 0*ones(size(pset3));
    pset3, 3*ones(size(pset3)), 0*ones(size(pset3));
    ];
DOFu = 1:NT;
DOFc =  DOF*(uc(:,1)-1)+uc(:,2);
DOFu(DOFc) = [];
U = zeros(size(F));
U(DOFc) = uc(:,3);
U(DOFu) = K(DOFu, DOFu) \ (F(DOFu,1)-K(DOFu, DOFc)*U(DOFc));


U1 = U(1:DOF:NT,1);
U2 = U(2:DOF:NT,1);
W3 = U(3:DOF:NT,1);

data.U1 = reshape(U1, [ndiv, ndiv]+1);
data.U2 = reshape(U2, [ndiv, ndiv]+1);
data.W3 = reshape(W3, [ndiv, ndiv]+1);
data.ndiv = ndiv+1;
data.maxdx = 1/ndiv;
save FEMdata data

figure(100); clf
patch('Faces',Elements(:,[1,2,3,4]),'Vertices',Nodes,'FaceVertexCData',U1,'FaceColor','interp', 'edgecolor', 'none');
axis equal
colormap(jet(20)); colorbar
title('U_1')

figure(200); clf
patch('Faces',Elements(:,[1,2,3,4]),'Vertices',Nodes,'FaceVertexCData',U2,'FaceColor','interp', 'edgecolor', 'none');
axis equal
colormap(jet(20)); colorbar
title('U_2')

figure(300); clf
patch('Faces',Elements(:,[1,2,3,4]),'Vertices',Nodes,'FaceVertexCData',W3,'FaceColor','interp', 'edgecolor', 'none');
axis equal
colormap(jet(20)); colorbar
title('\Omega_3')

fprintf('max(U2)=%f\n', max(U2));
fprintf('min(Omega3) = %f\n', min(W3));

% stress and strain
% [elementStrain, elementStrain] = calculateStrainStress(U, Element, Node, De);
GaussianPointStress = zeros(numel(W), 6, NE);
GaussianPointStrain = zeros(numel(W), 6, NE);
nodeStress = zeros(NN, 6);
nodeStrain = zeros(NN, 6);
nodeCount = zeros(NN,1);
for i = 1:1:size(Elements,1)
    PMatrix=repmat(DOF*(Elements(i,:)-1),DOF,1)+repmat((1:1:DOF)',1,ENN);
    GPS=reshape(PMatrix,DOF*ENN,1);
    eU = U(GPS);
    B  = zeros(6, DOF*ENN);
    for k = 1:length(W)
        dN4dLk = dN4dLk_Arr{k};
        N4k = N4_Arr{k};
        J4 = dN4dLk*enode;
        detJ4=J4(2,2)*J4(1,1)-J4(2,1)*J4(1,2);
        invJ4=[J4(2,2),-J4(1,2);-J4(2,1),J4(1,1)]/detJ4;
        dN4dX=invJ4*dN4dLk;
        B(1,1:DOF:end) = dN4dX(1,:);
        B(2,2:DOF:end) = dN4dX(2,:);
        B(3,2:DOF:end) = dN4dX(1,:);
        B(3,3:DOF:end) = -N4k;
        B(4,1:DOF:end) = dN4dX(2,:);
        B(4,3:DOF:end) = +N4k;
        B(5,3:DOF:end) = dN4dX(1,:);
        B(6,3:DOF:end) = dN4dX(2,:);
        strain = B*eU;
        stress = De*strain;
        GaussianPointStress(k,:,i) = stress(:);
        GaussianPointStrain(k,:,i) = strain(:);
    end
    a = 1 + sqrt(3)/2;
    b = -1/2;
    c = 1 + sqrt(3)/2;
    transMat = [a,b,b,c; b,a,c,b; b,c,a,b; c,b,b,a];
    enstrn = transMat*GaussianPointStrain(:,:,i);
    enstrs = transMat*GaussianPointStress(:,:,i);
    nodeStrain(Elements(i,1:4), :) = nodeStrain(Elements(i,1:4), :) + enstrn;
    nodeStress(Elements(i,1:4), :) = nodeStress(Elements(i,1:4), :) + enstrs;
    nodeCount(Elements(i,1:4)) = nodeCount(Elements(i,1:4)) + 1;
end
nodeStress = nodeStress./repmat(nodeCount,1,6); % mean stress
nodeStrain = nodeStrain./repmat(nodeCount,1,6);

eps = 1e-3;
figure(400); clf
patch('Faces',Elements(:,[1,2,3,4]),'Vertices',Nodes,'FaceVertexCData',nodeStress(:,1),'FaceColor','interp', 'edgecolor', 'none');
axis equal
colormap(jet(13)); colorbar
title('$\sigma_{11}$', 'interpreter', 'latex', 'fontsize', 16, 'fontname', 'times new roman')
xlabel('$x$' , 'interpreter', 'latex', 'fontsize', 16, 'fontname', 'times new roman')
ylabel('$y$' , 'interpreter', 'latex', 'fontsize', 16, 'fontname', 'times new roman')
set(gca, 'fontsize', 16, 'fontname', 'times new roman', 'xminortick', 'on', 'yminortick', 'on')
axis([-eps,1+eps,-eps,1+eps]); box on; 

eps = 1e-3;
figure(401); clf
patch('Faces',Elements(:,[1,2,3,4]),'Vertices',Nodes,'FaceVertexCData',nodeStress(:,2),'FaceColor','interp', 'edgecolor', 'none');
axis equal
colormap(jet(13)); colorbar
title('$\sigma_{22}$', 'interpreter', 'latex', 'fontsize', 16, 'fontname', 'times new roman')
xlabel('$x$' , 'interpreter', 'latex', 'fontsize', 16, 'fontname', 'times new roman')
ylabel('$y$' , 'interpreter', 'latex', 'fontsize', 16, 'fontname', 'times new roman')
set(gca, 'fontsize', 16, 'fontname', 'times new roman', 'xminortick', 'on', 'yminortick', 'on')
axis([-eps,1+eps,-eps,1+eps]); box on; 


eps = 1e-3;
figure(402); clf
patch('Faces',Elements(:,[1,2,3,4]),'Vertices',Nodes,'FaceVertexCData',nodeStress(:,3),'FaceColor','interp', 'edgecolor', 'none');
axis equal
colormap(jet(13)); colorbar
title('$\sigma_{12}$', 'interpreter', 'latex', 'fontsize', 16, 'fontname', 'times new roman')
xlabel('$x$' , 'interpreter', 'latex', 'fontsize', 16, 'fontname', 'times new roman')
ylabel('$y$' , 'interpreter', 'latex', 'fontsize', 16, 'fontname', 'times new roman')
set(gca, 'fontsize', 16, 'fontname', 'times new roman', 'xminortick', 'on', 'yminortick', 'on')
axis([-eps,1+eps,-eps,1+eps]); box on; 

eps = 1e-3;
figure(403); clf
patch('Faces',Elements(:,[1,2,3,4]),'Vertices',Nodes,'FaceVertexCData',nodeStress(:,4),'FaceColor','interp', 'edgecolor', 'none');
axis equal
colormap(jet(13)); colorbar
title('$\sigma_{21}$', 'interpreter', 'latex', 'fontsize', 16, 'fontname', 'times new roman')
xlabel('$x$' , 'interpreter', 'latex', 'fontsize', 16, 'fontname', 'times new roman')
ylabel('$y$' , 'interpreter', 'latex', 'fontsize', 16, 'fontname', 'times new roman')
set(gca, 'fontsize', 16, 'fontname', 'times new roman', 'xminortick', 'on', 'yminortick', 'on')
axis([-eps,1+eps,-eps,1+eps]); box on; 


eps = 1e-3;
figure(404); clf
patch('Faces',Elements(:,[1,2,3,4]),'Vertices',Nodes,'FaceVertexCData',nodeStress(:,5),'FaceColor','interp', 'edgecolor', 'none');
axis equal
colormap(jet(13)); colorbar
title('$\it{m}_{\rm{13}}$', 'interpreter', 'latex', 'fontsize', 16, 'fontname', 'times new roman')
xlabel('$x$' , 'interpreter', 'latex', 'fontsize', 16, 'fontname', 'times new roman')
ylabel('$y$' , 'interpreter', 'latex', 'fontsize', 16, 'fontname', 'times new roman')
set(gca, 'fontsize', 16, 'fontname', 'times new roman', 'xminortick', 'on', 'yminortick', 'on')
axis([-eps,1+eps,-eps,1+eps]); box on; 


eps = 1e-3;
figure(405); clf
patch('Faces',Elements(:,[1,2,3,4]),'Vertices',Nodes,'FaceVertexCData',nodeStress(:,6),'FaceColor','interp', 'edgecolor', 'none');
axis equal
colormap(jet(13)); colorbar
title('$\it{m}_{\rm{23}}$', 'interpreter', 'latex', 'fontsize', 16, 'fontname', 'times new roman')
xlabel('$x$' , 'interpreter', 'latex', 'fontsize', 16, 'fontname', 'times new roman')
ylabel('$y$' , 'interpreter', 'latex', 'fontsize', 16, 'fontname', 'times new roman')
set(gca, 'fontsize', 16, 'fontname', 'times new roman', 'xminortick', 'on', 'yminortick', 'on')
axis([-eps,1+eps,-eps,1+eps]); box on; 

% 400x400
% max(U2)=0.139300999439494
% min(Omega3) =  -0.050546321408304
% 200x200
% max(U2) = 0.139299;
% min(Omega3) = -0.050546;
% 100x100
% max(U2)=0.139294
% min(Omega3) = -0.050545
% 600x600
% 0.139301399336989
% -0.050546342173098