clc; clear
% 插值函数
syms L1 L2 L3
L10 = [-1, 1, 1, -1, -1, 1, 1, -1, ...
    0, 1, 0, -1, 0, 1, 0, -1, -1, 1, 1, -1];
L20 = [-1, -1, 1, 1, -1, -1, 1, 1, ...
    -1, 0, 1, 0, -1, 0, 1, 0, -1, -1, 1, 1];
L30 = [-1, -1, -1, -1, 1, 1, 1, 1, ...
    -1, -1, -1, -1, 1, 1, 1, 1, 0, 0, 0, 0];
p0 = 1:8;
p1 = 17:20;
p2 = [ 9, 11, 15, 13];
p3 = [12, 10, 14, 16];
syms N [1,20]
N(p0)  = 1/8*((1+L10(p0)*L1).*(1+L20(p0)*L2).*(1+L30(p0)*L3)).*...
    (L10(p0)*L1+L20(p0)*L2+L30(p0)*L3-2);
N(p1) = 1/4*(1-L3^2)*(1+L10(p1)*L1).*(1+L20(p1)*L2);
N(p2) = 1/4*(1-L1^2)*(1+L20(p2)*L2).*(1+L30(p2)*L3);
N(p3) = 1/4*(1-L2^2)*(1+L30(p3)*L3).*(1+L10(p3)*L1);

Nfun = matlabFunction(N, 'vars', {'L1', 'L2', 'L3'});
% for i = 1:20 % 确认插值函数的δ性
%     Nfun(L10(i), L20(i), L30(i))
% end
% 转化到L1和L2坐标
dNdL1 = diff(N, L1);
dNdL2 = diff(N, L2);
dNdL3 = diff(N, L3);
dNdL  = [dNdL1; dNdL2; dNdL3];
dNdL  = matlabFunction(dNdL, 'vars', {'L1', 'L2', 'L3'});

% syms a11 a12 a13 a21 a22 a23 a31 a32 a33
% A = [a11, a12, a13; a21, a22, a23; a31, a32, a33];
% inv(A)*det(A)
% [ a22*a33 - a23*a32, a13*a32 - a12*a33, a12*a23 - a13*a22]
% [ a23*a31 - a21*a33, a11*a33 - a13*a31, a13*a21 - a11*a23]
% [ a21*a32 - a22*a31, a12*a31 - a11*a32, a11*a22 - a12*a21]

ndiv   = 10;
[Nodes, Elements] = c3d20([1,1,1],[ndiv, ndiv, ndiv]);
[W,HP] = GaussianPoints3D(2); % 获取高斯积分权系数和积分点值,4个点即可

NN    = size(Nodes,   1);  % 结点个数
NE    = size(Elements,1);  % 单元个数
DOF   = 6;                 % 单个结点自由度个数
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);       % 每个单元的体积
dNdLk_Arr = cell(size(W));
N_Arr     = cell(size(W));
for k = 1:length(W)
    dNdLk_Arr{k} = dNdL(HP(k,1), HP(k,2), HP(k,3)); % 计算每个积分点处的形函数导数值
    N_Arr{k}     = Nfun(HP(k,1), HP(k,2), HP(k,3));
end
% 对每个单元进行积分计算刚度矩阵稀疏形态
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;
Ce = [
    Lambda+2*G,      Lambda,      Lambda,     0,     0,          0,         0,      0,      0;
    Lambda ,     Lambda+2*G,      Lambda,     0,     0,          0,         0,      0,      0;
    Lambda ,         Lambda,  Lambda+2*G,     0,     0,          0,         0,      0,      0;
    0,                    0,           0,  G+Gc,  G-Gc,          0,         0,      0,      0;
    0,                    0,           0,  G-Gc,  G+Gc,          0,         0,      0,      0;
    0,                    0,           0,     0,     0,       G+Gc,      G-Gc,      0,      0;
    0,                    0,           0,     0,     0,       G-Gc,      G+Gc,      0,      0;
    0,                    0,           0,     0,     0,          0,         0,   G+Gc,   G-Gc;
    0,                    0,           0,     0,     0,          0,         0,   G-Gc,   G+Gc;
    ];
