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

dopri5 integrated

parent f08db471
......@@ -20,6 +20,7 @@
#include "string.h"
#include "realtime.h"
#include "set_output.h"
#include "dopri5.h"
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
......@@ -54,7 +55,15 @@ MbsDirdyn* mbs_new_dirdyn_aux(MbsData* mbs_data, MbsAux* mbs_aux){
opts->buffersize = -1;
opts->resfilename = NULL;
opts->respath = NULL;
opts->realtime = 0;
opts->realtime = 0;
// adaptive time step integrator
opts->adapt_time_step = 0;
opts->adapt_rtoler = 1.0e-3;
opts->adapt_atoler = 1.0e-3;
opts->adapt_dt_max = 1.0e-3;
opts->adapt_max_steps = 10* ((opts->tf - opts->t0) / opts->dt0); // 10 is a safety factor because the total number of time steps is unknown
dirdyn->options = opts;
// initial the saving counter
......@@ -253,57 +262,63 @@ void mbs_dirdyn_loop(MbsDirdyn* dd, MbsData* mbs_data){
// NUMERICAL INTEGRATION
// - - - - - - - - - - -
while(dd->tsim < dd->options->tf){
if (dd->options->adapt_time_step)
{
dopri5(dd->nState, f_prim_dopri5, dd->options->t0, dd->y, dd->options->tf, &(dd->options->adapt_rtoler), &(dd->options->adapt_atoler),
0, NULL, 0, stdout, 0.0, 0.0, 0.0, 0.0, 0.0, dd->options->adapt_dt_max, dd->options->dt0, dd->options->adapt_max_steps, 1, 0, 0, NULL, 0, mbs_data, dd);
}
else
{
while(dd->tsim < dd->options->tf){
// user loop
user_loop(mbs_data, dd->mbs_aux);
// user loop
user_loop(mbs_data, dd->mbs_aux);
mbs_fct_dirdyn(dd->tsim, dd->y, dd->yd, mbs_data, dd->mbs_aux);
rk4(dd->y, dd->yd, dd->nState, dd->tsim, dd->dt, dd->yout, mbs_fct_dirdyn, mbs_data, dd->mbs_aux);
mbs_fct_dirdyn(dd->tsim, dd->y, dd->yd, mbs_data, dd->mbs_aux);
rk4(dd->y, dd->yd, dd->nState, dd->tsim, dd->dt, dd->yout, mbs_fct_dirdyn, mbs_data, dd->mbs_aux);
dd->tsim += dd->dt;
dd->tsim += dd->dt;
for (i=0 ; i<dd->nState ; i++)
{
dd->y[i] = dd->yout[i];
}
for (i=0 ; i<dd->nState ; i++)
{
dd->y[i] = dd->yout[i];
}
// save results to the buffer
if(dd->options->save2file){
dd->savePeriodCounter++;
if(dd->savePeriodCounter%dd->options->saveperiod == 0){
mbs_dirdyn_save(dd, mbs_data->tsim);
// save results to the buffer
if(dd->options->save2file){
dd->savePeriodCounter++;
if(dd->savePeriodCounter%dd->options->saveperiod == 0){
mbs_dirdyn_save(dd, mbs_data->tsim);
}
}
}
// real-time modules
#ifdef REAL_TIME
if (dd->options->realtime)
{
// update real-time buffers
mbs_realtime_update(realtime, mbs_data->tsim);
// real-time modules
#ifdef REAL_TIME
if (dd->options->realtime)
{
// update real-time buffers
mbs_realtime_update(realtime, mbs_data->tsim);
// real-time loop iteration
mbs_realtime_loop(realtime, mbs_data->tsim);
// real-time loop iteration
mbs_realtime_loop(realtime, mbs_data->tsim);
// break loop
if (realtime->simu_quit)
{
break;
}
}
#endif
// break loop
if (realtime->simu_quit)
// stop the simulation if 'flag_stop' set to 1
if (mbs_data->flag_stop)
{
break;
}
// printf("DIRDYN: current time: %f \n", dd->tsim);
}
#endif
// stop the simulation if 'flag_stop' set to 1
if (mbs_data->flag_stop)
{
break;
}
// printf("DIRDYN: current time: %f \n", dd->tsim);
}
}
void mbs_dirdyn_finish(MbsDirdyn* dd, MbsData* mbs_data){
......@@ -401,3 +416,48 @@ void mbs_fct_dirdyn(double tsim, double y[], double dydt[], MbsData *s, MbsAux *
}
}
/*! \brief function definig the differential equation for dopri5
*
* \param[in] n number of variables to integrate [-]
* \param[in] tsim current simulation time [s]
* \param[in] y state variables
* \param[out] dydt derivatives of the state variables
* \param[in] s Robotran main fields
* \param[in] dd direct dynamics
*/
void f_prim_dopri5(unsigned n, double tsim, double y[], double dydt[], MbsData *s, MbsDirdyn *dd)
{
// user loop
user_loop(s, dd->mbs_aux);
mbs_fct_dirdyn(tsim, y, dydt, s, dd->mbs_aux);
// save results to the buffer
if(dd->options->save2file){
dd->savePeriodCounter++;
if(dd->savePeriodCounter%dd->options->saveperiod == 0){
mbs_dirdyn_save(dd, s->tsim);
}
}
// real-time modules
#ifdef REAL_TIME
if (dd->options->realtime)
{
// update real-time buffers
mbs_realtime_update(realtime, mbs_data->tsim);
// real-time loop iteration
mbs_realtime_loop(realtime, mbs_data->tsim);
// break loop
if (realtime->simu_quit)
{
break;
}
}
#endif
}
......@@ -62,8 +62,14 @@ typedef struct MbsDirdynOptions
int buffersize;
int realtime; ///< 1 to activate to real-time features, 0 to deactivate them
int adapt_time_step; ///< 1 to use dopri5 (adaptive time step), 0 to use runge kutta 4 (fixed time step)
// adaptive time step options
int adapt_max_steps; ///< maximal number of stpes [-]
double adapt_rtoler; ///< relative error tolerances [-]
double adapt_atoler; ///< absolute error tolerances [-]
double adapt_dt_max; ///< maximal time step [s]
} MbsDirdynOptions;
......@@ -178,5 +184,6 @@ void mbs_delete_dirdyn(MbsDirdyn* dirdyn, MbsData* mbs_data);
*/
void mbs_fct_dirdyn(double t, double y[], double dydt[], MbsData *s, MbsAux *mbs_aux);
void f_prim_dopri5(unsigned n, double tsim, double y[], double dydt[], MbsData *s, MbsDirdyn *dd);
#endif
......@@ -76,9 +76,11 @@ static double max_d (double a, double b)
} /* max_d */
// modif: MbsData *s, MbsDirdyn *dd added
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)
double hmax, double* atoler, double* rtoler, int itoler,
MbsData *s, MbsDirdyn *dd)
{
double dnf, dny, atoli, rtoli, sk, h, h1, der2, der12, sqr;
unsigned i;
......@@ -118,7 +120,7 @@ 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 (n, x+h, yy1, f1);
fcn (n, x+h, yy1, f1, s, dd); // modif: s and dd added
/* estimate the second derivative of the solution */
der2 = 0.0;
......@@ -152,11 +154,13 @@ static double hinit (unsigned n, FcnEqDiff fcn, double x, double* y,
/* core integrator */
// modif: MbsData *s, MbsDirdyn *dd added
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)
double beta, double fac1, double fac2, unsigned* icont,
MbsData *s, MbsDirdyn *dd)
{
double facold, expo1, fac, facc1, facc2, fac11, posneg, xph;
double atoli, rtoli, hlamb, err, sk, hnew, yd0, ydiff, bspl;
......@@ -205,11 +209,11 @@ static int dopcor (unsigned n, FcnEqDiff fcn, double x, double* y, double xend,
last = 0;
hlamb = 0.0;
iasti = 0;
fcn (n, x, y, k1);
fcn (n, x, y, k1, s, dd); // modif: s and dd added
hmax = fabs (hmax);
iord = 5;
if (h == 0.0)
h = hinit (n, fcn, x, y, posneg, k1, k2, k3, iord, hmax, atoler, rtoler, itoler);
h = hinit (n, fcn, x, y, posneg, k1, k2, k3, iord, hmax, atoler, rtoler, itoler, s, dd); // modif: s and dd added
nfcn += 2;
reject = 0;
xold = x;
......@@ -259,23 +263,23 @@ 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 (n, x+c2*h, yy1, k2);
fcn (n, x+c2*h, yy1, k2, s, dd); // modif: s and dd added
for (i = 0; i < n; i++)
yy1[i] = y[i] + h * (a31*k1[i] + a32*k2[i]);
fcn (n, x+c3*h, yy1, k3);
fcn (n, x+c3*h, yy1, k3, s, dd); // modif: s and dd added
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);
fcn (n, x+c4*h, yy1, k4, s, dd); // modif: s and dd added
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);
fcn (n, x+c5*h, yy1, k5, s, dd); // modif: s and dd added
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);
fcn (n, xph, ysti, k6, s, dd); // modif: s and dd added
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);
fcn (n, xph, yy1, k2, s, dd); // modif: s and dd added
if (iout == 2)
{
if (nrds == n)
......@@ -442,11 +446,13 @@ static int dopcor (unsigned n, FcnEqDiff fcn, double x, double* y, double xend,
/* front-end */
// modif: MbsData *s, MbsDirdyn *dd added
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)
long nmax, int meth, long nstiff, unsigned nrdens, unsigned* icont, unsigned licont,
MbsData *s, MbsDirdyn *dd)
{
int arret, idid;
unsigned i;
......@@ -644,7 +650,7 @@ int dopri5
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);
solout, iout, nmax, uround, meth, nstiff, safe, beta, fac1, fac2, icont, s, dd); // modif: s and dd added
free (ysti);
free (k6);
free (k5); /* reverse order freeing too increase chances */
......
......@@ -176,7 +176,12 @@ nfcnRead Number of function calls.
#include <stdio.h>
#include <limits.h>
typedef void (*FcnEqDiff)(unsigned n, double x, double *y, double *f);
// modif: add mbs_data and mbs_aux headers
#include "mbs_data.h"
#include "mbs_dirdyn.h"
// modif: MbsData *s, MbsDirdyn *dd added
typedef void (*FcnEqDiff)(unsigned n, double x, double *y, double *f, MbsData *s, MbsDirdyn *dd);
typedef void (*SolTrait)(long nr, double xold, double x, double* y, unsigned n, int* irtrn);
......@@ -204,8 +209,10 @@ extern int dopri5
long nstiff, /* test for stiffness */
unsigned nrdens, /* number of components for which dense outpout is required */
unsigned* icont, /* indexes of components for which dense output is required, >= nrdens */
unsigned licont /* declared length of icon */
);
unsigned licont, /* declared length of icon */
MbsData *s, /* Robotran main fields */
MbsDirdyn *dd /* direct dynamics */
); // modif: MbsData *s, MbsDirdyn *dd added
extern double contd5
(unsigned ii, /* index of desired component */
......
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