KOKO\Mimi
1340 lines
62 KiB
C++
1340 lines
62 KiB
C++
#include "analysis/hht_reduced_internal.h"
|
|
|
|
#include <algorithm>
|
|
#include <cmath>
|
|
#include <cstring>
|
|
#include <limits>
|
|
#include <map>
|
|
#include <sstream>
|
|
#include <stdexcept>
|
|
#include <string>
|
|
#include <utility>
|
|
|
|
#include "fesa/solvers/linear/tangent_solver.h"
|
|
#include "math/dense_blas_internal.h"
|
|
#include "math/high_low_arithmetic.h"
|
|
|
|
namespace fesa::hht_reduced_internal {
|
|
namespace {
|
|
|
|
Status Failure(const std::string& message,
|
|
const std::string& code = "dynamic-invalid-state") {
|
|
return Status::Failure(FailureCategory::kSolver,
|
|
{{Severity::kError, code, {},
|
|
"DYNAMIC", {}, message}});
|
|
}
|
|
|
|
bool Finite(const Vector& values) {
|
|
for (std::size_t i = 0U; i < values.Size(); ++i)
|
|
if (!std::isfinite(values[i])) return false;
|
|
return true;
|
|
}
|
|
|
|
class RateSum {
|
|
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;
|
|
};
|
|
|
|
bool ExactlyEqual(const Vector& left, const Vector& right) {
|
|
if (left.Size() != right.Size()) return false;
|
|
for (std::size_t i = 0U; i < left.Size(); ++i)
|
|
if (left[i] != right[i]) return false;
|
|
return true;
|
|
}
|
|
|
|
Vector Local(const Vector& full,
|
|
const hht_internal::RuntimeShell& shell) {
|
|
Vector local{24U};
|
|
for (std::size_t i = 0U; i < 24U; ++i) local[i] = full[shell.scatter[i]];
|
|
return local;
|
|
}
|
|
|
|
NonlinearMitc4TrialDofs Trial(const Vector& local) {
|
|
NonlinearMitc4TrialDofs trial{};
|
|
for (std::size_t node = 0U; node < 4U; ++node) {
|
|
const auto first = 6U * node;
|
|
trial.displacement[node] =
|
|
Vector3{local[first], local[first + 1U], local[first + 2U]};
|
|
trial.rotation_vector[node] = Vector3{local[first + 3U],
|
|
local[first + 4U],
|
|
local[first + 5U]};
|
|
}
|
|
return trial;
|
|
}
|
|
|
|
Result<SparseMatrix> DenseSparse(const Matrix& matrix) {
|
|
std::vector<CooContribution> entries;
|
|
entries.reserve(matrix.Rows() * matrix.Columns());
|
|
for (std::size_t row = 0U; row < matrix.Rows(); ++row)
|
|
for (std::size_t column = 0U; column < matrix.Columns(); ++column) {
|
|
const double value = matrix(row, column);
|
|
if (value != 0.0)
|
|
entries.push_back(
|
|
{row, column, value, 0U, row * matrix.Columns() + column});
|
|
}
|
|
return hht_internal::MakeSparse(matrix.Rows(), matrix.Columns(),
|
|
std::move(entries));
|
|
}
|
|
|
|
bool ValidBasis(const FixedChart& chart) {
|
|
if (chart.p_columns.size() != chart.p_dofs.size() ||
|
|
chart.z_columns.size() != chart.z_dofs.size()) return false;
|
|
for (const auto* columns : {&chart.p_columns, &chart.z_columns})
|
|
for (const auto& column : *columns) {
|
|
if (column.channel > 1U || column.support.empty() ||
|
|
column.support_radii.size() != column.support.size()) return false;
|
|
std::size_t previous = 0U;
|
|
for (std::size_t i = 0U; i < column.support.size(); ++i) {
|
|
const auto& entry = column.support[i];
|
|
if (entry.dof >= chart.full_dof_count || !std::isfinite(entry.value) ||
|
|
column.support_radii[i] < 0.0 ||
|
|
!std::isfinite(column.support_radii[i]) ||
|
|
(i != 0U && entry.dof <= previous)) return false;
|
|
previous = entry.dof;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
double ColumnDot(const FixedBasisColumn& column, const Vector& full) {
|
|
RateSum sum;
|
|
for (const auto& entry : column.support) sum.Add(entry.value * full[entry.dof]);
|
|
return sum.Value();
|
|
}
|
|
|
|
double GradientDot(const std::vector<ConstraintEntry>& gradient,
|
|
const FixedBasisColumn& column) {
|
|
RateSum sum;
|
|
for (const auto& entry : gradient)
|
|
for (const auto& basis : column.support)
|
|
if (entry.dof == basis.dof) sum.Add(entry.value * basis.value);
|
|
return sum.Value();
|
|
}
|
|
|
|
FixedBasisColumn ComponentColumn(const std::size_t full) {
|
|
return {full / 6U, full % 6U < 3U ? 0U : 1U, false, {{full, 1.0}}, {0.0}};
|
|
}
|
|
|
|
bool BitwiseEqual(const Vector& left, const Vector& right) {
|
|
return left.Size() == right.Size() &&
|
|
(left.Size() == 0U ||
|
|
std::memcmp(left.Data(), right.Data(), left.Size() * sizeof(double)) == 0);
|
|
}
|
|
|
|
double RadiusUp(const double value) {
|
|
return value == 0.0 ? 0.0 : std::nextafter(value, std::numeric_limits<double>::infinity());
|
|
}
|
|
|
|
double RadiusProduct(const double left, const double right) {
|
|
const double value = left * right;
|
|
return value == 0.0 && left != 0.0 && right != 0.0
|
|
? std::numeric_limits<double>::denorm_min() : RadiusUp(value);
|
|
}
|
|
|
|
double RadiusQuotient(const double numerator, const double denominator) {
|
|
const double value = numerator / denominator;
|
|
return value == 0.0 && numerator != 0.0
|
|
? std::numeric_limits<double>::denorm_min() : RadiusUp(value);
|
|
}
|
|
|
|
double BasisRoundoff(const double value) {
|
|
if (value == 0.0) return 0.0;
|
|
const double upper = std::nextafter(value, std::numeric_limits<double>::infinity());
|
|
const double lower = std::nextafter(value, -std::numeric_limits<double>::infinity());
|
|
const double gap = (std::max)(std::abs(upper - value), std::abs(value - lower));
|
|
const double half = 0.5 * gap;
|
|
return half == 0.0 ? gap : RadiusUp(half);
|
|
}
|
|
|
|
struct BasisValue {
|
|
double value = 0.0;
|
|
double radius = 0.0;
|
|
};
|
|
|
|
BasisValue BasisAdd(const BasisValue left, const BasisValue right) {
|
|
const double value = left.value + right.value;
|
|
return {value, RadiusUp(RadiusUp(left.radius + right.radius) + BasisRoundoff(value))};
|
|
}
|
|
|
|
BasisValue BasisMultiply(const BasisValue left, const BasisValue right) {
|
|
const double value = left.value * right.value;
|
|
const double first = RadiusProduct(std::abs(left.value), right.radius);
|
|
const double second = RadiusProduct(std::abs(right.value), left.radius);
|
|
const double cross = RadiusProduct(left.radius, right.radius);
|
|
const double rounding = value == 0.0 && left.value != 0.0 && right.value != 0.0
|
|
? std::numeric_limits<double>::denorm_min()
|
|
: BasisRoundoff(value);
|
|
return {value, RadiusUp(RadiusUp(RadiusUp(first + second) + cross) + rounding)};
|
|
}
|
|
|
|
BasisValue BasisDivide(const BasisValue numerator, const BasisValue denominator) {
|
|
const double value = numerator.value / denominator.value;
|
|
const double lower = std::nextafter(std::abs(denominator.value) - denominator.radius,
|
|
-std::numeric_limits<double>::infinity());
|
|
if (!(lower > 0.0)) return {value, std::numeric_limits<double>::infinity()};
|
|
const double top = RadiusUp(numerator.radius + RadiusProduct(std::abs(value), denominator.radius));
|
|
const double rounding = value == 0.0 && numerator.value != 0.0
|
|
? std::numeric_limits<double>::denorm_min()
|
|
: BasisRoundoff(value);
|
|
const double amplified = RadiusProduct(rounding,
|
|
RadiusUp(1.0 + RadiusQuotient(denominator.radius, lower)));
|
|
return {value, RadiusUp(RadiusQuotient(top, lower) + amplified)};
|
|
}
|
|
|
|
BasisValue BasisNorm(const std::array<BasisValue, 3U>& vector) {
|
|
const double scale = (std::max)({std::abs(vector[0U].value),
|
|
std::abs(vector[1U].value), std::abs(vector[2U].value)});
|
|
if (!(scale > 0.0) || !std::isfinite(scale)) return {};
|
|
std::array<BasisValue, 3U> y{};
|
|
std::array<BasisValue, 3U> square{};
|
|
for (std::size_t i = 0U; i < 3U; ++i) {
|
|
// The actual chosen scale is held exact in this construction indicator.
|
|
y[i] = BasisDivide(vector[i], {scale, 0.0});
|
|
square[i] = BasisMultiply(y[i], y[i]);
|
|
}
|
|
const auto sum = BasisAdd(BasisAdd(square[0U], square[1U]), square[2U]);
|
|
const double root = std::sqrt(sum.value);
|
|
const double lower = std::nextafter(sum.value - sum.radius,
|
|
-std::numeric_limits<double>::infinity());
|
|
if (!(lower > 0.0)) return {scale * root, std::numeric_limits<double>::infinity()};
|
|
const double denominator = std::nextafter(root - BasisRoundoff(root),
|
|
-std::numeric_limits<double>::infinity());
|
|
if (!(denominator > 0.0))
|
|
return {scale * root, std::numeric_limits<double>::infinity()};
|
|
const BasisValue square_root{root,
|
|
RadiusUp(RadiusQuotient(sum.radius, denominator) + BasisRoundoff(root))};
|
|
return BasisMultiply({scale, 0.0}, square_root);
|
|
}
|
|
|
|
FixedBasisColumn PlaneColumn(const std::size_t node,
|
|
const std::array<BasisValue, 3U>& vector) {
|
|
FixedBasisColumn column{node, 1U, true, {}, {}};
|
|
for (std::size_t i = 0U; i < 3U; ++i) {
|
|
column.support.push_back({6U * node + 3U + i, vector[i].value});
|
|
column.support_radii.push_back(vector[i].radius);
|
|
}
|
|
return column;
|
|
}
|
|
|
|
} // namespace
|
|
|
|
Result<FixedChart> BuildFixedChart(
|
|
const DofManager& dofs,
|
|
const std::vector<DynamicRotationKinematics>& rotations) {
|
|
if (dofs.FullDofCount() != rotations.size() * 6U)
|
|
return Result<FixedChart>::Failure(
|
|
Failure("Reduced chart rotation and full-DOF inventories differ.",
|
|
"dynamic-mass-regularity"));
|
|
FixedChart chart{};
|
|
chart.full_dof_count = dofs.FullDofCount();
|
|
std::vector<bool> is_z(chart.full_dof_count, false);
|
|
std::map<std::size_t, FixedBasisColumn> plane_columns;
|
|
for (std::size_t node = 0U; node < rotations.size(); ++node) {
|
|
bool admissible = true;
|
|
for (std::size_t component = 0U; component < 3U; ++component) {
|
|
const auto full = 6U * node + 3U + component;
|
|
if (!dofs.FreeEquation(full).has_value() &&
|
|
rotations[node].drill_null[component] != 0.0) {
|
|
admissible = false;
|
|
}
|
|
}
|
|
if (!admissible) continue;
|
|
std::size_t pivot = 0U;
|
|
for (std::size_t component = 1U; component < 3U; ++component) {
|
|
if (std::abs(rotations[node].drill_null[component]) >
|
|
std::abs(rotations[node].drill_null[pivot]))
|
|
pivot = component;
|
|
}
|
|
const auto pivot_dof = 6U * node + 3U + pivot;
|
|
if (!dofs.FreeEquation(pivot_dof).has_value() ||
|
|
rotations[node].drill_null[pivot] == 0.0) {
|
|
return Result<FixedChart>::Failure(Failure(
|
|
"Reduced chart has no free nonzero canonical drilling pivot.",
|
|
"dynamic-mass-regularity"));
|
|
}
|
|
is_z[pivot_dof] = true;
|
|
chart.null_nodes.push_back(node);
|
|
chart.pivot_components.push_back(pivot);
|
|
chart.z_dofs.push_back(pivot_dof);
|
|
chart.initial_nulls.push_back({rotations[node].drill_null[0U],
|
|
rotations[node].drill_null[1U],
|
|
rotations[node].drill_null[2U]});
|
|
bool fully_free = true;
|
|
for (std::size_t component = 0U; component < 3U; ++component)
|
|
fully_free = fully_free &&
|
|
dofs.FreeEquation(6U * node + 3U + component).has_value();
|
|
if (!fully_free) {
|
|
chart.z_columns.push_back(ComponentColumn(pivot_dof));
|
|
chart.minimum_axes.push_back(3U);
|
|
continue;
|
|
}
|
|
if (!math_internal::ArithmeticEnvironmentSupported())
|
|
return Result<FixedChart>::Failure(Failure(
|
|
"Fixed null basis requires round-to-nearest and gradual underflow.",
|
|
"dynamic-mass-regularity"));
|
|
const auto& raw = rotations[node].drill_null;
|
|
std::array<BasisValue, 3U> raw_values{};
|
|
for (std::size_t i = 0U; i < 3U; ++i) raw_values[i] = {raw[i], 0.0};
|
|
const auto norm = BasisNorm(raw_values);
|
|
if (!std::isfinite(norm.value) || !(norm.value > 0.0) || !std::isfinite(norm.radius))
|
|
return Result<FixedChart>::Failure(Failure(
|
|
"Fixed null basis requires a positive finite analytic-null norm.",
|
|
"dynamic-mass-regularity"));
|
|
std::array<BasisValue, 3U> n{};
|
|
for (std::size_t i = 0U; i < 3U; ++i) n[i] = BasisDivide(raw_values[i], norm);
|
|
std::size_t axis = 0U;
|
|
for (std::size_t i = 1U; i < 3U; ++i)
|
|
if (std::abs(n[i].value) < std::abs(n[axis].value)) axis = i;
|
|
std::array<BasisValue, 3U> t1{};
|
|
for (std::size_t i = 0U; i < 3U; ++i) {
|
|
const auto product = BasisMultiply(n[axis], n[i]);
|
|
t1[i] = BasisAdd({i == axis ? 1.0 : 0.0, 0.0}, {-product.value, product.radius});
|
|
}
|
|
const auto tangent_norm = BasisNorm(t1);
|
|
if (!std::isfinite(tangent_norm.value) || !(tangent_norm.value > 0.0) ||
|
|
!std::isfinite(tangent_norm.radius))
|
|
return Result<FixedChart>::Failure(Failure(
|
|
"Fixed null basis tangent norm is invalid.", "dynamic-mass-regularity"));
|
|
for (auto& value : t1) value = BasisDivide(value, tangent_norm);
|
|
std::array<BasisValue, 3U> t2{};
|
|
for (std::size_t i = 0U; i < 3U; ++i) {
|
|
const auto first = BasisMultiply(n[(i + 1U) % 3U], t1[(i + 2U) % 3U]);
|
|
const auto second = BasisMultiply(n[(i + 2U) % 3U], t1[(i + 1U) % 3U]);
|
|
t2[i] = BasisAdd(first, {-second.value, second.radius});
|
|
}
|
|
const std::array<std::array<BasisValue, 3U>, 3U> basis{t1, t2, n};
|
|
for (std::size_t i = 0U; i < 3U; ++i)
|
|
for (std::size_t j = 0U; j < 3U; ++j) {
|
|
RateSum dot;
|
|
for (std::size_t c = 0U; c < 3U; ++c)
|
|
dot.Add(basis[i][c].value * basis[j][c].value);
|
|
const double defect = std::abs(dot.Value() - (i == j ? 1.0 : 0.0));
|
|
if (!std::isfinite(defect) || defect > 1e-12)
|
|
return Result<FixedChart>::Failure(Failure(
|
|
"Fixed null basis fails the original Gram check.",
|
|
"dynamic-mass-regularity"));
|
|
}
|
|
std::size_t retained = 0U;
|
|
for (std::size_t component = 0U; component < 3U; ++component)
|
|
if (component != pivot)
|
|
plane_columns.emplace(6U * node + 3U + component,
|
|
PlaneColumn(node, retained++ == 0U ? t1 : t2));
|
|
chart.z_columns.push_back(PlaneColumn(node, n));
|
|
chart.minimum_axes.push_back(axis);
|
|
}
|
|
for (const auto full : dofs.FreeDofs())
|
|
if (!is_z[full]) {
|
|
chart.p_dofs.push_back(full);
|
|
const auto plane = plane_columns.find(full);
|
|
chart.p_columns.push_back(plane == plane_columns.end()
|
|
? ComponentColumn(full) : plane->second);
|
|
}
|
|
if (!ValidBasis(chart))
|
|
return Result<FixedChart>::Failure(Failure(
|
|
"Fixed null basis construction radius is incomplete or nonfinite.",
|
|
"dynamic-mass-regularity"));
|
|
return Result<FixedChart>::Success(std::move(chart));
|
|
}
|
|
|
|
Result<Vector> Lift(const FixedChart& chart, const Vector& p, const Vector& z) {
|
|
if (!ValidBasis(chart) || p.Size() != chart.p_columns.size() ||
|
|
z.Size() != chart.z_columns.size())
|
|
return Result<Vector>::Failure(
|
|
Failure("Reduced chart coordinate dimensions differ from its split."));
|
|
if (!Finite(p) || !Finite(z))
|
|
return Result<Vector>::Failure(
|
|
Failure("Reduced chart coordinates must remain finite."));
|
|
std::vector<RateSum> sums(chart.full_dof_count);
|
|
for (std::size_t i = 0U; i < p.Size(); ++i)
|
|
for (const auto& entry : chart.p_columns[i].support)
|
|
sums[entry.dof].Add(entry.value * p[i]);
|
|
for (std::size_t i = 0U; i < z.Size(); ++i)
|
|
for (const auto& entry : chart.z_columns[i].support)
|
|
sums[entry.dof].Add(entry.value * z[i]);
|
|
Vector full{chart.full_dof_count};
|
|
for (std::size_t i = 0U; i < full.Size(); ++i) full[i] = sums[i].Value();
|
|
if (!Finite(full)) return Result<Vector>::Failure(
|
|
Failure("Reduced basis lift is nonfinite."));
|
|
return Result<Vector>::Success(std::move(full));
|
|
}
|
|
|
|
Result<Vector> ProjectPhysical(const FixedChart& chart, const Vector& full) {
|
|
if (!ValidBasis(chart) || full.Size() != chart.full_dof_count || !Finite(full))
|
|
return Result<Vector>::Failure(Failure("Reduced physical projection inventory is invalid."));
|
|
Vector result{chart.p_columns.size()};
|
|
for (std::size_t i = 0U; i < result.Size(); ++i)
|
|
result[i] = ColumnDot(chart.p_columns[i], full);
|
|
if (!Finite(result)) return Result<Vector>::Failure(Failure("Reduced physical projection is nonfinite."));
|
|
return Result<Vector>::Success(std::move(result));
|
|
}
|
|
|
|
Result<Vector> ProjectAlgebraic(const FixedChart& chart, const Vector& full) {
|
|
if (!ValidBasis(chart) || full.Size() != chart.full_dof_count || !Finite(full))
|
|
return Result<Vector>::Failure(Failure("Reduced algebraic projection inventory is invalid."));
|
|
Vector result{chart.z_columns.size()};
|
|
for (std::size_t i = 0U; i < result.Size(); ++i)
|
|
result[i] = ColumnDot(chart.z_columns[i], full);
|
|
if (!Finite(result)) return Result<Vector>::Failure(Failure("Reduced algebraic projection is nonfinite."));
|
|
return Result<Vector>::Success(std::move(result));
|
|
}
|
|
|
|
std::size_t PhysicalCoordinateChannel(const FixedChart& chart,
|
|
const std::size_t coordinate) {
|
|
return chart.p_columns[coordinate].channel;
|
|
}
|
|
|
|
Result<ConstraintOperators> EvaluateConstraintOperators(
|
|
const Domain& domain, const DofManager& dofs,
|
|
const std::vector<hht_internal::RuntimeShell>& shells,
|
|
const hht_internal::DynamicAssembly& assembly, const FixedChart& chart,
|
|
const Vector& force_rate, const Vector& force_acceleration) {
|
|
const auto count = dofs.FullDofCount();
|
|
if (!ValidBasis(chart) || chart.full_dof_count != count || force_rate.Size() != count ||
|
|
force_acceleration.Size() != count || !Finite(force_rate) ||
|
|
!Finite(force_acceleration) ||
|
|
assembly.frame.residual_scale.Size() != count ||
|
|
chart.null_nodes.size() != chart.z_dofs.size() ||
|
|
chart.null_nodes.size() != chart.pivot_components.size())
|
|
return Result<ConstraintOperators>::Failure(
|
|
Failure("Reduced constraint operator inventory is invalid.",
|
|
"dynamic-mass-regularity"));
|
|
if (hht_internal::NullNodes(dofs, assembly.rotations) != chart.null_nodes)
|
|
return Result<ConstraintOperators>::Failure(Failure(
|
|
"The admissible drilling-null inventory changed in the fixed chart.",
|
|
"dynamic-mass-regularity"));
|
|
for (std::size_t row = 0U; row < chart.null_nodes.size(); ++row) {
|
|
const auto node = chart.null_nodes[row];
|
|
RateSum transverse;
|
|
if (node < assembly.rotations.size())
|
|
for (const auto& entry : chart.z_columns[row].support)
|
|
transverse.Add(assembly.rotations[node].drill_null[entry.dof % 6U - 3U] *
|
|
entry.value);
|
|
if (node >= assembly.rotations.size() ||
|
|
!std::isfinite(transverse.Value()) || transverse.Value() == 0.0)
|
|
return Result<ConstraintOperators>::Failure(Failure(
|
|
"The fixed reduced basis is no longer transverse to the actual null.",
|
|
"dynamic-mass-regularity"));
|
|
}
|
|
const auto rows = chart.null_nodes.size();
|
|
ConstraintOperators result{};
|
|
result.rows.resize(rows);
|
|
std::vector<std::map<std::size_t, double>> gradients(rows);
|
|
std::vector<std::map<std::size_t, double>> time_gradients(rows);
|
|
std::vector<std::map<std::size_t, double>> third_gradients(rows);
|
|
std::vector<std::map<std::pair<std::size_t, std::size_t>, double>>
|
|
hessians(rows);
|
|
std::vector<std::size_t> node_row(domain.Nodes().size(), rows);
|
|
for (std::size_t row = 0U; row < rows; ++row) {
|
|
if (chart.null_nodes[row] >= node_row.size())
|
|
return Result<ConstraintOperators>::Failure(
|
|
Failure("Reduced chart null node is outside the Domain.",
|
|
"dynamic-mass-regularity"));
|
|
node_row[chart.null_nodes[row]] = row;
|
|
}
|
|
std::vector<bool> external_done(domain.Nodes().size(), false);
|
|
for (const auto& shell : shells) {
|
|
const auto q = Local(assembly.frame.generalized_coordinates, shell);
|
|
const auto v = Local(assembly.frame.velocity, shell);
|
|
const auto internal = EvaluateInternalResidualAndTangent(shell.internal,
|
|
Trial(q), &v);
|
|
if (!internal.HasValue())
|
|
return Result<ConstraintOperators>::Failure(internal.GetStatus());
|
|
for (std::size_t local_node = 0U; local_node < 4U; ++local_node) {
|
|
const auto global_node = shell.scatter[6U * local_node] / 6U;
|
|
if (global_node >= node_row.size() || node_row[global_node] == rows)
|
|
continue;
|
|
const auto row = node_row[global_node];
|
|
result.rows[row].value +=
|
|
internal.Value().drilling_constraint[local_node];
|
|
for (std::size_t i = 0U; i < 24U; ++i) {
|
|
const auto global_i = shell.scatter[i];
|
|
const double gradient =
|
|
internal.Value().drilling_constraint_gradient(local_node, i);
|
|
const double third = internal.Value()
|
|
.drilling_constraint_third_directional_gradient
|
|
[local_node][i];
|
|
if (gradient != 0.0) gradients[row][global_i] += gradient;
|
|
if (third != 0.0) third_gradients[row][global_i] += third;
|
|
for (std::size_t j = 0U; j < 24U; ++j) {
|
|
const double hessian =
|
|
internal.Value().drilling_constraint_hessian[local_node](i, j);
|
|
if (hessian != 0.0)
|
|
hessians[row][{global_i, shell.scatter[j]}] += hessian;
|
|
}
|
|
}
|
|
if (external_done[global_node]) continue;
|
|
external_done[global_node] = true;
|
|
const auto first = 6U * global_node + 3U;
|
|
const auto external = shell.internal.EvaluateExternalDrillingConstraint(
|
|
local_node, Vector3{q[6U * local_node + 3U],
|
|
q[6U * local_node + 4U],
|
|
q[6U * local_node + 5U]},
|
|
Vector3{v[6U * local_node + 3U], v[6U * local_node + 4U],
|
|
v[6U * local_node + 5U]},
|
|
Vector3{assembly.frame.external_force[first],
|
|
assembly.frame.external_force[first + 1U],
|
|
assembly.frame.external_force[first + 2U]},
|
|
Vector3{force_rate[first], force_rate[first + 1U],
|
|
force_rate[first + 2U]},
|
|
Vector3{force_acceleration[first], force_acceleration[first + 1U],
|
|
force_acceleration[first + 2U]});
|
|
if (!external.HasValue())
|
|
return Result<ConstraintOperators>::Failure(external.GetStatus());
|
|
const auto rate_term = shell.internal.EvaluateExternalDrillingConstraint(
|
|
local_node, Vector3{q[6U * local_node + 3U],
|
|
q[6U * local_node + 4U],
|
|
q[6U * local_node + 5U]},
|
|
{}, Vector3{force_rate[first], force_rate[first + 1U],
|
|
force_rate[first + 2U]}, {}, {});
|
|
if (!rate_term.HasValue())
|
|
return Result<ConstraintOperators>::Failure(rate_term.GetStatus());
|
|
const auto acceleration_term =
|
|
shell.internal.EvaluateExternalDrillingConstraint(
|
|
local_node, Vector3{q[6U * local_node + 3U],
|
|
q[6U * local_node + 4U],
|
|
q[6U * local_node + 5U]},
|
|
{}, Vector3{force_acceleration[first],
|
|
force_acceleration[first + 1U],
|
|
force_acceleration[first + 2U]}, {}, {});
|
|
if (!acceleration_term.HasValue())
|
|
return Result<ConstraintOperators>::Failure(
|
|
acceleration_term.GetStatus());
|
|
result.rows[row].value += external.Value().value;
|
|
for (std::size_t i = 0U; i < 3U; ++i) {
|
|
const double gradient = external.Value().gradient[i];
|
|
const double third = external.Value().gradient_directional_second[i];
|
|
if (gradient != 0.0) gradients[row][first + i] += gradient;
|
|
if (third != 0.0) third_gradients[row][first + i] += third;
|
|
for (std::size_t j = 0U; j < 3U; ++j) {
|
|
const double hessian = external.Value().hessian(i, j);
|
|
if (hessian != 0.0)
|
|
hessians[row][{first + i, first + j}] += hessian;
|
|
}
|
|
}
|
|
for (std::size_t i = 0U; i < 3U; ++i) {
|
|
// Use the direct Fdot primitive rather than subtracting H*v from a
|
|
// mixed directional quantity. In particular, constant public CLOAD
|
|
// keeps psi_t and psi_qt exactly zero in binary64.
|
|
if (rate_term.Value().gradient[i] != 0.0)
|
|
time_gradients[row][first + i] += rate_term.Value().gradient[i];
|
|
}
|
|
result.rows[row].time_first += rate_term.Value().value;
|
|
result.rows[row].time_second += acceleration_term.Value().value;
|
|
}
|
|
}
|
|
for (std::size_t row = 0U; row < rows; ++row) {
|
|
auto& destination = result.rows[row];
|
|
for (const auto& entry : gradients[row])
|
|
if (entry.second != 0.0)
|
|
destination.gradient.push_back({entry.first, entry.second});
|
|
for (const auto& entry : time_gradients[row])
|
|
if (entry.second != 0.0)
|
|
destination.time_gradient.push_back({entry.first, entry.second});
|
|
for (const auto& entry : third_gradients[row])
|
|
if (entry.second != 0.0)
|
|
destination.third_directional_gradient.push_back({entry.first,
|
|
entry.second});
|
|
for (const auto& entry : hessians[row])
|
|
if (entry.second != 0.0)
|
|
destination.hessian.push_back(
|
|
{entry.first.first, entry.first.second, entry.second});
|
|
const auto node = chart.null_nodes[row];
|
|
const auto& null = assembly.rotations[node].drill_null;
|
|
for (std::size_t component = 0U; component < 3U; ++component)
|
|
destination.value_scale +=
|
|
std::abs(null[component]) *
|
|
assembly.frame.residual_scale[6U * node + 3U + component];
|
|
if (!std::isfinite(destination.value) ||
|
|
!std::isfinite(destination.time_first) ||
|
|
!std::isfinite(destination.time_second) ||
|
|
!std::isfinite(destination.value_scale))
|
|
return Result<ConstraintOperators>::Failure(
|
|
Failure("Reduced drilling operator reduction is nonfinite."));
|
|
for (const auto& entry : destination.gradient)
|
|
if (!std::isfinite(entry.value))
|
|
return Result<ConstraintOperators>::Failure(
|
|
Failure("Reduced drilling gradient is nonfinite."));
|
|
for (const auto& entry : destination.time_gradient)
|
|
if (!std::isfinite(entry.value))
|
|
return Result<ConstraintOperators>::Failure(
|
|
Failure("Reduced drilling time gradient is nonfinite."));
|
|
for (const auto& entry : destination.third_directional_gradient)
|
|
if (!std::isfinite(entry.value))
|
|
return Result<ConstraintOperators>::Failure(
|
|
Failure("Reduced drilling third derivative is nonfinite."));
|
|
for (const auto& entry : destination.hessian)
|
|
if (!std::isfinite(entry.value))
|
|
return Result<ConstraintOperators>::Failure(
|
|
Failure("Reduced drilling Hessian is nonfinite."));
|
|
}
|
|
return Result<ConstraintOperators>::Success(std::move(result));
|
|
}
|
|
|
|
Matrix GraphJacobian(const ConstraintOperators& operators,
|
|
const FixedChart& chart) {
|
|
Matrix result{operators.rows.size(), chart.z_dofs.size()};
|
|
for (std::size_t row = 0U; row < result.Rows(); ++row)
|
|
for (std::size_t column = 0U; column < result.Columns(); ++column)
|
|
result(row, column) = GradientDot(operators.rows[row].gradient,
|
|
chart.z_columns[column]);
|
|
return result;
|
|
}
|
|
|
|
Matrix PhysicalJacobian(const ConstraintOperators& operators,
|
|
const FixedChart& chart) {
|
|
Matrix result{operators.rows.size(), chart.p_dofs.size()};
|
|
for (std::size_t row = 0U; row < result.Rows(); ++row)
|
|
for (std::size_t column = 0U; column < result.Columns(); ++column)
|
|
result(row, column) = GradientDot(operators.rows[row].gradient,
|
|
chart.p_columns[column]);
|
|
return result;
|
|
}
|
|
|
|
Vector ContractConstraintHessian(const ConstraintOperators& operators,
|
|
const Vector& left, const Vector& right) {
|
|
const dense_blas_internal::ScopedLocalDenseThreads local_threads{};
|
|
Vector result{operators.rows.size()};
|
|
for (std::size_t row = 0U; row < operators.rows.size(); ++row) {
|
|
const auto& hessian = operators.rows[row].hessian;
|
|
Vector coefficients{hessian.size()};
|
|
Vector left_values{hessian.size()};
|
|
Vector right_values{hessian.size()};
|
|
for (std::size_t i = 0U; i < hessian.size(); ++i) {
|
|
coefficients[i] = hessian[i].value;
|
|
left_values[i] = left[hessian[i].row];
|
|
right_values[i] = right[hessian[i].column];
|
|
}
|
|
try {
|
|
const Vector first =
|
|
dense_blas_internal::MultiplyDiagonal(coefficients, left_values);
|
|
const Vector products =
|
|
dense_blas_internal::MultiplyDiagonal(first, right_values);
|
|
// Retain the original entry order and left-associated products; only
|
|
// ordinary products move to MKL. This is failure-only replay evidence.
|
|
for (std::size_t i = 0U; i < products.Size(); ++i)
|
|
result[row] += products[i];
|
|
} catch (const std::invalid_argument&) {
|
|
// A nonfinite input/intermediate cannot yield a finite original sum.
|
|
// Keep the existing caller's finite failure without a scalar fallback.
|
|
result[row] = std::numeric_limits<double>::quiet_NaN();
|
|
}
|
|
}
|
|
return result;
|
|
}
|
|
|
|
Result<EmbeddingRoot> SolveEmbeddingRoot(const FixedChart& chart,
|
|
const Vector& p, const Vector& seed,
|
|
const ConstraintEvaluator& evaluate,
|
|
TangentSolver& solver) {
|
|
if (!evaluate || p.Size() != chart.p_dofs.size() ||
|
|
seed.Size() != chart.z_dofs.size())
|
|
return Result<EmbeddingRoot>::Failure(
|
|
Failure("Reduced embedding root inputs are invalid."));
|
|
Vector z = seed;
|
|
Vector prior_z{seed.Size()};
|
|
Vector prior_q{chart.full_dof_count};
|
|
bool has_prior_z = false;
|
|
double last_residual_norm = 0.0;
|
|
double last_correction_norm = 0.0;
|
|
std::size_t last_residual_row = 0U;
|
|
std::size_t last_correction_row = 0U;
|
|
for (std::size_t iteration = 0U; iteration <= 100U; ++iteration) {
|
|
auto q = Lift(chart, p, z);
|
|
if (!q.HasValue()) return Result<EmbeddingRoot>::Failure(q.GetStatus());
|
|
auto operators = evaluate(q.Value());
|
|
if (!operators.HasValue())
|
|
return Result<EmbeddingRoot>::Failure(operators.GetStatus());
|
|
if (operators.Value().rows.size() != z.Size())
|
|
return Result<EmbeddingRoot>::Failure(
|
|
Failure("Reduced graph row count changed during its solve.",
|
|
"dynamic-mass-regularity"));
|
|
Vector residual{z.Size()};
|
|
double norm = 0.0;
|
|
double relative_norm = 0.0;
|
|
bool position_certified = true;
|
|
for (std::size_t row = 0U; row < z.Size(); ++row) {
|
|
residual[row] = operators.Value().rows[row].value;
|
|
if (!std::isfinite(residual[row]))
|
|
return Result<EmbeddingRoot>::Failure(
|
|
Failure("Reduced graph residual is nonfinite."));
|
|
const double scale = operators.Value().rows[row].value_scale;
|
|
if (!std::isfinite(scale) || scale < 0.0)
|
|
return Result<EmbeddingRoot>::Failure(
|
|
Failure("Reduced graph residual scale is invalid."));
|
|
const double relative =
|
|
scale == 0.0 ? (residual[row] == 0.0
|
|
? 0.0
|
|
: std::numeric_limits<double>::infinity())
|
|
: std::abs(residual[row]) / scale;
|
|
if (scale != 0.0 && !std::isfinite(relative))
|
|
return Result<EmbeddingRoot>::Failure(
|
|
Failure("Reduced graph residual normalization is nonfinite."));
|
|
relative_norm = std::max(relative_norm, relative);
|
|
position_certified = position_certified &&
|
|
(scale == 0.0 ? residual[row] == 0.0
|
|
: relative <= 1.0e-10);
|
|
if (std::abs(residual[row]) > norm) {
|
|
norm = std::abs(residual[row]);
|
|
last_residual_row = row;
|
|
}
|
|
}
|
|
last_residual_norm = norm;
|
|
// Position termination reuses the existing N3 hidden-constraint scale and
|
|
// N2 rotational-coordinate correction criterion. The caller still
|
|
// certifies every full endpoint quantity before commit.
|
|
if (norm == 0.0 ||
|
|
(position_certified &&
|
|
(iteration == 0U || last_correction_norm <= 1.0e-10)))
|
|
return Result<EmbeddingRoot>::Success(
|
|
{p, z, std::move(q.Value()), std::move(operators.Value()), norm,
|
|
relative_norm, last_correction_norm, iteration});
|
|
if (iteration == 100U) {
|
|
std::ostringstream message;
|
|
message << "stage=inner-graph: reduced graph exhausted 100 Newton corrections; last residual infinity norm "
|
|
<< last_residual_norm << " at row " << last_residual_row
|
|
<< ", last correction infinity norm " << last_correction_norm
|
|
<< " at column " << last_correction_row
|
|
<< ", normalized residual infinity norm " << relative_norm << ".";
|
|
return Result<EmbeddingRoot>::Failure(
|
|
Failure(message.str(), "dynamic-nonconvergence"));
|
|
}
|
|
const Matrix graph = GraphJacobian(operators.Value(), chart);
|
|
auto correction = SolveGraphSystem(graph, residual, solver);
|
|
if (!correction.HasValue())
|
|
return Result<EmbeddingRoot>::Failure(correction.GetStatus());
|
|
last_correction_norm = 0.0;
|
|
bool changed = false;
|
|
Vector next_z{z.Size()};
|
|
for (std::size_t i = 0U; i < z.Size(); ++i) {
|
|
const double next = z[i] - correction.Value()[i];
|
|
changed = changed || next != z[i];
|
|
next_z[i] = next;
|
|
if (std::abs(correction.Value()[i]) > last_correction_norm) {
|
|
last_correction_norm = std::abs(correction.Value()[i]);
|
|
last_correction_row = i;
|
|
}
|
|
}
|
|
// A binary64 graph can reach an unrepresentable residual before it reaches
|
|
// literal zero. Return that immutable trial only so the caller can apply
|
|
// the existing full hidden-constraint acceptance gates; it is never an
|
|
// independent reduced acceptance tolerance.
|
|
if (!changed)
|
|
return Result<EmbeddingRoot>::Success(
|
|
{p, z, std::move(q.Value()), std::move(operators.Value()), norm,
|
|
relative_norm, last_correction_norm, iteration});
|
|
// A two-cycle is another binary64 stagnation state. Keep the already
|
|
// evaluated trial (rather than accepting the graph) so the caller's
|
|
// existing original hidden-constraint evidence decides whether it is
|
|
// admissible.
|
|
if (has_prior_z && ExactlyEqual(next_z, prior_z))
|
|
return Result<EmbeddingRoot>::Success(
|
|
{p, z, std::move(q.Value()), std::move(operators.Value()), norm,
|
|
relative_norm, last_correction_norm, iteration});
|
|
auto next_q = Lift(chart, p, next_z);
|
|
if (!next_q.HasValue()) return Result<EmbeddingRoot>::Failure(next_q.GetStatus());
|
|
// Fixed-basis lifting can absorb changed private coordinates. Return only
|
|
// the current evaluated trial; original full endpoint checks still decide.
|
|
if (BitwiseEqual(next_q.Value(), q.Value()) ||
|
|
(has_prior_z && BitwiseEqual(next_q.Value(), prior_q)))
|
|
return Result<EmbeddingRoot>::Success(
|
|
{p, z, std::move(q.Value()), std::move(operators.Value()), norm,
|
|
relative_norm, last_correction_norm, iteration});
|
|
prior_z = z;
|
|
prior_q = q.Value();
|
|
has_prior_z = true;
|
|
z = std::move(next_z);
|
|
}
|
|
return Result<EmbeddingRoot>::Failure(Failure("Unreachable graph loop."));
|
|
}
|
|
|
|
Result<EmbeddingFirstDerivatives> EvaluateEmbeddingFirstDerivatives(
|
|
const FixedChart& chart, const EmbeddingRoot& root, TangentSolver& solver) {
|
|
if (!ValidBasis(chart) || root.p.Size() != chart.p_dofs.size() || root.z.Size() != chart.z_dofs.size() ||
|
|
root.q.Size() != chart.full_dof_count ||
|
|
root.operators.rows.size() != chart.z_dofs.size())
|
|
return Result<EmbeddingFirstDerivatives>::Failure(
|
|
Failure("Reduced embedding root does not match its fixed chart.",
|
|
"dynamic-mass-regularity"));
|
|
const Matrix graph = GraphJacobian(root.operators, chart);
|
|
const Matrix physical = PhysicalJacobian(root.operators, chart);
|
|
Matrix jacobian{chart.full_dof_count, chart.p_dofs.size()};
|
|
for (std::size_t column = 0U; column < chart.p_dofs.size(); ++column) {
|
|
Vector rhs{chart.z_dofs.size()};
|
|
for (std::size_t row = 0U; row < rhs.Size(); ++row)
|
|
rhs[row] = -physical(row, column);
|
|
auto dz = SolveGraphSystem(graph, rhs, solver);
|
|
if (!dz.HasValue())
|
|
return Result<EmbeddingFirstDerivatives>::Failure(dz.GetStatus());
|
|
Vector unit{chart.p_columns.size()};
|
|
unit[column] = 1.0;
|
|
auto action = Lift(chart, unit, dz.Value());
|
|
if (!action.HasValue())
|
|
return Result<EmbeddingFirstDerivatives>::Failure(action.GetStatus());
|
|
for (std::size_t row = 0U; row < action.Value().Size(); ++row)
|
|
jacobian(row, column) = action.Value()[row];
|
|
}
|
|
Vector rhs{chart.z_dofs.size()};
|
|
for (std::size_t row = 0U; row < rhs.Size(); ++row)
|
|
rhs[row] = -root.operators.rows[row].time_first;
|
|
auto dzdt = SolveGraphSystem(graph, rhs, solver);
|
|
if (!dzdt.HasValue())
|
|
return Result<EmbeddingFirstDerivatives>::Failure(dzdt.GetStatus());
|
|
auto time = Lift(chart, Vector{chart.p_columns.size()}, dzdt.Value());
|
|
if (!time.HasValue())
|
|
return Result<EmbeddingFirstDerivatives>::Failure(time.GetStatus());
|
|
return Result<EmbeddingFirstDerivatives>::Success(
|
|
{std::move(jacobian), std::move(time.Value())});
|
|
}
|
|
|
|
Result<ReconstructedRates> ReconstructPhysicalRates(
|
|
const FixedChart& chart, const EmbeddingRoot& root, const Vector& vp,
|
|
const Vector& ap, TangentSolver& solver) {
|
|
if (!ValidBasis(chart) || root.q.Size() != chart.full_dof_count ||
|
|
root.operators.rows.size() != chart.z_dofs.size() ||
|
|
vp.Size() != chart.p_dofs.size() || ap.Size() != chart.p_dofs.size() ||
|
|
!Finite(vp) || !Finite(ap))
|
|
return Result<ReconstructedRates>::Failure(
|
|
Failure("Reduced physical-rate reconstruction inputs are invalid."));
|
|
const Matrix graph = GraphJacobian(root.operators, chart);
|
|
auto p_velocity = Lift(chart, vp, Vector{chart.z_columns.size()});
|
|
auto p_acceleration = Lift(chart, ap, Vector{chart.z_columns.size()});
|
|
if (!p_velocity.HasValue())
|
|
return Result<ReconstructedRates>::Failure(p_velocity.GetStatus());
|
|
if (!p_acceleration.HasValue())
|
|
return Result<ReconstructedRates>::Failure(p_acceleration.GetStatus());
|
|
Vector velocity_rhs{chart.z_dofs.size()};
|
|
for (std::size_t row = 0U; row < root.operators.rows.size(); ++row) {
|
|
RateSum rhs;
|
|
rhs.Add(-root.operators.rows[row].time_first);
|
|
for (const auto& entry : root.operators.rows[row].gradient)
|
|
rhs.Add(-entry.value * p_velocity.Value()[entry.dof]);
|
|
velocity_rhs[row] = rhs.Value();
|
|
}
|
|
auto z_velocity = SolveGraphSystem(graph, velocity_rhs, solver);
|
|
if (!z_velocity.HasValue())
|
|
return Result<ReconstructedRates>::Failure(z_velocity.GetStatus());
|
|
auto velocity = Lift(chart, vp, z_velocity.Value());
|
|
if (!velocity.HasValue())
|
|
return Result<ReconstructedRates>::Failure(velocity.GetStatus());
|
|
Vector acceleration_rhs{chart.z_dofs.size()};
|
|
for (std::size_t row = 0U; row < root.operators.rows.size(); ++row) {
|
|
RateSum hessian_velocity;
|
|
for (const auto& entry : root.operators.rows[row].hessian)
|
|
hessian_velocity.Add(entry.value * velocity.Value()[entry.row] *
|
|
velocity.Value()[entry.column]);
|
|
RateSum time_velocity;
|
|
for (const auto& entry : root.operators.rows[row].time_gradient)
|
|
time_velocity.Add(entry.value * velocity.Value()[entry.dof]);
|
|
RateSum rhs;
|
|
rhs.Add(-hessian_velocity.Value());
|
|
rhs.Add(-2.0 * time_velocity.Value());
|
|
rhs.Add(-root.operators.rows[row].time_second);
|
|
for (const auto& entry : root.operators.rows[row].gradient)
|
|
rhs.Add(-entry.value * p_acceleration.Value()[entry.dof]);
|
|
acceleration_rhs[row] = rhs.Value();
|
|
}
|
|
auto z_acceleration = SolveGraphSystem(graph, acceleration_rhs, solver);
|
|
if (!z_acceleration.HasValue())
|
|
return Result<ReconstructedRates>::Failure(z_acceleration.GetStatus());
|
|
auto acceleration = Lift(chart, ap, z_acceleration.Value());
|
|
if (!acceleration.HasValue())
|
|
return Result<ReconstructedRates>::Failure(acceleration.GetStatus());
|
|
return Result<ReconstructedRates>::Success(
|
|
{std::move(velocity.Value()), std::move(acceleration.Value()),
|
|
std::move(z_velocity.Value()), std::move(z_acceleration.Value())});
|
|
}
|
|
|
|
bool ShouldRefineAngular(
|
|
const FixedChart& chart, const std::array<double, 2U>& residual_metrics,
|
|
const std::array<double, 4U>& translation_corrections,
|
|
const bool translation_private_algorithmic_pass) {
|
|
bool has_translation = false;
|
|
bool has_rotation = !chart.z_dofs.empty();
|
|
if (!ValidBasis(chart)) return false;
|
|
for (const auto& column : chart.p_columns) {
|
|
if (column.channel == 0U) has_translation = true;
|
|
else has_rotation = true;
|
|
}
|
|
if (!has_translation || !has_rotation ||
|
|
!translation_private_algorithmic_pass ||
|
|
!std::isfinite(residual_metrics[0U]) ||
|
|
!std::isfinite(residual_metrics[1U]) ||
|
|
residual_metrics[0U] < 0.0 || residual_metrics[0U] > 1e-10 ||
|
|
residual_metrics[1U] <= 1e-10)
|
|
return false;
|
|
for (const auto correction : translation_corrections)
|
|
if (!std::isfinite(correction) || correction < 0.0 ||
|
|
correction > 1e-10)
|
|
return false;
|
|
return true;
|
|
}
|
|
|
|
Result<AugmentedSolveSelection> SelectAugmentedSolve(
|
|
const AugmentedReducedSystem& full, const FixedChart& chart,
|
|
const bool angular_only) {
|
|
const std::size_t np = chart.p_dofs.size();
|
|
const std::size_t size = np + 3U * chart.z_dofs.size();
|
|
if (!ValidBasis(chart) || full.physical_count != np ||
|
|
full.algebraic_count != chart.z_dofs.size() ||
|
|
full.tangent.Rows() != size || full.tangent.Columns() != size ||
|
|
full.rhs.Size() != size || !Finite(full.rhs))
|
|
return Result<AugmentedSolveSelection>::Failure(
|
|
Failure("Reduced Newton selector inventory is invalid."));
|
|
std::vector<std::size_t> indices;
|
|
for (std::size_t i = 0U; i < np; ++i) {
|
|
if (chart.p_dofs[i] >= chart.full_dof_count)
|
|
return Result<AugmentedSolveSelection>::Failure(
|
|
Failure("Reduced Newton selector DOF is invalid."));
|
|
if (!angular_only || chart.p_columns[i].channel == 1U)
|
|
indices.push_back(i);
|
|
}
|
|
for (std::size_t i = np; i < size; ++i) indices.push_back(i);
|
|
if (!angular_only)
|
|
return Result<AugmentedSolveSelection>::Success(
|
|
{full.tangent, full.rhs, std::move(indices), false});
|
|
std::vector<std::size_t> inverse(size, size);
|
|
for (std::size_t i = 0U; i < indices.size(); ++i) inverse[indices[i]] = i;
|
|
Vector rhs{indices.size()};
|
|
std::vector<CooContribution> entries;
|
|
for (std::size_t row = 0U; row < indices.size(); ++row) {
|
|
rhs[row] = full.rhs[indices[row]];
|
|
for (std::size_t k = full.tangent.RowOffsets()[indices[row]];
|
|
k < full.tangent.RowOffsets()[indices[row] + 1U]; ++k) {
|
|
const auto column = inverse[full.tangent.ColumnIndices()[k]];
|
|
if (column != size)
|
|
entries.push_back({row, column, full.tangent.Values()[k], 0U, k});
|
|
}
|
|
}
|
|
auto tangent = hht_internal::MakeSparse(indices.size(), indices.size(),
|
|
std::move(entries));
|
|
if (!tangent.HasValue())
|
|
return Result<AugmentedSolveSelection>::Failure(tangent.GetStatus());
|
|
return Result<AugmentedSolveSelection>::Success(
|
|
{std::move(tangent.Value()), std::move(rhs), std::move(indices), true});
|
|
}
|
|
|
|
Result<Vector> EmbedAugmentedSolution(
|
|
const AugmentedSolveSelection& selected, const Vector& solution,
|
|
const std::size_t full_size) {
|
|
if (solution.Size() != selected.full_indices.size() || !Finite(solution))
|
|
return Result<Vector>::Failure(
|
|
Failure("Reduced Newton selected solution is invalid."));
|
|
Vector full{full_size};
|
|
std::size_t previous = 0U;
|
|
for (std::size_t i = 0U; i < selected.full_indices.size(); ++i) {
|
|
const auto index = selected.full_indices[i];
|
|
if (index >= full_size || (i != 0U && index <= previous))
|
|
return Result<Vector>::Failure(
|
|
Failure("Reduced Newton solution embed inventory is invalid."));
|
|
full[index] = solution[i];
|
|
previous = index;
|
|
}
|
|
return Result<Vector>::Success(std::move(full));
|
|
}
|
|
|
|
Result<AugmentedReducedSystem> AssembleAugmentedReducedSystem(
|
|
const hht_internal::DynamicAssembly& assembly, const FixedChart& chart,
|
|
const ConstraintOperators& operators, const double velocity_rate,
|
|
const double acceleration_rate, const double reference_length,
|
|
const double time_increment) {
|
|
const std::size_t np = chart.p_columns.size();
|
|
const std::size_t nz = chart.z_columns.size();
|
|
const std::size_t total = np + 3U * nz;
|
|
const auto& mass = assembly.mass;
|
|
const auto& position = assembly.position_tangent;
|
|
const auto& velocity = assembly.velocity_tangent;
|
|
const auto& internal = assembly.internal_tangent;
|
|
const double h2 = time_increment * time_increment;
|
|
if (!ValidBasis(chart) || operators.rows.size() != nz ||
|
|
chart.full_dof_count != mass.Rows() || mass.Rows() != mass.Columns() ||
|
|
position.Rows() != mass.Rows() || velocity.Rows() != mass.Rows() ||
|
|
internal.Rows() != mass.Rows() ||
|
|
assembly.frame.residual.Size() != chart.full_dof_count ||
|
|
!std::isfinite(velocity_rate) || !std::isfinite(acceleration_rate) ||
|
|
!std::isfinite(reference_length) || !(reference_length > 0.0) ||
|
|
!std::isfinite(time_increment) || !(time_increment > 0.0) ||
|
|
!std::isfinite(h2) || !(h2 > 0.0) ||
|
|
!std::isfinite(1.0 / time_increment) || !std::isfinite(1.0 / h2))
|
|
return Result<AugmentedReducedSystem>::Failure(
|
|
Failure("Reduced augmented tangent inventory or scaling is invalid."));
|
|
if (mass.RowOffsets() != position.RowOffsets() ||
|
|
mass.ColumnIndices() != position.ColumnIndices() ||
|
|
mass.RowOffsets() != velocity.RowOffsets() ||
|
|
mass.ColumnIndices() != velocity.ColumnIndices() ||
|
|
mass.RowOffsets() != internal.RowOffsets() ||
|
|
mass.ColumnIndices() != internal.ColumnIndices())
|
|
return Result<AugmentedReducedSystem>::Failure(
|
|
Failure("Reduced augmented tangent requires one common full pattern."));
|
|
using Support = std::pair<std::size_t, double>;
|
|
std::vector<std::vector<Support>> p_support(chart.full_dof_count);
|
|
std::vector<std::vector<Support>> z_support(chart.full_dof_count);
|
|
for (std::size_t i = 0U; i < np; ++i)
|
|
for (const auto& entry : chart.p_columns[i].support)
|
|
p_support[entry.dof].push_back({i, entry.value});
|
|
for (std::size_t i = 0U; i < nz; ++i)
|
|
for (const auto& entry : chart.z_columns[i].support)
|
|
z_support[entry.dof].push_back({i, entry.value});
|
|
std::map<std::pair<std::size_t, std::size_t>, RateSum> blocks;
|
|
const auto add = [&](const std::size_t row, const std::size_t column,
|
|
const double value) { blocks[{row, column}].Add(value); };
|
|
auto residual = ProjectPhysical(chart, assembly.frame.residual);
|
|
if (!residual.HasValue())
|
|
return Result<AugmentedReducedSystem>::Failure(residual.GetStatus());
|
|
Vector rhs{total};
|
|
for (std::size_t pr = 0U; pr < np; ++pr) {
|
|
rhs[pr] = -residual.Value()[pr];
|
|
for (const auto& row_basis : chart.p_columns[pr].support) {
|
|
const auto row = row_basis.dof;
|
|
for (std::size_t offset = mass.RowOffsets()[row];
|
|
offset < mass.RowOffsets()[row + 1U]; ++offset) {
|
|
const auto column = mass.ColumnIndices()[offset];
|
|
const double kq = position.Values()[offset] + internal.Values()[offset];
|
|
const double iv = velocity.Values()[offset];
|
|
const double m = mass.Values()[offset];
|
|
for (const auto& basis : p_support[column])
|
|
add(pr, basis.first,
|
|
row_basis.value * (kq + velocity_rate * iv + acceleration_rate * m) *
|
|
basis.second);
|
|
for (const auto& basis : z_support[column]) {
|
|
add(pr, np + basis.first, row_basis.value * kq * basis.second);
|
|
add(pr, np + nz + basis.first, row_basis.value * iv * basis.second);
|
|
add(pr, np + 2U * nz + basis.first, row_basis.value * m * basis.second);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
for (std::size_t constraint = 0U; constraint < nz; ++constraint) {
|
|
const auto& row = operators.rows[constraint];
|
|
std::map<std::size_t, RateSum> gradient_w;
|
|
std::map<std::size_t, RateSum> curvature;
|
|
for (const auto& entry : row.time_gradient)
|
|
gradient_w[entry.dof].Add(entry.value);
|
|
for (const auto& entry : row.hessian) {
|
|
gradient_w[entry.row].Add(entry.value * assembly.frame.velocity[entry.column]);
|
|
curvature[entry.row].Add(entry.value * assembly.frame.acceleration[entry.column]);
|
|
}
|
|
for (const auto& entry : row.third_directional_gradient)
|
|
curvature[entry.dof].Add(entry.value);
|
|
const auto contract = [&](const std::map<std::size_t, RateSum>& values,
|
|
const FixedBasisColumn& basis) {
|
|
RateSum sum;
|
|
for (const auto& entry : basis.support) {
|
|
const auto found = values.find(entry.dof);
|
|
if (found != values.end()) sum.Add(found->second.Value() * entry.value);
|
|
}
|
|
return sum.Value();
|
|
};
|
|
const auto qrow = np + constraint;
|
|
const auto vrow = np + nz + constraint;
|
|
const auto arow = np + 2U * nz + constraint;
|
|
for (std::size_t i = 0U; i < np; ++i) {
|
|
const double c = GradientDot(row.gradient, chart.p_columns[i]);
|
|
const double w = contract(gradient_w, chart.p_columns[i]);
|
|
const double a = contract(curvature, chart.p_columns[i]);
|
|
add(qrow, i, c);
|
|
add(vrow, i, velocity_rate * c + w);
|
|
add(arow, i, acceleration_rate * c + a + 2.0 * velocity_rate * w);
|
|
}
|
|
for (std::size_t i = 0U; i < nz; ++i) {
|
|
const double c = GradientDot(row.gradient, chart.z_columns[i]);
|
|
const double w = contract(gradient_w, chart.z_columns[i]);
|
|
const double a = contract(curvature, chart.z_columns[i]);
|
|
add(qrow, np + i, c);
|
|
add(vrow, np + i, w);
|
|
add(vrow, np + nz + i, c);
|
|
add(arow, np + i, a);
|
|
add(arow, np + nz + i, 2.0 * w);
|
|
add(arow, np + 2U * nz + i, c);
|
|
}
|
|
}
|
|
Vector row_scale{total};
|
|
Vector unknown_scale{total};
|
|
for (std::size_t i = 0U; i < np; ++i)
|
|
row_scale[i] = unknown_scale[i] =
|
|
chart.p_columns[i].channel == 0U ? reference_length : 1.0;
|
|
for (std::size_t i = 0U; i < nz; ++i) {
|
|
row_scale[np + i] = unknown_scale[np + i] = 1.0;
|
|
row_scale[np + nz + i] = time_increment;
|
|
unknown_scale[np + nz + i] = 1.0 / time_increment;
|
|
row_scale[np + 2U * nz + i] = h2;
|
|
unknown_scale[np + 2U * nz + i] = 1.0 / h2;
|
|
}
|
|
std::vector<CooContribution> entries;
|
|
entries.reserve(blocks.size());
|
|
for (const auto& block : blocks) {
|
|
const double raw = block.second.Value();
|
|
const double scaled = row_scale[block.first.first] * raw *
|
|
unknown_scale[block.first.second];
|
|
if (!std::isfinite(raw) || !std::isfinite(scaled) ||
|
|
(raw != 0.0 && scaled == 0.0))
|
|
return Result<AugmentedReducedSystem>::Failure(
|
|
Failure("Reduced augmented operator scaling overflowed or underflowed."));
|
|
entries.push_back({block.first.first, block.first.second, scaled, 0U, entries.size()});
|
|
}
|
|
for (std::size_t i = 0U; i < rhs.Size(); ++i) {
|
|
const double raw = rhs[i];
|
|
rhs[i] *= row_scale[i];
|
|
if (!std::isfinite(rhs[i]) || (raw != 0.0 && rhs[i] == 0.0))
|
|
return Result<AugmentedReducedSystem>::Failure(
|
|
Failure("Reduced augmented RHS scaling overflowed or underflowed."));
|
|
}
|
|
auto tangent = hht_internal::MakeSparse(total, total, std::move(entries));
|
|
if (!tangent.HasValue())
|
|
return Result<AugmentedReducedSystem>::Failure(tangent.GetStatus());
|
|
return Result<AugmentedReducedSystem>::Success(
|
|
{std::move(tangent.Value()), std::move(rhs), np, nz,
|
|
std::move(row_scale), std::move(unknown_scale)});
|
|
}
|
|
|
|
Result<AugmentedCorrection> MapAugmentedCorrection(
|
|
const FixedChart& chart, const AugmentedReducedSystem& system,
|
|
const Vector& solution, const double velocity_rate,
|
|
const double acceleration_rate) {
|
|
const std::size_t np = chart.p_dofs.size();
|
|
const std::size_t nz = chart.z_dofs.size();
|
|
if (!ValidBasis(chart) || system.physical_count != np || system.algebraic_count != nz ||
|
|
solution.Size() != np + 3U * nz || !Finite(solution) ||
|
|
system.unknown_scale.Size() != solution.Size() ||
|
|
!std::isfinite(velocity_rate) || !std::isfinite(acceleration_rate))
|
|
return Result<AugmentedCorrection>::Failure(
|
|
Failure("Reduced augmented correction inventory is invalid."));
|
|
AugmentedCorrection result{Vector{np}, Vector{nz}, Vector{nz}, Vector{nz},
|
|
Vector{chart.full_dof_count},
|
|
Vector{chart.full_dof_count},
|
|
Vector{chart.full_dof_count}};
|
|
for (std::size_t i = 0U; i < solution.Size(); ++i) {
|
|
const double scale = system.unknown_scale[i];
|
|
const double raw = solution[i] * scale;
|
|
if (!std::isfinite(scale) || !(scale > 0.0) || !std::isfinite(raw) ||
|
|
(solution[i] != 0.0 && raw == 0.0))
|
|
return Result<AugmentedCorrection>::Failure(
|
|
Failure("Reduced augmented scale undo overflowed or underflowed."));
|
|
}
|
|
for (std::size_t i = 0U; i < np; ++i) {
|
|
result.p[i] = solution[i] * system.unknown_scale[i];
|
|
}
|
|
for (std::size_t i = 0U; i < nz; ++i) {
|
|
result.z[i] = solution[np + i] * system.unknown_scale[np + i];
|
|
result.z_velocity[i] = solution[np + nz + i] * system.unknown_scale[np + nz + i];
|
|
result.z_acceleration[i] = solution[np + 2U * nz + i] * system.unknown_scale[np + 2U * nz + i];
|
|
}
|
|
Vector p_velocity{np};
|
|
Vector p_acceleration{np};
|
|
for (std::size_t i = 0U; i < np; ++i) {
|
|
p_velocity[i] = velocity_rate * result.p[i];
|
|
p_acceleration[i] = acceleration_rate * result.p[i];
|
|
}
|
|
auto coordinate = Lift(chart, result.p, result.z);
|
|
auto velocity = Lift(chart, p_velocity, result.z_velocity);
|
|
auto acceleration = Lift(chart, p_acceleration, result.z_acceleration);
|
|
if (!coordinate.HasValue()) return Result<AugmentedCorrection>::Failure(coordinate.GetStatus());
|
|
if (!velocity.HasValue()) return Result<AugmentedCorrection>::Failure(velocity.GetStatus());
|
|
if (!acceleration.HasValue()) return Result<AugmentedCorrection>::Failure(acceleration.GetStatus());
|
|
result.coordinate = std::move(coordinate.Value());
|
|
result.velocity = std::move(velocity.Value());
|
|
result.acceleration = std::move(acceleration.Value());
|
|
if (!Finite(result.coordinate) || !Finite(result.velocity) ||
|
|
!Finite(result.acceleration))
|
|
return Result<AugmentedCorrection>::Failure(
|
|
Failure("Reduced augmented correction mapped to nonfinite rates."));
|
|
return Result<AugmentedCorrection>::Success(std::move(result));
|
|
}
|
|
|
|
Result<Vector> SolveGraphSystem(const Matrix& matrix, const Vector& rhs,
|
|
TangentSolver& solver) {
|
|
if (matrix.Rows() != matrix.Columns() || rhs.Size() != matrix.Rows() ||
|
|
!Finite(rhs))
|
|
return Result<Vector>::Failure(
|
|
Failure("Reduced graph solve dimensions or RHS are invalid."));
|
|
for (std::size_t i = 0U; i < matrix.Rows(); ++i)
|
|
for (std::size_t j = 0U; j < matrix.Columns(); ++j)
|
|
if (!std::isfinite(matrix(i, j)))
|
|
return Result<Vector>::Failure(
|
|
Failure("Reduced graph matrix must remain finite."));
|
|
if (rhs.Size() == 0U) return Result<Vector>::Success(Vector{0U});
|
|
auto sparse = DenseSparse(matrix);
|
|
if (!sparse.HasValue()) return Result<Vector>::Failure(sparse.GetStatus());
|
|
Status status = solver.FactorizeFresh(sparse.Value());
|
|
if (!status.IsOk()) return Result<Vector>::Failure(status);
|
|
Vector solution{rhs.Size()};
|
|
status = solver.SolveCorrection(rhs, solution);
|
|
if (!status.IsOk()) return Result<Vector>::Failure(status);
|
|
status = hht_internal::CheckLinearSolve(sparse.Value(), rhs, solution);
|
|
if (!status.IsOk()) return Result<Vector>::Failure(status);
|
|
return Result<Vector>::Success(std::move(solution));
|
|
}
|
|
|
|
Result<double> CheckGraphRegularity(
|
|
const hht_internal::DynamicAssembly& assembly, const FixedChart& chart,
|
|
const ConstraintOperators& operators, TangentSolver& solver) {
|
|
const std::size_t size = chart.z_dofs.size();
|
|
if (!ValidBasis(chart) || operators.rows.size() != size || chart.null_nodes.size() != size ||
|
|
chart.pivot_components.size() != size ||
|
|
assembly.drilling_metric.Rows() != chart.full_dof_count ||
|
|
assembly.drilling_metric.Columns() != chart.full_dof_count ||
|
|
assembly.rotations.size() * 6U != chart.full_dof_count)
|
|
return Result<double>::Failure(
|
|
Failure("Reduced graph regularity inventory is invalid.",
|
|
"dynamic-mass-regularity"));
|
|
auto transverse = CheckSelectorTransversality(assembly, chart, solver);
|
|
if (!transverse.HasValue()) return Result<double>::Failure(transverse.GetStatus());
|
|
if (size == 0U) return Result<double>::Success(0.0);
|
|
Vector weights{size};
|
|
for (std::size_t row = 0U; row < size; ++row) {
|
|
const auto node = chart.null_nodes[row];
|
|
if (node >= assembly.rotations.size())
|
|
return Result<double>::Failure(
|
|
Failure("Reduced graph regularity node is invalid.",
|
|
"dynamic-mass-regularity"));
|
|
const auto& null = assembly.rotations[node].drill_null;
|
|
const double metric = hht_internal::OrderedNullMetric(
|
|
assembly.drilling_metric, node, null);
|
|
if (!std::isfinite(metric) || !(metric > 0.0))
|
|
return Result<double>::Failure(Failure(
|
|
"Reduced graph requires a positive finite null drilling metric.",
|
|
"dynamic-mass-regularity"));
|
|
weights[row] = std::sqrt(metric);
|
|
}
|
|
const Matrix graph = GraphJacobian(operators, chart);
|
|
Matrix scaled{size, size};
|
|
double norm = 0.0;
|
|
for (std::size_t row = 0U; row < size; ++row) {
|
|
double row_sum = 0.0;
|
|
for (std::size_t column = 0U; column < size; ++column) {
|
|
scaled(row, column) = graph(row, column) / weights[row] / weights[column];
|
|
if (!std::isfinite(scaled(row, column)))
|
|
return Result<double>::Failure(
|
|
Failure("Reduced graph scaled operator is nonfinite.",
|
|
"dynamic-mass-regularity"));
|
|
row_sum += std::abs(scaled(row, column));
|
|
}
|
|
norm = std::max(norm, row_sum);
|
|
}
|
|
auto sparse = DenseSparse(scaled);
|
|
if (!sparse.HasValue()) return Result<double>::Failure(sparse.GetStatus());
|
|
Status status = solver.FactorizeFresh(sparse.Value());
|
|
if (!status.IsOk()) return Result<double>::Failure(status);
|
|
Vector inverse_sums{size};
|
|
for (std::size_t column = 0U; column < size; ++column) {
|
|
Vector rhs{size};
|
|
rhs[column] = 1.0;
|
|
Vector solution{size};
|
|
status = solver.SolveCorrection(rhs, solution);
|
|
if (!status.IsOk()) return Result<double>::Failure(status);
|
|
status = hht_internal::CheckLinearSolve(sparse.Value(), rhs, solution);
|
|
if (!status.IsOk()) return Result<double>::Failure(status);
|
|
for (std::size_t row = 0U; row < size; ++row)
|
|
inverse_sums[row] += std::abs(solution[row]);
|
|
}
|
|
double inverse_norm = 0.0;
|
|
for (std::size_t row = 0U; row < size; ++row)
|
|
inverse_norm = std::max(inverse_norm, inverse_sums[row]);
|
|
const double condition = 1.0 / norm / inverse_norm;
|
|
if (!std::isfinite(norm) || !std::isfinite(inverse_norm) ||
|
|
!std::isfinite(condition) || !(condition >= std::ldexp(1.0, -32))) {
|
|
std::ostringstream message;
|
|
message << "Reduced graph reciprocal infinity condition " << condition
|
|
<< " is below 2^-32.";
|
|
return Result<double>::Failure(
|
|
Failure(message.str(), "dynamic-mass-regularity"));
|
|
}
|
|
return Result<double>::Success(condition);
|
|
}
|
|
|
|
Result<double> CheckSelectorTransversality(
|
|
const hht_internal::DynamicAssembly& assembly, const FixedChart& chart,
|
|
TangentSolver& solver) {
|
|
if (!ValidBasis(chart) || chart.null_nodes.size() != chart.z_columns.size() ||
|
|
assembly.rotations.size() * 6U != chart.full_dof_count)
|
|
return Result<double>::Failure(Failure(
|
|
"Reduced spanning operator inventory is invalid.", "dynamic-mass-regularity"));
|
|
if (chart.p_columns.empty() && chart.z_columns.empty())
|
|
return Result<double>::Success(0.0);
|
|
// N1 scaling cancels for identity translation columns. Angular blocks use 1.
|
|
double norm = 1.0;
|
|
double inverse_norm = 1.0;
|
|
for (std::size_t node = 0U; node < assembly.rotations.size(); ++node) {
|
|
std::vector<std::size_t> rows;
|
|
std::vector<const FixedBasisColumn*> columns;
|
|
for (const auto& column : chart.p_columns)
|
|
if (column.channel == 1U && column.node == node) columns.push_back(&column);
|
|
std::size_t null_row = chart.null_nodes.size();
|
|
for (std::size_t i = 0U; i < chart.null_nodes.size(); ++i)
|
|
if (chart.null_nodes[i] == node) null_row = i;
|
|
for (std::size_t component = 0U; component < 3U; ++component) {
|
|
const auto full = 6U * node + 3U + component;
|
|
bool present = false;
|
|
for (const auto* column : columns)
|
|
for (const auto& entry : column->support) present = present || entry.dof == full;
|
|
if (null_row != chart.null_nodes.size())
|
|
for (const auto& entry : chart.z_columns[null_row].support)
|
|
present = present || entry.dof == full;
|
|
if (present) rows.push_back(full);
|
|
}
|
|
const bool has_null = null_row != chart.null_nodes.size();
|
|
if (columns.size() + (has_null ? 1U : 0U) != rows.size())
|
|
return Result<double>::Failure(Failure(
|
|
"Reduced spanning block is not square.", "dynamic-mass-regularity"));
|
|
if (rows.empty()) continue;
|
|
Matrix block{rows.size(), rows.size()};
|
|
for (std::size_t row = 0U; row < rows.size(); ++row)
|
|
for (std::size_t column = 0U; column < columns.size(); ++column)
|
|
for (const auto& entry : columns[column]->support)
|
|
if (entry.dof == rows[row]) block(row, column) = entry.value;
|
|
if (has_null) {
|
|
std::array<BasisValue, 3U> values{};
|
|
for (std::size_t i = 0U; i < 3U; ++i)
|
|
values[i] = {assembly.rotations[node].drill_null[i], 0.0};
|
|
const double length = BasisNorm(values).value;
|
|
if (!std::isfinite(length) || !(length > 0.0))
|
|
return Result<double>::Failure(Failure(
|
|
"Reduced spanning block has an invalid actual null.", "dynamic-mass-regularity"));
|
|
for (std::size_t row = 0U; row < rows.size(); ++row)
|
|
block(row, columns.size()) = assembly.rotations[node].drill_null[rows[row] % 6U - 3U] / length;
|
|
}
|
|
for (std::size_t row = 0U; row < block.Rows(); ++row) {
|
|
double sum = 0.0;
|
|
for (std::size_t column = 0U; column < block.Columns(); ++column)
|
|
sum += std::abs(block(row, column));
|
|
norm = (std::max)(norm, sum);
|
|
}
|
|
auto sparse = DenseSparse(block);
|
|
if (!sparse.HasValue()) return Result<double>::Failure(sparse.GetStatus());
|
|
auto status = solver.FactorizeFresh(sparse.Value());
|
|
if (!status.IsOk()) return Result<double>::Failure(status);
|
|
Vector inverse_rows{rows.size()};
|
|
for (std::size_t column = 0U; column < rows.size(); ++column) {
|
|
Vector rhs{rows.size()};
|
|
rhs[column] = 1.0;
|
|
Vector solution{rows.size()};
|
|
status = solver.SolveCorrection(rhs, solution);
|
|
if (!status.IsOk()) return Result<double>::Failure(status);
|
|
status = hht_internal::CheckLinearSolve(sparse.Value(), rhs, solution);
|
|
if (!status.IsOk()) return Result<double>::Failure(status);
|
|
for (std::size_t row = 0U; row < rows.size(); ++row)
|
|
inverse_rows[row] += std::abs(solution[row]);
|
|
}
|
|
for (std::size_t row = 0U; row < rows.size(); ++row)
|
|
inverse_norm = (std::max)(inverse_norm, inverse_rows[row]);
|
|
}
|
|
const double condition = 1.0 / norm / inverse_norm;
|
|
if (!std::isfinite(condition) || condition < std::ldexp(1.0, -32))
|
|
return Result<double>::Failure(Failure(
|
|
"Reduced normalized spanning reciprocal condition is below 2^-32.",
|
|
"dynamic-mass-regularity"));
|
|
return Result<double>::Success(condition);
|
|
}
|
|
|
|
} // namespace fesa::hht_reduced_internal
|