int sp_num_alloc(sp_num** N, int n, ErrorMsg error_message){
int maxnz, k;
class_alloc((*N),sizeof(sp_num),error_message);
printf("calling spnumalloc");
maxnz = n*(n+1);
maxnz /=2;
(*N)->n = n;
class_call(sp_mat_alloc(&((*N)->L), n, n, maxnz, error_message),
error_message,error_message);
class_call(sp_mat_alloc(&((*N)->U), n, n, maxnz, error_message),
error_message,error_message);
class_alloc((*N)->xi,n*sizeof(int*),error_message);
/* I really want xi to be a vector of pointers to vectors. */
class_alloc((*N)->xi[0],n*n*sizeof(int),error_message);
for (k=1;k<n;k++) (*N)->xi[k] = (*N)->xi[k-1]+n;
/*Assign pointers to rows.*/
class_alloc((*N)->topvec,n*sizeof(int),error_message);
class_alloc((*N)->pinv,n*sizeof(int),error_message);
class_alloc((*N)->p,n*sizeof(int),error_message);
/* Has to be n+1 because sp_amd uses it for storage:*/
class_alloc((*N)->q,(n+1)*sizeof(int),error_message);
class_alloc((*N)->w,n*sizeof(double),error_message);
class_alloc((*N)->wamd,(8*(n+1))*sizeof(int),error_message);
return _SUCCESS_;
}
int parser_read_file(
char * filename,
struct file_content * pfc,
ErrorMsg errmsg
){
FILE * inputfile;
char line[_LINE_LENGTH_MAX_];
int counter;
int is_data;
FileArg name;
FileArg value;
class_open(inputfile,filename,"r",errmsg);
counter = 0;
while (fgets(line,_LINE_LENGTH_MAX_,inputfile) != NULL) {
class_call(parser_read_line(line,&is_data,name,value,errmsg),errmsg,errmsg);
if (is_data == _TRUE_) counter++;
}
class_test(counter == 0,
errmsg,
"No readable input in file %s",filename);
class_alloc(pfc->filename,(strlen(filename)+1)*sizeof(char),errmsg);
strcpy(pfc->filename,filename);
class_call(parser_init(pfc,counter,errmsg),
errmsg,
errmsg);
rewind(inputfile);
counter = 0;
while (fgets(line,_LINE_LENGTH_MAX_,inputfile) != NULL) {
class_call(parser_read_line(line,&is_data,name,value,errmsg),errmsg,errmsg);
if (is_data == _TRUE_) {
strcpy(pfc->name[counter],name);
strcpy(pfc->value[counter],value);
pfc->read[counter]=_FALSE_;
counter++;
}
}
fclose(inputfile);
return _SUCCESS_;
}
std::string get_single_class(SEXP x) {
SEXP klass = Rf_getAttrib(x, R_ClassSymbol);
if (!Rf_isNull(klass)) {
CharacterVector classes(klass);
return collapse_utf8(classes, "/");
}
if (Rf_isMatrix(x)) {
return "matrix";
}
switch (TYPEOF(x)) {
case RAWSXP:
return "raw";
case INTSXP:
return "integer";
case REALSXP :
return "numeric";
case LGLSXP:
return "logical";
case STRSXP:
return "character";
case CPLXSXP:
return "complex";
case VECSXP:
return "list";
default:
break;
}
// just call R to deal with other cases
// we could call R_data_class directly but we might get a "this is not part of the api"
RObject class_call(Rf_lang2(Rf_install("class"), x));
klass = Rf_eval(class_call, R_GlobalEnv);
return CHAR(STRING_ELT(klass, 0));
}
int evolver_ndf15(
int (*derivs)(double x,double * y,double * dy,
void * parameters_and_workspace, ErrorMsg error_message),
double x_ini,
double x_final,
double * y_inout,
int * used_in_output,
int neq,
void * parameters_and_workspace_for_derivs,
double rtol,
double minimum_variation,
int (*timescale_and_approximation)(double x,
void * parameters_and_workspace,
double * timescales,
ErrorMsg error_message),
double timestep_over_timescale,
double * t_vec,
int tres,
int (*output)(double x,double y[],double dy[],int index_x,void * parameters_and_workspace,
ErrorMsg error_message),
int (*print_variables)(double x, double y[], double dy[], void *parameters_and_workspace,
ErrorMsg error_message),
ErrorMsg error_message){
/* Constants: */
double G[5]={1.0,3.0/2.0,11.0/6.0,25.0/12.0,137.0/60.0};
double alpha[5]={-37.0/200,-1.0/9.0,-8.23e-2,-4.15e-2, 0};
double invGa[5],erconst[5];
double abstol = 1e-18, eps=1e-19, threshold=abstol;
int maxit=4, maxk=5;
/* Logicals: */
int Jcurrent,havrate,done,at_hmin,nofailed,gotynew,tooslow,*interpidx;
/* Storage: */
double *f0,*y,*wt,*ddfddt,*pred,*ynew,*invwt,*rhs,*psi,*difkp1,*del,*yinterp;
double *tempvec1,*tempvec2,*ypinterp,*yppinterp;
double **dif;
struct jacobian jac;
struct numjac_workspace nj_ws;
/* Method variables: */
double t,t0,tfinal,tnew=0;
double rh,htspan,absh,hmin,hmax,h,tdel;
double abshlast,hinvGak,minnrm,oldnrm=0.,newnrm;
double err,hopt,errkm1,hkm1,errit,rate=0.,temp,errkp1,hkp1,maxtmp;
int k,klast,nconhk,iter,next,kopt,tdir;
/* Misc: */
int stepstat[6],nfenj,j,ii,jj, numidx, neqp=neq+1;
int verbose=0;
/** Allocate memory . */
void * buffer;
int buffer_size;
buffer_size=
15*neqp*sizeof(double)
+neqp*sizeof(int)
+neqp*sizeof(double*)
+(7*neq+1)*sizeof(double);
class_alloc(buffer,
buffer_size,
error_message);
f0 =(double*)buffer;
wt =f0+neqp;
ddfddt =wt+neqp;
pred =ddfddt+neqp;
y =pred+neqp;
invwt =y+neqp;
rhs =invwt+neqp;
psi =rhs+neqp;
difkp1 =psi+neqp;
del =difkp1+neqp;
yinterp =del+neqp;
ypinterp =yinterp+neqp;
yppinterp=ypinterp+neqp;
tempvec1 =yppinterp+neqp;
tempvec2 =tempvec1+neqp;
interpidx=(int*)(tempvec2+neqp);
dif =(double**)(interpidx+neqp);
dif[1] =(double*)(dif+neqp);
for(j=2;j<=neq;j++) dif[j] = dif[j-1]+7; /* Set row pointers... */
dif[0] = NULL;
/* for (ii=0;ii<(7*neq+1);ii++) dif[1][ii]=0.; */
for (j=1; j<=neq; j++) {
for (ii=1;ii<=7;ii++) {
dif[j][ii]=0.;
}
}
/* class_alloc(f0,sizeof(double)*neqp,error_message); */
/* class_alloc(wt,sizeof(double)*neqp,error_message); */
/* class_alloc(ddfddt,sizeof(double)*neqp,error_message); */
/* class_alloc(pred,sizeof(double)*neqp,error_message); */
/* class_alloc(y,sizeof(double)*neqp,error_message); */
/* class_alloc(invwt,sizeof(double)*neqp,error_message); */
/* class_alloc(rhs,sizeof(double)*neqp,error_message); */
/* class_alloc(psi,sizeof(double)*neqp,error_message); */
/* class_alloc(difkp1,sizeof(double)*neqp,error_message); */
/* class_alloc(del,sizeof(double)*neqp,error_message); */
/* class_alloc(yinterp,sizeof(double)*neqp,error_message); */
/* class_alloc(ypinterp,sizeof(double)*neqp,error_message); */
/* class_alloc(yppinterp,sizeof(double)*neqp,error_message); */
/* class_alloc(tempvec1,sizeof(double)*neqp,error_message); */
/* class_alloc(tempvec2,sizeof(double)*neqp,error_message); */
/* class_alloc(interpidx,sizeof(int)*neqp,error_message); */
/* Allocate vector of pointers to rows of dif:*/
/* class_alloc(dif,sizeof(double*)*neqp,error_message); */
/* class_calloc(dif[1],(7*neq+1),sizeof(double),error_message); */
/* dif[0] = NULL; */
/* for(j=2;j<=neq;j++) dif[j] = dif[j-1]+7; */ /* Set row pointers... */
/*Set pointers:*/
ynew = y_inout-1; /* This way y_inout is always up to date. */
/*Initialize the jacobian:*/
class_call(initialize_jacobian(&jac,neq,error_message),error_message,error_message);
/* Initialize workspace for numjac: */
class_call(initialize_numjac_workspace(&nj_ws,neq,error_message),error_message,error_message);
/* Initialize some method parameters:*/
for(ii=0;ii<5;ii++){
invGa[ii] = 1.0/(G[ii]*(1.0 - alpha[ii]));
erconst[ii] = alpha[ii]*G[ii] + 1.0/(2.0+ii);
}
/* Set the relevant indices which needs to be found by interpolation. */
/* But if we want to print variables for testing purposes, just interpolate everything.. */
for(ii=1;ii<=neq;ii++){
y[ii] = y_inout[ii-1];
if (print_variables==NULL){
interpidx[ii]=used_in_output[ii-1];
}
else{
interpidx[ii]=1;
}
}
t0 = x_ini;
tfinal = x_final;
/* Some CLASS-specific stuff:*/
next=0;
while (t_vec[next] < t0) next++;
if (verbose > 1){
numidx=0;
for(ii=1;ii<=neq;ii++){
if (interpidx[ii]==_TRUE_) numidx++;
}
printf("%d/%d ",numidx,neq);
}
htspan = fabs(tfinal-t0);
for(ii=0;ii<6;ii++) stepstat[ii] = 0;
class_call((*derivs)(t0,y+1,f0+1,parameters_and_workspace_for_derivs,error_message),error_message,error_message);
stepstat[2] +=1;
if ((tfinal-t0)<0.0){
tdir = -1;
}
else{
tdir = 1;
}
hmax = (tfinal-t0)/10.0;
t = t0;
nfenj=0;
class_call(numjac((*derivs),t,y,f0,&jac,&nj_ws,abstol,neq,
&nfenj,parameters_and_workspace_for_derivs,error_message),
error_message,error_message);
stepstat[3] += 1;
stepstat[2] += nfenj;
Jcurrent = _TRUE_; /* True */
hmin = 1.e-20;//16.0*eps*fabs(t);
/*Calculate initial step */
rh = 0.0;
for(jj=1;jj<=neq;jj++){
wt[jj] = MAX(fabs(y[jj]),threshold);
/*printf("wt: %4.8f \n",wt[jj]);*/
rh = MAX(rh,1.25/sqrt(rtol)*fabs(f0[jj]/wt[jj]));
}
absh = MIN(hmax, htspan);
if (absh * rh > 1.0) absh = 1.0 / rh;
absh = MAX(absh, hmin);
h = tdir * absh;
tdel = (t + tdir*MIN(sqrt(eps)*MAX(fabs(t),fabs(t+h)),absh)) - t;
class_call((*derivs)(t+tdel,y+1,tempvec1+1,parameters_and_workspace_for_derivs,error_message),
error_message,error_message);
stepstat[2] += 1;
/*I assume that a full jacobi matrix is always calculated in the beginning...*/
for(ii=1;ii<=neq;ii++){
ddfddt[ii]=0.0;
for(jj=1;jj<=neq;jj++){
ddfddt[ii]+=(jac.dfdy[ii][jj])*f0[jj];
}
}
rh = 0.0;
for(ii=1;ii<=neq;ii++){
ddfddt[ii] += (tempvec1[ii] - f0[ii]) / tdel;
rh = MAX(rh,1.25*sqrt(0.5*fabs(ddfddt[ii]/wt[ii])/rtol));
}
absh = MIN(hmax, htspan);
if (absh * rh > 1.0) absh = 1.0 / rh;
absh = MAX(absh, hmin);
h = tdir * absh;
/* Done calculating initial step
Get ready to do the loop:*/
k = 1; /*start at order 1 with BDF1 */
klast = k;
abshlast = absh;
for(ii=1;ii<=neq;ii++) dif[ii][1] = h*f0[ii];
hinvGak = h*invGa[k-1];
nconhk = 0; /*steps taken with current h and k*/
class_call(new_linearisation(&jac,hinvGak,neq,error_message),
error_message,error_message);
stepstat[4] += 1;
havrate = _FALSE_; /*false*/
/* Doing main loop: */
done = _FALSE_;
at_hmin = _FALSE_;
while (done==_FALSE_){
hmin = minimum_variation;
maxtmp = MAX(hmin,absh);
absh = MIN(hmax, maxtmp);
if (fabs(absh-hmin)<100*eps){
/* If the stepsize has not changed */
if (at_hmin==_TRUE_){
absh = abshlast; /*required by stepsize recovery */
}
at_hmin = _TRUE_;
}
else{
at_hmin = _FALSE_;
}
h = tdir * absh;
/* Stretch the step if within 10% of tfinal-t. */
if (1.1*absh >= fabs(tfinal - t)){
h = tfinal - t;
absh = fabs(h);
done = _TRUE_;
}
if (((fabs(absh-abshlast)/absh)>1e-6)||(k!=klast)){
adjust_stepsize(dif,(absh/abshlast),neq,k);
hinvGak = h * invGa[k-1];
nconhk = 0;
class_call(new_linearisation(&jac,hinvGak,neq,error_message),
error_message,error_message);
stepstat[4] += 1;
havrate = _FALSE_;
}
/* Loop for advancing one step */
nofailed = _TRUE_;
for( ; ; ){
gotynew = _FALSE_; /* is ynew evaluated yet?*/
while(gotynew==_FALSE_){
/*Compute the constant terms in the equation for ynew.
Next FOR lop is just: psi = matmul(dif(:,1:k),(G(1:k) * invGa(k)))*/
for(ii=1;ii<=neq;ii++){
psi[ii] = 0.0;
for(jj=1;jj<=k;jj++){
psi[ii] += dif[ii][jj]*G[jj-1]*invGa[k-1];
}
}
/* Predict a solution at t+h. */
tnew = t + h;
if (done==_TRUE_){
tnew = tfinal; /*Hit end point exactly. */
}
h = tnew - t; /* Purify h. */
for(ii=1;ii<=neq;ii++){
pred[ii] = y[ii];
for(jj=1;jj<=k;jj++){
pred[ii] +=dif[ii][jj];
}
}
eqvec(pred,ynew,neq);
/*The difference, difkp1, between pred and the final accepted
ynew is equal to the backward difference of ynew of order
k+1. Initialize to zero for the iteration to compute ynew.
*/
minnrm = 0.0;
for(j=1;j<=neq;j++){
difkp1[j] = 0.0;
maxtmp = MAX(fabs(ynew[j]),fabs(y[j]));
invwt[j] = 1.0 / MAX(maxtmp,threshold);
maxtmp = 100*eps*fabs(ynew[j]*invwt[j]);
minnrm = MAX(minnrm,maxtmp);
}
/* Iterate with simplified Newton method. */
tooslow = _FALSE_;
for(iter=1;iter<=maxit;iter++){
for (ii=1;ii<=neq;ii++){
tempvec1[ii]=(psi[ii]+difkp1[ii]);
}
class_call((*derivs)(tnew,ynew+1,f0+1,parameters_and_workspace_for_derivs,error_message),
error_message,error_message);
stepstat[2] += 1;
for(j=1;j<=neq;j++){
rhs[j] = hinvGak*f0[j]-tempvec1[j];
}
/*Solve the linear system A*x=del by using the LU decomposition stored in jac.*/
if (jac.use_sparse){
sp_lusolve(jac.Numerical, rhs+1, del+1);
}
else{
eqvec(rhs,del,neq);
lubksb(jac.LU,neq,jac.luidx,del);
}
stepstat[5]+=1;
newnrm = 0.0;
for(j=1;j<=neq;j++){
maxtmp = fabs(del[j]*invwt[j]);
newnrm = MAX(newnrm,maxtmp);
}
for(j=1;j<=neq;j++){
difkp1[j] += del[j];
ynew[j] = pred[j] + difkp1[j];
}
if (newnrm <= minnrm){
gotynew = _TRUE_;
break; /* Break Newton loop */
}
else if(iter == 1){
if (havrate==_TRUE_){
errit = newnrm * rate / (1.0 - rate);
if (errit <= 0.05*rtol){
gotynew = _TRUE_;
break; /* Break Newton Loop*/
}
}
else {
rate = 0.0;
}
}
else if(newnrm > 0.9*oldnrm){
tooslow = _TRUE_;
break; /*Break Newton lop */
}
else{
rate = MAX(0.9*rate, newnrm / oldnrm);
havrate = _TRUE_;
errit = newnrm * rate / (1.0 - rate);
if (errit <= 0.5*rtol){
gotynew = _TRUE_;
break; /* exit newton */
}
else if (iter == maxit){
tooslow = _TRUE_;
break; /*exit newton */
}
else if (0.5*rtol < errit*pow(rate,(maxit-iter))){
tooslow = _TRUE_;
break; /*exit Newton */
}
}
oldnrm = newnrm;
}
if (tooslow==_TRUE_){
stepstat[1] += 1;
/* ! Speed up the iteration by forming new linearization or reducing h. */
if (Jcurrent==_FALSE_){
class_call((*derivs)(t,y+1,f0+1,parameters_and_workspace_for_derivs,error_message),
error_message,error_message);
nfenj=0;
class_call(numjac((*derivs),t,y,f0,&jac,&nj_ws,abstol,neq,
&nfenj,parameters_and_workspace_for_derivs,error_message),
error_message,error_message);
stepstat[3] += 1;
stepstat[2] += (nfenj + 1);
Jcurrent = _TRUE_;
}
else if (absh <= hmin){
class_test(absh <= hmin, error_message,
"Step size too small: step:%g, minimum:%g, in interval: [%g:%g]\n",
absh,hmin,t0,tfinal);
}
else{
abshlast = absh;
absh = MAX(0.3 * absh, hmin);
h = tdir * absh;
done = _FALSE_;
adjust_stepsize(dif,(absh/abshlast),neq,k);
hinvGak = h * invGa[k-1];
nconhk = 0;
}
/* A new linearisation is needed in both cases */
class_call(new_linearisation(&jac,hinvGak,neq,error_message),
error_message,error_message);
stepstat[4] += 1;
havrate = _FALSE_;
}
}
/*end of while loop for getting ynew
difkp1 is now the backward difference of ynew of order k+1. */
err = 0.0;
for(jj=1;jj<=neq;jj++){
err = MAX(err,fabs(difkp1[jj]*invwt[jj]));
}
err = err * erconst[k-1];
if (err>rtol){
/*Step failed */
stepstat[1]+= 1;
if (absh <= hmin){ //BEN FLAG: I REMOVED THIS FOR NO GOOD REASON!/
class_test(absh <= hmin, error_message,
"Step size too small: step:%g, minimum:%g, in interval: [%g:%g]\n",
absh,hmin,t0,tfinal);
}
abshlast = absh;
if (nofailed==_TRUE_){
nofailed = _FALSE_;
hopt = absh * MAX(0.1, 0.833*pow((rtol/err),(1.0/(k+1))));
if (k > 1){
errkm1 = 0.0;
for(jj=1;jj<=neq;jj++){
errkm1 = MAX(errkm1,fabs((dif[jj][k]+difkp1[jj])*invwt[jj]));
}
errkm1 = errkm1*erconst[k-2];
hkm1 = absh * MAX(0.1, 0.769*pow((rtol/errkm1),(1.0/k)));
if (hkm1 > hopt){
hopt = MIN(absh,hkm1); /* don't allow step size increase */
k = k - 1;
}
}
absh = MAX(hmin, hopt);
}
else{
absh = MAX(hmin, 0.5 * absh);
}
h = tdir * absh;
if (absh < abshlast){
done = _FALSE_;
}
adjust_stepsize(dif,(absh/abshlast),neq,k);
hinvGak = h * invGa[k-1];
nconhk = 0;
class_call(new_linearisation(&jac,hinvGak,neq,error_message),
error_message,error_message);
stepstat[4] += 1;
havrate = _FALSE_;
}
else {
break; /* Succesfull step */
}
}
/* End of conditionless FOR loop */
stepstat[0] += 1;
/* Update dif: */
for(jj=1;jj<=neq;jj++){
dif[jj][k+2] = difkp1[jj] - dif[jj][k+1];
dif[jj][k+1] = difkp1[jj];
}
for(j=k;j>=1;j--){
for(ii=1;ii<=neq;ii++){
dif[ii][j] += dif[ii][j+1];
}
}
/** Output **/
while ((next<tres)&&(tdir * (tnew - t_vec[next]) >= 0.0)){
/* Do we need to write output? */
if (tnew==t_vec[next]){
class_call((*output)(t_vec[next],ynew+1,f0+1,next,parameters_and_workspace_for_derivs,error_message),
error_message,error_message);
// MODIFICATION BY LUC
// All print_variables have been moved to the end of time step
/*
if (print_variables != NULL){
class_call((*print_variables)(t_vec[next],ynew+1,f0+1,
parameters_and_workspace_for_derivs,error_message),
error_message,error_message);
}
*/
}
else {
/*Interpolate if we have overshot sample values*/
interp_from_dif(t_vec[next],tnew,ynew,h,dif,k,yinterp,ypinterp,yppinterp,interpidx,neq,2);
class_call((*output)(t_vec[next],yinterp+1,ypinterp+1,next,parameters_and_workspace_for_derivs,
error_message),error_message,error_message);
}
next++;
}
/** End of output **/
if (done==_TRUE_) {
break;
}
klast = k;
abshlast = absh;
nconhk = MIN(nconhk+1,maxk+2);
if (nconhk >= k + 2){
temp = 1.2*pow((err/rtol),(1.0/(k+1.0)));
if (temp > 0.1){
hopt = absh / temp;
}
else {
hopt = 10*absh;
}
kopt = k;
if (k > 1){
errkm1 = 0.0;
for(jj=1;jj<=neq;jj++){
errkm1 = MAX(errkm1,fabs(dif[jj][k]*invwt[jj]));
}
errkm1 = errkm1*erconst[k-2];
temp = 1.3*pow((errkm1/rtol),(1.0/k));
if (temp > 0.1){
hkm1 = absh / temp;
}
else {
hkm1 = 10*absh;
}
if (hkm1 > hopt){
hopt = hkm1;
kopt = k - 1;
}
}
if (k < maxk){
errkp1 = 0.0;
for(jj=1;jj<=neq;jj++){
errkp1 = MAX(errkp1,fabs(dif[jj][k+2]*invwt[jj]));
}
errkp1 = errkp1*erconst[k];
temp = 1.4*pow((errkp1/rtol),(1.0/(k+2.0)));
if (temp > 0.1){
hkp1 = absh / temp;
}
else {
hkp1 = 10*absh;
}
if (hkp1 > hopt){
hopt = hkp1;
kopt = k + 1;
}
}
if (hopt > absh){
absh = hopt;
if (k!=kopt){
k = kopt;
}
}
}
/* Advance the integration one step. */
t = tnew;
eqvec(ynew,y,neq);
Jcurrent = _FALSE_;
// MODIFICATION BY LUC
if (print_variables!=NULL){
class_call((*derivs)(tnew,
ynew+1,
f0+1,
parameters_and_workspace_for_derivs,error_message),
error_message,
error_message);
class_call((*print_variables)(tnew,ynew+1,f0+1,
parameters_and_workspace_for_derivs,error_message),
error_message,error_message);
}
// end of modification
}
/* a last call is compulsory to ensure that all quantitites in
y,dy,parameters_and_workspace_for_derivs are updated to the
last point in the covered range */
class_call(
(*derivs)(tnew,
ynew+1,
f0+1,
parameters_and_workspace_for_derivs,error_message),
error_message,
error_message);
if (verbose > 0){
printf("\n End of evolver. Next=%d, t=%e and tnew=%e.",next,t,tnew);
printf("\n Statistics: [%d %d %d %d %d %d] \n",stepstat[0],stepstat[1],
stepstat[2],stepstat[3],stepstat[4],stepstat[5]);
}
/** Deallocate memory */
free(buffer);
/* free(f0); */
/* free(wt); */
/* free(ddfddt); */
/* free(pred); */
/* free(y); */
/* free(invwt); */
/* free(rhs); */
/* free(psi); */
/* free(difkp1); */
/* free(del); */
/* free(yinterp); */
/* free(ypinterp); */
/* free(yppinterp); */
/* free(tempvec1); */
/* free(tempvec2); */
/* free(interpidx); */
/* free(dif[1]); */
/* free(dif); */
uninitialize_jacobian(&jac);
uninitialize_numjac_workspace(&nj_ws);
return _SUCCESS_;
} /*End of program*/
int initialize_jacobian(struct jacobian *jac, int neq, ErrorMsg error_message){
int i;
if (neq>15){
jac->use_sparse = 1;
}
else{
jac->use_sparse = 0;
}
jac->max_nonzero = (int)(MAX(3*neq,0.20*neq*neq));
jac->cnzmax = 12*jac->max_nonzero/5;
/*Maximal number of non-zero entries to be considered sparse */
jac->repeated_pattern = 0;
jac->trust_sparse = 4;
/* Number of times a pattern is repeated before we trust it. */
jac->has_grouping = 0;
jac->has_pattern = 0;
jac->sparse_stuff_initialized=0;
/*Setup memory for the pointers of the dense method:*/
class_alloc(jac->dfdy,sizeof(double*)*(neq+1),error_message); /* Allocate vector of pointers to rows of matrix.*/
class_alloc(jac->dfdy[1],sizeof(double)*(neq*neq+1),error_message);
jac->dfdy[0] = NULL;
for(i=2;i<=neq;i++) jac->dfdy[i] = jac->dfdy[i-1]+neq; /* Set row pointers... */
class_alloc(jac->LU,sizeof(double*)*(neq+1),error_message); /* Allocate vector of pointers to rows of matrix.*/
class_alloc(jac->LU[1],sizeof(double)*(neq*neq+1),error_message);
jac->LU[0] = NULL;
for(i=2;i<=neq;i++) jac->LU[i] = jac->LU[i-1]+neq; /* Set row pointers... */
class_alloc(jac->LUw,sizeof(double)*(neq+1),error_message);
class_alloc(jac->jacvec,sizeof(double)*(neq+1),error_message);
class_alloc(jac->luidx,sizeof(int)*(neq+1),error_message);
/*Setup memory for the sparse method, if used: */
if (jac->use_sparse){
jac->sparse_stuff_initialized = 1;
jac->xjac=(double*)(jac->luidx+neq+1);
jac->col_group=(int*)(jac->xjac+jac->max_nonzero);
jac->col_wi=jac->col_group+neq;
jac->Cp=jac->col_wi+neq;
jac->Ci=jac->Cp+neq+1;
class_alloc(jac->xjac,sizeof(double)*jac->max_nonzero,error_message);
class_alloc(jac->col_group,sizeof(int)*neq,error_message);
class_alloc(jac->col_wi,sizeof(int)*neq,error_message);
class_alloc(jac->Cp,sizeof(int)*(neq+1),error_message);
class_alloc(jac->Ci,sizeof(int)*jac->cnzmax,error_message);
class_call(sp_num_alloc(&jac->Numerical, neq,error_message),
error_message,error_message);
class_call(sp_mat_alloc(&jac->spJ, neq, neq, jac->max_nonzero,
error_message),error_message,error_message);
}
/* Initialize jacvec to sqrt(eps):*/
for (i=1;i<=neq;i++) jac->jacvec[i]=1.490116119384765597872e-8;
return _SUCCESS_;
}
int numjac(
int (*derivs)(double x, double * y,double * dy,
void * parameters_and_workspace, ErrorMsg error_message),
double t, double *y, double *fval,
struct jacobian *jac, struct numjac_workspace *nj_ws,
double thresh, int neq, int *nfe, void * parameters_and_workspace_for_derivs,
ErrorMsg error_message){
/* Routine that computes the jacobian numerically. It is based on the numjac
implementation in MATLAB, but a feature for recognising sparsity in the
jacobian and taking advantage of that has been added.
*/
double eps=1e-19, br=pow(eps,0.875),bl=pow(eps,0.75),bu=pow(eps,0.25);
double facmin=pow(eps,0.78),facmax=0.1;
int logjpos, pattern_broken;
double tmpfac,difmax2=0.,del2,ffscale;
int i,j,rowmax2;
double maxval1,maxval2;
int colmax,group,row,nz,nz2;
double Fdiff_absrm,Fdiff_new;
double **dFdy,*fac;
int *Ap=NULL, *Ai=NULL;
dFdy = jac->dfdy; /* Assign pointer to dfdy directly for easier notation. */
fac = jac->jacvec;
if (jac->use_sparse){
Ap = jac->spJ->Ap;
Ai = jac->spJ->Ai;
}
/* Set new_jacobian flag: */
jac->new_jacobian = _TRUE_;
for(j=1;j<=neq;j++){
nj_ws->yscale[j] = MAX(fabs(y[j]),thresh);
nj_ws->del[j] = (y[j] + fac[j] * nj_ws->yscale[j]) - y[j];
}
/*Select an increment del for a difference approximation to
column j of dFdy. The vector fac accounts for experience
gained in previous calls to numjac.
*/
for(j=1;j<=neq;j++){
if (nj_ws->del[j]==0.0){
for(;;){
if (fac[j] < facmax){
fac[j] = MIN(100*fac[j],facmax);
nj_ws->del[j] = (y[j] + fac[j]*nj_ws->yscale[j]) - y[j];
if(nj_ws->del[j]==0.0){
break;
}
}
else{
nj_ws->del[j] = thresh;
break;
}
}
}
}
/* keep del pointing into region: */
for(j=1;j<=neq;j++){
if (fval[j]>=0.0){
nj_ws->del[j] = fabs(nj_ws->del[j]);
}
else{
nj_ws->del[j] = -fabs(nj_ws->del[j]);
}
}
/* Sparse calculation?*/
if ((jac->use_sparse)&&(jac->repeated_pattern >= jac->trust_sparse)){
/* printf("\n Sparse calculation..neq=%d, has grouping=%d",neq,jac->has_grouping);*/
/* Everything done sparse'ly. Do we have a grouping? */
if (jac->has_grouping==0){
jac->max_group = column_grouping(jac->spJ,jac->col_group,jac->col_wi);
jac->has_grouping = 1;
}
colmax = jac->max_group+1;
/* printf("\n ->groups=%d/%d.",colmax,neq); */
for(j=1;j<=colmax;j++){
/*loop over groups */
group = j-1;
for(i=1;i<=neq;i++){
/*Add y-vector.. */
nj_ws->ydel_Fdel[i][j] = y[i];
/*Add del of all groupmembers:*/
if(jac->col_group[i-1]==group) nj_ws->ydel_Fdel[i][j] +=nj_ws->del[i];
}
}
}
else{
/*printf("\n Normal calculation..."); */
/*Normal calculation: */
colmax = neq;
for(j=1;j<=neq;j++){
for(i=1;i<=neq;i++){
nj_ws->ydel_Fdel[i][j] = y[i];
}
nj_ws->ydel_Fdel[j][j] += nj_ws->del[j];
}
}
/* The next section should work regardless of sparse...*/
/* Evaluate the function at y+delta vectors:*/
for(j=1;j<=colmax;j++){
for(i=1;i<=neq;i++){
nj_ws->yydel[i] = nj_ws->ydel_Fdel[i][j];
}
class_call((*derivs)(t,nj_ws->yydel+1,nj_ws->ffdel+1,
parameters_and_workspace_for_derivs,error_message),
error_message,error_message);
*nfe+=1;
for(i=1;i<=neq;i++) nj_ws->ydel_Fdel[i][j] = nj_ws->ffdel[i];
}
/*Using the Fdel array, form the jacobian and construct max-value arrays.
First we do it for the sparse case, then for the normal case:*/
if ((jac->use_sparse)&&(jac->repeated_pattern >= jac->trust_sparse)){
/* Sparse case:*/
for(j=0;j<neq;j++){
/*Loop over columns, and assign corresponding group:*/
group = jac->col_group[j];
Fdiff_new = 0.0;
Fdiff_absrm = 0.0;
for(i=Ap[j];i<Ap[j+1];i++){
/* Loop over rows in the sparse matrix */
row = Ai[i]+1;
/* Do I want to construct the full jacobian? No, that is ugly..*/
Fdiff_absrm = MAX(Fdiff_absrm,fabs(Fdiff_new));
Fdiff_new = nj_ws->ydel_Fdel[row][group+1]-fval[row]; /*Remember to access the column of the corresponding group */
if (fabs(Fdiff_new)>=Fdiff_absrm){
nj_ws->Rowmax[j+1] = row;
nj_ws->Difmax[j+1] = Fdiff_new;
}
/* Assign value to sparse rep of jacobian: */
jac->xjac[i] = Fdiff_new/nj_ws->del[j+1];
}
/* The maximum numerical value of Fdel in true column j+1*/
nj_ws->absFdelRm[j+1] = fabs(nj_ws->ydel_Fdel[nj_ws->Rowmax[j+1]][group+1]);
}
}
else{
/*Normal case:*/
for(j=1;j<=neq;j++){
Fdiff_new = 0.0;
Fdiff_absrm = 0.0;
for(i=1;i<=neq;i++){
Fdiff_absrm = MAX(fabs(Fdiff_new),Fdiff_absrm);
Fdiff_new = nj_ws->ydel_Fdel[i][j] - fval[i];
dFdy[i][j] = Fdiff_new/nj_ws->del[j];
/*Find row maximums:*/
if(fabs(Fdiff_new)>=Fdiff_absrm){
/* Found new max location in column */
nj_ws->Rowmax[j] = i;
nj_ws->Difmax[j] = fabs(Fdiff_new);
}
}
nj_ws->absFdelRm[j] = fabs(nj_ws->ydel_Fdel[nj_ws->Rowmax[j]][j]);
}
}
/* Adjust fac for next call to numjac. */
for(i=1;i<=neq;i++){
nj_ws->absFvalue[i] = fabs(fval[i]);
}
for(j=1;j<=neq;j++){
nj_ws->absFvalueRm[j] = nj_ws->absFvalue[nj_ws->Rowmax[j]];
}
logjpos = 0;
for(j=1;j<=neq;j++){
if (((nj_ws->absFdelRm[j]<TINY)&&(nj_ws->absFvalueRm[j] < TINY))||(fabs(nj_ws->Difmax[j])<TINY)){
nj_ws->logj[j] = 1;/*.true.*/
logjpos = 1;
}
else{
nj_ws->logj[j] = 0;
}
}
if (logjpos ==1){
for(i=1;i<=neq;i++){
nj_ws->yydel[i] = y[i];
nj_ws->Fscale[i] = MAX(nj_ws->absFdelRm[i],nj_ws->absFvalueRm[i]);
}
/* If the difference in f values is so small that the column might be just
! roundoff error, try a bigger increment. */
for(j=1;j<=neq;j++){
if ((nj_ws->logj[j]==1)&&(nj_ws->Difmax[j]<=(br*nj_ws->Fscale[j]))){
tmpfac = MIN(sqrt(fac[j]),facmax);
del2 = (y[j] + tmpfac*nj_ws->yscale[j]) - y[j];
if ((tmpfac!=fac[j])&&(del2!=0.0)){
if (fval[j] >= 0.0){
/*! keep del pointing into region */
del2 = fabs(del2);
}
else{
del2 = -fabs(del2);
}
nj_ws->yydel[j] = y[j] + del2;
class_call((*derivs)(t,nj_ws->yydel+1,nj_ws->ffdel+1,
parameters_and_workspace_for_derivs,error_message),
error_message,error_message);
*nfe+=1;
nj_ws->yydel[j] = y[j];
rowmax2 = 1;
Fdiff_new=0.0;
Fdiff_absrm = 0.0;
for(i=1;i<=neq;i++){
Fdiff_absrm = MAX(Fdiff_absrm,fabs(Fdiff_new));
Fdiff_new = nj_ws->ffdel[i]-fval[i];
nj_ws->tmp[i] = Fdiff_new/del2;
if(fabs(Fdiff_new)>=Fdiff_absrm){
rowmax2 = i;
difmax2 = fabs(Fdiff_new);
}
}
maxval1 = difmax2*fabs(del2)*tmpfac;
maxval2 = nj_ws->Difmax[j]*fabs(nj_ws->del[j]);
if(maxval1>=maxval2){
/* The new difference is more significant, so
use the column computed with this increment.
This depends on wether we are in sparse mode or not: */
if ((jac->use_sparse)&&(jac->repeated_pattern >= jac->trust_sparse)){
for(i=Ap[j-1];i<Ap[j];i++) jac->xjac[i]=nj_ws->tmp[Ai[i]+1];
}
else{
for(i=1;i<=neq;i++) dFdy[i][j]=nj_ws->tmp[i];
}
/* Adjust fac for the next call to numjac. */
ffscale = MAX(fabs(nj_ws->ffdel[rowmax2]),nj_ws->absFvalue[rowmax2]);
if (difmax2 <= bl*ffscale){
/* The difference is small, so increase the increment. */
fac[j] = MIN(10*tmpfac, facmax);
}
else if(difmax2 > bu*ffscale){
/* The difference is large, so reduce the increment. */
fac[j] = MAX(0.1*tmpfac, facmin);
}
else{
fac[j] = tmpfac;
}
}
}
}
}
}
/* If use_sparse is true but I still don't trust the sparsity pattern, go through the full calculated jacobi-
matrix, deduce the sparsity pattern, compare with the old pattern, and write the new sparse Jacobi matrix.
If I do this cleverly, I only have to walk through the jacobian once, and I don't need any local storage.*/
if ((jac->use_sparse)&&(jac->repeated_pattern < jac->trust_sparse)){
nz=0; /*Number of non-zeros */
Ap[0]=0; /*<-Always is.. */
pattern_broken = _FALSE_;
for(j=1;j<=neq;j++){
for(i=1;i<=neq;i++){
if ((i==j)||(fabs(dFdy[i][j])!=0.0)){
/* Diagonal or non-zero index found. */
if (nz>=jac->max_nonzero){
/* Too many non-zero points to take advantage of sparsity.*/
jac->use_sparse = 0;
break;
}
/* Test pattern if it is still unbroken: */
/* Two conditions must be met if the pattern is intact: Ap[j-1]<=nz<Ap[j],
so that we are in the right column, and (i-1) must exist in column. Ai[nz]*/
/* We should first test if nz is in the column, otherwise pattern is dead:*/
if ((pattern_broken==_FALSE_)&&(jac->has_pattern==_TRUE_)){
if ((nz<Ap[j-1])||(nz>=Ap[j])){
/* If we are no longer in the right column, pattern is broken for sure. */
pattern_broken = _TRUE_;
}
}
if ((pattern_broken==_FALSE_)&&(jac->has_pattern==_TRUE_)){
/* Up to this point, the new jacobian has managed to fit in the old
sparsity pattern..*/
if (Ai[nz]!=(i-1)){
/* The current non-zero rownumber does not fit the current entry in the
sparse matrix. Pattern MIGHT be broken. Scan ahead in the sparse matrix
to search for the row entry: (Remember: the indices are sorted..)*/
pattern_broken = _TRUE_;
for(nz2=nz; (nz2<Ap[j])&&(Ai[nz2]<=(i-1)); nz2++){
/* Go through the rest of the column with the added constraint that
the row index in the sparse matrix should be smaller than the current
row index i-1:*/
if (Ai[nz2]==(i-1)){
/* sparsity pattern recovered.. */
pattern_broken = _FALSE_;
nz = nz2;
break;
}
/* Write a zero entry in the sparse matrix, in case we recover pattern. */
jac->xjac[nz2] = 0.0;
}
}
}
/* The following works no matter the status of the pattern: */
/* Write row_number: */
Ai[nz] = i-1;
/* Write value: */
jac->xjac[nz] = dFdy[i][j];
nz++;
}
}
/* Break this loop too if I have hit max non-zero points: */
if (jac->use_sparse==_FALSE_) break;
Ap[j]=nz;
}
if (jac->use_sparse==_TRUE_){
if ((jac->has_pattern==_TRUE_)&&(pattern_broken==_FALSE_)){
/*New jacobian fitted into the current sparsity pattern:*/
jac->repeated_pattern++;
/* printf("\n Found repeated pattern. nz=%d/%d and
rep.pat=%d.",nz,neq*neq,jac->repeated_pattern); */
}
else{
/*Something has changed (or first run), better still do the full calculation..*/
jac->repeated_pattern = 0;
}
jac->has_pattern = 1;
}
}
return _SUCCESS_;
} /* End of numjac */
int fzero_Newton(int (*func)(double *x,
int x_size,
void *param,
double *F,
ErrorMsg error_message),
double *x_inout,
double *dxdF,
int x_size,
double tolx,
double tolF,
void *param,
int *fevals,
ErrorMsg error_message){
/**Given an initial guess x[1..n] for a root in n dimensions,
take ntrial Newton-Raphson steps to improve the root.
Stop if the root converges in either summed absolute
variable increments tolx or summed absolute function values tolf.*/
int k,i,j,*indx, ntrial=20;
double errx,errf,d,*F0,*Fdel,**Fjac,*p, *lu_work;
int has_converged = _FALSE_;
double toljac = 1e-3;
double *delx;
/** All arrays are indexed as [0, n-1] with the exception of p, indx,
lu_work and Fjac, since they are passed to ludcmp and lubksb. */
class_alloc(indx, sizeof(int)*(x_size+1), error_message);
class_alloc(p, sizeof(double)*(x_size+1), error_message);
class_alloc(lu_work, sizeof(double)*(x_size+1), error_message);
class_alloc(Fjac, sizeof(double *)*(x_size+1), error_message);
Fjac[0] = NULL;
class_alloc(Fjac[1], sizeof(double)*(x_size*x_size+1), error_message);
for (i=2; i<=x_size; i++){
Fjac[i] = Fjac[i-1] + x_size;
}
class_alloc(F0, sizeof(double)*x_size, error_message);
class_alloc(delx, sizeof(double)*x_size, error_message);
class_alloc(Fdel, sizeof(double)*x_size, error_message);
for (i=1; i<=x_size; i++){
delx[i-1] = toljac*dxdF[i-1];
}
for (k=1;k<=ntrial;k++) {
/** Compute F(x): */
/**printf("x = [%f, %f], delx = [%e, %e]\n",
x_inout[0],x_inout[1],delx[0],delx[1]);*/
class_call(func(x_inout, x_size, param, F0, error_message),
error_message, error_message);
/** printf("F0 = [%f, %f]\n",F0[0],F0[1]);*/
*fevals = *fevals + 1;
errf=0.0; //fvec and Jacobian matrix in fjac.
for (i=1; i<=x_size; i++)
errf += fabs(F0[i-1]); //Check function convergence.
if (errf <= tolF){
has_converged = _TRUE_;
break;
}
/**
if (k==1){
for (i=1; i<=x_size; i++){
delx[i-1] *= F0[i-1];
}
}
*/
/** Compute the jacobian of F: */
for (i=1; i<=x_size; i++){
if (F0[i-1]<0.0)
delx[i-1] *= -1;
x_inout[i-1] += delx[i-1];
/** printf("x = [%f, %f], delx = [%e, %e]\n",
x_inout[0],x_inout[1],delx[0],delx[1]);*/
class_call(func(x_inout, x_size, param, Fdel, error_message),
error_message, error_message);
/** printf("F = [%f, %f]\n",Fdel[0],Fdel[1]);*/
for (j=1; j<=x_size; j++)
Fjac[j][i] = (Fdel[j-1]-F0[j-1])/delx[i-1];
x_inout[i-1] -= delx[i-1];
}
*fevals = *fevals + x_size;
for (i=1; i<=x_size; i++)
p[i] = -F0[i-1]; //Right-hand side of linear equations.
ludcmp(Fjac, x_size, indx, &d, lu_work); //Solve linear equations using LU decomposition.
lubksb(Fjac, x_size, indx, p);
errx=0.0; //Check root convergence.
for (i=1; i<=x_size; i++) { //Update solution.
errx += fabs(p[i]);
x_inout[i-1] += p[i];
}
if (errx <= tolx){
has_converged = _TRUE_;
break;
}
}
free(p);
free(lu_work);
free(indx);
free(Fjac[1]);
free(Fjac);
free(F0);
free(delx);
free(Fdel);
if (has_converged == _TRUE_){
return _SUCCESS_;
}
else{
class_stop(error_message, "Newton's method failed to converge. Try improving initial guess on the parameters, decrease the tolerance requirements to Newtons method or increase the precision of the input function.\n");
}
}
