ordinarydifferentialeguation.inl

矩阵运算

        FunctionValueTI(s,y,d);
        for(j=0; j<n; j++) q[n+n+j]=d[j];

        for(j=0; j<n; j++)
        { 
			aa=q[n+n+j]-q[n+j];
            bb=w[n+n+j]-w[n+j];
            if(-aa*bb*h>0.0)
            {
				p[j]=-aa/bb; dd=-p[j]*h;
                r[j]=exp(dd);
                r[n+j]=(r[j]-1.0)/dd;
                r[n+n+j]=(r[n+j]-1.0)/dd;
                r[3*n+j]=(r[n+n+j]-1.0)/dd;
            }
            else p[j]=0.0;
            if(p[j]<0.0||FloatEqual(p[j],0.0)) g=q[n+n+j];
            else
            { 
				g=2.0*(q[n+n+j]-q[j])*r[n+n+j];
                g=g+(q[j]-q[n+j])*r[n+j]+q[n+j];
            }
            w[3*n+j]=w[j]+g*h;
            y[j]=w[3*n+j];
        }
        s=t+h;
        FunctionValueTI(s,y,d);
        for(j=0; j<n; j++) q[3*n+j]=d[j];
        for(j=0; j<n; j++)
        { 
			if(p[j]<0.0||FloatEqual(p[j],0.0))
            { 
				dy=q[j]+2.0*(q[n+j]+q[n+n+j]);
                dy=(dy+q[n+n+n+j])*h/6.0;
            }
            else
            { 
				dy=-3.0*(q[j]+p[j]*w[j])+2.0*(q[n+j]+p[j]*w[n+j]);
                dy=dy+2.0*(q[n+n+j]+p[j]*w[n+n+j]);
                dy=dy-(q[n+n+n+j]+p[j]*w[n+n+n+j]);
                dy=dy*r[n+n+j]+q[j]*r[n+j];
                dy1=q[j]-q[n+j]-q[n+n+j]+q[n+n+n+j];
                dy1=dy1+(w[j]-w[n+j]-w[n+n+j]+w[n+n+n+j])*p[j];
                dy=(dy+4.0*dy1*r[n+n+n+j])*h;
            }
            y[j]=w[j]+dy;
            z(j,i)=y[j];
        }
    }
}