alpha = 1;
beta  = 1;
gamma = 2;
G_g   = (beta + gamma)/2;
G_f   = (gamma - beta)/2;
De = [
    alpha+2*G_g,          alpha,              alpha,        0,        0,          0,         0,         0,         0;
    alpha,          alpha+2*G_g,              alpha,        0,        0,          0,         0,         0,         0;
    alpha,                alpha,        alpha+2*G_g,        0,        0,          0,         0,         0,         0;
    0,                        0,                  0,  G_g+G_f,  G_g-G_f,          0,         0,         0,         0;
    0,                        0,                  0,  G_g-G_f,  G_g+G_f,          0,         0,         0,         0;
    0,                        0,                  0,        0,        0,    G_g+G_f,   G_g-G_f,         0,         0;
    0,                        0,                  0,        0,        0,    G_g-G_f,   G_g+G_f,         0,         0;
    0,                        0,                  0,        0,        0,          0,         0,   G_g+G_f,   G_g-G_f;
    0,                        0,                  0,        0,        0,          0,         0,   G_g-G_f,   G_g+G_f;
    ];
D = [Ce, zeros(size(Ce)); zeros(size(De)), De];
Rho_e = diag(density*[1, 1, 1, micro_inertia*[1,1,1]]);

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(18, DOF*ENN);
    Bm = zeros(6,  DOF*ENN);
    for k = 1:length(W)
        dNdLk = dNdLk_Arr{k};
        Nk = N_Arr{k};
        J = dNdLk*enode;
        detJ = ...
            J(1,1)*J(2,2)*J(3,3) - ...
            J(1,1)*J(2,3)*J(3,2) - ...
            J(1,2)*J(2,1)*J(3,3) + ...
            J(1,2)*J(2,3)*J(3,1) + ...
            J(1,3)*J(2,1)*J(3,2) - ...
            J(1,3)*J(2,2)*J(3,1);
        invJ= [ 
            J(2,2)*J(3,3) - J(2,3)*J(3,2), J(1,3)*J(3,2) - J(1,2)*J(3,3), J(1,2)*J(2,3) - J(1,3)*J(2,2); ...
            J(2,3)*J(3,1) - J(2,1)*J(3,3), J(1,1)*J(3,3) - J(1,3)*J(3,1), J(1,3)*J(2,1) - J(1,1)*J(2,3); ...
            J(2,1)*J(3,2) - J(2,2)*J(3,1), J(1,2)*J(3,1) - J(1,1)*J(3,2), J(1,1)*J(2,2) - J(1,2)*J(2,1)] / detJ;

        dNdX=invJ*dNdLk;
        
        for j = 1:6
            Bm(j,j:DOF:end) = Nk;
        end
        
        B(1,1:DOF:end) = dNdX(1,:);
        B(2,2:DOF:end) = dNdX(2,:);
        B(3,3:DOF:end) = dNdX(3,:);
        B(4,2:DOF:end) = dNdX(1,:);
        B(4,6:DOF:end) = -Nk;
        B(5,1:DOF:end) = dNdX(2,:);
        B(5,6:DOF:end) = +Nk;

        B(6,3:DOF:end) = dNdX(2,:);
        B(6,4:DOF:end) = -Nk;
        B(7,2:DOF:end) = dNdX(3,:);
        B(7,4:DOF:end) = +Nk;
        
        B(8,1:DOF:end) = dNdX(3,:);
        B(8,5:DOF:end) = -Nk;
        B(9,3:DOF:end) = dNdX(1,:);
        B(9,5:DOF:end) = +Nk;
        
        B(10,4:DOF:end) = dNdX(1,:);
        B(11,5:DOF:end) = dNdX(2,:);
        B(12,6:DOF:end) = dNdX(3,:);
        B(13,5:DOF:end) = dNdX(1,:);
        B(14,4:DOF:end) = dNdX(2,:);
        B(15,6:DOF:end) = dNdX(2,:);
        B(16,5:DOF:end) = dNdX(3,:);
        B(17,4:DOF:end) = dNdX(3,:);
        B(18,6:DOF:end) = dNdX(1,:);

        
        Me = Me + W(k)*(Bm'*Rho_e*Bm)*(detJ);
        Ke = Ke + W(k)*(B'*D*B)*(detJ);
        Areas(i) = Areas(i) + W(k)*(detJ);
    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);       % 合成总体质量矩阵

dNode = find(Nodes(:,3)<=0);                                % downNode
uNode = find(Nodes(:,3)>=1);                                % upNode

uc = [
    dNode, 1*ones(size(dNode)), zeros(size(dNode));
    dNode, 2*ones(size(dNode)), zeros(size(dNode));
    dNode, 3*ones(size(dNode)), zeros(size(dNode));
    dNode, 4*ones(size(dNode)), zeros(size(dNode));
    dNode, 5*ones(size(dNode)), zeros(size(dNode));
    dNode, 6*ones(size(dNode)), zeros(size(dNode));
    
    uNode, 1*ones(size(uNode)),  ones(size(uNode));
    uNode, 2*ones(size(uNode)), zeros(size(uNode));
    uNode, 3*ones(size(uNode)), zeros(size(uNode));
    uNode, 4*ones(size(uNode)), zeros(size(uNode));
    uNode, 5*ones(size(uNode)), zeros(size(uNode));
    uNode, 6*ones(size(uNode)), zeros(size(uNode));
    ];
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);
U3 = U(3:DOF:NT,1);
W1 = U(4:DOF:NT,1);
W2 = U(5:DOF:NT,1);
W3 = U(6:DOF:NT,1);

