.. _program_listing_file_dynamics_surface_dynamics.cc:

Program Listing for File surface_dynamics.cc
============================================

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

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

.. code-block:: cpp

   
   
   #include "lupnt/dynamics/surface_dynamics.h"
   
   #include "lupnt/core/asset_factory.h"
   #include "lupnt/core/definitions.h"
   #include "lupnt/interfaces/yaml.h"
   #include "lupnt/numerics/math_utils.h"
   
   namespace lupnt {
   
     // StaticDynamics
     StaticDynamics::StaticDynamics() : Dynamics() {}
   
     StaticDynamics::StaticDynamics(Config& config) : Dynamics(config) {}
   
     State StaticDynamics::Propagate(const State& x0, Real t0, Real tf, const State* u) {
       (void)t0;
       (void)tf;
       (void)u;
       return x0;
     }
   
     REGISTER_FACTORY_CLASS(Dynamics, StaticDynamics)
   
     // Surface2DDynamics
     SurfaceDynamics2D::SurfaceDynamics2D() : Dynamics() {}
   
     SurfaceDynamics2D::SurfaceDynamics2D(Config& config) : Dynamics(config) {
       if (config["seed"]) {
         seed_ = config["seed"].as<int>();
         rng_ = MakePtr<std::mt19937>(seed_);
       }
       Q_.setZero();
       if (config["sigma_r"]) {
         Real sigma_r = config["sigma_r"].as<Real>();
         Q_(0, 0) = sigma_r * sigma_r;
         Q_(1, 1) = sigma_r * sigma_r;
         add_noise_ = true;
       }
       if (config["sigma_theta"]) {
         Real sigma_theta = RAD * config["sigma_theta"].as<Real>();
         Q_(2, 2) = sigma_theta * sigma_theta;
         add_noise_ = true;
       }
     }
   
     State SurfaceDynamics2D::Propagate(const State& x0, Real t0, Real tf, const State* u) {
       LUPNT_CHECK(u, "Control is required", "Surface2DDynamics");
       LUPNT_CHECK(u->size() == 2, "Control must be a 2D vector", "Surface2DDynamics");
   
       Real dt = tf - t0;
       State xf = x0;
       xf(0) += dt * (*u)(0) * cos(x0(2));
       xf(1) += dt * (*u)(0) * sin(x0(2));
       xf(2) += dt * (*u)(1);
   
       if (add_noise_) xf += SampleMvNormal(Vec3d::Zero(), Q_ * dt * dt, 1, rng_.get()).transpose();
   
       return xf;
     }
   
     REGISTER_FACTORY_CLASS(Dynamics, SurfaceDynamics2D)
   }  // namespace lupnt