//积分刚性方程组吉尔法
template <class _Ty>
int ODE_Gear(_Ty a, _Ty b, _Ty hmin, _Ty hmax, _Ty h, _Ty eps, 
		valarray<_Ty>& y0, int k, valarray<_Ty>& t, matrix<_Ty>& z)
{
    int kf(1),jt(0),nn(0),nq(1),m(2),irt1,irt(1),j,nqd,idb;
    int iw,j1,j2,nt,nqw,l;
    _Ty aa[7],hw(h),hd,rm,t0(a),td,r,dd,pr1,pr2,pr3,rr;
    _Ty enq1,enq2,enq3,eup,e,edwn,bnd,r1;

    _Ty pp[7][3] = 
	{
		{2.0,3.0,1.0}, {4.5,6.0,1.0},
        {7.333,9.167,0.5}, {10.42,12.5,0.1667},
        {13.7,15.98,0.04133}, {17.15,1.0,0.008267},
        {1.0,1.0,1.0}
	};
	
	int n = y0.size();	//微分方程组中方程的个数，也是未知函数的个数
	valarray<_Ty> d(n), s(10*n), s02(n), ym(n), er(n);
	valarray<_Ty> yy(n), y(8*n), iis(n), jjs(n);
	matrix<_Ty> p(n,n);

    aa[1]=-1.0;
    for(int i=0; i<8*n; i++) y[i]=0.0;
    for(i=0; i<n; i++) 
    {
		y[i*8]=y0[i];
		yy[i]=y[i*8];
	}
    FunctionValueG(t0,yy,d);
    for(i=0; i<n; i++) y[i*8+1]=h*d[i];
    for(i=0; i<n; i++) ym[i]=1.0;
  l20:
    irt=1;
	kf=1; 
	nn=nn+1;
    t[nn-1]=t0;
    for(i=0; i<n; i++) z(i,nn-1)=y[i*8];
    if((t0>b||FloatEqual(t0,b))||(nn==k))	return(kf);
    
	for(i=0; i<n; i++)
      for(j=0; j<m; j++) s[i*10+j]=y[i*8+j];
    hd=hw;
    if(h!=hd)
    {
		rm=h/hd;
		irt1=0;
        rr=Abs(hmin/hd);
        if(rm<rr) rm=rr;
        rr=Abs(hmax/hd);
        if(rm>rr) rm=rr;
		r=1.0; 
		irt1=irt1+1;
        for(j=1; j<m; j++)
        { 
			r=r*rm;
            for(i=0; i<n; i++)
              y[i*8+j]=s[i*10+j]*r;
        }
        h=hd*rm;
        for(i=0; i<n; i++)
			y[i*8]=s[i*10];
        idb=m;
    }
    nqd=nq;
	td=t0;
	rm=1.0;
    if(jt>0) goto l80;
  l60:
    switch(nq)
    { 
		case 1: aa[0]=-1.0;
				break;
        case 2: aa[0]=-2.0/3.0;
				aa[2]=-1.0/3.0; 
				break;
        case 3: aa[0]=-6.0/11.0; 
				aa[2]=aa[0];
                aa[3]=-1.0/11.0; 
				break;
        case 4: aa[0]=-0.48; 
				aa[2]=-0.7;
				aa[3]=-0.2;
                aa[4]=-0.02; 
				break;
        case 5: aa[0]=-120.0/274.0;
				aa[2]=-225.0/274.0;
                aa[3]=-85.0/274.0; 
				aa[4]=-15.0/274.0;
                aa[5]=-1.0/274.0; 
				break;
        case 6: aa[0]=-720.0/1764.0; 
				aa[2]=-1624.0/1764.0;
                aa[3]=-735.0/1764.0;
				aa[4]=-175.0/1764.0;
                aa[5]=-21.0/1764.0; 
				aa[6]=-1.0/1764.0;
                break;
        default:	return(-2);
    }
    m=nq+1; 
	idb=m;
    enq2=0.5/(nq+1.0);
	enq3=0.5/(nq+2.0);
    enq1=0.5/(nq+0.0);
    eup=pp[nq-1][1]*eps;
	eup=eup*eup;
    e=pp[nq-1][0]*eps;
	e=e*e;
    edwn=pp[nq-1][2]*eps;
	edwn=edwn*edwn;
    if(edwn==0.0)
    {
		for(i=0; i<n; i++)
          for(j=0; j<m; j++)
            y[i*8+j]=s[i*10+j];
        h=hd;
		nq=nqd; 
		jt=nq;
		return(-4);
    }
    bnd=eps*enq3/(n+0.0);
    iw=1;
    if(irt==2)
    { 
		r1=1.0;
        for(j=1; j<m; j++)
        {
			r1=r1*r;
            for(i=0; i<n; i++)
				y[i*8+j]=y[i*8+j]*r1;
        }
        idb=m;
        for(i=0; i<n; i++)
          if(ym[i]<Abs(y[i*8]))
				ym[i]=Abs(y[i*8]);
        jt=nq;
        goto l20;
    }
  l80:
    t0=t0+h;
    for(j=2; j<=m; j++)
      for(j1=j; j1<=m; j1++)
      { 
		  j2=m-j1+j-1;
          for(i=0; i<n; i++)
				y[i*8+j2-1]=y[i*8+j2-1]+y[i*8+j2];
      }
    for(i=0; i<n; i++) er[i]=0.0;
    j1=1; 
	nt=1;
    for(l=0; l<=2; l++)
    {
		if((j1!=0)&&(nt!=0))
        {
			for(i=0; i<n; i++) yy[i]=y[i*8];
            FunctionValueG(t0,yy,d);
            if(iw>=1)
            {
				for(i=0; i<n; i++) yy[i]=y[i*8];
                JacobiMatrix(t0,yy,p);			//计算雅可比矩阵的函数
                r=aa[0]*h;
                for(i=0; i<n; i++)
                  for(j=0; j<n; j++)
                    p(i,j)=p(i,j)*r;
                for(i=0; i<n; i++)
                  p(i,i)=1.0+p(i,i);
                iw=-1;
                jjs[0]=MatrixInversionGS(p);	//全选主元高斯-约当法求矩阵逆
                j1=jjs[0];
            }
            if(jjs[0]!=0)
            {
				for(i=0; i<n; i++)
					s02[i]=y[i*8+1]-d[i]*h;
                for(i=0; i<n; i++)
                { 
					dd=0.0;
                    for(j=0; j<n; j++)
						dd=dd+s02[j]*p(i,j);
                    s[i*10+8]=dd;
                }
                nt=n;
                for(i=0; i<n; i++)
                { 
					y[i*8]=y[i*8]+aa[0]*s[i*10+8];
                    y[i*8+1]=y[i*8+1]-s[i*10+8];
                    er[i]=er[i]+s[i*10+8];
                    if(Abs(s[i*10+8])<=(bnd*ym[i]))
						nt=nt-1;
                }
            }
        }
    }
    if(nt>0)
    {
		t0=td;
        if((h>(hmin*1.00001))||(iw>=0))
        {
			if(iw!=0) rm=0.25*rm;
            iw=1;
			irt1=2;
            rr=Abs(hmin/hd);
            if(rm<rr) rm=rr;
            rr=Abs(hmax/hd);
            if(rm>rr) rm=rr;
            r=1.0;
            for(j=1; j<m; j++)
            {
				r=r*rm;
                for(i=0; i<n; i++)
					y[i*8+j]=s[i*10+j]*r;
            }
            h=hd*rm;
            for(i=0; i<n; i++)
              y[i*8]=s[i*10];
            idb=m;
            goto l80;
        }
        for(i=0; i<n; i++)
          for(j=0; j<m; j++)
            y[i*8+j]=s[i*10+j];
        h=hd; 
		nq=nqd;
		jt=nq;
		return(-3);
    }
    dd=0.0;
    for(i=0; i<n; i++)
		dd=dd+(er[i]/ym[i])*(er[i]/ym[i]);
    iw=0;
    if(dd<=e)
    { 
		if(m>=3)
          for(j=2; j<m; j++)
            for(i=0; i<n; i++)
              y[i*8+j]=y[i*8+j]+aa[j]*er[i];
        kf=1;
		hw=h;
        if(idb>1)
        {
			idb=idb-1;
            if(idb<=1)
              for(i=0; i<n; i++)
                s[i*10+9]=er[i];
            for(i=0; i<n; i++)
	      if(ym[i]<Abs(y[i*8])) ym[i]=Abs(y[i*8]);
            jt=nq;
            goto l20;
        }
    }
    if(dd>e)
    { 
		kf=kf-2;
        if(h<=(hmin*1.00001))
        {
            hw=h;
			jt=nq; 
			return(-1);
        }
        t0=td;
        if(kf<=-5)
        { 
			if(nq==1)
            {
				for(i=0; i<n; i++)
                  for(j=0; j<m; j++)
                    y[i*8+j]=s[i*10+j];
                h=hd;
				nq=nqd;
				jt=nq;
				return(-4);
            }
            for(i=0; i<n; i++) yy[i]=y[i*8];
            FunctionValueG(t0,yy,d);
            r=h/hd;
            for(i=0; i<n; i++)
            { 
				y[i*8]=s[i*10];
                s[i*10+1]=hd*d[i];
                y[i*8+1]=s[i*10+1]*r;
            }
            nq=1;
			kf=1;
			goto l60;
        }
    }
    pr2=log(dd/e);
	pr2=enq2*pr2;
	pr2=exp(pr2);
    pr2=1.2*pr2;
    pr3=1.0e+20;
    if(nq<7)
      if(kf>-1)
      {
		  dd=0.0;
          for(i=0; i<n; i++)
          {
			  pr3=(er[i]-s[i*10+9])/ym[i];
              dd=dd+pr3*pr3;
          }
          pr3=log(dd/eup);
		  pr3=enq3*pr3;
          pr3=exp(pr3);
		  pr3=1.4*pr3;
      }
    pr1=1.0e+20;
    if(nq>1)
    { 
		dd=0.0;
        for(i=0; i<n; i++)
        {
			pr1=y[i*8+m-1]/ym[i];
            dd=dd+pr1*pr1;
        }
        pr1=log(dd/edwn); 
		pr1=enq1*pr1;
        pr1=exp(pr1);
		pr1=1.3*pr1;
    }
    if(pr2<=pr3)
    { 
		if(pr2>pr1)
		{
			r=1.0e+04;
            if(pr1>1.0e-04) r=1.0/pr1;
            nqw=nq-1;
        }
        else
        { 
			nqw=nq;
			r=1.0e+04;
            if(pr2>1.0e-04) r=1.0/pr2;
        }
    }
    else
    { 
		if(pr3<pr1)
        { 
			r=1.0e+04;
            if(pr3>1.0e-04) r=1.0/pr3;
            nqw=nq+1;
        }
        else
        { 
			r=1.0e+04;
            if(pr1>1.0e-04) r=1.0/pr1;
            nqw=nq-1;
        }
    }
    idb=10;
    if(kf==1)
      if(r<1.1)
      { 
		  for(i=0; i<n; i++)
            if(ym[i]<Abs(y[i*8])) ym[i]=Abs(y[i*8]);
          jt=nq; goto l20;
      }
    if(nqw>nq)
      for(i=0; i<n; i++)
        y[i*8+nqw]=er[i]*aa[m-1]/(m+0.0);
    m=nqw+1;
    if(kf==1)
    { 
		irt=2; 
		rr=hmax/Abs(h);
        if(r>rr) r=rr;
        h=h*r; 
		hw=h;
        if(nq==nqw)
        {
			r1=1.0;
            for(j=1; j<m; j++)
            {
				r1=r1*r;
                for(i=0; i<n; i++)
                  y[i*8+j]=y[i*8+j]*r1;
            }
            idb=m;
            for(i=0; i<n; i++)
              if(ym[i]<Abs(y[i*8])) ym[i]=Abs(y[i*8]);
            jt=nq;
			goto l20;
        }
        nq=nqw;
        goto l60;
    }
    rm=rm*r;
	irt1=3;
    rr=Abs(hmin/hd);
    if(rm<rr) rm=rr;
    rr=Abs(hmax/hd);
    if(rm>rr) rm=rr;
    r=1.0;
    for(j=1; j<m; j++)
    {
		r=r*rm;
        for(i=0; i<n; i++)
          y[i*8+j]=s[i*10+j]*r;
    }
    h=hd*rm;
    for(i=0; i<n; i++)
      y[i*8]=s[i*10];
    idb=m;
    if(nqw==nq) goto l80;
    nq=nqw; goto l60;
}

