Xcos solvers : Sundials Dormand-Price 4(5) implementation
[scilab.git] / scilab / modules / scicos / src / scicos_sundials / src / cvode / cvode.c
index 3b7559c..c0fdb3f 100644 (file)
 #include "sundials_extension.h"
 
 /*=================================================================*/
-/*             Macros                                              */
-/*=================================================================*/
-
-/* Macro: loop */
-#define loop for(;;)
-
-/*=================================================================*/
 /*             CVODE Private Constants                             */
 /*=================================================================*/
 
@@ -247,10 +240,12 @@ static int CVEwtSetSS(CVodeMem cv_mem, N_Vector ycur, N_Vector weight);
 static int CVEwtSetSV(CVodeMem cv_mem, N_Vector ycur, N_Vector weight);
 
 static int CVHin(CVodeMem cv_mem, realtype tout);
+static int CVHinFixed(CVodeMem cv_mem, realtype tout, realtype *tret);
 static realtype CVUpperBoundH0(CVodeMem cv_mem, realtype tdist);
 static int CVYddNorm(CVodeMem cv_mem, realtype hg, realtype *yddnrm);
 
 static int CVStep(CVodeMem cv_mem);
+static int CVStepDoPri(CVodeMem cv_mem);
 
 static int CVsldet(CVodeMem cv_mem);
 
@@ -303,6 +298,7 @@ static int CVRcheck2(CVodeMem cv_mem);
 static int CVRcheck3(CVodeMem cv_mem);
 static int CVRootfind(CVodeMem cv_mem);
 
+/* SUNDIALS STANDARD */
 static int CVRcheck1Std(CVodeMem cv_mem);
 static int CVRcheck2Std(CVodeMem cv_mem);
 static int CVRcheck3Std(CVodeMem cv_mem);
@@ -339,7 +335,7 @@ void *CVodeCreate(int lmm, int iter)
 
   /* Test inputs */
 
