KOKO\Mimi
98 lines
3.9 KiB
C++
98 lines
3.9 KiB
C++
#include "fesa/analysis/hht_time_integration.h"
|
|
|
|
#include <cmath>
|
|
#include <utility>
|
|
|
|
namespace fesa {
|
|
namespace {
|
|
|
|
/// @brief Reports invalid or unrepresentable HHT kinematics.
|
|
Status Failure() {
|
|
return Status::Failure(
|
|
FailureCategory::kSolver,
|
|
{{Severity::kError, "dynamic-invalid-state", {}, "DYNAMIC", {},
|
|
"HHT requires finite compatible rates, positive time, and coupled "
|
|
"coefficients in the supported range."}});
|
|
}
|
|
|
|
/// @brief Validates the coupled coefficient and time denominators.
|
|
bool Valid(const double h, const HhtParameters& p) {
|
|
return std::isfinite(h) && h > 0.0 && std::isfinite(h * h) &&
|
|
h * h > 0.0 && std::isfinite(p.alpha) &&
|
|
p.alpha >= -1.0 / 3.0 && p.alpha <= 0.0 &&
|
|
p.beta == 0.25 * (1.0 - p.alpha) * (1.0 - p.alpha) &&
|
|
p.gamma == 0.5 - p.alpha && p.beta * h * h > 0.0;
|
|
}
|
|
|
|
} // namespace
|
|
|
|
Result<HhtParameters> MakeHhtParameters(const double alpha) {
|
|
if (!std::isfinite(alpha) || alpha < -1.0 / 3.0 || alpha > 0.0) {
|
|
return Result<HhtParameters>::Failure(Failure());
|
|
}
|
|
return Result<HhtParameters>::Success(
|
|
{alpha, 0.25 * (1.0 - alpha) * (1.0 - alpha), 0.5 - alpha});
|
|
}
|
|
|
|
Result<HhtPredictor> PredictHht(const Vector& coordinates,
|
|
const Vector& velocity,
|
|
const Vector& algorithmic_acceleration,
|
|
const double time_increment,
|
|
const HhtParameters& parameters) {
|
|
if (!Valid(time_increment, parameters) ||
|
|
coordinates.Size() != velocity.Size() ||
|
|
coordinates.Size() != algorithmic_acceleration.Size()) {
|
|
return Result<HhtPredictor>::Failure(Failure());
|
|
}
|
|
HhtPredictor predictor{Vector(coordinates.Size()), Vector(coordinates.Size())};
|
|
for (std::size_t i = 0U; i < coordinates.Size(); ++i) {
|
|
predictor.coordinates[i] =
|
|
coordinates[i] + time_increment * velocity[i] +
|
|
time_increment * time_increment * (0.5 - parameters.beta) *
|
|
algorithmic_acceleration[i];
|
|
predictor.velocity[i] =
|
|
velocity[i] + time_increment * (1.0 - parameters.gamma) *
|
|
algorithmic_acceleration[i];
|
|
if (!std::isfinite(predictor.coordinates[i]) ||
|
|
!std::isfinite(predictor.velocity[i])) {
|
|
return Result<HhtPredictor>::Failure(Failure());
|
|
}
|
|
}
|
|
return Result<HhtPredictor>::Success(std::move(predictor));
|
|
}
|
|
|
|
Result<HhtRates> EvaluateHhtRates(const Vector& coordinates,
|
|
const HhtPredictor& predictor,
|
|
const Vector& old_acceleration,
|
|
const double time_increment,
|
|
const HhtParameters& parameters) {
|
|
if (!Valid(time_increment, parameters) ||
|
|
coordinates.Size() != predictor.coordinates.Size() ||
|
|
coordinates.Size() != predictor.velocity.Size() ||
|
|
coordinates.Size() != old_acceleration.Size()) {
|
|
return Result<HhtRates>::Failure(Failure());
|
|
}
|
|
HhtRates rates{Vector(coordinates.Size()), Vector(coordinates.Size()),
|
|
Vector(coordinates.Size())};
|
|
for (std::size_t i = 0U; i < coordinates.Size(); ++i) {
|
|
const double increment = coordinates[i] - predictor.coordinates[i];
|
|
rates.algorithmic_acceleration[i] =
|
|
increment / (parameters.beta * time_increment * time_increment);
|
|
rates.velocity[i] = predictor.velocity[i] +
|
|
parameters.gamma * increment /
|
|
(parameters.beta * time_increment);
|
|
rates.acceleration[i] =
|
|
(rates.algorithmic_acceleration[i] +
|
|
parameters.alpha * old_acceleration[i]) /
|
|
(1.0 + parameters.alpha);
|
|
if (!std::isfinite(rates.algorithmic_acceleration[i]) ||
|
|
!std::isfinite(rates.acceleration[i]) ||
|
|
!std::isfinite(rates.velocity[i])) {
|
|
return Result<HhtRates>::Failure(Failure());
|
|
}
|
|
}
|
|
return Result<HhtRates>::Success(std::move(rates));
|
|
}
|
|
|
|
} // namespace fesa
|