newtonnl.cpp

解非线性方程组的一种方法：先用一种优化方法将给定初值（它有可能会使得后续的牛顿法发散）通过一条比较快的途径收敛到精确解附近

// NewtonNL.cpp: implementation of the NewtonNL class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "NewtonNL.h"
#include "dmdKinematics.h"

template <class _Ty>
int LE_TotalChoiceGauss_(TNT::Matrix<_Ty>& a, TNT::Vector<_Ty>& b) ;

static NewtonNL* ptr = NULL;

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

#define V 0.000001
#define W 0.000001
#define N 0.001

#define OUT_NL 100
#define TOL_NL 8

//////////////////////////////////////////////////////////////////////////
// 

NewtonNL::NewtonNL(int _M, PCN pcn) 
: M(_M)
, m_pcn(pcn)
{
  Init(m_dXAdd, M);
  Init(m_dD, M);
  Init(m_dX, M);
  ptr = this;
}

NewtonNL::~NewtonNL()
{
  
}

void NewtonNL::PutX(TDoubleVec& a)
{
  for(int i=0;i<M;i++)
  {
    m_dX[i] = a[i];
  }
}

void NewtonNL::InitX(TDoubleVec& x)
{
  double h = 0;
  TDoubleMat J(M, M), Z(TOL_NL, M);
  TDoubleVec tmp1(M), tmp2(M), tmp5(M), tmp6(M), tmp7(M);
  double tmp4;

  // double J[M][M];
  // double tmp1[M] , tmp2[M] , tmp5[M] , tmp6[M] , tmp7[M] , Z[TOL_NL][M];
  // double tmp4;

  int k = 0;
  
  // 计算 [F'(x0)] * Z0 = F(x0) / TOL_NL 得到Z[0][M]

  for(int j=0;j<M;j++) 
  {
    // f0
    Fun(x);
    for(int l=0;l<M;l++)
    {
      tmp1[l] = m_dD[l];
      tmp2[l] = x[l];
    }
    
    tmp4 = tmp2[j];

    // 控制误差的技巧
    h = N * fabs(tmp4);
    if(h == 0.0) h = N;

    tmp2[j] = tmp4 + h;
    
    h = tmp2[j] - tmp4; 
    
    Fun(tmp2);

    // 得到雅戈比矩阵
    for(int i=0;i<M;i++)
    {
      J[i][j] = (m_dD[i] - tmp1[i]) / h;
    }
  }
  
  Fun(x);
  for(int i=0;i<M;i++)
    m_dD[i] = m_dD[i] * 1.0 / TOL_NL ;

  // 求解线形方程组，注意：它得到的是x(k)和Δx而不是x(k+1)
  LE_TotalChoiceGauss_(J, m_dD);
  
  // 计算z0即Z[0][M]
  for(i=0;i<M;i++)
  {
    Z[0][i] = m_dXAdd[i];          // m_dAdd[M]->Z[0][M]
    tmp5[i] = x[i];                // x0->tmp1[M]
  }
 
  // 计算x(k+1) = x(k) + Z[k][M]
  while(k < TOL_NL - 1)
  {
    // 中点求积公式
    for(int i=0;i<M;i++)
    {
      // x(k) = x(k-1) + Z[k-1][M]
      tmp6[i] = tmp5[i] + Z[k][i];
      // x(k+0.5)=x(k)+0.5*( x(k)-x(k-1) )
      tmp7[i] = tmp6[i] + 0.5 * (tmp6[i] - tmp5[i]);
    }
    
    // 计算 [F'(k+0.5)] * Z[k][M] = F(x0) / TOL_NL 得到Z[k][M] 
    for(int j=0;j<M;j++) 
    {
      Fun(tmp7);
      for(int l=0;l<M;l++)
      {
        tmp1[l] = m_dD[l];
        tmp2[l] = tmp7[l];
      }
      tmp4 = tmp2[j];
      h = N * fabs(tmp4);
      if(h == 0.0) h = N;
      tmp2[j] = tmp4 + h;
      h = tmp2[j] - tmp4;
      
      Fun(tmp2);
      // 得到雅戈比矩阵
      for(int i=0;i<M;i++)
        J[i][j] = (m_dD[i] - tmp1[i]) / h;
    }
    
    Fun(x);
    for(i=0;i<M;i++)
      m_dD[i] = m_dD[i] * (1.0 / TOL_NL);
    
    LE_TotalChoiceGauss_(J, m_dD);

    k++;
    for(i=0;i<M;i++)
    {
      Z[k][i] = m_dXAdd[i]; // Z[k][M]=Δx
      tmp5[i] = tmp6[i];    // x(k-1) = x(k)为计算x(k+1)作准备
    }
  }

  for(i=0;i<M;i++)
  {
    // 将x(TOL_NL - 1)及其增量Δx传入牛顿法
    m_dX[i] = tmp6[i];
    m_dXAdd[i] = Z[TOL_NL - 1][i];
  }
}

