Add math::FourthOrderTensor

math/BUILD.bazel

@@ -23,6 +23,7 @@ drake_cc_package_library(
         ":discrete_lyapunov_equation",
         ":eigen_sparse_triplet",
         ":evenly_distributed_pts_on_sphere",
+        ":fourth_order_tensor",
         ":geometric_transform",
         ":gradient",
         ":gray_code",
@@ -134,6 +135,16 @@ drake_cc_library(
     ],
 )
 
+drake_cc_library(
+    name = "fourth_order_tensor",
+    srcs = ["fourth_order_tensor.cc"],
+    hdrs = ["fourth_order_tensor.h"],
+    deps = [
+        "//common:default_scalars",
+        "//common:essential",
+    ],
+)
+
 # TODO(jwnimmer-tri) Improved name for this library, "pose_representations"?
 drake_cc_library(
     name = "geometric_transform",
@@ -433,6 +444,14 @@ drake_cc_googletest(
     ],
 )
 
+drake_cc_googletest(
+    name = "fourth_order_tensor_test",
+    deps = [
+        ":fourth_order_tensor",
+        "//common/test_utilities:eigen_matrix_compare",
+    ],
+)
+
 drake_cc_googletest(
     name = "jacobian_test",
     deps = [

math/fourth_order_tensor.cc

@@ -0,0 +1,71 @@
+#include "drake/math/fourth_order_tensor.h"
+
+#include "drake/common/default_scalars.h"
+
+namespace drake {
+namespace math {
+namespace internal {
+
+template <typename T>
+FourthOrderTensor<T>::FourthOrderTensor(const MatrixType& data) : data_(data) {}
+
+template <typename T>
+FourthOrderTensor<T>::FourthOrderTensor() = default;
+
+template <typename T>
+void FourthOrderTensor<T>::ContractWithVectors(
+    const Eigen::Ref<const Vector3<T>>& u,
+    const Eigen::Ref<const Vector3<T>>& v, EigenPtr<Matrix3<T>> B) const {
+  B->setZero();
+  for (int l = 0; l < 3; ++l) {
+    for (int j = 0; j < 3; ++j) {
+      *B += data_.template block<3, 3>(3 * j, 3 * l) * u(j) * v(l);
+    }
+  }
+}
+
+template <typename T>
+void FourthOrderTensor<T>::SetAsOuterProduct(
+    const Eigen::Ref<const Matrix3<T>>& M,
+    const Eigen::Ref<const Matrix3<T>>& N) {
+  const auto M_vec = Eigen::Map<const Vector<T, 9>>(M.data(), 9);
+  const auto N_vec = Eigen::Map<const Vector<T, 9>>(N.data(), 9);
+  data_.noalias() = M_vec * N_vec.transpose();
+}
+
+template <typename T>
+FourthOrderTensor<T> FourthOrderTensor<T>::MakeSymmetricIdentity(T scale) {
+  T half_scale = 0.5 * scale;
+  FourthOrderTensor<T> result;
+  /* The δᵢₖδⱼₗ term. */
+  result.data_ = half_scale * Eigen::Matrix<T, 9, 9>::Identity();
+  for (int j = 0; j < 3; ++j) {
+    /* The δᵢₗδⱼₖ term. */
+    for (int i = 0; i < 3; ++i) {
+      const int l = i;
+      const int k = j;
+      result.data_(3 * j + i, 3 * l + k) += half_scale;
+    }
+  }
+  return result;
+}
+
+template <typename T>
+FourthOrderTensor<T> FourthOrderTensor<T>::MakeMajorIdentity(T scale) {
+  return FourthOrderTensor<T>(scale * Eigen::Matrix<T, 9, 9>::Identity());
+}
+
+template <typename T>
+FourthOrderTensor<T>& FourthOrderTensor<T>::operator+=(
+    const FourthOrderTensor<T>& other) {
+  data_.noalias() += other.data_;
+  return *this;
+}
+
+}  // namespace internal
+}  // namespace math
+}  // namespace drake
+
+DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS(
+    class ::drake::math::internal::FourthOrderTensor);
+template class drake::math::internal::FourthOrderTensor<float>;

math/fourth_order_tensor.h

@@ -0,0 +1,109 @@
+#pragma once
+
+#include "drake/common/drake_throw.h"
+#include "drake/common/eigen_types.h"
+
+namespace drake {
+namespace math {
+namespace internal {
+
+/* This class provides functionalities related to 4th-order tensors of
+ dimension 3*3*3*3.  @tparam float, double, AutoDiffXd. */
+template <typename T>
+class FourthOrderTensor {
+ public:
+  DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(FourthOrderTensor)
+  using MatrixType = Eigen::Matrix<T, 9, 9>;
+
+  /* Constructs a 4th-order tensor represented by the given matrix using the
+   convention layed out in the class documentation. */
+  explicit FourthOrderTensor(const MatrixType& data);
+
+  /* Constructs a zero 4th-order tensor. */
+  FourthOrderTensor();
+
+  /* Performs contraction between this 4th-order tensor A and two vectors u and
+   v and outputs 2nd order tensor B. In Einstein notation, the contraction being
+   done is Bᵢₖ = uⱼ Aᵢⱼₖₗ vₗ. */
+  void ContractWithVectors(const Eigen::Ref<const Vector3<T>>& u,
+                           const Eigen::Ref<const Vector3<T>>& v,
+                           EigenPtr<Matrix3<T>> B) const;
+
+  /* Sets this 4th-order tensor as the outer product of the matrices (2nd-order
+   tensor) M and N. More specifically, in Einstein notion, sets Aᵢⱼₖₗ = MᵢⱼNₖₗ.
+   @warn This function assumes the input matrices are aliasing the data in this
+   4th-order tensor. */
+  void SetAsOuterProduct(const Eigen::Ref<const Matrix3<T>>& M,
+                         const Eigen::Ref<const Matrix3<T>>& N);
+
+  /* Returns a a scaled symmetric identity 4th-order tensor. In Einstein
+   notation, the result is scale * 1/2 * (δᵢₖδⱼₗ + δᵢₗδⱼₖ).  */
+  static FourthOrderTensor<T> MakeSymmetricIdentity(T scale);
+
+  /* Returns a a scaled major identity 4th-order tensor. In Einstein
+   notation, the result is scale * δᵢₖδⱼₗ.  */
+  static FourthOrderTensor<T> MakeMajorIdentity(T scale);
+
+  FourthOrderTensor<T>& operator+=(const FourthOrderTensor<T>& other);
+
+  /* Returns this 4th-order tensor encoded as a 2D matrix.
+   The tensor is represented using a a 9*9 matrix organized as following
+
+                  l = 1       l = 2       l = 3
+              -------------------------------------
+              |           |           |           |
+    j = 1     |   Aᵢ₁ₖ₁   |   Aᵢ₁ₖ₂   |   Aᵢ₁ₖ₃   |
+              |           |           |           |
+              -------------------------------------
+              |           |           |           |
+    j = 2     |   Aᵢ₂ₖ₁   |   Aᵢ₂ₖ₂   |   Aᵢ₂ₖ₃   |
+              |           |           |           |
+              -------------------------------------
+              |           |           |           |
+    j = 3     |   Aᵢ₃ₖ₁   |   Aᵢ₃ₖ₂   |   Aᵢ₃ₖ₃   |
+              |           |           |           |
+              -------------------------------------
+  Namely the ik-th entry in the jl-th block corresponds to the value Aᵢⱼₖₗ. */
+  const MatrixType& data() const { return data_; }
+
+  /* Returns this 4th-order tensor encoded as a mutable 2D matrix.
+   @see data() for the layout of the matrix. */
+  MatrixType& mutable_data() { return data_; }
+
+  /* Returns the value of the 4th-order tensor at the given indices.
+   @pre 0 <= i, j, k, l < 3. */
+  const T& operator()(int i, int j, int k, int l) const {
+    DRAKE_ASSERT(0 <= i && i < 3);
+    DRAKE_ASSERT(0 <= j && j < 3);
+    DRAKE_ASSERT(0 <= k && k < 3);
+    DRAKE_ASSERT(0 <= l && l < 3);
+    return data_(3 * j + i, 3 * l + k);
+  }
+
+  /* Returns the value of the 4th-order tensor at the given indices.
+   @pre 0 <= i, j, k, l < 3. */
+  T& operator()(int i, int j, int k, int l) {
+    DRAKE_ASSERT(0 <= i && i < 3);
+    DRAKE_ASSERT(0 <= j && j < 3);
+    DRAKE_ASSERT(0 <= k && k < 3);
+    DRAKE_ASSERT(0 <= l && l < 3);
+    return data_(3 * j + i, 3 * l + k);
+  }
+
+  /* Sets the data of this 4th-order tensor to the given matrix, using the
+   convention layed out in the class documentation. */
+  void set_data(const MatrixType& data) { data_ = data; }
+
+ private:
+  MatrixType data_{MatrixType::Zero()};
+};
+
+template <typename T>
+FourthOrderTensor<T> operator+(const FourthOrderTensor<T>& t1,
+                               const FourthOrderTensor<T>& t2) {
+  return FourthOrderTensor<T>(t1) += t2;
+}
+
+}  // namespace internal
+}  // namespace math
+}  // namespace drake

math/test/fourth_order_tensor_test.cc

@@ -0,0 +1,151 @@
+#include "drake/math/fourth_order_tensor.h"
+
+#include "drake/common/eigen_types.h"
+#include "drake/common/test_utilities/eigen_matrix_compare.h"
+
+using Eigen::Matrix3d;
+using Eigen::Vector3d;
+
+namespace drake {
+namespace math {
+namespace internal {
+namespace {
+
+Eigen::Matrix<double, 9, 9> MakeArbitraryMatrix() {
+  Eigen::Matrix<double, 9, 9> data;
+  for (int i = 0; i < 9; ++i) {
+    for (int j = 0; j < 9; ++j) {
+      data(i, j) = i + 2 * j;
+    }
+  }
+  return data;
+}
+
+GTEST_TEST(FourthOrderTensorTest, DefaultConstructor) {
+  FourthOrderTensor<double> tensor;
+  EXPECT_TRUE(tensor.data().isZero());
+  const Vector3d u(1.0, 2.0, 3.0);
+  const Vector3d v(4.0, 5.0, 6.0);
+  Matrix3d B;
+
+  tensor.ContractWithVectors(u, v, &B);
+  EXPECT_TRUE(B.isZero());
+}
+
+GTEST_TEST(FourthOrderTensorTest, ConstructWithData) {
+  const Eigen::Matrix<double, 9, 9> data = MakeArbitraryMatrix();
+  FourthOrderTensor<double> tensor(data);
+  /* Getter and mutable getter. */
+  EXPECT_EQ(tensor.data(), data);
+  EXPECT_EQ(tensor.data(), tensor.mutable_data());
+  tensor.mutable_data() = data.transpose();
+  EXPECT_EQ(tensor.data(), data.transpose());
+  /* Settor */
+  tensor.set_data(data);
+  EXPECT_EQ(tensor.data(), data);
+  /* Operator with four indices. */
+  EXPECT_EQ(tensor(0, 0, 0, 0), data(0, 0));
+  EXPECT_EQ(tensor(1, 1, 1, 1), data(4, 4));
+}
+
+GTEST_TEST(FourthOrderTensorTest, ContractWithVectors) {
+  FourthOrderTensor<double> tensor(MakeArbitraryMatrix());
+
+  /* If any vector input is zero, the contraction is zero. */
+  Vector3d u = Vector3d::Zero();
+  Vector3d v(4.0, 5.0, 6.0);
+  Matrix3d B;
+  tensor.ContractWithVectors(u, v, &B);
+  EXPECT_TRUE(B.isZero());
+  tensor.ContractWithVectors(v, u, &B);
+  EXPECT_TRUE(B.isZero());
+
+  /* If the 9x9 representation of the tensor has a repeating pattern in the
+   blocks, the contraction is a multiple of that block. */
+  Matrix3d block;
+  block << 1, 2, 3, 4, 5, 6, 7, 8, 9;
+  Eigen::Matrix<double, 9, 9> data;
+  for (int i = 0; i < 3; ++i) {
+    for (int j = 0; j < 3; ++j) {
+      data.block<3, 3>(3 * i, 3 * j) = block;
+    }
+  }
+  tensor = FourthOrderTensor<double>(data);
+  u << 1.0, 2.0, 3.0;
+  v << 4.0, 5.0, 6.0;
+  tensor.ContractWithVectors(u, v, &B);
+  EXPECT_TRUE(CompareMatrices(B, block * (u * v.transpose()).sum()));
+}
+
+GTEST_TEST(FourthOrderTensorTest, SetAsOuterProduct) {
+  FourthOrderTensor<double> t1, t2;
+  Matrix3d M, N;
+  M << 1, 0, 3, 0, 5, 0, 7, 0, 9;
+  N << 0, 2, 0, 4, 0, 6, 0, 8, 0;
+  t1.SetAsOuterProduct(M, N);
+  for (int i = 0; i < 3; ++i) {
+    for (int j = 0; j < 3; ++j) {
+      for (int k = 0; k < 3; ++k) {
+        for (int l = 0; l < 3; ++l) {
+          EXPECT_EQ(t1(i, j, k, l), M(i, j) * N(k, l));
+        }
+      }
+    }
+  }
+  t2.SetAsOuterProduct(N, M);
+  EXPECT_TRUE(CompareMatrices(t1.data(), t2.data().transpose()));
+}
+
+GTEST_TEST(FourthOrderTensorTest, MakeSymmetricIdentity) {
+  const double scale = 1.23;
+  const FourthOrderTensor<double> tensor =
+      FourthOrderTensor<double>::MakeSymmetricIdentity(scale);
+  /* The expected result is  scale * 1/2 * (δᵢₖδⱼₗ + δᵢₗδⱼₖ).  */
+  for (int i = 0; i < 3; ++i) {
+    for (int j = 0; j < 3; ++j) {
+      for (int k = 0; k < 3; ++k) {
+        for (int l = 0; l < 3; ++l) {
+          double expected_entry = 0.0;
+          if (i == k && j == l) expected_entry += 0.5 * scale;
+          if (i == l && j == k) expected_entry += 0.5 * scale;
+          EXPECT_EQ(tensor(i, j, k, l), expected_entry);
+        }
+      }
+    }
+  }
+}
+
+GTEST_TEST(FourthOrderTensorTest, MakeMajorIdentity) {
+  const double scale = 1.23;
+  const FourthOrderTensor<double> tensor =
+      FourthOrderTensor<double>::MakeMajorIdentity(scale);
+  /* The expected result is scale * δᵢₖδⱼₗ.  */
+  for (int i = 0; i < 3; ++i) {
+    for (int j = 0; j < 3; ++j) {
+      for (int k = 0; k < 3; ++k) {
+        for (int l = 0; l < 3; ++l) {
+          double expected_entry = 0.0;
+          if (i == k && j == l) expected_entry += scale;
+          EXPECT_EQ(tensor(i, j, k, l), expected_entry);
+        }
+      }
+    }
+  }
+}
+
+GTEST_TEST(FourthOrderTensorTest, Addition) {
+  const Eigen::Matrix<double, 9, 9> data1 = MakeArbitraryMatrix();
+  const Eigen::Matrix<double, 9, 9> data2 = MakeArbitraryMatrix().transpose();
+  FourthOrderTensor<double> tensor1(data1);
+  const FourthOrderTensor<double> tensor2(data2);
+  tensor1 += tensor2;
+  EXPECT_EQ(tensor1.data(), data1 + data2);
+
+  const FourthOrderTensor<double> tensor3 = tensor1 + tensor2;
+  EXPECT_EQ(tensor3.data(), data1 + 2.0 * data2);
+}
+
+}  // namespace
+}  // namespace internal
+}  // namespace math
+}  // namespace drake

multibody/fem/BUILD.bazel

@@ -83,6 +83,7 @@ drake_cc_library(
     deps = [
         "//common:essential",
         "//common:nice_type_name",
+        "//math:fourth_order_tensor",
     ],
 )
 
@@ -407,6 +408,7 @@ drake_cc_library(
     deps = [
         "//common:default_scalars",
         "//common:essential",
+        "//math:fourth_order_tensor",
     ],
 )