Merge branch 'harmonize_tsim_in_data_and_dirdyn' into 'dev'

Harmonize tsim in data and dirdyn

See merge request robotran/mbsysc!226

MBsysC/mbs_common/mbs_module/mbs_dirdyn.c

@@ -465,7 +465,7 @@ void mbs_dirdyn_init(MbsDirdyn* dd, MbsData* mbs_data)
 
        // Save first state of the system
         mbs_fct_dirdyn(dd->tsim, dd->y, dd->yd, mbs_data, dd);
-        mbs_dirdyn_save(dd, mbs_data, mbs_data->tsim);
+        mbs_dirdyn_save(dd, mbs_data, dd->tsim);
 
 
         // release memory

MBsysC/mbs_common/mbs_numerics/mbs_integrators/bader.c

@@ -131,6 +131,8 @@ void bader(double y[], double dydx[], int nv, double *xx, double htry, double ep
 		reduct = 1;
 	}		// try again.
 
+    dd->tsim = xnew; // Update of tsim
+
 	*xx = xnew;		// Successful step taken
 	*hdid = h;
 	first = 0;

MBsysC/mbs_common/mbs_numerics/mbs_integrators/dopri5.c

@@ -416,6 +416,8 @@ static int dopcor(unsigned n, FcnEqDiff fcn, double x, double* y, double xend,
             xold = x;
             x = xph;
 
+            dd->tsim = x; // Update of tsim
+
             if (iout)
             {
                 hout = h;
@@ -456,7 +458,7 @@ static int dopcor(unsigned n, FcnEqDiff fcn, double x, double* y, double xend,
                 nrejct = nrejct + 1;
             last = 0;
         }
-
+        
         h = hnew;
         last_hnew = hnew; // modif: line added
     }

MBsysC/mbs_common/mbs_numerics/mbs_integrators/mbs_bader.c

@@ -74,20 +74,20 @@ void loop_bader(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd, FILE
 
 	while (t < tf)
 	{
-		user_dirdyn_loop(mbs_data, mbs_dd); ///user loop
-
-		if (mbs_dd->options->flag_precise_dynamics) // Compute f' if asked
-		{
-			mbs_fct_dirdyn(mbs_dd->tsim, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd);
-		}
+        user_dirdyn_loop(mbs_data, mbs_dd); ///user loop
+        
+        if (mbs_dd->options->flag_precise_dynamics) // Compute f' if asked
+        {
+            mbs_fct_dirdyn(mbs_dd->tsim, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd);
+        }
 
 		for (i = 0; i < mbs_dd->nState; i++) {
 			yscal[i] = (1.0 > mbs_dd->y[i]) ? 1.0 : mbs_dd->y[i];
 		}
 
 		bader(mbs_dd->y, mbs_dd->yd, mbs_dd->nState, &t, h_cur, mbs_dd->options->toler, yscal, &hdid, &hnext, h_max, mbs_fct_dirdyn, mbs_data, mbs_dd);
-
-		mbs_fct_dirdyn(t, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd); // next f'
+        
+        mbs_fct_dirdyn(t, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd); // next f'
 		
 		h_cur = (hnext < tf - t) ? hnext : tf - t;
 		h_max = (mbs_dd->options->dt_max < tf - t) ? mbs_dd->options->dt_max : tf - t;

MBsysC/mbs_common/mbs_numerics/mbs_integrators/mbs_custom.c

@@ -50,12 +50,12 @@ void loop_custom(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd, FIL
 
     while (t < tf)
     {
-		user_dirdyn_loop(mbs_data, mbs_dd); ///user loop
+        user_dirdyn_loop(mbs_data, mbs_dd); ///user loop
 
-		if (mbs_dd->options->flag_precise_dynamics) // Compute f' if asked
-		{
-			mbs_fct_dirdyn(mbs_dd->tsim, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd);
-		}
+        if (mbs_dd->options->flag_precise_dynamics) // Compute f' if asked
+        {
+            mbs_fct_dirdyn(mbs_dd->tsim, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd);
+        }
 
         // ###### start of custom unknown integrator #########
         // Here we take ThetaSC which is not working
@@ -69,7 +69,7 @@ void loop_custom(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd, FIL
             mbs_dd->y[i] = mbs_dd->yout[i];
         }
 
-		mbs_fct_dirdyn(t, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd); // next f'
+        mbs_fct_dirdyn(t, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd); // next f'
 
         // save results if always asked or if waypoint reached
         if ((!mbs_dd->options->flag_solout_wp) || (mbs_dd->options->flag_solout_wp && t == tf)) {

MBsysC/mbs_common/mbs_numerics/mbs_integrators/mbs_euler_explicit.c

@@ -52,19 +52,19 @@ void loop_eulerEx(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd, FI
 
     while (mbs_dd->tsim <= tf)
     {
-		user_dirdyn_loop(mbs_data, mbs_dd); ///user loop
+        user_dirdyn_loop(mbs_data, mbs_dd); ///user loop
 
-		if (mbs_dd->options->flag_precise_dynamics) // Compute f' if asked
-		{
-			mbs_fct_dirdyn(mbs_dd->tsim, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd);
-		}
+        if (mbs_dd->options->flag_precise_dynamics) // Compute f' if asked
+        {
+            mbs_fct_dirdyn(mbs_dd->tsim, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd);
+        }
 		
         // calls the correct euler implementation
         ((MbsDirdynEulerEx *)mbs_dd->integrator_struct)->choice_of_euler(h, mbs_data, mbs_dd);
 
         mbs_dd->tsim += h;
 
-		mbs_fct_dirdyn(mbs_dd->tsim, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd); // next f'
+        mbs_fct_dirdyn(mbs_dd->tsim, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd); // next f'
 
         // save results if always asked or if waypoint reached
         if ((!mbs_dd->options->flag_solout_wp) || (mbs_dd->options->flag_solout_wp && mbs_dd->tsim == tf)) {

MBsysC/mbs_common/mbs_numerics/mbs_integrators/mbs_euler_implicit.c

@@ -59,19 +59,18 @@ void loop_euler_implicit(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs
 	mbs_fct_dirdyn(mbs_dd->tsim, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd);
 
     while (mbs_dd->tsim < tf) {
+        user_dirdyn_loop(mbs_data, mbs_dd); ///user loop
 
-		user_dirdyn_loop(mbs_data, mbs_dd); ///user loop
-
-		if (mbs_dd->options->flag_precise_dynamics) // Compute f' if asked
-		{
-			mbs_fct_dirdyn(mbs_dd->tsim, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd);
-		}
+        if (mbs_dd->options->flag_precise_dynamics) // Compute f' if asked
+        {
+            mbs_fct_dirdyn(mbs_dd->tsim, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd);
+        }
 		// Call the Euler Implicit method
 		euler_implicit(mbs_dd->y, mbs_dd->yd, mbs_dd->nState, &t, h, mbs_dd->yout, mbs_fct_dirdyn, mbs_data, mbs_dd);
 
-        mbs_fct_dirdyn(mbs_dd->tsim, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd);
+        mbs_dd->tsim += h;
 
-		mbs_dd->tsim += h;
+        mbs_fct_dirdyn(mbs_dd->tsim, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd);
 
         if (((!mbs_dd->options->flag_solout_wp) || (mbs_dd->options->flag_solout_wp && mbs_dd->tsim == tf))) {
             save_realtime_update(mbs_dd, mbs_data);

MBsysC/mbs_common/mbs_numerics/mbs_integrators/mbs_rk4.c

@@ -43,12 +43,12 @@ void loop_rk4(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd, FILE *
 
     while (mbs_dd->tsim <= tf)
     {
-		user_dirdyn_loop(mbs_data, mbs_dd); ///user loop
+        user_dirdyn_loop(mbs_data, mbs_dd); ///user loop
 
-		if (mbs_dd->options->flag_precise_dynamics) // Compute f' if asked
-		{
-			mbs_fct_dirdyn(mbs_dd->tsim, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd);
-		}
+        if (mbs_dd->options->flag_precise_dynamics) // Compute f' if asked
+        { 
+            mbs_fct_dirdyn(mbs_dd->tsim, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd);
+        }
 
         rk4(mbs_dd->y, mbs_dd->yd, mbs_dd->nState, mbs_dd->tsim, h, mbs_dd->yout, mbs_fct_dirdyn, mbs_data, mbs_dd);
 

MBsysC/mbs_common/mbs_numerics/mbs_integrators/mbs_rosenbrock.c

@@ -70,13 +70,14 @@ void loop_rosenbrock(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd,
 	user_dirdyn_loop(mbs_data, mbs_dd);
     mbs_fct_dirdyn(t, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd);
 
-    while (t < tf) {
-		user_dirdyn_loop(mbs_data, mbs_dd); ///user loop
+    while (t < tf) 
+    {
+        user_dirdyn_loop(mbs_data, mbs_dd); ///user loop
 
-		if (mbs_dd->options->flag_precise_dynamics) // Compute f' if asked
-		{
-			mbs_fct_dirdyn(t, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd);
-		}
+        if (mbs_dd->options->flag_precise_dynamics) // Compute f' if asked
+        {
+            mbs_fct_dirdyn(t, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd);
+        }
 
         for (i = 0; i < mbs_dd->nState; i++) {
             scaling[i] = (1.0 > mbs_dd->y[i]) ? 1.0 : mbs_dd->y[i];
@@ -87,6 +88,7 @@ void loop_rosenbrock(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd,
 
         mbs_fct_dirdyn(t, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd);
         
+        mbs_dd->tsim = t; // Update of tsim
 
         h_cur = (hnext < tf - t) ? hnext : tf - t;
         h_max = (mbs_dd->options->dt_max < tf - t) ? mbs_dd->options->dt_max : tf - t;

MBsysC/mbs_common/mbs_numerics/mbs_integrators/mbs_w_methods.c

@@ -58,21 +58,20 @@ void loop_w_methods(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd,
 	mbs_fct_dirdyn(mbs_dd->tsim, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd);
 
     while (mbs_dd->tsim < tf) {
-		
-		user_dirdyn_loop(mbs_data, mbs_dd); ///user loop
+        user_dirdyn_loop(mbs_data, mbs_dd); ///user loop
 
-		if (mbs_dd->options->flag_precise_dynamics) // Compute f' if asked
-		{
-			mbs_fct_dirdyn(mbs_dd->tsim, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd);
-		}
+        if (mbs_dd->options->flag_precise_dynamics) // Compute f' if asked
+        {
+            mbs_fct_dirdyn(mbs_dd->tsim, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd);
+        }
 
 		// Call the W Method function
 		w_methods(mbs_dd->y, mbs_dd->yd, mbs_dd->nState, mbs_dd->tsim, h, mbs_dd->yout, mbs_fct_dirdyn, mbs_data, mbs_dd);
 
+        mbs_dd->tsim += h;
+
         mbs_fct_dirdyn(mbs_dd->tsim, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd); // next f'
 		     
-		mbs_dd->tsim += h;
-
         if (((!mbs_dd->options->flag_solout_wp) || (mbs_dd->options->flag_solout_wp && mbs_dd->tsim == tf))) {
             save_realtime_update(mbs_dd, mbs_data);
         }