feat(hex8): integrate paired HHT dynamic state and bounded verification
This commit is contained in:
KOKO\Mimi
@@ -0,0 +1,190 @@
|
||||
#ifndef FESA_ANALYSIS_SOLID_HHT_HIGH_LOW_H_
|
||||
#define FESA_ANALYSIS_SOLID_HHT_HIGH_LOW_H_
|
||||
|
||||
#include "fesa/analysis/hht_time_integration.h"
|
||||
#include "fesa/analysis/solid_dynamic_analysis_state.h"
|
||||
#include "fesa/math/sparse_matrix.h"
|
||||
#include "math/high_low_arithmetic.h"
|
||||
|
||||
namespace fesa::detail {
|
||||
/// @brief Keeps both physical parts of the fixed-increment predictor.
|
||||
struct SolidHhtPairedPredictor {
|
||||
Vector displacement{0};
|
||||
Vector displacement_low{0};
|
||||
Vector velocity{0};
|
||||
Vector velocity_low{0};
|
||||
};
|
||||
|
||||
/// @brief Owns physical and auxiliary rates without collapsing either part.
|
||||
struct SolidHhtPairedRates {
|
||||
Vector velocity{0};
|
||||
Vector velocity_low{0};
|
||||
Vector acceleration{0};
|
||||
Vector acceleration_low{0};
|
||||
Vector algorithmic_acceleration{0};
|
||||
Vector algorithmic_acceleration_low{0};
|
||||
};
|
||||
|
||||
struct SolidHhtPairedVector {
|
||||
Vector high{0};
|
||||
Vector low{0};
|
||||
};
|
||||
|
||||
inline Status SolidHhtPairedKinematicsFailure() {
|
||||
return Status::Failure(FailureCategory::kSolver,
|
||||
{{Severity::kError,
|
||||
"solid-dynamic-invalid-paired-kinematics",
|
||||
{},
|
||||
"DYNAMIC",
|
||||
{},
|
||||
"Incomplete, unnormalized or nonfinite paired HHT "
|
||||
"kinematics."}});
|
||||
}
|
||||
|
||||
inline bool ValidSolidHhtPair(const Vector& high, const Vector& low,
|
||||
std::size_t size) {
|
||||
if (high.Size() != size || low.Size() != size) return false;
|
||||
for (std::size_t i = 0; i < size; ++i) {
|
||||
const math_internal::HighLow value(high[i], low[i]);
|
||||
if (!math_internal::IsFinite(value) || !math_internal::IsNormalized(value))
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
inline bool ValidSolidHhtParameters(double h, const HhtParameters& p) {
|
||||
if (!math_internal::ArithmeticEnvironmentSupported()) return false;
|
||||
const auto expected = MakeHhtParameters(p.alpha);
|
||||
if (!expected.HasValue() || p.beta != expected.Value().beta ||
|
||||
p.gamma != expected.Value().gamma)
|
||||
return false;
|
||||
const math_internal::HighLow step(h);
|
||||
const auto square = math_internal::Multiply(step, step);
|
||||
return math_internal::IsFinite(step) &&
|
||||
math_internal::Compare(step, math_internal::HighLow(0)) > 0 &&
|
||||
math_internal::IsFinite(square) &&
|
||||
math_internal::Compare(square, math_internal::HighLow(0)) > 0;
|
||||
}
|
||||
|
||||
/// @brief Uses the authoritative constant Float64 mass coefficients without
|
||||
/// collapsing either physical vector part or the ordered row accumulation.
|
||||
inline Result<SolidHhtPairedVector> MultiplySolidHhtMassHighLow(
|
||||
const SparseMatrix& mass, const Vector& high, const Vector& low) {
|
||||
using namespace math_internal;
|
||||
if (!ArithmeticEnvironmentSupported() || mass.Rows() != mass.Columns() ||
|
||||
!ValidSolidHhtPair(high, low, mass.Columns()))
|
||||
return Result<SolidHhtPairedVector>::Failure(
|
||||
SolidHhtPairedKinematicsFailure());
|
||||
SolidHhtPairedVector result{Vector(mass.Rows()), Vector(mass.Rows())};
|
||||
for (std::size_t row = 0; row < mass.Rows(); ++row) {
|
||||
HighLow sum;
|
||||
for (std::size_t k = mass.RowOffsets()[row]; k < mass.RowOffsets()[row + 1];
|
||||
++k) {
|
||||
const auto col = mass.ColumnIndices()[k];
|
||||
sum = Add(sum, Multiply(HighLow(mass.Values()[k]),
|
||||
HighLow(high[col], low[col])));
|
||||
if (!IsFinite(sum) || !IsNormalized(sum))
|
||||
return Result<SolidHhtPairedVector>::Failure(
|
||||
SolidHhtPairedKinematicsFailure());
|
||||
}
|
||||
result.high[row] = sum.high;
|
||||
result.low[row] = sum.low;
|
||||
}
|
||||
return Result<SolidHhtPairedVector>::Success(std::move(result));
|
||||
}
|
||||
|
||||
/// @brief Applies the unchanged Newmark predictor to the full physical state.
|
||||
inline Result<SolidHhtPairedPredictor> PredictSolidHhtHighLow(
|
||||
const SolidDynamicAcceptedFrameState& old, double h,
|
||||
const HhtParameters& p) {
|
||||
using namespace math_internal;
|
||||
const auto size = old.displacement.Size();
|
||||
if (!ValidSolidHhtPair(old.displacement, old.low.displacement, size) ||
|
||||
!ValidSolidHhtPair(old.velocity, old.low.velocity, size) ||
|
||||
!ValidSolidHhtPair(old.algorithmic_acceleration,
|
||||
old.low.algorithmic_acceleration, size) ||
|
||||
!ValidSolidHhtParameters(h, p))
|
||||
return Result<SolidHhtPairedPredictor>::Failure(
|
||||
SolidHhtPairedKinematicsFailure());
|
||||
SolidHhtPairedPredictor result{Vector(size), Vector(size), Vector(size),
|
||||
Vector(size)};
|
||||
const auto hh = Multiply(HighLow(h), HighLow(h));
|
||||
const auto u_coefficient =
|
||||
Multiply(hh, Subtract(HighLow(.5), HighLow(p.beta)));
|
||||
const auto v_coefficient =
|
||||
Multiply(HighLow(h), Subtract(HighLow(1), HighLow(p.gamma)));
|
||||
if (!IsFinite(u_coefficient) || !IsFinite(v_coefficient))
|
||||
return Result<SolidHhtPairedPredictor>::Failure(
|
||||
SolidHhtPairedKinematicsFailure());
|
||||
for (std::size_t i = 0; i < size; ++i) {
|
||||
const HighLow u(old.displacement[i], old.low.displacement[i]);
|
||||
const HighLow v(old.velocity[i], old.low.velocity[i]);
|
||||
const HighLow A(old.algorithmic_acceleration[i],
|
||||
old.low.algorithmic_acceleration[i]);
|
||||
const auto up =
|
||||
Add(Add(u, Multiply(HighLow(h), v)), Multiply(u_coefficient, A));
|
||||
const auto vp = Add(v, Multiply(v_coefficient, A));
|
||||
if (!IsFinite(up) || !IsNormalized(up) || !IsFinite(vp) ||
|
||||
!IsNormalized(vp))
|
||||
return Result<SolidHhtPairedPredictor>::Failure(
|
||||
SolidHhtPairedKinematicsFailure());
|
||||
result.displacement[i] = up.high;
|
||||
result.displacement_low[i] = up.low;
|
||||
result.velocity[i] = vp.high;
|
||||
result.velocity_low[i] = vp.low;
|
||||
}
|
||||
return Result<SolidHhtPairedPredictor>::Success(std::move(result));
|
||||
}
|
||||
|
||||
/// @brief Recovers A, v and physical a from the paired displacement difference.
|
||||
inline Result<SolidHhtPairedRates> EvaluateSolidHhtHighLowRates(
|
||||
const Vector& high, const Vector& low,
|
||||
const SolidHhtPairedPredictor& predictor, const Vector& old_acceleration,
|
||||
const Vector& old_acceleration_low, double h, const HhtParameters& p) {
|
||||
using namespace math_internal;
|
||||
const auto size = high.Size();
|
||||
if (!ValidSolidHhtPair(high, low, size) ||
|
||||
!ValidSolidHhtPair(predictor.displacement, predictor.displacement_low,
|
||||
size) ||
|
||||
!ValidSolidHhtPair(predictor.velocity, predictor.velocity_low, size) ||
|
||||
!ValidSolidHhtPair(old_acceleration, old_acceleration_low, size) ||
|
||||
!ValidSolidHhtParameters(h, p))
|
||||
return Result<SolidHhtPairedRates>::Failure(
|
||||
SolidHhtPairedKinematicsFailure());
|
||||
SolidHhtPairedRates result{Vector(size), Vector(size), Vector(size),
|
||||
Vector(size), Vector(size), Vector(size)};
|
||||
const auto denominator =
|
||||
Multiply(Multiply(HighLow(p.beta), HighLow(h)), HighLow(h));
|
||||
const auto velocity_coefficient = Multiply(HighLow(p.gamma), HighLow(h));
|
||||
const auto physical_coefficient = Add(HighLow(1), HighLow(p.alpha));
|
||||
if (!IsFinite(denominator) || Compare(denominator, HighLow(0)) <= 0 ||
|
||||
!IsFinite(velocity_coefficient) || !IsFinite(physical_coefficient))
|
||||
return Result<SolidHhtPairedRates>::Failure(
|
||||
SolidHhtPairedKinematicsFailure());
|
||||
for (std::size_t i = 0; i < size; ++i) {
|
||||
const auto A = Divide(Subtract(HighLow(high[i], low[i]),
|
||||
HighLow(predictor.displacement[i],
|
||||
predictor.displacement_low[i])),
|
||||
denominator);
|
||||
const auto v =
|
||||
Add(HighLow(predictor.velocity[i], predictor.velocity_low[i]),
|
||||
Multiply(velocity_coefficient, A));
|
||||
const auto a = Divide(
|
||||
Add(A, Multiply(HighLow(p.alpha),
|
||||
HighLow(old_acceleration[i], old_acceleration_low[i]))),
|
||||
physical_coefficient);
|
||||
if (!IsFinite(A) || !IsNormalized(A) || !IsFinite(v) || !IsNormalized(v) ||
|
||||
!IsFinite(a) || !IsNormalized(a))
|
||||
return Result<SolidHhtPairedRates>::Failure(
|
||||
SolidHhtPairedKinematicsFailure());
|
||||
result.algorithmic_acceleration[i] = A.high;
|
||||
result.algorithmic_acceleration_low[i] = A.low;
|
||||
result.velocity[i] = v.high;
|
||||
result.velocity_low[i] = v.low;
|
||||
result.acceleration[i] = a.high;
|
||||
result.acceleration_low[i] = a.low;
|
||||
}
|
||||
return Result<SolidHhtPairedRates>::Success(std::move(result));
|
||||
}
|
||||
} // namespace fesa::detail
|
||||
#endif // FESA_ANALYSIS_SOLID_HHT_HIGH_LOW_H_
|
||||
Reference in New Issue
Block a user