void NewtonNL::Main()
{
  double h = 0;

  TDoubleMat J(M, M);
  TDoubleVec tmp1(M), tmp2(M), Z(M);
  double tmp3 ,tmp4 , tmp5 , tmp6;

  // double J[M][M];
  // double tmp1[M] , tmp2[M] , Z[M];
  // double tmp3 ,tmp4 , tmp5 , tmp6;

  int k = 1;
  
  for(int i=0;i<M;i++)
    Z[i] = 0.000001f;
  
  while(k < OUT_NL)
  {  
    for(i=0;i<M;i++)
      m_dX[i] = m_dX[i] + m_dXAdd[i];
    
    tmp4 = 0 , tmp5 = 0 ,tmp6 = 0;
    for(int j=0;j<M;j++) 
    {
      Fun(m_dX);
      for(int l=0;l<M;l++)
      {
        tmp1[l] = m_dD[l];
        tmp2[l] = m_dX[l];
      }
      
      tmp3 = tmp2[j];
      h = N * fabs(tmp3);
      if(h == 0.0) h = N;
      tmp2[j] = tmp3 + h;
      h = tmp2[j] - tmp3;
      
      Fun(tmp2);
      for(int i=0;i<M;i++)
      {
        J[i][j] = (m_dD[i] - tmp1[i]) / h; 
      }
    }
    
    Fun(m_dX);
    LE_TotalChoiceGauss_(J, m_dD);
    
    for(int i=0;i<M;i++)
      tmp1[i] = m_dX[i] + m_dXAdd[i];
    Fun(tmp1);
    
    for(i=0;i<M;i++)
    {
      tmp4 +=pow(m_dXAdd[i],2);
      tmp5 +=pow(Z[i],2);
      tmp6 +=pow(m_dD[i],2);
    }
    tmp4 = sqrt(tmp4);
    tmp5 = sqrt(tmp5);
    tmp6 = sqrt(tmp6);
    k++;
    
    if(tmp4 < tmp5)
    {
      for(i=0;i<M;i++)
        Z[i] = m_dXAdd[i];
      if(tmp4 <= V)
      {
        for(i=0;i<M;i++)
          std::cout<< m_dX[i] + m_dXAdd[i] << " ";
        break;
      }
      else
      {
        if(tmp6 <= W)
        {
          for(i=0;i<M;i++)
            std::cout<< m_dX[i] + m_dXAdd[i] << " ";
          break;
        }
        else
          continue;
      }
    }
    else
      InitX(m_dX);
  }
}

void NewtonNL::Linear(TDoubleMat& a)
{
  TDoubleVec y(M);

  // double y[M];
  double tmp ,tmp1;
  int k ,g = 0;

  for(k=0;k<M;k++)
  {
    y[k] = m_dD[k] * (-1);
    
  }

  // for(k=0;k<M;k++)
  // a[k][k] = a[k][k] + ADD;
  // 将b[M]赋给y[M]
  k=0;
  while(k < M-1)
  {
    tmp1 = a[k][k];
    for(int i=k+1;i<M;i++)
    {
      if(fabs(a[i][k])> fabs(tmp1))
      {
        tmp1 = a[i][k];
        g = i;
      }
    }
    if(g > 0)    
    {
      for(int j=0;j<M;j++)
      {
        // 将矩阵i行和k行互换
        tmp = a[k][j];
        a[k][j] = a[g][j];
        a[g][j] = tmp;
      }
      // 将b[M]的i个和k个互换
      tmp = y[g];
      y[g] = y[k];
      y[k] = tmp;
    }
    g = 0;
    // 以上是寻找列主元，找到后将将增广矩阵i行和k行互换

    // 以下的算法是：
    // 1.输入a[M][M],b[M],置k=0。
    // 2.若a[k][k]!=0,则转3，否则提示出错，退出！
    // 3.对for(i=k+1;i<M;i++)
    // 　置a[i][k] =a[i][k]/a[k][k];
    // 　b[i]　=b[i]-a[i][k]*b[k];
    // 　对for(j=k+1;j<M;j++)
    //   置a[i][j] = a[i][j]-a[i][k]*a[k][j]。
    // 4.若k=M-2,转5，否则k++,转2。
    if(a[k][k] == 0)
      std::cout<< "failed inf !";
    for(i=k+1;i<M;i++)
    {
      a[i][k] = a[i][k] / a[k][k];
      y[i] = y[i] - a[i][k] * y[k];
      
      for(int j=k+1;j<M;j++)
        a[i][j] = a[i][j] - a[i][k] * a[k][j];
    }
    k++;
  }

  // 以下算法是：
  // 5.若a[M-1][M-1] == 0，则输出失败信息，退出，
  // 否则置b[M-1]=b[M-1]/a[M-1][M-1];
  // 
  // 6.对for(k=M-2;i>=0;k--)
  // 置（b[k]-∑(a[k][l] * b[l]))/a[k][k] = b[k].
  // 
  // 7.得到结果并且保存

  if(a[M-1][M-1] == 0)
    std::cout<< " failed inf !";
  else
  {
    y[M-1] = y[M-1] / a[M-1][M-1];
    for(k=M-2;k>=0;k--)
    {
      for(int l=k+1;l<M;l++)
        y[k] -= a[k][l] * y[l]; 
      y[k] = y[k] / a[k][k];
    }

    // 以下是将计算结果保存
    for(int i=0;i<M;i++)
      m_dXAdd[i] = y[i]; // add x(i) // m_dX[i] += y[i];  // x(i+1)           
  }
}

