#define A 5
#define B 5
#define C 7
#define D 1
#include <math.h>
float x0[4],x1[4],g[3],d[4],dif[4],dif0[4],dif1[4],h[4][4],c[4],b[4],r;
int i,m,l,j,n,time[100];
//目标函数
float ff(float x [4])
{
float ff;
ff = x[0]*x[0]+x[1]*x[1]+2*x[2]*x[2]+x[3]*x[3]-5*x[0]-5*x[1]-21*x[2]+7*x[3]+D ;
return ff;
}
//约束条件
float *pp(float x[4])
{
float *p3;
g[0] = -x[0]*x[0]-x[1]*x[1]-x[2]*x[2]-x[3]* x[3]-x[0]+ x[1]-x[2]+x[3]+A ;
g[1] = -x[0]*x[0]-2* x[1]*x[1]- x[2]*x[2]-2* x[3]* x[3]+ x[0]+x[3]+B ;
g[2] = -2*x[0]*x[0]- x[1]*x[1]- x[2]*x[2]+x[1]+ x[3]+C;
p3=g;
return(p3);
}
//障碍函数
float gg(float x[4])
{
float k[3],f,gg,*p0;
int i;
p0=pp(x);
for(i=0;i<3;i++)
k[i]=*(p0+i);
f=ff(x);
gg=f+r*(1/k[0]+1/k[1]+1/k[2]);
return gg;
}
//导数
float *diff(float x[4])
{
float *di,*p1;
p1=pp(x);
for(i=0;i<3;i++)
g[i]=*(p1+i);
dif[0]=2*x[0]-5-r*((-2*x[0]-1)/(g[0]*g[0])+(-2*x[0]+1)/(g[1]*g[1])+(-4*x[0]-2)/(g[2]*g[2]));
dif[1]=2*x[1]-5-r*((-2*x[1]+1)/(g[0]*g[0])+(-4*x[1])/(g[1]*g[1])+(-2*x[1]+1)/(g[2]*g[2]));
dif[2]=4*x[2]-21-r*((-2*x[2]-1)/(g[0]*g[0])+(-2*x[2])/(g[1]*g[1])+(-2*x[2])/(g[2]*g[2]));
dif[3]=2*x[3]+7-r*((-2*x[3]+1)/(g[0]*g[0])+(-4*x[3]+1)/(g[1]*g[1])+1/(g[2]*g[2]));
di=dif;
return(di);
}
//求h
void findh(float hh0[4][4],float b[4],float c[4])
{
float hh[4][4],hh1[4][4],hh2[4]={0,0,0,0},hh3[4][4],hh4[4][4],hh5[4][4],hh6[4]={0,0,0,0},bb,cc,s;
int k;
bb=0;
cc=0;
for(i=0;i<4;i++)
for(m=0;m<4;m++)
hh4[i][m]=0;
for(i=0;i<4;i++)
bb=bb+b[i]*c[i];
for(i=0;i<4;i++)
{
for(m=0;m<4;m++)
hh6[i]+=c[m]*hh0[m][i];
}
for(i=0;i<4;i++)
cc=cc+hh6[i]*c[i];
for(i=0;i<4;i++)
for(m=0;m<4;m++)
hh1[i][m]=b[i]*b[m]/bb;
for(i=0;i<4;i++)
for(m=0;m<4;m++)
hh2[i]+=hh0[i][m]*c[m];
for(i=0;i<4;i++)
for(m=0;m<4;m++)
hh3[i][m]=hh2[i]*c[m];
for(i=0;i<4;i++)
for(m=0;m<4;m++)
{
s=0;
for(k=0;k<4;k++)
s+=hh3[i][k]*hh0[k][m];
hh4[i][m]=s;
}
for(i=0;i<4;i++)
for(m=0;m<4;m++)
hh5[i][m]=hh4[i][m]/cc;
for(i=0;i<4;i++)
for(m=0;m<4;m++)
h[i][m]=hh0[i][m]+hh1[i][m]-hh5[i][m];
}
//求d
void findd(float h[4][4],float x[4])
{
float dd1[4],*p3;
float dd[4]={0,0,0,0};
p3=diff(x);
for(i=0;i<4;i++)
dd1[i]=*(p3+i);
for(i=0;i<4;i++)
d[i]=0;
for(i=0;i<4;i++)
for(m=0;m<4;m++)
d[i]+=(-h[i][m])*dd1[m];
}
//主函数
main()
{
float f,a,q,e,*p;
r=0.9;
do
{
printf("Please input the initial values:\n");
for(i=0;i<4;i++)
{
printf("x[%d]=",i);
scanf("%f",&x0[i]);
}
p=pp(x0);
if((*p)<0 || (*(p+1)<0) || (*(p+2)<0))
printf("The values do not satisfy restriction\n");
}
while((*p)<0 || (*(p+1)<0) || (*(p+2)<0));
p=diff(x0);
for(i=0;i<4;i++)
d[i]=*(p+i);
e=d[0]*d[0]+d[1]*d[1]+d[2]*d[2]+d[3]*d[3];
for(i=0;i<4;i++)
for(m=0;m<4;m++)
{
if(i==m)
h[i][m]=1;
else
h[i][m]=0;
}
for(l=0;l<100;l++)
{
r=r*0.4;
for(j=0;j<10000;j++)
{
a=0.01;
for(i=0;i<4;i++)
x1[i]=x0[i]-a*d[i];
while((gg(x1)-gg(x0))>0.0001)
{
a=a/2;
for(i=0;i<4;i++)
x1[i]=x0[i]+a*d[i];
}
if(a<0.000001)
break;
p=pp(x1);
if( (*p)<0 )
break;
for(i=0;i<4;i++)
b[i]=x1[i]-x0[i];
p=diff(x0);
for(i=0;i<4;i++)
dif0[i]=*(p+i);
p=diff(x1);
for(i=0;i<4;i++)
dif1[i]=*(p+i);
for(i=0;i<4;i++)
c[i]=dif1[i]-dif0[i];
findh(h,b,c);
findd(h,x1);
e=d[0]*d[0]+d[1]*d[1]+d[2]*d[2]+d[3]*d[3];
if(e<0.0001)
break;
for(i=0;i<4;i++)
x0[i]=x1[i];
}
time[l]=j+1;
if(r*(g[0]+g[1]+g[2])<0.00001)
break;
}
for(i=0;i<100;i++)
{
if(time[i]==0)
break;
n+=time[i];
}
printf("iterative: %d \n",n);
printf("The values are:\n");
for(i=0;i<4;i++)
printf("x[%d]=%f\n",i,x0[i]);
f=ff(x0);
printf("The function value is:%f\n",f);
}
