Skip to content
GitLab
Commit d2393f8f authored by Nicolas Van der Noot's avatar Nicolas Van der Noot
Browse files

remove f_prim_dopri5

parent a2277acf
Hide whitespace changes
Inline Side-by-side
MBsysC/mbs_common/mbs_module/mbs_dirdyn.c
View file @ d2393f8f
......@@ -257,16 +257,6 @@ void mbs_dirdyn_loop(MbsDirdyn* dd, MbsData* mbs_data){
FILE *fileout_dopri5;
int i;
#ifdef REAL_TIME
Simu_realtime *realtime;
if (dd->options->realtime)
{
realtime = (Simu_realtime*) mbs_data->realtime;
}
#endif
// NUMERICAL INTEGRATION
// - - - - - - - - - - -
......@@ -281,7 +271,7 @@ void mbs_dirdyn_loop(MbsDirdyn* dd, MbsData* mbs_data){
fileout_dopri5 = NULL;
}
dopri5(dd->nState, f_prim_dopri5, dd->options->t0, dd->y, dd->options->tf, &(dd->options->adapt_rtoler), &(dd->options->adapt_atoler),
dopri5(dd->nState, mbs_fct_dirdyn, dd->options->t0, dd->y, dd->options->tf, &(dd->options->adapt_rtoler), &(dd->options->adapt_atoler),
0, NULL, 0, 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, mbs_data, dd);
}
else
......@@ -291,8 +281,8 @@ void mbs_dirdyn_loop(MbsDirdyn* dd, MbsData* mbs_data){
// 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);
rk4(dd->y, dd->yd, dd->nState, dd->tsim, dd->dt, dd->yout, mbs_fct_dirdyn, mbs_data, dd);
dd->tsim += dd->dt;
......@@ -300,32 +290,8 @@ void mbs_dirdyn_loop(MbsDirdyn* dd, MbsData* mbs_data){
{
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);
}
}
// 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
save_realtime_update(dd, mbs_data);
// stop the simulation if 'flag_stop' set to 1
if (mbs_data->flag_stop)
......@@ -386,7 +352,7 @@ void mbs_dirdyn_finish(MbsDirdyn* dd, MbsData* mbs_data){
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
void mbs_fct_dirdyn(double tsim, double y[], double dydt[], MbsData *s, MbsAux *mbs_aux)
void mbs_fct_dirdyn(double tsim, double y[], double dydt[], MbsData *s, MbsDirdyn *dd)
{
int i;
......@@ -411,7 +377,7 @@ void mbs_fct_dirdyn(double tsim, double y[], double dydt[], MbsData *s, MbsAux *
}
else // Numerical solving of constraints and inv(M)
{
i = dirdynared(mbs_aux,s);
i = dirdynared(dd->mbs_aux,s);
}
if(i<0){
......@@ -433,63 +399,44 @@ void mbs_fct_dirdyn(double tsim, double y[], double dydt[], MbsData *s, MbsAux *
}
}
/*! \brief function definig the differential equation for dopri5
/*! \brief update the real-time and saving modules if requested
*
* \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
* \param[in] dd direct dynamics module
* \param[in] mbs_data Robotran main structure
*/
void f_prim_dopri5(unsigned n, double tsim, double y[], double dydt[], MbsData *s, MbsDirdyn *dd)
void save_realtime_update(MbsDirdyn* dd, MbsData* mbs_data)
{
// variables declaration
MbsDirdynOptions* opts;
#ifdef REAL_TIME
#ifdef REAL_TIME
Simu_realtime *realtime;
#endif
opts = dd->options;
// user loop
user_loop(s, dd->mbs_aux);
// derivative
mbs_fct_dirdyn(tsim, y, dydt, s, dd->mbs_aux);
// save results to the buffer
if(opts->save2file && (tsim - dd->adapt_last_t_save >= opts->adapt_dt_save))
if(dd->options->save2file)
{
dd->savePeriodCounter++;
dd->adapt_last_t_save = tsim;
if(dd->savePeriodCounter%opts->saveperiod == 0)
if(dd->savePeriodCounter%dd->options->saveperiod == 0)
{
mbs_dirdyn_save(dd, tsim);
mbs_dirdyn_save(dd, mbs_data->tsim);
}
}
// real-time modules
#ifdef REAL_TIME
if (opts->realtime && (tsim - dd->adapt_last_t_rt >= opts->adapt_dt_rt))
if (dd->options->realtime)
{
realtime = (Simu_realtime*) s->realtime;
dd->adapt_last_t_rt = tsim;
realtime = (Simu_realtime*) mbs_data->realtime;
// update real-time buffers
mbs_realtime_update(realtime, tsim);
mbs_realtime_update(realtime, mbs_data->tsim);
// real-time loop iteration
mbs_realtime_loop(realtime, tsim);
mbs_realtime_loop(realtime, mbs_data->tsim);
// break loop
// quit the simulation
if (realtime->simu_quit)
{
s->flag_stop = 1;
mbs_data->flag_stop = 1;
}
}
#endif
......
MBsysC/mbs_common/mbs_module/mbs_dirdyn.h
View file @ d2393f8f
......@@ -189,7 +189,7 @@ void mbs_delete_dirdyn(MbsDirdyn* dirdyn, MbsData* mbs_data);
* \p mbs_aux the local data structure
*/
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);
void mbs_fct_dirdyn(double t, double y[], double dydt[], MbsData *s, MbsDirdyn *dd);
void save_realtime_update(MbsDirdyn* dd, MbsData* mbs_data);
#endif
MBsysC/mbs_common/mbs_numerics/dopri5.c
View file @ d2393f8f
......@@ -120,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, s, dd); // modif: s and dd added
fcn (x+h, yy1, f1, s, dd); // modif: n removed, s and dd added
/* estimate the second derivative of the solution */
der2 = 0.0;
......@@ -209,7 +209,7 @@ 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, s, dd); // modif: s and dd added
fcn (x, y, k1, s, dd); // modif: n removed, s and dd added
hmax = fabs (hmax);
iord = 5;
if (h == 0.0)
......@@ -257,7 +257,7 @@ static int dopcor (unsigned n, FcnEqDiff fcn, double x, double* y, double xend,
{
xout = x;
hout = h;
return -4;
return 2;
}
if ((x + 1.01*h - xend) * posneg > 0.0)
......@@ -271,23 +271,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, s, dd); // modif: s and dd added
fcn (x+c2*h, yy1, k2, s, dd); // modif: n removed, 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, s, dd); // modif: s and dd added
fcn (x+c3*h, yy1, k3, s, dd); // modif: n removed, 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, s, dd); // modif: s and dd added
fcn (x+c4*h, yy1, k4, s, dd); // modif: n removed, 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, s, dd); // modif: s and dd added
fcn (x+c5*h, yy1, k5, s, dd); // modif: n removed, 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, s, dd); // modif: s and dd added
fcn (xph, ysti, k6, s, dd); // modif: n removed, 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, s, dd); // modif: s and dd added
fcn (xph, yy1, k2, s, dd); // modif: n removed, s and dd added
if (iout == 2)
{
if (nrds == n)
......
MBsysC/mbs_common/mbs_numerics/dopri5.h
View file @ d2393f8f
......@@ -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 (unsigned n, double x, double *y, double *f)
void fcn (double x, double *y, double *f, MbsData *s, MbsDirdyn *dd) -> modif: n removed, s and dd added
where the array f will be filled with the function result.
......@@ -59,7 +59,7 @@ itoler Switch for atoler and rtoler :
solout A pointer to the output function called during integration.
If iout >= 1, it is called after every successful step. If iout = 0,
pass a pointer equal to NULL. solout must must have the following
pass a pointer equal to NULL. solout must have the following
prototype
solout (long nr, double xold, double x, double* y, unsigned n, int* irtrn)
......@@ -180,8 +180,10 @@ nfcnRead Number of function calls.
#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);
// modif: n removed, MbsData *s, MbsDirdyn *dd added
typedef void (*FcnEqDiff)(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);
......
MBsysC/mbs_common/mbs_numerics/integrator.h
View file @ d2393f8f
......@@ -6,10 +6,11 @@
#include "MBSfun.h"
#include "nrutil.h"
#include "mbs_dirdyn.h"
void rk4(double y[], double dydx[], int n, double x, double h, double yout[],
void (*derivs)(double, double [], double [],MbsData *, MbsAux *),
MbsData *s, MbsAux *mbs_aux);
void (*derivs)(double, double [], double [],MbsData *, MbsDirdyn *),
MbsData *s, MbsDirdyn *dd);
void derivs(double x, double y[], double dydx[],MbsData *s, MbsAux *mbs_aux);
......
MBsysC/mbs_common/mbs_numerics/rk4.c
View file @ d2393f8f
......@@ -11,8 +11,8 @@
* supplies the routine derivs(x,y,dydx) , which returns derivatives dydx at x.
*/
void rk4(double y[], double dydx[], int n, double x, double h, double yout[],
void (*derivs)(double, double [], double [],MbsData *, MbsAux *),
MbsData *s, MbsAux *mbs_aux)
void (*derivs)(double, double [], double [],MbsData *, MbsDirdyn *),
MbsData *s, MbsDirdyn *dd)
{
int i;
double xh,hh,h6,*dym,*dyt,*yt;
......@@ -31,12 +31,12 @@ void rk4(double y[], double dydx[], int n, double x, double h, double yout[],
yt[i]=y[i]+hh*dydx[i];
// Second step
(*derivs)(xh,yt,dyt,s,mbs_aux);
(*derivs)(xh,yt,dyt,s,dd);
for (i=0;i<n;i++)
yt[i]=y[i]+hh*dyt[i];
// Third step
(*derivs)(xh,yt,dym,s,mbs_aux);
(*derivs)(xh,yt,dym,s,dd);
for (i=0;i<n;i++)
{
yt[i]=y[i]+h*dym[i];
......@@ -44,7 +44,7 @@ void rk4(double y[], double dydx[], int n, double x, double h, double yout[],
}
// Fourth step
(*derivs)(x+h,yt,dyt,s,mbs_aux);
(*derivs)(x+h,yt,dyt,s,dd);
// Accumulate increments with proper weights
for (i=0;i<n;i++)
......
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