-  if ((lmm != CV_ADAMS) && (lmm != CV_BDF)) {
+  if ((lmm != CV_ADAMS) && (lmm != CV_BDF) && (lmm != CV_DOPRI)) { /* Integration mode : ADAMS, BDF or DOPRI */
     CVProcessError(NULL, 0, "CVODE", "CVodeCreate", MSGCV_BAD_LMM);
     return(NULL);
   }
@@ -358,6 +354,9 @@ void *CVodeCreate(int lmm, int iter)
 
   maxord = (lmm == CV_ADAMS) ? ADAMS_Q_MAX : BDF_Q_MAX;
 
+  /* If Runge-Kutta is selected, then maxord = 9 to use the 7 extra vectors allocated (zn[2, 3, 4, 5, 6, 7, 8]) */
+  maxord = (lmm == CV_DOPRI) ? 8 : maxord;
+
   /* copy input parameters into cv_mem */
   cv_mem->cv_lmm  = lmm;
   cv_mem->cv_iter = iter;
@@ -1290,8 +1289,10 @@ int CVode(void *cvode_mem, realtype tout, N_Vector yout,
       return(CV_FIRST_RHSFUNC_ERR);
     }
 
-    /* Set initial h (from H0 or CVHin). */
+    /* Set initial h (from H0 or CVHin). 
+       If Dormand-Price is selected, then we choose to set h to hmax (= 1/hmax_inv). */
 
+    if ((lmm == CV_ADAMS) || (lmm == CV_BDF)) {
     h = hin;
     if ( (h != ZERO) && ((tout-tn)*h < ZERO) ) {
       CVProcessError(cv_mem, CV_ILL_INPUT, "CVODE", "CVode", MSGCV_BAD_H0);
@@ -1317,6 +1318,11 @@ int CVode(void *cvode_mem, realtype tout, N_Vector yout,
     rh = ABS(h)*hmax_inv;
     if (rh > ONE) h /= rh;
     if (ABS(h) < hmin) h *= hmin/ABS(h);
+       }
+
+    else { /* Compute the fixed step size h, and set the max number of steps */
+      mxstep = CVHinFixed(cv_mem, tout, tret);
+    }
 
     /* Check for approach to tstop */
 
@@ -1531,7 +1537,7 @@ int CVode(void *cvode_mem, realtype tout, N_Vector yout,
       }
 
     } /* end of istop tests block */
-    
+    if (lmm==CV_DOPRI) mxstep = CVHinFixed(cv_mem, tout, tret);
   } /* end stopping tests block */  
 
   /*
@@ -1551,7 +1557,7 @@ int CVode(void *cvode_mem, realtype tout, N_Vector yout,
    */
 
   nstloc = 0;
-  loop {
+  for(;;) {
    
     next_h = h;
     next_q = q;
@@ -1563,9 +1569,9 @@ int CVode(void *cvode_mem, realtype tout, N_Vector yout,
 
       if (ewtsetOK != 0) {
 
-        if (itol == CV_WF) 
+        if (itol == CV_WF)
           CVProcessError(cv_mem, CV_ILL_INPUT, "CVODE", "CVode", MSGCV_EWT_NOW_FAIL, tn);
-        else 
+        else
           CVProcessError(cv_mem, CV_ILL_INPUT, "CVODE", "CVode", MSGCV_EWT_NOW_BAD, tn);
        
         istate = CV_ILL_INPUT;
@@ -1609,7 +1615,10 @@ int CVode(void *cvode_mem, realtype tout, N_Vector yout,
     }
 
     /* Call CVStep to take a step */
-    kflag = CVStep(cv_mem);
+    if (lmm == CV_DOPRI)
+    kflag = CVStepDoPri(cv_mem);
+       else
+       kflag = CVStep(cv_mem);
 
     /* Process failed step cases, and exit loop */
     if (kflag != CV_SUCCESS) {
@@ -2079,9 +2088,9 @@ static int CVInitialSetup(CVodeMem cv_mem)
 
 static int CVHin(CVodeMem cv_mem, realtype tout)
 {
-  int retval, sign, count1, count2;
-  realtype tdiff, tdist, tround, hlb, hub;
-  realtype hg, hgs, hs, hnew, hrat, h0, yddnrm;
+  int retval = 0, sign = 0, count1 = 0, count2 = 0;
+  realtype tdiff = 0., tdist = 0., tround = 0., hlb = 0., hub = 0.;
+  realtype hg = 0., hgs = 0., hs = 0., hnew = 0., hrat = 0., h0 = 0., yddnrm = 0.;
   booleantype hgOK, hnewOK;
 
   /* If tout is too close to tn, give up */
@@ -2182,6 +2191,46 @@ static int CVHin(CVodeMem cv_mem, realtype tout)
 }
 
 /*
+ * CVHinFixed
+ *
+ * This routine computes the fixed step size h.
+ * The objective is to approach hmax (= 1/hmax_inv) with h by trying to split the time interval (t-*told) into hmax-long parts.
+ * - if t-*told is smaller than hmax, then set h = t-*told (one iteration)
+ * - if it is divisible by hmax, then set h = hmax.
+ * - if it is not, then "add an integration point" by setting h < hmax, just enough to fit the interval
+ *
+ * Dormand-Price being a fixed step size method, we know the maximum number of steps to take.
+ * This procedure returns that number (minus 2 because nstloc starts at 0).
+ */
+
+static int CVHinFixed(CVodeMem cv_mem, realtype tout, realtype *tret)
+{
+  long int n_points;
+  realtype interval_size, hmax, test_div, floor_test;
+
+  hmax = 1./hmax_inv;
+  interval_size = tout-*tret;
+
+  if (interval_size <= hmax) {  /* "Small" interval, h is the size of it */
+    n_points = 2;
+    h = interval_size;
+  }
+  else {
+    test_div = interval_size/hmax;
+    floor_test = FLOOR(test_div);
+    if (test_div-floor_test <= TINY) {  /* t-*told divisible by hmax, cutting the interval into hmax-long parts */
+      n_points = floor_test+1;
+      h = interval_size/(n_points-1);
+    }
+    else {  /* Adding a point and h < hmax to fit the interval */
+      n_points = floor_test+2;
+      h = interval_size/(n_points-1);
+    }
+  }
+  return(n_points-1);
+}
+
+/*
  * CVUpperBoundH0
  *
  * This routine sets an upper bound on abs(h0) based on
@@ -2283,7 +2332,7 @@ static int CVStep(CVodeMem cv_mem)
   if ((nst > 0) && (hprime != h)) CVAdjustParams(cv_mem);
   
   /* Looping point for attempts to take a step */
-  loop {  
+  for(;;) {  
 
     CVPredict(cv_mem);  
     CVSet(cv_mem);
@@ -2334,6 +2383,117 @@ static int CVStep(CVodeMem cv_mem)
 }
 
 /*
+ * CVStepDoPri
+ *
+ * This routine performs one internal cvode step using the Dormand-Price method, from tn to tn + h.
+ * Proceed to computing the K[i] coefficients, build the final solution, increment tn and return CV_SUCCESS.
+ *
+ * In order to temporarily store the results, we use tempv and ftemp.
+ * While ftempv is just going be an accumulator, tempv will represent the "correction solution".
+ */
+
+static int CVStepDoPri(CVodeMem cv_mem)
+{
+  int retval;
+
+  /* Constant coefficients */
+  realtype c2, c3, c4, c5, b1, b3, b4, b5, b6, d1, d3, d4, d5, d6, d7;
+  realtype a21, a31, a32, a41, a42, a43, a51, a52, a53, a54, a61, a62, a63, a64, a65, a71, a73, a74, a75, a76;
+  a21 =  0.2;                   c2 =  0.2;
+  a31 =  0.075;                 c3 =  0.3;
+  a32 =  0.225;                 c4 =  0.8;
+  a41 =  0.9777777777777778;    c5 =  0.8888888888888888;
+  a42 = -3.7333333333333333;    b1 =  0.0911458333333333;
+  a43 =  3.5555555555555556;    b3 =  0.4492362982929021;
+  a51 =  2.9525986892242036;    b4 =  0.6510416666666667;
+  a52 = -11.595793324188386;    b5 = -0.3223761792452830;
+  a53 =  9.8228928516994361;    b6 =  0.1309523809523810;
+  a54 = -0.2908093278463649;    d1 =  0.0899131944444444;
+  a61 =  2.8462752525252525;    d3 =  0.4534890685834082;
+  a62 = -10.757575757575758;    d4 =  0.6140625;
+  a63 =  8.9064227177434725;    d5 = -0.2715123820754717;
+  a64 =  0.2784090909090909;    d6 =  0.0890476190476190;
+  a65 = -0.2735313036020583;    d7 =  0.025;
+  a71 =  0.0911458333333333;
+  a73 =  0.4492362982929021;
+  a74 =  0.6510416666666667;
+  a75 = -0.3223761792452830;
+  a76 =  0.1309523809523810;
+
+  /* K1, K2 */
+  retval = f(tn, zn[0], zn[2], f_data);                    /* zn[2] = K1 = f(Tn, Yn), */
+  N_VLinearSum_Serial (a21*h, zn[2], 1., zn[0], y);        /* y = K1*a21*h + Yn */
+  retval = f(tn + c2*h, y, zn[3], f_data);                 /* zn[3] = K2 = f(Tn+c2*h, Yn + a21*h*K1), */
+  N_VScale(b1, zn[2], y);                                  /* y = b1*K1 + 0*K2, */
+  N_VScale(d1, zn[2], tempv);                              /* y = d1*K1 + 0*K2, */
+
+  /* K3 */
+  N_VLinearSum_Serial (a31*h, zn[2], 1., zn[0], ftemp);    /* ftemp = Yn + K1*a31*h, */
+  N_VLinearSum_Serial (a32*h, zn[3], 1., ftemp, ftemp);    /* ftemp += K2*a32*h, */
+  retval = f(tn + c3*h, ftemp, zn[4], f_data);             /* zn[4] = K3 = f(Tn+c3*h, Yn + a31*h*K1 + a32*h*K2), */
+  N_VLinearSum_Serial (1., y, b3, zn[4], y);               /* y = b1*K1 + 0*K2 + b3*K3, */
+  N_VLinearSum_Serial (1., tempv, d3, zn[4], tempv);       /* tempv = d1*K1 + 0*K2 + d3*K3, */
+
+  /* K4 */
+  N_VLinearSum_Serial (a41*h, zn[2], 1., zn[0], ftemp);    /* ftemp = Yn + K1*a41*h, */
+  N_VLinearSum_Serial (a42*h, zn[3], 1., ftemp, ftemp);    /* ftemp += K2*a42*h, */
+  N_VLinearSum_Serial (a43*h, zn[4], 1., ftemp, ftemp);    /* ftemp += K3*a43*h, */
+  retval = f(tn + c4*h, ftemp, zn[5], f_data);             /* zn[5] = K4 = f(Tn+c4*h, Yn + a41*h*K1 + a42*h*K2 + a43*h*K3), */
+  N_VLinearSum_Serial (1., y, b4, zn[5], y);               /* y = b1*K1 + 0*K2 + b3*K3 + b4*K4, */
+  N_VLinearSum_Serial (1., tempv, d4, zn[5], tempv);       /* tempv = d1*K1 + 0*K2 + d3*K3 + d4*K4, */
+
+  /* K5 */
+  N_VLinearSum_Serial (a51*h, zn[2], 1., zn[0], ftemp);    /* ftemp = Yn + K1*a51*h, */
+  N_VLinearSum_Serial (a52*h, zn[3], 1., ftemp, ftemp);    /* ftemp += K2*a52*h, */
+  N_VLinearSum_Serial (a53*h, zn[4], 1., ftemp, ftemp);    /* ftemp += K3*a53*h, */
+  N_VLinearSum_Serial (a54*h, zn[5], 1., ftemp, ftemp);    /* ftemp += K4*a54*h, */
+  retval = f(tn + c5*h, ftemp, zn[6], f_data);             /* zn[6] = K5 = f(Tn+c5*h, Yn + a51*h*K1 + a52*h*K2 + a53*h*K3 + a54*h*K4), */
+  N_VLinearSum_Serial (1., y, b5, zn[6], y);               /* y = b1*K1 + 0*K2 + b3*K3 + b4*K4 + b5*K5, */
+  N_VLinearSum_Serial (1., tempv, d5, zn[6], tempv);       /* tempv = d1*K1 + 0*K2 + d3*K3 + d4*K4 + d5*K5, */
+
+  /* K6 */
+  N_VLinearSum_Serial (a61*h, zn[2], 1., zn[0], ftemp);    /* ftemp = Yn + K1*a61*h, */
+  N_VLinearSum_Serial (a62*h, zn[3], 1., ftemp, ftemp);    /* ftemp += K2*a62*h, */
+  N_VLinearSum_Serial (a63*h, zn[4], 1., ftemp, ftemp);    /* ftemp += K3*a63*h, */
+  N_VLinearSum_Serial (a64*h, zn[5], 1., ftemp, ftemp);    /* ftemp += K3*a64*h, */
+  N_VLinearSum_Serial (a65*h, zn[6], 1., ftemp, ftemp);    /* ftemp += K3*a65*h, */
+  retval = f(tn + h, ftemp, zn[7], f_data);                /* zn[7] = K6 = f(Tn+h, Yn + a61*h*K1 + a62*h*K2 + a63*h*K3 + a64*h*K4 + a65*h*K5), */
+  N_VLinearSum_Serial (1., y, b6, zn[7], y);               /* y = b1*K1 + 0*K2 + b3*K3 + b4*K4 + b5*K5 + b6*K6, */
+  N_VLinearSum_Serial (1., tempv, d6, zn[7], tempv);       /* tempv = d1*K1 + 0*K2 + d3*K3 + d4*K4 + d5*K5 + d6*K6, */
+
+  /* K7 */
+  N_VLinearSum_Serial (a71*h, zn[2], 1., zn[0], ftemp);    /* ftemp = Yn + K1*a71*h, */
+  N_VLinearSum_Serial (a73*h, zn[4], 1., ftemp, ftemp);    /* ftemp += K3*a73*h, */
+  N_VLinearSum_Serial (a74*h, zn[5], 1., ftemp, ftemp);    /* ftemp += K3*a74*h, */
+  N_VLinearSum_Serial (a75*h, zn[6], 1., ftemp, ftemp);    /* ftemp += K3*a75*h, */
+  N_VLinearSum_Serial (a76*h, zn[7], 1., ftemp, ftemp);    /* ftemp += K3*a76*h, */
+  retval = f(tn + h, ftemp, zn[8], f_data);                /* zn[8] = K7 = f(Tn+h, Yn + a71*h*K1 + 0*h*K2 + a73*h*K3 + a74*h*K4 + a75*h*K5 + a76*h*K6), */
+  N_VLinearSum_Serial (1., tempv, d7, zn[8], tempv);       /* tempv = d1*K1 + 0*K2 + d3*K3 + d4*K4 + d5*K5 + d6*K6 + d7*K7, */
+
+  /* Yn+1 */
+  N_VLinearSum_Serial(1., zn[0], h, y, zn[0]);             /* zn[0] = Yn+1 = Yn + h*y */
+
+  /* Check for errors in the evaluations of f thanks to retval */
+  if (retval < 0) {
+    CVProcessError(cv_mem, CV_RHSFUNC_FAIL, "Dormand-Price", "CVStepDoPri", MSGCV_RHSFUNC_FAILED, tn);
+    return(CV_RHSFUNC_FAIL);
+  }
+  if (retval > 0) {
+    CVProcessError(cv_mem, CV_FIRST_RHSFUNC_ERR, "Dormand-Price", "CVStepDoPri", MSGCV_RHSFUNC_FIRST);
+    return(CV_FIRST_RHSFUNC_ERR);
+  }
+
+  /* Increment tn => take a step. Increment solver calls as well */
+  tn += h;
+  nst++;
+
+  /* Update the Nordsieck history array */
+  retval = f(tn, zn[0], zn[1], f_data);    /* zn[1] = y'(tn) */
+
+  return(CV_SUCCESS);
+}
+
+/*
  * CVAdjustParams
  *
  * This routine is called when a change in step size was decided upon,
@@ -2847,7 +3007,7 @@ static int CVNlsFunctional(CVodeMem cv_mem)
 
   /* Loop until convergence; accumulate corrections in acor */
 
-  loop {
+  for(;;) {
 
     nni++;
 
@@ -2931,7 +3091,7 @@ static int CVNlsNewton(CVodeMem cv_mem, int nflag)
      Evaluate f at the predicted y, call lsetup if indicated, and
      call CVNewtonIteration for the Newton iteration itself.      */
   
-  loop {
+  for(;;) {
 
     retval = f(tn, zn[0], ftemp, f_data);
     nfe++; 
@@ -2993,7 +3153,7 @@ static int CVNewtonIteration(CVodeMem cv_mem)
   del = delp = ZERO;
 
   /* Looping point for Newton iteration */
-  loop {
+  for(;;) {
 
     /* Evaluate the residual of the nonlinear system*/
     N_VLinearSum(rl1, zn[1], ONE, acor, tempv);
@@ -4150,7 +4310,7 @@ static int CVRootfindStd(CVodeMem cv_mem)
   /* A sign change was found.  Loop to locate nearest root. */
 
   side = 0;  sideprev = -1;
-  loop {                                    /* Looping point */
+  for(;;) {                                    /* Looping point */
 
     /* Set weight alpha.
        On the first two passes, set alpha = 1.  Thereafter, reset alpha
@@ -4662,7 +4822,7 @@ static int CVRootfindExt(CVodeMem cv_mem)
        /* A sign change was found.  Loop to locate nearest root. */
 
        side = 0;  sideprev = -1;
-       loop {                                    /* Looping point */
+       for(;;) {                                    /* Looping point */
 
                /* Set weight alpha.
                On the first two passes, set alpha = 1.  Thereafter, reset alpha