feat: implement consistent-mass HHT shell dynamics

src/fesa/elements/nonlinear_mitc4_inertia.cpp

@@ -0,0 +1,871 @@
+#include "fesa/elements/nonlinear_mitc4_inertia.h"
+
+#include <algorithm>
+#include <array>
+#include <cmath>
+#include <cstddef>
+#include <limits>
+#include <string>
+#include <utility>
+
+#include "fesa/elements/finite_rotation.h"
+
+namespace fesa {
+namespace {
+
+constexpr std::size_t kNodes = 4U;
+constexpr std::size_t kDofs = 24U;
+const std::array<double, 3> kGauss{-std::sqrt(3.0 / 5.0), 0.0,
+                                   std::sqrt(3.0 / 5.0)};
+constexpr std::array<double, 3> kWeights{5.0 / 9.0, 8.0 / 9.0, 5.0 / 9.0};
+
+template <class T>
+Result<T> Failure(const std::string& code, const SourceLocation& location,
+                  const std::string& identity, std::string message) {
+  return Result<T>::Failure(Status::Failure(
+      FailureCategory::kModel, {{Severity::kError, code, location, "*ELEMENT",
+                                 identity, std::move(message)}}));
+}
+
+std::string Identity(const std::array<const Node*, kNodes>& nodes) {
+  std::string identity;
+  for (const Node* node : nodes) {
+    if (!identity.empty()) {
+      identity += "-";
+    }
+    identity += node == nullptr ? "?" : node->source_id.source_label_text;
+  }
+  return identity;
+}
+
+Matrix FrameMatrix(const ShellNodeInitialFrame& frame) {
+  Matrix result{3U, 3U};
+  for (std::size_t row = 0U; row < 3U; ++row) {
+    result(row, 0U) = frame.tangent_a[row];
+    result(row, 1U) = frame.tangent_b[row];
+    result(row, 2U) = frame.director[row];
+  }
+  return result;
+}
+
+bool IsFinite(const Matrix& matrix) {
+  for (std::size_t row = 0U; row < matrix.Rows(); ++row) {
+    for (std::size_t column = 0U; column < matrix.Columns(); ++column) {
+      if (!std::isfinite(matrix(row, column))) {
+        return false;
+      }
+    }
+  }
+  return true;
+}
+
+bool IsFinite(const Vector& vector) {
+  for (std::size_t index = 0U; index < vector.Size(); ++index) {
+    if (!std::isfinite(vector[index])) {
+      return false;
+    }
+  }
+  return true;
+}
+
+/// @brief Carries all derivatives through order three in one nodal rotation.
+struct Jet {
+  double value{};
+  std::array<double, 3> d{};
+  std::array<std::array<double, 3>, 3> dd{};
+  std::array<std::array<std::array<double, 3>, 3>, 3> ddd{};
+};
+
+Jet Constant(const double value) { return Jet{value, {}, {}, {}}; }
+
+Jet Variable(const double value, const std::size_t component) {
+  Jet result = Constant(value);
+  result.d[component] = 1.0;
+  return result;
+}
+
+Jet operator+(const Jet& left, const Jet& right) {
+  Jet result{};
+  result.value = left.value + right.value;
+  for (std::size_t i = 0U; i < 3U; ++i) {
+    result.d[i] = left.d[i] + right.d[i];
+    for (std::size_t j = 0U; j < 3U; ++j) {
+      result.dd[i][j] = left.dd[i][j] + right.dd[i][j];
+      for (std::size_t k = 0U; k < 3U; ++k) {
+        result.ddd[i][j][k] = left.ddd[i][j][k] + right.ddd[i][j][k];
+      }
+    }
+  }
+  return result;
+}
+
+Jet operator-(const Jet& left, const Jet& right) {
+  Jet result{};
+  result.value = left.value - right.value;
+  for (std::size_t i = 0U; i < 3U; ++i) {
+    result.d[i] = left.d[i] - right.d[i];
+    for (std::size_t j = 0U; j < 3U; ++j) {
+      result.dd[i][j] = left.dd[i][j] - right.dd[i][j];
+      for (std::size_t k = 0U; k < 3U; ++k) {
+        result.ddd[i][j][k] = left.ddd[i][j][k] - right.ddd[i][j][k];
+      }
+    }
+  }
+  return result;
+}
+
+Jet operator*(const Jet& left, const Jet& right) {
+  Jet result{};
+  result.value = left.value * right.value;
+  for (std::size_t i = 0U; i < 3U; ++i) {
+    result.d[i] = left.d[i] * right.value + left.value * right.d[i];
+    for (std::size_t j = 0U; j < 3U; ++j) {
+      result.dd[i][j] = left.dd[i][j] * right.value + left.d[i] * right.d[j] +
+                        left.d[j] * right.d[i] + left.value * right.dd[i][j];
+      for (std::size_t k = 0U; k < 3U; ++k) {
+        result.ddd[i][j][k] =
+            left.ddd[i][j][k] * right.value + left.dd[i][j] * right.d[k] +
+            left.dd[i][k] * right.d[j] + left.dd[j][k] * right.d[i] +
+            left.d[i] * right.dd[j][k] + left.d[j] * right.dd[i][k] +
+            left.d[k] * right.dd[i][j] + left.value * right.ddd[i][j][k];
+      }
+    }
+  }
+  return result;
+}
+
+Jet Reciprocal(const Jet& value) {
+  Jet result{};
+  result.value = 1.0 / value.value;
+  const double squared = result.value * result.value;
+  const double cubed = squared * result.value;
+  const double fourth = cubed * result.value;
+  for (std::size_t i = 0U; i < 3U; ++i) {
+    result.d[i] = -value.d[i] * squared;
+    for (std::size_t j = 0U; j < 3U; ++j) {
+      result.dd[i][j] =
+          2.0 * value.d[i] * value.d[j] * cubed - value.dd[i][j] * squared;
+      for (std::size_t k = 0U; k < 3U; ++k) {
+        result.ddd[i][j][k] =
+            -6.0 * value.d[i] * value.d[j] * value.d[k] * fourth +
+            2.0 *
+                (value.dd[i][j] * value.d[k] + value.dd[i][k] * value.d[j] +
+                 value.dd[j][k] * value.d[i]) *
+                cubed -
+            value.ddd[i][j][k] * squared;
+      }
+    }
+  }
+  return result;
+}
+
+Jet InverseSqrt(const Jet& value) {
+  Jet result{};
+  result.value = 1.0 / std::sqrt(value.value);
+  const double cubed = result.value * result.value * result.value;
+  const double fifth = cubed * result.value * result.value;
+  const double seventh = fifth * result.value * result.value;
+  for (std::size_t i = 0U; i < 3U; ++i) {
+    result.d[i] = -0.5 * cubed * value.d[i];
+    for (std::size_t j = 0U; j < 3U; ++j) {
+      result.dd[i][j] =
+          0.75 * fifth * value.d[i] * value.d[j] - 0.5 * cubed * value.dd[i][j];
+      for (std::size_t k = 0U; k < 3U; ++k) {
+        result.ddd[i][j][k] =
+            -1.875 * seventh * value.d[i] * value.d[j] * value.d[k] +
+            0.75 * fifth *
+                (value.dd[i][j] * value.d[k] + value.dd[i][k] * value.d[j] +
+                 value.dd[j][k] * value.d[i]) -
+            0.5 * cubed * value.ddd[i][j][k];
+      }
+    }
+  }
+  return result;
+}
+
+Jet Scale(const double scalar, const Jet& value) {
+  return Constant(scalar) * value;
+}
+
+using JetVector = std::array<Jet, 3>;
+using JetMatrix = std::array<std::array<Jet, 3>, 3>;
+
+JetVector Multiply(const JetMatrix& matrix, const JetVector& vector) {
+  JetVector result{Constant(0.0), Constant(0.0), Constant(0.0)};
+  for (std::size_t row = 0U; row < 3U; ++row) {
+    for (std::size_t column = 0U; column < 3U; ++column) {
+      result[row] = result[row] + matrix[row][column] * vector[column];
+    }
+  }
+  return result;
+}
+
+JetMatrix Multiply(const JetMatrix& left, const JetMatrix& right) {
+  JetMatrix result{};
+  for (std::size_t row = 0U; row < 3U; ++row) {
+    for (std::size_t column = 0U; column < 3U; ++column) {
+      result[row][column] = Constant(0.0);
+      for (std::size_t index = 0U; index < 3U; ++index) {
+        result[row][column] =
+            result[row][column] + left[row][index] * right[index][column];
+      }
+    }
+  }
+  return result;
+}
+
+JetMatrix IdentityMatrix() {
+  JetMatrix result{};
+  for (std::size_t index = 0U; index < 3U; ++index) {
+    result[index][index] = Constant(1.0);
+  }
+  return result;
+}
+
+JetMatrix Skew(const JetVector& value) {
+  JetMatrix result{};
+  result[0U][1U] = Scale(-1.0, value[2U]);
+  result[0U][2U] = value[1U];
+  result[1U][0U] = value[2U];
+  result[1U][2U] = Scale(-1.0, value[0U]);
+  result[2U][0U] = Scale(-1.0, value[1U]);
+  result[2U][1U] = value[0U];
+  return result;
+}
+
+Jet Dot(const JetVector& left, const JetVector& right) {
+  Jet result = Constant(0.0);
+  for (std::size_t component = 0U; component < 3U; ++component) {
+    result = result + left[component] * right[component];
+  }
+  return result;
+}
+
+JetMatrix Inverse(const JetMatrix& matrix) {
+  JetMatrix cofactor{};
+  for (std::size_t row = 0U; row < 3U; ++row) {
+    for (std::size_t column = 0U; column < 3U; ++column) {
+      const std::size_t a = (row + 1U) % 3U;
+      const std::size_t b = (row + 2U) % 3U;
+      const std::size_t c = (column + 1U) % 3U;
+      const std::size_t d = (column + 2U) % 3U;
+      cofactor[row][column] =
+          matrix[a][c] * matrix[b][d] - matrix[a][d] * matrix[b][c];
+      // Cyclic minor ordering already includes the cofactor sign.
+    }
+  }
+  Jet determinant = Constant(0.0);
+  for (std::size_t column = 0U; column < 3U; ++column) {
+    determinant = determinant + matrix[0U][column] * cofactor[0U][column];
+  }
+  const Jet inverse_determinant = Reciprocal(determinant);
+  JetMatrix result{};
+  for (std::size_t row = 0U; row < 3U; ++row) {
+    for (std::size_t column = 0U; column < 3U; ++column) {
+      result[row][column] = cofactor[column][row] * inverse_determinant;
+    }
+  }
+  return result;
+}
+
+JetMatrix LeftJacobian(const JetVector& phi) {
+  const JetMatrix skew = Skew(phi);
+  JetMatrix result = IdentityMatrix();
+  JetMatrix term = IdentityMatrix();
+  double factorial = 1.0;
+  for (std::size_t order = 1U; order <= 32U; ++order) {
+    term = Multiply(term, skew);
+    factorial *= static_cast<double>(order + 1U);
+    for (std::size_t row = 0U; row < 3U; ++row) {
+      for (std::size_t column = 0U; column < 3U; ++column) {
+        result[row][column] =
+            result[row][column] + Scale(1.0 / factorial, term[row][column]);
+      }
+    }
+  }
+  return result;
+}
+
+/// @brief Uses a converged exponential series so D1/D2/D3 share one analytic
+/// map.
+JetMatrix ExpSkew(const JetVector& phi) {
+  const JetMatrix skew = Skew(phi);
+  JetMatrix result = IdentityMatrix();
+  JetMatrix term = IdentityMatrix();
+  double factorial = 1.0;
+  for (std::size_t order = 1U; order <= 32U; ++order) {
+    term = Multiply(term, skew);
+    factorial *= static_cast<double>(order);
+    for (std::size_t row = 0U; row < 3U; ++row) {
+      for (std::size_t column = 0U; column < 3U; ++column) {
+        result[row][column] =
+            result[row][column] + Scale(1.0 / factorial, term[row][column]);
+      }
+    }
+  }
+  return result;
+}
+
+double Determinant(const Vector3& a, const Vector3& b, const Vector3& c) {
+  return a.Dot(b.Cross(c));
+}
+
+/// @brief Checks the scaled physical twenty-dimensional mass without a rank
+/// cut.
+Status CheckPhysicalMass(const Matrix& mass,
+                         const std::array<Vector3, kNodes>& nulls,
+                         const double length, const SourceLocation& location,
+                         const std::string& identity) {
+  const auto fail = [&](const std::string& code, const std::string& message) {
+    return Status::Failure(
+        FailureCategory::kModel,
+        {{Severity::kError, code, location, "*ELEMENT", identity, message}});
+  };
+  constexpr std::size_t kPhysical = 20U;
+  Matrix scaled{kDofs, kDofs};
+  double scale = 0.0;
+  for (std::size_t row = 0U; row < kDofs; ++row) {
+    for (std::size_t column = 0U; column < kDofs; ++column) {
+      scaled(row, column) = mass(row, column) * (row % 6U < 3U ? length : 1.0) *
+                            (column % 6U < 3U ? length : 1.0);
+      if (mass(row, column) != 0.0 && scaled(row, column) == 0.0) {
+        return fail("invalid-nonlinear-inertia-positive-mass",
+                    "Physical mass scaling underflowed a nonzero entry.");
+      }
+      scale = std::max(scale, std::abs(scaled(row, column)));
+    }
+  }
+  if (!IsFinite(scaled) || !std::isfinite(scale) || !(scale > 0.0)) {
+    return fail("invalid-nonlinear-inertia-positive-mass",
+                "Scaled physical mass must have a finite positive scale.");
+  }
+  // Scaling by a positive entry preserves conditioning and avoids density
+  // overflow in the Cholesky and inverse-action norms.
+  for (std::size_t row = 0U; row < kDofs; ++row) {
+    for (std::size_t column = 0U; column < kDofs; ++column) {
+      scaled(row, column) /= scale;
+    }
+  }
+  Matrix complement{kDofs, kPhysical};
+  for (std::size_t node = 0U; node < kNodes; ++node) {
+    const Vector3& normal = nulls[node];
+    std::array<std::size_t, 3> axes{0U, 1U, 2U};
+    std::stable_sort(axes.begin(), axes.end(),
+                     [&](const std::size_t a, const std::size_t b) {
+                       return std::abs(normal[a]) < std::abs(normal[b]);
+                     });
+    std::array<Vector3, 2> tangent{};
+    for (std::size_t column = 0U; column < 2U; ++column) {
+      std::array<double, 3> axis{};
+      axis[axes[column]] = 1.0;
+      Vector3 direction{axis};
+      direction = direction - direction.Dot(normal) * normal;
+      for (std::size_t previous = 0U; previous < column; ++previous) {
+        direction =
+            direction - direction.Dot(tangent[previous]) * tangent[previous];
+      }
+      const double norm = direction.Norm();
+      if (!std::isfinite(norm) || !(norm > 0.0)) {
+        return fail("invalid-nonlinear-inertia-null",
+                    "The analytic physical complement must remain finite.");
+      }
+      tangent[column] = direction / norm;
+    }
+    for (std::size_t row = 0U; row < 3U; ++row) {
+      complement(6U * node + row, 5U * node + row) = 1.0;
+      for (std::size_t column = 0U; column < 2U; ++column) {
+        complement(6U * node + 3U + row, 5U * node + 3U + column) =
+            tangent[column][row];
+      }
+    }
+  }
+  double mass_norm = 0.0;
+  double symmetry_norm = 0.0;
+  double null_action_norm = 0.0;
+  for (std::size_t row = 0U; row < kDofs; ++row) {
+    for (std::size_t column = 0U; column < kDofs; ++column) {
+      mass_norm = std::hypot(mass_norm, scaled(row, column));
+      symmetry_norm =
+          std::hypot(symmetry_norm, scaled(row, column) - scaled(column, row));
+    }
+    for (std::size_t node = 0U; node < kNodes; ++node) {
+      double action = 0.0;
+      for (std::size_t component = 0U; component < 3U; ++component) {
+        action +=
+            scaled(row, 6U * node + 3U + component) * nulls[node][component];
+      }
+      null_action_norm = std::hypot(null_action_norm, action);
+    }
+  }
+  double gram_error = 0.0;
+  double orthogonality_error = 0.0;
+  for (std::size_t row = 0U; row < kPhysical; ++row) {
+    for (std::size_t column = 0U; column < kPhysical; ++column) {
+      double entry = row == column ? -1.0 : 0.0;
+      for (std::size_t inner = 0U; inner < kDofs; ++inner) {
+        entry += complement(inner, row) * complement(inner, column);
+      }
+      gram_error = std::hypot(gram_error, entry);
+    }
+    for (std::size_t node = 0U; node < kNodes; ++node) {
+      double entry = 0.0;
+      for (std::size_t component = 0U; component < 3U; ++component) {
+        entry += complement(6U * node + 3U + component, row) *
+                 nulls[node][component];
+      }
+      orthogonality_error = std::hypot(orthogonality_error, entry);
+    }
+  }
+  double null_gram_error = 0.0;
+  for (const Vector3& normal : nulls) {
+    null_gram_error = std::hypot(null_gram_error, normal.Dot(normal) - 1.0);
+  }
+  const double full_gram_error = std::hypot(
+      gram_error,
+      std::hypot(null_gram_error, std::sqrt(2.0) * orthogonality_error));
+  if (!(symmetry_norm / mass_norm <= 1.0e-12) ||
+      !(null_action_norm / mass_norm / 2.0 <= 1.0e-12) ||
+      !(full_gram_error <= 1.0e-12)) {
+    return fail("invalid-nonlinear-inertia-null",
+                "Scaled mass symmetry and analytic null identities failed.");
+  }
+  Matrix block{kPhysical, kPhysical};
+  Matrix mass_complement{kDofs, kPhysical};
+  for (std::size_t row = 0U; row < kDofs; ++row) {
+    for (std::size_t column = 0U; column < kPhysical; ++column) {
+      for (std::size_t inner = 0U; inner < kDofs; ++inner) {
+        mass_complement(row, column) +=
+            scaled(row, inner) * complement(inner, column);
+      }
+    }
+  }
+  double block_norm = 0.0;
+  for (std::size_t row = 0U; row < kPhysical; ++row) {
+    double row_sum = 0.0;
+    for (std::size_t column = 0U; column < kPhysical; ++column) {
+      for (std::size_t inner = 0U; inner < kDofs; ++inner) {
+        block(row, column) +=
+            complement(inner, row) * mass_complement(inner, column);
+      }
+      row_sum += std::abs(block(row, column));
+    }
+    block_norm = std::max(block_norm, row_sum);
+  }
+  Matrix lower{kPhysical, kPhysical};
+  for (std::size_t row = 0U; row < kPhysical; ++row) {
+    for (std::size_t column = 0U; column <= row; ++column) {
+      double entry = block(row, column);
+      for (std::size_t inner = 0U; inner < column; ++inner) {
+        entry -= lower(row, inner) * lower(column, inner);
+      }
+      if (!std::isfinite(entry) || (row == column && !(entry > 0.0))) {
+        return fail(
+            "invalid-nonlinear-inertia-positive-mass",
+            "Physical mass requires strictly positive Cholesky pivots.");
+      }
+      lower(row, column) =
+          row == column ? std::sqrt(entry) : entry / lower(column, column);
+    }
+  }
+  std::array<double, kPhysical> inverse_row_sums{};
+  double reconstruction_error = 0.0;
+  double block_frobenius = 0.0;
+  for (std::size_t row = 0U; row < kPhysical; ++row) {
+    for (std::size_t column = 0U; column < kPhysical; ++column) {
+      double reconstructed = 0.0;
+      for (std::size_t inner = 0U; inner <= std::min(row, column); ++inner) {
+        reconstructed += lower(row, inner) * lower(column, inner);
+      }
+      reconstruction_error =
+          std::hypot(reconstruction_error, reconstructed - block(row, column));
+      block_frobenius = std::hypot(block_frobenius, block(row, column));
+    }
+  }
+  if (!(reconstruction_error / block_frobenius <= 1.0e-12)) {
+    return fail("invalid-nonlinear-inertia-positive-mass",
+                "Physical mass Cholesky reconstruction failed.");
+  }
+  for (std::size_t column = 0U; column < kPhysical; ++column) {
+    std::array<double, kPhysical> solution{};
+    for (std::size_t row = 0U; row < kPhysical; ++row) {
+      double rhs = row == column ? 1.0 : 0.0;
+      for (std::size_t inner = 0U; inner < row; ++inner) {
+        rhs -= lower(row, inner) * solution[inner];
+      }
+      solution[row] = rhs / lower(row, row);
+    }
+    for (std::size_t reverse = kPhysical; reverse > 0U; --reverse) {
+      const std::size_t row = reverse - 1U;
+      double rhs = solution[row];
+      for (std::size_t inner = row + 1U; inner < kPhysical; ++inner) {
+        rhs -= lower(inner, row) * solution[inner];
+      }
+      solution[row] = rhs / lower(row, row);
+      inverse_row_sums[row] += std::abs(solution[row]);
+    }
+  }
+  const double inverse_norm =
+      *std::max_element(inverse_row_sums.begin(), inverse_row_sums.end());
+  const double reciprocal_condition = 1.0 / block_norm / inverse_norm;
+  if (!std::isfinite(inverse_norm) || !std::isfinite(reciprocal_condition) ||
+      !(reciprocal_condition >= std::ldexp(1.0, -32))) {
+    return fail("ill-conditioned-nonlinear-inertia-positive-mass",
+                "Physical mass reciprocal infinity condition is below 2^-32.");
+  }
+  return Status::Ok();
+}
+
+}  // namespace
+
+Result<DynamicRotationKinematics> EvaluateDynamicRotation(
+    const Vector3& phi_value, const Vector3& initial_director,
+    const Vector3& velocity, const Vector3& acceleration) {
+  if (!phi_value.IsFinite() || !initial_director.IsFinite() ||
+      !velocity.IsFinite() || !acceleration.IsFinite() ||
+      !(std::abs(initial_director.Norm() - 1.0) <= 1.0e-12) ||
+      !(std::acos(-1.0) - phi_value.Norm() >
+        std::sqrt(std::numeric_limits<double>::epsilon()))) {
+    return Failure<DynamicRotationKinematics>(
+        "invalid-dynamic-rotation", {{}, 0U}, "DYNAMIC_ROTATION",
+        "Dynamic rotation requires finite principal coordinates and a unit "
+        "initial director.");
+  }
+  JetVector phi{};
+  for (std::size_t component = 0U; component < 3U; ++component) {
+    phi[component] = Variable(phi_value[component], component);
+  }
+  const JetVector initial{Constant(initial_director.X()),
+                          Constant(initial_director.Y()),
+                          Constant(initial_director.Z())};
+  const JetVector director_jet = Multiply(ExpSkew(phi), initial);
+  const JetMatrix left_jacobian = LeftJacobian(phi);
+  JetVector null_jet = Multiply(Inverse(left_jacobian), director_jet);
+  const Jet null_norm_squared = Dot(null_jet, null_jet);
+  if (!std::isfinite(null_norm_squared.value) ||
+      !(null_norm_squared.value > 0.0)) {
+    return Failure<DynamicRotationKinematics>(
+        "invalid-dynamic-rotation", {{}, 0U}, "DYNAMIC_ROTATION",
+        "Dynamic rotation produced a nonpositive drilling-null norm.");
+  }
+  const Jet inverse_norm = InverseSqrt(null_norm_squared);
+  for (Jet& component : null_jet) {
+    component = component * inverse_norm;
+  }
+  std::array<double, 3> directional_second{};
+  std::array<double, 3> angular_velocity{};
+  std::array<double, 3> angular_acceleration{};
+  Matrix gradient{3U, 3U};
+  for (std::size_t row = 0U; row < 3U; ++row) {
+    for (std::size_t column = 0U; column < 3U; ++column) {
+      gradient(row, column) = null_jet[row].d[column];
+    }
+  }
+  for (std::size_t row = 0U; row < 3U; ++row) {
+    double second = 0.0;
+    double omega = 0.0;
+    double omega_dot = 0.0;
+    for (std::size_t column = 0U; column < 3U; ++column) {
+      omega += left_jacobian[row][column].value * velocity[column];
+      omega_dot += left_jacobian[row][column].value * acceleration[column];
+      for (std::size_t other = 0U; other < 3U; ++other) {
+        second += null_jet[row].dd[column][other] * velocity[column] *
+                  velocity[other];
+        omega_dot += left_jacobian[row][column].d[other] * velocity[other] *
+                     velocity[column];
+      }
+    }
+    directional_second[row] = second;
+    angular_velocity[row] = omega;
+    angular_acceleration[row] = omega_dot;
+  }
+  const Vector3 director{director_jet[0U].value, director_jet[1U].value,
+                         director_jet[2U].value};
+  const Vector3 drill_null{null_jet[0U].value, null_jet[1U].value,
+                           null_jet[2U].value};
+  const Vector3 directional_second_value{directional_second};
+  const Vector3 angular_velocity_value{angular_velocity};
+  const Vector3 angular_acceleration_value{angular_acceleration};
+  if (!director.IsFinite() || !drill_null.IsFinite() ||
+      !directional_second_value.IsFinite() ||
+      !angular_velocity_value.IsFinite() ||
+      !angular_acceleration_value.IsFinite() || !IsFinite(gradient)) {
+    return Failure<DynamicRotationKinematics>(
+        "invalid-dynamic-rotation", {{}, 0U}, "DYNAMIC_ROTATION",
+        "Dynamic rotation evaluation must remain finite.");
+  }
+  DynamicRotationKinematics result{director,
+                                   drill_null,
+                                   std::move(gradient),
+                                   directional_second_value,
+                                   angular_velocity_value,
+                                   angular_acceleration_value,
+                                   {},
+                                   {},
+                                   {}};
+  for (std::size_t i = 0U; i < 3U; ++i) {
+    result.director_first[i] = {director_jet[0].d[i], director_jet[1].d[i],
+                                director_jet[2].d[i]};
+    for (std::size_t j = 0U; j < 3U; ++j) {
+      result.director_second[i][j] = {director_jet[0].dd[i][j],
+                                      director_jet[1].dd[i][j],
+                                      director_jet[2].dd[i][j]};
+      for (std::size_t k = 0U; k < 3U; ++k) {
+        result.director_third[i][j][k] = {director_jet[0].ddd[i][j][k],
+                                          director_jet[1].ddd[i][j][k],
+                                          director_jet[2].ddd[i][j][k]};
+      }
+    }
+  }
+  return Result<DynamicRotationKinematics>::Success(std::move(result));
+}
+
+NonlinearMitc4Inertia::NonlinearMitc4Inertia(
+    std::array<Vector3, 4> coordinates, std::array<Matrix, 4> initial_frames,
+    std::array<Vector3, 4> initial_directors, const double thickness,
+    const double density, SourceLocation source_location, std::string identity)
+    : coordinates_(std::move(coordinates)),
+      initial_frames_(std::move(initial_frames)),
+      initial_directors_(std::move(initial_directors)),
+      thickness_(thickness),
+      density_(density),
+      source_location_(std::move(source_location)),
+      identity_(std::move(identity)) {}
+
+Result<NonlinearMitc4Inertia> NonlinearMitc4Inertia::Create(
+    std::array<const Node*, 4> nodes,
+    std::array<ShellNodeInitialFrame, 4> initial_frames,
+    const ShellSection& section, const LinearElasticMaterial& material,
+    const double density) {
+  const std::string identity = Identity(nodes);
+  if (!std::isfinite(density) || !(density > 0.0)) {
+    return Failure<NonlinearMitc4Inertia>(
+        "invalid-nonlinear-inertia-density", section.location, identity,
+        "Nonlinear MITC4 inertia requires a finite positive density.");
+  }
+  const auto shell =
+      NonlinearMitc4Shell::Create(nodes, initial_frames, section, material);
+  if (!shell.HasValue()) {
+    return Result<NonlinearMitc4Inertia>::Failure(shell.GetStatus());
+  }
+  std::array<Vector3, kNodes> coordinates{};
+  std::array<Matrix, kNodes> frames{Matrix{3U, 3U}, Matrix{3U, 3U},
+                                    Matrix{3U, 3U}, Matrix{3U, 3U}};
+  std::array<Vector3, kNodes> directors{};
+  for (std::size_t node = 0U; node < kNodes; ++node) {
+    coordinates[node] = Vector3{nodes[node]->coordinates};
+    frames[node] = FrameMatrix(initial_frames[node]);
+    directors[node] = Vector3{initial_frames[node].director};
+  }
+  NonlinearMitc4Inertia inertia{
+      coordinates, std::move(frames),   directors, section.thickness,
+      density,     nodes[0U]->location, identity};
+  const auto initial = inertia.Evaluate({}, Vector{kDofs}, Vector{kDofs});
+  if (!initial.HasValue()) {
+    return Result<NonlinearMitc4Inertia>::Failure(initial.GetStatus());
+  }
+  return Result<NonlinearMitc4Inertia>::Success(std::move(inertia));
+}
+
+Result<NonlinearMitc4InertiaEvaluation> NonlinearMitc4Inertia::Evaluate(
+    const NonlinearMitc4TrialDofs& trial, const Vector& velocity,
+    const Vector& acceleration) const {
+  if (velocity.Size() != kDofs || acceleration.Size() != kDofs ||
+      !IsFinite(velocity) || !IsFinite(acceleration)) {
+    return Failure<NonlinearMitc4InertiaEvaluation>(
+        "invalid-nonlinear-inertia-rate", source_location_, identity_,
+        "Nonlinear MITC4 inertia requires 24 finite rates.");
+  }
+  std::array<DynamicRotationKinematics, kNodes> rotations{};
+  std::array<Vector3, kNodes> nulls{};
+  double length = 0.0;
+  for (std::size_t node = 0U; node < kNodes; ++node) {
+    length = std::max(
+        length,
+        (coordinates_[(node + 1U) % kNodes] - coordinates_[node]).Norm());
+    if (!trial.displacement[node].IsFinite() ||
+        !MakeFiniteRotationState(trial.rotation_vector[node],
+                                 initial_frames_[node])
+             .HasValue()) {
+      return Failure<NonlinearMitc4InertiaEvaluation>(
+          "invalid-nonlinear-inertia-trial", source_location_, identity_,
+          "Nonlinear MITC4 inertia trial must be in the principal chart.");
+    }
+    const auto rotation = EvaluateDynamicRotation(
+        trial.rotation_vector[node], initial_directors_[node], {}, {});
+    if (!rotation.HasValue()) {
+      return Result<NonlinearMitc4InertiaEvaluation>::Failure(
+          rotation.GetStatus());
+    }
+    rotations[node] = rotation.Value();
+    nulls[node] = rotation.Value().drill_null;
+    for (std::size_t previous = 0U; previous < node; ++previous) {
+      if (!(rotations[node].director.Dot(rotations[previous].director) >
+            -1.0 + 1.0e-8)) {
+        return Failure<NonlinearMitc4InertiaEvaluation>(
+            "invalid-nonlinear-inertia-director", source_location_, identity_,
+            "Nonlinear MITC4 inertia trial directors must not be opposed.");
+      }
+    }
+  }
+  if (!std::isfinite(length) || !(length > 0.0)) {
+    return Failure<NonlinearMitc4InertiaEvaluation>(
+        "invalid-nonlinear-inertia-geometry", source_location_, identity_,
+        "Nonlinear MITC4 inertia requires a finite positive initial edge.");
+  }
+
+  NonlinearMitc4InertiaEvaluation result{Matrix{kDofs, kDofs},
+                                         Matrix{kDofs, kDofs},
+                                         Matrix{kDofs, kDofs},
+                                         Vector{kDofs},
+                                         Vector{kDofs},
+                                         0.0,
+                                         {},
+                                         {},
+                                         {},
+                                         {}};
+  for (std::size_t xi = 0U; xi < kGauss.size(); ++xi) {
+    for (std::size_t eta = 0U; eta < kGauss.size(); ++eta) {
+      const Mitc4ShapeFunctions shape =
+          Mitc4Shell::ShapeFunctions(kGauss[xi], kGauss[eta]);
+      for (std::size_t zeta = 0U; zeta < kGauss.size(); ++zeta) {
+        const double z = 0.5 * thickness_ * kGauss[zeta];
+        const double weight = kWeights[xi] * kWeights[eta] * kWeights[zeta];
+        Vector3 ref_xi{};
+        Vector3 ref_eta{};
+        Vector3 ref_zeta{};
+        Vector3 current_xi{};
+        Vector3 current_eta{};
+        Vector3 current_zeta{};
+        Vector3 position{};
+        Vector3 velocity_point{};
+        Vector3 acceleration_point{};
+        Vector3 convective_point{};
+        std::array<Vector3, kDofs> h{};
+        std::array<Vector3, kDofs> velocity_derivative{};
+        std::array<Vector3, kDofs> position_derivative{};
+        for (std::size_t node = 0U; node < kNodes; ++node) {
+          const std::size_t base = 6U * node;
+          const auto& rotation = rotations[node];
+          const Vector3 reference =
+              coordinates_[node] + z * initial_directors_[node];
+          const Vector3 current = coordinates_[node] +
+                                  trial.displacement[node] +
+                                  z * rotation.director;
+          ref_xi = ref_xi + shape.xi_derivatives[node] * reference;
+          ref_eta = ref_eta + shape.eta_derivatives[node] * reference;
+          ref_zeta = ref_zeta + 0.5 * thickness_ * shape.values[node] *
+                                    initial_directors_[node];
+          current_xi = current_xi + shape.xi_derivatives[node] * current;
+          current_eta = current_eta + shape.eta_derivatives[node] * current;
+          current_zeta = current_zeta + 0.5 * thickness_ * shape.values[node] *
+                                            rotation.director;
+          position = position + shape.values[node] * current;
+          for (std::size_t component = 0U; component < 3U; ++component) {
+            std::array<double, 3> translation{};
+            translation[component] = shape.values[node];
+            h[base + component] = Vector3{translation};
+            h[base + 3U + component] =
+                z * shape.values[node] * rotation.director_first[component];
+            for (std::size_t other = 0U; other < 3U; ++other) {
+              const Vector3 second = z * shape.values[node] *
+                                     rotation.director_second[component][other];
+              convective_point =
+                  convective_point + second * velocity[base + 3U + component] *
+                                         velocity[base + 3U + other];
+              velocity_derivative[base + 3U + component] =
+                  velocity_derivative[base + 3U + component] +
+                  2.0 * second * velocity[base + 3U + other];
+              position_derivative[base + 3U + component] =
+                  position_derivative[base + 3U + component] +
+                  second * acceleration[base + 3U + other];
+              for (std::size_t third = 0U; third < 3U; ++third) {
+                position_derivative[base + 3U + component] =
+                    position_derivative[base + 3U + component] +
+                    z * shape.values[node] *
+                        rotation.director_third[component][other][third] *
+                        velocity[base + 3U + other] *
+                        velocity[base + 3U + third];
+              }
+            }
+          }
+        }
+        const double jacobian = Determinant(ref_xi, ref_eta, ref_zeta);
+        const double current_jacobian =
+            Determinant(current_xi, current_eta, current_zeta);
+        if (!std::isfinite(jacobian) || !(jacobian > 0.0) ||
+            !std::isfinite(current_jacobian) || !(current_jacobian > 0.0)) {
+          return Failure<NonlinearMitc4InertiaEvaluation>(
+              "invalid-nonlinear-inertia-geometry", source_location_, identity_,
+              "Inertia requires positive reference and current mass-point "
+              "Jacobians.");
+        }
+        for (std::size_t row = 0U; row < kDofs; ++row) {
+          velocity_point = velocity_point + h[row] * velocity[row];
+          acceleration_point = acceleration_point + h[row] * acceleration[row];
+        }
+        acceleration_point = acceleration_point + convective_point;
+        const double scale = density_ * weight * jacobian;
+        if (!std::isfinite(scale) || !(scale > 0.0)) {
+          return Failure<NonlinearMitc4InertiaEvaluation>(
+              "invalid-nonlinear-inertia-positive-mass", source_location_,
+              identity_,
+              "Mass-point integration weight must be finite and positive.");
+        }
+        result.kinetic_energy +=
+            0.5 * scale * velocity_point.Dot(velocity_point);
+        result.momentum = result.momentum + scale * velocity_point;
+        result.momentum_rate =
+            result.momentum_rate + scale * acceleration_point;
+        result.angular_momentum =
+            result.angular_momentum + scale * position.Cross(velocity_point);
+        result.angular_momentum_rate =
+            result.angular_momentum_rate +
+            scale * position.Cross(acceleration_point);
+        result.momentum_rate_scale += scale * acceleration_point.Norm();
+        result.angular_momentum_rate_scale +=
+            scale * position.Cross(acceleration_point).Norm();
+        for (std::size_t row = 0U; row < kDofs; ++row) {
+          result.force[row] += scale * h[row].Dot(acceleration_point);
+          // Keep c independent of a; subtracting M*a destroys small c values.
+          result.convective_force[row] += scale * h[row].Dot(convective_point);
+          for (std::size_t column = 0U; column < kDofs; ++column) {
+            result.mass(row, column) += scale * h[row].Dot(h[column]);
+            result.velocity_tangent(row, column) +=
+                scale * h[row].Dot(velocity_derivative[column]);
+            double position_entry = h[row].Dot(position_derivative[column]);
+            if (row / 6U == column / 6U && row % 6U >= 3U &&
+                column % 6U >= 3U) {
+              const std::size_t node = row / 6U;
+              position_entry +=
+                  z * shape.values[node] *
+                  rotations[node]
+                      .director_second[row % 6U - 3U][column % 6U - 3U]
+                      .Dot(acceleration_point);
+            }
+            result.position_tangent(row, column) += scale * position_entry;
+          }
+        }
+      }
+    }
+  }
+  if (!IsFinite(result.mass) || !IsFinite(result.position_tangent) ||
+      !IsFinite(result.velocity_tangent) || !IsFinite(result.force) ||
+      !IsFinite(result.convective_force) ||
+      !std::isfinite(result.kinetic_energy) || !result.momentum.IsFinite() ||
+      !result.angular_momentum.IsFinite() || !result.momentum_rate.IsFinite() ||
+      !result.angular_momentum_rate.IsFinite() ||
+      !std::isfinite(result.momentum_rate_scale) ||
+      !std::isfinite(result.angular_momentum_rate_scale)) {
+    return Failure<NonlinearMitc4InertiaEvaluation>(
+        "invalid-nonlinear-inertia-evaluation", source_location_, identity_,
+        "Nonlinear MITC4 inertia evaluation must remain finite.");
+  }
+  const Status physical_mass = CheckPhysicalMass(result.mass, nulls, length,
+                                                 source_location_, identity_);
+  if (!physical_mass.IsOk()) {
+    return Result<NonlinearMitc4InertiaEvaluation>::Failure(physical_mass);
+  }
+  return Result<NonlinearMitc4InertiaEvaluation>::Success(std::move(result));
+}
+
+}  // namespace fesa

