%=========================================================================%
% 2019.03.15 BIT 宇航学院 DONGJZ
% 参考：
% [1]https://www.researchgate.net/profile/Ole_Sigmund
% [3]博客园
%=========================================================================%
clc;clear all;format long g;warning off all;
global volfrac nelx nely h1 h2 nyi nyj nxi nxj NFORCE NFIXED
global YOUNG POSSION THICK
global FORCE FIXED
%=========================================================================%
% 变量名称
%=========================================================================%
% volfrac-质量约束，nelx*nely=单元个数，h1*h2=网格面积
% NFORCE -受力节点总数，NFIXED-固定节点总数
% YOUUNG-模量，POISS-泊松比，THICK-厚度
% nyi nyj nxi nxj 空洞单元区域，i为下届，j为上届 %此段有问题 初置零；空洞区域
% 有限元程序没有搞出来，不均匀场太大，刚度阵奇异了？？？怎么办？
% FORCE-节点力数组（n*3）={受力节点，x方向，y方向}
% FIXED-约束信息数组（n*3）={约束点，x方向，y方向}1为约束，否则0
% FP1 数据文件指针
% 拓扑优化很简单，大部分都已经软件化了，这一块的创新在于算法，可以说第一篇关于
%拓扑优化设计的思想，就好比刘正猷的mass in mass模型，是开创篇，Sigmund确实厉害，
%能够整出个MMA不一般啊！！！这些个东西都很简单，基本的数学知识，现在想来，初高中
%的数学教育是有点用的。解决问题，好像真的是最简单的就是不错的。
%在oc求解器的基准上，做了个mma嫁接。mama对非线性函数有很大的帮助，对于线性约束，
%先转化为非线性约束，这个好像是复杂化了，不过只是一个探讨。
%=========================================================================%
% 输入相关数据
%=========================================================================%
FP1=fopen('input.txt','rt');
volfrac=fscanf(FP1,'%f',1);
nely=fscanf(FP1,'%f',1);
nelx=fscanf(FP1,'%f',1);
h1=fscanf(FP1,'%f',1);
h2=fscanf(FP1,'%f',1);
nyi=fscanf(FP1,'%f',1);nyj=fscanf(FP1,'%f',1);
nxi=fscanf(FP1,'%f',1);nxj=fscanf(FP1,'%f',1);
NFORCE=fscanf(FP1,'%d',1);NFIXED=fscanf(FP1,'%d',1);
YOUNG=fscanf(FP1,'%f',1);
POSSION=fscanf(FP1,'%f',1);
THICK=fscanf(FP1,'%f',1);
FORCE=fscanf(FP1,'%f',[3,NFORCE])';%节点力数组
FIXED=fscanf(FP1,'%f',[3,NFIXED])';%约束信息
%==========================================================================%
% 初始化
%==========================================================================%
penal=5;
rmin=h1*1.2;
%==========================================================================%
% Minimize f_ 0(x) + a_ 0*z + sum( c_i*y_i + 0.5*d_i*(y_i)^2 )
% subject to f_i(x) - a_i*z - y_i <=0, i=1,...,m
% xmin_j <=x_j <=xmax_j, j=1,...,n
% z >=0, y_i >=0, i=1,...,m
%==========================================================================%
nx=nely*nelx;nm=1;
xcur(1:nely,1:nelx)=0.3;
xold1=volfrac*ones(nx,1);
xold2=volfrac*ones(nx,1);
xmin=0.001*ones(nx,1);xmax=ones(nx,1);
lowcur=zeros(nx,1);uppcur=zeros(nx,1);
a0=0;a=0;c=0;d=0;
%==========================================================================%
% 开始迭代
%==========================================================================%
loop=0; change=1;
while change>0.001
loop=loop+1;
%==========================================================================%
% 调用有限元求解目标函数、敏度信息
%==========================================================================%
[U]=FE(xcur,penal);
% objective function and sensitivity analysis
[KE]=lk(h1,h2);
c=0.; dc=zeros(nely,nelx);dc2=zeros(nely,nelx);
for ely=1:nely
for elx=1:nelx
if (ely>=nyi&&ely<=nyj)&&(elx>=nxi&&elx<=nxj)
continue
else
n1=(nely+1)*(elx-1)+ely;
n2=(nely+1)*elx +ely;
Ue=U([2*n1-1;2*n1;2*n2-1;2*n2;2*n2+1;2*n2+2;2*n1+1;2*n1+2],1);
c=c+xcur(ely,elx)^penal*Ue'*KE*Ue;%长度为有单元个数
dc(ely,elx)=-penal*xcur(ely,elx)^(penal-1)*Ue'*KE*Ue;%有限单元敏度
dc2(ely,elx)=-penal*(penal-1)*xcur(ely,elx)^(penal-2)*Ue'*KE*Ue;
end
end
end
%==========================================================================%
% 拓扑网格过滤
%==========================================================================%
[dc]=check(rmin,xcur,dc);
[dc2]=check(rmin,xcur,dc2);
%==========================================================================%
% 凸近似展开系数
%==========================================================================%
xcurc=reshape(xcur,nx,1);
f0val=c;
df0dx=reshape(dc,nx,1);
df0dx2=reshape(dc2,nx,1);
fval_nm=exp(sum(xcurc)/(nx))-exp(volfrac);
fval=reshape(fval_nm,nm,1);

