Connect MITC4 drilling evidence across analysis procedures

src/fesa/analysis/hht_dynamic_assembly.cpp

@@ -18,6 +18,7 @@
 #include "fesa/elements/finite_rotation.h"
 #include "fesa/loads/load.h"
 #include "fesa/model/source_target_resolver.h"
+#include "results/mitc4_drilling_evidence.h"
 
 namespace fesa::hht_internal {
 
@@ -498,6 +499,10 @@ Result<DynamicAssembly> EvaluateAssembly(
     internal[e] = std::move(f.Value());
     inertia[e] = std::move(m.Value());
   });
+  if (shells.empty())
+    return Result<DynamicAssembly>::Failure(Failure("HHT requires a shell kernel."));
+  frame.mitc4_drilling =
+      Mitc4DrillingEvidence{shells.front().internal_kernel->DrillingMode()};
   frame.inertia_force = Vector(count);
   frame.internal_force = Vector(count);
   frame.residual = Vector(count);
@@ -515,6 +520,7 @@ Result<DynamicAssembly> EvaluateAssembly(
   std::vector<std::map<std::size_t, double>>
       constraint_gradients(domain.Nodes().size());
   std::vector<bool> external_done(domain.Nodes().size(), false);
+  Vector hidden_constraint(directional_third ? domain.Nodes().size() : 0U);
   Vector third(count);
   Vector third_scale(count);
   std::array<std::vector<CooContribution>, 6> entries;
@@ -532,9 +538,31 @@ Result<DynamicAssembly> EvaluateAssembly(
       return Result<DynamicAssembly>::Failure(*failures[e]);
     const auto& f = *internal[e];
     const auto& m = *inertia[e];
+    if (shells[e].internal_kernel->DrillingMode() != frame.mitc4_drilling->mode ||
+        f.drilling_force.Size() != 24U)
+      return Result<DynamicAssembly>::Failure(Failure("Invalid actual drilling mode or force."));
+    for (std::size_t node = 0U; node < 4U; ++node) {
+      const auto local = 6U * node;
+      const auto global = shells[e].scatter[local];
+      const Vector3 force = Triple(f.drilling_force, local);
+      const auto spatial = SpatialMomentSO3(
+          Triple(frame.generalized_coordinates, global + 3U),
+          Triple(f.drilling_force, local + 3U));
+      if (!spatial.HasValue())
+        return Result<DynamicAssembly>::Failure(spatial.GetStatus());
+      const Vector3 position = Vector3(domain.Nodes()[global / 6U].coordinates) +
+                               Triple(frame.generalized_coordinates, global);
+      const Vector3 moment = position.Cross(force) + spatial.Value();
+      for (std::size_t c = 0U; c < 3U; ++c)
+        frame.mitc4_drilling->drilling_moment[c] += moment[c];
+    }
+    for (const double value : frame.mitc4_drilling->drilling_moment)
+      if (!std::isfinite(value))
+        return Result<DynamicAssembly>::Failure(Failure("Drilling spatial moment overflowed."));
     if (directional_third) {
       for (std::size_t local = 0U; local < 4U; ++local) {
         const auto node = shells[e].scatter[6U * local] / 6U;
+        hidden_constraint[node] += f.drilling_constraint[local];
         auto& gradient = constraint_gradients[node];
         auto& hessian = constraint_hessians[node];
         for (std::size_t i = 0U; i < 24U; ++i)
@@ -555,6 +583,7 @@ Result<DynamicAssembly> EvaluateAssembly(
                   Triple(frame.external_force, first), {}, {});
           if (!external.HasValue())
             return Result<DynamicAssembly>::Failure(external.GetStatus());
+          hidden_constraint[node] += external.Value().value;
           for (std::size_t i = 0U; i < 3U; ++i) {
             gradient[first + i] += external.Value().gradient[i];
             for (std::size_t j = 0U; j < 3U; ++j)
@@ -740,7 +769,8 @@ Result<DynamicAssembly> EvaluateAssembly(
        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(trial_constraints), std::move(field_flux)});
+       std::move(trial_constraints), std::move(field_flux),
+       std::move(hidden_constraint)});
 }
 
 Status CompleteEquilibrium(const Domain& domain, const DofManager& dofs,
@@ -776,10 +806,20 @@ Status CompleteEquilibrium(const Domain& domain, const DofManager& dofs,
     frame->global_equilibrium[c] = force_error[c];
     frame->global_equilibrium[3U + c] = moment_error[c];
   }
+  Vector3 acceptance_moment = moment_error;
+  if (frame->mitc4_drilling) {
+    results_internal::CompleteDrillingEvidence(frame->global_equilibrium,
+                                               &*frame->mitc4_drilling);
+    if (!results_internal::ValidDrillingEvidence(frame->mitc4_drilling,
+                                                 frame->global_equilibrium))
+      return Failure("Invalid dynamic drilling equilibrium evidence.");
+    if (frame->mitc4_drilling->mode == Mitc4DrillingMode::kDirectSpring)
+      acceptance_moment = moment_error - Vector3(frame->mitc4_drilling->drilling_moment);
+  }
   if (!force_error.IsFinite() || !moment_error.IsFinite() ||
       !std::isfinite(force_scale) || !std::isfinite(moment_scale) ||
       Ratio(force_error.Norm(), force_scale) > 1e-8 ||
-      Ratio(moment_error.Norm(), moment_scale) > 1e-8)
+      Ratio(acceptance_moment.Norm(), moment_scale) > 1e-8)
     return Failure("Dynamic physical force or moment balance exceeds 1e-8.");
   return Status::Ok();
 }