78 lines
2.2 KiB
C++
78 lines
2.2 KiB
C++
#include <cmath>
|
|
#include "nr.h"
|
|
using namespace std;
|
|
|
|
void NR::stiff(Vec_IO_DP &y, Vec_IO_DP &dydx, DP &x, const DP htry,
|
|
const DP eps, Vec_I_DP &yscal, DP &hdid, DP &hnext,
|
|
void derivs(const DP, Vec_I_DP &, Vec_O_DP &))
|
|
{
|
|
const DP SAFETY=0.9,GROW=1.5,PGROW= -0.25,SHRNK=0.5;
|
|
const DP PSHRNK=(-1.0/3.0),ERRCON=0.1296;
|
|
const int MAXTRY=40;
|
|
const DP GAM=1.0/2.0,A21=2.0,A31=48.0/25.0,A32=6.0/25.0,C21= -8.0,
|
|
C31=372.0/25.0,C32=12.0/5.0,C41=(-112.0/125.0),
|
|
C42=(-54.0/125.0),C43=(-2.0/5.0),B1=19.0/9.0,B2=1.0/2.0,
|
|
B3=25.0/108.0,B4=125.0/108.0,E1=17.0/54.0,E2=7.0/36.0,E3=0.0,
|
|
E4=125.0/108.0,C1X=1.0/2.0,C2X=(-3.0/2.0),C3X=(121.0/50.0),
|
|
C4X=(29.0/250.0),A2X=1.0,A3X=3.0/5.0;
|
|
int i,j,jtry;
|
|
DP d,errmax,h,xsav;
|
|
|
|
int n=y.size();
|
|
Mat_DP a(n,n),dfdy(n,n);
|
|
Vec_INT indx(n);
|
|
Vec_DP dfdx(n),dysav(n),err(n),ysav(n),g1(n),g2(n),g3(n),g4(n);
|
|
xsav=x;
|
|
for (i=0;i<n;i++) {
|
|
ysav[i]=y[i];
|
|
dysav[i]=dydx[i];
|
|
}
|
|
jacobn_s(xsav,ysav,dfdx,dfdy);
|
|
h=htry;
|
|
for (jtry=0;jtry<MAXTRY;jtry++) {
|
|
for (i=0;i<n;i++) {
|
|
for (j=0;j<n;j++) a[i][j] = -dfdy[i][j];
|
|
a[i][i] += 1.0/(GAM*h);
|
|
}
|
|
ludcmp(a,indx,d);
|
|
for (i=0;i<n;i++)
|
|
g1[i]=dysav[i]+h*C1X*dfdx[i];
|
|
lubksb(a,indx,g1);
|
|
for (i=0;i<n;i++)
|
|
y[i]=ysav[i]+A21*g1[i];
|
|
x=xsav+A2X*h;
|
|
derivs(x,y,dydx);
|
|
for (i=0;i<n;i++)
|
|
g2[i]=dydx[i]+h*C2X*dfdx[i]+C21*g1[i]/h;
|
|
lubksb(a,indx,g2);
|
|
for (i=0;i<n;i++)
|
|
y[i]=ysav[i]+A31*g1[i]+A32*g2[i];
|
|
x=xsav+A3X*h;
|
|
derivs(x,y,dydx);
|
|
for (i=0;i<n;i++)
|
|
g3[i]=dydx[i]+h*C3X*dfdx[i]+(C31*g1[i]+C32*g2[i])/h;
|
|
lubksb(a,indx,g3);
|
|
for (i=0;i<n;i++)
|
|
g4[i]=dydx[i]+h*C4X*dfdx[i]+(C41*g1[i]+C42*g2[i]+C43*g3[i])/h;
|
|
lubksb(a,indx,g4);
|
|
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) nrerror("stepsize not significant in stiff");
|
|
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;
|
|
} else {
|
|
hnext=SAFETY*h*pow(errmax,PSHRNK);
|
|
h=(h >= 0.0 ? MAX(hnext,SHRNK*h) : MIN(hnext,SHRNK*h));
|
|
}
|
|
}
|
|
nrerror("exceeded MAXTRY in stiff");
|
|
}
|