dfdx(nm,1:nx)=exp(sum(xcurc)/(nx))/nx;
dfdx2(nm,1:nx)=exp(sum(xcurc)/(nx))/nx/nx;
%==========================================================================%
% 进入求解器
%==========================================================================%
[x,low,upp]=mmasub(nm,nx,loop,xcurc,xmin,xmax,lowcur,uppcur,xold1,xold2, ...
f0val,df0dx,df0dx2,fval,dfdx,dfdx2,a0,a,c,d);
xold2=xold1;
xold1=xcurc;
xcur=reshape(x,nely,nelx);
lowcur=low;uppcur=upp;
%==========================================================================%
% 显示优化过程
%==========================================================================%
change=max(x-xcurc);
disp(['It.:' sprintf( '%4i',loop) 'Obj.:' sprintf('%10.4f',c) ...
' Vol.:' sprintf('%6.3f',sum(x)/(nx)) ...
' ch.:' sprintf('%6.3f',change)])
% plot densities
colormap(gray);imagesc(-xcur);axis equal;axis tight; axis off;pause(1e-6);
end
%==========================================================================%
% 有限元程序
%==========================================================================%
function [U]=FE(x,penal)
global nely nelx nyi nyj nxi nxj FORCE FIXED h1 h2
NPION=(nely+1)*(nelx+1);
[KE]=lk(h1,h2);
K=sparse(2*NPION,2*NPION);
F=sparse(2*NPION,1);
U=sparse(2*NPION,1);
for elx=1:nelx %组装总钢
for ely=1:nely
if (ely>=nyi&&ely<=nyj)&&(elx>=nxi&&elx<=nxj)
continue
else
n1=(nely+1)*(elx-1)+ely;
n2=(nely+1)*elx +ely;
edof=[2*n1-1;2*n1;2*n2-1;2*n2;2*n2+1;2*n2+2;2*n1+1;2*n1+2];
K(edof,edof)=K(edof,edof)+x(ely,elx)^penal*KE;
end
end
end

% define loads and supports
for fk=1:length(FORCE(:,1))
n1=2*FORCE(fk,1)-1;
F(n1,1)=FORCE(fk,2);
F(n1+1,1)=FORCE(fk,3);
end

fixeddofs=[2*FIXED(:,1).*FIXED(:,2)-1 2*FIXED(:,1).*FIXED(:,3)];
alldofs=1:2*NPION;
m=nyj-nyi;n=nxj-nxi;
n11=(nely+1)*(nxi)+nyi+1;
subnode=zeros(m,n);
for i=1:m
for j=1:n
subnode(i,j)=n11+i-1+(j-1)*nely;
end
end

