johnpye/dopri5/dopri5.c

/*
    This is a modified form of the DOPRI5 code, originally written in Fortran 77
    by E Hairer and G Wanner, then translated to C by J Colinge.
    Changes have been made to suite ASCEND requirements. Minor only.
    -- John Pye, May 2007.
*//*
    DOPRI5
    Copyright (c) 2004, Ernst Hairer
 
    Redistribution and use in source and binary forms, with or without 
    modification, are permitted provided that the following conditions are 
    met:
 
    - Redistributions of source code must retain the above copyright 
    notice, this list of conditions and the following disclaimer.
 
    - Redistributions in binary form must reproduce the above copyright 
    notice, this list of conditions and the following disclaimer in the 
    documentation and/or other materials provided with the distribution.
 
    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS “AS 
    IS” AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 
    TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 
    PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR 
    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 
    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 
    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 
    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 
    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 
    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include <math.h>
#include <stdio.h>
#include <malloc.h>
#include <limits.h>
#include <memory.h>
#include "dopri5.h"


static long      nfcn, nstep, naccpt, nrejct;
static double    hout, xold, xout;
static unsigned  nrds, *indir;
static double    *yy1, *k1, *k2, *k3, *k4, *k5, *k6, *ysti;
static double    *rcont1, *rcont2, *rcont3, *rcont4, *rcont5;


long nfcnRead (void){
    return nfcn;

} /* nfcnRead */


long nstepRead (void){
    return nstep;

} /* stepRead */


long naccptRead (void){
    return naccpt;

} /* naccptRead */


long nrejctRead (void){
    return nrejct;

} /* nrejct */


double hRead (void){
    return hout;

} /* hRead */


double xRead (void){
    return xout;

} /* xRead */


static double sign (double a, double b){
    return (b > 0.0) ? fabs(a) : -fabs(a);

} /* sign */


static double min_d (double a, double b){
    return (a < b)?a:b;

} /* min_d */


static double max_d (double a, double b){
    return (a > b)?a:b;

} /* max_d */


static double hinit (unsigned n, FcnEqDiff *fcn, double x, double* y,
                     double posneg, double* f0, double* f1, double* yy1, int iord,
                     double hmax, double* atoler, double* rtoler, int itoler
        ,void *user_data
){
    double   dnf, dny, atoli, rtoli, sk, h, h1, der2, der12, sqr;
    unsigned i;

    dnf = 0.0;
    dny = 0.0;
    atoli = atoler[0];
    rtoli = rtoler[0];

    if (!itoler)
        for (i = 0; i < n; i++)
        {
            sk = atoli + rtoli * fabs(y[i]);
            sqr = f0[i] / sk;
            dnf += sqr*sqr;
            sqr = y[i] / sk;
            dny += sqr*sqr;
        }
    else
        for (i = 0; i < n; i++)
        {
            sk = atoler[i] + rtoler[i] * fabs(y[i]);
            sqr = f0[i] / sk;
            dnf += sqr*sqr;
            sqr = y[i] / sk;
            dny += sqr*sqr;
        }

    if ((dnf <= 1.0E-10) || (dny <= 1.0E-10))
        h = 1.0E-6;
    else
        h = sqrt (dny/dnf) * 0.01;

    h = min_d (h, hmax);
    h = sign (h, posneg);

    /* perform an explicit Euler step */
    for (i = 0; i < n; i++)
        yy1[i] = y[i] + h * f0[i];
    fcn (n, x+h, yy1, f1, user_data);

    /* estimate the second derivative of the solution */
    der2 = 0.0;
    if (!itoler)
        for (i = 0; i < n; i++)
        {
            sk = atoli + rtoli * fabs(y[i]);
            sqr = (f1[i] - f0[i]) / sk;
            der2 += sqr*sqr;
        }
    else
        for (i = 0; i < n; i++)
        {
            sk = atoler[i] + rtoler[i] * fabs(y[i]);
            sqr = (f1[i] - f0[i]) / sk;
            der2 += sqr*sqr;
        }
    der2 = sqrt (der2) / h;

    /* step size is computed such that h**iord * max_d(norm(f0),norm(der2)) = 0.01 */
    der12 = max_d (fabs(der2), sqrt(dnf));
    if (der12 <= 1.0E-15)
        h1 = max_d (1.0E-6, fabs(h)*1.0E-3);
    else
        h1 = pow (0.01/der12, 1.0/(double)iord);
    h = min_d (100.0 * h, min_d (h1, hmax));

    return sign (h, posneg);

} /* hinit */


