feat(nonlinear-static-newton-raphson): step 2 - finite-rotation-state

src/fesa/CMakeLists.txt

@@ -17,6 +17,7 @@ add_library(
     core/status.cpp
     elements/element_factory.cpp
     elements/euler_beam_3d.cpp
+    elements/finite_rotation.cpp
     elements/mitc4_shell.cpp
     fem/dof_manager.cpp
     io/abaqus/domain_builder.cpp

src/fesa/elements/finite_rotation.cpp

@@ -0,0 +1,286 @@
+#include "fesa/elements/finite_rotation.h"
+
+#include <cmath>
+#include <cstddef>
+#include <limits>
+#include <string>
+#include <utility>
+
+namespace fesa {
+namespace {
+
+constexpr std::size_t kRotationDimension = 3U;
+constexpr double kRotationStateTolerance = 1.0e-12;
+constexpr double kSmallAngleThreshold = 1.0e-4;
+
+/// @brief Returns the SO(3) skew-symmetric matrix for a global vector.
+Matrix Skew(const Vector3& vector) {
+  Matrix result{kRotationDimension, kRotationDimension};
+  result(0U, 1U) = -vector.Z();
+  result(0U, 2U) = vector.Y();
+  result(1U, 0U) = vector.Z();
+  result(1U, 2U) = -vector.X();
+  result(2U, 0U) = -vector.Y();
+  result(2U, 1U) = vector.X();
+  return result;
+}
+
+/// @brief Returns the 3-by-3 identity matrix.
+Matrix Identity() {
+  Matrix result{kRotationDimension, kRotationDimension};
+  for (std::size_t index = 0U; index < kRotationDimension; ++index) {
+    result(index, index) = 1.0;
+  }
+  return result;
+}
+
+/// @brief Builds I plus linear and quadratic skew terms in stable row-major
+/// order.
+Matrix Polynomial(const Matrix& skew, const Matrix& skew_squared,
+                  const double linear_coefficient,
+                  const double quadratic_coefficient) {
+  Matrix result = Identity();
+  for (std::size_t row = 0U; row < kRotationDimension; ++row) {
+    for (std::size_t column = 0U; column < kRotationDimension; ++column) {
+      result(row, column) += linear_coefficient * skew(row, column) +
+                             quadratic_coefficient *
+                                 skew_squared(row, column);
+    }
+  }
+  return result;
+}
+
+/// @brief Computes the determinant of one 3-by-3 matrix.
+double Determinant(const Matrix& matrix) {
+  return matrix(0U, 0U) *
+             (matrix(1U, 1U) * matrix(2U, 2U) -
+              matrix(1U, 2U) * matrix(2U, 1U)) -
+         matrix(0U, 1U) *
+             (matrix(1U, 0U) * matrix(2U, 2U) -
+              matrix(1U, 2U) * matrix(2U, 0U)) +
+         matrix(0U, 2U) *
+             (matrix(1U, 0U) * matrix(2U, 1U) -
+              matrix(1U, 1U) * matrix(2U, 0U));
+}
+
+/// @brief Checks that all matrix entries are finite.
+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;
+}
+
+/// @brief Checks the exact 3-by-3 proper-orthogonal frame contract.
+bool IsProperOrthogonal(const Matrix& matrix) {
+  if (matrix.Rows() != kRotationDimension ||
+      matrix.Columns() != kRotationDimension || !IsFinite(matrix)) {
+    return false;
+  }
+
+  for (std::size_t row = 0U; row < kRotationDimension; ++row) {
+    for (std::size_t column = 0U; column < kRotationDimension; ++column) {
+      double value = 0.0;
+      for (std::size_t index = 0U; index < kRotationDimension; ++index) {
+        value += matrix(index, row) * matrix(index, column);
+      }
+      if (!std::isfinite(value) ||
+          std::abs(value - (row == column ? 1.0 : 0.0)) >
+              kRotationStateTolerance) {
+        return false;
+      }
+    }
+  }
+
+  const double determinant = Determinant(matrix);
+  return std::isfinite(determinant) &&
+         std::abs(determinant - 1.0) <= kRotationStateTolerance;
+}
+
+/// @brief Checks that the coordinate is finite and away from the pi branch.
+bool IsPrincipalCoordinate(const Vector3& phi) {
+  if (!phi.IsFinite()) {
+    return false;
+  }
+
+  const double angle = phi.Norm();
+  const double pi = std::acos(-1.0);
+  const double margin = std::sqrt(std::numeric_limits<double>::epsilon());
+  return std::isfinite(angle) && pi - angle > margin;
+}
+
+/// @brief Multiplies a 3-by-3 frame by a global vector.
+Vector3 Multiply(const Matrix& matrix, const Vector3& vector) {
+  return Vector3{
+      matrix(0U, 0U) * vector.X() + matrix(0U, 1U) * vector.Y() +
+          matrix(0U, 2U) * vector.Z(),
+      matrix(1U, 0U) * vector.X() + matrix(1U, 1U) * vector.Y() +
+          matrix(1U, 2U) * vector.Z(),
+      matrix(2U, 0U) * vector.X() + matrix(2U, 1U) * vector.Y() +
+          matrix(2U, 2U) * vector.Z()};
+}
+
+/// @brief Checks componentwise agreement without scale-dependent repair.
+bool IsNear(const Matrix& actual, const Matrix& expected) {
+  if (actual.Rows() != expected.Rows() ||
+      actual.Columns() != expected.Columns()) {
+    return false;
+  }
+  for (std::size_t row = 0U; row < actual.Rows(); ++row) {
+    for (std::size_t column = 0U; column < actual.Columns(); ++column) {
+      if (!std::isfinite(actual(row, column)) ||
+          !std::isfinite(expected(row, column)) ||
+          std::abs(actual(row, column) - expected(row, column)) >
+              kRotationStateTolerance) {
+        return false;
+      }
+    }
+  }
+  return true;
+}
+
+/// @brief Checks componentwise agreement for a derived director.
+bool IsNear(const Vector3& actual, const Vector3& expected) {
+  return actual.IsFinite() && expected.IsFinite() &&
+         std::abs(actual.X() - expected.X()) <= kRotationStateTolerance &&
+         std::abs(actual.Y() - expected.Y()) <= kRotationStateTolerance &&
+         std::abs(actual.Z() - expected.Z()) <= kRotationStateTolerance;
+}
+
+/// @brief Returns the stable failure used when a nonlinear state is invalid.
+Result<FiniteRotationState> InvalidAcceptedState(const std::string& message) {
+  return Result<FiniteRotationState>::Failure(Status::Failure(
+      FailureCategory::kSolver,
+      {{Severity::kError, "invalid-accepted-state", {{}, 0U},
+        "FINITE_ROTATION", "", message}}));
+}
+
+/// @brief Validates that an existing state remains derived from its coordinate.
+bool IsConsistentState(const FiniteRotationState& state,
+                       const Matrix& initial_frame) {
+  if (!IsPrincipalCoordinate(state.phi) || !IsProperOrthogonal(state.q) ||
+      !state.director.IsFinite()) {
+    return false;
+  }
+
+  const Matrix expected_q = ExpSkew(state.phi).Multiply(initial_frame);
+  return IsNear(state.q, expected_q) &&
+         IsNear(state.director, Multiply(state.q, Vector3{0.0, 0.0, 1.0}));
+}
+
+}  // namespace
+
+Matrix ExpSkew(const Vector3& phi) {
+  const Matrix skew = Skew(phi);
+  const Matrix skew_squared = skew.Multiply(skew);
+  const double theta = phi.Norm();
+  const double theta_squared = theta * theta;
+
+  double linear_coefficient = 1.0;
+  double quadratic_coefficient = 0.5;
+  if (theta >= kSmallAngleThreshold) {
+    linear_coefficient = std::sin(theta) / theta;
+    quadratic_coefficient = (1.0 - std::cos(theta)) / theta_squared;
+  } else {
+    const double theta_fourth = theta_squared * theta_squared;
+    const double theta_sixth = theta_fourth * theta_squared;
+    linear_coefficient =
+        1.0 - theta_squared / 6.0 + theta_fourth / 120.0 -
+        theta_sixth / 5040.0;
+    quadratic_coefficient =
+        0.5 - theta_squared / 24.0 + theta_fourth / 720.0 -
+        theta_sixth / 40320.0;
+  }
+  return Polynomial(skew, skew_squared, linear_coefficient,
+                    quadratic_coefficient);
+}
+
+Matrix LeftJacobianSO3(const Vector3& phi) {
+  const Matrix skew = Skew(phi);
+  const Matrix skew_squared = skew.Multiply(skew);
+  const double theta = phi.Norm();
+  const double theta_squared = theta * theta;
+
+  double linear_coefficient = 0.5;
+  double quadratic_coefficient = 1.0 / 6.0;
+  if (theta >= kSmallAngleThreshold) {
+    linear_coefficient = (1.0 - std::cos(theta)) / theta_squared;
+    quadratic_coefficient = (theta - std::sin(theta)) /
+                            (theta_squared * theta);
+  } else {
+    const double theta_fourth = theta_squared * theta_squared;
+    const double theta_sixth = theta_fourth * theta_squared;
+    linear_coefficient =
+        0.5 - theta_squared / 24.0 + theta_fourth / 720.0 -
+        theta_sixth / 40320.0;
+    quadratic_coefficient =
+        1.0 / 6.0 - theta_squared / 120.0 + theta_fourth / 5040.0 -
+        theta_sixth / 362880.0;
+  }
+  return Polynomial(skew, skew_squared, linear_coefficient,
+                    quadratic_coefficient);
+}
+
+Matrix InverseTransposeLeftJacobianSO3(const Vector3& phi) {
+  const Matrix skew = Skew(phi);
+  const Matrix skew_squared = skew.Multiply(skew);
+  const double theta = phi.Norm();
+  const double theta_squared = theta * theta;
+
+  double quadratic_coefficient = 1.0 / 12.0;
+  if (theta >= kSmallAngleThreshold) {
+    const double half_theta = 0.5 * theta;
+    quadratic_coefficient =
+        (1.0 - half_theta / std::tan(half_theta)) / theta_squared;
+  } else {
+    const double theta_fourth = theta_squared * theta_squared;
+    const double theta_sixth = theta_fourth * theta_squared;
+    quadratic_coefficient =
+        1.0 / 12.0 + theta_squared / 720.0 + theta_fourth / 30240.0 +
+        theta_sixth / 1209600.0;
+  }
+  return Polynomial(skew, skew_squared, 0.5, quadratic_coefficient);
+}
+
+Result<FiniteRotationState> MakeFiniteRotationState(
+    const Vector3& phi, const Matrix& initial_frame) {
+  if (!IsPrincipalCoordinate(phi)) {
+    return InvalidAcceptedState(
+        "Finite rotation coordinates must be finite and remain inside the "
+        "principal branch margin.");
+  }
+  if (!IsProperOrthogonal(initial_frame)) {
+    return InvalidAcceptedState(
+        "The initial finite-rotation frame must be a finite proper "
+        "orthogonal triad.");
+  }
+
+  Matrix q = ExpSkew(phi).Multiply(initial_frame);
+  const Vector3 director = Multiply(q, Vector3{0.0, 0.0, 1.0});
+  if (!IsProperOrthogonal(q) || !director.IsFinite()) {
+    return InvalidAcceptedState(
+        "Finite rotation state construction produced an invalid triad.");
+  }
+  return Result<FiniteRotationState>::Success(
+      FiniteRotationState{phi, std::move(q), director});
+}
+
+Result<FiniteRotationState> ApplyGlobalRotationIncrement(
+    const FiniteRotationState& committed, const Vector3& delta_phi,
+    const Matrix& initial_frame) {
+  if (!IsProperOrthogonal(initial_frame) ||
+      !IsConsistentState(committed, initial_frame) ||
+      !delta_phi.IsFinite()) {
+    return InvalidAcceptedState(
+        "The accepted finite rotation state and its correction must remain "
+        "finite and consistent with the initial frame.");
+  }
+
+  return MakeFiniteRotationState(committed.phi + delta_phi, initial_frame);
+}
+
+}  // namespace fesa