.. _program_listing_file_dynamics_dynamics.cc:

Program Listing for File dynamics.cc
====================================

|exhale_lsh| :ref:`Return to documentation for file <file_dynamics_dynamics.cc>` (``dynamics/dynamics.cc``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   #include "lupnt/dynamics/dynamics.h"
   
   #include "lupnt/core/asset_factory.h"
   #include "lupnt/core/logger.h"
   #include "lupnt/core/progress_bar.h"
   
   namespace lupnt {
   
     Dynamics::Dynamics(Config& config) {
       if (config["print_progress"]) SetPrintProgress(config["print_progress"].as<bool>());
     }
   
     State Dynamics::Propagate(const State& x0, Real t0, Real tf, const State* u, MatXd* stm) {
       if (stm == nullptr) return Propagate(x0, t0, tf, u);
       auto func = [=, this](const VecX& x) {
         State xs = State(x, x0.GetNames(), x0.GetUnits());
         return Propagate(xs, t0, tf, u);
       };
       VecX xf_tmp;
       VecX x0_tmp = x0.cast<double>();
       jacobian(func, wrt(x0_tmp), at(x0_tmp), xf_tmp, *stm);
       State xf(xf_tmp, x0.GetNames(), x0.GetUnits());
       return xf;
     }
   
     MatX Dynamics::Propagate(const State& x0, const VecX& tfs, const State* u) {
       MatX xf = MatX::Zero(tfs.size(), x0.size());
       Ptr<ProgressBar> pbar = nullptr;
       if (print_progress_) pbar = Logger::GetProgressBar(tfs.size(), "Propagating", "Dynamics");
       xf.row(0) = x0;
       for (int i = 1; i < tfs.size(); i++) {
         Real t0_i = tfs(i - 1);
         Real tf_i = tfs(i);
         VecX x0_i = xf.row(i - 1);
         VecX xf_i = Propagate(x0_i, t0_i, tf_i, u);
         xf.row(i) = xf_i;
         if (print_progress_) pbar->Update(i);
       }
       if (print_progress_) pbar->Finish();
       return xf;
     }
   
     State Dynamics::PropagateWithParams(const State& x0, Real t0, Real tf, const ParamState& params,
                                         const State* u) {
       // Set parameters
       SetParams(params);
       return Propagate(x0, t0, tf, u);
     }
   
     State Dynamics::PropagateWithParams(const State& x0, Real t0, Real tf, const ParamState& params,
                                         const State* u, MatXd* stm_state, MatXd* stm_param) {
       if (stm_state == nullptr && stm_param == nullptr)
         return PropagateWithParams(x0, t0, tf, params, u);
       auto func = [=, this](const VecX& x, const VecX& p) {
         State xs = State(x, x0.GetNames(), x0.GetUnits());
         ParamState ps = ParamState(p, params.GetNames(), params.GetUnits());
         return PropagateWithParams(xs, t0, tf, ps, u);
       };
       VecX xf_tmp;
       VecX x0_tmp = x0.cast<double>();
       VecX p0_tmp = params.cast<double>();
       jacobian(func, wrt(x0_tmp), at(x0_tmp, p0_tmp), xf_tmp, *stm_state);
       jacobian(func, wrt(p0_tmp), at(x0_tmp, p0_tmp), xf_tmp, *stm_param);
   
       State xf(xf_tmp, x0.GetNames(), x0.GetUnits());
   
       return xf;
     }
   
     MatX Dynamics::PropagateWithParams(const State& x0, const VecX& tfs, const ParamState& params,
                                        const State* u) {
       // Set parameters
       MatX xf = MatX::Zero(tfs.size(), x0.size());
       Ptr<ProgressBar> pbar = nullptr;
       if (print_progress_) pbar = Logger::GetProgressBar(tfs.size(), "Propagating", "Dynamics");
       xf.row(0) = x0;
       for (int i = 1; i < tfs.size(); i++) {
         Real t0_i = tfs(i - 1);
         Real tf_i = tfs(i);
         VecX x0_i = xf.row(i - 1);
         VecX xf_i = PropagateWithParams(x0_i, t0_i, tf_i, params, u);
         xf.row(i) = xf_i;
         if (print_progress_) pbar->Update(i);
       }
       if (print_progress_) pbar->Finish();
       return xf;
     }
   
     // Define the GetRegistry function for this specialization (must come before explicit
     // instantiation)
     template <> std::unordered_map<std::string, AssetFactory<Dynamics, Config&>::Creator>&
     AssetFactory<Dynamics, Config&>::GetRegistry() {
       return Registry();
     }
   
     // Explicit template instantiation to ensure single registry across library boundaries
     template class AssetFactory<Dynamics, Config&>;
   
   }  // namespace lupnt