Add ability to reset the integrator state (#952)

* Add ability to reset the integrator state

* Make reset logic changes conditional on USE_ER7_UTILS_INTEGRATORS

include/trick/Integrator.hh

@@ -63,6 +63,10 @@ namespace Trick {
         virtual int integrate_2nd_order_ode (
            double const* accel, double* velocity, double* position);
 
+#ifndef SWIGPYTHON
+        void state_reset ();
+#endif
+
 #ifndef SWIGPYTHON
         void state_in (double* arg1, va_list argp);
 #endif
@@ -123,6 +127,9 @@ namespace Trick {
         bool use_deriv2;                  // -- set by integration technique
 
         double dt;               // -- set by IntegLoopSimObject.cpp
+#ifndef USE_ER7_UTILS_INTEGRATORS
+        double **state_origin;
+#endif
         double *state;
         double **deriv;
         double **deriv2;
@@ -145,10 +152,6 @@ namespace Trick {
 
     Integrator* getIntegrator( Integrator_type Alg, unsigned int State_size, double Dt = 0.0 );
 
-//    Integrator* getEr7Integrator(
-//       er7_utils::Integration::Technique, unsigned int State_size, double Dt);
-
-//    void deleteIntegrator( Integrator*&);
 }
 
 #endif

include/trick/integrator_c_intf.h

@@ -12,6 +12,7 @@ int integrate_1st_order_ode(const double* deriv, double* state);
 int integrate_2nd_order_ode(const double* acc, double* vel, double * pos);
 double get_integ_time(void);
 void set_integ_time(double time_value);
+void reset_state();
 void load_state(double* arg1, ... );
 void load_deriv( double* arg1, ...);
 void load_deriv2( double* arg1, ...);

trick_source/er7_utils/trick/integration/src/trick_integrator.cc

@@ -209,6 +209,7 @@ TrickIntegrator::initialize_trick_workspace (
    deriv    = alloc::allocate_array<double*> (buf_size);
    deriv2   = alloc::allocate_array<double*> (buf_size);
    state_ws = alloc::allocate_array<double*> (buf_size);
+
    for (unsigned int ii = 0; ii < buf_size; ++ii) {
       deriv[ii]    = er7_deriv;
       deriv2[ii]   = er7_deriv2;

trick_source/sim_services/Integrator/src/Integrator.cpp

@@ -19,6 +19,9 @@ Trick::Integrator::Integrator() {
    deriv = NULL;
    deriv2 = NULL;
    state_ws = NULL;
+#ifndef USE_ER7_UTILS_INTEGRATORS
+   state_origin = NULL;
+#endif
    time = 0.0;
    time_0 = 0.0;
    verbosity = 0 ;
@@ -85,18 +88,56 @@ int Trick::Integrator::integrate_2nd_order_ode (
     return rc;
 }
 
+void Trick::Integrator::state_reset () {
+#ifdef USE_ER7_UTILS_INTEGRATORS
+#else
+    if (intermediate_step == 0) {
+        int i = 0;
+        double* next_arg = state_origin[i];
+        if (verbosity) std::cout << "RESET STATE: \n";
+        while (next_arg != (double*) NULL) {
+            *next_arg = state[i];
+            if (verbosity) std::cout << "  " << *next_arg << "\n";
+            i++;
+            next_arg = state_origin[i];
+        }
+        if (verbosity) std::cout << std::endl;
+    }
+#endif
+}
+
+#ifdef USE_ER7_UTILS_INTEGRATORS
 void Trick::Integrator::state_in (double* arg1, va_list argp) {
     int i = 0;
     double* next_arg = arg1;
     if (verbosity) message_publish(MSG_DEBUG, "LOAD STATE: ");
     while (next_arg != (double*) NULL) { 
         state[i] = *next_arg;
-        if (verbosity) message_publish(MSG_DEBUG,"  %g", *next_arg);
+        if (verbosity) message_publish(MSG_DEBUG, "  %g", *next_arg);
         next_arg = va_arg(argp, double*);
         i++;
     }
-    if (verbosity) message_publish(MSG_DEBUG,"\n");
+    if (verbosity) message_publish(MSG_DEBUG, "\n");
 }
+#else
+void Trick::Integrator::state_in (double* arg1, va_list argp) {
+    if (intermediate_step == 0) {
+        int i = 0;
+        double* next_arg = arg1;
+        state_origin[i] = next_arg;
+        if (verbosity) std::cout << "LOAD STATE: \n";
+        while (next_arg != (double*) NULL) { 
+            state_origin[i] = next_arg;
+            state[i] = *next_arg;
+            if (verbosity) std::cout << "  " << *next_arg << "\n";
+            next_arg = va_arg(argp, double*);
+            i++;
+        }
+        state_origin[i] = (double*)NULL;
+        if (verbosity) std::cout << std::endl;
+    }
+}
+#endif
 
 void Trick::Integrator::state_in (double* arg1, ...) {
     va_list argp;

trick_source/sim_services/Integrator/src/Integrator_C_Intf.cpp

@@ -29,6 +29,13 @@ extern "C" void set_integ_time(double time_value) {
     trick_curr_integ->time = time_value;
 }
 
+extern "C" void reset_state() {
+#ifdef USE_ER7_UTILS_INTEGRATORS
+#else
+    trick_curr_integ->state_reset();
+#endif
+}
+
 extern "C" void load_state(double* arg1, ... ) {
     va_list argp;
     if (trick_curr_integ != NULL) {

trick_source/sim_services/Integrator/trick_algorithms/ABM_Integrator.cpp

@@ -11,6 +11,9 @@ void Trick::ABM_Integrator::initialize(int State_size, double Dt) {
     dt = Dt;
     num_state = State_size;
 
+    state_origin =  INTEG_ALLOC( double*, num_state );
+    state_origin[0] = (double*)NULL;
+
     /** Allocate the state vector.*/
     state =  INTEG_ALLOC( double, num_state );
 
@@ -46,6 +49,8 @@ Trick::ABM_Integrator::~ABM_Integrator() {
 
     int i;
 
+    if (state_origin) INTEG_FREE(state_origin);
+
     if (state) INTEG_FREE(state);
 
     for(i=0; i<4 ; i++) {

trick_source/sim_services/Integrator/trick_algorithms/Euler_Cromer_Integrator.cpp

@@ -13,6 +13,9 @@ void Trick::Euler_Cromer_Integrator::initialize(int State_size, double Dt) {
     dt = Dt;
     num_state = State_size;
 
+    state_origin =  INTEG_ALLOC( double*, num_state );
+    state_origin[0] = (double*)NULL;
+
     /** Allocate the state vector.*/
     state =  INTEG_ALLOC( double, num_state );
 
@@ -51,6 +54,8 @@ Trick::Euler_Cromer_Integrator::~Euler_Cromer_Integrator() {
     const int n_steps = 1;
     int i;
 
+    if (state_origin) INTEG_FREE(state_origin);
+
     /** Free the state vector.*/
     if (state) INTEG_FREE(state);
 

trick_source/sim_services/Integrator/trick_algorithms/Euler_Integrator.cpp

@@ -15,6 +15,9 @@ void Trick::Euler_Integrator::initialize(int State_size, double Dt) {
     dt = Dt;
     num_state = State_size;
 
+    state_origin =  INTEG_ALLOC( double*, num_state );
+    state_origin[0] = (double*)NULL;
+
     /** Allocate the state vector.*/
     state = INTEG_ALLOC( double, num_state );
 
@@ -40,6 +43,7 @@ Trick::Euler_Integrator::Euler_Integrator(int State_size, double Dt) {
  */
 Trick::Euler_Integrator::~Euler_Integrator() {
 
+    if (state_origin) INTEG_FREE(state_origin);
     if (state) INTEG_FREE(state);
     if (deriv[0]) INTEG_FREE(deriv[0]);
     if (deriv) INTEG_FREE(deriv);

trick_source/sim_services/Integrator/trick_algorithms/MM4_Integrator.cpp

@@ -12,6 +12,9 @@ void Trick::MM4_Integrator::initialize(int State_size, double Dt) {
     dt = Dt;
     num_state = State_size;
 
+    state_origin =  INTEG_ALLOC( double*, num_state );
+    state_origin[0] = (double*)NULL;
+
     /** Allocate the state vector.*/
     state =  INTEG_ALLOC( double, num_state );
 
@@ -44,6 +47,8 @@ Trick::MM4_Integrator::~MM4_Integrator() {
     const int n_steps = 3;
     int i;
 
+    if (state_origin) INTEG_FREE(state_origin);
+
     /** Free the state vector.*/
     if (state) INTEG_FREE(state);
 

trick_source/sim_services/Integrator/trick_algorithms/NL2_Integrator.cpp

@@ -13,6 +13,9 @@ void Trick::NL2_Integrator::initialize(int State_size, double Dt) {
     dt = Dt;
     num_state = State_size;
 
+    state_origin =  INTEG_ALLOC( double*, num_state );
+    state_origin[0] = (double*)NULL;
+
     /** Allocate the state vector.*/
     state =  INTEG_ALLOC( double, num_state );
 
@@ -45,6 +48,8 @@ Trick::NL2_Integrator::~NL2_Integrator() {
     const int n_steps = 2;
     int i;
 
+    if (state_origin) INTEG_FREE(state_origin);
+
     /** Free the state vector.*/
     if (state) INTEG_FREE(state);
 

trick_source/sim_services/Integrator/trick_algorithms/RK2_Integrator.cpp

@@ -14,6 +14,9 @@ void Trick::RK2_Integrator::initialize(int State_size, double Dt) {
     num_state = State_size;
     dt = Dt;
 
+    state_origin =  INTEG_ALLOC( double*, num_state );
+    state_origin[0] = (double*)NULL;
+
     /** Allocate the state vector.*/
     state =  INTEG_ALLOC( double, num_state );
 
@@ -46,6 +49,8 @@ Trick::RK2_Integrator::~RK2_Integrator() {
     const int n_steps = 2;
     int i;
 
+    if (state_origin) INTEG_FREE(state_origin);
+
     /** Free the state vector.*/
     if (state) INTEG_FREE(state);
 

trick_source/sim_services/Integrator/trick_algorithms/RK4_Integrator.cpp

@@ -12,6 +12,9 @@ void Trick::RK4_Integrator::initialize(int State_size, double Dt) {
     dt = Dt;
     num_state = State_size;
 
+    state_origin =  INTEG_ALLOC( double*, num_state );
+    state_origin[0] = (double*)NULL;
+
     /** Allocate the state vector.*/
     state =  INTEG_ALLOC( double, num_state );
 
@@ -44,6 +47,8 @@ Trick::RK4_Integrator::~RK4_Integrator() {
     const int n_steps = 4;
     int i;
 
+    if (state_origin) INTEG_FREE(state_origin);
+
     /** Free the state vector.*/
     if (state) INTEG_FREE(state);
 

trick_source/sim_services/Integrator/trick_algorithms/RKF45_Integrator.cpp

@@ -12,6 +12,9 @@ void Trick::RKF45_Integrator::initialize(int State_size, double Dt) {
     dt = Dt;
     num_state = State_size;
 
+    state_origin =  INTEG_ALLOC( double*, num_state );
+    state_origin[0] = (double*)NULL;
+
     /** Allocate the state vector.*/
     state =  INTEG_ALLOC( double, num_state );
 
@@ -44,6 +47,8 @@ Trick::RKF45_Integrator::~RKF45_Integrator() {
     const int n_steps = 6;
     int i;
 
+    if (state_origin) INTEG_FREE(state_origin);
+
     /** Free the state vector.*/
     if (state) INTEG_FREE(state);
 

trick_source/sim_services/Integrator/trick_algorithms/RKF78_Integrator.cpp

@@ -12,6 +12,9 @@ void Trick::RKF78_Integrator::initialize(int State_size, double Dt) {
     dt = Dt;
     num_state = State_size;
 
+    state_origin =  INTEG_ALLOC( double*, num_state );
+    state_origin[0] = (double*)NULL;
+
     /** Allocate the state vector.*/
     state =  INTEG_ALLOC( double, num_state );
 
@@ -44,6 +47,8 @@ Trick::RKF78_Integrator::~RKF78_Integrator() {
     const int n_steps = 12;
     int i;
 
+    if (state_origin) INTEG_FREE(state_origin);
+
     /** Free the state vector.*/
     if (state) INTEG_FREE(state);
 

trick_source/sim_services/Integrator/trick_algorithms/RKG4_Integrator.cpp

@@ -12,6 +12,9 @@ void Trick::RKG4_Integrator::initialize(int State_size, double Dt) {
     dt = Dt;
     num_state = State_size;
 
+    state_origin =  INTEG_ALLOC( double*, num_state );
+    state_origin[0] = (double*)NULL;
+
     /** Allocate the state vector.*/
     state =  INTEG_ALLOC( double, num_state );
 
@@ -45,6 +48,8 @@ Trick::RKG4_Integrator::~RKG4_Integrator() {
     const int n_steps = 4;
     int i;
 
+    if (state_origin) INTEG_FREE(state_origin);
+
     /** Free the state vector.*/
     if (state) INTEG_FREE(state);