/* core integrator */
static int dopcor (unsigned n, FcnEqDiff *fcn, double x, double* y, double xend,
        double hmax, double h, double* rtoler, double* atoler,
        int itoler, FILE* fileout, SolTrait *solout, int iout,
        long nmax, double uround, int meth, long nstiff, double safe,
        double beta, double fac1, double fac2, unsigned* icont
        , void *user_data   
){
    double   facold, expo1, fac, facc1, facc2, fac11, posneg, xph;
    double   atoli, rtoli, hlamb, err, sk, hnew, yd0, ydiff, bspl;
    double   stnum, stden, sqr;
    int      iasti, iord, irtrn, reject, last, nonsti;
    unsigned i, j;
    double   c2, c3, c4, c5, e1, e3, e4, e5, e6, e7, d1, d3, d4, d5, d6, d7;
    double   a21, a31, a32, a41, a42, a43, a51, a52, a53, a54;
    double   a61, a62, a63, a64, a65, a71, a73, a74, a75, a76;

    /* initialisations */
    switch (meth)
    {
    case 1:

        c2=0.2, c3=0.3, c4=0.8, c5=8.0/9.0;
        a21=0.2, a31=3.0/40.0, a32=9.0/40.0;
        a41=44.0/45.0, a42=-56.0/15.0; a43=32.0/9.0;
        a51=19372.0/6561.0, a52=-25360.0/2187.0;
        a53=64448.0/6561.0, a54=-212.0/729.0;
        a61=9017.0/3168.0, a62=-355.0/33.0, a63=46732.0/5247.0;
        a64=49.0/176.0, a65=-5103.0/18656.0;
        a71=35.0/384.0, a73=500.0/1113.0, a74=125.0/192.0;
        a75=-2187.0/6784.0, a76=11.0/84.0;
        e1=71.0/57600.0, e3=-71.0/16695.0, e4=71.0/1920.0;
        e5=-17253.0/339200.0, e6=22.0/525.0, e7=-1.0/40.0;
        d1=-12715105075.0/11282082432.0, d3=87487479700.0/32700410799.0;
        d4=-10690763975.0/1880347072.0, d5=701980252875.0/199316789632.0;
        d6=-1453857185.0/822651844.0, d7=69997945.0/29380423.0;

        break;
    }

    facold = 1.0E-4;
    expo1 = 0.2 - beta * 0.75;
    facc1 = 1.0 / fac1;
    facc2 = 1.0 / fac2;
    posneg = sign (1.0, xend-x);

    /* initial preparations */
    atoli = atoler[0];
    rtoli = rtoler[0];
    last  = 0;
    hlamb = 0.0;
    iasti = 0;
    fcn(n, x, y, k1, user_data);
    hmax = fabs (hmax);
    iord = 5;
    if (h == 0.0)
        h = hinit (n, fcn, x, y, posneg, k1, k2, k3, iord, hmax, atoler, rtoler, itoler, user_data);
    nfcn += 2;
    reject = 0;
    xold = x;
    if (iout)
    {
        irtrn = 1;
        hout = h;
        xout = x;
        solout (naccpt+1, xold, x, y, n, &irtrn, user_data);
        if (irtrn < 0)
        {
            if (fileout)
                fprintf (fileout, "Exit of dopri5 at x = %.16e\r\n", x);
            return 2;
        }
    }

    /* basic integration step */
    while (1)
    {
        if (nstep > nmax)
        {
            if (fileout)
                fprintf (fileout, "Exit of dopri5 at x = %.16e, more than nmax = %li are needed\r\n", x, nmax);
            xout = x;
            hout = h;
            return -2;
        }

        if (0.1 * fabs(h) <= fabs(x) * uround)
        {
            if (fileout)
                fprintf (fileout, "Exit of dopri5 at x = %.16e, step size too small h = %.16e\r\n", x, h);
            xout = x;
            hout = h;
            return -3;
        }

        if ((x + 1.01*h - xend) * posneg > 0.0)
        {
            h = xend - x;
            last = 1;
        }

        nstep++;

        /* the first 6 stages */
        for (i = 0; i < n; i++)
            yy1[i] = y[i] + h * a21 * k1[i];
        fcn (n, x+c2*h, yy1, k2, user_data);
        for (i = 0; i < n; i++)
            yy1[i] = y[i] + h * (a31*k1[i] + a32*k2[i]);
        fcn (n, x+c3*h, yy1, k3, user_data);
        for (i = 0; i < n; i++)
            yy1[i] = y[i] + h * (a41*k1[i] + a42*k2[i] + a43*k3[i]);
        fcn (n, x+c4*h, yy1, k4, user_data);
        for (i = 0; i <n; i++)
            yy1[i] = y[i] + h * (a51*k1[i] + a52*k2[i] + a53*k3[i] + a54*k4[i]);
        fcn (n, x+c5*h, yy1, k5, user_data);
        for (i = 0; i < n; i++)
            ysti[i] = y[i] + h * (a61*k1[i] + a62*k2[i] + a63*k3[i] + a64*k4[i] + a65*k5[i]);
        xph = x + h;
        fcn (n, xph, ysti, k6, user_data);
        for (i = 0; i < n; i++)
            yy1[i] = y[i] + h * (a71*k1[i] + a73*k3[i] + a74*k4[i] + a75*k5[i] + a76*k6[i]);
        fcn (n, xph, yy1, k2, user_data);
        if (iout == 2){
            if (nrds == n){
                for (i = 0; i < n; i++)
                {
                    rcont5[i] = h * (d1*k1[i] + d3*k3[i] + d4*k4[i] + d5*k5[i] + d6*k6[i] + d7*k2[i]);
                }
            }else{
                for (j = 0; j < nrds; j++)
                {
                    i = icont[j];
                    rcont5[j] = h * (d1*k1[i] + d3*k3[i] + d4*k4[i] + d5*k5[i] + d6*k6[i] + d7*k2[i]);
                }
            }
        }
        for (i = 0; i < n; i++)
            k4[i] = h * (e1*k1[i] + e3*k3[i] + e4*k4[i] + e5*k5[i] + e6*k6[i] + e7*k2[i]);
        nfcn += 6;

        /* error estimation */
        err = 0.0;
        if (!itoler)
            for (i = 0; i < n; i++)
            {
                sk = atoli + rtoli * max_d (fabs(y[i]), fabs(yy1[i]));
                sqr = k4[i] / sk;
                err += sqr*sqr;
            }
        else
            for (i = 0; i < n; i++)
            {
                sk = atoler[i] + rtoler[i] * max_d (fabs(y[i]), fabs(yy1[i]));
                sqr = k4[i] / sk;
                err += sqr*sqr;
            }
        err = sqrt (err / (double)n);

        /* computation of hnew */
        fac11 = pow (err, expo1);
        /* Lund-stabilization */
        fac = fac11 / pow(facold,beta);
        /* we require fac1 <= hnew/h <= fac2 */
        fac = max_d (facc2, min_d (facc1, fac/safe));
        hnew = h / fac;

        if (err <= 1.0)
        {
            /* step accepted */

            facold = max_d (err, 1.0E-4);
            naccpt++;

            /* stiffness detection */
            if (!(naccpt % nstiff) || (iasti > 0))
            {
                stnum = 0.0;
                stden = 0.0;
                for (i = 0; i < n; i++)
                {
                    sqr = k2[i] - k6[i];
                    stnum += sqr*sqr;
                    sqr = yy1[i] - ysti[i];
                    stden += sqr*sqr;
                }
                if (stden > 0.0){
                    hlamb = h * sqrt (stnum / stden);
                }

                if (hlamb > 3.25){
                    nonsti = 0;
                    iasti++;
                    if (iasti == 15){
                        if (fileout){
                            fprintf (fileout, "The problem seems to become stiff at x = %.16e\r\n", x);
                        }else{
                            xout = x;
                            hout = h;
                            return -4;
                        }
                    }
                }
                else
                {
                    nonsti++;
                    if (nonsti == 6)
                        iasti = 0;
                }
            }

            if (iout == 2){
                if (nrds == n){
                    for (i = 0; i < n; i++)
                    {
                        yd0 = y[i];
                        ydiff = yy1[i] - yd0;
                        bspl = h * k1[i] - ydiff;
                        rcont1[i] = y[i];
                        rcont2[i] = ydiff;
                        rcont3[i] = bspl;
                        rcont4[i] = -h * k2[i] + ydiff - bspl;
                    }
                }else{
                    for (j = 0; j < nrds; j++)
                    {
                        i = icont[j];
                        yd0 = y[i];
                        ydiff = yy1[i] - yd0;
                        bspl = h * k1[i] - ydiff;
                        rcont1[j] = y[i];
                        rcont2[j] = ydiff;
                        rcont3[j] = bspl;
                        rcont4[j] = -h * k2[i] + ydiff - bspl;
                    }
                }
            }
            memcpy (k1, k2, n * sizeof(double));
            memcpy (y, yy1, n * sizeof(double));
            xold = x;
            x = xph;

            if (iout)
            {
                hout = h;
                xout = x;
                solout (naccpt+1, xold, x, y, n, &irtrn, user_data);
                if (irtrn < 0)
                {
                    if (fileout)
                        fprintf (fileout, "Exit of dopri5 at x = %.16e\r\n", x);
                    return 2;
                }
            }

            /* normal exit */
            if (last)
            {
                hout=hnew;
                xout = x;
                return 1;
            }

            if (fabs(hnew) > hmax)
                hnew = posneg * hmax;
            if (reject)
                hnew = posneg * min_d (fabs(hnew), fabs(h));

            reject = 0;
        }
        else
        {
            /* step rejected */
            hnew = h / min_d (facc1, fac11/safe);
            reject = 1;
            if (naccpt >= 1)
                nrejct=nrejct + 1;
            last = 0;
        }

        h = hnew;
    }

} /* dopcor */


