feat(nonlinear-static-newton-raphson): step 11 - equilibrium-projection-and-reference-gate

src/fesa/analysis/nonlinear_static_procedure.cpp

@@ -1,5 +1,6 @@
 #include "fesa/analysis/nonlinear_static_procedure.h"
 
+#include <algorithm>
 #include <array>
 #include <cmath>
 #include <cstddef>
@@ -501,23 +502,101 @@ Vector CurrentIncrementComponents(const Vector& current, const Vector& start,
   return SelectFreeComponents(difference, dofs, rotations);
 }
 
-std::array<double, 6> GlobalEquilibrium(const Domain& domain,
-                                        const Vector& residual) {
-  std::array<double, 6> equilibrium{};
+Vector3 SpatialMoment(const Matrix& transform,
+                      const std::array<double, 3>& generalized) {
+  return {transform(0U, 0U) * generalized[0U] +
+              transform(0U, 1U) * generalized[1U] +
+              transform(0U, 2U) * generalized[2U],
+          transform(1U, 0U) * generalized[0U] +
+              transform(1U, 1U) * generalized[1U] +
+              transform(1U, 2U) * generalized[2U],
+          transform(2U, 0U) * generalized[0U] +
+              transform(2U, 1U) * generalized[1U] +
+              transform(2U, 2U) * generalized[2U]};
+}
+
+/// @brief Computes accepted-state applied-plus-reaction balance evidence.
+Result<std::array<double, 6>> GlobalEquilibrium(
+    const Domain& domain, const DofManager& dofs,
+    const NonlinearTrialState& trial) {
+  const std::size_t full_dof_count = domain.Nodes().size() * kDofsPerNode;
+  if (trial.displacement.Size() != full_dof_count ||
+      trial.rotation_vector.Size() != full_dof_count ||
+      trial.external_force.Size() != full_dof_count ||
+      trial.residual.Size() != full_dof_count) {
+    return Result<std::array<double, 6>>::Failure(ProcedureFailure(
+        FailureCategory::kSolver, "nonfinite-newton-data",
+        {domain.SourcePath(), 0U}, "NONLINEAR_STATIC_PROCEDURE",
+        domain.SourceContentIdentity(),
+        "Nonlinear equilibrium vectors do not match the Domain node inventory."));
+  }
+
+  std::vector<bool> constrained(full_dof_count, false);
+  for (const std::size_t full_dof : dofs.ConstrainedDofs()) {
+    if (full_dof >= constrained.size()) {
+      return Result<std::array<double, 6>>::Failure(ProcedureFailure(
+          FailureCategory::kSolver, "nonfinite-newton-data",
+          {domain.SourcePath(), 0U}, "NONLINEAR_STATIC_PROCEDURE",
+          domain.SourceContentIdentity(),
+          "Nonlinear constraint identity is outside the full DOF range."));
+    }
+    constrained[full_dof] = true;
+  }
+
+  Vector3 applied_force_resultant;
+  Vector3 applied_moment_resultant;
+  Vector3 reaction_force_resultant;
+  Vector3 reaction_moment_resultant;
   for (std::size_t node = 0U; node < domain.Nodes().size(); ++node) {
     const std::size_t first = node * kDofsPerNode;
-    const Vector3 force{residual[first], residual[first + 1U],
-                        residual[first + 2U]};
-    const Vector3 position{domain.Nodes()[node].coordinates};
-    const Vector3 moment = position.Cross(force);
-    equilibrium[0U] += force.X();
-    equilibrium[1U] += force.Y();
-    equilibrium[2U] += force.Z();
-    equilibrium[3U] += moment.X() + residual[first + 3U];
-    equilibrium[4U] += moment.Y() + residual[first + 4U];
-    equilibrium[5U] += moment.Z() + residual[first + 5U];
+    const Vector3 position = Vector3{domain.Nodes()[node].coordinates} +
+                             Vector3{trial.displacement[first],
+                                     trial.displacement[first + 1U],
+                                     trial.displacement[first + 2U]};
+    const Vector3 phi{trial.rotation_vector[first + 3U],
+                      trial.rotation_vector[first + 4U],
+                      trial.rotation_vector[first + 5U]};
+    const Vector3 applied_force{trial.external_force[first],
+                                trial.external_force[first + 1U],
+                                trial.external_force[first + 2U]};
+    const Vector3 reaction_force{
+        constrained[first] ? trial.residual[first] : 0.0,
+        constrained[first + 1U] ? trial.residual[first + 1U] : 0.0,
+        constrained[first + 2U] ? trial.residual[first + 2U] : 0.0};
+    const Matrix transform = InverseTransposeLeftJacobianSO3(phi);
+    const std::array<double, 3> applied_generalized{
+        trial.external_force[first + 3U], trial.external_force[first + 4U],
+        trial.external_force[first + 5U]};
+    const std::array<double, 3> reaction_generalized{
+        constrained[first + 3U] ? trial.residual[first + 3U] : 0.0,
+        constrained[first + 4U] ? trial.residual[first + 4U] : 0.0,
+        constrained[first + 5U] ? trial.residual[first + 5U] : 0.0};
+    const Vector3 applied_moment = SpatialMoment(transform, applied_generalized);
+    const Vector3 reaction_moment =
+        SpatialMoment(transform, reaction_generalized);
+    applied_force_resultant = applied_force_resultant + applied_force;
+    applied_moment_resultant =
+        applied_moment_resultant + position.Cross(applied_force) + applied_moment;
+    reaction_force_resultant = reaction_force_resultant + reaction_force;
+    reaction_moment_resultant = reaction_moment_resultant +
+                                position.Cross(reaction_force) + reaction_moment;
   }
-  return equilibrium;
+  const Vector3 force_balance =
+      applied_force_resultant + reaction_force_resultant;
+  const Vector3 moment_balance =
+      applied_moment_resultant + reaction_moment_resultant;
+  const std::array<double, 6> equilibrium{
+      force_balance.X(), force_balance.Y(), force_balance.Z(),
+      moment_balance.X(), moment_balance.Y(), moment_balance.Z()};
+  if (!std::all_of(equilibrium.begin(), equilibrium.end(),
+                   [](const double value) { return std::isfinite(value); })) {
+    return Result<std::array<double, 6>>::Failure(ProcedureFailure(
+        FailureCategory::kSolver, "nonfinite-newton-data",
+        {domain.SourcePath(), 0U}, "NONLINEAR_STATIC_PROCEDURE",
+        domain.SourceContentIdentity(),
+        "Nonlinear global equilibrium balance is nonfinite."));
+  }
+  return Result<std::array<double, 6>>::Success(equilibrium);
 }
 
 Status ApplyIncrementConstraints(const StepDefinition& step,
@@ -689,8 +768,14 @@ NonlinearStaticProcedureExecutor::Execute(
       trial.Value().shell_results = std::move(internal.Value().shell_rows);
       trial.Value().physical_strain_energy =
           internal.Value().physical_strain_energy;
-      trial.Value().global_equilibrium =
-          GlobalEquilibrium(domain, trial.Value().residual);
+      Result<std::array<double, 6>> global_equilibrium =
+          GlobalEquilibrium(domain, dofs.Value(), trial.Value());
+      if (!global_equilibrium.HasValue()) {
+        return Result<NonlinearStaticExecutionResult>::Failure(RecordFailure(
+            log, &record, global_equilibrium.GetStatus(),
+            NewtonIterationStage::kExternalAssembled));
+      }
+      trial.Value().global_equilibrium = global_equilibrium.Value();
 
       Result<PartitionedStiffness> partition =
           constraints.Partition(tangent.Value(), dofs.Value());