From 5c49bf8fef3fee788534ed05ee9ce60ea3393bc3 Mon Sep 17 00:00:00 2001 From: jmpenn Date: Tue, 18 Feb 2020 15:18:57 -0600 Subject: [PATCH] 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 | 11 +++-- include/trick/integrator_c_intf.h | 1 + .../trick/integration/src/trick_integrator.cc | 1 + .../Integrator/src/Integrator.cpp | 45 ++++++++++++++++++- .../Integrator/src/Integrator_C_Intf.cpp | 7 +++ .../trick_algorithms/ABM_Integrator.cpp | 5 +++ .../Euler_Cromer_Integrator.cpp | 5 +++ .../trick_algorithms/Euler_Integrator.cpp | 4 ++ .../trick_algorithms/MM4_Integrator.cpp | 5 +++ .../trick_algorithms/NL2_Integrator.cpp | 5 +++ .../trick_algorithms/RK2_Integrator.cpp | 5 +++ .../trick_algorithms/RK4_Integrator.cpp | 5 +++ .../trick_algorithms/RKF45_Integrator.cpp | 5 +++ .../trick_algorithms/RKF78_Integrator.cpp | 5 +++ .../trick_algorithms/RKG4_Integrator.cpp | 5 +++ 15 files changed, 108 insertions(+), 6 deletions(-) diff --git a/include/trick/Integrator.hh b/include/trick/Integrator.hh index 538ca847..b3d25330 100644 --- a/include/trick/Integrator.hh +++ b/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 diff --git a/include/trick/integrator_c_intf.h b/include/trick/integrator_c_intf.h index 4d5eef4b..62a8f4bf 100644 --- a/include/trick/integrator_c_intf.h +++ b/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, ...); diff --git a/trick_source/er7_utils/trick/integration/src/trick_integrator.cc b/trick_source/er7_utils/trick/integration/src/trick_integrator.cc index 844ea2ed..7009b79e 100644 --- a/trick_source/er7_utils/trick/integration/src/trick_integrator.cc +++ b/trick_source/er7_utils/trick/integration/src/trick_integrator.cc @@ -209,6 +209,7 @@ TrickIntegrator::initialize_trick_workspace ( deriv = alloc::allocate_array (buf_size); deriv2 = alloc::allocate_array (buf_size); state_ws = alloc::allocate_array (buf_size); + for (unsigned int ii = 0; ii < buf_size; ++ii) { deriv[ii] = er7_deriv; deriv2[ii] = er7_deriv2; diff --git a/trick_source/sim_services/Integrator/src/Integrator.cpp b/trick_source/sim_services/Integrator/src/Integrator.cpp index 344703d2..9200b04b 100644 --- a/trick_source/sim_services/Integrator/src/Integrator.cpp +++ b/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; diff --git a/trick_source/sim_services/Integrator/src/Integrator_C_Intf.cpp b/trick_source/sim_services/Integrator/src/Integrator_C_Intf.cpp index b4566606..c1d2b43b 100644 --- a/trick_source/sim_services/Integrator/src/Integrator_C_Intf.cpp +++ b/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) { diff --git a/trick_source/sim_services/Integrator/trick_algorithms/ABM_Integrator.cpp b/trick_source/sim_services/Integrator/trick_algorithms/ABM_Integrator.cpp index 90328819..036493ef 100644 --- a/trick_source/sim_services/Integrator/trick_algorithms/ABM_Integrator.cpp +++ b/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++) { diff --git a/trick_source/sim_services/Integrator/trick_algorithms/Euler_Cromer_Integrator.cpp b/trick_source/sim_services/Integrator/trick_algorithms/Euler_Cromer_Integrator.cpp index 9d4f9fda..d8b46443 100644 --- a/trick_source/sim_services/Integrator/trick_algorithms/Euler_Cromer_Integrator.cpp +++ b/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); diff --git a/trick_source/sim_services/Integrator/trick_algorithms/Euler_Integrator.cpp b/trick_source/sim_services/Integrator/trick_algorithms/Euler_Integrator.cpp index 13b15f55..18f470f9 100644 --- a/trick_source/sim_services/Integrator/trick_algorithms/Euler_Integrator.cpp +++ b/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); diff --git a/trick_source/sim_services/Integrator/trick_algorithms/MM4_Integrator.cpp b/trick_source/sim_services/Integrator/trick_algorithms/MM4_Integrator.cpp index bf511c33..4f09f045 100644 --- a/trick_source/sim_services/Integrator/trick_algorithms/MM4_Integrator.cpp +++ b/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); diff --git a/trick_source/sim_services/Integrator/trick_algorithms/NL2_Integrator.cpp b/trick_source/sim_services/Integrator/trick_algorithms/NL2_Integrator.cpp index ef1d0e0f..9f92433f 100644 --- a/trick_source/sim_services/Integrator/trick_algorithms/NL2_Integrator.cpp +++ b/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); diff --git a/trick_source/sim_services/Integrator/trick_algorithms/RK2_Integrator.cpp b/trick_source/sim_services/Integrator/trick_algorithms/RK2_Integrator.cpp index 40e8f745..9bad0f2d 100644 --- a/trick_source/sim_services/Integrator/trick_algorithms/RK2_Integrator.cpp +++ b/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); diff --git a/trick_source/sim_services/Integrator/trick_algorithms/RK4_Integrator.cpp b/trick_source/sim_services/Integrator/trick_algorithms/RK4_Integrator.cpp index 269dff51..0037fa10 100644 --- a/trick_source/sim_services/Integrator/trick_algorithms/RK4_Integrator.cpp +++ b/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); diff --git a/trick_source/sim_services/Integrator/trick_algorithms/RKF45_Integrator.cpp b/trick_source/sim_services/Integrator/trick_algorithms/RKF45_Integrator.cpp index 3558204d..ae29beeb 100644 --- a/trick_source/sim_services/Integrator/trick_algorithms/RKF45_Integrator.cpp +++ b/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); diff --git a/trick_source/sim_services/Integrator/trick_algorithms/RKF78_Integrator.cpp b/trick_source/sim_services/Integrator/trick_algorithms/RKF78_Integrator.cpp index 7bf13893..737470b3 100644 --- a/trick_source/sim_services/Integrator/trick_algorithms/RKF78_Integrator.cpp +++ b/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); diff --git a/trick_source/sim_services/Integrator/trick_algorithms/RKG4_Integrator.cpp b/trick_source/sim_services/Integrator/trick_algorithms/RKG4_Integrator.cpp index 498eba40..caaff599 100644 --- a/trick_source/sim_services/Integrator/trick_algorithms/RKG4_Integrator.cpp +++ b/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);