Linear.hh

Go to the documentation of this file.

/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *\
 * Copyright (c) 2025, Davide Stocco and Enrico Bertolazzi.                                      *
 *                                                                                               *
 * The Sandals project is distributed under the BSD 2-Clause License.                            *
 *                                                                                               *
 * Davide Stocco                                                               Enrico Bertolazzi *
 * University of Trento                                                     University of Trento *
 * e-mail: davide.stocco@unitn.it                             e-mail: enrico.bertolazzi@unitn.it *
\* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
#pragma once
 
#ifndef SANDALS_LINEAR_SYSTEM_HH
#define SANDALS_LINEAR_SYSTEM_HH
 
#include <Sandals.hh>
#include <Sandals/System/Explicit.hh>
 
namespace Sandals {
 
  /*\
   |   _     _
   |  | |   (_)_ __   ___  __ _ _ __
   |  | |   | | '_ \ / _ \/ _` | '__|
   |  | |___| | | | |  __/ (_| | |
   |  |_____|_|_| |_|\___|\__,_|_|
   |
  \*/

  template <typename Real, Integer N, Integer M = 0>
  class Linear : public Explicit<Real, N, M>
  {
  public:
    SANDALS_BASIC_CONSTANTS(Real) 
    const Real SQRT_EPSILON{std::sqrt(EPSILON)};  \
 
    using Pointer = std::shared_ptr<SemiExplicit<Real, N, M>>; 
    using VectorF = typename Explicit<Real, N, M>::VectorF; 
    using MatrixJF = typename Explicit<Real, N, M>::MatrixJF; 
    using MatrixE = typename Explicit<Real, N, M>::MatrixJF; 
    using MatrixA = typename Explicit<Real, N, M>::MatrixJF; 
    using VectorB = typename Explicit<Real, N, M>::VectorF; 
    using Type = typename Explicit<Real, N, M>::Type; 
 
  private:
    Eigen::FullPivLU<MatrixE> m_lu; 
 
  public:
    Linear() : Explicit<Real, N, M>(Type::LINEAR, "(missing name)") {}

    Linear(std::string t_name) : Explicit<Real, N, M>(Type::LINEAR, t_name) {}

    VectorF F(VectorF const & x, VectorF const & x_dot, Real const t) const override
    {
      return this->E(t)*x_dot - this->A(t)*x - this->b(t);
    }

    MatrixJF JF_x(VectorF const & /*x*/, VectorF const & /*x_dot*/, Real const t) const override {return -this->A(t);}

    MatrixJF JF_x_dot(VectorF const & /*x*/, VectorF const & /*x_dot*/, Real const t) const override {return this->E(t);}

    VectorF f(VectorF const & x, Real const t) const override
    {
      this->m_lu.compute(this->E(t));
      SANDALS_ASSERT(this->m_lu.rank() == N, "Sandals:Linear::f(...): singular mass matrix E(t) detected.");
      return this->m_lu.solve(this->A(t)*x + this->b(t));
    }

    MatrixJF Jf_x(VectorF const & /*x*/, Real const t) const override
    {
      this->m_lu.compute(this->E(t));
      SANDALS_ASSERT(this->m_lu.rank() == N, "Sandals:Linear::Jf_x(...): singular mass matrix E(t) detected.");
      return this->m_lu.solve(this->A(t));
    }

    virtual MatrixE E(Real const t) const = 0;

    virtual MatrixA A(Real const t) const = 0;

    virtual VectorB b(Real const t) const = 0;
 
  }; // class Linear
 
  /*\
   |   _     _                     __        __
   |  | |   (_)_ __   ___  __ _ _ _\ \      / / __ __ _ _ __  _ __   ___ _ __
   |  | |   | | '_ \ / _ \/ _` | '__\ \ /\ / / '__/ _` | '_ \| '_ \ / _ \ '__|
   |  | |___| | | | |  __/ (_| | |   \ V  V /| | | (_| | |_) | |_) |  __/ |
   |  |_____|_|_| |_|\___|\__,_|_|    \_/\_/ |_|  \__,_| .__/| .__/ \___|_|
   |                                                   |_|   |_|
  \*/

  template <typename Real, Integer N, Integer M = 0>
  class LinearWrapper : public Linear<Real, N, M>
  {
  public:
    SANDALS_BASIC_CONSTANTS(Real) 
 
    using Pointer = std::shared_ptr<LinearWrapper<Real, N, M>>; 
    using VectorF = typename Linear<Real, N, M>::VectorF; 
    using MatrixJF = typename Linear<Real, N, M>::MatrixJF; 
    using MatrixE = typename Linear<Real, N, M>::MatrixJF; 
    using MatrixA = typename Linear<Real, N, M>::MatrixJF; 
    using VectorB = typename Linear<Real, N, M>::VectorF; 
    using VectorH = typename Explicit<Real, N, M>::VectorH; 
    using MatrixJH = typename Explicit<Real, N, M>::MatrixJH; 
    using FunctionE = std::function<MatrixE(Real const)>; 
    using FunctionA = std::function<MatrixA(Real const)>; 
    using FunctionB = std::function<VectorB(Real const)>; 
    using FunctionH = std::function<VectorH(VectorF const &, Real const)>; 
    using FunctionJH = std::function<MatrixJH(VectorF const &, Real const)>; 
    using FunctionID = std::function<bool(VectorF const &, Real const)>; 
 
    inline const static FunctionH DefaultH = [](VectorF const &, Real const) {return VectorH::Zero();}; 
    inline const static FunctionJH DefaultJH = [](VectorF const &, Real const) {return MatrixJH::Zero();}; 
    inline const static FunctionID DefaultID = [](VectorF const &, Real const) {return true;}; 
 
  private:
    FunctionE m_E{nullptr}; 
    FunctionA m_A{nullptr}; 
    FunctionB m_b{nullptr}; 
    FunctionH m_h{nullptr}; 
    FunctionJH m_Jh_x{nullptr}; 
    FunctionID m_in_domain{nullptr}; 
 
  public:
    LinearWrapper(FunctionE t_E, FunctionA t_A, FunctionB t_b, FunctionH t_h = DefaultH, FunctionJH
      t_Jh_x = DefaultJH, FunctionID t_in_domain = DefaultID) : Linear<Real, N, M>("(missing name)"),
      m_E(t_E), m_A(t_A), m_b(t_b), m_h(t_h), m_Jh_x(t_Jh_x), m_in_domain(t_in_domain)
    {}

    LinearWrapper(std::string t_name, FunctionE t_E, FunctionA t_A, FunctionB t_b, FunctionH t_h = DefaultH,
      FunctionJH t_Jh_x = DefaultJH, FunctionID t_in_domain = DefaultID) : Linear<Real, N, M>(t_name),
      m_E(t_E), m_A(t_A), m_b(t_b), m_h(t_h), m_Jh_x(t_Jh_x), m_in_domain(t_in_domain)
    {}

    ~LinearWrapper() {}

    FunctionE & E() {return this->m_E;}

    FunctionA & A() {return this->m_A;}

    FunctionB & b() {return this->m_b;}

    FunctionH & h() {return this->m_h;}

    FunctionJH & Jh_x() {return this->m_Jh_x;}

    FunctionID & in_domain() {return this->m_in_domain;}

    MatrixE E(Real const t) const override
    {
      return this->m_E(t);
    }

    MatrixA A(Real const t) const override
    {
      return this->m_A(t);
    }

    VectorB b(Real const t) const override
    {
      return this->m_b(t);
    }

    VectorH h(VectorF const & x, Real const t) const override
    {
      return this->m_h(x, t);
    }

    MatrixJH Jh_x(VectorF const & x, Real const t) const override
    {
      return this->m_Jh_x(x, t);
    }

    bool in_domain(VectorF const & x, Real const t) const override
    {
      return this->m_in_domain(x, t);
    }
 
  }; // class LinearWrapper
 
} // namespace Sandals
 
#endif // SANDALS_LINEAR_SYSTEM_HH