KOKO\Mimi
872 lines
35 KiB
C++
872 lines
35 KiB
C++
#include "fesa/elements/nonlinear_mitc4_inertia.h"
|
|
|
|
#include <algorithm>
|
|
#include <array>
|
|
#include <cmath>
|
|
#include <cstddef>
|
|
#include <limits>
|
|
#include <string>
|
|
#include <utility>
|
|
|
|
#include "fesa/elements/finite_rotation.h"
|
|
|
|
namespace fesa {
|
|
namespace {
|
|
|
|
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};
|
|
|
|
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) {
|
|
if (!identity.empty()) {
|
|
identity += "-";
|
|
}
|
|
identity += node == nullptr ? "?" : node->source_id.source_label_text;
|
|
}
|
|
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{};
|
|
std::array<double, 3> d{};
|
|
std::array<std::array<double, 3>, 3> dd{};
|
|
std::array<std::array<std::array<double, 3>, 3>, 3> ddd{};
|
|
};
|
|
|
|
Jet Constant(const double value) { return Jet{value, {}, {}, {}}; }
|
|
|
|
Jet Variable(const double value, const std::size_t component) {
|
|
Jet result = Constant(value);
|
|
result.d[component] = 1.0;
|
|
return result;
|
|
}
|
|
|
|
Jet operator+(const Jet& left, const Jet& right) {
|
|
Jet result{};
|
|
result.value = left.value + right.value;
|
|
for (std::size_t i = 0U; i < 3U; ++i) {
|
|
result.d[i] = left.d[i] + right.d[i];
|
|
for (std::size_t j = 0U; j < 3U; ++j) {
|
|
result.dd[i][j] = left.dd[i][j] + right.dd[i][j];
|
|
for (std::size_t k = 0U; k < 3U; ++k) {
|
|
result.ddd[i][j][k] = left.ddd[i][j][k] + right.ddd[i][j][k];
|
|
}
|
|
}
|
|
}
|
|
return result;
|
|
}
|
|
|
|
Jet operator-(const Jet& left, const Jet& right) {
|
|
Jet result{};
|
|
result.value = left.value - right.value;
|
|
for (std::size_t i = 0U; i < 3U; ++i) {
|
|
result.d[i] = left.d[i] - right.d[i];
|
|
for (std::size_t j = 0U; j < 3U; ++j) {
|
|
result.dd[i][j] = left.dd[i][j] - right.dd[i][j];
|
|
for (std::size_t k = 0U; k < 3U; ++k) {
|
|
result.ddd[i][j][k] = left.ddd[i][j][k] - right.ddd[i][j][k];
|
|
}
|
|
}
|
|
}
|
|
return result;
|
|
}
|
|
|
|
Jet operator*(const Jet& left, const Jet& right) {
|
|
Jet result{};
|
|
result.value = left.value * right.value;
|
|
for (std::size_t i = 0U; i < 3U; ++i) {
|
|
result.d[i] = left.d[i] * right.value + left.value * right.d[i];
|
|
for (std::size_t j = 0U; j < 3U; ++j) {
|
|
result.dd[i][j] = left.dd[i][j] * right.value + left.d[i] * right.d[j] +
|
|
left.d[j] * right.d[i] + left.value * right.dd[i][j];
|
|
for (std::size_t k = 0U; k < 3U; ++k) {
|
|
result.ddd[i][j][k] =
|
|
left.ddd[i][j][k] * right.value + left.dd[i][j] * right.d[k] +
|
|
left.dd[i][k] * right.d[j] + left.dd[j][k] * right.d[i] +
|
|
left.d[i] * right.dd[j][k] + left.d[j] * right.dd[i][k] +
|
|
left.d[k] * right.dd[i][j] + left.value * right.ddd[i][j][k];
|
|
}
|
|
}
|
|
}
|
|
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);
|
|
const double cubed = result.value * result.value * result.value;
|
|
const double fifth = cubed * result.value * result.value;
|
|
const double seventh = fifth * result.value * result.value;
|
|
for (std::size_t i = 0U; i < 3U; ++i) {
|
|
result.d[i] = -0.5 * cubed * value.d[i];
|
|
for (std::size_t j = 0U; j < 3U; ++j) {
|
|
result.dd[i][j] =
|
|
0.75 * fifth * value.d[i] * value.d[j] - 0.5 * cubed * value.dd[i][j];
|
|
for (std::size_t k = 0U; k < 3U; ++k) {
|
|
result.ddd[i][j][k] =
|
|
-1.875 * seventh * value.d[i] * value.d[j] * value.d[k] +
|
|
0.75 * fifth *
|
|
(value.dd[i][j] * value.d[k] + value.dd[i][k] * value.d[j] +
|
|
value.dd[j][k] * value.d[i]) -
|
|
0.5 * cubed * value.ddd[i][j][k];
|
|
}
|
|
}
|
|
}
|
|
return result;
|
|
}
|
|
|
|
Jet Scale(const double scalar, const Jet& value) {
|
|
return Constant(scalar) * value;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
JetMatrix IdentityMatrix() {
|
|
JetMatrix result{};
|
|
for (std::size_t index = 0U; index < 3U; ++index) {
|
|
result[index][index] = Constant(1.0);
|
|
}
|
|
return result;
|
|
}
|
|
|
|
JetMatrix Skew(const JetVector& value) {
|
|
JetMatrix result{};
|
|
result[0U][1U] = Scale(-1.0, value[2U]);
|
|
result[0U][2U] = value[1U];
|
|
result[1U][0U] = value[2U];
|
|
result[1U][2U] = Scale(-1.0, value[0U]);
|
|
result[2U][0U] = Scale(-1.0, value[1U]);
|
|
result[2U][1U] = value[0U];
|
|
return result;
|
|
}
|
|
|
|
Jet Dot(const JetVector& left, const JetVector& right) {
|
|
Jet result = Constant(0.0);
|
|
for (std::size_t component = 0U; component < 3U; ++component) {
|
|
result = result + left[component] * right[component];
|
|
}
|
|
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;
|
|
}
|
|
}
|
|
return result;
|
|
}
|
|
|
|
JetMatrix LeftJacobian(const JetVector& phi) {
|
|
const JetMatrix skew = Skew(phi);
|
|
JetMatrix result = IdentityMatrix();
|
|
JetMatrix term = IdentityMatrix();
|
|
double factorial = 1.0;
|
|
for (std::size_t order = 1U; order <= 32U; ++order) {
|
|
term = Multiply(term, skew);
|
|
factorial *= static_cast<double>(order + 1U);
|
|
for (std::size_t row = 0U; row < 3U; ++row) {
|
|
for (std::size_t column = 0U; column < 3U; ++column) {
|
|
result[row][column] =
|
|
result[row][column] + Scale(1.0 / factorial, term[row][column]);
|
|
}
|
|
}
|
|
}
|
|
return result;
|
|
}
|
|
|
|
/// @brief Uses a converged exponential series so D1/D2/D3 share one analytic
|
|
/// map.
|
|
JetMatrix ExpSkew(const JetVector& phi) {
|
|
const JetMatrix skew = Skew(phi);
|
|
JetMatrix result = IdentityMatrix();
|
|
JetMatrix term = IdentityMatrix();
|
|
double factorial = 1.0;
|
|
for (std::size_t order = 1U; order <= 32U; ++order) {
|
|
term = Multiply(term, skew);
|
|
factorial *= static_cast<double>(order);
|
|
for (std::size_t row = 0U; row < 3U; ++row) {
|
|
for (std::size_t column = 0U; column < 3U; ++column) {
|
|
result[row][column] =
|
|
result[row][column] + Scale(1.0 / factorial, term[row][column]);
|
|
}
|
|
}
|
|
}
|
|
return result;
|
|
}
|
|
|
|
double Determinant(const Vector3& a, const Vector3& b, const Vector3& c) {
|
|
return a.Dot(b.Cross(c));
|
|
}
|
|
|
|
/// @brief Checks the scaled physical twenty-dimensional mass without a rank
|
|
/// cut.
|
|
Status CheckPhysicalMass(const Matrix& mass,
|
|
const std::array<Vector3, kNodes>& nulls,
|
|
const double length, const SourceLocation& location,
|
|
const std::string& identity) {
|
|
const auto fail = [&](const std::string& code, const std::string& message) {
|
|
return Status::Failure(
|
|
FailureCategory::kModel,
|
|
{{Severity::kError, code, location, "*ELEMENT", identity, message}});
|
|
};
|
|
constexpr std::size_t kPhysical = 20U;
|
|
Matrix scaled{kDofs, kDofs};
|
|
double scale = 0.0;
|
|
for (std::size_t row = 0U; row < kDofs; ++row) {
|
|
for (std::size_t column = 0U; column < kDofs; ++column) {
|
|
scaled(row, column) = mass(row, column) * (row % 6U < 3U ? length : 1.0) *
|
|
(column % 6U < 3U ? length : 1.0);
|
|
if (mass(row, column) != 0.0 && scaled(row, column) == 0.0) {
|
|
return fail("invalid-nonlinear-inertia-positive-mass",
|
|
"Physical mass scaling underflowed a nonzero entry.");
|
|
}
|
|
scale = std::max(scale, std::abs(scaled(row, column)));
|
|
}
|
|
}
|
|
if (!IsFinite(scaled) || !std::isfinite(scale) || !(scale > 0.0)) {
|
|
return fail("invalid-nonlinear-inertia-positive-mass",
|
|
"Scaled physical mass must have a finite positive scale.");
|
|
}
|
|
// Scaling by a positive entry preserves conditioning and avoids density
|
|
// overflow in the Cholesky and inverse-action norms.
|
|
for (std::size_t row = 0U; row < kDofs; ++row) {
|
|
for (std::size_t column = 0U; column < kDofs; ++column) {
|
|
scaled(row, column) /= scale;
|
|
}
|
|
}
|
|
Matrix complement{kDofs, kPhysical};
|
|
for (std::size_t node = 0U; node < kNodes; ++node) {
|
|
const Vector3& normal = nulls[node];
|
|
std::array<std::size_t, 3> axes{0U, 1U, 2U};
|
|
std::stable_sort(axes.begin(), axes.end(),
|
|
[&](const std::size_t a, const std::size_t b) {
|
|
return std::abs(normal[a]) < std::abs(normal[b]);
|
|
});
|
|
std::array<Vector3, 2> tangent{};
|
|
for (std::size_t column = 0U; column < 2U; ++column) {
|
|
std::array<double, 3> axis{};
|
|
axis[axes[column]] = 1.0;
|
|
Vector3 direction{axis};
|
|
direction = direction - direction.Dot(normal) * normal;
|
|
for (std::size_t previous = 0U; previous < column; ++previous) {
|
|
direction =
|
|
direction - direction.Dot(tangent[previous]) * tangent[previous];
|
|
}
|
|
const double norm = direction.Norm();
|
|
if (!std::isfinite(norm) || !(norm > 0.0)) {
|
|
return fail("invalid-nonlinear-inertia-null",
|
|
"The analytic physical complement must remain finite.");
|
|
}
|
|
tangent[column] = direction / norm;
|
|
}
|
|
for (std::size_t row = 0U; row < 3U; ++row) {
|
|
complement(6U * node + row, 5U * node + row) = 1.0;
|
|
for (std::size_t column = 0U; column < 2U; ++column) {
|
|
complement(6U * node + 3U + row, 5U * node + 3U + column) =
|
|
tangent[column][row];
|
|
}
|
|
}
|
|
}
|
|
double mass_norm = 0.0;
|
|
double symmetry_norm = 0.0;
|
|
double null_action_norm = 0.0;
|
|
for (std::size_t row = 0U; row < kDofs; ++row) {
|
|
for (std::size_t column = 0U; column < kDofs; ++column) {
|
|
mass_norm = std::hypot(mass_norm, scaled(row, column));
|
|
symmetry_norm =
|
|
std::hypot(symmetry_norm, scaled(row, column) - scaled(column, row));
|
|
}
|
|
for (std::size_t node = 0U; node < kNodes; ++node) {
|
|
double action = 0.0;
|
|
for (std::size_t component = 0U; component < 3U; ++component) {
|
|
action +=
|
|
scaled(row, 6U * node + 3U + component) * nulls[node][component];
|
|
}
|
|
null_action_norm = std::hypot(null_action_norm, action);
|
|
}
|
|
}
|
|
double gram_error = 0.0;
|
|
double orthogonality_error = 0.0;
|
|
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);
|
|
}
|
|
gram_error = std::hypot(gram_error, entry);
|
|
}
|
|
for (std::size_t node = 0U; node < kNodes; ++node) {
|
|
double entry = 0.0;
|
|
for (std::size_t component = 0U; component < 3U; ++component) {
|
|
entry += complement(6U * node + 3U + component, row) *
|
|
nulls[node][component];
|
|
}
|
|
orthogonality_error = std::hypot(orthogonality_error, entry);
|
|
}
|
|
}
|
|
double null_gram_error = 0.0;
|
|
for (const Vector3& normal : nulls) {
|
|
null_gram_error = std::hypot(null_gram_error, normal.Dot(normal) - 1.0);
|
|
}
|
|
const double full_gram_error = std::hypot(
|
|
gram_error,
|
|
std::hypot(null_gram_error, std::sqrt(2.0) * orthogonality_error));
|
|
if (!(symmetry_norm / mass_norm <= 1.0e-12) ||
|
|
!(null_action_norm / mass_norm / 2.0 <= 1.0e-12) ||
|
|
!(full_gram_error <= 1.0e-12)) {
|
|
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);
|
|
}
|
|
}
|
|
}
|
|
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);
|
|
}
|
|
}
|
|
std::array<double, kPhysical> inverse_row_sums{};
|
|
double reconstruction_error = 0.0;
|
|
double block_frobenius = 0.0;
|
|
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));
|
|
block_frobenius = std::hypot(block_frobenius, block(row, column));
|
|
}
|
|
}
|
|
if (!(reconstruction_error / block_frobenius <= 1.0e-12)) {
|
|
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 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))) {
|
|
return fail("ill-conditioned-nonlinear-inertia-positive-mass",
|
|
"Physical mass reciprocal infinity condition is below 2^-32.");
|
|
}
|
|
return Status::Ok();
|
|
}
|
|
|
|
} // namespace
|
|
|
|
Result<DynamicRotationKinematics> EvaluateDynamicRotation(
|
|
const Vector3& phi_value, const Vector3& initial_director,
|
|
const Vector3& velocity, const Vector3& acceleration) {
|
|
if (!phi_value.IsFinite() || !initial_director.IsFinite() ||
|
|
!velocity.IsFinite() || !acceleration.IsFinite() ||
|
|
!(std::abs(initial_director.Norm() - 1.0) <= 1.0e-12) ||
|
|
!(std::acos(-1.0) - phi_value.Norm() >
|
|
std::sqrt(std::numeric_limits<double>::epsilon()))) {
|
|
return Failure<DynamicRotationKinematics>(
|
|
"invalid-dynamic-rotation", {{}, 0U}, "DYNAMIC_ROTATION",
|
|
"Dynamic rotation requires finite principal coordinates and a unit "
|
|
"initial director.");
|
|
}
|
|
JetVector phi{};
|
|
for (std::size_t component = 0U; component < 3U; ++component) {
|
|
phi[component] = Variable(phi_value[component], component);
|
|
}
|
|
const JetVector initial{Constant(initial_director.X()),
|
|
Constant(initial_director.Y()),
|
|
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);
|
|
const Jet null_norm_squared = Dot(null_jet, null_jet);
|
|
if (!std::isfinite(null_norm_squared.value) ||
|
|
!(null_norm_squared.value > 0.0)) {
|
|
return Failure<DynamicRotationKinematics>(
|
|
"invalid-dynamic-rotation", {{}, 0U}, "DYNAMIC_ROTATION",
|
|
"Dynamic rotation produced a nonpositive drilling-null norm.");
|
|
}
|
|
const Jet inverse_norm = InverseSqrt(null_norm_squared);
|
|
for (Jet& component : null_jet) {
|
|
component = component * inverse_norm;
|
|
}
|
|
std::array<double, 3> directional_second{};
|
|
std::array<double, 3> angular_velocity{};
|
|
std::array<double, 3> angular_acceleration{};
|
|
Matrix gradient{3U, 3U};
|
|
for (std::size_t row = 0U; row < 3U; ++row) {
|
|
for (std::size_t column = 0U; column < 3U; ++column) {
|
|
gradient(row, column) = null_jet[row].d[column];
|
|
}
|
|
}
|
|
for (std::size_t row = 0U; row < 3U; ++row) {
|
|
double second = 0.0;
|
|
double omega = 0.0;
|
|
double omega_dot = 0.0;
|
|
for (std::size_t column = 0U; column < 3U; ++column) {
|
|
omega += left_jacobian[row][column].value * velocity[column];
|
|
omega_dot += left_jacobian[row][column].value * acceleration[column];
|
|
for (std::size_t other = 0U; other < 3U; ++other) {
|
|
second += null_jet[row].dd[column][other] * velocity[column] *
|
|
velocity[other];
|
|
omega_dot += left_jacobian[row][column].d[other] * velocity[other] *
|
|
velocity[column];
|
|
}
|
|
}
|
|
directional_second[row] = second;
|
|
angular_velocity[row] = omega;
|
|
angular_acceleration[row] = omega_dot;
|
|
}
|
|
const Vector3 director{director_jet[0U].value, director_jet[1U].value,
|
|
director_jet[2U].value};
|
|
const Vector3 drill_null{null_jet[0U].value, null_jet[1U].value,
|
|
null_jet[2U].value};
|
|
const Vector3 directional_second_value{directional_second};
|
|
const Vector3 angular_velocity_value{angular_velocity};
|
|
const Vector3 angular_acceleration_value{angular_acceleration};
|
|
if (!director.IsFinite() || !drill_null.IsFinite() ||
|
|
!directional_second_value.IsFinite() ||
|
|
!angular_velocity_value.IsFinite() ||
|
|
!angular_acceleration_value.IsFinite() || !IsFinite(gradient)) {
|
|
return Failure<DynamicRotationKinematics>(
|
|
"invalid-dynamic-rotation", {{}, 0U}, "DYNAMIC_ROTATION",
|
|
"Dynamic rotation evaluation must remain finite.");
|
|
}
|
|
DynamicRotationKinematics result{director,
|
|
drill_null,
|
|
std::move(gradient),
|
|
directional_second_value,
|
|
angular_velocity_value,
|
|
angular_acceleration_value,
|
|
{},
|
|
{},
|
|
{}};
|
|
for (std::size_t i = 0U; i < 3U; ++i) {
|
|
result.director_first[i] = {director_jet[0].d[i], director_jet[1].d[i],
|
|
director_jet[2].d[i]};
|
|
for (std::size_t j = 0U; j < 3U; ++j) {
|
|
result.director_second[i][j] = {director_jet[0].dd[i][j],
|
|
director_jet[1].dd[i][j],
|
|
director_jet[2].dd[i][j]};
|
|
for (std::size_t k = 0U; k < 3U; ++k) {
|
|
result.director_third[i][j][k] = {director_jet[0].ddd[i][j][k],
|
|
director_jet[1].ddd[i][j][k],
|
|
director_jet[2].ddd[i][j][k]};
|
|
}
|
|
}
|
|
}
|
|
return Result<DynamicRotationKinematics>::Success(std::move(result));
|
|
}
|
|
|
|
NonlinearMitc4Inertia::NonlinearMitc4Inertia(
|
|
std::array<Vector3, 4> coordinates, std::array<Matrix, 4> initial_frames,
|
|
std::array<Vector3, 4> initial_directors, const double thickness,
|
|
const double density, SourceLocation source_location, std::string identity)
|
|
: coordinates_(std::move(coordinates)),
|
|
initial_frames_(std::move(initial_frames)),
|
|
initial_directors_(std::move(initial_directors)),
|
|
thickness_(thickness),
|
|
density_(density),
|
|
source_location_(std::move(source_location)),
|
|
identity_(std::move(identity)) {}
|
|
|
|
Result<NonlinearMitc4Inertia> NonlinearMitc4Inertia::Create(
|
|
std::array<const Node*, 4> nodes,
|
|
std::array<ShellNodeInitialFrame, 4> initial_frames,
|
|
const ShellSection& section, const LinearElasticMaterial& material,
|
|
const double density) {
|
|
const std::string identity = Identity(nodes);
|
|
if (!std::isfinite(density) || !(density > 0.0)) {
|
|
return Failure<NonlinearMitc4Inertia>(
|
|
"invalid-nonlinear-inertia-density", section.location, identity,
|
|
"Nonlinear MITC4 inertia requires a finite positive density.");
|
|
}
|
|
const auto shell =
|
|
NonlinearMitc4Shell::Create(nodes, initial_frames, section, material);
|
|
if (!shell.HasValue()) {
|
|
return Result<NonlinearMitc4Inertia>::Failure(shell.GetStatus());
|
|
}
|
|
std::array<Vector3, kNodes> coordinates{};
|
|
std::array<Matrix, kNodes> frames{Matrix{3U, 3U}, Matrix{3U, 3U},
|
|
Matrix{3U, 3U}, Matrix{3U, 3U}};
|
|
std::array<Vector3, kNodes> directors{};
|
|
for (std::size_t node = 0U; node < kNodes; ++node) {
|
|
coordinates[node] = Vector3{nodes[node]->coordinates};
|
|
frames[node] = FrameMatrix(initial_frames[node]);
|
|
directors[node] = Vector3{initial_frames[node].director};
|
|
}
|
|
NonlinearMitc4Inertia inertia{
|
|
coordinates, std::move(frames), directors, section.thickness,
|
|
density, nodes[0U]->location, identity};
|
|
const auto initial = inertia.Evaluate({}, Vector{kDofs}, Vector{kDofs});
|
|
if (!initial.HasValue()) {
|
|
return Result<NonlinearMitc4Inertia>::Failure(initial.GetStatus());
|
|
}
|
|
return Result<NonlinearMitc4Inertia>::Success(std::move(inertia));
|
|
}
|
|
|
|
Result<NonlinearMitc4InertiaEvaluation> NonlinearMitc4Inertia::Evaluate(
|
|
const NonlinearMitc4TrialDofs& trial, const Vector& velocity,
|
|
const Vector& acceleration) const {
|
|
if (velocity.Size() != kDofs || acceleration.Size() != kDofs ||
|
|
!IsFinite(velocity) || !IsFinite(acceleration)) {
|
|
return Failure<NonlinearMitc4InertiaEvaluation>(
|
|
"invalid-nonlinear-inertia-rate", source_location_, identity_,
|
|
"Nonlinear MITC4 inertia requires 24 finite rates.");
|
|
}
|
|
std::array<DynamicRotationKinematics, kNodes> rotations{};
|
|
std::array<Vector3, kNodes> nulls{};
|
|
double length = 0.0;
|
|
for (std::size_t node = 0U; node < kNodes; ++node) {
|
|
length = std::max(
|
|
length,
|
|
(coordinates_[(node + 1U) % kNodes] - coordinates_[node]).Norm());
|
|
if (!trial.displacement[node].IsFinite() ||
|
|
!MakeFiniteRotationState(trial.rotation_vector[node],
|
|
initial_frames_[node])
|
|
.HasValue()) {
|
|
return Failure<NonlinearMitc4InertiaEvaluation>(
|
|
"invalid-nonlinear-inertia-trial", source_location_, identity_,
|
|
"Nonlinear MITC4 inertia trial must be in the principal chart.");
|
|
}
|
|
const auto rotation = EvaluateDynamicRotation(
|
|
trial.rotation_vector[node], initial_directors_[node], {}, {});
|
|
if (!rotation.HasValue()) {
|
|
return Result<NonlinearMitc4InertiaEvaluation>::Failure(
|
|
rotation.GetStatus());
|
|
}
|
|
rotations[node] = rotation.Value();
|
|
nulls[node] = rotation.Value().drill_null;
|
|
for (std::size_t previous = 0U; previous < node; ++previous) {
|
|
if (!(rotations[node].director.Dot(rotations[previous].director) >
|
|
-1.0 + 1.0e-8)) {
|
|
return Failure<NonlinearMitc4InertiaEvaluation>(
|
|
"invalid-nonlinear-inertia-director", source_location_, identity_,
|
|
"Nonlinear MITC4 inertia trial directors must not be opposed.");
|
|
}
|
|
}
|
|
}
|
|
if (!std::isfinite(length) || !(length > 0.0)) {
|
|
return Failure<NonlinearMitc4InertiaEvaluation>(
|
|
"invalid-nonlinear-inertia-geometry", source_location_, identity_,
|
|
"Nonlinear MITC4 inertia requires a finite positive initial edge.");
|
|
}
|
|
|
|
NonlinearMitc4InertiaEvaluation result{Matrix{kDofs, kDofs},
|
|
Matrix{kDofs, kDofs},
|
|
Matrix{kDofs, kDofs},
|
|
Vector{kDofs},
|
|
Vector{kDofs},
|
|
0.0,
|
|
{},
|
|
{},
|
|
{},
|
|
{}};
|
|
for (std::size_t xi = 0U; xi < kGauss.size(); ++xi) {
|
|
for (std::size_t eta = 0U; eta < kGauss.size(); ++eta) {
|
|
const Mitc4ShapeFunctions shape =
|
|
Mitc4Shell::ShapeFunctions(kGauss[xi], kGauss[eta]);
|
|
for (std::size_t zeta = 0U; zeta < kGauss.size(); ++zeta) {
|
|
const double z = 0.5 * thickness_ * kGauss[zeta];
|
|
const double weight = kWeights[xi] * kWeights[eta] * kWeights[zeta];
|
|
Vector3 ref_xi{};
|
|
Vector3 ref_eta{};
|
|
Vector3 ref_zeta{};
|
|
Vector3 current_xi{};
|
|
Vector3 current_eta{};
|
|
Vector3 current_zeta{};
|
|
Vector3 position{};
|
|
Vector3 velocity_point{};
|
|
Vector3 acceleration_point{};
|
|
Vector3 convective_point{};
|
|
std::array<Vector3, kDofs> h{};
|
|
std::array<Vector3, kDofs> velocity_derivative{};
|
|
std::array<Vector3, kDofs> position_derivative{};
|
|
for (std::size_t node = 0U; node < kNodes; ++node) {
|
|
const std::size_t base = 6U * node;
|
|
const auto& rotation = rotations[node];
|
|
const Vector3 reference =
|
|
coordinates_[node] + z * initial_directors_[node];
|
|
const Vector3 current = coordinates_[node] +
|
|
trial.displacement[node] +
|
|
z * rotation.director;
|
|
ref_xi = ref_xi + shape.xi_derivatives[node] * reference;
|
|
ref_eta = ref_eta + shape.eta_derivatives[node] * reference;
|
|
ref_zeta = ref_zeta + 0.5 * thickness_ * shape.values[node] *
|
|
initial_directors_[node];
|
|
current_xi = current_xi + shape.xi_derivatives[node] * current;
|
|
current_eta = current_eta + shape.eta_derivatives[node] * current;
|
|
current_zeta = current_zeta + 0.5 * thickness_ * shape.values[node] *
|
|
rotation.director;
|
|
position = position + shape.values[node] * current;
|
|
for (std::size_t component = 0U; component < 3U; ++component) {
|
|
std::array<double, 3> translation{};
|
|
translation[component] = shape.values[node];
|
|
h[base + component] = Vector3{translation};
|
|
h[base + 3U + component] =
|
|
z * shape.values[node] * rotation.director_first[component];
|
|
for (std::size_t other = 0U; other < 3U; ++other) {
|
|
const Vector3 second = z * shape.values[node] *
|
|
rotation.director_second[component][other];
|
|
convective_point =
|
|
convective_point + second * velocity[base + 3U + component] *
|
|
velocity[base + 3U + other];
|
|
velocity_derivative[base + 3U + component] =
|
|
velocity_derivative[base + 3U + component] +
|
|
2.0 * second * velocity[base + 3U + other];
|
|
position_derivative[base + 3U + component] =
|
|
position_derivative[base + 3U + component] +
|
|
second * acceleration[base + 3U + other];
|
|
for (std::size_t third = 0U; third < 3U; ++third) {
|
|
position_derivative[base + 3U + component] =
|
|
position_derivative[base + 3U + component] +
|
|
z * shape.values[node] *
|
|
rotation.director_third[component][other][third] *
|
|
velocity[base + 3U + other] *
|
|
velocity[base + 3U + third];
|
|
}
|
|
}
|
|
}
|
|
}
|
|
const double jacobian = Determinant(ref_xi, ref_eta, ref_zeta);
|
|
const double current_jacobian =
|
|
Determinant(current_xi, current_eta, current_zeta);
|
|
if (!std::isfinite(jacobian) || !(jacobian > 0.0) ||
|
|
!std::isfinite(current_jacobian) || !(current_jacobian > 0.0)) {
|
|
return Failure<NonlinearMitc4InertiaEvaluation>(
|
|
"invalid-nonlinear-inertia-geometry", source_location_, identity_,
|
|
"Inertia requires positive reference and current mass-point "
|
|
"Jacobians.");
|
|
}
|
|
for (std::size_t row = 0U; row < kDofs; ++row) {
|
|
velocity_point = velocity_point + h[row] * velocity[row];
|
|
acceleration_point = acceleration_point + h[row] * acceleration[row];
|
|
}
|
|
acceleration_point = acceleration_point + convective_point;
|
|
const double scale = density_ * weight * jacobian;
|
|
if (!std::isfinite(scale) || !(scale > 0.0)) {
|
|
return Failure<NonlinearMitc4InertiaEvaluation>(
|
|
"invalid-nonlinear-inertia-positive-mass", source_location_,
|
|
identity_,
|
|
"Mass-point integration weight must be finite and positive.");
|
|
}
|
|
result.kinetic_energy +=
|
|
0.5 * scale * velocity_point.Dot(velocity_point);
|
|
result.momentum = result.momentum + scale * velocity_point;
|
|
result.momentum_rate =
|
|
result.momentum_rate + scale * acceleration_point;
|
|
result.angular_momentum =
|
|
result.angular_momentum + scale * position.Cross(velocity_point);
|
|
result.angular_momentum_rate =
|
|
result.angular_momentum_rate +
|
|
scale * position.Cross(acceleration_point);
|
|
result.momentum_rate_scale += scale * acceleration_point.Norm();
|
|
result.angular_momentum_rate_scale +=
|
|
scale * position.Cross(acceleration_point).Norm();
|
|
for (std::size_t row = 0U; row < kDofs; ++row) {
|
|
result.force[row] += scale * h[row].Dot(acceleration_point);
|
|
// Keep c independent of a; subtracting M*a destroys small c values.
|
|
result.convective_force[row] += scale * h[row].Dot(convective_point);
|
|
for (std::size_t column = 0U; column < kDofs; ++column) {
|
|
result.mass(row, column) += scale * h[row].Dot(h[column]);
|
|
result.velocity_tangent(row, column) +=
|
|
scale * h[row].Dot(velocity_derivative[column]);
|
|
double position_entry = h[row].Dot(position_derivative[column]);
|
|
if (row / 6U == column / 6U && row % 6U >= 3U &&
|
|
column % 6U >= 3U) {
|
|
const std::size_t node = row / 6U;
|
|
position_entry +=
|
|
z * shape.values[node] *
|
|
rotations[node]
|
|
.director_second[row % 6U - 3U][column % 6U - 3U]
|
|
.Dot(acceleration_point);
|
|
}
|
|
result.position_tangent(row, column) += scale * position_entry;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
if (!IsFinite(result.mass) || !IsFinite(result.position_tangent) ||
|
|
!IsFinite(result.velocity_tangent) || !IsFinite(result.force) ||
|
|
!IsFinite(result.convective_force) ||
|
|
!std::isfinite(result.kinetic_energy) || !result.momentum.IsFinite() ||
|
|
!result.angular_momentum.IsFinite() || !result.momentum_rate.IsFinite() ||
|
|
!result.angular_momentum_rate.IsFinite() ||
|
|
!std::isfinite(result.momentum_rate_scale) ||
|
|
!std::isfinite(result.angular_momentum_rate_scale)) {
|
|
return Failure<NonlinearMitc4InertiaEvaluation>(
|
|
"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_);
|
|
if (!physical_mass.IsOk()) {
|
|
return Result<NonlinearMitc4InertiaEvaluation>::Failure(physical_mass);
|
|
}
|
|
return Result<NonlinearMitc4InertiaEvaluation>::Success(std::move(result));
|
|
}
|
|
|
|
} // namespace fesa
|