figure(100); clf
patchC3D8(Elements, Nodes, U1)
axis equal
colormap(jet(13)); colorbar
title('$u_{1}$', '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')
zlabel('$z$' , 'interpreter', 'latex', 'fontsize', 16, 'fontname', 'times new roman')
set(gca, 'fontsize', 16, 'fontname', 'times new roman', 'xminortick', 'on', 'yminortick', 'on', 'zminortick', 'on')
view(3)

figure(200); clf
patchC3D8(Elements, Nodes, U2)
axis equal
colormap(jet(13)); colorbar
title('$u_{2}$', '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')
zlabel('$z$' , 'interpreter', 'latex', 'fontsize', 16, 'fontname', 'times new roman')
set(gca, 'fontsize', 16, 'fontname', 'times new roman', 'xminortick', 'on', 'yminortick', 'on', 'zminortick', 'on')
view(3)

figure(300); clf
patchC3D8(Elements, Nodes, U3)
axis equal
colormap(jet(13)); colorbar
title('$u_{3}$', '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')
zlabel('$z$' , 'interpreter', 'latex', 'fontsize', 16, 'fontname', 'times new roman')
set(gca, 'fontsize', 16, 'fontname', 'times new roman', 'xminortick', 'on', 'yminortick', 'on', 'zminortick', 'on')
view(3)


GaussianPointStress = zeros(numel(W), 18, NE);
GaussianPointStrain = zeros(numel(W), 18, NE);
nodeStress = zeros(NN, 18);
nodeStrain = zeros(NN, 18);
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(18, DOF*ENN);
    for k = 1:length(W)
        dNdLk = dNdLk_Arr{k};
        Nk = N_Arr{k};
        J = dNdLk*enode;
        detJ = ...
            J(1,1)*J(2,2)*J(3,3) - ...
            J(1,1)*J(2,3)*J(3,2) - ...
            J(1,2)*J(2,1)*J(3,3) + ...
            J(1,2)*J(2,3)*J(3,1) + ...
            J(1,3)*J(2,1)*J(3,2) - ...
            J(1,3)*J(2,2)*J(3,1);
        invJ= [ 
            J(2,2)*J(3,3) - J(2,3)*J(3,2), J(1,3)*J(3,2) - J(1,2)*J(3,3), J(1,2)*J(2,3) - J(1,3)*J(2,2); ...
            J(2,3)*J(3,1) - J(2,1)*J(3,3), J(1,1)*J(3,3) - J(1,3)*J(3,1), J(1,3)*J(2,1) - J(1,1)*J(2,3); ...
            J(2,1)*J(3,2) - J(2,2)*J(3,1), J(1,2)*J(3,1) - J(1,1)*J(3,2), J(1,1)*J(2,2) - J(1,2)*J(2,1)] / detJ;

        dNdX=invJ*dNdLk;
        
        for j = 1:6
            Bm(j,j:DOF:end) = Nk;
        end
        
        B(1,1:DOF:end) = dNdX(1,:);
        B(2,2:DOF:end) = dNdX(2,:);
        B(3,3:DOF:end) = dNdX(3,:);
        B(4,2:DOF:end) = dNdX(1,:);
        B(4,6:DOF:end) = -Nk;
        B(5,1:DOF:end) = dNdX(2,:);
        B(5,6:DOF:end) = +Nk;

        B(6,3:DOF:end) = dNdX(2,:);
        B(6,4:DOF:end) = -Nk;
        B(7,2:DOF:end) = dNdX(3,:);
        B(7,4:DOF:end) = +Nk;
        
        B(8,1:DOF:end) = dNdX(3,:);
        B(8,5:DOF:end) = -Nk;
        B(9,3:DOF:end) = dNdX(1,:);
        B(9,5:DOF:end) = +Nk;
        
        B(10,4:DOF:end) = dNdX(1,:);
        B(11,5:DOF:end) = dNdX(2,:);
        B(12,6:DOF:end) = dNdX(3,:);
        B(13,5:DOF:end) = dNdX(1,:);
        B(14,4:DOF:end) = dNdX(2,:);
        B(15,6:DOF:end) = dNdX(2,:);
        B(16,5:DOF:end) = dNdX(3,:);
        B(17,4:DOF:end) = dNdX(3,:);
        B(18,6:DOF:end) = dNdX(1,:);
        
        strain = B*eU;
        stress = D*strain;
        GaussianPointStress(k,:,i) = stress(:);
        GaussianPointStrain(k,:,i) = strain(:);
        
        
    end
    a = (5 + 3*sqrt(3))/4;
    b = -(1 + sqrt(3))/4;
    c = -(1 - sqrt(3))/4;
    d = (5 - 3*sqrt(3))/4;
    transMat = [
        a, b, c, b, b, c, d, c;
        b, a, b, c, c, b, c, d;
        c, b, a, b, d, c, b, c;
        b, c, b, a, c, d, c, b;
        b, c, d, c, a, b, c, b;
        c, b, c, d, b, a, b, c;
        d, c, b, c, c, b, a, b;
        c, d, c, b, b, c, b, a;
        ];
    p1 = [1,4,2,3,5,8,6,7];
    p2 = [7,8,5,6,3,4,1,2];
    transMat(p1, p1) = transMat(p2, p2);
    enstrn = transMat*GaussianPointStrain(:,:,i);
    enstrs = transMat*GaussianPointStress(:,:,i);
    nodeStrain(Elements(i,1:8), :) = nodeStrain(Elements(i,1:8), :) + enstrn;
    nodeStress(Elements(i,1:8), :) = nodeStress(Elements(i,1:8), :) + enstrs;
    nodeCount(Elements(i,1:8)) = nodeCount(Elements(i,1:8)) + 1;
