Lie.hpp

Go to the documentation of this file.

/**
 * @file Lie.hpp
 * @brief Lie algebra implementation
 * @version 0.1
 * @date 2024-12-22
 *
 * @copyright Copyright (c) 2024
 *
 */
 
#pragma once
 
namespace LieGroups {
 
template <class T> struct TensorTypeExtractorBase {
  using Type =
      std::remove_cv_t<std::remove_reference_t<std::remove_pointer_t<T>>>;
};
 
template <class T, int I>
struct TensorTypeExtractorBase<FTensor::PackPtr<T, I>> {
  using Type =
      std::remove_cv_t<std::remove_reference_t<std::remove_pointer_t<T>>>;
};
 
template <class T, int G>
struct TensorTypeExtractorBase<FTensor::CursorPtr<T, G>> {
  using Type =
      std::remove_cv_t<std::remove_reference_t<std::remove_pointer_t<T>>>;
};
 
template <class T>
struct TensorTypeExtractor
    : TensorTypeExtractorBase<std::remove_cv_t<std::remove_reference_t<T>>> {};
 
/**
 * @brief SO3
 * 
 * Note that: theta == norm(t_w_vee), and t_w_hat is the skew-symmetric matrix corresponding to t_w_vee.
 */
struct SO3 {
 
  SO3() = delete;
  ~SO3() = delete;
 
  template <typename T> inline static auto getVee(T &&w1, T &&w2, T &&w3) {
    return FTensor::Tensor1<T, dim>(
 
        std::forward<T>(w1), std::forward<T>(w2), std::forward<T>(w3)
 
    );
  }
 
  template <typename T> inline static auto getHat(T &&w1, T &&w2, T &&w3) {
    auto t_w_vee =
        getVee(std::forward<T>(w1), std::forward<T>(w2), std::forward<T>(w3));
    return getHatImpl(t_w_vee);
  }
 
  template <typename A> inline static auto getVee(A &&t_w_hat) {
    return getVeeImpl(std::forward<A>(t_w_hat));
  }
 
  template <typename A> inline static auto getHat(A &&t_w_vee) {
    return getHatImpl(std::forward<A>(t_w_vee));
  }
 
  template <typename A, typename B>
  inline static auto exp(A &&t_w_vee, B &&theta) {
    return expImpl(std::forward<A>(t_w_vee), std::forward<B>(theta));
  }
 
  template <typename A, typename B>
  inline static auto Jl(A &&t_w_vee, B &&theta) {
    return JlImpl(std::forward<A>(t_w_vee), std::forward<B>(theta));
  }
 
  template <typename A, typename B>
  inline static auto Jr(A &&t_w_vee, B &&theta) {
    return JrImpl(std::forward<A>(t_w_vee), std::forward<B>(theta));
  }
 
  template <typename A> inline static auto right_jacobian(A &&t_w_vee) {
    const auto theta = t_w_vee.l2();
    return Jr(std::forward<A>(t_w_vee), theta);
  }
 
  template <typename A, typename B, typename C>
  inline static auto action(A &&t_w_vee, B &&theta, C &&t_A) {
    return actionImpl(std::forward<A>(t_w_vee), std::forward<B>(theta),
                      std::forward<C>(t_A));
  }
 
  template <typename A, typename B>
  inline static auto diffJl(A &&t_w_vee, B &&theta) {
    return diffJlImpl(std::forward<A>(t_w_vee), std::forward<B>(theta));
  }
 
  template <typename A, typename B>
  inline static auto diffJr(A &&t_w_vee, B &&theta) {
    return diffJrImpl(std::forward<A>(t_w_vee), std::forward<B>(theta));
  }
 
  template <typename A, typename B>
  inline static auto diffExp(A &&t_w_vee, B &&theta) {
    return diffExpImpl(std::forward<A>(t_w_vee), std::forward<B>(theta));
  }
 
  template <typename A, typename B>
  inline static auto diffDiffExp(A &&t_w_vee, B &&theta) {
    return diffDiffExpImpl(std::forward<A>(t_w_vee), std::forward<B>(theta));
  }
 
  // w = Jl(omega) * delta_omega  (spatial infinitesimal rotation vector)
  template <typename T1, typename T2, typename T3>
  inline static auto spatialSpin(T1 &&t_w_vee, T2 theta, T3 &&t_delta_w_vee) {
    return spatialSpinImpl(std::forward<T1>(t_w_vee), theta,
                            std::forward<T3>(t_delta_w_vee));
  }
 
