baram2584/FESADev
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

perf(shell): route local products through MKL and reduce Hessian storage work

Browse Source
This commit is contained in:
KOKO\Mimi
2026-10-14 18:07:16 +09:00
parent 8c7c41729e
commit f9e0df66dc
9 changed files with 758 additions and 179 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/fesa/elements/nonlinear_mitc4_inertia.cpp
+92 -73
View File
@@ -4,11 +4,13 @@
#include <array>
#include <cmath>
#include <cstddef>
#include <exception>
#include <limits>
#include <string>
#include <utility>
#include "fesa/elements/finite_rotation.h"
#include "math/dense_blas_internal.h"
namespace fesa {
namespace {
@@ -35,6 +37,16 @@ class InertiaSum {
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,
const std::array<double, 18U>& right,
std::array<double, 36U>& result) {
if (!dense_blas_internal::MultiplyTransposeRight(
left.data(), 6U, right.data(), 6U, 3U, result.data()))
result.fill(std::numeric_limits<double>::quiet_NaN());
}
Vector3 WeightedSum(const std::array<double, kNodes>& weights,
const std::array<Vector3, kNodes>& values) {
Vector3 result{};
@@ -442,12 +454,11 @@ Status CheckPhysicalMass(const Matrix& mass,
}
double gram_error = 0.0;
double orthogonality_error = 0.0;
const Matrix gram =
dense_blas_internal::MultiplyTransposeLeft(complement, complement);
for (std::size_t row = 0U; row < kPhysical; ++row) {
for (std::size_t column = 0U; column < kPhysical; ++column) {
double entry = row == column ? -1.0 : 0.0;
for (std::size_t inner = 0U; inner < kDofs; ++inner) {
entry += complement(inner, row) * complement(inner, column);
}
const double entry = gram(row, column) - (row == column ? 1.0 : 0.0);
gram_error = std::hypot(gram_error, entry);
}
for (std::size_t node = 0U; node < kNodes; ++node) {
@@ -472,55 +483,42 @@ Status CheckPhysicalMass(const Matrix& mass,
return fail("invalid-nonlinear-inertia-null",
"Scaled mass symmetry and analytic null identities failed.");
}
Matrix block{kPhysical, kPhysical};
Matrix mass_complement{kDofs, kPhysical};
for (std::size_t row = 0U; row < kDofs; ++row) {
for (std::size_t column = 0U; column < kPhysical; ++column) {
for (std::size_t inner = 0U; inner < kDofs; ++inner) {
mass_complement(row, column) +=
scaled(row, inner) * complement(inner, column);
}
}
}
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) {
for (std::size_t inner = 0U; inner < kDofs; ++inner) {
block(row, column) +=
complement(inner, row) * mass_complement(inner, column);
}
row_sum += std::abs(block(row, column));
}
block_norm = std::max(block_norm, row_sum);
}
Matrix lower{kPhysical, kPhysical};
for (std::size_t row = 0U; row < kPhysical; ++row) {
for (std::size_t column = 0U; column <= row; ++column) {
double entry = block(row, column);
for (std::size_t inner = 0U; inner < column; ++inner) {
entry -= lower(row, inner) * lower(column, inner);
}
if (!std::isfinite(entry) || (row == column && !(entry > 0.0))) {
return fail(
"invalid-nonlinear-inertia-positive-mass",
"Physical mass requires strictly positive Cholesky pivots.");
}
lower(row, column) =
row == column ? std::sqrt(entry) : entry / lower(column, column);
}
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.");
return fail("invalid-nonlinear-inertia-positive-mass",
"Physical mass requires strictly positive Cholesky pivots.");
}
std::array<double, kPhysical> inverse_row_sums{};
double reconstruction_error = 0.0;
double block_frobenius = 0.0;
Matrix transpose{kPhysical, kPhysical};
for (std::size_t row = 0U; row < kPhysical; ++row)
for (std::size_t column = 0U; column < kPhysical; ++column)
transpose(row, column) = lower(column, row);
const Matrix reconstructed = lower.Multiply(transpose);
for (std::size_t row = 0U; row < kPhysical; ++row) {
for (std::size_t column = 0U; column < kPhysical; ++column) {
double reconstructed = 0.0;
for (std::size_t inner = 0U; inner <= std::min(row, column); ++inner) {
reconstructed += lower(row, inner) * lower(column, inner);
}
reconstruction_error =
std::hypot(reconstruction_error, reconstructed - block(row, column));
std::hypot(reconstruction_error,
reconstructed(row, column) - block(row, column));
block_frobenius = std::hypot(block_frobenius, block(row, column));
}
}
@@ -528,25 +526,11 @@ Status CheckPhysicalMass(const Matrix& mass,
return fail("invalid-nonlinear-inertia-positive-mass",
"Physical mass Cholesky reconstruction failed.");
}
for (std::size_t column = 0U; column < kPhysical; ++column) {
std::array<double, kPhysical> solution{};
for (std::size_t row = 0U; row < kPhysical; ++row) {
double rhs = row == column ? 1.0 : 0.0;
for (std::size_t inner = 0U; inner < row; ++inner) {
rhs -= lower(row, inner) * solution[inner];
}
solution[row] = rhs / lower(row, row);
}
for (std::size_t reverse = kPhysical; reverse > 0U; --reverse) {
const std::size_t row = reverse - 1U;
double rhs = solution[row];
for (std::size_t inner = row + 1U; inner < kPhysical; ++inner) {
rhs -= lower(inner, row) * solution[inner];
}
solution[row] = rhs / lower(row, row);
inverse_row_sums[row] += std::abs(solution[row]);
}
}
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;
@@ -712,6 +696,7 @@ Result<NonlinearMitc4Inertia> NonlinearMitc4Inertia::Create(
Result<NonlinearMitc4InertiaEvaluation> NonlinearMitc4Inertia::Evaluate(
const NonlinearMitc4TrialDofs& trial, const Vector& velocity,
const Vector& acceleration) const {
const dense_blas_internal::ScopedLocalDenseThreads local_threads{};
if (velocity.Size() != kDofs || acceleration.Size() != kDofs ||
!IsFinite(velocity) || !IsFinite(acceleration)) {
return Failure<NonlinearMitc4InertiaEvaluation>(
@@ -899,6 +884,42 @@ Result<NonlinearMitc4InertiaEvaluation> NonlinearMitc4Inertia::Evaluate(
result.momentum_rate_scale += scale * acceleration_point.Norm();
result.angular_momentum_rate_scale +=
scale * position.Cross(acceleration_point).Norm();
std::array<double, 18U> h_block;
std::array<double, 18U> velocity_block;
std::array<double, 18U> position_block;
for (std::size_t local = 0U; local < 6U; ++local) {
const auto field = 6U * mass_node + local;
for (std::size_t axis = 0U; axis < 3U; ++axis) {
h_block[3U * local + axis] = h[field][axis];
velocity_block[3U * local + axis] =
velocity_derivative[field][axis];
position_block[3U * local + axis] =
position_derivative[field][axis];
}
}
std::array<double, 36U> mass_block;
std::array<double, 36U> velocity_product;
std::array<double, 36U> position_product;
DotBlocks(h_block, h_block, mass_block);
DotBlocks(h_block, velocity_block, velocity_product);
DotBlocks(h_block, position_block, position_product);
std::array<std::array<double, 36U>, 3U> mass_variation;
for (std::size_t direction = 0U; direction < 3U; ++direction) {
std::array<double, 18U> derivative_block;
for (std::size_t local = 0U; local < 6U; ++local) {
const Vector3 derivative =
local < 3U ? Vector3{}
: z * rotations[mass_node]
.director_second[local - 3U][direction];
for (std::size_t axis = 0U; axis < 3U; ++axis)
derivative_block[3U * local + axis] = derivative[axis];
}
std::array<double, 36U> reverse;
DotBlocks(derivative_block, h_block, mass_variation[direction]);
DotBlocks(h_block, derivative_block, reverse);
for (std::size_t index = 0U; index < 36U; ++index)
mass_variation[direction][index] += reverse[index];
}
for (std::size_t row = 6U * mass_node; row < 6U * (mass_node + 1U);
++row) {
// Keep c independent of a; subtracting M*a destroys small c values.
@@ -906,10 +927,11 @@ Result<NonlinearMitc4InertiaEvaluation> NonlinearMitc4Inertia::Evaluate(
scale * h[row].Dot(convective_point);
for (std::size_t column = 6U * mass_node;
column < 6U * (mass_node + 1U); ++column) {
result.mass(row, column) += scale * h[row].Dot(h[column]);
const auto local_index = 6U * (row % 6U) + column % 6U;
result.mass(row, column) += scale * mass_block[local_index];
result.velocity_tangent(row, column) +=
scale * h[row].Dot(velocity_derivative[column]);
double position_entry = h[row].Dot(position_derivative[column]);
scale * velocity_product[local_index];
double position_entry = position_product[local_index];
if (row / 6U == column / 6U && row % 6U >= 3U &&
column % 6U >= 3U) {
const std::size_t node = row / 6U;
@@ -922,20 +944,10 @@ Result<NonlinearMitc4InertiaEvaluation> NonlinearMitc4Inertia::Evaluate(
result.position_tangent(row, column) += scale * position_entry;
if (column % 6U >= 3U) {
const auto direction = column % 6U - 3U;
const Vector3 dh_row =
row % 6U < 3U
? Vector3{}
: z * rotations[mass_node]
.director_second[row % 6U - 3U][direction];
for (std::size_t local = 0U; local < 6U; ++local) {
const auto field = 6U * mass_node + local;
const Vector3 dh_field =
local < 3U
? Vector3{}
: z * rotations[mass_node]
.director_second[local - 3U][direction];
mass_derivative[mass_node][direction][row % 6U][local] +=
scale * (dh_row.Dot(h[field]) + h[row].Dot(dh_field));
scale *
mass_variation[direction][6U * (row % 6U) + local];
}
}
}
@@ -1004,8 +1016,15 @@ Result<NonlinearMitc4InertiaEvaluation> NonlinearMitc4Inertia::Evaluate(
"invalid-nonlinear-inertia-evaluation", source_location_, identity_,
"Nonlinear MITC4 inertia evaluation must remain finite.");
}
const Status physical_mass = CheckPhysicalMass(result.mass, nulls, length,
source_location_, identity_);
Status physical_mass = Status::Ok();
try {
physical_mass = CheckPhysicalMass(result.mass, nulls, length,
source_location_, identity_);
} catch (const std::exception& error) {
return Failure<NonlinearMitc4InertiaEvaluation>(
"invalid-nonlinear-inertia-positive-mass", source_location_, identity_,
error.what());
}
if (!physical_mass.IsOk()) {
return Result<NonlinearMitc4InertiaEvaluation>::Failure(physical_mass);
}