end
nodeStress = nodeStress./repmat(nodeCount,1,18); % mean stress
nodeStrain = nodeStrain./repmat(nodeCount,1,18);

figure(400); clf
patchC3D8(Elements(:,1:8), Nodes, nodeStress(:,4))
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')
zlabel('$z$' , 'interpreter', 'latex', 'fontsize', 16, 'fontname', 'times new roman')
set(gca, 'fontsize', 16, 'fontname', 'times new roman', 'xminortick', 'on', 'yminortick', 'on', 'zminortick', 'on')
view(3)

figure(401); clf
patchC3D8(Elements(:,1:8), Nodes, nodeStress(:,5))
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')
zlabel('$z$' , 'interpreter', 'latex', 'fontsize', 16, 'fontname', 'times new roman')
set(gca, 'fontsize', 16, 'fontname', 'times new roman', 'xminortick', 'on', 'yminortick', 'on', 'zminortick', 'on')
view(3)



figure(403); clf
patchC3D8(Elements(:,1:8), Nodes, nodeStress(:,6))
axis equal
colormap(jet(13)); colorbar
title('$\sigma_{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')
zlabel('$z$' , 'interpreter', 'latex', 'fontsize', 16, 'fontname', 'times new roman')
set(gca, 'fontsize', 16, 'fontname', 'times new roman', 'xminortick', 'on', 'yminortick', 'on', 'zminortick', 'on')
view(3)


figure(404); clf
patchC3D8(Elements(:,1:8), Nodes, nodeStress(:,7))
axis equal
colormap(jet(13)); colorbar
title('$\sigma_{32}$', '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')
zlabel('$z$' , 'interpreter', 'latex', 'fontsize', 16, 'fontname', 'times new roman')
set(gca, 'fontsize', 16, 'fontname', 'times new roman', 'xminortick', 'on', 'yminortick', 'on', 'zminortick', 'on')
view(3)


figure(405); clf
patchC3D8(Elements(:,1:8), Nodes, nodeStress(:,8))
axis equal
colormap(jet(13)); colorbar
title('$\sigma_{31}$', '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')
zlabel('$z$' , 'interpreter', 'latex', 'fontsize', 16, 'fontname', 'times new roman')
set(gca, 'fontsize', 16, 'fontname', 'times new roman', 'xminortick', 'on', 'yminortick', 'on', 'zminortick', 'on')
view(3)


figure(406); clf
patchC3D8(Elements(:,1:8), Nodes, nodeStress(:,9))
axis equal
colormap(jet(13)); colorbar
title('$\sigma_{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')
zlabel('$z$' , 'interpreter', 'latex', 'fontsize', 16, 'fontname', 'times new roman')
set(gca, 'fontsize', 16, 'fontname', 'times new roman', 'xminortick', 'on', 'yminortick', 'on', 'zminortick', 'on')
view(3)