완성된 HHT trial의 constraint 자료 중복 평가 제거

src/fesa/analysis/hht_dynamic_assembly.cpp

@@ -1,4 +1,5 @@
 #include <algorithm>
+#include <cfenv>
 #include <cmath>
 #include <limits>
 #include <map>
@@ -6,6 +7,10 @@
 #include <string>
 #include <utility>
 
+#if defined(_M_X64)
+#include <xmmintrin.h>
+#endif
+
 #include "analysis/hht_dynamic_internal.h"
 #include "math/dense_blas_internal.h"
 #include "fesa/assembly/parallel_for.h"
@@ -14,6 +19,29 @@
 #include "fesa/model/source_target_resolver.h"
 
 namespace fesa::hht_internal {
+
+TrialEvaluationContext ReadTrialEvaluationContext() noexcept {
+  TrialEvaluationContext result{};
+#if defined(_M_X64)
+  result.rounding = std::fegetround();
+  result.mxcsr_control = _mm_getcsr() & 0xFFC0U;
+  result.dense_policy = dense_blas_internal::ReadScopedLocalDensePolicy();
+  result.supported = result.rounding >= 0;
+#endif
+  return result;
+}
+
+bool ReusableTrialContext(const TrialEvaluationContext& before,
+                          const TrialEvaluationContext& after) noexcept {
+  return before.supported && after.supported &&
+         before.rounding == after.rounding &&
+         before.mxcsr_control == after.mxcsr_control &&
+         (before.mxcsr_control & 0x1F80U) == 0x1F80U &&
+         before.dense_policy.blas_max_threads == 1 &&
+         after.dense_policy.blas_max_threads == 1 &&
+         before.dense_policy.dynamic == 0 && after.dense_policy.dynamic == 0;
+}
+
 namespace {
 
 /// @brief Reports a dynamic assembly or equilibrium failure.
@@ -388,6 +416,10 @@ Result<DynamicAssembly> EvaluateAssembly(
   std::vector<std::optional<NonlinearMitc4InertiaEvaluation>> inertia(
       shells.size());
   std::vector<std::optional<Status>> failures(shells.size());
+  const bool retain_constraints =
+      directional_third && !frame.is_initial && std::isfinite(frame.time);
+  std::vector<TrialEvaluationContext> evaluation_contexts(
+      retain_constraints ? shells.size() : 0U);
   parallel_for.Execute(shells.size(), [&](const std::size_t e) {
     const auto q = Gather(frame.generalized_coordinates, shells[e]);
     const auto v = Gather(frame.velocity, shells[e]);
@@ -397,12 +429,18 @@ Result<DynamicAssembly> EvaluateAssembly(
       trial.displacement[node] = Triple(q, 6U * node);
       trial.rotation_vector[node] = Triple(q, 6U * node + 3U);
     }
+    const auto before = retain_constraints ? ReadTrialEvaluationContext()
+                                           : TrialEvaluationContext{};
     auto f = EvaluateInternalResidualAndTangent(
         shells[e].internal, trial, directional_third ? &v : nullptr);
     if (!f.HasValue()) {
       failures[e] = f.GetStatus();
       return;
     }
+    if (retain_constraints) {
+      const auto after = ReadTrialEvaluationContext();
+      if (ReusableTrialContext(before, after)) evaluation_contexts[e] = after;
+    }
     auto m = shells[e].inertia.Evaluate(trial, v, a);
     if (!m.HasValue()) {
       failures[e] = m.GetStatus();
@@ -605,13 +643,52 @@ Result<DynamicAssembly> EvaluateAssembly(
       return Result<DynamicAssembly>::Failure(gradient.GetStatus());
     hidden_gradient = std::move(gradient.Value());
   }
+  std::optional<TrialConstraintData> trial_constraints;
+  if (retain_constraints) {
+    TrialConstraintData data{};
+    data.source_path = domain.SourcePath();
+    data.time = frame.time;
+    data.frame_index = frame.frame_index;
+    data.increment_index = frame.increment_index;
+    data.iteration_count = frame.iteration_count;
+    data.shells.resize(shells.size());
+    for (std::size_t e = 0U; e < shells.size(); ++e) {
+      if (!ReusableTrialContext(evaluation_contexts[e], evaluation_contexts[e]))
+        continue;
+      auto& record = data.shells[e];
+      record.element_index = shells[e].element_index;
+      record.scatter = shells[e].scatter;
+      record.configuration =
+          nonlinear_mitc4_internal::ConfigurationSnapshot::Read(
+              shells[e].internal);
+      record.evaluation_context = evaluation_contexts[e];
+      for (std::size_t i = 0U; i < 24U; ++i) {
+        record.q[i] = frame.generalized_coordinates[shells[e].scatter[i]];
+        record.v[i] = frame.velocity[shells[e].scatter[i]];
+      }
+      for (std::size_t node = 0U; node < 4U; ++node)
+        record.node_source_ids[node] =
+            domain.Nodes()[shells[e].scatter[6U * node] / 6U].source_id;
+      auto& response = *internal[e];
+      record.drilling_constraint = std::move(response.drilling_constraint);
+      record.drilling_constraint_gradient =
+          std::move(response.drilling_constraint_gradient);
+      record.drilling_constraint_hessian =
+          std::move(response.drilling_constraint_hessian);
+      record.drilling_constraint_third_directional_gradient =
+          std::move(response.drilling_constraint_third_directional_gradient);
+      record.directional_internal = true;
+    }
+    trial_constraints = std::move(data);
+  }
   return Result<DynamicAssembly>::Success(
       {std::move(frame), std::move(mass.Value()), std::move(position.Value()),
        std::move(velocity.Value()), std::move(stiffness.Value()),
        std::move(effective.Value()), std::move(metric.Value()),
        std::move(third), std::move(third_scale), std::move(rotations),
        momentum_scale, angular_scale, std::move(hidden_second),
-       std::move(hidden_second_scale), std::move(hidden_gradient)});
+       std::move(hidden_second_scale), std::move(hidden_gradient),
+       std::move(trial_constraints)});
 }
 
 Status CompleteEquilibrium(const Domain& domain, const DofManager& dofs,