[Fix#263][dd][integrators] finishing properly

MBsysC/mbs_common/mbs_numerics/mbs_integrators/mbs_bader.c

@@ -156,29 +156,48 @@ int loop_bader(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd) {
 }
 
 void finish_bader(MbsData *mbs_data, MbsDirdyn *dd) {
-    if (dd->nState != 0)
-    {
-        free_dvec_0(((MbsDirdynBader *)dd->integrator_struct)->yseq);
-        free_dvec_0(((MbsDirdynBader *)dd->integrator_struct)->ysav);
-        free_dvec_0(((MbsDirdynBader *)dd->integrator_struct)->yerr);
-        free_dvec_1(((MbsDirdynBader *)dd->integrator_struct)->x);
-        free_dvec_0(((MbsDirdynBader *)dd->integrator_struct)->err);
-        free_dmat_0(((MbsDirdynBader *)dd->integrator_struct)->dfdy);
-        free_dvec_0(((MbsDirdynBader *)dd->integrator_struct)->dfdx);
-        free_dmat_1(((MbsDirdynBader *)dd->integrator_struct)->d);
-
-
-        free_dvec_0(((MbsDirdynBader *)dd->integrator_struct)->Freeze_dydx);
-        free_dvec_0(((MbsDirdynBader *)dd->integrator_struct)->Freeze_dfdx);
-        free_dmat_0(((MbsDirdynBader *)dd->integrator_struct)->Freeze_dfdy);
-
-        free_ivec_0(((MbsDirdynBader *)dd->integrator_struct)->indx);
-        free_dvec_0(((MbsDirdynBader *)dd->integrator_struct)->ytemp);
-        free_dmat_0(((MbsDirdynBader *)dd->integrator_struct)->a);
-        free_dvec_0(((MbsDirdynBader *)dd->integrator_struct)->del);
-
-        free_dvec_0(((MbsDirdynBader *)dd->integrator_struct)->c);
-        free_dvec_1(((MbsDirdynBader *)dd->integrator_struct)->fx);
+    if (!dd) { // Check dd has not been set to NULL
+        if (dd->nState != 0 && dd->integrator_struct) // Check integrator structure has not been set to NULL
+        {
+            free_dvec_0(((MbsDirdynBader *)dd->integrator_struct)->yseq);
+            ((MbsDirdynBader *)dd->integrator_struct)->yseq = NULL;
+            free_dvec_0(((MbsDirdynBader *)dd->integrator_struct)->ysav);
+            ((MbsDirdynBader *)dd->integrator_struct)->ysav = NULL;
+            free_dvec_0(((MbsDirdynBader *)dd->integrator_struct)->yerr);
+            ((MbsDirdynBader *)dd->integrator_struct)->yerr = NULL;
+            free_dvec_1(((MbsDirdynBader *)dd->integrator_struct)->x);
+            ((MbsDirdynBader *)dd->integrator_struct)->x = NULL;
+            free_dvec_0(((MbsDirdynBader *)dd->integrator_struct)->err);
+            ((MbsDirdynBader *)dd->integrator_struct)->err = NULL;
+            free_dmat_0(((MbsDirdynBader *)dd->integrator_struct)->dfdy);
+            ((MbsDirdynBader *)dd->integrator_struct)->dfdy = NULL;
+            free_dvec_0(((MbsDirdynBader *)dd->integrator_struct)->dfdx);
+            ((MbsDirdynBader *)dd->integrator_struct)->dfdx = NULL;
+            free_dmat_1(((MbsDirdynBader *)dd->integrator_struct)->d);
+            ((MbsDirdynBader *)dd->integrator_struct)->d = NULL;
+
+
+            free_dvec_0(((MbsDirdynBader *)dd->integrator_struct)->Freeze_dydx);
+            ((MbsDirdynBader *)dd->integrator_struct)->Freeze_dydx = NULL;
+            free_dvec_0(((MbsDirdynBader *)dd->integrator_struct)->Freeze_dfdx);
+            ((MbsDirdynBader *)dd->integrator_struct)->Freeze_dfdx = NULL;
+            free_dmat_0(((MbsDirdynBader *)dd->integrator_struct)->Freeze_dfdy);
+            ((MbsDirdynBader *)dd->integrator_struct)->Freeze_dfdy = NULL;
+
+            free_ivec_0(((MbsDirdynBader *)dd->integrator_struct)->indx);
+            ((MbsDirdynBader *)dd->integrator_struct)->indx = NULL;
+            free_dvec_0(((MbsDirdynBader *)dd->integrator_struct)->ytemp);
+            ((MbsDirdynBader *)dd->integrator_struct)->ytemp = NULL;
+            free_dmat_0(((MbsDirdynBader *)dd->integrator_struct)->a);
+            ((MbsDirdynBader *)dd->integrator_struct)->a = NULL;
+            free_dvec_0(((MbsDirdynBader *)dd->integrator_struct)->del);
+            ((MbsDirdynBader *)dd->integrator_struct)->del = NULL;
+
+            free_dvec_0(((MbsDirdynBader *)dd->integrator_struct)->c);
+            ((MbsDirdynBader *)dd->integrator_struct)->c = NULL;
+            free_dvec_1(((MbsDirdynBader *)dd->integrator_struct)->fx);
+            ((MbsDirdynBader *)dd->integrator_struct)->fx = NULL;
+        }
     }
 }
 
@@ -186,5 +205,5 @@ void error_bader(MbsData *mbs_data, MbsDirdyn *dd, int err) {
     mbs_msg(">>DIRDYN>>\n");
     mbs_msg(">>DIRDYN>> Error during integration in direct dynamics at time %g s !\n", mbs_data->tsim);
     mbs_msg(">>DIRDYN>> During Bader integrator [%d] \n", err);
-    // finish_bader(mbs_data, dd); // already called in mbs_dirdyn_finish
+    finish_bader(mbs_data, dd); // already called in mbs_dirdyn_finish
 }

MBsysC/mbs_common/mbs_numerics/mbs_integrators/mbs_custom.c

@@ -113,9 +113,12 @@ int loop_custom(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd) {
 // Free the elements in the structure of custom integrator
 // (See the allocations in initialize_custom() )
 void finish_custom(MbsData *mbs_data, MbsDirdyn *mbs_dd) {
-    if (mbs_dd->nState != 0)
-    {
-        free(((MbsDirdynCustom *)mbs_dd->integrator_struct)->yt2);
+    if (!mbs_dd) { // Check dd has not been set to NULL
+        if (mbs_dd->nState != 0 && mbs_dd->integrator_struct) // Check integrator structure has not been set to NULL
+        {
+            free(((MbsDirdynCustom *)mbs_dd->integrator_struct)->yt2);
+            ((MbsDirdynCustom *)mbs_dd->integrator_struct)->yt2 = NULL;
+        }
     }
 }
 
@@ -123,5 +126,5 @@ void error_custom(MbsData *mbs_data, MbsDirdyn *dd, int err) {
     mbs_msg("\t >>Custom>>\n");
     mbs_msg("\t >>Custom>> Error during integration in direct dynamics at time %g s !\n", mbs_data->tsim);
     mbs_msg("\t >>Custom>> During Custom integrator [%d] \n", err);
-    // finish_custom(mbs_data, dd); // already called in mbs_dirdyn_finish
+    finish_custom(mbs_data, dd); // already called in mbs_dirdyn_finish
 }
\ No newline at end of file

MBsysC/mbs_common/mbs_numerics/mbs_integrators/mbs_dopri5.c

@@ -64,17 +64,23 @@ int loop_dopri5(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd) {
 void finish_dopri5(MbsData *mbs_data, MbsDirdyn *mbs_dd) {
 
     int i = 0;
-    if (mbs_dd->nState != 0)
-    {
-        free(((MbsDirdynDopri5 *)mbs_dd->integrator_struct)->dydt_save);
 
-        for (i = 0; i < DOPRI5_NB_ALLOC_VEC; i++)
+
+    if (!mbs_dd) { // Check dd has not been set to NULL
+        if (mbs_dd->nState != 0 && mbs_dd->integrator_struct) // Check integrator structure has not been set to NULL
         {
-            free(((MbsDirdynDopri5 *)mbs_dd->integrator_struct)->alloc_tab[i]);
+            free(((MbsDirdynDopri5 *)mbs_dd->integrator_struct)->dydt_save);
+            ((MbsDirdynDopri5 *)mbs_dd->integrator_struct)->dydt_save = NULL;
+
+            for (i = 0; i < DOPRI5_NB_ALLOC_VEC; i++)
+            {
+                free(((MbsDirdynDopri5 *)mbs_dd->integrator_struct)->alloc_tab[i]);
+                ((MbsDirdynDopri5 *)mbs_dd->integrator_struct)->alloc_tab[i] = NULL;
+            }
+            free(((MbsDirdynDopri5 *)mbs_dd->integrator_struct)->alloc_tab);
+            ((MbsDirdynDopri5 *)mbs_dd->integrator_struct)->alloc_tab = NULL;
         }
-        free(((MbsDirdynDopri5 *)mbs_dd->integrator_struct)->alloc_tab);
     }
-
 }
 
 int fcn_dopri5(unsigned int n, long nr, double tsim, double y[], double dydt[], MbsData *s, MbsDirdyn *mbs_dd)
@@ -153,5 +159,5 @@ void error_dopri5(MbsData *mbs_data, MbsDirdyn *dd, int err) {
     mbs_msg("\t >>DIRDYN>>\n");
     mbs_msg("\t >>DIRDYN>> Error during integration in direct dynamics at time %g s !\n", mbs_data->tsim);
     mbs_msg("\t >>DIRDYN>> During Dopri 5 integrator [%d] \n", err);
-    // finish_dopri5(mbs_data, dd); // already called in mbs_dirdyn_finish
+    finish_dopri5(mbs_data, dd); // already called in mbs_dirdyn_finish
 }
\ No newline at end of file

MBsysC/mbs_common/mbs_numerics/mbs_integrators/mbs_euler_explicit.c

@@ -115,11 +115,18 @@ int loop_eulerEx(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd) {
 
 void finish_eulerEx(MbsData *mbs_data, MbsDirdyn *dd) {
 
-    if (dd->options->integrator == AlphaM)
-    {
-        // free the vectors
-        free_dvec_0(((MbsDirdynEulerEx *)dd->integrator_struct)->a_n_1);
-        free_dvec_0(((MbsDirdynEulerEx *)dd->integrator_struct)->a_n);
+    if (!dd) { // Check dd has not been set to NULL
+        if (dd->integrator_struct) // Check integrator structure has not been set to NULL
+        {
+            if (dd->options->integrator == AlphaM)
+            {
+                // free the vectors
+                free_dvec_0(((MbsDirdynEulerEx *)dd->integrator_struct)->a_n_1);
+                ((MbsDirdynEulerEx *)dd->integrator_struct)->a_n_1 = NULL;
+                free_dvec_0(((MbsDirdynEulerEx *)dd->integrator_struct)->a_n);
+                ((MbsDirdynEulerEx *)dd->integrator_struct)->a_n = NULL;
+            }
+        }
     }
 }
 
@@ -128,7 +135,7 @@ void error_eulerEx(MbsData *mbs_data, MbsDirdyn *dd, int err)
     mbs_msg("\t >>Euler explicit>>\n");
     mbs_msg("\t >>Euler explicit>> Error during integration in direct dynamics at time %g s !\n", mbs_data->tsim);
     mbs_msg("\t >>Euler explicit>> During time integration [%d] \n", err);
-    // finish_eulerEx(mbs_data, dd); // already called in mbs_dirdyn_finish
+    finish_eulerEx(mbs_data, dd); // already called in mbs_dirdyn_finish
 }
 
 

MBsysC/mbs_common/mbs_numerics/mbs_integrators/mbs_euler_implicit.c

@@ -126,24 +126,39 @@ int loop_euler_implicit(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_
 }
 
 void finish_euler_implicit(MbsData *mbs_data, MbsDirdyn *dd) {
-    if (dd->nState != 0)
-    {
-        free_dvec_0(((MbsDirdynEulerIm *)dd->integrator_struct)->dfdx);
-        free_dmat_0(((MbsDirdynEulerIm *)dd->integrator_struct)->dfdy);
-        free_ivec_0(((MbsDirdynEulerIm *)dd->integrator_struct)->indx);
-        free_dvec_0(((MbsDirdynEulerIm *)dd->integrator_struct)->ysav);
-        free_dvec_0(((MbsDirdynEulerIm *)dd->integrator_struct)->dysav);
-
-        free_dvec_0(((MbsDirdynEulerIm *)dd->integrator_struct)->delta_p);
-        free_dvec_0(((MbsDirdynEulerIm *)dd->integrator_struct)->delta_v);
-
-        free_dmat_0(((MbsDirdynEulerIm *)dd->integrator_struct)->A);
-        free_dvec_0(((MbsDirdynEulerIm *)dd->integrator_struct)->B);
-        free_dvec_0(((MbsDirdynEulerIm *)dd->integrator_struct)->jpxv);
-
-        free_dvec_0(((MbsDirdynEulerIm *)dd->integrator_struct)->Freeze_dydx);
-        free_dvec_0(((MbsDirdynEulerIm *)dd->integrator_struct)->Freeze_dfdx);
-        free_dmat_0(((MbsDirdynEulerIm *)dd->integrator_struct)->Freeze_dfdy);
+
+    if (!dd) { // Check dd has not been set to NULL
+        if (dd->nState != 0 && dd->integrator_struct) // Check integrator structure has not been set to NULL
+        {
+            free_dvec_0(((MbsDirdynEulerIm *)dd->integrator_struct)->dfdx);
+            ((MbsDirdynEulerIm *)dd->integrator_struct)->dfdx = NULL;
+            free_dmat_0(((MbsDirdynEulerIm *)dd->integrator_struct)->dfdy);
+            ((MbsDirdynEulerIm *)dd->integrator_struct)->dfdy = NULL;
+            free_ivec_0(((MbsDirdynEulerIm *)dd->integrator_struct)->indx);
+            ((MbsDirdynEulerIm *)dd->integrator_struct)->indx = NULL;
+            free_dvec_0(((MbsDirdynEulerIm *)dd->integrator_struct)->ysav);
+            ((MbsDirdynEulerIm *)dd->integrator_struct)->ysav = NULL;
+            free_dvec_0(((MbsDirdynEulerIm *)dd->integrator_struct)->dysav);
+            ((MbsDirdynEulerIm *)dd->integrator_struct)->dysav = NULL;
+
+            free_dvec_0(((MbsDirdynEulerIm *)dd->integrator_struct)->delta_p);
+            ((MbsDirdynEulerIm *)dd->integrator_struct)->delta_p = NULL;
+            free_dvec_0(((MbsDirdynEulerIm *)dd->integrator_struct)->delta_v);
+            ((MbsDirdynEulerIm *)dd->integrator_struct)->delta_v = NULL;
+            free_dmat_0(((MbsDirdynEulerIm *)dd->integrator_struct)->A);
+            ((MbsDirdynEulerIm *)dd->integrator_struct)->A = NULL;
+            free_dvec_0(((MbsDirdynEulerIm *)dd->integrator_struct)->B);
+            ((MbsDirdynEulerIm *)dd->integrator_struct)->B = NULL;
+            free_dvec_0(((MbsDirdynEulerIm *)dd->integrator_struct)->jpxv);
+            ((MbsDirdynEulerIm *)dd->integrator_struct)->jpxv = NULL;
+
+            free_dvec_0(((MbsDirdynEulerIm *)dd->integrator_struct)->Freeze_dydx);
+            ((MbsDirdynEulerIm *)dd->integrator_struct)->Freeze_dydx = NULL;
+            free_dvec_0(((MbsDirdynEulerIm *)dd->integrator_struct)->Freeze_dfdx);
+            ((MbsDirdynEulerIm *)dd->integrator_struct)->Freeze_dfdx = NULL;
+            free_dmat_0(((MbsDirdynEulerIm *)dd->integrator_struct)->Freeze_dfdy);
+            ((MbsDirdynEulerIm *)dd->integrator_struct)->Freeze_dfdy = NULL;
+        }
     }
 }
 
@@ -151,5 +166,5 @@ void error_euler_implicit(MbsData *mbs_data, MbsDirdyn *dd, int err) {
     mbs_msg(">>DIRDYN>>\n");
     mbs_msg(">>DIRDYN>> Error during integration in direct dynamics at time %g s !\n", mbs_data->tsim);
     mbs_msg(">>DIRDYN>> During Euler Implicit integrator [%d] \n", err);
-    // finish_euler_implicit(mbs_data, dd); // already called in mbs_dirdyn_finish
+    finish_euler_implicit(mbs_data, dd); // already called in mbs_dirdyn_finish
 }
\ No newline at end of file

MBsysC/mbs_common/mbs_numerics/mbs_integrators/mbs_rosenbrock.c

@@ -141,26 +141,42 @@ int loop_rosenbrock(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd)
 }
 
 void finish_rosenbrock(MbsData *mbs_data, MbsDirdyn *dd) {
-
-    if (dd->nState != 0)
-    {
-        free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->dydt_save);
-
-        free_ivec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->indx);
-        free_dmat_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->a);
-        free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->dfdx);
-        free_dmat_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->dfdy);
-        free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->dysav);
-        free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->err);
-        free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->g1);
-        free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->g2);
-        free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->g3);
-        free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->g4);
-        free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->ysav);
-
-        free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->Freeze_dydx);
-        free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->Freeze_dfdx);
-        free_dmat_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->Freeze_dfdy);
+    if (dd) {  // New check dd has not been set to NULL
+        if (dd->nState != 0 && dd->integrator_struct)  // New check integrator structure has not been set to NULL
+        {
+            free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->dydt_save);
+            ((MbsDirdynRosenbrock *)dd->integrator_struct)->dydt_save = NULL;
+
+            free_ivec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->indx);
+            ((MbsDirdynRosenbrock *)dd->integrator_struct)->indx = NULL;
+            free_dmat_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->a);
+            ((MbsDirdynRosenbrock *)dd->integrator_struct)->a = NULL;
+            free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->dfdx);
+            ((MbsDirdynRosenbrock *)dd->integrator_struct)->dfdx = NULL;
+            free_dmat_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->dfdy);
+            ((MbsDirdynRosenbrock *)dd->integrator_struct)->dfdy = NULL;
+            free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->dysav);
+            ((MbsDirdynRosenbrock *)dd->integrator_struct)->dysav = NULL;
+            free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->err);
+            ((MbsDirdynRosenbrock *)dd->integrator_struct)->err = NULL;
+            free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->g1);
+            ((MbsDirdynRosenbrock *)dd->integrator_struct)->g1 = NULL;
+            free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->g2);
+            ((MbsDirdynRosenbrock *)dd->integrator_struct)->g2 = NULL;
+            free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->g3);
+            ((MbsDirdynRosenbrock *)dd->integrator_struct)->g3 = NULL;
+            free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->g4);
+            ((MbsDirdynRosenbrock *)dd->integrator_struct)->g4 = NULL;
+            free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->ysav);
+            ((MbsDirdynRosenbrock *)dd->integrator_struct)->ysav = NULL;
+
+            free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->Freeze_dydx);
+            ((MbsDirdynRosenbrock *)dd->integrator_struct)->Freeze_dydx = NULL;
+            free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->Freeze_dfdx);
+            ((MbsDirdynRosenbrock *)dd->integrator_struct)->Freeze_dfdx = NULL;
+            free_dmat_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->Freeze_dfdy);
+            ((MbsDirdynRosenbrock *)dd->integrator_struct)->Freeze_dfdy = NULL;
+        }
     }
 }
 
