Commit d86690be authored by Nicolas Van der Noot's avatar Nicolas Van der Noot
Browse files

dopri5 with obligatory tsim points

parent 6818edae
......@@ -70,8 +70,8 @@ int main(int argc, char const *argv[])
mbs_dirdyn->options->respath = PROJECT_SOURCE_DIR"/../resultsR";
mbs_dirdyn->options->adapt_time_step = 1;
mbs_dirdyn->options->adapt_rtoler = 1.0e-14;
mbs_dirdyn->options->adapt_atoler = 1.0e-14;
mbs_dirdyn->options->adapt_rtoler = 1.0e-3;//1.0e-14;
mbs_dirdyn->options->adapt_atoler = 1.0e-3;//1.0e-14;
mbs_dirdyn->options->adapt_dt_max = 0.001;
mbs_dirdyn->options->adapt_verbose = 1;
......
......@@ -73,6 +73,9 @@ MbsDirdyn* mbs_new_dirdyn_aux(MbsData* mbs_data, MbsAux* mbs_aux){
// initial the saving counter
dirdyn->savePeriodCounter = 0;
// adaptive time step
dirdyn->adapt_flag_save = 0;
return dirdyn;
}
......@@ -137,6 +140,9 @@ void mbs_dirdyn_init(MbsDirdyn* dd, MbsData* mbs_data){
dd->yout = (double*) malloc(dd->nState*sizeof(double));
dd->yd = (double*) malloc(dd->nState*sizeof(double));
// adaptive time step
dd->adapt_dydt_save = (double*) malloc(dd->nState*sizeof(double));
// real-time modules activated
#ifdef REAL_TIME
if (dd->options->realtime)
......@@ -254,7 +260,7 @@ void mbs_dirdyn_loop(MbsDirdyn* dd, MbsData* mbs_data){
FILE *fileout_dopri5;
int i;
double **dopri5_alloc_tab;
double cur_t0, cur_tf;
double cur_t0, cur_tf, delta_dopri5_dt;
// NUMERICAL INTEGRATION
// - - - - - - - - - - -
......@@ -278,12 +284,24 @@ void mbs_dirdyn_loop(MbsDirdyn* dd, MbsData* mbs_data){
dopri5_alloc_tab[i] = (double*) malloc (dd->nState*sizeof(double));
}
delta_dopri5_dt = 0.001;
cur_t0 = dd->options->t0;
cur_tf = dd->options->tf;
cur_tf = cur_t0 + delta_dopri5_dt;
dopri5(dd->nState, mbs_fct_dirdyn, cur_t0, dd->y, cur_tf, &(dd->options->adapt_rtoler), &(dd->options->adapt_atoler),
0, solout_dopri5, 1, fileout_dopri5, 0.0, 0.0, 0.0, 0.0, 0.0, dd->options->adapt_dt_max, dd->options->dt0, dd->options->adapt_nmax,
1, 0, 0, NULL, 0, dopri5_alloc_tab, mbs_data, dd);
while (cur_tf <= dd->options->tf)
{
loop_dopri5(cur_t0, cur_tf, dopri5_alloc_tab, mbs_data, dd, fileout_dopri5);
cur_t0 = cur_tf;
cur_tf += delta_dopri5_dt;
// stop the simulation if 'flag_stop' set to 1
if (mbs_data->flag_stop)
{
break;
}
}
// release memory allocated for dopri5 vectors
for(i=0; i<DOPRI5_NB_ALLOC_VEC; i++)
......@@ -344,6 +362,7 @@ void mbs_dirdyn_finish(MbsDirdyn* dd, MbsData* mbs_data){
free(dd->y);
free(dd->yd);
free(dd->yout);
free(dd->adapt_dydt_save);
if(dd->options->save2file){
// write the buffer (in case they were not saved automatically)
......@@ -369,10 +388,9 @@ void mbs_dirdyn_finish(MbsDirdyn* dd, MbsData* mbs_data){
}
}
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*! \brief direct dynymics derivative computation
/*! \brief direct dynamics derivative computation
*
* \param[in] tsim current simulation time [s]
* \param[in] y state vector of size n
......@@ -468,19 +486,74 @@ void save_realtime_update(MbsDirdyn* dd, MbsData* mbs_data)
#endif
}
/*! \brief dopri5 loop (one call to dopri 5)
*
* \param[in] cur_t0 initial time [s]
* \param[in] cur_tf final time [s]
* \param[in,out] dopri5_alloc_tab allocated vectors for dopri5
* \param[in,out] s Robotran main structure
* \param[in,out] dd direct dynamic main module structure
* \param[out] fileout_dopri5 fileout for dopri5 fprintf
*/
void loop_dopri5(double cur_t0, double cur_tf, double **dopri5_alloc_tab, MbsData *s, MbsDirdyn *dd, FILE *fileout_dopri5)
{
dopri5(dd->nState, fcn_dopri5, cur_t0, dd->y, cur_tf, &(dd->options->adapt_rtoler), &(dd->options->adapt_atoler),
0, solout_dopri5, 1, fileout_dopri5, 0.0, 0.0, 0.0, 0.0, 0.0, dd->options->adapt_dt_max, dd->options->dt0, dd->options->adapt_nmax,
1, 0, 0, NULL, 0, dopri5_alloc_tab, s, dd);
}
/*! \brief dopri5 derivative function
*
* \param[in] n dimension of the system [-]
* \param[in] nr solour iteration [-]
* \param[in] tsim current simulation time [s]
* \param[in] y state vector of size n
* \param[out] dydt derivative of state vector y
* \param[in,out] s Robotran main structure
* \param[in,out] dd direct dynamic main module structure
*/
void fcn_dopri5(unsigned n, long nr, double tsim, double y[], double dydt[], MbsData *s, MbsDirdyn *dd)
{
int i;
if ( (!nr) && dd->adapt_flag_save ) // use previously computed solution dor first time step
{
for(i=0; i<n; i++)
{
dydt[i] = dd->adapt_dydt_save[i];
}
}
else // compute new solution
{
mbs_fct_dirdyn(tsim, y, dydt, s, dd);
if (nr == 6)
{
for(i=0; i<n; i++)
{
dd->adapt_dydt_save[i] = dydt[i];
}
dd->adapt_flag_save = 1;
}
}
}
/*! \brief dopri5 simulation own gestion at each succesful step
*
* \param[in] nr solour iteration [-]
* \param[in] tsim_old last time solout was called [s]
* \param[in] tsim current simulation time [s]
* \param[in] y state vector of size n
* \param[in] n dimension of the system
* \param[in] n dimension of the system [-]
* \param[out] irtrn set a negative value to stop the simulation
* \param[in,out] s Robotran main structure
* \param[in,out] dd direct dynamic main module structure
*/
void solout_dopri5(long nr, double tsim_old, double tsim, double y[], unsigned n, int* irtrn, MbsData *s, MbsDirdyn *dd)
{
//printf("%f\n", tsim*1.0e3);
// user loop
user_loop(s, dd);
......
......@@ -107,6 +107,10 @@ typedef struct MbsDirdyn
/** counter for checking wheter results must be saved */
int savePeriodCounter;
// adaptive time step
int adapt_flag_save;
double *adapt_dydt_save;
} MbsDirdyn;
......@@ -187,7 +191,8 @@ void mbs_delete_dirdyn(MbsDirdyn* dirdyn, MbsData* mbs_data);
void mbs_fct_dirdyn(double t, double y[], double dydt[], MbsData *s, MbsDirdyn *dd);
void save_realtime_update(MbsDirdyn* dd, MbsData* mbs_data);
void loop_dopri5(double cur_t0, double cur_tf, double **dopri5_alloc_tab, MbsData *s, MbsDirdyn *dd, FILE *fileout_dopri5);
void fcn_dopri5(unsigned n, long nr, double tsim, double y[], double dydt[], MbsData *s, MbsDirdyn *dd);
void solout_dopri5(long nr, double tsim_old, double tsim, double y[], unsigned n, int* irtrn, MbsData *s, MbsDirdyn *dd);
#endif
......@@ -6,7 +6,7 @@
#include "dopri5.h"
static long nfcn, nstep, naccpt, nrejct;
static long nfcn, nstep, naccpt, nrejct, n_fcn; // modif: n_fcn added
static double hout, xold, xout;
static unsigned nrds, *indir;
double *yy1, *k1, *k2, *k3, *k4, *k5, *k6, *ysti;
......@@ -120,7 +120,8 @@ static double hinit (unsigned n, FcnEqDiff fcn, double x, double* y,
/* perform an explicit Euler step */
for (i = 0; i < n; i++)
yy1[i] = y[i] + h * f0[i];
fcn (x+h, yy1, f1, s, dd); // modif: n removed, s and dd added
fcn (n, n_fcn, x+h, yy1, f1, s, dd); // modif: n_fcn, s and dd added
n_fcn++; // modif: new_line
/* estimate the second derivative of the solution */
der2 = 0.0;
......@@ -209,7 +210,8 @@ static int dopcor (unsigned n, FcnEqDiff fcn, double x, double* y, double xend,
last = 0;
hlamb = 0.0;
iasti = 0;
fcn (x, y, k1, s, dd); // modif: n removed, s and dd added
fcn (n, n_fcn, x, y, k1, s, dd); // modif: n_fcn, s and dd added
n_fcn++; // modif: new_line
hmax = fabs (hmax);
iord = 5;
if (h == 0.0)
......@@ -263,23 +265,29 @@ static int dopcor (unsigned n, FcnEqDiff fcn, double x, double* y, double xend,
/* the first 6 stages */
for (i = 0; i < n; i++)
yy1[i] = y[i] + h * a21 * k1[i];
fcn (x+c2*h, yy1, k2, s, dd); // modif: n removed, s and dd added
fcn (n, n_fcn, x+c2*h, yy1, k2, s, dd); // modif: n_fcn, s and dd added
n_fcn++; // modif: new_line
for (i = 0; i < n; i++)
yy1[i] = y[i] + h * (a31*k1[i] + a32*k2[i]);
fcn (x+c3*h, yy1, k3, s, dd); // modif: n removed, s and dd added
fcn (n, n_fcn, x+c3*h, yy1, k3, s, dd); // modif: n_fcn, s and dd added
n_fcn++; // modif: new_line
for (i = 0; i < n; i++)
yy1[i] = y[i] + h * (a41*k1[i] + a42*k2[i] + a43*k3[i]);
fcn (x+c4*h, yy1, k4, s, dd); // modif: n removed, s and dd added
fcn (n, n_fcn, x+c4*h, yy1, k4, s, dd); // modif: n_fcn, s and dd added
n_fcn++; // modif: new_line
for (i = 0; i <n; i++)
yy1[i] = y[i] + h * (a51*k1[i] + a52*k2[i] + a53*k3[i] + a54*k4[i]);
fcn (x+c5*h, yy1, k5, s, dd); // modif: n removed, s and dd added
fcn (n, n_fcn, x+c5*h, yy1, k5, s, dd); // modif: n_fcn, s and dd added
n_fcn++; // modif: new_line
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 (xph, ysti, k6, s, dd); // modif: n removed, s and dd added
fcn (n, n_fcn, xph, ysti, k6, s, dd); // modif: n_fcn, s and dd added
n_fcn++; // modif: new_line
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 (xph, yy1, k2, s, dd); // modif: n removed, s and dd added
fcn (n, n_fcn, xph, yy1, k2, s, dd); // modif: n_fcn, s and dd added
n_fcn++; // modif: new_line
if (iout == 2)
{
if (nrds == n)
......@@ -458,7 +466,7 @@ int dopri5
unsigned i;
/* initialisations */
nfcn = nstep = naccpt = nrejct = arret = 0;
nfcn = nstep = naccpt = nrejct = arret = n_fcn = 0; // modif: n_fcn added
rcont1 = rcont2 = rcont3 = rcont4 = rcont5 = NULL;
indir = NULL;
......
......@@ -35,7 +35,7 @@ n Dimension of the system (n < UINT_MAX).
fcn A pointer the the function definig the differential equation, this
function must have the following prototype
void fcn (double x, double *y, double *f, MbsData *s, MbsDirdyn *dd) -> modif: n removed, s and dd added
void fcn (unsigned n, long nr, double x, double *y, double *f, MbsData *s, MbsDirdyn *dd) -> modif: nr, s and dd added
where the array f will be filled with the function result.
......@@ -180,8 +180,8 @@ nfcnRead Number of function calls.
#include "mbs_data.h"
#include "mbs_dirdyn.h"
// modif: n removed, MbsData *s, MbsDirdyn *dd added
typedef void (*FcnEqDiff)(double x, double *y, double *f, MbsData *s, MbsDirdyn *dd);
// modif: long nr, MbsData *s, MbsDirdyn *dd added
typedef void (*FcnEqDiff)(unsigned n, long nr, double x, double *y, double *f, MbsData *s, MbsDirdyn *dd);
// modif, MbsData *s, MbsDirdyn *dd added
typedef void (*SolTrait)(long nr, double xold, double x, double* y, unsigned n, int* irtrn, MbsData *s, MbsDirdyn *dd);
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment