Integrate Refactor solver sources and dual MSVC CMake builds

src/fesa/elements/nonlinear_mitc4_inertia.cpp

@@ -11,32 +11,29 @@
 
 #include "fesa/elements/finite_rotation.h"
 #include "math/dense_blas_internal.h"
+#include "math/jet_arithmetic.h"
+#include "math/jet_linear_solve.h"
+#include "math/finite_values.h"
+#include "math/vector3_operations.h"
+#include "elements/shell_kernel_values.h"
+#include "math/compensated_sum.h"
 
 namespace fesa {
 namespace {
 
+using shell_kernel_internal::Failure;
+using InertiaSum = math_internal::CompensatedSum;
+using shell_kernel_internal::FrameMatrix;
+using math_internal::DerivativeSum;
+using math_internal::WeightedSum;
+using math_internal::IsFinite;
+
 constexpr std::size_t kNodes = 4U;
 constexpr std::size_t kDofs = 24U;
 const std::array<double, 3> kGauss{-std::sqrt(3.0 / 5.0), 0.0,
                                    std::sqrt(3.0 / 5.0)};
 constexpr std::array<double, 3> kWeights{5.0 / 9.0, 8.0 / 9.0, 5.0 / 9.0};
 
-class InertiaSum {
- public:
-  void Add(const double term) {
-    const double next = sum_ + term;
-    correction_ += std::abs(sum_) >= std::abs(term)
-                       ? (sum_ - next) + term
-                       : (term - next) + sum_;
-    sum_ = next;
-  }
-  double Value() const { return sum_ + correction_; }
-
- private:
-  double sum_ = 0.0;
-  double correction_ = 0.0;
-};
-
 /// @brief Contracts two nodal 6-by-3 coefficient blocks through dense MKL.
 /// @note Nonfinite coefficients remain failures of the final inertia gate.
 void DotBlocks(const std::array<double, 18U>& left,
@@ -47,25 +44,6 @@ void DotBlocks(const std::array<double, 18U>& left,
     result.fill(std::numeric_limits<double>::quiet_NaN());
 }
 
-Vector3 WeightedSum(const std::array<double, kNodes>& weights,
-                    const std::array<Vector3, kNodes>& values) {
-  Vector3 result{};
-  for (std::size_t node = 0U; node < kNodes; ++node)
-    result = result + weights[node] * values[node];
-  return result;
-}
-
-// A derivative partition has no absolute-position contribution. Anchoring
-// before reduction preserves that identity at large global offsets and keeps
-// the reference/current covariants in the same canonical form.
-Vector3 DerivativeSum(const std::array<double, kNodes>& derivatives,
-                      const std::array<Vector3, kNodes>& values) {
-  std::array<Vector3, kNodes> relative{};
-  for (std::size_t node = 0U; node < kNodes; ++node)
-    relative[node] = values[node] - values[0U];
-  return WeightedSum(derivatives, relative);
-}
-
 Vector3 InterpolatedSum(const std::array<double, kNodes>& weights,
                         const std::array<Vector3, kNodes>& values) {
   std::array<Vector3, kNodes> relative{};
@@ -74,14 +52,6 @@ Vector3 InterpolatedSum(const std::array<double, kNodes>& weights,
   return values[0U] + WeightedSum(weights, relative);
 }
 
-template <class T>
-Result<T> Failure(const std::string& code, const SourceLocation& location,
-                  const std::string& identity, std::string message) {
-  return Result<T>::Failure(Status::Failure(
-      FailureCategory::kModel, {{Severity::kError, code, location, "*ELEMENT",
-                                 identity, std::move(message)}}));
-}
-
 std::string Identity(const std::array<const Node*, kNodes>& nodes) {
   std::string identity;
   for (const Node* node : nodes) {
@@ -93,36 +63,6 @@ std::string Identity(const std::array<const Node*, kNodes>& nodes) {
   return identity;
 }
 
-Matrix FrameMatrix(const ShellNodeInitialFrame& frame) {
-  Matrix result{3U, 3U};
-  for (std::size_t row = 0U; row < 3U; ++row) {
-    result(row, 0U) = frame.tangent_a[row];
-    result(row, 1U) = frame.tangent_b[row];
-    result(row, 2U) = frame.director[row];
-  }
-  return result;
-}
-
-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;
-}
-
-bool IsFinite(const Vector& vector) {
-  for (std::size_t index = 0U; index < vector.Size(); ++index) {
-    if (!std::isfinite(vector[index])) {
-      return false;
-    }
-  }
-  return true;
-}
-
 /// @brief Carries all derivatives through order three in one nodal rotation.
 struct Jet {
   double value{};
@@ -189,31 +129,6 @@ Jet operator*(const Jet& left, const Jet& right) {
   return result;
 }
 
-Jet Reciprocal(const Jet& value) {
-  Jet result{};
-  result.value = 1.0 / value.value;
-  const double squared = result.value * result.value;
-  const double cubed = squared * result.value;
-  const double fourth = cubed * result.value;
-  for (std::size_t i = 0U; i < 3U; ++i) {
-    result.d[i] = -value.d[i] * squared;
-    for (std::size_t j = 0U; j < 3U; ++j) {
-      result.dd[i][j] =
-          2.0 * value.d[i] * value.d[j] * cubed - value.dd[i][j] * squared;
-      for (std::size_t k = 0U; k < 3U; ++k) {
-        result.ddd[i][j][k] =
-            -6.0 * value.d[i] * value.d[j] * value.d[k] * fourth +
-            2.0 *
-                (value.dd[i][j] * value.d[k] + value.dd[i][k] * value.d[j] +
-                 value.dd[j][k] * value.d[i]) *
-                cubed -
-            value.ddd[i][j][k] * squared;
-      }
-    }
-  }
-  return result;
-}
-
 Jet InverseSqrt(const Jet& value) {
   Jet result{};
   result.value = 1.0 / std::sqrt(value.value);
@@ -246,27 +161,11 @@ using JetVector = std::array<Jet, 3>;
 using JetMatrix = std::array<std::array<Jet, 3>, 3>;
 
 JetVector Multiply(const JetMatrix& matrix, const JetVector& vector) {
-  JetVector result{Constant(0.0), Constant(0.0), Constant(0.0)};
-  for (std::size_t row = 0U; row < 3U; ++row) {
-    for (std::size_t column = 0U; column < 3U; ++column) {
-      result[row] = result[row] + matrix[row][column] * vector[column];
-    }
-  }
-  return result;
+  return math_internal::MultiplyJetMatrixVector(matrix, vector, Constant);
 }
 
 JetMatrix Multiply(const JetMatrix& left, const JetMatrix& right) {
-  JetMatrix result{};
-  for (std::size_t row = 0U; row < 3U; ++row) {
-    for (std::size_t column = 0U; column < 3U; ++column) {
-      result[row][column] = Constant(0.0);
-      for (std::size_t index = 0U; index < 3U; ++index) {
-        result[row][column] =
-            result[row][column] + left[row][index] * right[index][column];
-      }
-    }
-  }
-  return result;
+  return math_internal::MultiplyJetMatrix(left, right, Constant);
 }
 
 JetMatrix IdentityMatrix() {
@@ -296,30 +195,32 @@ Jet Dot(const JetVector& left, const JetVector& right) {
   return result;
 }
 
-JetMatrix Inverse(const JetMatrix& matrix) {
-  JetMatrix cofactor{};
-  for (std::size_t row = 0U; row < 3U; ++row) {
-    for (std::size_t column = 0U; column < 3U; ++column) {
-      const std::size_t a = (row + 1U) % 3U;
-      const std::size_t b = (row + 2U) % 3U;
-      const std::size_t c = (column + 1U) % 3U;
-      const std::size_t d = (column + 2U) % 3U;
-      cofactor[row][column] =
-          matrix[a][c] * matrix[b][d] - matrix[a][d] * matrix[b][c];
-      // Cyclic minor ordering already includes the cofactor sign.
-    }
-  }
-  Jet determinant = Constant(0.0);
-  for (std::size_t column = 0U; column < 3U; ++column) {
-    determinant = determinant + matrix[0U][column] * cofactor[0U][column];
-  }
-  const Jet inverse_determinant = Reciprocal(determinant);
-  JetMatrix result{};
-  for (std::size_t row = 0U; row < 3U; ++row) {
-    for (std::size_t column = 0U; column < 3U; ++column) {
-      result[row][column] = cofactor[column][row] * inverse_determinant;
-    }
-  }
+/// @brief Reads every nodal-rotation derivative in increasing total order.
+double ReadChannel(const Jet& jet, std::size_t degree, std::size_t channel) {
+  if (degree == 0) return jet.value;
+  if (degree == 1) return jet.d[channel];
+  if (degree == 2) return jet.dd[channel / 3][channel % 3];
+  return jet.ddd[channel / 9][(channel / 3) % 3][channel % 3];
+}
+void WriteChannel(Jet& jet, std::size_t degree, std::size_t channel,
+                   double value) {
+  if (degree == 0) jet.value = value;
+  else if (degree == 1) jet.d[channel] = value;
+  else if (degree == 2) jet.dd[channel / 3][channel % 3] = value;
+  else jet.ddd[channel / 9][(channel / 3) % 3][channel % 3] = value;
+}
+/// @brief Solves the actual null-preimage RHS with all third derivatives.
+JetVector Solve(const JetMatrix& matrix, const JetVector& rhs) {
+  Matrix primal{3, 3};
+  for (std::size_t row = 0; row < 3; ++row)
+    for (std::size_t column = 0; column < 3; ++column)
+      primal(row, column) = matrix[row][column].value;
+  const auto factor = dense_blas_internal::DenseFactor::Lu(primal);
+  JetVector result{};
+  if (factor.HasValue() && math_internal::SolveJetAction(
+      matrix, rhs, factor.Value(), {1, 3, 9, 27}, Constant, ReadChannel,
+      WriteChannel, result)) return result;
+  result.fill(Constant(std::numeric_limits<double>::quiet_NaN()));
   return result;
 }
 
@@ -486,27 +387,12 @@ Status CheckPhysicalMass(const Matrix& mass,
   const Matrix mass_complement = scaled.Multiply(complement);
   const Matrix block =
       dense_blas_internal::MultiplyTransposeLeft(complement, mass_complement);
-  double block_norm = 0.0;
-  for (std::size_t row = 0U; row < kPhysical; ++row) {
-    double row_sum = 0.0;
-    for (std::size_t column = 0U; column < kPhysical; ++column) {
-      row_sum += std::abs(block(row, column));
-    }
-    block_norm = std::max(block_norm, row_sum);
-  }
-  Matrix lower{kPhysical, kPhysical};
-  Matrix inverse_lower{kPhysical, kPhysical};
-  auto factor_failure = dense_blas_internal::CholeskyFailure::kNone;
-  if (!dense_blas_internal::CholeskyLowerAndInverse(block, lower, inverse_lower,
-                                                    &factor_failure)) {
-    if (factor_failure == dense_blas_internal::CholeskyFailure::kInverse)
-      return fail(
-          "ill-conditioned-nonlinear-inertia-positive-mass",
-          "Physical mass reciprocal infinity condition is below 2^-32.");
+  const auto factor = dense_blas_internal::DenseFactor::Cholesky(block);
+  if (!factor.HasValue()) {
     return fail("invalid-nonlinear-inertia-positive-mass",
                 "Physical mass requires strictly positive Cholesky pivots.");
   }
-  std::array<double, kPhysical> inverse_row_sums{};
+  const Matrix lower = std::move(factor.Value().Lower().Value());
   double reconstruction_error = 0.0;
   double block_frobenius = 0.0;
   Matrix transpose{kPhysical, kPhysical};
@@ -526,16 +412,9 @@ Status CheckPhysicalMass(const Matrix& mass,
     return fail("invalid-nonlinear-inertia-positive-mass",
                 "Physical mass Cholesky reconstruction failed.");
   }
-  const Matrix inverse =
-      dense_blas_internal::MultiplyTransposeLeft(inverse_lower, inverse_lower);
-  for (std::size_t column = 0U; column < kPhysical; ++column)
-    for (std::size_t row = 0U; row < kPhysical; ++row)
-      inverse_row_sums[row] += std::abs(inverse(row, column));
-  const double inverse_norm =
-      *std::max_element(inverse_row_sums.begin(), inverse_row_sums.end());
-  const double reciprocal_condition = 1.0 / block_norm / inverse_norm;
-  if (!std::isfinite(inverse_norm) || !std::isfinite(reciprocal_condition) ||
-      !(reciprocal_condition >= std::ldexp(1.0, -32))) {
+  const auto reciprocal_condition = factor.Value().ReciprocalConditionInfinity();
+  if (!reciprocal_condition.HasValue() ||
+      !(reciprocal_condition.Value() >= std::ldexp(1.0, -32))) {
     return fail("ill-conditioned-nonlinear-inertia-positive-mass",
                 "Physical mass reciprocal infinity condition is below 2^-32.");
   }
@@ -566,7 +445,7 @@ Result<DynamicRotationKinematics> EvaluateDynamicRotation(
                           Constant(initial_director.Z())};
   const JetVector director_jet = Multiply(ExpSkew(phi), initial);
   const JetMatrix left_jacobian = LeftJacobian(phi);
-  JetVector null_jet = Multiply(Inverse(left_jacobian), director_jet);
+  JetVector null_jet = Solve(left_jacobian, director_jet);
   const Jet null_norm_squared = Dot(null_jet, null_jet);
   if (!std::isfinite(null_norm_squared.value) ||
       !(null_norm_squared.value > 0.0)) {