home *** CD-ROM | disk | FTP | other *** search
/ Liren Large Software Subsidy 10 / 10.iso / l / l460 / 2.ddi / FUNFUN.DI$ / ODE23.C < prev    next >
Encoding:
C/C++ Source or Header  |  1993-02-01  |  7.2 KB  |  382 lines
  1. /*
  2.  
  3.    ODE23.C    MEX file implementation of:
  4.  
  5.     [tout, yout] = ode23(F, t0, tfinal, y0, tol)
  6.  
  7.    Integrate a system of ordinary differential equations.
  8.  
  9.    INPUT:
  10.    F     - String containing name of user-supplied problem description.
  11.          Call: yprime = fun(t,y) where F = 'fun'.
  12.          t      - Time (scalar).
  13.          y      - Solution column-vector.
  14.          yprime - Returned derivative column-vector; yprime(i) = dy(i)/dt.
  15.    t0    - Initial value of t.
  16.    tfinal- Final value of t.
  17.    y0    - Initial value column-vector.
  18.    tol   - The desired accuracy. (Default: tol = 1.e-3).
  19.  
  20.    OUTPUT:
  21.    tout  - Returned integration time points (row-vector).
  22.    yout  - Returned solution, one solution column-vector per tout-value.
  23.  
  24.    C.B. Moler, 3-25-87.
  25.    Marc Ullman  June 9, 1987
  26.    Copyright (C) 1987  The Mathworks Inc.
  27.    All Rights Reserved
  28. */
  29.  
  30. #include <math.h>
  31. #include "mex.h"
  32.  
  33. /* Input Arguments */
  34.  
  35. #define    F_IN    prhs[0]
  36. #define    T0_IN    prhs[1]
  37. #define    TF_IN    prhs[2]
  38. #define    Y0_IN    prhs[3]
  39. #define    TOL_IN    prhs[4]
  40.  
  41.  
  42. /* Output Arguments */
  43.  
  44. #define    T_OUT    plhs[0]
  45. #define    Y_OUT    plhs[1]
  46.  
  47.  
  48. /* Temporary Variables */
  49.  
  50. #define    OLD_T_OUT    old_plhs[0]
  51. #define    OLD_Y_OUT    old_plhs[1]
  52.  
  53. #define    T_TEMP        tmp_prhs[0]
  54. #define    Y_TEMP        tmp_prhs[1]
  55. #define    Y_OUT_TEMP    tmp_plhs[0]
  56.  
  57.  
  58. #define    MAX(A, B)    ((A) > (B) ? (A) : (B))
  59. #define    MIN(A, B)    ((A) < (B) ? (A) : (B))
  60.  
  61. #ifdef __STDC__
  62. static double inf_norm(
  63.     double    y[],
  64.     unsigned int M
  65.     )
  66. #else
  67. static double inf_norm(y,M)
  68. double    y[];
  69. unsigned int M;
  70. #endif
  71. {
  72.     register unsigned int m;
  73.     double    temp_max,yabs;
  74.  
  75.     temp_max = fabs(y[0]);
  76.  
  77.     for (m = 1; m < M; m++) {
  78.         yabs = fabs(y[m]);
  79.         temp_max = MAX(temp_max,yabs);
  80.     }
  81.  
  82.     return(temp_max);
  83. }
  84. #ifdef __STDC__
  85. void mexFunction(
  86.     int        nlhs,
  87.     Matrix    *plhs[],
  88.     int        nrhs,
  89.     Matrix    *prhs[]
  90.     )
  91. #else
  92. mexFunction(nlhs, plhs, nrhs, prhs)
  93. int nlhs, nrhs;
  94. Matrix *plhs[], *prhs[];
  95. #endif
  96. {
  97.     double    *tout,*yout; 
  98.     double    *toutp,*youtp;
  99.     double    *t,*y;
  100.     double    *y0;
  101.     char    fcn_name[20];
  102.  
  103.     register unsigned int    m,k,kk;
  104.     unsigned int    M,K;
  105.     int        nr, nc;
  106.  
  107.     double    ts;
  108.     double    *s0,*s1,*s2,*s3;
  109.     double    t0,tfinal;
  110.     double    tol,tau,delta,h,hmax;
  111.     double    ymax;
  112.     double    *ydel;
  113.  
  114.     static double powr = 1.0/3.0;
  115.  
  116.     Matrix    *tmp_plhs[1],*tmp_prhs[2];
  117.     Matrix    *s_matptr[3];
  118.     Matrix    *old_plhs[2];
  119.  
  120.  
  121.     /*
  122.      * Check out the arguments
  123.      */
  124.  
  125.     if ((nrhs < 4) || (nrhs > 6)) {
  126.         mexErrMsgTxt("Wrong number of input arguments for ODE23");
  127.     } else if (nlhs != 2) {
  128.         mexErrMsgTxt("Wrong number of output arguments for ODE23");
  129.     }
  130.  
  131.  
  132.     /*
  133.      * Get user function name
  134.      */
  135.  
  136.     if (!mxIsString(F_IN))
  137.         mexErrMsgTxt("String argument expected for function name in ODE23.");
  138.     mxGetString(F_IN, fcn_name, 20);
  139.  
  140.     /*
  141.      * Get Input Arguments
  142.      */
  143.  
  144.  
  145.     nr = mxGetM(T0_IN);
  146.     nc = mxGetN(T0_IN);
  147.     if (!mxIsNumeric(T0_IN) || mxIsComplex(T0_IN) || 
  148.         !mxIsFull(T0_IN)  || !mxIsDouble(T0_IN) || nr*nc != 1)
  149.         mexErrMsgTxt("Bad t0 input for ODE23.");
  150.     t0 = mxGetScalar(T0_IN);
  151.  
  152.     nr = mxGetM(TF_IN);
  153.     nc = mxGetN(TF_IN);
  154.     if (!mxIsNumeric(TF_IN) || mxIsComplex(TF_IN) || 
  155.         !mxIsFull(TF_IN)  || !mxIsDouble(TF_IN) || nr*nc != 1)
  156.         mexErrMsgTxt("Bad tfinal input for ODE23.");
  157.     tfinal = mxGetScalar(TF_IN);
  158.  
  159.     M = MAX(mxGetM(Y0_IN),mxGetN(Y0_IN));
  160.  
  161.     if (!mxIsNumeric(Y0_IN) || mxIsComplex(Y0_IN) || 
  162.         !mxIsFull(Y0_IN)  || !mxIsDouble(Y0_IN) || !M)
  163.         mexErrMsgTxt("Bad y0 input for ODE23.");
  164.     y0 = mxGetPr(Y0_IN);
  165.  
  166.  
  167.  
  168.     /*
  169.      * Create and Initialize Return Arguments
  170.      */
  171.  
  172.     T_OUT = mxCreateFull(1, 1, REAL);
  173.     Y_OUT = mxCreateFull(mxGetM(Y0_IN),mxGetN(Y0_IN), REAL);
  174.  
  175.     tout = mxGetPr(T_OUT);
  176.     yout = mxGetPr(Y_OUT);
  177.  
  178.     tout[0] = t0;
  179.     for (m = 0; m <M; m++) {
  180.         yout[m] = y0[m];
  181.     }
  182.  
  183.     /*
  184.      * Create arguments for calling user function
  185.      */
  186.  
  187.     T_TEMP = mxCreateFull(1, 1, REAL);
  188.     Y_TEMP = mxCreateFull(mxGetM(Y0_IN),mxGetN(Y0_IN), REAL);
  189.  
  190.     t = mxGetPr(T_TEMP);
  191.     y = mxGetPr(Y_TEMP);
  192.  
  193.     /*
  194.      * Create a temporary array for ydel
  195.      */
  196.  
  197.     ydel = (double *) mxCalloc(M, sizeof(double));
  198.  
  199.  
  200.     /*
  201.      * Initialization
  202.      */
  203.  
  204.     if (nrhs < 5) {
  205.         tol = 0.001;
  206.     } else {
  207.         nr = mxGetM(TOL_IN);
  208.         nc = mxGetN(TOL_IN);
  209.         if (!mxIsNumeric(TOL_IN) || mxIsComplex(TOL_IN) || 
  210.             !mxIsFull(TOL_IN)  || !mxIsDouble(TOL_IN) || nr*nc != 1)
  211.             mexErrMsgTxt("Bad tol input for ODE23.");
  212.         tol = mxGetScalar(TOL_IN);
  213.     }
  214.  
  215.     hmax = (tfinal - t0)/16;
  216.     h = hmax/8;
  217.  
  218.     *t = t0;
  219.  
  220.     k = 0;
  221.  
  222.     /*
  223.      * The main loop
  224.      */
  225.  
  226.     while ((*t < tfinal) && ((*t + h) >= *t)) {
  227.  
  228.         if ((*t + h) > tfinal) {
  229.             h = tfinal - *t;
  230.         }
  231.  
  232.         /*
  233.          * Compute the slopes
  234.          */
  235.  
  236.         /* ts = t  and  s0 = y */
  237.  
  238.         ts = tout[k];
  239.         s0 = &yout[k*M];
  240.  
  241.         /* s1 = feval(F, t, y); */
  242.  
  243.         *t = ts;
  244.         for  (m = 0; m < M; m++) {
  245.             y[m] = s0[m];
  246.         }
  247.  
  248.         mexCallMATLAB(1,tmp_plhs,2,tmp_prhs,fcn_name);
  249.         if (mxGetM(Y_OUT_TEMP)*mxGetN(Y_OUT_TEMP) != M) {
  250.             goto errorexit;
  251.         }
  252.         s1 = mxGetPr(Y_OUT_TEMP);
  253.         s_matptr[0] = Y_OUT_TEMP;
  254.  
  255.         /* s2 = feval(F, t+h, y+h*s1); */
  256.  
  257.         *t = ts + h;
  258.         for  (m = 0; m < M; m++) {
  259.             y[m] = s0[m]+h*s1[m];
  260.         }
  261.  
  262.         mexCallMATLAB(1,tmp_plhs,2,tmp_prhs,fcn_name);
  263.         if (mxGetM(Y_OUT_TEMP)*mxGetN(Y_OUT_TEMP) != M) {
  264.             goto errorexit;
  265.         }
  266.         s2 = mxGetPr(Y_OUT_TEMP);
  267.         s_matptr[1] = Y_OUT_TEMP;
  268.  
  269.         /* s3 = feval(F, t+h/2, y+h*(s1+s2)/4); */
  270.  
  271.         *t = ts + h/2;
  272.         for  (m = 0; m < M; m++) {
  273.             y[m] = s0[m]+h*(s1[m]+s2[m])/4;
  274.         }
  275.  
  276.         mexCallMATLAB(1,tmp_plhs,2,tmp_prhs,fcn_name);
  277.         if (mxGetM(Y_OUT_TEMP)*mxGetN(Y_OUT_TEMP) != M) {
  278.             goto errorexit;
  279.         }
  280.         s3 = mxGetPr(Y_OUT_TEMP);
  281.         s_matptr[2] = Y_OUT_TEMP;
  282.  
  283.         /*
  284.          * Estimate the error and the acceptable error
  285.          */
  286.  
  287.         for  (m = 0; m < M; m++) {
  288.             ydel[m] = h*(s1[m] - 2*s3[m] + s2[m])/3;
  289.         }
  290.         delta = inf_norm(ydel,M);
  291.  
  292.         ymax = inf_norm(&yout[k*M],M);
  293.         tau = tol*MAX(ymax,1.0);
  294.  
  295.         /*
  296.          * Update the solution only if the error is acceptable
  297.          */
  298.  
  299.         if (delta <= tau) {
  300.  
  301.             K = k+1;
  302.  
  303.             OLD_T_OUT = T_OUT;
  304.             OLD_Y_OUT = Y_OUT;
  305.             T_OUT = mxCreateFull(1,K+1,REAL);
  306.             Y_OUT = mxCreateFull(m,K+1,REAL);
  307.  
  308.             toutp = tout;
  309.             youtp = yout;
  310.             tout = mxGetPr(T_OUT);
  311.             yout = mxGetPr(Y_OUT);
  312.  
  313.             for (kk = 0;kk < K; kk++) {
  314.                 tout[kk] = toutp[kk];
  315.                 for(m = 0; m < M; m++) {
  316.                     yout[kk*M+m] = youtp[kk*M+m];
  317.                 }
  318.             }
  319.  
  320.             /* if k <> 0 Free OLD_T_OUT,OLD_Y_OUT */
  321.  
  322.             if (k) {
  323.                 mxFreeMatrix(OLD_T_OUT);
  324.                 mxFreeMatrix(OLD_Y_OUT);
  325.             }
  326.  
  327.             tout[K] = *t = tout[k] + h;
  328.             for (m = 0; m < M; m++) {
  329.                 yout[K*M+m] = yout[k*M+m]
  330.                     + h*(s1[m] + 4*s3[m] + s2[m])/6;
  331.             }
  332.  
  333.             k++;
  334.         }
  335.  
  336.         /*
  337.          * Free s_matptr[0], s_matptr[1], s_matptr[2]
  338.          */
  339.  
  340.         mxFreeMatrix(s_matptr[0]);
  341.         mxFreeMatrix(s_matptr[1]);
  342.         mxFreeMatrix(s_matptr[2]);
  343.  
  344.         /*
  345.          * Update the step size
  346.          */
  347.  
  348.         if (delta) {
  349.             h = MIN(hmax, 0.9*h*pow(tau/delta,powr));
  350.         }
  351.  
  352.  
  353.     }
  354.  
  355.     if (*t < tfinal) {
  356. #ifdef THINK_C
  357.         mexPrintf("Singularity likely at t = %f\r",*t);
  358. #else
  359.         mexPrintf("Singularity likely at t = %f\n",*t);
  360. #endif
  361.     }
  362.  
  363.     /*
  364.      * transpose outputs
  365.      */
  366.     mxSetM(T_OUT, mxGetN(T_OUT));
  367.     mxSetN(T_OUT, 1);
  368.  
  369.     Y_TEMP = Y_OUT;
  370.     mexCallMATLAB(1,&Y_OUT, 1, &Y_TEMP, ".'");
  371.     mxFreeMatrix(Y_TEMP);
  372.  
  373.     return;
  374.  
  375. errorexit:
  376.     mexPrintf("Function %s is not returing the correct number of state \
  377. derivatives.",fcn_name);
  378.     mexErrMsgTxt("Error in ODE23.");
  379.  
  380. }
  381.  
  382.