  // W = hat( Jl(omega) * delta_omega ) = deltaR * R^T
  template <typename T1, typename T2, typename T3>
  inline static auto spatialSpinHat(T1 &&t_w_vee, T2 theta,
                                     T3 &&t_delta_w_vee) {
    return spatialSpinHatImpl(std::forward<T1>(t_w_vee), theta,
                              std::forward<T3>(t_delta_w_vee));
  }
 
  // a = Jr(omega) * delta_omega  (material/body infinitesimal rotation vector)
  template <typename T1, typename T2, typename T3>
  inline static auto materialSpin(T1 &&t_w_vee, T2 theta, T3 &&t_delta_w_vee) {
    return materialSpinImpl(std::forward<T1>(t_w_vee), theta,
                            std::forward<T3>(t_delta_w_vee));
  }
 
  // A = hat( Jr(omega) * delta_omega ) = R^T * deltaR
  template <typename T1, typename T2, typename T3>
  inline static auto materialSpinHat(T1 &&t_w_vee, T2 theta,
                                     T3 &&t_delta_w_vee) {
    return materialSpinHatImpl(std::forward<T1>(t_w_vee), theta,
                               std::forward<T3>(t_delta_w_vee));
  }
 
  // Intrinsic continuum variation: deltaR = R * A, with A in so(3)
  template <typename T1, typename T2, typename T3>
  inline static auto deltaR(T1 &&t_w_vee, T2 theta, T3 &&t_A_hat) {
    return deltaRImpl(std::forward<T1>(t_w_vee), theta,
                      std::forward<T3>(t_A_hat));
  }
 
 
private:
  inline static constexpr int dim = 3;
  inline static FTENSOR_INDEX(dim, i);
  inline static FTENSOR_INDEX(dim, j);
  inline static FTENSOR_INDEX(dim, k);
  inline static FTENSOR_INDEX(dim, l);
  inline static FTENSOR_INDEX(dim, m);
  inline static FTENSOR_INDEX(dim, n);
 
  template <typename T>
  inline static auto getVeeImpl(const FTensor::Tensor2<T, dim, dim> &t_w_hat) {
    using D = typename TensorTypeExtractor<T>::Type;
    FTensor::Tensor1<D, dim> t_w_vee;
    t_w_vee(k) = (levi_civita(i, j, k) * t_w_hat(i, j)) / 2;
    return t_w_vee;
  }
 
  template <typename T>
  inline static auto getHatImpl(const FTensor::Tensor1<T, dim> &t_w_vee) {
    using D = typename TensorTypeExtractor<T>::Type;
    FTensor::Tensor2<D, dim, dim> t_w_hat;
    t_w_hat(i, j) = levi_civita(i, j, k) * t_w_vee(k);
    return t_w_hat;
  }
 
  template <typename T1, typename T2, typename T3>
  inline static auto genericFormImpl(const FTensor::Tensor1<T1, dim> &t_w_vee,
                                     const T2 alpha, const T3 beta) {
    using D = typename TensorTypeExtractor<T1>::Type;
    FTensor::Tensor2<D, dim, dim> t_X;
    auto t_hat = getHat(t_w_vee);
    t_X(i, j) = FTensor::Kronecker_Delta<int>()(i, j) + alpha * t_hat(i, j) +
                beta * (t_hat(i, k) * t_hat(k, j));
    return t_X;
  }
 
  template <typename T1, typename T2>
  inline static auto expImpl(const FTensor::Tensor1<T1, dim> &t_w_vee,
                             const T2 theta) {
    if (fabs(theta) < std::numeric_limits<T2>::epsilon()) {
      return genericFormImpl(t_w_vee, 1, 0.5);
    }
    const auto s = sin(theta);
    const auto s_half = sin(theta / 2);
    const auto a = s / theta;
    const auto b = 2 * (s_half / theta) * (s_half / theta);
    return genericFormImpl(t_w_vee, a, b);
  }
 
