Add a class for Taylor polynomials

src/analysis/TaylorPolynomial.hpp

+#ifndef TAYLOR_POLYNOMIAL_HPP
+#define TAULOR_POLYNOMIAL_HPP
+
+#include <algebra/TinyVector.hpp>
+#include <analysis/CubeGaussQuadrature.hpp>
+#include <analysis/GaussQuadratureDescriptor.hpp>
+#include <analysis/QuadratureManager.hpp>
+#include <analysis/SquareGaussQuadrature.hpp>
+#include <analysis/TriangleGaussQuadrature.hpp>
+#include <geometry/SquareTransformation.hpp>
+#include <geometry/TriangleTransformation.hpp>
+#include <utils/Exceptions.hpp>
+
+template <size_t N, size_t Dimension>
+class TaylorPolynomial
+{
+ private:
+  static constexpr size_t size_coef = [] {
+    if constexpr (Dimension == 1) {
+      return N + 1;
+    } else if constexpr (Dimension == 2) {
+      return (N + 1) * (N + 2) / 2;
+    } else {
+      static_assert(Dimension == 3);
+      return (N + 1) * (N + 2) * (N + 3) / 6;
+    }
+  }();
+
+  using Coefficients = TinyVector<size_coef, double>;
+  Coefficients m_coefficients;
+  TinyVector<Dimension> m_x0;
+  static_assert((N >= 0), "TaylorPolynomial degree must be non-negative");
+  static_assert((Dimension > 0), "TaylorPolynomial dimension must be positive");
+  static_assert((Dimension <= 3), "TaylorPolynomial dimension must no greater than three");
+
+ public:
+  PUGS_INLINE
+  constexpr size_t
+  degree() const
+  {
+    return N;
+  }
+
+  constexpr size_t
+  dim() const
+  {
+    return Dimension;
+  }
+
+  PUGS_INLINE
+  constexpr const TinyVector<size_coef, double>&
+  coefficients() const
+  {
+    return m_coefficients;
+  }
+
+  PUGS_INLINE
+  constexpr const TinyVector<Dimension, double>&
+  x0() const
+  {
+    return m_x0;
+  }
+
+  PUGS_INLINE
+  constexpr TinyVector<size_coef, double>&
+  coefficients()
+  {
+    return m_coefficients;
+  }
+
+  PUGS_INLINE constexpr bool
+  operator==(const TaylorPolynomial& q) const
+  {
+    return (m_coefficients == q.m_coefficients && m_x0 = q.m_x0);
+  }
+
+  PUGS_INLINE constexpr TaylorPolynomial(const TinyVector<size_coef, double>& coefficients,
+                                         const TinyVector<Dimension, double>& x0) noexcept
+    : m_coefficients{coefficients}, m_x0(x0)
+  {}
+
+  PUGS_INLINE
+  constexpr TaylorPolynomial(TinyVector<size_coef, double>&& coefficients,
+                             const TinyVector<Dimension, double>&& x0) noexcept
+    : m_coefficients{coefficients}, m_x0(x0)
+  {}
+
+  PUGS_INLINE
+  constexpr bool
+  operator!=(const TaylorPolynomial& q) const
+  {
+    return not this->operator==(q);
+  }
+
+  PUGS_INLINE constexpr TaylorPolynomial
+  operator+(const TaylorPolynomial Q) const
+  {
+    Assert(m_x0 == Q.m_x0, "You cannot add Taylor polynomials with different origins");
+    TaylorPolynomial<N, Dimension> P(m_coefficients);
+    for (size_t i = 0; i < size_coef; ++i) {
+      P.coefficients()[i] += Q.coefficients()[i];
+    }
+    return P;
+  }
+
+  PUGS_INLINE constexpr TaylorPolynomial
+  operator-() const
+  {
+    TaylorPolynomial<N, Dimension> P;
+    P.coefficients() = -coefficients();
+    return P;
+  }
+
+  PUGS_INLINE constexpr TaylorPolynomial
+  operator-(const TaylorPolynomial Q) const
+  {
+    Assert(m_x0 == Q.m_x0, "You cannot subtract Taylor polynomials with different origins");
+    TaylorPolynomial<N, Dimension> P(m_coefficients);
+    P = P + (-Q);
+    return P;
+  }
+
+  template <size_t M, size_t Dim>
+  PUGS_INLINE constexpr TaylorPolynomial&
+  operator=(const TaylorPolynomial<M, Dim>& Q)
+  {
+    coefficients() = zero;
+    for (size_t i = 0; i < size_coef; ++i) {
+      coefficients()[i] = Q.coefficients()[i];
+    }
+    m_x0 = Q.m_x0;
+    return *this;
+  }
+
+  PUGS_INLINE constexpr TaylorPolynomial&
+  operator+=(const TaylorPolynomial& Q)
+  {
+    Assert(m_x0 == Q.m_x0, "You cannot add Taylor polynomials with different origins");
+    m_coefficients += Q.coefficients();
+    return *this;
+  }
+
+  template <size_t M>
+  PUGS_INLINE constexpr TaylorPolynomial&
+  operator-=(const TaylorPolynomial& Q)
+  {
+    Assert(m_x0 == Q.m_x0, "You cannot subtract Taylor polynomials with different origins");
+    m_coefficients -= Q.coefficients();
+    return *this;
+  }
+
+  PUGS_INLINE
+  constexpr TaylorPolynomial
+  operator*(const double& lambda) const
+  {
+    TinyVector<size_coef> mult_coefs = lambda * m_coefficients;
+    TaylorPolynomial<N, Dimension> M(mult_coefs);
+    return M;
+  }
+
+  PUGS_INLINE
+  constexpr friend TaylorPolynomial<N, Dimension>
+  operator*(const double& lambda, const TaylorPolynomial<N, Dimension> P)
+  {
+    return P * lambda;
+  }
+
+  PUGS_INLINE
+  constexpr double
+  operator[](const TinyVector<Dimension, size_t> relative_pos) const
+  {
+    size_t total_degree = 0;
+    for (size_t i = 0; i < Dimension; ++i) {
+      Assert((relative_pos[i] <= N),
+             "You are looking for a coefficient of order greater than the degree of the polynomial");
+      total_degree += relative_pos[i];
+    }
+    Assert((total_degree <= N), "The sum of the degrees of the coefficient you are looking for is greater than the "
+                                "degree of the polynomial");
+    TinyVector<size_coef> absolute_coefs = this->coefficients();
+    size_t absolute_position             = 0;
+    if constexpr (Dimension == 1) {
+      absolute_position = relative_pos[0];
+    } else if constexpr (Dimension == 2) {
+      size_t total_degree = relative_pos[0] + relative_pos[1];
+      absolute_position   = total_degree * (total_degree + 1) / 2 + relative_pos[1];
+    } else {
+      // throw NotImplementedError("Not yet Available in 3D");
+      static_assert(Dimension == 3);
+      size_t total_degree     = relative_pos[0] + relative_pos[1] + relative_pos[2];
+      size_t total_sub_degree = relative_pos[1] + relative_pos[2];
+      return total_degree * (total_degree + 1) * (total_degree + 2) / 6 +
+             total_sub_degree * (total_sub_degree + 1) / 2 + relative_pos[2];
+    }
+
+    return absolute_coefs[absolute_position];
+  }
+
+  PUGS_INLINE
+  constexpr double
+  operator[](const TinyVector<Dimension, size_t> relative_pos)
+  {
+    size_t total_degree = 0;
+    for (size_t i = 0; i < Dimension; ++i) {
+      Assert((relative_pos[i] <= N),
+             "You are looking for a coefficient of order greater than the degree of the polynomial");
+      total_degree += relative_pos[i];
+    }
+    Assert((total_degree <= N), "The sum of the degrees of the coefficient you are looking for is greater than the "
+                                "degree of the polynomial");
+    TinyVector<size_coef> absolute_coefs = this->coefficients();
+    size_t absolute_position             = 0;
+    if constexpr (Dimension == 1) {
+      absolute_position = relative_pos[0];
+    } else if constexpr (Dimension == 2) {
+      size_t total_degree = relative_pos[0] + relative_pos[1];
+      absolute_position   = total_degree * (total_degree + 1) / 2 + relative_pos[1];
+    } else {
+      // throw NotImplementedError("Not yet Available in 3D");
+      static_assert(Dimension == 3);
+      size_t total_degree     = relative_pos[0] + relative_pos[1] + relative_pos[2];
+      size_t total_sub_degree = relative_pos[1] + relative_pos[2];
+      absolute_position       = total_degree * (total_degree + 1) * (total_degree + 2) / 6 +
+                          total_sub_degree * (total_sub_degree + 1) / 2 + relative_pos[2];
+    }
+
+    return absolute_coefs[absolute_position];
+  }
+
+  PUGS_INLINE
+  constexpr double
+  absolute_position(const TinyVector<Dimension, size_t> relative_pos) const
+  {
+    size_t total_degree = 0;
+    for (size_t i = 0; i < Dimension; ++i) {
+      Assert((relative_pos[i] <= N),
+             "You are looking for a coefficient of order greater than the degree of the polynomial");
+      total_degree += relative_pos[i];
+    }
+    Assert((total_degree <= N), "The sum of the degrees of the coefficient you are looking for is greater than the "
+                                "degree of the polynomial");
+    size_t abs_pos = 0;
+    if constexpr (Dimension == 1) {
+      abs_pos = relative_pos[0];
+    } else if constexpr (Dimension == 2) {
+      abs_pos = total_degree * (total_degree + 1) / 2 + relative_pos[1];
+    } else {
+      static_assert(Dimension == 3);
+      size_t total_degree     = relative_pos[0] + relative_pos[1] + relative_pos[2];
+      size_t total_sub_degree = relative_pos[1] + relative_pos[2];
+      abs_pos                 = total_degree * (total_degree + 1) * (total_degree + 2) / 6 +
+                total_sub_degree * (total_sub_degree + 1) / 2 + relative_pos[2];
+      // throw NotImplementedError("Not yet Available in 3D");
+    }
+
+    return abs_pos;
+  }
+
+  PUGS_INLINE
+  constexpr double
+  operator()(const TinyVector<Dimension> x) const
+  {
+    const auto& P = *this;
+    double value  = 0.;
+    if constexpr (Dimension == 1) {
+      value = m_coefficients[N];
+      for (size_t i = N; i > 0; --i) {
+        value *= x - m_x0;
+        value += m_coefficients[i - 1];
+      }
+    } else if constexpr (Dimension == 2) {
+      TinyVector<Dimension, size_t> relative_pos(0, N);
+      value = P[relative_pos];
+      for (size_t i = N; i > 0; --i) {
+        value *= (x[1] - m_x0[1]);
+        relative_pos  = TinyVector<Dimension, size_t>(N - i + 1, i - 1);
+        double valuex = P[relative_pos];
+        for (size_t j = N - i + 1; j > 0; --j) {
+          valuex *= (x[0] - m_x0[0]);
+          relative_pos = TinyVector<Dimension, size_t>(j - 1, i - 1);
+          valuex += P[relative_pos];
+        }
+        value += valuex;
+      }
+    } else {
+      throw NotImplementedError("Not yet Available in 3D");
+    }
+
+    return value;
+  }
+
+  PUGS_INLINE size_t
+  find_size_coef(const size_t degree)
+  {
+    if constexpr (Dimension == 1) {
+      return degree + 1;
+    } else if constexpr (Dimension == 2) {
+      return (degree + 1) * (degree + 2) / 2;
+    } else {
+      static_assert(Dimension == 3);
+      return (degree + 1) * (degree + 2) * (degree + 3) / 6;
+    }
+  }
+
+  PUGS_INLINE constexpr TaylorPolynomial<N, Dimension>
+  derivative(const size_t var) const
+  {
+    const auto P = *this;
+    TinyVector<size_coef> coefs(zero);
+    TaylorPolynomial<N, Dimension> Q(coefs);
+    if constexpr (N != 0) {
+      Assert(var < Dimension,
+             "You can not derive a polynomial with respect to a variable of rank greater than the dimension");
+      if constexpr (Dimension == 1) {
+        for (size_t i = 0; i < size_coef - 1; ++i) {
+          coefs[i] = double(i + 1) * P.coefficients()[i + 1];
+        }
+      } else if constexpr (Dimension == 2) {
+        if (var == 0) {
+          for (size_t i = 0; i < N; ++i) {
+            for (size_t j = 0; j < N - i; ++j) {
+              TinyVector<Dimension, size_t> relative_pos(i, j);
+              TinyVector<Dimension, size_t> relative_posp(i + 1, j);
+              size_t absolute_position            = Q.absolute_position(relative_pos);
+              size_t absolute_positionp           = P.absolute_position(relative_posp);
+              Q.coefficients()[absolute_position] = double(i + 1) * m_coefficients[absolute_positionp];
+            }
+          }
+        } else {
+          for (size_t i = 0; i < N; ++i) {
+            for (size_t j = 0; j < N - i; ++j) {
+              TinyVector<Dimension, size_t> relative_pos(i, j);
+              TinyVector<Dimension, size_t> relative_posp(i, j + 1);
+              size_t absolute_position            = Q.absolute_position(relative_pos);
+              size_t absolute_positionp           = P.absolute_position(relative_posp);
+              Q.coefficients()[absolute_position] = double(j + 1) * m_coefficients[absolute_positionp];
+            }
+          }
+        }
+      } else {
+        static_assert(Dimension == 3);
+        if (var == 0) {
+          for (size_t i = 0; i < N; ++i) {
+            for (size_t j = 0; j < N - i; ++j) {
+              for (size_t k = 0; k < N - i - j; ++k) {
+                TinyVector<Dimension, size_t> relative_pos(i, j, k);
+                TinyVector<Dimension, size_t> relative_posp(i + 1, j, k);
+                size_t absolute_position            = Q.absolute_position(relative_pos);
+                size_t absolute_positionp           = P.absolute_position(relative_posp);
+                Q.coefficients()[absolute_position] = double(i + 1) * m_coefficients[absolute_positionp];
+              }
+            }
+          }
+        } else if (var == 1) {
+          for (size_t i = 0; i < N; ++i) {
+            for (size_t j = 0; j < N - i; ++j) {
+              for (size_t k = 0; k < N - i - j; ++k) {
+                TinyVector<Dimension, size_t> relative_pos(i, j, k);
+                TinyVector<Dimension, size_t> relative_posp(i, j + 1, k);
+                size_t absolute_position            = Q.absolute_position(relative_pos);
+                size_t absolute_positionp           = P.absolute_position(relative_posp);
+                Q.coefficients()[absolute_position] = double(j + 1) * m_coefficients[absolute_positionp];
+              }
+            }
+          }
+        } else {
+          for (size_t i = 0; i < N; ++i) {
+            for (size_t j = 0; j < N - i; ++j) {
+              for (size_t k = 0; k < N - i - j; ++k) {
+                TinyVector<Dimension, size_t> relative_pos(i, j, k);
+                TinyVector<Dimension, size_t> relative_posp(i, j, k + 1);
+                size_t absolute_position            = Q.absolute_position(relative_pos);
+                size_t absolute_positionp           = P.absolute_position(relative_posp);
+                Q.coefficients()[absolute_position] = double(k + 1) * m_coefficients[absolute_positionp];
+              }
+            }
+          }
+        }
+        // throw NotImplementedError("Not yet Available in 3D");
+      }
+    }
+    return Q;
+  }
+
+  PUGS_INLINE
+  constexpr friend std::ostream&
+  operator<<(std::ostream& os, const TaylorPolynomial<N, Dimension>& P)
+  {
+    //    os << "P(x) = ";
+    bool all_coef_zero = true;
+    if (N == 0) {
+      os << P.coefficients()[0];
+      return os;
+    }
+    if constexpr (Dimension == 1) {
+      if (N != 1) {
+        if (P.coefficients()[N] != 0.) {
+          if (P.coefficients()[N] < 0.) {
+            os << "- ";
+          }
+          if (P.coefficients()[N] != 1 && P.coefficients()[N] != -1) {
+            os << std::abs(P.coefficients()[N]);
+          }
+          os << "(x - " << P.x0()[0] << ")"
+             << "^" << N;
+          all_coef_zero = false;
+        }
+      }
+      for (size_t i = N - 1; i > 1; --i) {
+        if (P.coefficients()[i] != 0.) {
+          if (P.coefficients()[i] > 0.) {
+            os << " + ";
+          } else if (P.coefficients()[i] < 0.) {
+            os << " - ";
+          }
+          if (P.coefficients()[i] != 1 && P.coefficients()[i] != -1) {
+            os << std::abs(P.coefficients()[i]);
+          }
+          os << "(x - " << P.x0()[0] << ")"
+             << "^" << i;
+          all_coef_zero = false;
+        }
+      }
+      if (P.coefficients()[1] != 0.) {
+        if (P.coefficients()[1] > 0. && N != 1) {
+          os << " + ";
+        } else if (P.coefficients()[1] < 0.) {
+          os << " - ";
+        }
+        if (P.coefficients()[1] != 1 && P.coefficients()[1] != -1) {
+          os << std::abs(P.coefficients()[1]);
+        }
+        os << "(x - " << P.x0()[0] << ")";
+        all_coef_zero = false;
+      }
+      if (P.coefficients()[0] != 0. || all_coef_zero) {
+        if (P.coefficients()[0] > 0.) {
+          os << " + ";
+        } else if (P.coefficients()[0] < 0.) {
+          os << " - ";
+        }
+        os << std::abs(P.coefficients()[0]);
+      }
+      return os;
+    } else if constexpr (Dimension == 2) {
+      size_t i = 0;
+      size_t j = N;
+      TinyVector<Dimension, size_t> rel_pos(i, j);
+      double coef = P[rel_pos];
+      if (coef != 0.) {
+        if (coef < 0.) {
+          os << " - ";
+        }
+        if (coef != 1 && coef != -1) {
+          os << std::abs(coef);
+        }
+        os << "(y - " << P.x0()[1] << ")"
+           << "^" << N;
+      }
+      size_t degree = N;
+      for (size_t k = size_coef - 1; k > 0; --k) {
+        if (j > 0) {
+          j--;
+          i++;
+        } else {
+          degree--;
+          j = degree;
+          i = 0;
+        }
+        rel_pos     = TinyVector<Dimension, size_t>(i, j);
+        double coef = P[rel_pos];
+        if (coef != 0.) {
+          if (coef > 0.) {
+            os << " + ";
+          } else if (coef < 0.) {
+            os << " - ";
+          }
+          if ((coef != 1 && coef != -1) || (i == 0 && j == 0)) {
+            os << std::abs(coef);
+          }
+          if (i == 0 && j == 0)
+            continue;
+          if (i == 0) {
+            if (j != 1) {
+              os << "(y - " << P.x0()[1] << ")"
+                 << "^" << j;
+            } else {
+              os << "y";
+            }
+          } else if (j == 0) {
+            if (i == 1) {
+              os << "(x - " << P.x0()[0] << ")";
+            } else {
+              os << "(x - " << P.x0()[0] << ")"
+                 << "^" << i;
+            }
+          } else {
+            if (i == 1 && j == 1) {
+              os << "(x - " << P.x0()[0] << ")"
+                 << "(y - " << P.x0()[1] << ")";
+            } else if (i == 1) {
+              os << "(x - " << P.x0()[0] << ")"
+                 << "(y - " << P.x0()[1] << ")^" << j;
+            } else if (j == 1) {
+              os << "(x - " << P.x0()[0] << ")"
+                 << "^i"
+                 << "(y - " << P.x0()[1] << ")";
+            } else {
+              os << "(x - " << P.x0()[0] << ")"
+                 << "^i"
+                 << "(y - " << P.x0()[1] << ")^" << j;
+            }
+          }
+          all_coef_zero = false;
+        }
+      }
+
+      return os;
+    } else {
+      // size_t i = 0;
+      // size_t j = 0;
+      // size_t k = N;
+      // TinyVector<Dimension, size_t> rel_pos(i, j, k);
+      // double coef = P[rel_pos];
+      // if (coef != 0.) {
+      //   if (coef < 0.) {
+      //     os << " - ";
+      //   }
+      //   if (coef != 1 && coef != -1) {
+      //     os << std::abs(coef);
+      //   }
+      //   os << "z^" << N;
+      // }
+      // size_t degree = N;
+      // for (size_t l = size_coef - 1; l > 0; --l) {
+      //   if (k > 0) {
+      //     k--;
+      //     if (j < k) {
+      //       j++;
+      //     } else {
+      //       j--;
+      //       i++;
+      //     }
+      //   } else {
+      //     degree--;
+      //     k = degree;
+      //     i = 0;
+      //     j = 0;
+      //   }
+
+      //   rel_pos     = TinyVector<Dimension, size_t>(i, j, k);
+      //   double coef = P[rel_pos];
+      //   if (coef != 0.) {
+      //     if (coef > 0.) {
+      //       os << " + ";
+      //     } else if (coef < 0.) {
+      //       os << " - ";
+      //     }
+      //     if ((coef != 1 && coef != -1) || (i == 0 && j == 0 && k == 0)) {
+      //       os << std::abs(coef);
+      //     }
+      //     if (i == 0 && j == 0 && k == 0)
+      //       continue;
+      //     if (i == 0 && j == 0) {
+      //       if (k != 1) {
+      //         os << "z^" << j;
+      //       } else {
+      //         os << "z";
+      //       }
+      //     } else if (i == 0 && k == 0) {
+      //       if (j == 1) {
+      //         os << "y";
+      //       } else {
+      //         os << "y^" << i;
+      //       }
+      //     } else if (j == 0 && k == 0) {
+      //       if (i == 1) {
+      //         os << "x";
+      //       } else {
+      //         os << "x^" << i;
+      //       }
+      //     } else {
+      //       if (i == 1 && j == 1 && k == 1) {
+      //         os << "xyz";
+      //       } else if (i == 1) {
+      //         os << "x"
+      //            << "y^" << j << "z^" << k;
+      //       } else if (j == 1) {
+      //         os << "x^" << i << "y"
+      //            << "z^" << k;
+      //       } else if (k == 1) {
+      //         os << "x^" << i << "y^" << j << "z";
+      //       } else {
+      //         os << "x^" << i << "y^" << j << "z^" << k;
+      //       }
+      //     }
+      //     all_coef_zero = false;
+      //   }
+      //
+      for (size_t l = 0; l < size_coef; ++l) {
+        double coef = P.coefficients()[l];
+        os << coef << " ";
+      }
+      return os;
+      //      throw NotImplementedError("Not yet Available in 3D");
+    }
+  }
+
+  PUGS_INLINE constexpr TaylorPolynomial() noexcept = default;
+  ~TaylorPolynomial()                               = default;
+};
+
+template <size_t N, size_t Dimension, size_t P>
+PUGS_INLINE double
+integrate(const TaylorPolynomial<N, Dimension> Q, const std::array<TinyVector<Dimension>, P>& positions)
+{
+  double integral = 0.;
+  static_assert(P > 1, "For the integration, number of positions should be greater or equal to 2");
+  static_assert(N >= 0, "invalid degree");
+  if constexpr (Dimension == 1) {
+    static_assert(P == 2, "In 1D number of positions should be 2");
+    throw NotImplementedError("Not yet Available in 1D");
+  } else if constexpr (Dimension == 2) {
+    static_assert(P <= 4, "In 2D number of positions should be lesser or equal to 4");
+    if constexpr (P == 2) {
+    } else if constexpr (P == 3) {
+      const QuadratureFormula<2>& lN = QuadratureManager::instance().getTriangleFormula(GaussQuadratureDescriptor(N));
+      auto point_list                = lN.pointList();
+      auto weight_list               = lN.weightList();
+      TriangleTransformation<2> t{positions[0], positions[1], positions[2]};
+      auto value = weight_list[0] * Q(t(point_list[0]));
+      for (size_t i = 1; i < weight_list.size(); ++i) {
+        value += weight_list[i] * Q(t(point_list[i]));
+      }
+      integral = value;
+    } else {
+      const QuadratureFormula<2>& lN = QuadratureManager::instance().getSquareFormula(GaussQuadratureDescriptor(N));
+      auto point_list                = lN.pointList();
+      auto weight_list               = lN.weightList();
+      SquareTransformation<2> s{positions[0], positions[1], positions[2], positions[3]};
+      auto value = weight_list[0] * Q(s(point_list[0]));
+      for (size_t i = 1; i < weight_list.size(); ++i) {
+        value += weight_list[i] * Q(s(point_list[i]));
+      }
+      integral = value;
+    }
+  } else {
+    static_assert(Dimension == 3, "Dimension should be <=3");
+    throw NotImplementedError("Not yet Available in 3D");
+  }
+  return integral;
+}
+
+#endif   // POLYNOMIALP_HPP