From ee6f380bb3168a33bfb229e663b98f862039b8a2 Mon Sep 17 00:00:00 2001 From: "KOKO\\Mimi" Date: Mon, 24 Aug 2026 13:26:00 +0900 Subject: [PATCH] feat(nonlinear-static-newton-raphson): step 2 - finite-rotation-state --- include/fesa/elements/finite_rotation.h | 63 ++++ src/fesa/CMakeLists.txt | 1 + src/fesa/elements/finite_rotation.cpp | 286 +++++++++++++++++++ tests/CMakeLists.txt | 1 + tests/unit/elements/finite_rotation_test.cpp | 229 +++++++++++++++ 5 files changed, 580 insertions(+) create mode 100644 include/fesa/elements/finite_rotation.h create mode 100644 src/fesa/elements/finite_rotation.cpp create mode 100644 tests/unit/elements/finite_rotation_test.cpp diff --git a/include/fesa/elements/finite_rotation.h b/include/fesa/elements/finite_rotation.h new file mode 100644 index 0000000..29513b5 --- /dev/null +++ b/include/fesa/elements/finite_rotation.h @@ -0,0 +1,63 @@ +#ifndef FESA_ELEMENTS_FINITE_ROTATION_H_ +#define FESA_ELEMENTS_FINITE_ROTATION_H_ + +#include "fesa/core/status.h" +#include "fesa/math/matrix.h" +#include "fesa/math/vector3.h" + +namespace fesa { + +/// @brief Stores one finite-rotation coordinate and its derived triad state. +struct FiniteRotationState { + /// @brief Fixed-global principal-chart rotation coordinate in radians. + Vector3 phi; + + /// @brief Proper orthogonal triad computed as exp(phi^) times the initial + /// frame. + Matrix q; + + /// @brief Current director computed from the third column of q. + Vector3 director; +}; + +/// @brief Computes Rodrigues' exponential of the three-dimensional skew map. +/// @param phi Fixed-global rotation coordinate in radians. +/// @return The 3-by-3 rotation tensor exp(phi^). +/// @pre phi is finite and lies inside the approved principal chart. +Matrix ExpSkew(const Vector3& phi); + +/// @brief Computes the SO(3) left Jacobian for a fixed-global coordinate. +/// @param phi Fixed-global rotation coordinate in radians. +/// @return The 3-by-3 left Jacobian that maps coordinate variations to spatial +/// rotations. +/// @pre phi is finite and lies inside the approved principal chart. +Matrix LeftJacobianSO3(const Vector3& phi); + +/// @brief Computes the inverse transpose of the SO(3) left Jacobian. +/// @param phi Fixed-global rotation coordinate in radians. +/// @return The 3-by-3 inverse-transpose left Jacobian. +/// @pre phi is finite and lies inside the approved principal chart. +Matrix InverseTransposeLeftJacobianSO3(const Vector3& phi); + +/// @brief Builds a checked finite-rotation state from its reference frame. +/// @param phi Fixed-global principal-chart rotation coordinate in radians. +/// @param initial_frame Valid initial right-handed 3-by-3 triad. +/// @return A complete finite state or an `invalid-accepted-state` failure. +/// @note The input frame is validated but never normalized or repaired. +Result MakeFiniteRotationState( + const Vector3& phi, const Matrix& initial_frame); + +/// @brief Applies a fixed-global Newton rotation increment to one state. +/// @param committed Fully validated state that remains unchanged by this call. +/// @param delta_phi Fixed-global additive rotation correction in radians. +/// @param initial_frame Valid initial right-handed 3-by-3 triad. +/// @return A rebuilt trial state or an `invalid-accepted-state` failure. +/// @note The result is recomputed from phi plus delta_phi and initial_frame; +/// incremental tensors are not successively composed. +Result ApplyGlobalRotationIncrement( + const FiniteRotationState& committed, const Vector3& delta_phi, + const Matrix& initial_frame); + +} // namespace fesa + +#endif // FESA_ELEMENTS_FINITE_ROTATION_H_ diff --git a/src/fesa/CMakeLists.txt b/src/fesa/CMakeLists.txt index 4d41a7f..725a8b1 100644 --- a/src/fesa/CMakeLists.txt +++ b/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 diff --git a/src/fesa/elements/finite_rotation.cpp b/src/fesa/elements/finite_rotation.cpp new file mode 100644 index 0000000..4e82390 --- /dev/null +++ b/src/fesa/elements/finite_rotation.cpp @@ -0,0 +1,286 @@ +#include "fesa/elements/finite_rotation.h" + +#include +#include +#include +#include +#include + +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::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 InvalidAcceptedState(const std::string& message) { + return Result::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 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::Success( + FiniteRotationState{phi, std::move(q), director}); +} + +Result 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 diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index da05c4c..e7444f5 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -18,6 +18,7 @@ add_executable( unit/core/status_test.cpp unit/elements/element_factory_test.cpp unit/elements/euler_beam_3d_test.cpp + unit/elements/finite_rotation_test.cpp unit/elements/mitc4_shell_test.cpp unit/fem/dof_manager_test.cpp unit/math/dense_blas_internal_test.cpp diff --git a/tests/unit/elements/finite_rotation_test.cpp b/tests/unit/elements/finite_rotation_test.cpp new file mode 100644 index 0000000..913ec79 --- /dev/null +++ b/tests/unit/elements/finite_rotation_test.cpp @@ -0,0 +1,229 @@ +#include "fesa/elements/finite_rotation.h" + +#include + +#include +#include +#include +#include + +namespace fesa { +namespace { + +constexpr std::size_t kRotationDimension = 3U; + +Matrix InitialFrame() { + Matrix frame{kRotationDimension, kRotationDimension}; + frame(0U, 0U) = 1.0; + frame(1U, 2U) = -1.0; + frame(2U, 1U) = 1.0; + return frame; +} + +Matrix Transpose(const Matrix& matrix) { + Matrix result{matrix.Columns(), matrix.Rows()}; + for (std::size_t row = 0U; row < matrix.Rows(); ++row) { + for (std::size_t column = 0U; column < matrix.Columns(); ++column) { + result(column, row) = matrix(row, column); + } + } + return result; +} + +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()}; +} + +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)); +} + +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; +} + +void ExpectMatrixNear(const Matrix& actual, const Matrix& expected, + const double tolerance = 1.0e-12) { + ASSERT_EQ(actual.Rows(), expected.Rows()); + ASSERT_EQ(actual.Columns(), expected.Columns()); + for (std::size_t row = 0U; row < actual.Rows(); ++row) { + for (std::size_t column = 0U; column < actual.Columns(); ++column) { + EXPECT_NEAR(actual(row, column), expected(row, column), tolerance) + << "matrix entry (" << row << ", " << column << ")"; + } + } +} + +void ExpectVectorNear(const Vector3& actual, const Vector3& expected, + const double tolerance = 1.0e-12) { + EXPECT_NEAR(actual.X(), expected.X(), tolerance); + EXPECT_NEAR(actual.Y(), expected.Y(), tolerance); + EXPECT_NEAR(actual.Z(), expected.Z(), tolerance); +} + +void ExpectProperOrthogonal(const Matrix& rotation) { + const Matrix orthogonality = Transpose(rotation).Multiply(rotation); + ASSERT_EQ(orthogonality.Rows(), kRotationDimension); + ASSERT_EQ(orthogonality.Columns(), kRotationDimension); + for (std::size_t row = 0U; row < kRotationDimension; ++row) { + for (std::size_t column = 0U; column < kRotationDimension; ++column) { + EXPECT_NEAR(orthogonality(row, column), row == column ? 1.0 : 0.0, + 1.0e-12); + } + } + EXPECT_NEAR(Determinant(rotation), 1.0, 1.0e-12); +} + +TEST(FiniteRotation, ZeroCoordinatePreservesInitialFrameAndDirector) { + const Matrix initial_frame = InitialFrame(); + + const Result result = + MakeFiniteRotationState(Vector3{}, initial_frame); + + ASSERT_TRUE(result.HasValue()); + EXPECT_EQ(result.Value().phi, Vector3{}); + ExpectMatrixNear(result.Value().q, initial_frame); + ExpectVectorNear(result.Value().director, + Multiply(initial_frame, Vector3{0.0, 0.0, 1.0})); +} + +TEST(FiniteRotation, FiniteCoordinateBuildsProperTriadAndDirector) { + const Matrix initial_frame = InitialFrame(); + const Vector3 phi{0.3, -0.4, 0.5}; + + const Result result = + MakeFiniteRotationState(phi, initial_frame); + + ASSERT_TRUE(result.HasValue()); + const Matrix expected_q = ExpSkew(phi).Multiply(initial_frame); + ExpectMatrixNear(result.Value().q, expected_q); + ExpectProperOrthogonal(result.Value().q); + ExpectVectorNear(result.Value().director, + Multiply(result.Value().q, Vector3{0.0, 0.0, 1.0})); +} + +TEST(FiniteRotation, GlobalIncrementRebuildsFixedReferenceOrientation) { + const Matrix initial_frame = InitialFrame(); + const Vector3 phi{0.2, -0.1, 0.3}; + const Vector3 delta_phi{-0.05, 0.15, 0.1}; + const Result committed = + MakeFiniteRotationState(phi, initial_frame); + ASSERT_TRUE(committed.HasValue()); + + const Result updated = + ApplyGlobalRotationIncrement(committed.Value(), delta_phi, initial_frame); + + ASSERT_TRUE(updated.HasValue()); + const Vector3 expected_phi = phi + delta_phi; + EXPECT_EQ(updated.Value().phi, expected_phi); + ExpectMatrixNear(updated.Value().q, + ExpSkew(expected_phi).Multiply(initial_frame)); + ExpectVectorNear(updated.Value().director, + Multiply(updated.Value().q, Vector3{0.0, 0.0, 1.0})); + ExpectMatrixNear(committed.Value().q, ExpSkew(phi).Multiply(initial_frame)); +} + +TEST(FiniteRotation, RejectsCoordinatesInsidePrincipalBranchMargin) { + const Matrix initial_frame = InitialFrame(); + const double pi = std::acos(-1.0); + const double margin = std::sqrt(std::numeric_limits::epsilon()); + + EXPECT_TRUE(MakeFiniteRotationState(Vector3{pi - 2.0 * margin, 0.0, 0.0}, + initial_frame) + .HasValue()); + const Result ambiguous = + MakeFiniteRotationState(Vector3{pi - margin, 0.0, 0.0}, initial_frame); + ASSERT_FALSE(ambiguous.HasValue()); + ASSERT_FALSE(ambiguous.GetStatus().IsOk()); + ASSERT_EQ(ambiguous.GetStatus().Diagnostics().size(), 1U); + EXPECT_EQ(ambiguous.GetStatus().Diagnostics()[0U].code, + "invalid-accepted-state"); +} + +TEST(FiniteRotation, + JacobiansAreFiniteInverseTransposeAndDerivativeConsistent) { + const double pi = std::acos(-1.0); + const std::array rotations{ + Vector3{1.0e-9, -2.0e-9, 3.0e-9}, Vector3{0.4, -0.2, 0.3}, + Vector3{0.0, pi - 1.0e-3, 0.0}}; + const Vector3 direction{0.2, -0.3, 0.5}; + const double perturbation = 1.0e-7; + + for (const Vector3& phi : rotations) { + const Matrix left_jacobian = LeftJacobianSO3(phi); + const Matrix inverse_transpose = InverseTransposeLeftJacobianSO3(phi); + EXPECT_TRUE(IsFinite(left_jacobian)); + EXPECT_TRUE(IsFinite(inverse_transpose)); + + const Matrix identity = + Transpose(left_jacobian).Multiply(inverse_transpose); + for (std::size_t row = 0U; row < kRotationDimension; ++row) { + for (std::size_t column = 0U; column < kRotationDimension; ++column) { + EXPECT_NEAR(identity(row, column), row == column ? 1.0 : 0.0, + 1.0e-11); + } + } + + const Vector3 director = Multiply(ExpSkew(phi), Vector3{0.0, 0.0, 1.0}); + const Vector3 positive = + Multiply(ExpSkew(phi + perturbation * direction), + Vector3{0.0, 0.0, 1.0}); + const Vector3 negative = + Multiply(ExpSkew(phi - perturbation * direction), + Vector3{0.0, 0.0, 1.0}); + const Vector3 observed = (positive - negative) / (2.0 * perturbation); + const Vector3 expected = + Multiply(left_jacobian, direction).Cross(director); + ExpectVectorNear(observed, expected, 1.0e-8); + } +} + +TEST(FiniteRotation, RejectsNonfiniteCoordinatesAndInvalidInitialFrames) { + const Matrix initial_frame = InitialFrame(); + const double infinity = std::numeric_limits::infinity(); + const Result nonfinite = + MakeFiniteRotationState(Vector3{infinity, 0.0, 0.0}, initial_frame); + ASSERT_FALSE(nonfinite.HasValue()); + ASSERT_EQ(nonfinite.GetStatus().Diagnostics().size(), 1U); + EXPECT_EQ(nonfinite.GetStatus().Diagnostics()[0U].code, + "invalid-accepted-state"); + + Matrix invalid_frame = initial_frame; + invalid_frame(0U, 0U) = 2.0; + EXPECT_FALSE(MakeFiniteRotationState(Vector3{}, invalid_frame).HasValue()); + + const Result committed = + MakeFiniteRotationState(Vector3{}, initial_frame); + ASSERT_TRUE(committed.HasValue()); + const Result invalid_increment = + ApplyGlobalRotationIncrement(committed.Value(), + Vector3{0.0, infinity, 0.0}, initial_frame); + ASSERT_FALSE(invalid_increment.HasValue()); + ASSERT_EQ(invalid_increment.GetStatus().Diagnostics().size(), 1U); + EXPECT_EQ(invalid_increment.GetStatus().Diagnostics()[0U].code, + "invalid-accepted-state"); +} + +} // namespace +} // namespace fesa