  template <typename T1, typename T2>
  inline static auto diffExpImpl(const FTensor::Tensor1<T1, dim> &t_w_vee,
                                 const T2 theta) {
 
    auto get_tensor = [&t_w_vee](auto a, auto diff_a, auto b, auto diff_b) {
      FTENSOR_INDEX(3, i);
      FTENSOR_INDEX(3, j);
      FTENSOR_INDEX(3, k);
      FTENSOR_INDEX(3, l);
 
      using D = typename TensorTypeExtractor<T1>::Type;
      FTensor::Tensor3<D, 3, 3, 3> t_diff_exp;
      auto t_hat = getHat(t_w_vee);
      t_diff_exp(i, j, k) =
 
          a * FTensor::levi_civita<int>(i, j, k)
 
          +
 
          diff_a * t_hat(i, j) * t_w_vee(k)
 
          +
 
          b * (t_hat(i, l) * FTensor::levi_civita<int>(l, j, k) +
               FTensor::levi_civita<int>(i, l, k) * t_hat(l, j))
 
          +
 
          diff_b * t_hat(i, l) * t_hat(l, j) * t_w_vee(k);
 
      return t_diff_exp;
    };
 
    if (fabs(theta) < std::numeric_limits<T2>::epsilon()) {
      return get_tensor(1., -1. / 3., 1. / 2., -1. / 12);
    }
 
    const auto ss = sin(theta);
    const auto a = ss / theta;
 
    const auto theta2 = theta * theta;
    const auto cc = cos(theta);
    const auto diff_a = (theta * cc - ss) / (theta2 * theta);
 
    const auto ss_2 = sin(theta / 2.);
    // const auto cc_2 = cos(theta / 2.);
    const auto b = 2. * ss_2 * ss_2 / theta2;
    const auto diff_b = (-2 + 2 * cc + theta * ss) / (theta2 * theta2);
 
    return get_tensor(a, diff_a, b, diff_b);
  }
 
  template <typename T1, typename T2>
  inline static auto diffDiffExpImpl(const FTensor::Tensor1<T1, dim> &t_w_vee,
                                     const T2 theta) {
 
    auto get_tensor = [&t_w_vee](auto a, auto diff_a, auto diff_diff_a, auto b,
                                 auto diff_b, auto diff_diff_b) {
      FTENSOR_INDEX(3, i);
      FTENSOR_INDEX(3, j);
      FTENSOR_INDEX(3, k);
      FTENSOR_INDEX(3, l);
      FTENSOR_INDEX(3, m);
 
      constexpr auto t_kd = FTensor::Kronecker_Delta<double>();
 
      using D = typename TensorTypeExtractor<T1>::Type;
      FTensor::Tensor4<D, 3, 3, 3, 3> t_diff_diff_exp;
 
      auto t_hat = getHat(t_w_vee);
      t_diff_diff_exp(i, j, k, m) =
 
          diff_a * FTensor::levi_civita<int>(i, j, k) * t_w_vee(m)
 
          +
 
          diff_a * (
 
                       t_hat(i, j) * t_kd(k, m) +
                       FTensor::levi_civita<int>(i, j, m) * t_w_vee(k)
 
                           )
 
          +
 
          diff_diff_a * t_hat(i, j) * t_w_vee(k) * t_w_vee(m)
 
          +
 
          b * (FTensor::levi_civita<int>(i, l, m) *
                   FTensor::levi_civita<int>(l, j, k) +
               FTensor::levi_civita<int>(i, l, k) *
                   FTensor::levi_civita<int>(l, j, m))
 
          +
 
          diff_b * ((t_hat(i, l) * FTensor::levi_civita<int>(l, j, k) +
                     FTensor::levi_civita<int>(i, l, k) * t_hat(l, j)) *
                    t_w_vee(m))
 
          +
 
          diff_b *
              (
 
                  t_hat(i, l) * t_hat(l, j) * t_kd(k, m)
 
                  +
 
                  FTensor::levi_civita<int>(i, l, m) * t_hat(l, j) * t_w_vee(k)
 
                  +
 
                  t_hat(i, l) * FTensor::levi_civita<int>(l, j, m) * t_w_vee(k)
 
                      )
 
          +
 
          diff_diff_b * t_hat(i, l) * t_hat(l, j) * t_w_vee(k) * t_w_vee(m);
 
      return t_diff_diff_exp;
    };
 
    if (fabs(theta) < std::numeric_limits<T2>::epsilon()) {
      return get_tensor(1., -1. / 3., 1. / 15, 1. / 2, -1. / 12, 1. / 90);
    }
 
    const auto ss = sin(theta);
    const auto a = ss / theta;
 
    const auto theta2 = theta * theta;
    const auto cc = cos(theta);
    const auto diff_a = (theta * cc - ss) / (theta2 * theta);
    const auto diff_diff_a =
        (3. * ss - 3. * theta * cc - theta2 * ss) / (theta2 * theta2 * theta);
 
    const auto ss_2 = sin(theta / 2.);
    const auto b = 2. * ss_2 * ss_2 / theta2;
    const auto diff_b = (-2 + 2 * cc + theta * ss) / (theta2 * theta2);
    const auto diff_diff_b = (theta2 * cc - 5. * theta * ss - 8. * cc + 8.) /
                             (theta2 * theta2 * theta2);
 
    return get_tensor(a, diff_a, diff_diff_a, b, diff_b, diff_diff_b);
  }
 