void NewtonNL::Fun(TDoubleVec& x)
{
  TFloatVec xf(M);
  TFloatVec ff(M);

  for (int i=0; i<M; i++)
  {
    xf[i] = x[i];
  }

  m_pcn(M, &(xf[0]), &(ff[0]));
  for (int j=0; j<M; j++)
  {
    m_dD[j] = ff[j];
  }
}

//////////////////////////////////////////////////////////////////////////
// 

// 全选主元高斯消去法
template <class _Ty>
int LE_TotalChoiceGauss_(TNT::Matrix<_Ty>& a, TNT::Vector<_Ty>& b)  
{ 
  long double MaxValue, tmp;           // 记录主元绝对值
  int l(1), i, j, is;
  bool yn;
  
  int n = a.num_cols();                // 方程组阶数
  
  TNT::Vector<int> js(n);              // 保存换列位置

  // NOTE:
  for(i=0;i<b.size();i++)
    b[i] = -b[i] ;
  
  for(int k = 0; k < n - 1; k++)       // 全选主元
  { 
    MaxValue = 0.0;                    // 给保存主元绝对值变量赋初值
    
    for(i = k; i < n; i++)
      for(j = k; j < n; j++)
      {   
        tmp = Abs(a[i][j]);            // 求m(i,j)绝对值
        if(tmp > MaxValue)             // 发现一个更大的主元
        { 
          MaxValue = tmp;              // 保存新主元绝对值
          js[k] = j;                   // 新主元所在列
          is = i;                      // 新主元所在行
        }
      }
      
      yn = FloatEqual(MaxValue, 0);
      if(yn) l = 0;                    // 主元为0
      else
      {
        if(js[k] != k)                 // 换列
          for(i = 0; i < n; i++) swap(a[i][k], a[i][js[k]]);
          
          if(is != k)                  // 换行
          { 
            for (j = k; j < n; j++) swap(a[k][j], a[is][j]);
            
            swap(b[k], b[is]);         // 方程组右边第k元素与第is元素交换
          }
      }
      
      if(l == 0)                       // 矩阵奇异(主元为0)
      {
        printf("fail 1\n");
        return 0;                      // 求解失败，返回0值
      }
      
      MaxValue =  Abs(a[k][k]);
      // MaxValue;
      for(j = k + 1; j < n; j++)  a[k][j] /= a[k][k];
      // MaxValue;
      b[k] /= a[k][k]; 
      for(i = k + 1; i < n; i++)
      {
        for(j = k + 1; j < n; j++)
        {
          a[i][j] = a[i][j] - a[i][k] * a[k][j];
        }
        
        b[i] = b[i] - a[i][k] * b[k];
      }
  }
  
  MaxValue = Abs(a[n - 1][n - 1]);     // 主元
  
  yn = FloatEqual(MaxValue, 0);
  if(yn)                               // 主元为0
  {
    std::cout<<"fail 2"<<endl;
    return(0);                         // 求解失败，返回0值
  }
  
  b[n - 1] /= a[n - 1][n - 1];         // 求解方程组右边X的解
  
  for(i = n - 2; i >= 0; i--)          // 回代过程
  {
    _Ty t = 0.0;
    
    for(j = i + 1; j < n; j++)  t = t + a[i][j] * b[j];
    
    b[i] = b[i] - t;
  }
  
  js[n - 1] = n - 1;                   // X最后一个元素不用换
  for(k = n - 2; k >= 0; k --)         // k可以从n-2开始
    if(js[k] != k)                     // 交换X的元素位置(由全选换列产生的)
      swap(b[k], b[js[k]]);

  // NOTE:
  for(i=0;i<b.size();i++)              // 将结果保留到m_dXAdd
    ptr->m_dXAdd[i] = b[i];
    
    return(1);                         // 方程组求解成功！
}