subdofs=[2.*subnode-1 2.*subnode];
freedofs=setdiff(alldofs,subdofs); %减掉固定约束行，求方程要求
freed=setdiff(freedofs,fixeddofs); %减去空心区域
% solving
% KG=K(freed,freed);
% [UU,SS,VV]=svds(KG);
% BB=UU'*F(freed,:);
% XX=SS\BB;
% U(freed,1)=VV*XX;
ufreed=setdiff(alldofs,freed);
U(ufreed,1)=0;
U(freed,1)=K(freed,freed)\F(freed,1);
end

%==========================================================================%
% 单元刚度矩阵
%==========================================================================%
function [KE]=lk(a,b)
global THICK YOUNG POSSION
E=YOUNG;
nu=POSSION;
k=THICK.*[-1/6/a/b*(nu*a^2-2*b^2-a^2),1/8*nu+1/8,-1/12/a/b*(nu*a^2+4*b^2-a^2),3/8*nu-1/8, ...
1/12/a/b*(nu*a^2-2*b^2-a^2),-1/8*nu-1/8,1/6/a/b*(nu*a^2+b^2-a^2),-3/8*nu+1/8];
KE=E/(1-nu^2)*[ k(1) k(2) k(3) k(4) k(5) k(6) k(7) k(8)
k(2) k(1) k(8) k(7) k(6) k(5) k(4) k(3)
k(3) k(8) k(1) k(6) k(7) k(4) k(5) k(2)
k(4) k(7) k(6) k(1) k(8) k(3) k(2) k(5)
k(5) k(6) k(7) k(8) k(1) k(2) k(3) k(4)
k(6) k(5) k(4) k(3) k(2) k(1) k(8) k(7)
k(7) k(4) k(5) k(2) k(3) k(8) k(1) k(6)
k(8) k(3) k(2) k(5) k(4) k(7) k(6) k(1)];
end
%==========================================================================%
% 网格过滤
%==========================================================================%
function [dcn]=check(rmin,x,dc)
global nely nelx nyi nyj nxi nxj
dcn=zeros(nely,nelx);
for i=1:nelx
for j=1:nely
if (j>=nyi&&j<=nyj)&&(i>=nxi&&i<=nxj)
continue
else
sum=0.0;
for k=max(i-floor(rmin),1):min(i+floor(rmin),nelx)
for l=max(j-floor(rmin),1):min(j+floor(rmin),nely)
fac=rmin-sqrt((i-k)^2+(j-l)^2);
sum=sum+max(0,fac);
dcn(j,i)=dcn(j,i)+max(0,fac)*x(l,k)*dc(l,k);
end
end
dcn(j,i)=dcn(j,i)/(x(j,i)*sum);
end
end
end
end
%==========================================================================%
% MMA求解器
%==========================================================================%
function [xmma,low,upp]=...
mmasub(m,n,iter,xval,xmin,xmax,low,upp,xold1,xold2, ...
f0val,df0dx,df0dx2,fval,dfdx,dfdx2,a0,a,c,d)
%
% written in January 1999 by
%
% Krister Svanberg (krille@math.kth.se)
% Optimization and Systems Theory, KTH,
% SE-10044 Stockholm, Sweden.
%
% mmasub performs one MMA-iteration, aimed at
% solving the nonlinear programming problem:
%
% Minimize f_ 0(x) + a_ 0*z + sum( c_i*y_i + 0.5*d_i*(y_i)^2 )
% subject to f_i(x) - a_i*z - y_i <=0, i=1,...,m
% xmax_j <=x_j <=xmin_j, j=1,...,n
% z >=0, y_i >=0, i=1,...,m
%*** INPUT:
%
% m=The number of general constraints.
% n=The number of variables x_j.
% iter=Current iteration number (=1 the first time mmasub is called).
% xval=Column vector with the current values of the variables x_j.
% xmin=Column vector with the lower bounds for the variables x_j.
% xmax=Column vector with the upper bounds for the variables x_j.
% xold1=xval, one iteration ago (provided that iter>1).
% xold2=xval, two iterations ago (provided that iter>2).
% f0val=The value of the objective function f_ 0 at xval.
% df0dx=Column vector with the derivatives of the objective function f_ 0
% with respect to the variables x_j, calculated at xval.
% df0dx2=Column vector with the non-mixed second derivatives of the
% objective function f_ 0 with respect to the variables x_j,
% calculated at xval.
% fval=Column vector with the values of the constraint functions f_i,
% calculated at xval.
% dfdx=(m x n)-matrix with the derivatives of the constraint functions
% f_i with respect to the variables x_j, calculated at xval.
% dfdx(i,j)=the derivative of f_i with respect to x_j.
% dfdx2=(m x n)-matrix with the non-mixed second derivatives of the
% constraint functions f_i with respect to the variables x_j,
% calculated at xval.
% dfdx2(i,j)=the second derivative of f_i with respect to x_j.
% low=Column vector with the lower asymptotes from the previous
% iteration (provided that iter>1).
% upp=Column vector with the upper asymptotes from the previous
% iteration (provided that iter>1).
% a0=The constants a_ 0 in the term a_ 0*z.
% a=Column vector with the constants a_i in the terms a_i*z.
% c=Column vector with the constants c_i in the terms c_i*y_i.
% d=Column vector with the constants d_i in the terms 0.5*d_i*(y_i)^2.
%
%*** OUTPUT:
%
% xmma=Column vector with the optimal values of the variables x_j
% in the current MMA subproblem.
% ymma=Column vector with the optimal values of the variables y_i
% in the current MMA subproblem.
% zmma=Scalar with the optimal value of the variable z
% in the current MMA subproblem.
% lam=Lagrange multipliers for the m general MMA constraints.
% xsi=Lagrange multipliers for the n constraints alfa_j - x_j <=0.
% eta=Lagrange multipliers for the n constraints x_j - beta_j <=0.
% mu=Lagrange multipliers for the m constraints -y_i <=0.
% zet=Lagrange multiplier for the single constraint -z <=0.
% s=Slack variables for the m general MMA constraints.
% low=Column vector with the lower asymptotes, calculated and used
% in the current MMA subproblem.
% upp=Column vector with the upper asymptotes, calculated and used
% in the current MMA subproblem.
epsimin=0.000000005;
feps=0.000001;
asyinit=0.5;
asyincr=1.2;
asydecr=0.7;
albefa=0.1;
een=ones(n,1);
zeron=zeros(n,1);
% Calculation of the asymptotes low and upp :
if iter <=2
low=xval - asyinit*(xmax-xmin);
upp=xval + asyinit*(xmax-xmin);
end
if iter >=3
xxx=(xval-xold1).*(xold1-xold2);
factor=een;
factor(find(xxx > 0))=asyincr;
factor(find(xxx < 0))=asydecr;
low=xval - factor.*(xold1 - low);
upp=xval + factor.*(upp - xold1);
end
% Calculation of the bounds alfa and beta :
xxx=low + albefa*(xval-low);
alfa=max(xxx,xmin);
xxx=upp - albefa*(upp-xval);
beta=min(xxx,xmax);
% Calculations of p0, q0, P, Q and b.
ux1=upp-xval;
ux2=ux1.*ux1;
ux3=ux2.*ux1;
xl1=xval-low;
xl2=xl1.*xl1;
xl3=xl2.*xl1;
ul1=upp-low;
ulinv1=eenhttps://www.cnblogs.com/dongjz1011/p/ul1;
uxinv1=eenhttps://www.cnblogs.com/dongjz1011/p/ux1;
xlinv1=eenhttps://www.cnblogs.com/dongjz1011/p/xl1;
uxinv3=eenhttps://www.cnblogs.com/dongjz1011/p/ux3;
xlinv3=eenhttps://www.cnblogs.com/dongjz1011/p/xl3;
diap=(ux3.*xl1)https://www.cnblogs.com/dongjz1011/p/(2*ul1);
diaq=(ux1.*xl3)https://www.cnblogs.com/dongjz1011/p/(2*ul1);
p0=zeron;
p0(find(df0dx > 0))=df0dx(find(df0dx > 0));
p0=p0 + 0.001*abs(df0dx) + feps*ulinv1;
p0=p0.*ux2;
q0=zeron;
q0(find(df0dx < 0))=-df0dx(find(df0dx < 0));
q0=q0 + 0.001*abs(df0dx) + feps*ulinv1;
q0=q0.*xl2;
dg0dx2=2*(p0https://www.cnblogs.com/dongjz1011/p/ux3 + q0https://www.cnblogs.com/dongjz1011/p/xl3);
del0=df0dx2 - dg0dx2;
delpos0=zeron;
delpos0(find(del0 > 0))=del0(find(del0 > 0));
p0=p0 + delpos0.*diap;
q0=q0 + delpos0.*diaq;
P=zeros(m,n);
P(find(dfdx > 0))=dfdx(find(dfdx > 0));
P=P * diag(ux2);
Q=zeros(m,n);
Q(find(dfdx < 0))=-dfdx(find(dfdx < 0));
Q=Q * diag(xl2);
dgdx2=2*(P*diag(uxinv3) + Q*diag(xlinv3));
del=dfdx2 - dgdx2;
delpos=zeros(m,n);
delpos(find(del > 0))=del(find(del > 0));
P=P + delpos*diag(diap);
Q=Q + delpos*diag(diaq);
b=P*uxinv1 + Q*xlinv1 - fval ;
[xmma,ymma,zmma,lam,xsi,eta,mu,zet,s]=...
subsolv(m,n,epsimin,low,upp,alfa,beta,p0,q0,P,Q,a0,a,b,c,d);
end
%==========================================================================%
% 内点法求解器
%==========================================================================%
function [xmma,ymma,zmma,lamma,xsimma,etamma,mumma,zetmma,smma]=...
subsolv(m,n,epsimin,low,upp,alfa,beta,p0,q0,P,Q,a0,a,b,c,d);
%
% written in January 1999 by
%
% Krister Svanberg (krille@math.kth.se)
% Optimization and Systems Theory, KTH,
% SE-10044 Stockholm, Sweden.
%
% SUBSOLV solves the MMA subproblem:
%
% Minimize SUM[ p0j/(uppj-xj) + q0j/(xj-lowj) ] + a0*z +
% + SUM[ ci*yi + 0.5*di*(yi)^2 ],
%
% subject to SUM[ pij/(uppj-xj) + qij/(xj-lowj) ] - ai*z - yi <=bi,
% alfaj <=xj <=betaj, yi >=0, z >=0.
%
% Input: m, n, low, upp, alfa, beta, p0, q0, P, Q, a0, a, b, c, d.
%
% Output: xmma,ymma,zmma, slack variables and Lagrange multiplers.
%
tidnewton=0;
een=ones(n,1);
eem=ones(m,1);
epsi=1;
epsvecn=epsi*een;
epsvecm=epsi*eem;
x=0.5*(alfa+beta);
y=eem;
z=1;
lam=eem;
xsi=eenhttps://www.cnblogs.com/dongjz1011/p/(x-alfa);
xsi=max(xsi,een);
eta=eenhttps://www.cnblogs.com/dongjz1011/p/(beta-x);
eta=max(eta,een);
mu=max(eem,0.5*c);
zet=1;
s=eem;
itera=0;

while epsi > epsimin
epsvecn=epsi*een;
epsvecm=epsi*eem;
ux1=upp-x;
xl1=x-low;
ux2=ux1.*ux1;
xl2=xl1.*xl1;
uxinv1=eenhttps://www.cnblogs.com/dongjz1011/p/ux1;
xlinv1=eenhttps://www.cnblogs.com/dongjz1011/p/xl1;
plam=p0 + P'*lam ;
qlam=q0 + Q'*lam ;
gvec=P*uxinv1 + Q*xlinv1;
dpsidx=plamhttps://www.cnblogs.com/dongjz1011/p/ux2 - qlamhttps://www.cnblogs.com/dongjz1011/p/xl2 ;

rex=dpsidx - xsi + eta;
rey=c + d.*y - mu - lam;
rez=a0 - zet - a'*lam;
relam=gvec - a*z - y + s - b;
rexsi=xsi.*(x-alfa) - epsvecn;
reeta=eta.*(beta-x) - epsvecn;
remu=mu.*y - epsvecm;
rezet=zet*z - epsi;
res=lam.*s - epsvecm;

residu1=[rex' rey' rez]';
residu2=[relam' rexsi' reeta' remu' rezet res']';
residu=[residu1' residu2']';
residunorm=sqrt(residu'*residu);
residumax=max(abs(residu));

ittt=0;
while residumax > 0.9*epsi && ittt < 100
ittt=ittt + 1;
itera=itera + 1;

ux1=upp-x;
xl1=x-low;
ux2=ux1.*ux1;
xl2=xl1.*xl1;
ux3=ux1.*ux2;
xl3=xl1.*xl2;
uxinv1=eenhttps://www.cnblogs.com/dongjz1011/p/ux1;
xlinv1=eenhttps://www.cnblogs.com/dongjz1011/p/xl1;
uxinv2=eenhttps://www.cnblogs.com/dongjz1011/p/ux2;
xlinv2=eenhttps://www.cnblogs.com/dongjz1011/p/xl2;
plam=p0 + P'*lam ;
qlam=q0 + Q'*lam ;
gvec=P*uxinv1 + Q*xlinv1;
GG=P*spdiags(uxinv2,0,n,n) - Q*spdiags(xlinv2,0,n,n);
dpsidx=plamhttps://www.cnblogs.com/dongjz1011/p/ux2 - qlamhttps://www.cnblogs.com/dongjz1011/p/xl2 ;
delx=dpsidx - epsvecnhttps://www.cnblogs.com/dongjz1011/p/(x-alfa) + epsvecnhttps://www.cnblogs.com/dongjz1011/p/(beta-x);
dely=c + d.*y - lam - epsvecmhttps://www.cnblogs.com/dongjz1011/p/y;
delz=a0 - a'*lam - epsi/z;
dellam=gvec - a*z - y - b + epsvecmhttps://www.cnblogs.com/dongjz1011/p/lam;
diagx=plamhttps://www.cnblogs.com/dongjz1011/p/ux3 + qlamhttps://www.cnblogs.com/dongjz1011/p/xl3;
diagx=2*diagx + xsihttps://www.cnblogs.com/dongjz1011/p/(x-alfa) + etahttps://www.cnblogs.com/dongjz1011/p/(beta-x);
diagxinv=eenhttps://www.cnblogs.com/dongjz1011/p/diagx;
diagy=d + muhttps://www.cnblogs.com/dongjz1011/p/y;
diagyinv=eemhttps://www.cnblogs.com/dongjz1011/p/diagy;
diaglam=shttps://www.cnblogs.com/dongjz1011/p/lam;
diaglamyi=diaglam+diagyinv;
diaglamyiinv=eemhttps://www.cnblogs.com/dongjz1011/p/diaglamyi;
dellamyi=dellam + delyhttps://www.cnblogs.com/dongjz1011/p/diagy;

Axx=diag(diagx) + GG'*spdiags(diaglamyiinv,0,m,m)*GG;
azz=zet/z + a'*(ahttps://www.cnblogs.com/dongjz1011/p/diaglamyi);
axz=-GG'*(ahttps://www.cnblogs.com/dongjz1011/p/diaglamyi);

bx=delx + GG'*(dellamyihttps://www.cnblogs.com/dongjz1011/p/diaglamyi);
bz=delz - a'*(dellamyihttps://www.cnblogs.com/dongjz1011/p/diaglamyi);

AA=[Axx axz
axz' azz ];
bb=[-bx' -bz]';

solut=AA\bb;

dx=solut(1:n);
dz=solut(n+1);
dlam=(GG*dx)https://www.cnblogs.com/dongjz1011/p/diaglamyi - dz*(ahttps://www.cnblogs.com/dongjz1011/p/diaglamyi) + dellamyihttps://www.cnblogs.com/dongjz1011/p/diaglamyi;
dy=-delyhttps://www.cnblogs.com/dongjz1011/p/diagy + dlamhttps://www.cnblogs.com/dongjz1011/p/diagy;
dxsi=-xsi + epsvecnhttps://www.cnblogs.com/dongjz1011/p/(x-alfa) - (xsi.*dx)https://www.cnblogs.com/dongjz1011/p/(x-alfa);
deta=-eta + epsvecnhttps://www.cnblogs.com/dongjz1011/p/(beta-x) + (eta.*dx)https://www.cnblogs.com/dongjz1011/p/(beta-x);
dmu=-mu + epsvecmhttps://www.cnblogs.com/dongjz1011/p/y - (mu.*dy)https://www.cnblogs.com/dongjz1011/p/y;
dzet=-zet + epsi/z - zet*dz/z;
ds=-s + epsvecmhttps://www.cnblogs.com/dongjz1011/p/lam - (s.*dlam)https://www.cnblogs.com/dongjz1011/p/lam;
xx=[ y' z lam' xsi' eta' mu' zet s']';
dxx=[dy' dz dlam' dxsi' deta' dmu' dzet ds']';

stepxx=-1.01*dxxhttps://www.cnblogs.com/dongjz1011/p/xx;
stmxx=max(stepxx);
stepalfa=-1.01*dxhttps://www.cnblogs.com/dongjz1011/p/(x-alfa);
stmalfa=max(stepalfa);
stepbeta=1.01*dxhttps://www.cnblogs.com/dongjz1011/p/(beta-x);
stmbeta=max(stepbeta);
stmalbe=max(stmalfa,stmbeta);
stmalbexx=max(stmalbe,stmxx);
stminv=max(stmalbexx,1);
steg=1/stminv;

xold=x;
yold=y;
zold=z;
lamold=lam;
xsiold=xsi;
etaold=eta;
muold=mu;
zetold=zet;
sold=s;

itto=0;
resinew=2*residunorm;
while resinew > residunorm & itto < 50
itto=itto+1;

x=xold + steg*dx;
y=yold + steg*dy;
z=zold + steg*dz;
lam=lamold + steg*dlam;
xsi=xsiold + steg*dxsi;
eta=etaold + steg*deta;
mu=muold + steg*dmu;
zet=zetold + steg*dzet;
s=sold + steg*ds;
ux1=upp-x;
xl1=x-low;
ux2=ux1.*ux1;
xl2=xl1.*xl1;
uxinv1=eenhttps://www.cnblogs.com/dongjz1011/p/ux1;
xlinv1=eenhttps://www.cnblogs.com/dongjz1011/p/xl1;
plam=p0 + P'*lam ;
qlam=q0 + Q'*lam ;
gvec=P*uxinv1 + Q*xlinv1;
dpsidx=plamhttps://www.cnblogs.com/dongjz1011/p/ux2 - qlamhttps://www.cnblogs.com/dongjz1011/p/xl2 ;

rex=dpsidx - xsi + eta;
rey=c + d.*y - mu - lam;
rez=a0 - zet - a'*lam;
relam=gvec - a*z - y + s - b;
rexsi=xsi.*(x-alfa) - epsvecn;
reeta=eta.*(beta-x) - epsvecn;
remu=mu.*y - epsvecm;
rezet=zet*z - epsi;
res=lam.*s - epsvecm;

residu1=[rex' rey' rez]';
residu2=[relam' rexsi' reeta' remu' rezet res']';
residu=[residu1' residu2']';
resinew=sqrt(residu'*residu);
steg=steg/2;
end
residunorm=resinew;
residumax=max(abs(residu));
steg=2*steg;
end
epsi=0.1*epsi;
end

xmma=x;
ymma=y;
zmma=z;
lamma=lam;
xsimma=xsi;
etamma=eta;
mumma=mu;
zetmma=zet;
smma=s;
end
%==========================================================================%
% input.txt
%==========================================================================%
0.45 20 30 1 1 0 0 0 0 1 2
1 0.3 1
316 0 -100
21 0 1
651 0 1