  template <typename T1, typename T2>
  inline static auto JlImpl(const FTensor::Tensor1<T1, dim> &t_w_vee,
                            const T2 &theta) {
    if (fabs(theta) < std::numeric_limits<T2>::epsilon()) {
      return genericFormImpl(t_w_vee, 0.5, 1. / 6.);
    }
    const auto s = sin(theta);
    const auto s_half = sin(theta / 2);
    const auto a = 2 * (s_half / theta) * (s_half / theta);
    const auto b = ((theta - s) / theta) / theta / theta;
    return genericFormImpl(t_w_vee, a, b);
  }
 
  template <typename T1, typename T2>
  inline static auto JrImpl(const FTensor::Tensor1<T1, dim> &t_w_vee,
                            const T2 theta) {
    if (fabs(theta) < std::numeric_limits<T2>::epsilon()) {
      return genericFormImpl(t_w_vee, -0.5, 1. / 6.);
    }
    const auto s = sin(theta);
    const auto s_half = sin(theta / 2);
    const auto a = 2 * (s_half / theta) * (s_half / theta);
    const auto b = ((theta - s) / theta) / theta / theta;
    return genericFormImpl(t_w_vee, -a, b);
  }
 
  // private helper
  template <typename T1, typename T2, typename T3, typename T4, typename T5>
  inline static auto
  genericDiffFormImpl(const FTensor::Tensor1<T1, dim> &t_w_vee, const T2 alpha,
                      const T3 diff_alpha, const T4 beta, const T5 diff_beta) {
    FTENSOR_INDEX(3, i);
    FTENSOR_INDEX(3, j);
    FTENSOR_INDEX(3, k);
    FTENSOR_INDEX(3, l);
 
    using D = typename TensorTypeExtractor<T1>::Type;
    FTensor::Tensor3<D, dim, dim, dim> t_diff_X;
 
    auto t_hat = getHat(t_w_vee);
 
    t_diff_X(i, j, k) =
 
        alpha * FTensor::levi_civita<int>(i, j, k)
 
        +
 
        diff_alpha * t_hat(i, j) * t_w_vee(k)
 
        +
 
        beta * (t_hat(i, l) * FTensor::levi_civita<int>(l, j, k) +
                FTensor::levi_civita<int>(i, l, k) * t_hat(l, j))
 
        +
 
        diff_beta * t_hat(i, l) * t_hat(l, j) * t_w_vee(k);
 
    return t_diff_X;
  }
 
  template <typename T1, typename T2>
  inline static auto diffJlImpl(const FTensor::Tensor1<T1, dim> &t_w_vee,
                                const T2 theta) {
    if (fabs(theta) < std::numeric_limits<T2>::epsilon()) {
      // Taylor:
      // a      =  1/2  - theta^2/24  + ...
      // diff_a = -1/12 + theta^2/180 + ...
      // b      =  1/6  - theta^2/120 + ...
      // diff_b = -1/60 + theta^2/1260 + ...
      return genericDiffFormImpl(t_w_vee, 0.5, -1. / 12., 1. / 6., -1. / 60.);
    }
 
    const auto s = sin(theta);
    const auto c = cos(theta);
 
    const auto theta2 = theta * theta;
    const auto theta4 = theta2 * theta2;
    const auto theta5 = theta4 * theta;
 
    // Jl = I + a * hat + b * hat^2
    const auto a = (1. - c) / theta2;
    const auto diff_a = (theta * s + 2. * c - 2.) / theta4;
 
    const auto b = (theta - s) / (theta2 * theta);
    const auto diff_b = (3. * s - theta * c - 2. * theta) / theta5;
 
    return genericDiffFormImpl(t_w_vee, a, diff_a, b, diff_b);
  }
 
