#include <stdlib.h>
#include <math.h>
#include "xpplim.h"
extern int NFlags;
#define RKQS 8
#define STIFF 9
#define MAX(a,b) ((a)>(b)?(a):(b))
#define MIN(a,b) ((a)<(b)?(a):(b))
#define SIGN(a,b) ((b)>=0.0 ? fabs(a):-fabs(a))
/* #define MAXODE 100 */
#define SAFETY 0.9
#define GROW 1.5
#define PGROW -0.25
#define SHRNK 0.5
#define PSHRNK (-1.0/3.0)
#define ERRCON 0.1296
#define MAXTRY 40
#define GAM (1.0/2.0)
#define A21 2.0
#define A31 (48.0/25.0)
#define A32 (6.0/25.0)
#define C21 -8.0
#define C31 (372.0/25.0)
#define C32 (12.0/5.0)
#define C41 (-112.0/125.0)
#define C42 (-54.0/125.0)
#define C43 (-2.0/5.0)
#define B1 (19.0/9.0)
#define B2 (1.0/2.0)
#define B3 (25.0/108.0)
#define B4 (125.0/108.0)
#define E1 (17.0/54.0)
#define E2 (7.0/36.0)
#define E3 0.0
#define E4 (125.0/108.0)
#define C1X (1.0/2.0)
#define C2X (-3.0/2.0)
#define C3X (121.0/50.0)
#define C4X (29.0/250.0)
#define A2X 1.0
#define A3X (3.0/5.0)
#define MAXSTP 100000
#define TINY 1.0e-30
#define PGROW2 -0.2
#define PSHRNK2 -0.25
#define ERRCON2 1.89e-4
double sdot();
extern (*rhs)();
jacobn(x,y,dfdx,dermat,eps,work,n)
double x,*y,*dermat,*dfdx,eps,*work;
int n;
{
int i,j,k;
double r;
double *yval,*ynew,ytemp;
yval=work;
ynew=work+n;
rhs(x,y,yval,n);
r=eps*MAX(eps,fabs(x));
rhs(x+r,y,ynew,n);
for(i=0;i<n;i++){
dfdx[i]=(ynew[i]-yval[i])/r;
}
for(i=0;i<n;i++)
{
ytemp=y[i];
r=eps*MAX(eps,fabs(ytemp));
y[i]=ytemp+r;
rhs(x,y,ynew,n);
for(j=0;j<n;j++)
{
dermat[j*n+i]=(ynew[j]-yval[j])/r;
/* printf(" %g ",dermat[j*n+i]); */
}
y[i]=ytemp;
}
}
adaptive(ystart,nvar,xs,x2,eps,hguess,hmin,work,ier,epjac,iflag,jstart)
double *ystart,*xs,x2,eps,*hguess,hmin,*work,epjac;
int nvar,*ier,iflag,*jstart;
{
if(NFlags==0)
return(gadaptive(ystart,nvar,xs,x2,eps,
hguess,hmin,work,ier,epjac,iflag,jstart));
return(one_flag_step_adap(ystart,nvar,xs,x2,eps,hguess,
hmin,work,ier,epjac,iflag,jstart));
}
gadaptive(ystart,nvar,xs,x2,eps,hguess,hmin,work,ier,epjac,iflag,jstart)
double *ystart,*xs,x2,eps,*hguess,hmin,*work,epjac;
int nvar,*ier,iflag,*jstart;
{
double h1=*hguess;
int nstp,i;
double x1=*xs;
double xsav,x,hnext,hdid,h;
double *yscal,*y,*dydx,*work2;
yscal=work;
y=work+nvar;
dydx=y+nvar;
work2=dydx+nvar;
x=x1;
h=SIGN(h1,x2-x1);
set_wieners(*hguess,ystart,x1);
*ier=0;
for (i=0;i<nvar;i++) y[i]=ystart[i];
for(nstp=1;nstp<=MAXSTP;nstp++){
rhs(x,y,dydx,nvar);
for(i=0;i<nvar;i++)
if(iflag==STIFF)
yscal[i]=MAX(1,fabs(y[i]));
else
yscal[i]=fabs(y[i])+fabs(dydx[i]*h)+TINY;
if ((x+h-x2)*(x+h-x1) > 0.0) h=x2-x;
if(iflag==STIFF)
stiff(y,dydx,nvar,&x,h,eps,yscal,&hdid,&hnext,work2,epjac,ier);
else
rkqs(y,dydx,nvar,&x,h,eps,yscal,&hdid,&hnext,work2,ier);
if(*ier>0)return -1;
if ((x-x2)*(x2-x1) >= 0.0)
{
for (i=0;i<nvar;i++) ystart[i]=y[i];
*hguess=SIGN(hnext,x2-x1);
*xs=x2;
return 0;
}
if (fabs(hnext) <= hmin) {
*ier=2;
return -1;
}
h=hnext;
}
*ier=3;
return -1;
}
/* Need work size of 2n^2+12n */
/* This will integrate a maximum of htry and actually do hmin */
stiff(y,dydx,n,x,htry,eps,yscal,hdid,hnext,work,epjac,ier)
double *work,*hdid,*hnext,*x,dydx[],eps,htry,y[],yscal[],epjac;
int n,*ier;
{
int i,j,jtry,indx[700];
int info,sz;
double d,errmax,h,xsav,*a,*dfdx,*dfdy,*dysav,*err;
double *g1,*g2,*g3,*g4,*ysav,*work2;
*ier=0;
a=work;
dfdx=work+n*n;
dfdy=dfdx+n;
dysav=dfdy+n*n;
err=dysav+n;
g1=err+n;
g2=g1+n;
g3=g2+n;
g4=g3+n;
ysav=g4+n;
work2=ysav+n;
xsav=(*x);
for (i=0;i<n;i++) {
ysav[i]=y[i];
dysav[i]=dydx[i];
}
jacobn(xsav,ysav,dfdx,dfdy,epjac,work2,n);
h=htry;
for (jtry=1;jtry<=MAXTRY;jtry++) {
for (i=0;i<n;i++) {
for (j=0;j<n;j++) a[i+n*j] = -dfdy[i+n*j];
a[i+n*i] += 1.0/(GAM*h);
}
sgefa(a,n,n,indx,&info);
if(info!=-1){
*ier=-1;
return -1;
}
for (i=0;i<n;i++)
g1[i]=dysav[i]+h*C1X*dfdx[i];
sgesl(a,n,n,indx,g1,0);
for (i=0;i<n;i++)
y[i]=ysav[i]+A21*g1[i];
*x=xsav+A2X*h;
rhs(*x,y,dydx,n);
for (i=0;i<n;i++)
g2[i]=dydx[i]+h*C2X*dfdx[i]+C21*g1[i]/h;
sgesl(a,n,n,indx,g2,0);
for (i=0;i<n;i++)
y[i]=ysav[i]+A31*g1[i]+A32*g2[i];
*x=xsav+A3X*h;
rhs(*x,y,dydx,n);
for (i=0;i<n;i++)
g3[i]=dydx[i]+h*C3X*dfdx[i]+(C31*g1[i]+C32*g2[i])/h;
sgesl(a,n,n,indx,g3,0);
for (i=0;i<n;i++)
g4[i]=dydx[i]+h*C4X*dfdx[i]+(C41*g1[i]+C42*g2[i]+C43*g3[i])/h;
sgesl(a,n,n,indx,g4,0);
for (i=0;i<n;i++) {
y[i]=ysav[i]+B1*g1[i]+B2*g2[i]+B3*g3[i]+B4*g4[i];
err[i]=E1*g1[i]+E2*g2[i]+E3*g3[i]+E4*g4[i];
}
*x=xsav+h;
if (*x == xsav) {
*ier=1;
return -1;
}
errmax=0.0;
for (i=0;i<n;i++) errmax=MAX(errmax,fabs(err[i]/yscal[i]));
errmax /= eps;
if (errmax <= 1.0) {
*hdid=h;
*hnext=(errmax > ERRCON ? SAFETY*h*pow(errmax,PGROW) : GROW*h);
return 0;
} else {
*hnext=SAFETY*h*pow(errmax,PSHRNK);
h=(h >= 0.0 ? MAX(*hnext,SHRNK*h) : MIN(*hnext,SHRNK*h));
}
}
*ier=4;
return -1;
}
rkqs(y,dydx,n,x,htry,eps,yscal,hdid,hnext,work,ier)
double *hdid,*hnext,*x,*dydx,eps,htry,*y,*yscal,*work;
int n,*ier;
{
int i;
double errmax,h,htemp,xnew,*yerr,*ytemp;
double *work2;
yerr=work;
ytemp=work+n;
work2=ytemp+n;
h=htry;
*ier=0;
for (;;) {
rkck(y,dydx,n,*x,h,ytemp,yerr,work2);
errmax=0.0;
for (i=0;i<n;i++) errmax=MAX(errmax,fabs(yerr[i]/yscal[i]));
errmax /= eps;
if (errmax > 1.0) {
htemp=SAFETY*h*pow(errmax,PSHRNK2);
h=(h >= 0.0 ? MAX(htemp,0.1*h) : MIN(htemp,0.1*h));
xnew=(*x)+h;
if (xnew == *x) {
*ier=1;
return -1;
}
continue;
} else {
if (errmax > ERRCON2) *hnext=SAFETY*h*pow(errmax,PGROW2);
else *hnext=5.0*h;
*x += (*hdid=h);
for (i=0;i<n;i++) y[i]=ytemp[i];
break;
}
}
return 0;
}
/* This takes one step of Cash-Karp RK method */
rkck(y,dydx,n,x,h,yout,yerr,work)
double *dydx,h,x,*y,*yerr,*yout,*work;
int n;
{
int i;
static double a2=0.2,a3=0.3,a4=0.6,a5=1.0,a6=0.875,b21=0.2,
b31=3.0/40.0,b32=9.0/40.0,b41=0.3,b42 = -0.9,b43=1.2,
b51 = -11.0/54.0, b52=2.5,b53 = -70.0/27.0,b54=35.0/27.0,
b61=1631.0/55296.0,b62=175.0/512.0,b63=575.0/13824.0,
b64=44275.0/110592.0,b65=253.0/4096.0,c1=37.0/378.0,
c3=250.0/621.0,c4=125.0/594.0,c6=512.0/1771.0,
dc5 = -277.00/14336.0;
double dc1=c1-2825.0/27648.0,dc3=c3-18575.0/48384.0,
dc4=c4-13525.0/55296.0,dc6=c6-0.25;
double *ak2,*ak3,*ak4,*ak5,*ak6,*ytemp;
ak2=work;
ak3=work+n;
ak4=ak3+n;
ak5=ak4+n;
ak6=ak5+n;
ytemp=ak6+n;
for (i=0;i<n;i++)
ytemp[i]=y[i]+b21*h*dydx[i];
rhs(x+a2*h,ytemp,ak2,n);
for (i=0;i<n;i++)
ytemp[i]=y[i]+h*(b31*dydx[i]+b32*ak2[i]);
rhs(x+a3*h,ytemp,ak3,n);
for (i=0;i<n;i++)
ytemp[i]=y[i]+h*(b41*dydx[i]+b42*ak2[i]+b43*ak3[i]);
rhs(x+a4*h,ytemp,ak4,n);
for (i=0;i<n;i++)
ytemp[i]=y[i]+h*(b51*dydx[i]+b52*ak2[i]+b53*ak3[i]+b54*ak4[i]);
rhs(x+a5*h,ytemp,ak5,n);
for (i=0;i<n;i++)
ytemp[i]=y[i]+h*(b61*dydx[i]+b62*ak2[i]+b63*ak3[i]+
b64*ak4[i]+b65*ak5[i]);
rhs(x+a6*h,ytemp,ak6,n);
for (i=0;i<n;i++)
yout[i]=y[i]+h*(c1*dydx[i]+c3*ak3[i]+c4*ak4[i]+c6*ak6[i]);
for (i=0;i<n;i++)
yerr[i]=h*(dc1*dydx[i]+dc3*ak3[i]+
dc4*ak4[i]+dc5*ak5[i]+dc6*ak6[i]);
}
syntax highlighted by Code2HTML, v. 0.9.1