D:/Basile/VisualCppProjects/hydroMEX3/hydroMEX3/f.cpp

Go to the documentation of this file.

/*
Wrap function for solver in dx=f(t,x,parameters)
*/



#include "math.h"
#include "mex.h"   

#include "string.h"

#include "hydro_source/hydro_includes.h"

#include "CVODEincludes.h"  // ODE solver includes


#include "f.h"


#ifndef PI_DEF
#define pi 3.141592653589793
#define PI 3.141592653589793
#endif




#define Ith(v,i)    NV_Ith_S(v,i-1)       /* Ith numbers components 1..NEQ */
#define IJth(A,i,j) DENSE_ELEM(A,i-1,j-1) /* IJth numbers rows,cols 1..NEQ */

/* Problem Constants */

#define NEQ   12//18                /* number of equations  */
#define T0    RCONST(0.0)      /* initial time           */






//  dy/dt = f(t,y,parameters)


static int f(realtype t, N_Vector y, N_Vector ydot, void *f_data)
{
        datas *d;
        parameters *P;
        ODE_data *f_dat;
        double *state;

        f_dat = (ODE_data*)f_data;

        d = f_dat->d;
        P = f_dat->P;
        state = f_dat->state;



        state[0] = Ith(y,1);
        state[1] = Ith(y,2);
        state[2] = Ith(y,3);
        state[3] = Ith(y,4);
        state[4] = Ith(y,5);
        state[5] = Ith(y,6);
        state[6] = Ith(y,7);
        state[7] = Ith(y,8);
        state[8] = Ith(y,9);
        state[9] = Ith(y,10);
        state[10] = Ith(y,11);
        state[11] = Ith(y,12);
        /*state[12] = Ith(y,13); //unused camera stuff
        state[13] = Ith(y,14);
        state[14] = Ith(y,15);
        state[15] = Ith(y,16);
        state[16] = Ith(y,17);
        state[17] = Ith(y,18);*/

        data_update_state(d, state);
        
        compute_state_dot(d,P,state);

        Ith(ydot,1) = d->dx;
        Ith(ydot,2) = d->dy;
        Ith(ydot,3) = d->dz;
        Ith(ydot,4) = d->dphi;
        Ith(ydot,5) = d->dtheta;
        Ith(ydot,6) = d->dpsi;
        Ith(ydot,7) = d->ddq[0];
        Ith(ydot,8) = d->ddq[1];
        Ith(ydot,9) = d->ddq[2];
        Ith(ydot,10) = d->ddq[3];
        Ith(ydot,11) = d->ddq[4];
        Ith(ydot,12) = d->ddq[5];
        /*Ith(ydot,13) = d->dx_cam;  //unused camera stuff
        Ith(ydot,14) = d->dy_cam;
        Ith(ydot,15) = d->dz_cam;
        Ith(ydot,16) = d->dd_cam[0];
        Ith(ydot,17) = d->dd_cam[1];
        Ith(ydot,18) = d->dd_cam[2];*/

/*
        Ith(ydot,1) = f_dat->d->t;  //integrator test:  dy/dt = t
        Ith(ydot,2) = f_dat->d->t;
        Ith(ydot,3) = f_dat->d->t;
        Ith(ydot,4) = f_dat->d->t;
        Ith(ydot,5) = f_dat->d->t;
        Ith(ydot,6) = f_dat->d->t;
        Ith(ydot,7) = f_dat->d->t;
        Ith(ydot,8) = f_dat->d->t;
        Ith(ydot,9) = f_dat->d->t;
        Ith(ydot,10) = f_dat->d->t;
        Ith(ydot,11) = f_dat->d->t;
        Ith(ydot,12) = f_dat->d->t;
        Ith(ydot,13) = f_dat->d->t;
        Ith(ydot,14) = f_dat->d->t;
        Ith(ydot,15) = f_dat->d->t;
        Ith(ydot,16) = f_dat->d->t;
        Ith(ydot,17) = f_dat->d->t;
        Ith(ydot,18) = f_dat->d->t;
*/      

  return(0);
}






void solver_init(ODE_data* f_dat)
{
        int k;
        
        f_dat->y = NULL;
        f_dat->abstol = NULL;
        f_dat->cvode_mem = NULL;

  /* Create serial vector of length NEQ for I.C. and abstol */
        f_dat->y = N_VNew_Serial(NEQ);
        f_dat->abstol = N_VNew_Serial(NEQ); 

  /* Initialize y */
        for (k=1; k<=NEQ; k++)
        {
                Ith(f_dat->y,k) = f_dat->state[k-1];
                //printf("Ith(y,%d) = %f \n",k,Ith(y,k));
        }


  /* Set the scalar relative tolerance */
        f_dat->reltol = f_dat->P->reltol;
  /* Set the vector absolute tolerance */
        for (k=1; k<=NEQ; k++)
        {
                Ith(f_dat->abstol,k) = f_dat->P->abstol[k-1];
        }


          /* 
     Call CVodeCreate to create the solver memory:
     
     CV_BDF     specifies the Backward Differentiation Formula
     CV_NEWTON  specifies a Newton iteration

     A pointer to the integrator problem memory is returned and stored in cvode_mem.
  */

        f_dat->cvode_mem = CVodeCreate(CV_BDF, CV_NEWTON);


  /* 
     Call CVodeMalloc to initialize the integrator memory: 
     
     cvode_mem is the pointer to the integrator memory returned by CVodeCreate
     f         is the user's right hand side function in y'=f(t,y)
     T0        is the initial time
     y         is the initial dependent variable vector
     CV_SV     specifies scalar relative and vector absolute tolerances
     &reltol   is a pointer to the scalar relative tolerance
     abstol    is the absolute tolerance vector
  */
        
        f_dat->flag = CVodeMalloc(f_dat->cvode_mem, f, T0, f_dat->y, CV_SV, f_dat->reltol, f_dat->abstol);

        f_dat->flag = CVodeSetFdata(f_dat->cvode_mem, (void*)f_dat);

        /* Call CVodeRootInit to specify the root function g with 2 components */
        //flag = CVodeRootInit(cvode_mem, 2, g, NULL);

        /* Call CVDense to specify the CVDENSE dense linear solver */
        f_dat->flag = CVDense(f_dat->cvode_mem, NEQ);  
        //flag = CVDiag(cvode_mem);

        /* Set the Jacobian routine to Jac (user-supplied) */
        //flag = CVDenseSetJacFn(cvode_mem, Jac, NULL);
}







void solve_ODE(ODE_data* f_dat, realtype tout)
{
        int k, tempk;
        long ti, nst;

        //integrate to next time value:
        f_dat->flag = CVode(f_dat->cvode_mem, tout, f_dat->y, &f_dat->d->t, CV_NORMAL);

        //store results and time if necessary:  
        if (f_dat->time_param != 0.0) store_state(f_dat->d->t, f_dat->y);


        f_dat->flag = CVodeGetNumSteps(f_dat->cvode_mem, &nst);
        f_dat->CumulNumOfSteps = (double)nst;
}



void solver_free(ODE_data* f_dat)
{
        /* Free y vector */
        N_VDestroy_Serial(f_dat->y);

        /* Free integrator memory */
        CVodeFree(&f_dat->cvode_mem);
}

---