niniudun.c

基于牛顿迭代法的二次规划程序

#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);

}