  template <typename T1, typename T2>
  inline static auto diffJrImpl(const FTensor::Tensor1<T1, dim> &t_w_vee,
                                const T2 theta) {
    if (fabs(theta) < std::numeric_limits<T2>::epsilon()) {
      // Taylor:
      // alpha      = -1/2  + theta^2/24  + ...
      // diff_alpha =  1/12 - theta^2/180 + ...
      // beta       =  1/6  - theta^2/120 + ...
      // diff_beta  = -1/60 + theta^2/1260 + ...
      return genericDiffFormImpl(t_w_vee, -0.5, 1. / 12., 1. / 6., -1. / 60.);
    }
 
    const auto s = sin(theta);
    const auto c = cos(theta);
 
    const auto theta2 = theta * theta;
    const auto theta4 = theta2 * theta2;
    const auto theta5 = theta4 * theta;
 
    // Jr = I + alpha * hat + beta * hat^2
    // with alpha = -(1 - cos(theta)) / theta^2
    const auto alpha = -(1. - c) / theta2;
    const auto diff_alpha = (2. - 2. * c - theta * s) / theta4;
    // equivalent:
    // const auto diff_alpha = -(theta * s + 2. * c - 2.) / theta4;
 
    const auto beta = (theta - s) / (theta2 * theta);
    const auto diff_beta = (3. * s - theta * c - 2. * theta) / theta5;
 
    return genericDiffFormImpl(t_w_vee, alpha, diff_alpha, beta, diff_beta);
  }
 
  template <typename T1, typename T2, typename T3>
  inline static auto
  actionImpl(const FTensor::Tensor1<T1, dim> &t_w_vee, const T2 theta,
             const FTensor::Tensor2_symmetric<T3, dim> &t_A) {
    using D = typename TensorTypeExtractor<T3>::Type;
    FTensor::Tensor2<D, dim, dim> t_B;
    t_B(i, j) = exp(t_w_vee, theta)(i, k) * t_A(k, j);
    return t_B;
  }
 
  template <typename T1, typename T2, typename T3>
  inline static auto
  spatialSpinImpl(const FTensor::Tensor1<T1, dim> &t_w_vee, const T2 theta,
                   const FTensor::Tensor1<T3, dim> &t_delta_w_vee) {
    using D = typename TensorTypeExtractor<T1>::Type;
    FTensor::Tensor1<D, dim> t_w;
    auto t_Jl = Jl(t_w_vee, theta);
    t_w(i) = t_Jl(i, j) * t_delta_w_vee(j);
    return t_w;
  }
 
  template <typename T1, typename T2, typename T3>
  inline static auto
  spatialSpinHatImpl(const FTensor::Tensor1<T1, dim> &t_w_vee, const T2 theta,
                      const FTensor::Tensor1<T3, dim> &t_delta_w_vee) {
    auto t_w = spatialSpinImpl(t_w_vee, theta, t_delta_w_vee);
    return getHat(t_w);
  }
 
  template <typename T1, typename T2, typename T3>
  inline static auto
  materialSpinImpl(const FTensor::Tensor1<T1, dim> &t_w_vee, const T2 theta,
                   const FTensor::Tensor1<T3, dim> &t_delta_w_vee) {
    using D = typename TensorTypeExtractor<T1>::Type;
    FTensor::Tensor1<D, dim> t_a;
    auto t_Jr = Jr(t_w_vee, theta);
    t_a(i) = t_Jr(i, j) * t_delta_w_vee(j);
    return t_a;
  }
 
  template <typename T1, typename T2, typename T3>
  inline static auto
  materialSpinHatImpl(const FTensor::Tensor1<T1, dim> &t_w_vee, const T2 theta,
                      const FTensor::Tensor1<T3, dim> &t_delta_w_vee) {
    auto t_a = materialSpinImpl(t_w_vee, theta, t_delta_w_vee);
    return getHat(t_a);
  }
 
  // deltaR = R * A
  template <typename T1, typename T2, typename T3>
  inline static auto deltaRImpl(const FTensor::Tensor1<T1, dim> &t_w_vee,
                                const T2 theta,
                                const FTensor::Tensor2<T3, dim, dim> &t_A_hat) {
    using D = typename TensorTypeExtractor<T1>::Type;
    FTensor::Tensor2<D, dim, dim> t_delta_R;
    auto t_R = exp(t_w_vee, theta);
    t_delta_R(i, j) = t_R(i, k) * t_A_hat(k, j);
    return t_delta_R;
  }
 
  // deltaR = R * hat( Jr(omega) * delta_omega )
  template <typename T1, typename T2, typename T3>
  inline static auto
  deltaRImpl(const FTensor::Tensor1<T1, dim> &t_w_vee,
             const T2 theta,
             const FTensor::Tensor1<T3, dim> &t_delta_w_vee) {
    auto t_A_hat = materialSpinHatImpl(t_w_vee, theta, t_delta_w_vee);
    return deltaRImpl(t_w_vee, theta, t_A_hat);
  }
 
}; // namespace SO3
 
}; // namespace LieGroups