//擬牛頓法（變尺度法）DFP算法的c/c++源碼
//問題  minf(x)=100*( x1-x0*x0 )^2 + (1-x0)^2

#include "iostream.h"
#include "math.h"
#define pi  3.1415926/4
//計算梯度
void comput_grad(double (*pf)(double *x),  int n,  double *point,  double *grad); 
//0.618法線搜索         
double line_search1(double (*pf)(double *x),  int n,  double *start,  double *direction);
//解析法線搜索  
double line_search(double (*pf)(double *x),   int n,  double *start,  double *direction);
//無約束變尺度法  
double DFP(double (*pf)(double *x),   int n,   double *min_point);       

//梯度計算模塊
//參數：指向目標函數的指針，變量個數，求梯度的點，結果
void comput_grad(double (*pf)(double *x),int n,double *point,double *grad)
{
  double h=1E-3;
  int i;
  double *temp;
  temp = new double[n];
  for(i=1;i<=n;i++)
   {
    temp[i-1]=point[i-1];
    }
   for(i=1;i<=n;i++)
     {
       temp[i-1]+=0.5*h;
       grad[i-1]=4*pf(temp)/(3*h);
       temp[i-1]-=h;
       grad[i-1]-=4*pf(temp)/(3*h);
       temp[i-1]+=(3*h/2);
       grad[i-1]-=(pf(temp)/(6*h));
       temp[i-1]-=(2*h);
       grad[i-1]+=(pf(temp)/(6*h));
       temp[i-1]=point[i-1];
      }
    delete[] temp;
}

//一維搜索模塊
//參數：指向目標函數的指針，變量個數，出發點，搜索方向
//返回：最優步長
double line_search(double (*pf)(double *x),int n,double *start,double *direction)
{
  int i;
  double step=0.001;
  double a=0,value_a,diver_a;
  double b,value_b,diver_b;
  double t,value_t,diver_t;
  double s,z,w;
  double *grad,*temp_point;

  grad=new double[n];
  temp_point=new double[n];
  comput_grad(pf,n,start,grad);
  diver_a=0;
  for(i=1;i<=n;i++)
    diver_a=diver_a+grad[i-1]*direction[i-1];
   do
    {
      b=a+step;
      for(i=1;i<=n;i++)
         temp_point[i-1]=start[i-1]+b*direction[i-1];
         comput_grad(pf,n,temp_point,grad);
         diver_b=0;
       for(i=1;i<=n;i++)
          diver_b=diver_b+grad[i-1]*direction[i-1];
          if( fabs(diver_b)<1E-10 )
            {
              delete[] grad;
              delete[] temp_point;
              return(b);
             }
           if( diver_b<-1E-15 )
             {
               a=b;
               diver_a=diver_b;
               step=2*step;
              }
        }while( diver_b<=1E-15 );

        for(i=1;i<=n;i++)
                temp_point[i-1]=start[i-1]+a*direction[i-1];
        value_a=(*pf)(temp_point);
        for(i=1;i<=n;i++)
                temp_point[i-1]=start[i-1]+b*direction[i-1];
        value_b=(*pf)(temp_point);

do
 {
   s=3*(value_b-value_a)/(b-a);
   z=s-diver_a-diver_b;
   w=sqrt( fabs(z*z-diver_a*diver_b) );        /////////!!!!!!!!!!!!!!!!!!!!!!
   t=a+(w-z-diver_a)*(b-a)/(diver_b-diver_a+2*w);
   value_b=(*pf)(temp_point);
   for(i=1;i<=n;i++)
     temp_point[i-1]=start[i-1]+t*direction[i-1];
     value_t=(*pf)(temp_point);
     comput_grad(pf,n,temp_point,grad);
     diver_t=0;
   for(i=1;i<=n;i++)
     diver_t=diver_t+grad[i-1]*direction[i-1];
   if(diver_t>1E-6)
    {
      b=t;
      value_b=value_t;
      diver_b=diver_t;
              }
    else if(diver_t<-1E-6)
     {
       a=t;
       value_a=value_t;
       diver_a=diver_t;
      }
     else break;
 }while( (fabs(diver_t)>=1E-6) && (fabs(b-a)>1E-6) );

delete[] grad;
delete[] temp_point;
return(t);

}