@@ -169,5 +185,5 @@ void error_rosenbrock(MbsData *mbs_data, MbsDirdyn *dd, int err)
     mbs_msg("\t >>Rosenbrock>>\n");
     mbs_msg("\t >>Rosenbrock>> Error during integration in direct dynamics at time %g s !\n", mbs_data->tsim);
     mbs_msg("\t >>Rosenbrock>> During Rosenbrock integrator [%d] \n", err);
-    // finish_rosenbrock(mbs_data, dd); // already called in mbs_dirdyn_finish
+    finish_rosenbrock(mbs_data, dd); // already called in mbs_dirdyn_finish
 }
\ No newline at end of file

MBsysC/mbs_common/mbs_numerics/mbs_integrators/mbs_w_methods.c

@@ -146,37 +146,56 @@ int loop_w_methods(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd) {
 
 void finish_w_methods(MbsData *mbs_data, MbsDirdyn *dd) {
 
-    if (!dd) {
-        return;
-    }
-
-    if (dd->nState != 0)
-    {
-        free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->dfdx);
-        free_dmat_0(((MbsDirdynWMethods *)dd->integrator_struct)->dfdy);
-        free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->f);
-        free_ivec_0(((MbsDirdynWMethods *)dd->integrator_struct)->indx);
-        free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->dysav);
-        free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->ysav);
-
-        free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->Freeze_dydx);
-        free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->Freeze_dfdx);
-        free_dmat_0(((MbsDirdynWMethods *)dd->integrator_struct)->Freeze_dfdy);
-
-        // Free the memory
-        free_dmat_0(((MbsDirdynWMethods *)dd->integrator_struct)->delta_p);
-        free_dmat_0(((MbsDirdynWMethods *)dd->integrator_struct)->delta_v);
-        free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->A_param);
-        free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->B_param);
-        free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->C_param);
-        free_dmat_0(((MbsDirdynWMethods *)dd->integrator_struct)->GAM_param);
-        free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->sum_alpha_gamma);
-        free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->partial_right_hand_term);
-        free_dmat_0(((MbsDirdynWMethods *)dd->integrator_struct)->A);
-        free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->B);
-        free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->jpxv);
-
-        free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->intermediate_y);
+    if (!dd) { // Check dd has not been set to NULL
+        if (dd->nState != 0 && dd->integrator_struct) // Check integrator structure has not been set to NULL
+        {
+            free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->dfdx);
+            ((MbsDirdynWMethods *)dd->integrator_struct)->dfdx = NULL;
+            free_dmat_0(((MbsDirdynWMethods *)dd->integrator_struct)->dfdy);
+            ((MbsDirdynWMethods *)dd->integrator_struct)->dfdy = NULL;
+            free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->f);
+            ((MbsDirdynWMethods *)dd->integrator_struct)->f = NULL;
+            free_ivec_0(((MbsDirdynWMethods *)dd->integrator_struct)->indx);
+            ((MbsDirdynWMethods *)dd->integrator_struct)->indx = NULL;
+            free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->dysav);
+            ((MbsDirdynWMethods *)dd->integrator_struct)->dysav = NULL;
+            free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->ysav);
+            ((MbsDirdynWMethods *)dd->integrator_struct)->ysav = NULL;
+
+            free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->Freeze_dydx);
+            ((MbsDirdynWMethods *)dd->integrator_struct)->Freeze_dydx = NULL;
+            free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->Freeze_dfdx);
+            ((MbsDirdynWMethods *)dd->integrator_struct)->Freeze_dfdx = NULL;
+            free_dmat_0(((MbsDirdynWMethods *)dd->integrator_struct)->Freeze_dfdy);
+            ((MbsDirdynWMethods *)dd->integrator_struct)->Freeze_dfdy = NULL;
+
+            // Free the memory
+            free_dmat_0(((MbsDirdynWMethods *)dd->integrator_struct)->delta_p);
+            ((MbsDirdynWMethods *)dd->integrator_struct)->delta_p = NULL;
+            free_dmat_0(((MbsDirdynWMethods *)dd->integrator_struct)->delta_v);
+            ((MbsDirdynWMethods *)dd->integrator_struct)->delta_v = NULL;
+            free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->A_param);
+            ((MbsDirdynWMethods *)dd->integrator_struct)->A_param = NULL;
+            free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->B_param);
+            ((MbsDirdynWMethods *)dd->integrator_struct)->B_param = NULL;
+            free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->C_param);
+            ((MbsDirdynWMethods *)dd->integrator_struct)->C_param = NULL;
+            free_dmat_0(((MbsDirdynWMethods *)dd->integrator_struct)->GAM_param);
+            ((MbsDirdynWMethods *)dd->integrator_struct)->GAM_param = NULL;
+            free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->sum_alpha_gamma);
+            ((MbsDirdynWMethods *)dd->integrator_struct)->sum_alpha_gamma = NULL;
+            free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->partial_right_hand_term);
+            ((MbsDirdynWMethods *)dd->integrator_struct)->partial_right_hand_term = NULL;
+            free_dmat_0(((MbsDirdynWMethods *)dd->integrator_struct)->A);
+            ((MbsDirdynWMethods *)dd->integrator_struct)->A = NULL;
+            free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->B);
+            ((MbsDirdynWMethods *)dd->integrator_struct)->B = NULL;
+            free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->jpxv);
+            ((MbsDirdynWMethods *)dd->integrator_struct)->jpxv = NULL;
+
+            free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->intermediate_y);
+            ((MbsDirdynWMethods *)dd->integrator_struct)->intermediate_y = NULL;
+        }
     }
 }
 
@@ -184,5 +203,5 @@ void error_w_methods(MbsData *mbs_data, MbsDirdyn *dd, int err) {
     mbs_msg("\t >>DIRDYN>>\n");
     mbs_msg("\t >>DIRDYN>> Error during integration in direct dynamics at time %g s !\n", mbs_data->tsim);
     mbs_msg("\t >>DIRDYN>> During W Methods integrator [%d] \n", err);
-    // finish_w_methods(mbs_data, dd); // already called in mbs_dirdyn_finish
+    finish_w_methods(mbs_data, dd); // already called in mbs_dirdyn_finish
 }
\ No newline at end of file