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));
  }
 
  /** Third derivative of exp contracted with a direction,
   * d^3 R_ij / d w_k d w_m d w_n delta_w_n. */
  template <typename A, typename B, typename C>
  inline static auto diffDiffDiffExp(A &&t_w_vee, B &&theta,
                                     C &&t_delta_w_vee) {
    return diffDiffDiffExpImpl(std::forward<A>(t_w_vee), std::forward<B>(theta),
                               std::forward<C>(t_delta_w_vee));
  }
 
  // 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);
  }
 
  /**
   * Rodrigues coefficients a = sin(t)/t, b = (1 - cos t)/t^2 and the
   * derivatives that appear in the derivatives of exp. With x = t^2 = w.w the
   * chain rule gives d a/d w_k = diff_a w_k, d^2 a/d w_k d w_m = diff_a d_km
   * + diff_diff_a w_k w_m and d^3 a/d w_k d w_m d w_n = diff_diff_a (d_km w_n
   * + d_kn w_m + d_mn w_k) + diff_diff_diff_a w_k w_m w_n, i.e. diff_a =
   * 2 da/dx, diff_diff_a = 4 d2a/dx2, diff_diff_diff_a = 8 d3a/dx3, and the
   * same for b. For t < 1 the series in x is used: the closed forms cancel
   * catastrophically there (the second derivative loses about eps/t^5, the
   * third about eps/t^7).
   */
  template <typename T> struct RodriguesCoefficients {
    T a, diff_a, diff_diff_a, diff_diff_diff_a;
    T b, diff_b, diff_diff_b, diff_diff_diff_b;
  };
 
  template <typename T>
  inline static auto rodriguesCoefficients(const T theta) {
    RodriguesCoefficients<T> c;
    if (theta < 1.) {
      // a = sum (-x)^n/(2n+1)!, b = sum (-x)^n/(2n+2)!; the m-th x-derivative
      // carries n!/(n-m)! x^(n-m) and the factor 2^m.
      const T x = theta * theta;
      constexpr int terms = 14;
      T power[terms];
      power[0] = 1.;
      for (int n = 1; n != terms; ++n)
        power[n] = power[n - 1] * x;
      T *a[4] = {&c.a, &c.diff_a, &c.diff_diff_a, &c.diff_diff_diff_a};
      T *b[4] = {&c.b, &c.diff_b, &c.diff_diff_b, &c.diff_diff_diff_b};
      for (int m = 0; m != 4; ++m)
        *a[m] = *b[m] = 0.;
      double factorial_odd = 1., factorial_even = 2., sign = 1.;
      for (int n = 0; n != terms; ++n) {
        if (n) {
          factorial_odd *= (2 * n) * (2 * n + 1);
          factorial_even *= (2 * n + 1) * (2 * n + 2);
          sign = -sign;
        }
        double falling = 1., scale = 1.;
        for (int m = 0; m <= std::min(n, 3); ++m) {
          *a[m] += (sign * scale * falling / factorial_odd) * power[n - m];
          *b[m] += (sign * scale * falling / factorial_even) * power[n - m];
          falling *= (n - m);
          scale *= 2.;
        }
      }
      return c;
    }
    const auto s = sin(theta);
    const auto cc = cos(theta);
    const auto t2 = theta * theta;
    const auto t3 = t2 * theta, t4 = t3 * theta, t5 = t4 * theta,
               t6 = t5 * theta, t7 = t6 * theta, t8 = t7 * theta;
    c.a = s / theta;
    c.diff_a = (theta * cc - s) / t3;
    c.diff_diff_a = (3. * s - 3. * theta * cc - t2 * s) / t5;
    c.diff_diff_diff_a =
        (-t3 * cc + 6. * t2 * s + 15. * theta * cc - 15. * s) / t7;
    c.b = (1. - cc) / t2;
    c.diff_b = (-2. + 2. * cc + theta * s) / t4;
    c.diff_diff_b = (t2 * cc - 5. * theta * s - 8. * cc + 8.) / t6;
    c.diff_diff_diff_b =
        (-t3 * s - 9. * t2 * cc + 33. * theta * s + 48. * cc - 48.) / t8;
    return c;
  }
 
  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;
    };
 
    const auto c = rodriguesCoefficients(theta);
    return get_tensor(c.a, c.diff_a, c.b, c.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;
    };
 
    const auto c = rodriguesCoefficients(theta);
    return get_tensor(c.a, c.diff_a, c.diff_diff_a, c.b, c.diff_b,
                      c.diff_diff_b);
  }
 
  /**
   * d^3 R_ij / d w_k d w_m d w_n delta_n with R = I + a hat + b hat^2,
   * G_ijk = d(hat^2)_ij/d w_k = hat_il e_ljk + e_ilk hat_lj and
   * K_ijkm = d^2(hat^2)_ij/d w_k d w_m = e_ilm e_ljk + e_ilk e_ljm; the third
   * derivative of hat^2 vanishes. The derivative slots are distributed over
   * the coefficient and the tensor factor and the last slot is contracted
   * with delta.
   */
  template <typename T1, typename T2, typename T3>
  inline static auto
  diffDiffDiffExpImpl(const FTensor::Tensor1<T1, dim> &t_w_vee, const T2 theta,
                      const FTensor::Tensor1<T3, dim> &t_delta_w_vee) {
    FTENSOR_INDEX(3, i);
    FTENSOR_INDEX(3, j);
    FTENSOR_INDEX(3, k);
    FTENSOR_INDEX(3, l);
    FTENSOR_INDEX(3, m);
    FTENSOR_INDEX(3, n);
 
    constexpr auto t_kd = FTensor::Kronecker_Delta<double>();
    FTensor::Levi_Civita<int> t_eps;
 
    using D = typename TensorTypeExtractor<T1>::Type;
    const auto c = rodriguesCoefficients(theta);
    const auto t_hat = getHat(t_w_vee);
    const auto t_delta_hat = getHat(t_delta_w_vee);
    const D wd = t_w_vee(n) * t_delta_w_vee(n);
 
    FTensor::Tensor2<D, dim, dim> t_hat2, t_hat_delta;
    t_hat2(i, j) = t_hat(i, l) * t_hat(l, j);
    // d(hat^2)_ij/d w_n delta_n
    t_hat_delta(i, j) =
        t_hat(i, l) * t_delta_hat(l, j) + t_delta_hat(i, l) * t_hat(l, j);
    // G_ijk and K_ijkn delta_n
    FTensor::Tensor3<D, dim, dim, dim> t_G, t_G_delta;
    t_G(i, j, k) = t_hat(i, l) * t_eps(l, j, k) + t_eps(i, l, k) * t_hat(l, j);
    t_G_delta(i, j, k) =
        t_delta_hat(i, l) * t_eps(l, j, k) + t_eps(i, l, k) * t_delta_hat(l, j);
    FTensor::Tensor4<D, dim, dim, dim, dim> t_K;
    t_K(i, j, k, m) =
        t_eps(i, l, m) * t_eps(l, j, k) + t_eps(i, l, k) * t_eps(l, j, m);
 
    // coefficient derivatives with the last slot contracted with delta
    FTensor::Tensor2<D, dim, dim> t_dda, t_ddb, t_ddda, t_dddb;
    FTensor::Tensor1<D, dim> t_da_delta, t_db_delta;
    t_dda(k, m) =
        c.diff_a * t_kd(k, m) + c.diff_diff_a * t_w_vee(k) * t_w_vee(m);
    t_ddb(k, m) =
        c.diff_b * t_kd(k, m) + c.diff_diff_b * t_w_vee(k) * t_w_vee(m);
    t_ddda(k, m) =
        c.diff_diff_a * (t_kd(k, m) * wd + t_delta_w_vee(k) * t_w_vee(m) +
                         t_delta_w_vee(m) * t_w_vee(k)) +
        c.diff_diff_diff_a * wd * t_w_vee(k) * t_w_vee(m);
    t_dddb(k, m) =
        c.diff_diff_b * (t_kd(k, m) * wd + t_delta_w_vee(k) * t_w_vee(m) +
                         t_delta_w_vee(m) * t_w_vee(k)) +
        c.diff_diff_diff_b * wd * t_w_vee(k) * t_w_vee(m);
    t_da_delta(k) =
        c.diff_a * t_delta_w_vee(k) + c.diff_diff_a * wd * t_w_vee(k);
    t_db_delta(k) =
        c.diff_b * t_delta_w_vee(k) + c.diff_diff_b * wd * t_w_vee(k);
 
    FTensor::Tensor4<D, dim, dim, dim, dim> t_result;
    t_result(i, j, k, m) =
 
        t_ddda(k, m) * t_hat(i, j) + t_dda(k, m) * t_delta_hat(i, j) +
        t_da_delta(k) * t_eps(i, j, m) + t_da_delta(m) * t_eps(i, j, k)
 
        +
 
        t_dddb(k, m) * t_hat2(i, j) + t_ddb(k, m) * t_hat_delta(i, j) +
        t_db_delta(k) * t_G(i, j, m) + t_db_delta(m) * t_G(i, j, k) +
        (c.diff_b * wd) * t_K(i, j, k, m) +
        c.diff_b *
            (t_w_vee(k) * t_G_delta(i, j, m) + t_w_vee(m) * t_G_delta(i, j, k));
 
    return t_result;
  }
 
  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