//二阶微分方程边值问题数值解法
template <class _Ty>
void ODE_LinearBoundaryValude(_Ty a, _Ty b, _Ty ya, _Ty yb, int n, 
													valarray<_Ty>& y)
{
    int i,k,nn,m1;
    _Ty h,x;
	valarray<_Ty> g(6*n), d(2*n), z(4);

    h=(b-a)/(n-1.0);
	nn=2*n-1;
    g[0]=1.0;
	g[1]=0.0;
    y[0]=ya; 
	y[n-1]=yb;
    g[3*n-3]=1.0;
	g[3*n-4]=0.0;
    for(i=2; i<n; i++)
    { 
		x=a+(i-1)*h;
        FunctionValueLBV(x,z);
        k=3*(i-1)-1;
        g[k]=z[0]-h*z[1]/2.0;
        g[k+1]=h*h*z[2]-2.0*z[0];
        g[k+2]=z[0]+h*z[1]/2.0;
        y[i-1]=h*h*z[3];
    }
    m1=3*n-2;
	
	valarray<_Ty> gg(m1);
	for(i=0;i<m1;i++)	gg[i] = g[i];
	LE_TridiagonalEquationGauss(gg,y);	//求解三对角方程组

	for(i=0;i<m1;i++)	g[i] = gg[i];

    h=h/2.0;
    g[0]=1.0; 
	g[1]=0.0;
    d[0]=ya; 
	d[nn-1]=yb;
    g[3*nn-3]=1.0;
	g[3*nn-4]=0.0;
    for(i=2; i<nn; i++)
    { 
		x=a+(i-1)*h;
        FunctionValueLBV(x,z);
        k=3*(i-1)-1;
        g[k]=z[0]-h*z[1]/2.0;
        g[k+1]=h*h*z[2]-2.0*z[0];
        g[k+2]=z[0]+h*z[1]/2.0;
        d[i-1]=h*h*z[3];
    }
    m1=3*nn-2;

	valarray<_Ty> ggg(m1), ddd(nn);
	for(i=0;i<m1;i++)	ggg[i] = g[i];
	for(i=0;i<nn;i++)	ddd[i] = d[i];
	LE_TridiagonalEquationGauss(ggg,ddd);	//求解三对角方程组
	
    for(i=0;i<m1;i++)	g[i] = ggg[i];
	for(i=0;i<nn;i++)	d[i] = ddd[i];

	for(i=2; i<n; i++)
    { 
		k=i+i-1;
        y[i-1]=(4.0*d[k-1]-y[i-1])/3.0;
    }
}

#endif		// _ORDINARYDIFFERENTIALEGUATION_INL