/* front-end */
int dopri5(
    unsigned n, FcnEqDiff *fcn, double x, double* y, double xend, double* rtoler,
    double* atoler, int itoler, SolTrait *solout, int iout, FILE* fileout, double uround,
    double safe, double fac1, double fac2, double beta, double hmax, double h,
    long nmax, int meth, long nstiff, unsigned nrdens, unsigned* icont, unsigned licont
    , void *user_data
){
    int       arret, idid;
    unsigned  i;

    /* initialisations */
    nfcn = nstep = naccpt = nrejct = arret = 0;
    rcont1 = rcont2 = rcont3 = rcont4 = rcont5 = NULL;
    indir = NULL;

    /* n, the dimension of the system */
    if (n == UINT_MAX)
    {
        if (fileout)
            fprintf (fileout, "System too big, max. n = %u\r\n", UINT_MAX-1);
        arret = 1;
    }

    /* nmax, the maximal number of steps */
    if (!nmax)
        nmax = 100000;
    else if (nmax <= 0)
    {
        if (fileout)
            fprintf (fileout, "Wrong input, nmax = %li\r\n", nmax);
        arret = 1;
    }

    /* meth, coefficients of the method */
    if (!meth)
        meth = 1;
    else if ((meth <= 0) || (meth >= 2))
    {
        if (fileout)
            fprintf (fileout, "Curious input, meth = %i\r\n", meth);
        arret = 1;
    }

    /* nstiff, parameter for stiffness detection */
    if (!nstiff)
        nstiff = 1000;
    else if (nstiff < 0)
        nstiff = nmax + 10;

    /* iout, switch for calling solout */
    if ((iout < 0) || (iout > 2))
    {
        if (fileout)
            fprintf (fileout, "Wrong input, iout = %i\r\n", iout);
        arret = 1;
    }

    /* nrdens, number of dense output components */
    if (nrdens > n)
    {
        if (fileout)
            fprintf (fileout, "Curious input, nrdens = %u\r\n", nrdens);
        arret = 1;
    }
    else if (nrdens)
    {
        /* is there enough memory to allocate rcont12345&indir ? */
        rcont1 = (double*) malloc (nrdens*sizeof(double));
        rcont2 = (double*) malloc (nrdens*sizeof(double));
        rcont3 = (double*) malloc (nrdens*sizeof(double));
        rcont4 = (double*) malloc (nrdens*sizeof(double));
        rcont5 = (double*) malloc (nrdens*sizeof(double));
        if (nrdens < n)
            indir = (unsigned*) malloc (n*sizeof(unsigned));

        if (!rcont1 || !rcont2 || !rcont3 || !rcont4 || !rcont5 || (!indir && (nrdens < n)))
        {
            if (fileout)
                fprintf (fileout, "Not enough free memory for rcont12345&indir\r\n");
            arret = 1;
        }

        /* control of length of icont */
        if (nrdens == n)
        {
            if (icont && fileout)
                fprintf (fileout, "Warning : when nrdens = n there is no need allocating memory for icont\r\n");
            nrds = n;
        }
        else if (licont < nrdens)
        {
            if (fileout)
                fprintf (fileout, "Insufficient storage for icont, min. licont = %u\r\n", nrdens);
            arret = 1;
        }
        else
        {
            if ((iout < 2) && fileout)
                fprintf (fileout, "Warning : put iout = 2 for dense output\r\n");
            nrds = nrdens;
            for (i = 0; i < n; i++)
                indir[i] = UINT_MAX;
            for (i = 0; i < nrdens; i++)
                indir[icont[i]] = i;
        }
    }

    /* uround, smallest number satisfying 1.0+uround > 1.0 */
    if (uround == 0.0)
        uround = 2.3E-16;
    else if ((uround <= 1.0E-35) || (uround >= 1.0))
    {
        if (fileout)
            fprintf (fileout, "Which machine do you have ? Your uround was : %.16e\r\n", uround);
        arret = 1;
    }

    /* safety factor */
    if (safe == 0.0)
        safe = 0.9;
    else if ((safe >= 1.0) || (safe <= 1.0E-4))
    {
        if (fileout)
            fprintf (fileout, "Curious input for safety factor, safe = %.16e\r\n", safe);
        arret = 1;
    }

    /* fac1, fac2, parameters for step size selection */
    if (fac1 == 0.0)
        fac1 = 0.2;
    if (fac2 == 0.0)
        fac2 = 10.0;

    /* beta for step control stabilization */
    if (beta == 0.0)
        beta = 0.04;
    else if (beta < 0.0)
        beta = 0.0;
    else if (beta > 0.2)
    {
        if (fileout)
            fprintf (fileout, "Curious input for beta : beta = %.16e\r\n", beta);
        arret = 1;
    }

    /* maximal step size */
    if (hmax == 0.0)
        hmax = xend - x;

    /* is there enough free memory for the method ? */
    yy1 = (double*) malloc (n*sizeof(double));
    k1 = (double*) malloc (n*sizeof(double));
    k2 = (double*) malloc (n*sizeof(double));
    k3 = (double*) malloc (n*sizeof(double));
    k4 = (double*) malloc (n*sizeof(double));
    k5 = (double*) malloc (n*sizeof(double));
    k6 = (double*) malloc (n*sizeof(double));
    ysti = (double*) malloc (n*sizeof(double));

    if (!yy1 || !k1 || !k2 || !k3 || !k4 || !k5 || !k6 || !ysti)
    {
        if (fileout)
            fprintf (fileout, "Not enough free memory for the method\r\n");
        arret = 1;
    }

    /* when a failure has occured, we return -1 */
    if (arret)
    {
        if (ysti)
            free (ysti);
        if (k6)
            free (k6);
        if (k5)
            free (k5);
        if (k4)
            free (k4);
        if (k3)
            free (k3);
        if (k2)
            free (k2);
        if (k1)
            free (k1);
        if (yy1)
            free (yy1);
        if (indir)
            free (indir);
        if (rcont5)
            free (rcont5);
        if (rcont4)
            free (rcont4);
        if (rcont3)
            free (rcont3);
        if (rcont2)
            free (rcont2);
        if (rcont1)
            free (rcont1);

        return -1;
    }
    else
    {
        idid = dopcor (n, fcn, x, y, xend, hmax, h, rtoler, atoler, itoler, fileout
            , solout, iout, nmax, uround, meth, nstiff, safe, beta, fac1, fac2, icont
            , user_data
        );
        free (ysti);
        free (k6);
        free (k5);    /* reverse order freeing too increase chances */
        free (k4);    /* of efficient dynamic memory managing       */
        free (k3);
        free (k2);
        free (k1);
        free (yy1);
        if (indir)
            free (indir);
        if (rcont5)
        {
            free (rcont5);
            free (rcont4);
            free (rcont3);
            free (rcont2);
            free (rcont1);
        }

        return idid;
    }

} /* dopri5 */


