%% THIS THREE-FIELD FLOATING PROJECTION TOPOLOGY OPTIMIZATION (FPTO) CODE IS WRITTEN
%% BY XIAODONG HUANG, SWINBURNE UNIVERSITY OF TECHNOLOGY, AUSTRALIA, IN 2022 
function FPTO_OC(nelx,nely,volfrac,rmin)
% OPTIMIZATION PARAMETERS
xmin = 1.e-6; move = 0.2; dbeta = 1;
% ELASTICITY MATRIX OF SOLID AND ELEMENTAL STIFFNESS MATRIX
D = 1.0/(1-0.3^2)*[1 0.3 0; 0.3 1 0; 0 0 0.5*(1-0.3)]; KE = lk(D);
% PREPARE FINITE ELEMENT ANALYSIS
nodenrs = reshape(1:(1+nelx)*(1+nely),1+nely,1+ nelx);
edofVec = reshape(2*nodenrs(1:end-1,1:end-1)+1,nelx*nely,1);
edofMat = repmat(edofVec,1,8)+repmat([0 1 2*nely+[2 3 0 1] -2 -1],nelx*nely,1);
iK = reshape(kron(edofMat,ones(8,1))',64*nelx*nely,1);
jK = reshape(kron(edofMat,ones(1,8))',64*nelx*nely,1);
% PREPARE FILTER
iH = ones(nelx*nely*(2*(ceil(rmin)+1))^2,1);
jH = ones(size(iH)); sH = zeros(size(iH));
k =0;
for i1 = 1:nelx
    for j1 = 1:nely
        e1 = (i1-1)*nely+j1;
        for i2 = max(i1-ceil(rmin),1):min(i1+ceil(rmin),nelx)
            for j2 = max(j1-ceil(rmin),1):min(j1+ceil(rmin),nely)
                e2 = (i2-1)*nely+j2; k = k+1; 
                iH(k) = e1; jH(k) = e2;
                sH(k) = max(0,(rmin-sqrt((i1-i2)^2+(j1-j2)^2)));
            end
        end
    end
end
H = sparse(iH,jH,sH); Hs = sum(H,2);
% LOWER AND UPPER BOUNDS OF DESIGN VARIABELS
xupp = ones(nely,nelx); xlow = ones(nely,nelx)*xmin;
% INITIALIZATION
xphys = max(xlow,min(xupp,ones(nely,nelx)*volfrac)); xnew = xphys; xfilt = xphys; 
beta = 1.e-6; iter = 0; loop = 0; tol = 1; 
% START ITERATION
while (tol > 0.01 && beta < 10)
    iter = iter + 1; loop = loop + 1;  beta1 = 1.2*beta;
    oldx = xnew; oldxp = xphys;
    [Ced,dc] = FEA_SA(iK,jK,edofMat,KE,nelx,nely,xphys);
    dv = ones(nely,nelx)./(nelx*nely);
    % SENSITIVITY TRANSFORMATION
    dc = reshape(H*dc(:)./Hs,nely,nelx);
    dv = reshape(H*dv(:)./Hs,nely,nelx);
    dc(:) = dc(:).*(1-tanh(beta1*(xfilt(:)-0.5)).^2)*beta1/2/tanh(beta1*0.5);
    dv(:) = dv(:).*(1-tanh(beta1*(xfilt(:)-0.5)).^2)*beta1/2/tanh(beta1*0.5);
    % UPDATING DESIGN AND PHYSICAL VARIABLES
    l1 = 0; l2 = 1.e10;
    while l2-l1 > 1.0e-6
        lmid = (l1+l2)/2;
        xnew = max(xlow,min(xupp,oldx.*sqrt(max(0,-lmid.*dc./dv))));
        xfilt = reshape(max(xlow(:),min(xupp(:),H*xnew(:)./Hs)),nely,nelx);
        xphys = (tanh(beta1*0.5)+tanh(beta1*(xfilt-0.5)))/2/tanh(beta1*0.5);
        xphys = max(xlow,min(xupp,max(oldxp-move,min(oldxp+move,xphys))));
        if sum(xphys(:))/(nelx*nely) < volfrac; l1 = lmid; else; l2 = lmid; end
    end
    % IMPLICIT FLOATING PROJECTION (FP) CONSTRAINT FOR DESIGN VARIABLES
    l1 = 0; l2 = 1; sum_xnew = sum(xnew(:));
    while (l2-l1) > 1.0e-10
        th = (l1+l2)/2.0;
        xnew = max(xlow,min(xupp,max(oldx-move,min(oldx+move,(tanh(beta*th)...
            +tanh(beta*(xfilt-th)))/(tanh(beta*th)+tanh(beta*(1.-th)))))));
        if sum(xnew(:)) > sum_xnew; l1 = th; else; l2 = th; end
    end    
    % CHECK CONVERGENCE
    change  = max(abs(oldx(:)-xnew(:)));
    % EXTRACT SMOOTH DESIGN IF NECESSARY
    if (change < 0.01 || loop >= 100 ) && loop >= 10
        [ve] = smooth_design(oldxp,nelx,nely,xmin);
        [Csd,~] = FEA_SA(iK,jK,edofMat,KE,nelx,nely,ve);
        figure(2); hold on;
        title(['\beta = ',num2str(beta),'; {\it C} = ',num2str(Csd),]);
        tol = max(abs((Csd-Ced)/Ced),immse(oldxp,ve)/volfrac);
        beta = beta + dbeta; loop = 0; 
    end
    % OUTPUT RESULTS
    fprintf('It.:%3i C.:%8.4f Vf.:%4.3f beta:%4.5f\n',...
        iter,Ced,mean(xphys(:)),beta);
    figure(1); clf; imagesc(-xphys,[-1,0]); colormap(gca,'gray');
    title(['{\it it.} = ',num2str(iter),'; {\it C} = ',num2str(Ced)]);
    axis equal;  axis tight; axis off; pause(1e-6);
end
%% FINITE ELEMENT ANALYSIS AND SENSITIVITY ANALYSIS
function [c,dc] = FEA_SA(iK,jK,edofMat,KE,nelx,nely,xe)
F = sparse(2,1,-1,2*(nely+1)*(nelx+1),1);
fixeddofs = union(1:2:2*(nely+1), 2*(nely+1)*(nelx+1));
U = zeros(2*(nely+1)*(nelx+1),1);
alldofs = (1:2*(nely+1)*(nelx+1));
freedofs = setdiff(alldofs,fixeddofs);
sK = KE(:)*xe(:)';
K = sparse(iK,jK,sK); K = (K+K')/2;
U(freedofs) = K(freedofs,freedofs)\F(freedofs);
ce = reshape(sum((U(edofMat)*KE).*U(edofMat),2),nely,nelx);
c = sum(sum(xe.*ce)); dc = - ce; 
%% VOLUME FRACTIONS OF SMOOTH DESIGN 
function [ve] = smooth_design(xe,nelx,nely,xmin)
ve = xe; ngrid = 20;
[elex,eley] = meshgrid(1:nelx,1:nely);
[fnx,fny] = meshgrid(1:1/ngrid:nelx,1:1/ngrid:nely);
xgrid = interp2(elex,eley,xe,fnx,fny,'linear');
l1 =0; l2 = 1;
while (l2-l1) > 1.0e-10
    ls = (l1+l2)/2.0;
    for i = 2:nelx-1
        for j = 2:nely-1
            i1 = ngrid*(i-1)-ngrid/2+1:ngrid*(i-1)+ngrid/2+1;
            j1 = ngrid*(j-1)-ngrid/2+1:ngrid*(j-1)+ngrid/2+1;
            ve(j,i) = sum(sum(max(sign(xgrid(j1,i1)-ls),xmin)))/(ngrid+1)^2;
        end
    end
    if sum(ve(:))-sum(xe(:)) > 0; l1 = ls; else; l2 = ls; end
end
% DISPLAY THE SMOOTH DESIGN
figure(2); clf; contourf(elex, flipud(eley), xe-ls, [0 0],'edgecolor','none');
colormap([0.3,0.3,0.3]); axis equal;  axis tight; axis off;
%% ELEMENTAL STIFFNESS MATRIX 
function [KE] = lk(D)
syms x y;
N1 = 1/4*(1+x)*(1+y); N2 = 1/4*(1-x)*(1+y);
N3 = 1/4*(1-x)*(1-y); N4 = 1/4*(1+x)*(1-y);
B1 = [diff(N1,'x') 0; 0 diff(N1,'y'); diff(N1,'y') diff(N1,'x')];
B2 = [diff(N2,'x') 0; 0 diff(N2,'y'); diff(N2,'y') diff(N2,'x')];
B3 = [diff(N3,'x') 0; 0 diff(N3,'y'); diff(N3,'y') diff(N3,'x')];
B4 = [diff(N4,'x') 0; 0 diff(N4,'y'); diff(N4,'y') diff(N4,'x')];
B = [B1 B2 B3 B4]; KE = eval(int(int(B'*D*B,x,-1,1),y,-1,1));