src/fesa/elements/nonlinear_mitc4_shell.cpp

@@ -42,9 +42,8 @@ template <class T>
 Result<T> Failure(const std::string& code, const SourceLocation& location,
                   const std::string& identity, std::string message) {
   return Result<T>::Failure(Status::Failure(
-      FailureCategory::kModel,
-      {{Severity::kError, code, location, "*ELEMENT", identity,
-        std::move(message)}}));
+      FailureCategory::kModel, {{Severity::kError, code, location, "*ELEMENT",
+                                 identity, std::move(message)}}));
 }
 
 /// @brief Builds the stable source label concatenation used by diagnostics.
@@ -134,8 +133,7 @@ bool MakeLocalFrame(const Vector3& midsurface_xi, const Vector3& midsurface_eta,
   }
   const Vector3 e1_candidate = midsurface_xi - midsurface_xi.Dot(e3) * e3;
   const double e1_norm = e1_candidate.Norm();
-  if (!e1_candidate.IsFinite() || !std::isfinite(e1_norm) ||
-      !(e1_norm > 0.0)) {
+  if (!e1_candidate.IsFinite() || !std::isfinite(e1_norm) || !(e1_norm > 0.0)) {
     return false;
   }
   const Vector3 e1 = e1_candidate / e1_norm;
@@ -155,8 +153,7 @@ bool EvaluateGeometry(const std::array<Vector3, kNodeCount>& coordinates,
                       const double thickness, const double xi, const double eta,
                       const double zeta, Geometry& geometry) {
   const Mitc4ShapeFunctions shape = Mitc4Shell::ShapeFunctions(xi, eta);
-  const Vector3 reference_xi =
-      DerivativeSum(shape.xi_derivatives, coordinates);
+  const Vector3 reference_xi = DerivativeSum(shape.xi_derivatives, coordinates);
   const Vector3 reference_eta =
       DerivativeSum(shape.eta_derivatives, coordinates);
   const Vector3 initial_director_xi =
@@ -218,11 +215,10 @@ Tensor DirectStrain(const Geometry& geometry) {
   Tensor strain{};
   for (std::size_t row = 0U; row < 3U; ++row) {
     for (std::size_t column = 0U; column < 3U; ++column) {
-      strain[row][column] =
-          0.5 * (geometry.current_covariant[row].Dot(
-                     geometry.current_covariant[column]) -
-                 geometry.reference_covariant[row].Dot(
-                     geometry.reference_covariant[column]));
+      strain[row][column] = 0.5 * (geometry.current_covariant[row].Dot(
+                                       geometry.current_covariant[column]) -
+                                   geometry.reference_covariant[row].Dot(
+                                       geometry.reference_covariant[column]));
     }
   }
   strain[2U][2U] = 0.0;
@@ -278,17 +274,17 @@ EngineeringStress Stress(const EngineeringStrain& strain, const double youngs,
   const double shear = kShearCorrection * youngs / (2.0 * (1.0 + poisson));
   return {factor * (strain[0U] + poisson * strain[1U]),
           factor * (poisson * strain[0U] + strain[1U]),
-          factor * (1.0 - poisson) * 0.5 * strain[2U],
-          shear * strain[3U], shear * strain[4U]};
+          factor * (1.0 - poisson) * 0.5 * strain[2U], shear * strain[3U],
+          shear * strain[4U]};
 }
 
 /// @brief Evaluates direct strain at one mandatory edge-midpoint tying point.
-bool EvaluateTyingSample(const std::array<Vector3, kNodeCount>& coordinates,
-                         const std::array<Vector3, kNodeCount>& initial_directors,
-                         const std::array<Vector3, kNodeCount>& directors,
-                         const NonlinearMitc4TrialDofs& trial,
-                         const double thickness, const double xi,
-                         const double eta, Tensor& strain) {
+bool EvaluateTyingSample(
+    const std::array<Vector3, kNodeCount>& coordinates,
+    const std::array<Vector3, kNodeCount>& initial_directors,
+    const std::array<Vector3, kNodeCount>& directors,
+    const NonlinearMitc4TrialDofs& trial, const double thickness,
+    const double xi, const double eta, Tensor& strain) {
   Geometry geometry{};
   if (!EvaluateGeometry(coordinates, initial_directors, directors, trial,
                         thickness, xi, eta, 0.0, geometry)) {
@@ -314,12 +310,13 @@ bool EvaluatePoint(const std::array<Vector3, kNodeCount>& coordinates,
   Tensor covariant = DirectStrain(point.geometry);
   const Mitc4TyingWeights weights = Mitc4Shell::TyingWeights(xi, eta);
   covariant[0U][2U] = weights.xi_zeta[0U] * tying_samples[0U][0U][2U] +
-                       weights.xi_zeta[1U] * tying_samples[1U][0U][2U];
+                      weights.xi_zeta[1U] * tying_samples[1U][0U][2U];
   covariant[2U][0U] = covariant[0U][2U];
   covariant[1U][2U] = weights.eta_zeta[0U] * tying_samples[2U][1U][2U] +
-                       weights.eta_zeta[1U] * tying_samples[3U][1U][2U];
+                      weights.eta_zeta[1U] * tying_samples[3U][1U][2U];
   covariant[2U][1U] = covariant[1U][2U];
-  const Tensor cartesian = CartesianStrain(covariant, point.geometry.reciprocal);
+  const Tensor cartesian =
+      CartesianStrain(covariant, point.geometry.reciprocal);
   point.strain = LocalEngineering(cartesian, point.geometry.local_frame);
   point.stress = Stress(point.strain, youngs, poisson);
   return IsFinite(covariant) && IsFinite(cartesian) && IsFinite(point.strain) &&
@@ -372,6 +369,10 @@ struct Jet {
   double value{};
   std::array<double, kGlobalDofCount> first{};
   Hessian second;
+  double directional_first{};
+  double directional_second{};
+  std::array<double, kGlobalDofCount> mixed_first{};
+  std::array<double, kGlobalDofCount> mixed_second{};
 };
 
 using JetVector3 = std::array<Jet, 3>;
@@ -386,9 +387,11 @@ Jet Constant(const double value) {
 }
 
 /// @brief Returns one independent global trial coordinate.
-Jet Variable(const double value, const std::size_t index) {
+Jet Variable(const double value, const std::size_t index,
+             const double direction = 0.0) {
   Jet result = Constant(value);
   result.first[index] = 1.0;
+  result.directional_first = direction;
   return result;
 }
 
@@ -396,8 +399,15 @@ Jet Variable(const double value, const std::size_t index) {
 Jet operator+(const Jet& left, const Jet& right) {
   Jet result{};
   result.value = left.value + right.value;
+  result.directional_first = left.directional_first + right.directional_first;
+  result.directional_second =
+      left.directional_second + right.directional_second;
   for (std::size_t index = 0U; index < kGlobalDofCount; ++index) {
     result.first[index] = left.first[index] + right.first[index];
+    result.mixed_first[index] =
+        left.mixed_first[index] + right.mixed_first[index];
+    result.mixed_second[index] =
+        left.mixed_second[index] + right.mixed_second[index];
   }
   for (std::size_t index = 0U; index < kHessianSize; ++index) {
     result.second[index] = left.second[index] + right.second[index];
@@ -409,8 +419,15 @@ Jet operator+(const Jet& left, const Jet& right) {
 Jet operator-(const Jet& left, const Jet& right) {
   Jet result{};
   result.value = left.value - right.value;
+  result.directional_first = left.directional_first - right.directional_first;
+  result.directional_second =
+      left.directional_second - right.directional_second;
   for (std::size_t index = 0U; index < kGlobalDofCount; ++index) {
     result.first[index] = left.first[index] - right.first[index];
+    result.mixed_first[index] =
+        left.mixed_first[index] - right.mixed_first[index];
+    result.mixed_second[index] =
+        left.mixed_second[index] - right.mixed_second[index];
   }
   for (std::size_t index = 0U; index < kHessianSize; ++index) {
     result.second[index] = left.second[index] - right.second[index];
@@ -422,9 +439,26 @@ Jet operator-(const Jet& left, const Jet& right) {
 Jet operator*(const Jet& left, const Jet& right) {
   Jet result{};
   result.value = left.value * right.value;
+  result.directional_first = left.directional_first * right.value +
+                             left.value * right.directional_first;
+  result.directional_second =
+      left.directional_second * right.value +
+      2.0 * left.directional_first * right.directional_first +
+      left.value * right.directional_second;
   for (std::size_t index = 0U; index < kGlobalDofCount; ++index) {
     result.first[index] =
         left.first[index] * right.value + left.value * right.first[index];
+    result.mixed_first[index] = left.mixed_first[index] * right.value +
+                                left.first[index] * right.directional_first +
+                                left.directional_first * right.first[index] +
+                                left.value * right.mixed_first[index];
+    result.mixed_second[index] =
+        left.mixed_second[index] * right.value +
+        2.0 * left.mixed_first[index] * right.directional_first +
+        left.first[index] * right.directional_second +
+        left.directional_second * right.first[index] +
+        2.0 * left.directional_first * right.mixed_first[index] +
+        left.value * right.mixed_second[index];
   }
   for (std::size_t row = 0U; row < kGlobalDofCount; ++row) {
     for (std::size_t column = 0U; column < kGlobalDofCount; ++column) {
@@ -442,8 +476,12 @@ Jet operator*(const Jet& left, const Jet& right) {
 Jet operator*(const double scalar, const Jet& value) {
   Jet result{};
   result.value = scalar * value.value;
+  result.directional_first = scalar * value.directional_first;
+  result.directional_second = scalar * value.directional_second;
   for (std::size_t index = 0U; index < kGlobalDofCount; ++index) {
     result.first[index] = scalar * value.first[index];
+    result.mixed_first[index] = scalar * value.mixed_first[index];
+    result.mixed_second[index] = scalar * value.mixed_second[index];
   }
   for (std::size_t index = 0U; index < kHessianSize; ++index) {
     result.second[index] = scalar * value.second[index];
@@ -456,11 +494,24 @@ Jet operator*(const Jet& value, const double scalar) { return scalar * value; }
 
 /// @brief Applies a scalar unary function through its first two derivatives.
 Jet Compose(const Jet& input, const double value, const double first,
-            const double second) {
+            const double second, const double third) {
   Jet result{};
   result.value = value;
+  result.directional_first = first * input.directional_first;
+  result.directional_second =
+      second * input.directional_first * input.directional_first +
+      first * input.directional_second;
   for (std::size_t index = 0U; index < kGlobalDofCount; ++index) {
     result.first[index] = first * input.first[index];
+    result.mixed_first[index] =
+        second * input.first[index] * input.directional_first +
+        first * input.mixed_first[index];
+    result.mixed_second[index] =
+        third * input.first[index] * input.directional_first *
+            input.directional_first +
+        second * (2.0 * input.mixed_first[index] * input.directional_first +
+                  input.first[index] * input.directional_second) +
+        first * input.mixed_second[index];
   }
   for (std::size_t row = 0U; row < kGlobalDofCount; ++row) {
     for (std::size_t column = 0U; column < kGlobalDofCount; ++column) {
@@ -476,7 +527,8 @@ Jet Compose(const Jet& input, const double value, const double first,
 Jet Reciprocal(const Jet& value) {
   const double inverse = 1.0 / value.value;
   return Compose(value, inverse, -inverse * inverse,
-                 2.0 * inverse * inverse * inverse);
+                 2.0 * inverse * inverse * inverse,
+                 -6.0 * inverse * inverse * inverse * inverse);
 }
 
 /// @brief Divides two second-order trial-coordinate quantities.
@@ -487,19 +539,20 @@ Jet operator/(const Jet& left, const Jet& right) {
 /// @brief Computes the analytic square root variation.
 Jet Sqrt(const Jet& value) {
   const double root = std::sqrt(value.value);
-  return Compose(value, root, 0.5 / root, -0.25 / (root * root * root));
+  return Compose(value, root, 0.5 / root, -0.25 / (root * root * root),
+                 0.375 / (root * root * root * root * root));
 }
 
 /// @brief Computes the analytic sine variation.
 Jet Sin(const Jet& value) {
   return Compose(value, std::sin(value.value), std::cos(value.value),
-                 -std::sin(value.value));
+                 -std::sin(value.value), -std::cos(value.value));
 }
 
 /// @brief Computes the analytic cosine variation.
 Jet Cos(const Jet& value) {
   return Compose(value, std::cos(value.value), -std::sin(value.value),
-                 -std::cos(value.value));
+                 -std::cos(value.value), std::sin(value.value));
 }
 
 /// @brief Computes the two-argument angle and its analytic first/second map.
@@ -529,15 +582,40 @@ Jet Atan2(const Jet& y, const Jet& x) {
                              derivative_xx * x.first[row] * x.first[column];
     }
   }
+  // Use a bounded ratio for the directional third derivative only. Retain
+  // the established value/gradient/Hessian reduction above for static runs.
+  const bool use_x = std::abs(x.value) >= std::abs(y.value);
+  const Jet ratio = use_x ? y / x : x / y;
+  const double denominator = 1.0 + ratio.value * ratio.value;
+  const Jet angle = Compose(ratio, std::atan(ratio.value), 1.0 / denominator,
+                            -2.0 * ratio.value / (denominator * denominator),
+                            (6.0 * ratio.value * ratio.value - 2.0) /
+                                (denominator * denominator * denominator));
+  const double sign = use_x ? 1.0 : -1.0;
+  result.directional_first = sign * angle.directional_first;
+  result.directional_second = sign * angle.directional_second;
+  for (std::size_t i = 0U; i < kGlobalDofCount; ++i) {
+    result.mixed_first[i] = sign * angle.mixed_first[i];
+    result.mixed_second[i] = sign * angle.mixed_second[i];
+  }
   return result;
 }
 
 /// @brief Checks every coefficient in an automatic-differentiation quantity.
 bool IsFinite(const Jet& value) {
-  if (!std::isfinite(value.value)) {
+  if (!std::isfinite(value.value) || !std::isfinite(value.directional_first) ||
+      !std::isfinite(value.directional_second)) {
     return false;
   }
-  return std::all_of(value.first.begin(), value.first.end(),
+  return std::all_of(value.mixed_first.begin(), value.mixed_first.end(),
+                     [](const double coefficient) {
+                       return std::isfinite(coefficient);
+                     }) &&
+         std::all_of(value.mixed_second.begin(), value.mixed_second.end(),
+                     [](const double coefficient) {
+                       return std::isfinite(coefficient);
+                     }) &&
+         std::all_of(value.first.begin(), value.first.end(),
                      [](const double coefficient) {
                        return std::isfinite(coefficient);
                      }) &&
@@ -728,8 +806,39 @@ JetMatrix3 ExpSkew(const JetVector3& rotation) {
         (Constant(1.0) - Cos(norm)) / squared_norm;
   }
   const JetMatrix3 skew = Skew(rotation);
-  return Add(Add(Identity(), Scale(sine_over_norm, skew)),
-             Scale(one_minus_cosine_over_norm_squared, Multiply(skew, skew)));
+  JetMatrix3 result =
+      Add(Add(Identity(), Scale(sine_over_norm, skew)),
+          Scale(one_minus_cosine_over_norm_squared, Multiply(skew, skew)));
+  if (squared_norm.value < 0.25) {
+    // Third derivatives amplify cancellation in 1-cos(theta). Preserve the
+    // established static coefficients and use a stable series for the new
+    // directional derivatives of the complete rotation tensor.
+    Jet sine_series = Constant(1.0);
+    Jet cosine_series = Constant(0.5);
+    Jet power = Constant(1.0);
+    double sine_coefficient = 1.0;
+    double cosine_coefficient = 0.5;
+    for (unsigned int k = 1U; k <= 9U; ++k) {
+      power = power * squared_norm;
+      sine_coefficient /= -static_cast<double>((2U * k) * (2U * k + 1U));
+      cosine_coefficient /= -static_cast<double>((2U * k + 1U) * (2U * k + 2U));
+      sine_series = sine_series + sine_coefficient * power;
+      cosine_series = cosine_series + cosine_coefficient * power;
+    }
+    const JetMatrix3 stable = Add(Add(Identity(), Scale(sine_series, skew)),
+                                  Scale(cosine_series, Multiply(skew, skew)));
+    for (std::size_t row = 0U; row < 3U; ++row) {
+      for (std::size_t column = 0U; column < 3U; ++column) {
+        result[row][column].directional_first =
+            stable[row][column].directional_first;
+        result[row][column].directional_second =
+            stable[row][column].directional_second;
+        result[row][column].mixed_first = stable[row][column].mixed_first;
+        result[row][column].mixed_second = stable[row][column].mixed_second;
+      }
+    }
+  }
+  return result;
 }
 
 /// @brief Stores the complete Jet-valued global element trial coordinates.
@@ -741,14 +850,18 @@ struct JetTrial {
 
 /// @brief Lifts one validated nonlinear trial into independent Jet variables.
 JetTrial MakeJetTrial(const NonlinearMitc4TrialDofs& trial,
-                      const std::array<Matrix, kNodeCount>& initial_frames) {
+                      const std::array<Matrix, kNodeCount>& initial_frames,
+                      const Vector* direction) {
   JetTrial result{};
   for (std::size_t node = 0U; node < kNodeCount; ++node) {
     for (std::size_t component = 0U; component < 3U; ++component) {
-      result.displacement[node][component] =
-          Variable(trial.displacement[node][component], 6U * node + component);
+      result.displacement[node][component] = Variable(
+          trial.displacement[node][component], 6U * node + component,
+          direction == nullptr ? 0.0 : (*direction)[6U * node + component]);
       result.rotation[node][component] = Variable(
-          trial.rotation_vector[node][component], 6U * node + 3U + component);
+          trial.rotation_vector[node][component], 6U * node + 3U + component,
+          direction == nullptr ? 0.0
+                               : (*direction)[6U * node + 3U + component]);
     }
     JetMatrix3 frame{};
     for (std::size_t row = 0U; row < 3U; ++row) {
@@ -764,52 +877,42 @@ JetTrial MakeJetTrial(const NonlinearMitc4TrialDofs& trial,
   return result;
 }
 
-/// @brief Forms one current covariant basis from the exact trial variables.
-std::array<JetVector3, 3> CurrentCovariant(
-    const std::array<Vector3, kNodeCount>& coordinates, const JetTrial& trial,
-    const double thickness, const double xi, const double eta,
-    const double zeta) {
-  const Mitc4ShapeFunctions shape = Mitc4Shell::ShapeFunctions(xi, eta);
-  JetVector3 displacement_xi{};
-  JetVector3 displacement_eta{};
-  JetVector3 director{};
-  JetVector3 director_xi{};
-  JetVector3 director_eta{};
-  Vector3 reference_xi{};
-  Vector3 reference_eta{};
+/// @brief Forms covariant increments directly, preserving exact zero motion.
+std::array<JetVector3, 3> CovariantIncrement(
+    const std::array<Vector3, kNodeCount>& initial_directors,
+    const JetTrial& trial, const double thickness, const double xi,
+    const double eta, const double zeta) {
+  const auto shape = Mitc4Shell::ShapeFunctions(xi, eta);
+  std::array<JetVector3, 3> increment{};
   for (std::size_t node = 0U; node < kNodeCount; ++node) {
-    reference_xi =
-        reference_xi + shape.xi_derivatives[node] * coordinates[node];
-    reference_eta =
-        reference_eta + shape.eta_derivatives[node] * coordinates[node];
-    displacement_xi = Add(displacement_xi, Scale(shape.xi_derivatives[node],
-                                                 trial.displacement[node]));
-    displacement_eta = Add(displacement_eta, Scale(shape.eta_derivatives[node],
-                                                   trial.displacement[node]));
-    director = Add(director, Scale(shape.values[node], trial.director[node]));
-    director_xi = Add(director_xi,
-                      Scale(shape.xi_derivatives[node], trial.director[node]));
-    director_eta = Add(
-        director_eta, Scale(shape.eta_derivatives[node], trial.director[node]));
+    const auto displacement =
+        Subtract(trial.displacement[node], trial.displacement[0U]);
+    const auto director =
+        Subtract(trial.director[node], ConstantVector(initial_directors[node]));
+    const auto layer =
+        Add(displacement, Scale(0.5 * thickness * zeta, director));
+    increment[0U] =
+        Add(increment[0U], Scale(shape.xi_derivatives[node], layer));
+    increment[1U] =
+        Add(increment[1U], Scale(shape.eta_derivatives[node], layer));
+    increment[2U] = Add(increment[2U],
+                        Scale(0.5 * thickness * shape.values[node], director));
   }
-  const double half_thickness = 0.5 * thickness;
-  return {Add(Add(ConstantVector(reference_xi), displacement_xi),
-              Scale(half_thickness * zeta, director_xi)),
-          Add(Add(ConstantVector(reference_eta), displacement_eta),
-              Scale(half_thickness * zeta, director_eta)),
-          Scale(half_thickness, director)};
+  return increment;
 }
 
 /// @brief Forms the direct covariant Green--Lagrange strain tensor.
-JetTensor DirectStrain(const std::array<JetVector3, 3>& current,
+JetTensor DirectStrain(const std::array<JetVector3, 3>& increment,
                        const Geometry& reference) {
   JetTensor result{};
   for (std::size_t row = 0U; row < 3U; ++row) {
     for (std::size_t column = 0U; column < 3U; ++column) {
       result[row][column] =
-          0.5 * (Dot(current[row], current[column]) -
-                 Constant(reference.reference_covariant[row].Dot(
-                     reference.reference_covariant[column])));
+          0.5 * (Dot(ConstantVector(reference.reference_covariant[row]),
+                     increment[column]) +
+                 Dot(increment[row],
+                     ConstantVector(reference.reference_covariant[column])) +
+                 Dot(increment[row], increment[column]));
     }
   }
   result[2U][2U] = Constant(0.0);
@@ -867,9 +970,9 @@ Jet PhysicalEnergy(const std::array<Vector3, kNodeCount>& coordinates,
       return {};
     }
     tying[point] =
-        DirectStrain(CurrentCovariant(coordinates, trial, thickness,
-                                      tying_locations[point][0U],
-                                      tying_locations[point][1U], 0.0),
+        DirectStrain(CovariantIncrement(initial_directors, trial, thickness,
+                                        tying_locations[point][0U],
+                                        tying_locations[point][1U], 0.0),
                      reference);
   }
 
@@ -892,8 +995,8 @@ Jet PhysicalEnergy(const std::array<Vector3, kNodeCount>& coordinates,
         return {};
       }
       JetTensor covariant =
-          DirectStrain(CurrentCovariant(coordinates, trial, thickness,
-                                        surface[0U], surface[1U], zeta),
+          DirectStrain(CovariantIncrement(initial_directors, trial, thickness,
+                                          surface[0U], surface[1U], zeta),
                        reference);
       const Mitc4TyingWeights weights =
           Mitc4Shell::TyingWeights(surface[0U], surface[1U]);
@@ -940,11 +1043,41 @@ JetMatrix3 PolarRotation(const JetMatrix3& deformation) {
   return result;
 }
 
+/// @brief Returns the speed of physical spin for a unit chart null vector.
+Jet DrillSpinRate(const JetVector3& phi, const Vector3& director) {
+  const Jet square = Dot(phi, phi);
+  Jet coefficient{};
+  if (square.value < 0.25) {
+    constexpr std::array<double, 9> series{1.0 / 12.0,
+                                           1.0 / 720.0,
+                                           1.0 / 30240.0,
+                                           1.0 / 1209600.0,
+                                           1.0 / 47900160.0,
+                                           691.0 / 1307674368000.0,
+                                           1.0 / 74724249600.0,
+                                           3617.0 / 10670622842880000.0,
+                                           43867.0 / 5109094217170944000.0};
+    coefficient = Constant(series.back());
+    for (std::size_t i = series.size() - 1U; i > 0U; --i)
+      coefficient = Constant(series[i - 1U]) + square * coefficient;
+  } else {
+    const Jet half = 0.5 * Sqrt(square);
+    coefficient = (Constant(1.0) - half * Cos(half) * Reciprocal(Sin(half))) *
+                  Reciprocal(square);
+  }
+  const auto d0 = ConstantVector(director);
+  const auto cross = Cross(phi, d0);
+  const auto inverse_action =
+      Add(Add(d0, Scale(0.5, cross)), Scale(coefficient, Cross(phi, cross)));
+  return Reciprocal(Sqrt(Dot(inverse_action, inverse_action)));
+}
+
 /// @brief Evaluates the objective numerical drilling potential and variations.
 Jet DrillingEnergy(const std::array<Vector3, kNodeCount>& coordinates,
                    const std::array<Vector3, kNodeCount>& initial_directors,
                    const JetTrial& trial, const double thickness,
-                   const double drilling_stiffness, bool& valid) {
+                   const double drilling_stiffness, bool& valid, Matrix* metric,
+                   std::array<Jet, 4>* constraints) {
   const NonlinearMitc4TrialDofs reference_trial{};
   Geometry reference{};
   if (!EvaluateGeometry(coordinates, initial_directors, initial_directors,
@@ -953,8 +1086,8 @@ Jet DrillingEnergy(const std::array<Vector3, kNodeCount>& coordinates,
     return {};
   }
   const auto current =
-      CurrentCovariant(coordinates, trial, thickness, 0.0, 0.0, 0.0);
-  JetMatrix3 deformation{};
+      CovariantIncrement(initial_directors, trial, thickness, 0.0, 0.0, 0.0);
+  JetMatrix3 deformation = Identity();
   for (std::size_t row = 0U; row < 3U; ++row) {
     for (std::size_t column = 0U; column < 3U; ++column) {
       for (std::size_t alpha = 0U; alpha < 3U; ++alpha) {
@@ -991,7 +1124,19 @@ Jet DrillingEnergy(const std::array<Vector3, kNodeCount>& coordinates,
     const Jet cosine =
         0.5 * (twist[0U][0U] + twist[1U][1U] + twist[2U][2U] - Constant(1.0));
     const Jet gamma = Atan2(sine, cosine);
+    if (constraints != nullptr) {
+      // n^T grad(Uphys)=0 and Dgamma_j[n_i]=delta_ij/||Jl^-1 d||.
+      (*constraints)[node] =
+          drilling_stiffness *
+          DrillSpinRate(trial.rotation[node], initial_directors[node]) * gamma;
+    }
     energy = energy + 0.5 * drilling_stiffness * gamma * gamma;
+    for (std::size_t row = 0U; row < kGlobalDofCount; ++row) {
+      for (std::size_t column = 0U; column < kGlobalDofCount; ++column) {
+        (*metric)(row, column) +=
+            drilling_stiffness * gamma.first[row] * gamma.first[column];
+      }
+    }
   }
   if (!IsFinite(energy)) {
     valid = false;
@@ -1040,13 +1185,15 @@ Result<NonlinearMitc4Shell> NonlinearMitc4Shell::Create(
     for (std::size_t previous = 0U; previous < node; ++previous) {
       if (coordinates[node] == coordinates[previous]) {
         return Failure<NonlinearMitc4Shell>(
-            "invalid-nonlinear-shell-geometry", nodes[node]->location,
-            identity, "Nonlinear MITC4 nodes must have distinct coordinates.");
+            "invalid-nonlinear-shell-geometry", nodes[node]->location, identity,
+            "Nonlinear MITC4 nodes must have distinct coordinates.");
       }
-      if (initial_frames[node].node_index == initial_frames[previous].node_index) {
+      if (initial_frames[node].node_index ==
+          initial_frames[previous].node_index) {
         return Failure<NonlinearMitc4Shell>(
             "invalid-nonlinear-shell-frame", nodes[node]->location, identity,
-            "Nonlinear MITC4 initial frames must identify four distinct nodes.");
+            "Nonlinear MITC4 initial frames must identify four distinct "
+            "nodes.");
       }
     }
   }
@@ -1089,7 +1236,8 @@ Result<NonlinearMitc4Shell> NonlinearMitc4Shell::Create(
                   })) {
     return Failure<NonlinearMitc4Shell>(
         "invalid-nonlinear-shell-frame", nodes[0U]->location, identity,
-        "Nonlinear MITC4 initial directors must follow the source positive face.");
+        "Nonlinear MITC4 initial directors must follow the source positive "
+        "face.");
   }
 
   if (!std::isfinite(section.thickness) || !(section.thickness > 0.0)) {
@@ -1125,14 +1273,14 @@ Result<NonlinearMitc4Shell> NonlinearMitc4Shell::Create(
   }
 
   NonlinearMitc4Shell shell{coordinates,
-                             std::move(frames),
-                             directors,
-                             section.thickness,
-                             material.youngs_modulus,
-                             material.poisson_ratio,
-                             linear_stiffness.Value().drilling_stiffness,
-                             nodes[0U]->location,
-                             identity};
+                            std::move(frames),
+                            directors,
+                            section.thickness,
+                            material.youngs_modulus,
+                            material.poisson_ratio,
+                            linear_stiffness.Value().drilling_stiffness,
+                            nodes[0U]->location,
+                            identity};
   const auto reference = shell.EvaluateKinematics(NonlinearMitc4TrialDofs{});
   if (!reference.HasValue()) {
     return Result<NonlinearMitc4Shell>::Failure(reference.GetStatus());
@@ -1150,12 +1298,13 @@ Result<NonlinearMitc4Recovery> NonlinearMitc4Shell::EvaluateKinematics(
           "invalid-nonlinear-shell-trial", source_location_, identity_,
           "Nonlinear MITC4 trial displacements and rotations must be finite.");
     }
-    const auto state =
-        MakeFiniteRotationState(trial.rotation_vector[node], initial_frames_[node]);
+    const auto state = MakeFiniteRotationState(trial.rotation_vector[node],
+                                               initial_frames_[node]);
     if (!state.HasValue()) {
       return Failure<NonlinearMitc4Recovery>(
           "invalid-nonlinear-shell-rotation", source_location_, identity_,
-          "Nonlinear MITC4 trial rotations must remain in the principal chart.");
+          "Nonlinear MITC4 trial rotations must remain in the principal "
+          "chart.");
     }
     directors[node] = state.Value().director;
   }
@@ -1185,10 +1334,12 @@ Result<NonlinearMitc4Recovery> NonlinearMitc4Shell::EvaluateKinematics(
   for (std::size_t point = 0U; point < tying_locations.size(); ++point) {
     if (!EvaluateTyingSample(coordinates_, initial_directors_, directors, trial,
                              thickness_, tying_locations[point][0U],
-                             tying_locations[point][1U], tying_samples[point])) {
+                             tying_locations[point][1U],
+                             tying_samples[point])) {
       return Failure<NonlinearMitc4Recovery>(
           "invalid-nonlinear-shell-geometry", source_location_, identity_,
-          "Nonlinear MITC4 tying-point reference or trial geometry is invalid.");
+          "Nonlinear MITC4 tying-point reference or trial geometry is "
+          "invalid.");
     }
   }
 
@@ -1198,8 +1349,7 @@ Result<NonlinearMitc4Recovery> NonlinearMitc4Shell::EvaluateKinematics(
       ShellMidsurfaceLocation::kGp3, ShellMidsurfaceLocation::kGp4};
   const std::array<std::array<double, 2>, 4> surface_points{
       std::array<double, 2>{-gauss, -gauss},
-      std::array<double, 2>{gauss, -gauss},
-      std::array<double, 2>{gauss, gauss},
+      std::array<double, 2>{gauss, -gauss}, std::array<double, 2>{gauss, gauss},
       std::array<double, 2>{-gauss, gauss}};
   constexpr std::array<double, 2> thickness_gauss{-1.0, 1.0};
   constexpr std::array<double, 3> stress_zeta{-1.0, 0.0, 1.0};
@@ -1236,12 +1386,12 @@ Result<NonlinearMitc4Recovery> NonlinearMitc4Shell::EvaluateKinematics(
       for (std::size_t component = 0U; component < 3U; ++component) {
         row.generalized_strain[component] += 0.5 * point.strain[component];
         row.generalized_strain[3U + component] +=
-            3.0 * thickness_sign * gauss * point.strain[component] /
-            thickness_;
+            3.0 * thickness_sign * gauss * point.strain[component] / thickness_;
         row.section_resultant[component] +=
             0.5 * thickness_ * point.stress[component];
-        row.section_resultant[3U + component] +=
-            0.25 * thickness_ * thickness_sign * gauss * point.stress[component];
+        row.section_resultant[3U + component] += 0.25 * thickness_ *
+                                                 thickness_sign * gauss *
+                                                 point.stress[component];
       }
       for (std::size_t component = 0U; component < 2U; ++component) {
         row.generalized_strain[6U + component] +=
@@ -1269,9 +1419,10 @@ Result<NonlinearMitc4Recovery> NonlinearMitc4Shell::EvaluateKinematics(
             "invalid-nonlinear-shell-geometry", source_location_, identity_,
             "Nonlinear MITC4 section-position recovery geometry is invalid.");
       }
-      row.stress[position] = {stress_position[position], stress_zeta[position],
-                              {point.stress[0U], point.stress[1U],
-                               point.stress[2U]}};
+      row.stress[position] = {
+          stress_position[position],
+          stress_zeta[position],
+          {point.stress[0U], point.stress[1U], point.stress[2U]}};
     }
   }
 
@@ -1289,7 +1440,22 @@ Result<NonlinearMitc4Recovery> NonlinearMitc4Shell::RecoverState(
 }
 
 Result<NonlinearMitc4InternalEquilibrium> EvaluateInternalResidualAndTangent(
-    const NonlinearMitc4Shell& element, const NonlinearMitc4TrialDofs& trial) {
+    const NonlinearMitc4Shell& element, const NonlinearMitc4TrialDofs& trial,
+    const Vector* coordinate_direction) {
+  if (coordinate_direction != nullptr) {
+    if (coordinate_direction->Size() != kGlobalDofCount) {
+      return Failure<NonlinearMitc4InternalEquilibrium>(
+          "invalid-nonlinear-shell-equilibrium", element.source_location_,
+          element.identity_, "The directional derivative requires 24 DOFs.");
+    }
+    for (std::size_t i = 0U; i < kGlobalDofCount; ++i) {
+      if (!std::isfinite((*coordinate_direction)[i])) {
+        return Failure<NonlinearMitc4InternalEquilibrium>(
+            "invalid-nonlinear-shell-equilibrium", element.source_location_,
+            element.identity_, "The directional derivative must be finite.");
+      }
+    }
+  }
   const auto recovery = element.EvaluateKinematics(trial);
   if (!recovery.HasValue()) {
     return Result<NonlinearMitc4InternalEquilibrium>::Failure(
@@ -1297,14 +1463,18 @@ Result<NonlinearMitc4InternalEquilibrium> EvaluateInternalResidualAndTangent(
   }
 
   bool valid = true;
-  const JetTrial jet_trial = MakeJetTrial(trial, element.initial_frames_);
+  const JetTrial jet_trial =
+      MakeJetTrial(trial, element.initial_frames_, coordinate_direction);
   const Jet physical =
       PhysicalEnergy(element.coordinates_, element.initial_directors_,
                      jet_trial, element.thickness_, element.youngs_modulus_,
                      element.poisson_ratio_, valid);
+  Matrix drilling_metric{kGlobalDofCount, kGlobalDofCount};
+  std::array<Jet, 4> constraints{};
   const Jet drilling = DrillingEnergy(
       element.coordinates_, element.initial_directors_, jet_trial,
-      element.thickness_, element.drilling_stiffness_, valid);
+      element.thickness_, element.drilling_stiffness_, valid, &drilling_metric,
+      coordinate_direction != nullptr ? &constraints : nullptr);
   const Jet internal_energy = physical + drilling;
   if (!valid || !IsFinite(internal_energy)) {
     return Failure<NonlinearMitc4InternalEquilibrium>(
@@ -1344,8 +1514,43 @@ Result<NonlinearMitc4InternalEquilibrium> EvaluateInternalResidualAndTangent(
       std::move(internal_residual), std::move(internal_tangent),
       Vector{kGlobalDofCount}, recovery.Value(),
       recovery.Value().physical_strain_energy};
+  if (coordinate_direction != nullptr) {
+    equilibrium.drilling_constraint = Vector{4U};
+    equilibrium.drilling_constraint_gradient = Matrix{4U, kGlobalDofCount};
+    for (std::size_t node = 0U; node < 4U; ++node) {
+      if (!IsFinite(constraints[node]))
+        return Failure<NonlinearMitc4InternalEquilibrium>(
+            "invalid-nonlinear-shell-equilibrium", element.source_location_,
+            element.identity_,
+            "Drilling constraint derivatives must remain finite.");
+      equilibrium.drilling_constraint[node] = constraints[node].value;
+      equilibrium.drilling_constraint_hessian[node] =
+          Matrix{kGlobalDofCount, kGlobalDofCount};
+      for (std::size_t i = 0U; i < kGlobalDofCount; ++i) {
+        equilibrium.drilling_constraint_gradient(node, i) =
+            constraints[node].first[i];
+        for (std::size_t j = 0U; j < kGlobalDofCount; ++j)
+          equilibrium.drilling_constraint_hessian[node](i, j) =
+              constraints[node].second[i * kGlobalDofCount + j];
+      }
+    }
+  }
+  equilibrium.physical_force = Vector{kGlobalDofCount};
+  equilibrium.drilling_force = Vector{kGlobalDofCount};
+  equilibrium.drilling_tangent = Matrix{kGlobalDofCount, kGlobalDofCount};
+  equilibrium.drilling_metric = std::move(drilling_metric);
+  equilibrium.drilling_energy = drilling.value;
+  equilibrium.force_directional_second = Vector{kGlobalDofCount};
   for (std::size_t index = 0U; index < kGlobalDofCount; ++index) {
     equilibrium.internal_force[index] = equilibrium.internal_residual[index];
+    equilibrium.physical_force[index] = physical.first[index];
+    equilibrium.drilling_force[index] = drilling.first[index];
+    equilibrium.force_directional_second[index] =
+        internal_energy.mixed_second[index];
+    for (std::size_t column = 0U; column < kGlobalDofCount; ++column) {
+      equilibrium.drilling_tangent(index, column) =
+          drilling.second[index * kGlobalDofCount + column];
+    }
   }
   return Result<NonlinearMitc4InternalEquilibrium>::Success(
       std::move(equilibrium));