/* dense output function */
double contd5 (unsigned ii, double x){
    unsigned i;
    double   theta, theta1;

    i = UINT_MAX;

    if (!indir)
        i = ii;
    else
        i = indir[ii];

    if (i == UINT_MAX)
    {
        printf ("No dense output available for %uth component", ii);
        return 0.0;
    }

    theta = (x - xold) / hout;
    theta1 = 1.0 - theta;

    return rcont1[i] + theta*(rcont2[i] + theta1*(rcont3[i] + theta*(rcont4[i] + theta1*rcont5[i])));

} /* contd5 */

1	jpye	1445	/*
2			This is a modified form of the DOPRI5 code, originally written in Fortran 77
3			by E Hairer and G Wanner, then translated to C by J Colinge.
4			Changes have been made to suite ASCEND requirements. Minor only.
5			-- John Pye, May 2007.
6			//
7			DOPRI5
8			Copyright (c) 2004, Ernst Hairer
9
10			Redistribution and use in source and binary forms, with or without
11			modification, are permitted provided that the following conditions are
12			met:
13
14			- Redistributions of source code must retain the above copyright
15			notice, this list of conditions and the following disclaimer.
16
17			- Redistributions in binary form must reproduce the above copyright
18			notice, this list of conditions and the following disclaimer in the
19			documentation and/or other materials provided with the distribution.
20
21			THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS “AS
22			IS” AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
23			TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
24			PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR
25			CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
26			EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
27			PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
28			PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
29			LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
30			NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
31			SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
32			*/
33
34			#include <math.h>
35			#include <stdio.h>
36			#include <malloc.h>
37			#include <limits.h>
38			#include <memory.h>
39			#include "dopri5.h"
40
41
42			static long nfcn, nstep, naccpt, nrejct;
43			static double hout, xold, xout;
44			static unsigned nrds, *indir;
45			static double yy1, k1, k2, k3, k4, k5, k6, ysti;
46			static double rcont1, rcont2, rcont3, rcont4, *rcont5;
47
48
49			long nfcnRead (void){
50			return nfcn;
51
52			} /* nfcnRead */
53
54
55			long nstepRead (void){
56			return nstep;
57
58			} /* stepRead */
59
60
61			long naccptRead (void){
62			return naccpt;
63
64			} /* naccptRead */
65
66
67			long nrejctRead (void){
68			return nrejct;
69
70			} /* nrejct */
71
72
73			double hRead (void){
74			return hout;
75
76			} /* hRead */
77
78
79			double xRead (void){
80			return xout;
81
82			} /* xRead */
83
84
85			static double sign (double a, double b){
86			return (b > 0.0) ? fabs(a) : -fabs(a);
87
88			} /* sign */
89
90
91			static double min_d (double a, double b){
92			return (a < b)?a:b;
93
94			} /* min_d */
95
96
97			static double max_d (double a, double b){
98			return (a > b)?a:b;
99
100			} /* max_d */
101
102
103			static double hinit (unsigned n, FcnEqDiff fcn, double x, double y,
104			double posneg, double* f0, double* f1, double* yy1, int iord,
105	jpye	1451	double hmax, double* atoler, double* rtoler, int itoler
106			,void *user_data
107			){
108	jpye	1445	double dnf, dny, atoli, rtoli, sk, h, h1, der2, der12, sqr;
109			unsigned i;
110
111			dnf = 0.0;
112			dny = 0.0;
113			atoli = atoler[0];
114			rtoli = rtoler[0];
115
116			if (!itoler)
117			for (i = 0; i < n; i++)
118			{
119			sk = atoli + rtoli * fabs(y[i]);
120			sqr = f0[i] / sk;
121			dnf += sqr*sqr;
122			sqr = y[i] / sk;
123			dny += sqr*sqr;
124			}
125			else
126			for (i = 0; i < n; i++)
127			{
128			sk = atoler[i] + rtoler[i] * fabs(y[i]);
129			sqr = f0[i] / sk;
130			dnf += sqr*sqr;
131			sqr = y[i] / sk;
132			dny += sqr*sqr;
133			}
134
135			if ((dnf <= 1.0E-10) \|\| (dny <= 1.0E-10))
136			h = 1.0E-6;
137			else
138			h = sqrt (dny/dnf) * 0.01;
139
140			h = min_d (h, hmax);
141			h = sign (h, posneg);
142
143			/* perform an explicit Euler step */
144			for (i = 0; i < n; i++)
145			yy1[i] = y[i] + h * f0[i];
146	jpye	1451	fcn (n, x+h, yy1, f1, user_data);
147	jpye	1445
148			/* estimate the second derivative of the solution */
149			der2 = 0.0;
150			if (!itoler)
151			for (i = 0; i < n; i++)
152			{
153			sk = atoli + rtoli * fabs(y[i]);
154			sqr = (f1[i] - f0[i]) / sk;
155			der2 += sqr*sqr;
156			}
157			else
158			for (i = 0; i < n; i++)
159			{
160			sk = atoler[i] + rtoler[i] * fabs(y[i]);
161			sqr = (f1[i] - f0[i]) / sk;
162			der2 += sqr*sqr;
163			}
164			der2 = sqrt (der2) / h;
165
166			/* step size is computed such that h*iord max_d(norm(f0),norm(der2)) = 0.01 */
167			der12 = max_d (fabs(der2), sqrt(dnf));
168			if (der12 <= 1.0E-15)
169			h1 = max_d (1.0E-6, fabs(h)*1.0E-3);
170			else
171			h1 = pow (0.01/der12, 1.0/(double)iord);
172			h = min_d (100.0 * h, min_d (h1, hmax));
173
174			return sign (h, posneg);
175
176			} /* hinit */
177
178
179			/* core integrator */
180			static int dopcor (unsigned n, FcnEqDiff fcn, double x, double y, double xend,
181	jpye	1451	double hmax, double h, double* rtoler, double* atoler,
182			int itoler, FILE* fileout, SolTrait *solout, int iout,
183			long nmax, double uround, int meth, long nstiff, double safe,
184			double beta, double fac1, double fac2, unsigned* icont
185			, void *user_data
186			){
187	jpye	1445	double facold, expo1, fac, facc1, facc2, fac11, posneg, xph;
188			double atoli, rtoli, hlamb, err, sk, hnew, yd0, ydiff, bspl;
189			double stnum, stden, sqr;
190			int iasti, iord, irtrn, reject, last, nonsti;
191			unsigned i, j;
192			double c2, c3, c4, c5, e1, e3, e4, e5, e6, e7, d1, d3, d4, d5, d6, d7;
193			double a21, a31, a32, a41, a42, a43, a51, a52, a53, a54;
194			double a61, a62, a63, a64, a65, a71, a73, a74, a75, a76;
195
196			/* initialisations */
197			switch (meth)
198			{
199			case 1:
200
201			c2=0.2, c3=0.3, c4=0.8, c5=8.0/9.0;
202			a21=0.2, a31=3.0/40.0, a32=9.0/40.0;
203			a41=44.0/45.0, a42=-56.0/15.0; a43=32.0/9.0;
204			a51=19372.0/6561.0, a52=-25360.0/2187.0;
205			a53=64448.0/6561.0, a54=-212.0/729.0;
206			a61=9017.0/3168.0, a62=-355.0/33.0, a63=46732.0/5247.0;
207			a64=49.0/176.0, a65=-5103.0/18656.0;
208			a71=35.0/384.0, a73=500.0/1113.0, a74=125.0/192.0;
209			a75=-2187.0/6784.0, a76=11.0/84.0;
210			e1=71.0/57600.0, e3=-71.0/16695.0, e4=71.0/1920.0;
211			e5=-17253.0/339200.0, e6=22.0/525.0, e7=-1.0/40.0;
212			d1=-12715105075.0/11282082432.0, d3=87487479700.0/32700410799.0;
213			d4=-10690763975.0/1880347072.0, d5=701980252875.0/199316789632.0;
214			d6=-1453857185.0/822651844.0, d7=69997945.0/29380423.0;
215
216			break;
217			}
218
219			facold = 1.0E-4;
220			expo1 = 0.2 - beta * 0.75;
221			facc1 = 1.0 / fac1;
222			facc2 = 1.0 / fac2;
223			posneg = sign (1.0, xend-x);
224
225			/* initial preparations */
226			atoli = atoler[0];
227			rtoli = rtoler[0];
228			last = 0;
229			hlamb = 0.0;
230			iasti = 0;
231	jpye	1451	fcn(n, x, y, k1, user_data);
232	jpye	1445	hmax = fabs (hmax);
233			iord = 5;
234			if (h == 0.0)
235	jpye	1451	h = hinit (n, fcn, x, y, posneg, k1, k2, k3, iord, hmax, atoler, rtoler, itoler, user_data);
236	jpye	1445	nfcn += 2;
237			reject = 0;
238			xold = x;
239			if (iout)
240			{
241			irtrn = 1;
242			hout = h;
243			xout = x;
244	jpye	1451	solout (naccpt+1, xold, x, y, n, &irtrn, user_data);
245	jpye	1445	if (irtrn < 0)
246			{
247			if (fileout)
248			fprintf (fileout, "Exit of dopri5 at x = %.16e\r\n", x);
249			return 2;
250			}
251			}
252
253			/* basic integration step */
254			while (1)
255			{
256			if (nstep > nmax)
257			{
258			if (fileout)
259			fprintf (fileout, "Exit of dopri5 at x = %.16e, more than nmax = %li are needed\r\n", x, nmax);
260			xout = x;
261			hout = h;
262			return -2;
263			}
264
265			if (0.1 * fabs(h) <= fabs(x) * uround)
266			{
267			if (fileout)
268			fprintf (fileout, "Exit of dopri5 at x = %.16e, step size too small h = %.16e\r\n", x, h);
269			xout = x;
270			hout = h;
271			return -3;
272			}
273
274			if ((x + 1.01h - xend) posneg > 0.0)
275			{
276			h = xend - x;
277			last = 1;
278			}
279
280			nstep++;
281
282			/* the first 6 stages */
283			for (i = 0; i < n; i++)
284			yy1[i] = y[i] + h * a21 * k1[i];
285	jpye	1451	fcn (n, x+c2*h, yy1, k2, user_data);
286	jpye	1445	for (i = 0; i < n; i++)
287			yy1[i] = y[i] + h * (a31k1[i] + a32k2[i]);
288	jpye	1451	fcn (n, x+c3*h, yy1, k3, user_data);
289	jpye	1445	for (i = 0; i < n; i++)
290			yy1[i] = y[i] + h * (a41k1[i] + a42k2[i] + a43*k3[i]);
291	jpye	1451	fcn (n, x+c4*h, yy1, k4, user_data);
292	jpye	1445	for (i = 0; i <n; i++)
293			yy1[i] = y[i] + h * (a51k1[i] + a52k2[i] + a53k3[i] + a54k4[i]);
294	jpye	1451	fcn (n, x+c5*h, yy1, k5, user_data);
295	jpye	1445	for (i = 0; i < n; i++)
296			ysti[i] = y[i] + h * (a61k1[i] + a62k2[i] + a63k3[i] + a64k4[i] + a65*k5[i]);
297			xph = x + h;
298	jpye	1451	fcn (n, xph, ysti, k6, user_data);
299	jpye	1445	for (i = 0; i < n; i++)
300			yy1[i] = y[i] + h * (a71k1[i] + a73k3[i] + a74k4[i] + a75k5[i] + a76*k6[i]);
301	jpye	1451	fcn (n, xph, yy1, k2, user_data);
302	jpye	1445	if (iout == 2){
303			if (nrds == n){
304			for (i = 0; i < n; i++)
305			{
306			rcont5[i] = h * (d1k1[i] + d3k3[i] + d4k4[i] + d5k5[i] + d6k6[i] + d7k2[i]);
307			}
308			}else{
309			for (j = 0; j < nrds; j++)
310			{
311			i = icont[j];
312			rcont5[j] = h * (d1k1[i] + d3k3[i] + d4k4[i] + d5k5[i] + d6k6[i] + d7k2[i]);
313			}
314			}
315			}
316			for (i = 0; i < n; i++)
317			k4[i] = h * (e1k1[i] + e3k3[i] + e4k4[i] + e5k5[i] + e6k6[i] + e7k2[i]);
318			nfcn += 6;
319
320			/* error estimation */
321			err = 0.0;
322			if (!itoler)
323			for (i = 0; i < n; i++)
324			{
325			sk = atoli + rtoli * max_d (fabs(y[i]), fabs(yy1[i]));
326			sqr = k4[i] / sk;
327			err += sqr*sqr;
328			}
329			else
330			for (i = 0; i < n; i++)
331			{
332			sk = atoler[i] + rtoler[i] * max_d (fabs(y[i]), fabs(yy1[i]));
333			sqr = k4[i] / sk;
334			err += sqr*sqr;
335			}
336			err = sqrt (err / (double)n);
337
338			/* computation of hnew */
339			fac11 = pow (err, expo1);
340			/* Lund-stabilization */
341			fac = fac11 / pow(facold,beta);
342			/* we require fac1 <= hnew/h <= fac2 */
343			fac = max_d (facc2, min_d (facc1, fac/safe));
344			hnew = h / fac;
345
346			if (err <= 1.0)
347			{
348			/* step accepted */
349
350			facold = max_d (err, 1.0E-4);
351			naccpt++;
352
353			/* stiffness detection */
354			if (!(naccpt % nstiff) \|\| (iasti > 0))
355			{
356			stnum = 0.0;
357			stden = 0.0;
358			for (i = 0; i < n; i++)
359			{
360			sqr = k2[i] - k6[i];
361			stnum += sqr*sqr;
362			sqr = yy1[i] - ysti[i];
363			stden += sqr*sqr;
364			}
365			if (stden > 0.0){
366			hlamb = h * sqrt (stnum / stden);
367			}
368
369			if (hlamb > 3.25){
370			nonsti = 0;
371			iasti++;
372			if (iasti == 15){
373			if (fileout){
374			fprintf (fileout, "The problem seems to become stiff at x = %.16e\r\n", x);
375			}else{
376			xout = x;
377			hout = h;
378			return -4;
379			}
380			}
381			}
382			else
383			{
384			nonsti++;
385			if (nonsti == 6)
386			iasti = 0;
387			}
388			}
389
390			if (iout == 2){
391			if (nrds == n){
392			for (i = 0; i < n; i++)
393			{
394			yd0 = y[i];
395			ydiff = yy1[i] - yd0;
396			bspl = h * k1[i] - ydiff;
397			rcont1[i] = y[i];
398			rcont2[i] = ydiff;
399			rcont3[i] = bspl;
400			rcont4[i] = -h * k2[i] + ydiff - bspl;
401			}
402			}else{
403			for (j = 0; j < nrds; j++)
404			{
405			i = icont[j];
406			yd0 = y[i];
407			ydiff = yy1[i] - yd0;
408			bspl = h * k1[i] - ydiff;
409			rcont1[j] = y[i];
410			rcont2[j] = ydiff;
411			rcont3[j] = bspl;
412			rcont4[j] = -h * k2[i] + ydiff - bspl;
413			}
414			}
415			}
416			memcpy (k1, k2, n * sizeof(double));
417			memcpy (y, yy1, n * sizeof(double));
418			xold = x;
419			x = xph;
420
421			if (iout)
422			{
423			hout = h;
424			xout = x;
425	jpye	1451	solout (naccpt+1, xold, x, y, n, &irtrn, user_data);
426	jpye	1445	if (irtrn < 0)
427			{
428			if (fileout)
429			fprintf (fileout, "Exit of dopri5 at x = %.16e\r\n", x);
430			return 2;
431			}
432			}
433
434			/* normal exit */
435			if (last)
436			{
437			hout=hnew;
438			xout = x;
439			return 1;
440			}
441
442			if (fabs(hnew) > hmax)
443			hnew = posneg * hmax;
444			if (reject)
445			hnew = posneg * min_d (fabs(hnew), fabs(h));
446
447			reject = 0;
448			}
449			else
450			{
451			/* step rejected */
452			hnew = h / min_d (facc1, fac11/safe);
453			reject = 1;
454			if (naccpt >= 1)
455			nrejct=nrejct + 1;
456			last = 0;
457			}
458
459			h = hnew;
460			}
461
462			} /* dopcor */
463
464
465			/* front-end */
466			int dopri5(
467			unsigned n, FcnEqDiff fcn, double x, double y, double xend, double* rtoler,
468			double* atoler, int itoler, SolTrait solout, int iout, FILE fileout, double uround,
469			double safe, double fac1, double fac2, double beta, double hmax, double h,
470			long nmax, int meth, long nstiff, unsigned nrdens, unsigned* icont, unsigned licont
471	jpye	1451	, void *user_data
472	jpye	1445	){
473			int arret, idid;
474			unsigned i;
475
476			/* initialisations */
477			nfcn = nstep = naccpt = nrejct = arret = 0;
478			rcont1 = rcont2 = rcont3 = rcont4 = rcont5 = NULL;
479			indir = NULL;
480
481			/* n, the dimension of the system */
482			if (n == UINT_MAX)
483			{
484			if (fileout)
485			fprintf (fileout, "System too big, max. n = %u\r\n", UINT_MAX-1);
486			arret = 1;
487			}
488
489			/* nmax, the maximal number of steps */
490			if (!nmax)
491			nmax = 100000;
492			else if (nmax <= 0)
493			{
494			if (fileout)
495			fprintf (fileout, "Wrong input, nmax = %li\r\n", nmax);
496			arret = 1;
497			}
498
499			/* meth, coefficients of the method */
500			if (!meth)
501			meth = 1;
502			else if ((meth <= 0) \|\| (meth >= 2))
503			{
504			if (fileout)
505			fprintf (fileout, "Curious input, meth = %i\r\n", meth);
506			arret = 1;
507			}
508
509			/* nstiff, parameter for stiffness detection */
510			if (!nstiff)
511			nstiff = 1000;
512			else if (nstiff < 0)
513			nstiff = nmax + 10;
514
515			/* iout, switch for calling solout */
516			if ((iout < 0) \|\| (iout > 2))
517			{
518			if (fileout)
519			fprintf (fileout, "Wrong input, iout = %i\r\n", iout);
520			arret = 1;
521			}
522
523			/* nrdens, number of dense output components */
524			if (nrdens > n)
525			{
526			if (fileout)
527			fprintf (fileout, "Curious input, nrdens = %u\r\n", nrdens);
528			arret = 1;
529			}
530			else if (nrdens)
531			{
532			/* is there enough memory to allocate rcont12345&indir ? */
533			rcont1 = (double) malloc (nrdenssizeof(double));
534			rcont2 = (double) malloc (nrdenssizeof(double));
535			rcont3 = (double) malloc (nrdenssizeof(double));
536			rcont4 = (double) malloc (nrdenssizeof(double));
537			rcont5 = (double) malloc (nrdenssizeof(double));
538			if (nrdens < n)
539			indir = (unsigned) malloc (nsizeof(unsigned));
540
541			if (!rcont1 \|\| !rcont2 \|\| !rcont3 \|\| !rcont4 \|\| !rcont5 \|\| (!indir && (nrdens < n)))
542			{
543			if (fileout)
544			fprintf (fileout, "Not enough free memory for rcont12345&indir\r\n");
545			arret = 1;
546			}
547
548			/* control of length of icont */
549			if (nrdens == n)
550			{
551			if (icont && fileout)
552			fprintf (fileout, "Warning : when nrdens = n there is no need allocating memory for icont\r\n");
553			nrds = n;
554			}
555			else if (licont < nrdens)
556			{
557			if (fileout)
558			fprintf (fileout, "Insufficient storage for icont, min. licont = %u\r\n", nrdens);
559			arret = 1;
560			}
561			else
562			{
563			if ((iout < 2) && fileout)
564			fprintf (fileout, "Warning : put iout = 2 for dense output\r\n");
565			nrds = nrdens;
566			for (i = 0; i < n; i++)
567			indir[i] = UINT_MAX;
568			for (i = 0; i < nrdens; i++)
569			indir[icont[i]] = i;
570			}
571			}
572
573			/* uround, smallest number satisfying 1.0+uround > 1.0 */
574			if (uround == 0.0)
575			uround = 2.3E-16;
576			else if ((uround <= 1.0E-35) \|\| (uround >= 1.0))
577			{
578			if (fileout)
579			fprintf (fileout, "Which machine do you have ? Your uround was : %.16e\r\n", uround);
580			arret = 1;
581			}
582
583			/* safety factor */
584			if (safe == 0.0)
585			safe = 0.9;
586			else if ((safe >= 1.0) \|\| (safe <= 1.0E-4))
587			{
588			if (fileout)
589			fprintf (fileout, "Curious input for safety factor, safe = %.16e\r\n", safe);
590			arret = 1;
591			}
592
593			/* fac1, fac2, parameters for step size selection */
594			if (fac1 == 0.0)
595			fac1 = 0.2;
596			if (fac2 == 0.0)
597			fac2 = 10.0;
598
599			/* beta for step control stabilization */
600			if (beta == 0.0)
601			beta = 0.04;
602			else if (beta < 0.0)
603			beta = 0.0;
604			else if (beta > 0.2)
605			{
606			if (fileout)
607			fprintf (fileout, "Curious input for beta : beta = %.16e\r\n", beta);
608			arret = 1;
609			}
610
611			/* maximal step size */
612			if (hmax == 0.0)
613			hmax = xend - x;
614
615			/* is there enough free memory for the method ? */
616			yy1 = (double) malloc (nsizeof(double));
617			k1 = (double) malloc (nsizeof(double));
618			k2 = (double) malloc (nsizeof(double));
619			k3 = (double) malloc (nsizeof(double));
620			k4 = (double) malloc (nsizeof(double));
621			k5 = (double) malloc (nsizeof(double));
622			k6 = (double) malloc (nsizeof(double));
623			ysti = (double) malloc (nsizeof(double));
624
625			if (!yy1 \|\| !k1 \|\| !k2 \|\| !k3 \|\| !k4 \|\| !k5 \|\| !k6 \|\| !ysti)
626			{
627			if (fileout)
628			fprintf (fileout, "Not enough free memory for the method\r\n");
629			arret = 1;
630			}
631
632			/* when a failure has occured, we return -1 */
633			if (arret)
634			{
635			if (ysti)
636			free (ysti);
637			if (k6)
638			free (k6);
639			if (k5)
640			free (k5);
641			if (k4)
642			free (k4);
643			if (k3)
644			free (k3);
645			if (k2)
646			free (k2);
647			if (k1)
648			free (k1);
649			if (yy1)
650			free (yy1);
651			if (indir)
652			free (indir);
653			if (rcont5)
654			free (rcont5);
655			if (rcont4)
656			free (rcont4);
657			if (rcont3)
658			free (rcont3);
659			if (rcont2)
660			free (rcont2);
661			if (rcont1)
662			free (rcont1);
663
664			return -1;
665			}
666			else
667			{
668	jpye	1451	idid = dopcor (n, fcn, x, y, xend, hmax, h, rtoler, atoler, itoler, fileout
669			, solout, iout, nmax, uround, meth, nstiff, safe, beta, fac1, fac2, icont
670			, user_data
671			);
672	jpye	1445	free (ysti);
673			free (k6);
674			free (k5); /* reverse order freeing too increase chances */
675			free (k4); /* of efficient dynamic memory managing */
676			free (k3);
677			free (k2);
678			free (k1);
679			free (yy1);
680			if (indir)
681			free (indir);
682			if (rcont5)
683			{
684			free (rcont5);
685			free (rcont4);
686			free (rcont3);
687			free (rcont2);
688			free (rcont1);
689			}
690
691			return idid;
692			}
693
694			} /* dopri5 */
695
696
697			/* dense output function */
698			double contd5 (unsigned ii, double x){
699			unsigned i;
700			double theta, theta1;
701
702			i = UINT_MAX;
703
704			if (!indir)
705			i = ii;
706			else
707			i = indir[ii];
708
709			if (i == UINT_MAX)
710			{
711			printf ("No dense output available for %uth component", ii);
712			return 0.0;
713			}
714
715			theta = (x - xold) / hout;
716			theta1 = 1.0 - theta;
717
718			return rcont1[i] + theta(rcont2[i] + theta1(rcont3[i] + theta(rcont4[i] + theta1rcont5[i])));
719
720			} /* contd5 */
721