mesh.hpp

Go to the documentation of this file.

// Copyright (C) 2022 Petter Nilsson. MIT License.
 
#pragma once
 
#include <ranges>
#include <span>
#include <vector>
 
#include <Eigen/Core>
#include <Eigen/LU>
#include <smooth/detail/traits.hpp>
#include <smooth/detail/utils.hpp>
#include <smooth/polynomial/quadrature.hpp>
 
#include "smooth/feedback/traits.hpp"
#include "smooth/feedback/utils/sparse.hpp"
 
namespace smooth::feedback {
 
using smooth::utils::zip;
 
using std::views::iota, std::views::drop, std::views::reverse, std::views::take, std::views::transform,
  std::views::join;
 
namespace detail {
 
template<std::size_t K, std::size_t I = 8>
constexpr std::pair<std::array<double, K + 1>, std::array<double, K + 1>> lgr_plus_one()
{
  auto lgr_norm = ::smooth::lgr_nodes<K, I>();
 
  std::array<double, K + 1> ns, ws;
  for (auto i = 0u; i < K; ++i) {
    ns[i] = lgr_norm.first[i];
    ws[i] = lgr_norm.second[i];
  }
  ns[K] = 1;
  ws[K] = 0;
  return {ns, ws};
}
 
}  // namespace detail
 
template<std::size_t _Kmin = 5, std::size_t _Kmax = 10>
  requires(_Kmin <= _Kmax)
class Mesh
{
  using MatMap = Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>;
 
public:
  static constexpr auto Kmin = _Kmin;
  static constexpr auto Kmax = _Kmax;
 
  inline Mesh() : intervals_(1, Interval{.K = Kmin, .tau0 = 0.}) {}
 
  inline Mesh(const std::size_t n, const std::size_t k = Kmin)
  {
    assert(Kmin <= k && k <= Kmax + 1);
 
    if (n < 2) {
      intervals_.emplace_back(k, 0.);
    } else {
      const double dx = 1. / static_cast<double>(n);
      intervals_.reserve(n);
      for (std::size_t i = 0; i < n; ++i) { intervals_.emplace_back(k, static_cast<double>(i) * dx); }
    }
  }
 
  inline std::size_t N_ivals() const { return intervals_.size(); }
 
  inline std::size_t N_colloc() const
  {
    return std::accumulate(
      intervals_.begin(), intervals_.end(), 0u, [](std::size_t curr, const auto & x) { return curr + x.K; });
  }
 
  inline std::size_t N_colloc_ival(std::size_t i) const
  {
    assert(i < intervals_.size());
    return intervals_[i].K;
  }
 
  inline void refine_ph(std::size_t i, std::size_t D)
  {
    assert(i < intervals_.size());
    if (D > Kmax || intervals_[i].K > Kmax) {
      // refine by splitting interval into n intervals, each with degree Kmin_
      std::size_t n = std::max<std::size_t>(2u, (D + Kmin - 1) / Kmin);
 
      const double tau0 = intervals_[i].tau0;
      const double tauf = i + 1 < intervals_.size() ? intervals_[i + 1].tau0 : 1.;
      const double taum = (tauf - tau0) / static_cast<double>(n);
 
      while (n-- > 1) {
        intervals_.insert(
          std::next(intervals_.begin(), static_cast<intptr_t>(i + 1)),
          Interval{.K = Kmin, .tau0 = tau0 + static_cast<double>(n) * taum});
      }
    } else if (D < intervals_[i].K) {
      return;
    } else if (D <= Kmax) {
      // refine by increasing degree in interval
      intervals_[i].K = D;
    }
  }
 
  inline void refine_errors(std::ranges::sized_range auto && errs, double target_err)
  {
    const auto N = N_ivals();
 
    assert(N == std::size_t(std::ranges::size(errs)));
 
    for (const auto & [i, e] : zip(iota(0u, N) | reverse, errs | reverse)) {
 
      const auto Ki = N_colloc_ival(i);
 
      if (e > target_err) {
        const auto Ktarget = Ki + std::lround(std::log(e / target_err) / std::log(Ki) + 1);
        refine_ph(i, Ktarget);
      }
    }
  }
 
  inline void set_N_colloc_ival(std::size_t i, std::size_t K)
  {
    assert(Kmin <= K);
    assert(K <= Kmax + 1);
    intervals_[i].K = K;
  }
 
  inline auto interval_nodes(std::size_t i) const
  {
    const std::size_t k = intervals_[i].K;
 
    assert(Kmin <= k && k <= Kmax + 1);
 
    std::span<const double> sp;
 
    utils::static_for<Kmax + 2 - Kmin>([&](auto ivar) {
      static constexpr auto K = Kmin + ivar;
      if (K == k) {
        static constexpr auto nw_ext_s = detail::lgr_plus_one<K>();
 
        sp = std::span<const double>(nw_ext_s.first.data(), k + 1);
      }
    });
 
    const double tau0 = intervals_[i].tau0;
    const double tauf = i + 1 < intervals_.size() ? intervals_[i + 1].tau0 : 1.;
    const double al   = (tauf - tau0) / 2;
 
    return transform(std::move(sp), [tau0, al](double d) -> double { return tau0 + al * (d + 1); });
  }
 
  inline auto all_nodes() const
  {
    const auto n_ivals = N_ivals();
    auto all_views     = iota(0u, n_ivals) | transform([this, n_ivals = n_ivals](auto i) {
                       const auto n_ival = N_colloc_ival(i);
                       const auto n_take = static_cast<int64_t>(i + 1 < n_ivals ? n_ival : n_ival + 1);
                       return interval_nodes(i) | take(n_take);
                     });
 
    return join(std::move(all_views));
  }
 
  inline auto interval_weights(std::size_t i) const
  {
    const std::size_t k = intervals_[i].K;
 
    assert(Kmin <= k && k <= Kmax + 1);
 
    std::span<const double> sp;
 
    utils::static_for<Kmax + 2 - Kmin>([&](auto ivar) {
      static constexpr auto K = Kmin + ivar;
      if (K == k) {
        static constexpr auto nw_ext_s = detail::lgr_plus_one<K>();
 
        sp = std::span<const double>(nw_ext_s.second.data(), k + 1);
      }
    });
 
    const double tau0 = intervals_[i].tau0;
    const double tauf = i + 1 < intervals_.size() ? intervals_[i + 1].tau0 : 1.;
    const double al   = (tauf - tau0) / 2;
 
    return transform(std::move(sp), [al](double d) -> double { return al * d; });
  }
 
  inline auto all_weights() const
  {
    const auto n_ivals = N_ivals();
    auto all_views     = iota(0u, n_ivals) | transform([this, n_ivals = n_ivals](auto i) {
                       const auto n_ival    = N_colloc_ival(i);
                       const int64_t n_take = i + 1 < n_ivals ? n_ival : n_ival + 1;
                       return interval_weights(i) | take(n_take);
                     });
 
    return join(std::move(all_views));
  }
 
  inline Eigen::MatrixXd interval_diffmat(std::size_t i) const
  {
    const std::size_t k = intervals_[i].K;
 
    const double tau0 = intervals_[i].tau0;
    const double tauf = i + 1 < intervals_.size() ? intervals_[i + 1].tau0 : 1.;
 
    Eigen::MatrixXd ret(k + 1, k);
 
    utils::static_for<Kmax + 2 - Kmin>([&](auto ivar) {
      static constexpr auto K = Kmin + ivar;
      if (K == k) {
        static constexpr auto nw_ext_s = detail::lgr_plus_one<K>();
        static constexpr auto B_ext_s  = lagrange_basis<K>(nw_ext_s.first);
        static constexpr auto D_ext_s =
          polynomial_basis_derivatives<K, K + 1>(B_ext_s, nw_ext_s.first).template block<K + 1, K>(0, 0);
        ret = MatMap(D_ext_s[0].data(), k + 1, k);
      }
    });
 
    ret *= 2. / (tauf - tau0);
    return ret;
  }
 
