solout gestion

MBsysC/mbs_common/mbs_module/mbs_dirdyn.c

@@ -64,8 +64,6 @@ MbsDirdyn* mbs_new_dirdyn_aux(MbsData* mbs_data, MbsAux* mbs_aux){
     opts->adapt_rtoler = 1.0e-4;
     opts->adapt_atoler = 1.0e-3;
     opts->adapt_dt_max = 1.0e-3;
-    opts->adapt_dt_save = 1.0e-3;
-    opts->adapt_dt_rt = 1.0e-3;
     opts->adapt_nmax = 1.0e9;
 
     dirdyn->options = opts;
@@ -155,10 +153,6 @@ void mbs_dirdyn_init(MbsDirdyn* dd, MbsData* mbs_data){
     dd->tsim = dd->options->t0;
     dd->dt   = dd->options->dt0;
 
-    // adaptive time step
-    dd->adapt_last_t_save = dd->tsim - dd->options->adapt_dt_save;
-    dd->adapt_last_t_rt   = dd->tsim - dd->options->adapt_dt_rt;
-
     // user intialization
     user_init(mbs_data, dd);
 
@@ -273,7 +267,7 @@ void mbs_dirdyn_loop(MbsDirdyn* dd, MbsData* mbs_data){
         }
 
         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);
+            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, mbs_data, dd);
     }
     else
     {
@@ -292,6 +286,7 @@ void mbs_dirdyn_loop(MbsDirdyn* dd, MbsData* mbs_data){
                 dd->y[i] = dd->yout[i];
             }
    
+            // save data + real-time features
             save_realtime_update(dd, mbs_data);
 
             // stop the simulation if 'flag_stop' set to 1
@@ -353,6 +348,14 @@ void mbs_dirdyn_finish(MbsDirdyn* dd, MbsData* mbs_data){
 
 /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
 
+/*! \brief direct dynymics derivative computation
+ * 
+ * \param[in] tsim current simulation time [s]
+ * \param[in] y state vector of size n
+ * \param[out] dydt derivative
+ * \param[in,out] s Robotran main structure
+ * \param[in,out] dd direct dynamic main module structure
+ */
 void mbs_fct_dirdyn(double tsim, double y[], double dydt[], MbsData *s, MbsDirdyn *dd)
 {
     int i;
@@ -440,3 +443,26 @@ void save_realtime_update(MbsDirdyn* dd, MbsData* mbs_data)
     }
     #endif
 }
+
+/*! \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[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)
+{
+    // user loop
+    user_loop(s, dd);
+
+    // save data + real-time features
+    save_realtime_update(dd, s);
+
+    // quit the simulation if requested
+    *irtrn = (s->flag_stop) ? -1 : 1;
+}

MBsysC/mbs_common/mbs_module/mbs_dirdyn.h

@@ -62,6 +62,7 @@ typedef struct MbsDirdynOptions
     int buffersize;
 
     int realtime; ///< 1 to activate to real-time features, 0 to deactivate them
+
     int accelred; ///< 1 to use accelred, 0 otherwise    
 
     // adaptive time step
@@ -71,8 +72,6 @@ typedef struct MbsDirdynOptions
     double adapt_rtoler;  ///< relative error tolerances [-]
     double adapt_atoler;  ///< absolute error tolerances [-]
     double adapt_dt_max;  ///< maximal time step [s]
-    double adapt_dt_save; ///< minimal time interval between two saved values [s]
-    double adapt_dt_rt;   ///< minimial time interval between two calls to the real-time features [s]
 
 } MbsDirdynOptions;
 
@@ -193,4 +192,6 @@ 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 solout_dopri5(long nr, double tsim_old, double tsim, double y[], unsigned n, int* irtrn, MbsData *s, MbsDirdyn *dd);
+
 #endif

MBsysC/mbs_common/mbs_numerics/dopri5.c

@@ -222,7 +222,7 @@ static int dopcor (unsigned n, FcnEqDiff fcn, double x, double* y, double xend,
         irtrn = 1;
         hout = h;
         xout = x;
-        solout (naccpt+1, xold, x, y, n, &irtrn);
+        solout (naccpt+1, xold, x, y, n, &irtrn, s, dd);
         if (irtrn < 0)
         {
             if (fileout)
@@ -252,14 +252,6 @@ static int dopcor (unsigned n, FcnEqDiff fcn, double x, double* y, double xend,
             return -3;
         }
 
-        // modif: stop the simulation if 'flag_stop' is 1
-        if (s->flag_stop) 
-        {
-          xout = x;
-          hout = h;
-          return 2;
-        }
-
         if ((x + 1.01*h - xend) * posneg > 0.0)
         {
             h = xend - x;
@@ -413,7 +405,7 @@ static int dopcor (unsigned n, FcnEqDiff fcn, double x, double* y, double xend,
             {
         hout = h;
         xout = x;
-        solout (naccpt+1, xold, x, y, n, &irtrn);
+        solout (naccpt+1, xold, x, y, n, &irtrn, s, dd);
         if (irtrn < 0)
         {
             if (fileout)

MBsysC/mbs_common/mbs_numerics/dopri5.h

@@ -62,7 +62,7 @@ solout   A pointer to the output function called during integration.
          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)
+             solout (long nr, double xold, double x, double* y, unsigned n, int* irtrn, MbsData* s, MbsDirdyn* dd) -> modif: s and dd added
 
          where y is the solution the at nr-th grid point x, xold is the
          previous grid point and irtrn serves to interrupt the integration
@@ -183,8 +183,8 @@ nfcnRead    Number of function calls.
 // 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);
+// 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);
 
 
 extern int dopri5