//無約束變尺度法DFP函數聲明
//
//參數：pf指向目標函數的指針，n變量個數，min_point接受初始點、存放結果
//返回：極小點處函數值
//
double DFP(double (*pf)(double *x),int n, double *min_point)
{
  int i,j;
  int k=0;
  double e=1E-5;
  double g_norm;
                
  double *g0=new double[n];                //梯度
  double *g1=new double[n];
  double *dg=new double[n];

  double *p=new double[n];                //搜索方向 =-g
  double t;                               //一維搜索步長

  double *x0=new double[n];
  double *x1=new double[n];
  double *dx=new double[n];

  double **H=new double*[n];
 for (i=0; i<n; i++)   H[i] = new double[n];

 double **tempH=new double*[n];
 for (i=0; i<n; i++)   tempH[i] = new double[n];

 double *gH=new double[n];
 double *Hg=new double[n];
 double num1;
 double num2;

for(i=0;i<n;i++)
for(j=0;j<n;j++)
 {
   if(i==j)        H[i][j]=1.0;        // H0=I
   else        H[i][j]=0.0;
   tempH[i][j]=0.0;
  }

for(i=0;i<n;i++)
  x0[i]=min_point[i];        

comput_grad(pf,n,x0,g0);

g_norm=0.0;
for(i=0;i<n;i++)        g_norm=g_norm+g0[i]*g0[i];        
  g_norm=sqrt(g_norm);

for(i=0;i<n;i++)        p[i]=-g0[i];        

for(;;)
{ 
  t=line_search(pf,n,x0,p);                                
 for(i=0;i<n;i++)        x1[i]=x0[i]+t*p[i];
 comput_grad(pf,n,x1,g1);
 g_norm=0.0;
 for(i=0;i<n;i++)        g_norm=g_norm+g1[i]*g1[i];
 g_norm=sqrt(g_norm);
if (g_norm<e) 
 {
   for(i=0;i<n;i++)        min_point[i]=x1[i];
   
   delete[] g0;                
   delete[] g1;
   delete[] dg;
   delete[] p;
   delete[] x0;
   delete[] x1;
   delete[] dx;
   for (i=0; i<n; i++)             delete[] H[i];
   delete []H;
   for (i=0; i<n; i++)            delete[] tempH[i];
   delete []tempH;
   delete[] gH;
   delete[] Hg;
   cout<<"總的迭代次數為:"<<k<<"\n";                     
  return pf(min_point);
 }
for(i=0;i<n;i++)
 {
   dx[i]=x1[i]-x0[i];
   dg[i]=g1[i]-g0[i];
 }
/////求Hk+1的矩陣運算
//g*H,H*g
for(i=0;i<n;i++)
 {
   gH[i]=0.0;
   Hg[i]=0.0;
 }
for(i=0;i<n;i++)
{
 for(j=0;j<n;j++)
 {
    gH[i]=gH[i]+dg[j]*H[j][i];
   //Hg[i]=Hg[i]+H[i][j]*dg[j];
    Hg[i]=gH[i];
 }                        
}

//num1,num2
num1=0.0;
num2=0.0;
for(i=0;i<n;i++)
 {
   num1=num1+dx[i]*dg[i];
   num2=num2+gH[i]*dg[i];
  }

//tempH[i][j]
for(i=0;i<n;i++)
  for(j=0;j<n;j++)
    tempH[i][j]=0.0;
for(i=0;i<n;i++)
 {
   for(j=0;j<n;j++)
     {
       tempH[i][j]=tempH[i][j]+H[i][j];
       tempH[i][j]=tempH[i][j]+dx[i]*dx[j]/num1;
       tempH[i][j]=tempH[i][j]-Hg[i]*gH[j]/num2;
     }
 }

for(i=0;i<n;i++)
  {
   for(j=0;j<n;j++)
   {
     H[i][j]=tempH[i][j];
   }
  }
/////////////////////////////
 
for(i=0;i<n;i++)        p[i]=0.0;
  for(i=0;i<n;i++)
  {
    for(j=0;j<n;j++)
       {
         p[i]=p[i]-H[i][j]*g1[j];
       }                        
  }
for(i=0;i<n;i++)
  {
    g0[i]=g1[i];
    x0[i]=x1[i];
 }
k=k+1;
}
}

//目標函數
double fun(double *x)
{  
  double f,u1,u2,u3,u4;
u1=x[2]*(x[3]*x[3]+x[4]*x[4])+28*pow(x[3],2)+32*pow(x[4],2);
u2=0.92*x[0]*x[4]*x[4]-x[0]*x[3]*x[3]-85*x[0]*x[1]*x[1];
u3=-1.6*x[0]*x[1]*x[4]+1340.64*x[0];
u4=1428*x[0]*x[1]+13.44*x[0]*x[4];
f=pi*(u1+u2+u3+u4);
return f;
	  // return 100*( x[1]-x[0]*x[0] )*( x[1]-x[0]*x[0] ) + (1-x[0])*(1-x[0]);
}
//主函數
void main()
{
  int n=5;
  double min_point[5]={23,0.8,42,12,16},f0;
  f0=fun(min_point);
  double min_value=DFP(fun,n,min_point);
 
  cout<<"\n\tf0="<<f0;
for(int k=0;k<n;k++)
      cout<<"\n\t"<<"min_point["<<k<<"]="<<min_point[k];
  cout<<"\n\tmin_value="<<min_value<<"\n";

}