  inline std::pair<double, MatMap> interval_diffmat_unscaled(std::size_t i) const
  {
    const std::size_t k = intervals_[i].K;
 
    const double tau0 = intervals_[i].tau0;
    const double tauf = i + 1 < intervals_.size() ? intervals_[i + 1].tau0 : 1.;
 
    MatMap ret(nullptr, 0, 0);
 
    utils::static_for<Kmax + 2 - Kmin>([&](auto ivar) {
      static constexpr auto K = Kmin + ivar;
      if (K == k) {
        static constexpr auto nw_ext_s = detail::lgr_plus_one<K>();
        static constexpr auto B_ext_s  = lagrange_basis<K>(nw_ext_s.first);
        static constexpr auto D_ext_s =
          polynomial_basis_derivatives<K, K + 1>(B_ext_s, nw_ext_s.first).template block<K + 1, K>(0, 0);
        // Eigen maps don't have copy constructors so use placement new
        new (&ret) MatMap(D_ext_s[0].data(), k + 1, k);
      }
    });
 
    return {2. / (tauf - tau0), ret};
  }
 
  inline Eigen::MatrixXd interval_intmat(std::size_t i) const
  {
    const std::size_t k = intervals_[i].K;
    return interval_diffmat(i).block(1, 0, k, k).inverse();
  }
 
  inline std::size_t interval_find(double t) const
  {
    if (t < 0) { return 0; }
    if (t > 1) { return intervals_.size() - 1; }
    auto it =
      utils::binary_interval_search(intervals_, t, [](const auto & ival, double _t) { return ival.tau0 <=> _t; });
    if (it != intervals_.end()) { return std::distance(intervals_.begin(), it); }
    return 0;
  }
 
  void increase_degrees()
  {
    for (auto & ival : intervals_) { ival.K = std::min(ival.K + 1, Kmax + 1); }
  }
 
  void decrease_degrees()
  {
    for (auto & ival : intervals_) { ival.K = std::max(ival.K - 1, Kmin); }
  }
 
  template<smooth::RnType RetT>
  RetT eval(double t, std::ranges::range auto && r, std::size_t p = 0, bool extend = true) const
  {
    const std::size_t ival = interval_find(t);
    const std::size_t k    = intervals_[ival].K;
 
    const double tau0 = intervals_[ival].tau0;
    const double tauf = ival + 1 < intervals_.size() ? intervals_[ival + 1].tau0 : 1.;
 
    const double u = 2 * (t - tau0) / (tauf - tau0) - 1;
 
    int64_t N_before = 0;
    for (auto i = 0u; i < ival; ++i) { N_before += intervals_[i].K; }
 
    // initialize output variable
    RetT ret = RetT::Zero(std::ranges::begin(r)->size());
 
    utils::static_for<Kmax + 2 - Kmin>([&](auto i) {
      static constexpr auto K = Kmin + i;
      if (K == k) {
        if (extend || ival + 1 < intervals_.size()) {
          static constexpr auto nw_ext_s = detail::lgr_plus_one<K>();
          static constexpr auto B_ext_s  = lagrange_basis<K>(nw_ext_s.first);  // K+1 x K+1
          const auto U                   = monomial_derivative<K>(u, p);       // 1 x K+1
          const auto W                   = U * B_ext_s;                        // 1 x K+1
 
          for (const auto & [w, v] : zip(std::span(W[0].data(), k + 1), r | drop(N_before))) { ret += w * v; }
        } else {
          static constexpr auto nw_s = lgr_nodes<K>();
          static constexpr auto B_s  = lagrange_basis<K - 1>(nw_s.first);  // K x K
          const auto U               = monomial_derivative<K - 1>(u, p);   // 1 x K
          const auto W               = U * B_s;                            // 1 x K
 
          for (const auto & [w, v] : zip(std::span(W[0].data(), k), r | drop(N_before))) { ret += w * v; }
        }
      }
    });
 
    return ret;
  }
 
private:
  struct Interval
  {
    std::size_t K;
    double tau0;
  };
 
  std::vector<Interval> intervals_;
};
 
template<typename T>
concept MeshType = traits::is_specialization_of_sizet_v<std::decay_t<T>, Mesh>;
 
}  // namespace smooth::feedback