KOKO\Mimi
1052 lines
44 KiB
C++
1052 lines
44 KiB
C++
#include "fesa/elements/nonlinear_mitc4_inertia.h"
|
|
|
|
#include <gtest/gtest.h>
|
|
|
|
#include <algorithm>
|
|
#include <array>
|
|
#include <cmath>
|
|
#include <cstddef>
|
|
#include <cstdint>
|
|
#include <iostream>
|
|
#include <limits>
|
|
#include <string>
|
|
#include <vector>
|
|
|
|
#include "fesa/elements/finite_rotation.h"
|
|
#include "fesa/model/model_types.h"
|
|
#include "fesa/model/shell_geometry.h"
|
|
|
|
namespace {
|
|
|
|
fesa::Node Node(const std::int64_t label,
|
|
const std::array<double, 3>& coordinates) {
|
|
return {{"Shell-Instance", label, std::to_string(label)},
|
|
coordinates,
|
|
{"nonlinear-inertia.inp", static_cast<std::size_t>(label)}};
|
|
}
|
|
|
|
std::array<fesa::Node, 4> Nodes() {
|
|
return {Node(1, {-1.0, -1.0, 0.0}), Node(2, {1.0, -1.0, 0.0}),
|
|
Node(3, {1.0, 1.0, 0.0}), Node(4, {-1.0, 1.0, 0.0})};
|
|
}
|
|
|
|
std::array<const fesa::Node*, 4> Pointers(
|
|
const std::array<fesa::Node, 4>& nodes) {
|
|
return {&nodes[0], &nodes[1], &nodes[2], &nodes[3]};
|
|
}
|
|
|
|
std::array<fesa::ShellNodeInitialFrame, 4> Frames() {
|
|
return {{{0U, {0.0, 0.0, 1.0}, {1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}},
|
|
{1U, {0.0, 0.0, 1.0}, {1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}},
|
|
{2U, {0.0, 0.0, 1.0}, {1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}},
|
|
{3U, {0.0, 0.0, 1.0}, {1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}}}};
|
|
}
|
|
|
|
fesa::NonlinearMitc4Inertia Inertia(const std::array<fesa::Node, 4>& nodes) {
|
|
const fesa::ShellSection section{
|
|
"Section", 0.2, 0U, {"nonlinear-inertia.inp", 20U}};
|
|
const fesa::LinearElasticMaterial material{
|
|
"Material", 120.0, 0.25, {"nonlinear-inertia.inp", 21U}};
|
|
const auto inertia = fesa::NonlinearMitc4Inertia::Create(
|
|
Pointers(nodes), Frames(), section, material, 1.0);
|
|
EXPECT_TRUE(inertia.HasValue());
|
|
return inertia.Value();
|
|
}
|
|
|
|
fesa::NonlinearMitc4TrialDofs AddRotation(fesa::NonlinearMitc4TrialDofs trial,
|
|
const fesa::Vector& direction,
|
|
const double step) {
|
|
for (std::size_t node = 0U; node < 4U; ++node) {
|
|
trial.displacement[node] =
|
|
trial.displacement[node] + fesa::Vector3{direction[6U * node],
|
|
direction[6U * node + 1U],
|
|
direction[6U * node + 2U]} *
|
|
step;
|
|
trial.rotation_vector[node] =
|
|
trial.rotation_vector[node] + fesa::Vector3{direction[6U * node + 3U],
|
|
direction[6U * node + 4U],
|
|
direction[6U * node + 5U]} *
|
|
step;
|
|
}
|
|
return trial;
|
|
}
|
|
|
|
fesa::Vector Difference(const fesa::Vector& left, const fesa::Vector& right,
|
|
const double denominator) {
|
|
fesa::Vector result = left;
|
|
result.Axpy(-1.0, right);
|
|
result.Scale(1.0 / denominator);
|
|
return result;
|
|
}
|
|
|
|
TEST(NonlinearMitc4Inertia, IntegratesTheRectangleConsistentMassAndNull) {
|
|
const auto nodes = Nodes();
|
|
const auto evaluation =
|
|
Inertia(nodes).Evaluate({}, fesa::Vector{24U}, fesa::Vector{24U});
|
|
ASSERT_TRUE(evaluation.HasValue());
|
|
const fesa::Matrix& mass = evaluation.Value().mass;
|
|
constexpr std::array<double, 4> kWeights{4.0, 2.0, 1.0, 2.0};
|
|
constexpr double kC0 = 0.8 / 36.0;
|
|
for (std::size_t row = 0U; row < 4U; ++row) {
|
|
for (std::size_t column = 0U; column < 4U; ++column) {
|
|
const std::size_t offset = (column + 4U - row) % 4U;
|
|
EXPECT_NEAR(mass(6U * row, 6U * column), kC0 * kWeights[offset], 1.0e-12);
|
|
}
|
|
EXPECT_NEAR(mass(6U * row + 5U, 6U * row + 5U), 0.0, 1.0e-12);
|
|
}
|
|
fesa::Vector drilling_null{24U};
|
|
for (std::size_t node = 0U; node < 4U; ++node) {
|
|
drilling_null[6U * node + 5U] = 1.0;
|
|
}
|
|
EXPECT_LE(mass.Multiply(drilling_null).Norm(), 1.0e-12);
|
|
}
|
|
|
|
TEST(NonlinearMitc4Inertia, UsesPhysicalAccelerationAndExactCurrentForce) {
|
|
const auto nodes = Nodes();
|
|
fesa::NonlinearMitc4TrialDofs trial{};
|
|
for (std::size_t node = 0U; node < 4U; ++node) {
|
|
trial.rotation_vector[node] = {0.05 * (node + 1U), -0.03 * (node + 1U),
|
|
0.02 * (node + 1U)};
|
|
}
|
|
fesa::Vector acceleration{24U};
|
|
for (std::size_t index = 0U; index < acceleration.Size(); ++index) {
|
|
acceleration[index] = 0.01 * static_cast<double>(index + 1U);
|
|
}
|
|
const auto evaluation =
|
|
Inertia(nodes).Evaluate(trial, fesa::Vector{24U}, acceleration);
|
|
ASSERT_TRUE(evaluation.HasValue());
|
|
const fesa::Vector expected = evaluation.Value().mass.Multiply(acceleration);
|
|
for (std::size_t index = 0U; index < expected.Size(); ++index) {
|
|
EXPECT_NEAR(evaluation.Value().force[index], expected[index], 1.0e-11);
|
|
EXPECT_NEAR(evaluation.Value().convective_force[index], 0.0, 1.0e-11);
|
|
}
|
|
}
|
|
|
|
using V3 = fesa::Vector3;
|
|
|
|
V3 Multiply(const fesa::Matrix& matrix, const V3& value) {
|
|
return {matrix(0, 0) * value.X() + matrix(0, 1) * value.Y() +
|
|
matrix(0, 2) * value.Z(),
|
|
matrix(1, 0) * value.X() + matrix(1, 1) * value.Y() +
|
|
matrix(1, 2) * value.Z(),
|
|
matrix(2, 0) * value.X() + matrix(2, 1) * value.Y() +
|
|
matrix(2, 2) * value.Z()};
|
|
}
|
|
|
|
// exp of the 24x24 subset-block matrix applied to (d0,0,...,0).
|
|
// Its off-diagonal blocks are the three direction skew matrices; the
|
|
// subset-7 block is the mixed third derivative. No Jl scalar is reused.
|
|
V3 BlockExponentialDerivative(const V3& phi, const V3& initial,
|
|
const std::vector<V3>& directions) {
|
|
std::array<V3, 8> term{};
|
|
std::array<V3, 8> sum{};
|
|
term[0] = initial;
|
|
sum[0] = initial;
|
|
const std::size_t blocks = std::size_t{1U} << directions.size();
|
|
for (std::size_t order = 1U; order <= 64U; ++order) {
|
|
std::array<V3, 8> next{};
|
|
for (std::size_t subset = 0U; subset < blocks; ++subset) {
|
|
next[subset] = phi.Cross(term[subset]);
|
|
for (std::size_t axis = 0U; axis < directions.size(); ++axis) {
|
|
const std::size_t bit = std::size_t{1U} << axis;
|
|
if ((subset & bit) != 0U) {
|
|
next[subset] =
|
|
next[subset] + directions[axis].Cross(term[subset ^ bit]);
|
|
}
|
|
}
|
|
next[subset] = next[subset] / static_cast<double>(order);
|
|
sum[subset] = sum[subset] + next[subset];
|
|
}
|
|
term = next;
|
|
}
|
|
return sum[blocks - 1U];
|
|
}
|
|
|
|
V3 Second(const fesa::DynamicRotationKinematics& value, const V3& p,
|
|
const V3& r) {
|
|
V3 result{};
|
|
for (std::size_t i = 0; i < 3; ++i) {
|
|
for (std::size_t j = 0; j < 3; ++j) {
|
|
result = result + value.director_second[i][j] * p[i] * r[j];
|
|
}
|
|
}
|
|
return result;
|
|
}
|
|
|
|
V3 Third(const fesa::DynamicRotationKinematics& value, const V3& p, const V3& r,
|
|
const V3& t) {
|
|
V3 result{};
|
|
for (std::size_t i = 0; i < 3; ++i) {
|
|
for (std::size_t j = 0; j < 3; ++j) {
|
|
for (std::size_t k = 0; k < 3; ++k) {
|
|
result = result + value.director_third[i][j][k] * p[i] * r[j] * t[k];
|
|
}
|
|
}
|
|
}
|
|
return result;
|
|
}
|
|
|
|
double MatrixNorm(const fesa::Matrix& matrix) {
|
|
double norm = 0.0;
|
|
for (std::size_t i = 0; i < matrix.Rows(); ++i) {
|
|
for (std::size_t j = 0; j < matrix.Columns(); ++j) {
|
|
norm = std::hypot(norm, matrix(i, j));
|
|
}
|
|
}
|
|
return norm;
|
|
}
|
|
|
|
struct Fixture {
|
|
std::array<fesa::Node, 4> nodes = Nodes();
|
|
std::array<fesa::ShellNodeInitialFrame, 4> frames = Frames();
|
|
double thickness = 0.2;
|
|
double density = 1.0;
|
|
double modulus = 1000.0;
|
|
|
|
double Length() const {
|
|
double length = 0.0;
|
|
for (std::size_t i = 0; i < 4; ++i) {
|
|
length = std::max(length, (V3{nodes[(i + 1U) % 4U].coordinates} -
|
|
V3{nodes[i].coordinates})
|
|
.Norm());
|
|
}
|
|
return length;
|
|
}
|
|
|
|
fesa::Result<fesa::NonlinearMitc4Inertia> Create() const {
|
|
return fesa::NonlinearMitc4Inertia::Create(
|
|
Pointers(nodes), frames,
|
|
{"Section", thickness, 0U, {"nonlinear-inertia.inp", 20U}},
|
|
{"Material", modulus, 0.3, {"nonlinear-inertia.inp", 21U}}, density);
|
|
}
|
|
};
|
|
|
|
Fixture Warped() {
|
|
Fixture fixture;
|
|
fixture.nodes[2].coordinates[2] = 0.1;
|
|
const fesa::Mitc4ShellDefinition element{{"Shell-Instance", 1, "1"},
|
|
fesa::ShellSourceElementType::kS4,
|
|
{0U, 1U, 2U, 3U},
|
|
0U,
|
|
0U,
|
|
{"nonlinear-inertia.inp", 10U}};
|
|
const auto geometry = fesa::PreprocessShellGeometry(
|
|
std::vector<fesa::Node>{fixture.nodes.begin(), fixture.nodes.end()},
|
|
{element}, {{"Section", 0.2, 0U, {"nonlinear-inertia.inp", 20U}}});
|
|
EXPECT_TRUE(geometry.HasValue());
|
|
for (std::size_t i = 0; i < 4; ++i) {
|
|
fixture.frames[i] = geometry.Value().nodal_frames[i];
|
|
}
|
|
return fixture;
|
|
}
|
|
|
|
struct State {
|
|
fesa::NonlinearMitc4TrialDofs q{};
|
|
fesa::Vector v{24U};
|
|
fesa::Vector a{24U};
|
|
};
|
|
|
|
State FiniteState() {
|
|
State state;
|
|
for (std::size_t node = 0; node < 4; ++node) {
|
|
const double i = static_cast<double>(node + 1U);
|
|
state.q.displacement[node] = {0.01 * i, -0.01 * i, 0.005 * i};
|
|
state.q.rotation_vector[node] = {0.05 * i, -0.03 * i, 0.02 * i};
|
|
const std::array<double, 6> v{0.1, 0.05 * i, -0.03 * i,
|
|
0.2, -0.1 * i, 0.04 * i};
|
|
const std::array<double, 6> a{0.3, -0.2, 0.1 * i, -0.07 * i, 0.06, 0.05};
|
|
for (std::size_t j = 0; j < 6; ++j) {
|
|
state.v[6U * node + j] = v[j];
|
|
state.a[6U * node + j] = a[j];
|
|
}
|
|
}
|
|
return state;
|
|
}
|
|
|
|
fesa::Vector ApplyD(fesa::Vector vector, const double length) {
|
|
for (std::size_t i = 0; i < vector.Size(); ++i) {
|
|
if (i % 6U < 3U) {
|
|
vector[i] *= length;
|
|
}
|
|
}
|
|
return vector;
|
|
}
|
|
|
|
fesa::Matrix ScaledMatrix(const fesa::Matrix& matrix, const double length) {
|
|
fesa::Matrix result{matrix.Rows(), matrix.Columns()};
|
|
for (std::size_t i = 0; i < matrix.Rows(); ++i) {
|
|
for (std::size_t j = 0; j < matrix.Columns(); ++j) {
|
|
result(i, j) = matrix(i, j) * (i % 6U < 3U ? length : 1.0) *
|
|
(j % 6U < 3U ? length : 1.0);
|
|
}
|
|
}
|
|
return result;
|
|
}
|
|
|
|
TEST(DynamicRotation, InverseIdentityAndCurrentAnalyticNullAtFiniteRotation) {
|
|
for (const V3 phi : {V3{0.5, 0.0, 0.0}, V3{0.2, -0.3, 0.1}}) {
|
|
const auto rotation = fesa::ExpSkew(phi);
|
|
const auto jacobian = fesa::LeftJacobianSO3(phi);
|
|
const auto inverse_transpose = fesa::InverseTransposeLeftJacobianSO3(phi);
|
|
for (std::size_t column = 0; column < 3; ++column) {
|
|
// R^T e_j is a unit initial director; its current director is e_j.
|
|
const V3 initial{rotation(column, 0), rotation(column, 1),
|
|
rotation(column, 2)};
|
|
const auto value = fesa::EvaluateDynamicRotation(phi, initial, {}, {});
|
|
ASSERT_TRUE(value.HasValue());
|
|
const V3 inverse_column{inverse_transpose(column, 0),
|
|
inverse_transpose(column, 1),
|
|
inverse_transpose(column, 2)};
|
|
EXPECT_LE(
|
|
(value.Value().drill_null - inverse_column / inverse_column.Norm())
|
|
.Norm(),
|
|
1.0e-12);
|
|
const V3 identity_column =
|
|
Multiply(jacobian, value.Value().drill_null) * inverse_column.Norm();
|
|
for (std::size_t row = 0; row < 3; ++row) {
|
|
EXPECT_NEAR(identity_column[row], row == column ? 1.0 : 0.0, 1.0e-12);
|
|
}
|
|
}
|
|
}
|
|
for (const Fixture fixture : {Fixture{}, Warped()}) {
|
|
const auto inertia = fixture.Create();
|
|
ASSERT_TRUE(inertia.HasValue());
|
|
for (const bool uniform : {false, true}) {
|
|
State state = FiniteState();
|
|
if (uniform) {
|
|
state.q.rotation_vector.fill(V3{0.2, -0.3, 0.1});
|
|
}
|
|
const auto value = inertia.Value().Evaluate(state.q, state.v, state.a);
|
|
ASSERT_TRUE(value.HasValue());
|
|
const auto mass = ScaledMatrix(value.Value().mass, fixture.Length());
|
|
for (std::size_t node = 0; node < 4; ++node) {
|
|
const auto rotation = fesa::EvaluateDynamicRotation(
|
|
state.q.rotation_vector[node], V3{fixture.frames[node].director},
|
|
{}, {});
|
|
ASSERT_TRUE(rotation.HasValue());
|
|
fesa::Vector n{24U};
|
|
for (std::size_t j = 0; j < 3; ++j) {
|
|
n[6U * node + 3U + j] = rotation.Value().drill_null[j];
|
|
}
|
|
EXPECT_LE(mass.Multiply(n).Norm() / MatrixNorm(mass), 1.0e-12);
|
|
const auto scaled_force = ApplyD(value.Value().force, fixture.Length());
|
|
EXPECT_LE(std::abs(n.Dot(scaled_force)) / scaled_force.Norm(), 1.0e-10);
|
|
fesa::Vector fixed_drill{24U};
|
|
fixed_drill[6U * node + 5U] = 1.0;
|
|
EXPECT_GT(mass.Multiply(fixed_drill).Norm() / MatrixNorm(mass), 1.0e-8);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
TEST(NonlinearMitc4Inertia,
|
|
RejectsIllConditionedPhysicalMassWithoutDeletingIt) {
|
|
for (const double density : {1.0e-180, 1.0, 1.0e180}) {
|
|
Fixture normal;
|
|
normal.density = density;
|
|
EXPECT_TRUE(normal.Create().HasValue());
|
|
Fixture thin = normal;
|
|
thin.thickness = 1.0e-5;
|
|
const auto rejected = thin.Create();
|
|
ASSERT_FALSE(rejected.HasValue());
|
|
EXPECT_EQ(rejected.GetStatus().Diagnostics()[0].code,
|
|
"ill-conditioned-nonlinear-inertia-positive-mass");
|
|
}
|
|
}
|
|
|
|
TEST(NonlinearMitc4Inertia,
|
|
ConvectiveForceDoesNotLoseSmallValuesToAcceleration) {
|
|
const auto inertia = Fixture{}.Create();
|
|
ASSERT_TRUE(inertia.HasValue());
|
|
State state = FiniteState();
|
|
const auto ordinary = inertia.Value().Evaluate(state.q, state.v, state.a);
|
|
state.a.Scale(1.0e15);
|
|
const auto large = inertia.Value().Evaluate(state.q, state.v, state.a);
|
|
ASSERT_TRUE(ordinary.HasValue());
|
|
ASSERT_TRUE(large.HasValue());
|
|
EXPECT_GT(ordinary.Value().convective_force.Norm(), 0.0);
|
|
for (std::size_t i = 0; i < 24; ++i) {
|
|
EXPECT_EQ(ordinary.Value().convective_force[i],
|
|
large.Value().convective_force[i]);
|
|
}
|
|
}
|
|
|
|
TEST(NonlinearMitc4Inertia,
|
|
PartialAndEffectiveTangentsPassTheScaledFixedSweep) {
|
|
constexpr std::array<int, 7> kExponents{8, 10, 12, 14, 16, 18, 20};
|
|
for (const Fixture fixture : {Fixture{}, Warped()}) {
|
|
const auto inertia = fixture.Create();
|
|
ASSERT_TRUE(inertia.HasValue());
|
|
const State state = FiniteState();
|
|
const auto value = inertia.Value().Evaluate(state.q, state.v, state.a);
|
|
ASSERT_TRUE(value.HasValue());
|
|
for (std::size_t channel = 0; channel < 3; ++channel) {
|
|
fesa::Vector p{24U};
|
|
for (std::size_t i = 0; i < 24; ++i) {
|
|
if (channel == 2U || (channel == 0U) == (i % 6U < 3U)) {
|
|
p[i] = static_cast<double>(i + 1U) * (i % 2U == 0U ? 1.0 : -1.0);
|
|
}
|
|
}
|
|
p.Scale(1.0 / p.Norm());
|
|
const auto direction = ApplyD(p, fixture.Length());
|
|
// q, v, a partials and the two HHT dependent-rate effective tangents.
|
|
for (std::size_t mode = 0; mode < 5; ++mode) {
|
|
const double h = mode == 4U ? 0.025 : 0.1;
|
|
const double cv =
|
|
mode >= 3U ? 0.55 / (0.275625 * h) : (mode == 1U ? 1.0 : 0.0);
|
|
const double ca = mode >= 3U ? 1.0 / (0.95 * 0.275625 * h * h)
|
|
: (mode == 2U ? 1.0 : 0.0);
|
|
const double cq = mode == 0U || mode >= 3U ? 1.0 : 0.0;
|
|
fesa::Matrix tangent{24U, 24U};
|
|
for (std::size_t i = 0; i < 24; ++i) {
|
|
for (std::size_t j = 0; j < 24; ++j) {
|
|
tangent(i, j) = cq * value.Value().position_tangent(i, j) +
|
|
cv * value.Value().velocity_tangent(i, j) +
|
|
ca * value.Value().mass(i, j);
|
|
}
|
|
}
|
|
const auto scaled = ScaledMatrix(tangent, fixture.Length());
|
|
const auto analytical = scaled.Multiply(p);
|
|
bool previous_pass = false;
|
|
bool consecutive = false;
|
|
for (const int exponent : kExponents) {
|
|
const double step = std::ldexp(1.0, -exponent);
|
|
State plus = state;
|
|
State minus = state;
|
|
plus.q = AddRotation(state.q, direction, cq * step);
|
|
minus.q = AddRotation(state.q, direction, -cq * step);
|
|
plus.v.Axpy(cv * step, direction);
|
|
minus.v.Axpy(-cv * step, direction);
|
|
plus.a.Axpy(ca * step, direction);
|
|
minus.a.Axpy(-ca * step, direction);
|
|
const auto vp = inertia.Value().Evaluate(plus.q, plus.v, plus.a);
|
|
const auto vm = inertia.Value().Evaluate(minus.q, minus.v, minus.a);
|
|
ASSERT_TRUE(vp.HasValue());
|
|
ASSERT_TRUE(vm.HasValue());
|
|
const auto fd =
|
|
ApplyD(Difference(vp.Value().force, vm.Value().force, 2.0 * step),
|
|
fixture.Length());
|
|
const double error = Difference(analytical, fd, 1.0).Norm();
|
|
const double scale = analytical.Norm() > 0.0
|
|
? analytical.Norm() + fd.Norm()
|
|
: MatrixNorm(scaled);
|
|
const double metric = scale > 0.0 ? error / scale : error;
|
|
const double tolerance = analytical.Norm() > 0.0 ? 1.0e-6 : 1.0e-10;
|
|
std::cout << "C06 tangent channel=" << channel << " mode=" << mode
|
|
<< " j=" << exponent << " raw=" << error
|
|
<< " metric=" << metric << '\n';
|
|
const bool pass =
|
|
metric <= tolerance && exponent >= 10 && exponent <= 18;
|
|
consecutive = consecutive || (previous_pass && pass);
|
|
previous_pass = pass;
|
|
}
|
|
EXPECT_TRUE(consecutive) << "channel=" << channel << " mode=" << mode;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
struct Integral {
|
|
std::array<std::array<std::array<double, 4>, 4>, 3> moments{};
|
|
fesa::Matrix mass{24U, 24U};
|
|
fesa::Vector force{24U};
|
|
double energy = 0.0;
|
|
double power = 0.0;
|
|
double total_mass = 0.0;
|
|
V3 momentum{};
|
|
V3 angular_momentum{};
|
|
V3 momentum_rate{};
|
|
V3 angular_momentum_rate{};
|
|
};
|
|
|
|
Integral IntegrateFivePoints(const Fixture& fixture, const State& state) {
|
|
const double r = std::sqrt(10.0 / 7.0);
|
|
const double outer = std::sqrt(5.0 + 2.0 * r) / 3.0;
|
|
const double inner = std::sqrt(5.0 - 2.0 * r) / 3.0;
|
|
const double wouter = (322.0 - 13.0 * std::sqrt(70.0)) / 900.0;
|
|
const double winner = (322.0 + 13.0 * std::sqrt(70.0)) / 900.0;
|
|
const std::array<double, 5> points{-outer, -inner, 0.0, inner, outer};
|
|
const std::array<double, 5> weights{wouter, winner, 128.0 / 225.0, winner,
|
|
wouter};
|
|
const std::array<double, 4> sx{-1, 1, 1, -1};
|
|
const std::array<double, 4> sy{-1, -1, 1, 1};
|
|
std::array<V3, 4> directors{};
|
|
std::array<std::array<V3, 3>, 4> first{};
|
|
std::array<V3, 4> convective{};
|
|
for (std::size_t node = 0; node < 4; ++node) {
|
|
const V3 phi = state.q.rotation_vector[node];
|
|
const V3 d0{fixture.frames[node].director};
|
|
directors[node] = BlockExponentialDerivative(phi, d0, {});
|
|
const V3 vr{state.v[6U * node + 3U], state.v[6U * node + 4U],
|
|
state.v[6U * node + 5U]};
|
|
convective[node] = BlockExponentialDerivative(phi, d0, {vr, vr});
|
|
for (std::size_t j = 0; j < 3; ++j) {
|
|
std::array<double, 3> basis{};
|
|
basis[j] = 1.0;
|
|
first[node][j] = BlockExponentialDerivative(phi, d0, {V3{basis}});
|
|
}
|
|
}
|
|
Integral integral;
|
|
for (std::size_t xi = 0; xi < 5; ++xi) {
|
|
for (std::size_t eta = 0; eta < 5; ++eta) {
|
|
for (std::size_t zeta = 0; zeta < 5; ++zeta) {
|
|
const double z = 0.5 * fixture.thickness * points[zeta];
|
|
std::array<double, 4> shape{};
|
|
std::array<V3, 24> h{};
|
|
V3 a1{};
|
|
V3 a2{};
|
|
V3 a3{};
|
|
V3 position{};
|
|
V3 velocity{};
|
|
V3 acceleration{};
|
|
for (std::size_t node = 0; node < 4; ++node) {
|
|
shape[node] = 0.25 * (1.0 + sx[node] * points[xi]) *
|
|
(1.0 + sy[node] * points[eta]);
|
|
const double dx = 0.25 * sx[node] * (1.0 + sy[node] * points[eta]);
|
|
const double dy = 0.25 * sy[node] * (1.0 + sx[node] * points[xi]);
|
|
const V3 d0{fixture.frames[node].director};
|
|
const V3 x0{fixture.nodes[node].coordinates};
|
|
a1 = a1 + dx * (x0 + z * d0);
|
|
a2 = a2 + dy * (x0 + z * d0);
|
|
a3 = a3 + 0.5 * fixture.thickness * shape[node] * d0;
|
|
position = position + shape[node] * (x0 + state.q.displacement[node] +
|
|
z * directors[node]);
|
|
acceleration = acceleration + z * shape[node] * convective[node];
|
|
for (std::size_t j = 0; j < 3; ++j) {
|
|
std::array<double, 3> axis{};
|
|
axis[j] = shape[node];
|
|
h[6U * node + j] = V3{axis};
|
|
h[6U * node + 3U + j] = z * shape[node] * first[node][j];
|
|
}
|
|
}
|
|
const double weight = fixture.density * weights[xi] * weights[eta] *
|
|
weights[zeta] * a1.Dot(a2.Cross(a3));
|
|
EXPECT_GT(weight, 0.0);
|
|
integral.total_mass += weight;
|
|
for (std::size_t i = 0; i < 24; ++i) {
|
|
velocity = velocity + h[i] * state.v[i];
|
|
acceleration = acceleration + h[i] * state.a[i];
|
|
}
|
|
integral.energy += 0.5 * weight * velocity.Dot(velocity);
|
|
integral.power += weight * velocity.Dot(acceleration);
|
|
integral.momentum = integral.momentum + weight * velocity;
|
|
integral.angular_momentum =
|
|
integral.angular_momentum + weight * position.Cross(velocity);
|
|
integral.momentum_rate = integral.momentum_rate + weight * acceleration;
|
|
integral.angular_momentum_rate = integral.angular_momentum_rate +
|
|
weight * position.Cross(acceleration);
|
|
for (std::size_t i = 0; i < 24; ++i) {
|
|
integral.force[i] += weight * h[i].Dot(acceleration);
|
|
for (std::size_t j = 0; j < 24; ++j) {
|
|
integral.mass(i, j) += weight * h[i].Dot(h[j]);
|
|
}
|
|
}
|
|
for (std::size_t i = 0; i < 4; ++i) {
|
|
for (std::size_t j = 0; j < 4; ++j) {
|
|
double moment = weight * shape[i] * shape[j];
|
|
for (std::size_t k = 0; k < 3; ++k) {
|
|
integral.moments[k][i][j] += moment;
|
|
moment *= z;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
return integral;
|
|
}
|
|
|
|
TEST(NonlinearMitc4Inertia,
|
|
MatchesIndependentWarpedVolumeMomentsEnergyAndPower) {
|
|
for (const Fixture fixture : {Fixture{}, Warped()}) {
|
|
const auto inertia = fixture.Create();
|
|
ASSERT_TRUE(inertia.HasValue());
|
|
const State state = FiniteState();
|
|
const auto value = inertia.Value().Evaluate(state.q, state.v, state.a);
|
|
ASSERT_TRUE(value.HasValue());
|
|
const Integral oracle = IntegrateFivePoints(fixture, state);
|
|
const auto actual_mass = ScaledMatrix(value.Value().mass, fixture.Length());
|
|
const auto expected_mass = ScaledMatrix(oracle.mass, fixture.Length());
|
|
double mass_error = 0.0;
|
|
double symmetry_error = 0.0;
|
|
for (std::size_t i = 0; i < 24; ++i) {
|
|
for (std::size_t j = 0; j < 24; ++j) {
|
|
mass_error =
|
|
std::hypot(mass_error, actual_mass(i, j) - expected_mass(i, j));
|
|
symmetry_error =
|
|
std::hypot(symmetry_error, actual_mass(i, j) - actual_mass(j, i));
|
|
}
|
|
}
|
|
EXPECT_LE(mass_error / MatrixNorm(expected_mass), 1.0e-12);
|
|
EXPECT_LE(symmetry_error / MatrixNorm(expected_mass), 1.0e-12);
|
|
EXPECT_LE(
|
|
std::abs(value.Value().kinetic_energy - oracle.energy) / oracle.energy,
|
|
1.0e-12);
|
|
EXPECT_LE(std::abs(0.5 * state.v.Dot(value.Value().mass.Multiply(state.v)) -
|
|
oracle.energy) /
|
|
oracle.energy,
|
|
1.0e-12);
|
|
EXPECT_LE(std::abs(state.v.Dot(value.Value().force) - oracle.power) /
|
|
std::abs(oracle.power),
|
|
1.0e-10);
|
|
EXPECT_LE(Difference(ApplyD(value.Value().force, fixture.Length()),
|
|
ApplyD(oracle.force, fixture.Length()), 1.0)
|
|
.Norm() /
|
|
ApplyD(oracle.force, fixture.Length()).Norm(),
|
|
1.0e-12);
|
|
EXPECT_LE((value.Value().momentum - oracle.momentum).Norm() /
|
|
oracle.momentum.Norm(),
|
|
1.0e-12);
|
|
EXPECT_LE(
|
|
(value.Value().angular_momentum - oracle.angular_momentum).Norm() /
|
|
oracle.angular_momentum.Norm(),
|
|
1.0e-12);
|
|
EXPECT_LE((value.Value().momentum_rate - oracle.momentum_rate).Norm() /
|
|
oracle.momentum_rate.Norm(),
|
|
1.0e-12);
|
|
EXPECT_LE(
|
|
(value.Value().angular_momentum_rate - oracle.angular_momentum_rate)
|
|
.Norm() /
|
|
oracle.angular_momentum_rate.Norm(),
|
|
1.0e-12);
|
|
// Recover C0/C1/C2 from their known tensor factors, with the prescribed
|
|
// m_e*(t/2)^k scales even when a centered C1 is exactly zero.
|
|
for (std::size_t i = 0; i < 4; ++i) {
|
|
const auto bi = fesa::EvaluateDynamicRotation(
|
|
state.q.rotation_vector[i], V3{fixture.frames[i].director}, {}, {});
|
|
ASSERT_TRUE(bi.HasValue());
|
|
for (std::size_t j = 0; j < 4; ++j) {
|
|
const auto bj = fesa::EvaluateDynamicRotation(
|
|
state.q.rotation_vector[j], V3{fixture.frames[j].director}, {}, {});
|
|
ASSERT_TRUE(bj.HasValue());
|
|
std::array<double, 3> numerator{};
|
|
std::array<double, 3> denominator{1.0, 0.0, 0.0};
|
|
numerator[0] = value.Value().mass(6U * i, 6U * j);
|
|
for (std::size_t a = 0; a < 3; ++a) {
|
|
for (std::size_t b = 0; b < 3; ++b) {
|
|
const double b1 = bj.Value().director_first[b][a];
|
|
const double b2 =
|
|
bi.Value().director_first[a].Dot(bj.Value().director_first[b]);
|
|
numerator[1] +=
|
|
value.Value().mass(6U * i + a, 6U * j + 3U + b) * b1;
|
|
denominator[1] += b1 * b1;
|
|
numerator[2] +=
|
|
value.Value().mass(6U * i + 3U + a, 6U * j + 3U + b) * b2;
|
|
denominator[2] += b2 * b2;
|
|
}
|
|
}
|
|
double scale = oracle.total_mass;
|
|
for (std::size_t k = 0; k < 3; ++k) {
|
|
EXPECT_LE(std::abs(numerator[k] / denominator[k] -
|
|
oracle.moments[k][i][j]) /
|
|
scale,
|
|
1.0e-12);
|
|
scale *= fixture.thickness / 2.0;
|
|
}
|
|
}
|
|
}
|
|
State uniform;
|
|
for (std::size_t i = 0; i < 4; ++i) {
|
|
uniform.v[6U * i] = 0.3;
|
|
uniform.v[6U * i + 1U] = -0.4;
|
|
uniform.v[6U * i + 2U] = 0.5;
|
|
}
|
|
const auto translation = inertia.Value().Evaluate({}, uniform.v, uniform.a);
|
|
ASSERT_TRUE(translation.HasValue());
|
|
EXPECT_NEAR(translation.Value().kinetic_energy / oracle.total_mass, 0.25,
|
|
1.0e-12);
|
|
}
|
|
}
|
|
|
|
TEST(DynamicRotation, ThirdDerivativesMatchAllPermutationsAndInteriorSweep) {
|
|
const std::array<V3, 5> states{
|
|
V3{}, V3{1.0e-6, -2.0e-6, 3.0e-6}, V3{0.2, -0.3, 0.4},
|
|
V3{1.0e-4 * (1.0 - std::ldexp(1.0, -10)), 0.0, 0.0},
|
|
V3{1.0e-4 * (1.0 + std::ldexp(1.0, -10)), 0.0, 0.0}};
|
|
const std::array<V3, 4> directions{V3{1, 0, 0}, V3{0, 1, 0}, V3{0, 0, 1},
|
|
V3{1, 2, -1} / std::sqrt(6.0)};
|
|
const V3 initial{0, 0, 1};
|
|
for (std::size_t state_index = 0; state_index < states.size();
|
|
++state_index) {
|
|
const auto value =
|
|
fesa::EvaluateDynamicRotation(states[state_index], initial, {}, {});
|
|
ASSERT_TRUE(value.HasValue());
|
|
for (std::size_t p = 0; p < 4; ++p) {
|
|
std::array<std::array<bool, 4>, 4> previous{};
|
|
std::array<std::array<bool, 4>, 4> consecutive{};
|
|
for (const int exponent : {8, 10, 12, 14, 16, 18, 20}) {
|
|
const double step = std::ldexp(1.0, -exponent);
|
|
const auto plus = fesa::EvaluateDynamicRotation(
|
|
states[state_index] + step * directions[p], initial, {}, {});
|
|
const auto minus = fesa::EvaluateDynamicRotation(
|
|
states[state_index] - step * directions[p], initial, {}, {});
|
|
ASSERT_TRUE(plus.HasValue());
|
|
ASSERT_TRUE(minus.HasValue());
|
|
for (std::size_t r = 0; r < 4; ++r) {
|
|
for (std::size_t t = 0; t < 4; ++t) {
|
|
const V3 analytic = Third(value.Value(), directions[p],
|
|
directions[r], directions[t]);
|
|
const V3 fd =
|
|
(Second(plus.Value(), directions[r], directions[t]) -
|
|
Second(minus.Value(), directions[r], directions[t])) /
|
|
(2.0 * step);
|
|
const V3 oracle = BlockExponentialDerivative(
|
|
states[state_index], initial,
|
|
{directions[p], directions[r], directions[t]});
|
|
EXPECT_LE((analytic - oracle).Norm(), 1.0e-12);
|
|
EXPECT_LE((analytic - Third(value.Value(), directions[t],
|
|
directions[p], directions[r]))
|
|
.Norm(),
|
|
1.0e-12);
|
|
const double error = (analytic - fd).Norm();
|
|
const double scale =
|
|
analytic.Norm() > 0.0 ? analytic.Norm() + fd.Norm() : 1.0;
|
|
const double metric = error / scale;
|
|
const double tolerance = analytic.Norm() > 0.0 ? 1.0e-6 : 1.0e-10;
|
|
std::cout << "C06 D3 state=" << state_index << " triple=" << p << r
|
|
<< t << " j=" << exponent << " raw=" << error
|
|
<< " metric=" << metric << '\n';
|
|
const bool pass =
|
|
metric <= tolerance && exponent >= 10 && exponent <= 18;
|
|
consecutive[r][t] = consecutive[r][t] || (previous[r][t] && pass);
|
|
previous[r][t] = pass;
|
|
}
|
|
}
|
|
}
|
|
for (std::size_t r = 0; r < 4; ++r) {
|
|
for (std::size_t t = 0; t < 4; ++t) {
|
|
EXPECT_TRUE(consecutive[r][t])
|
|
<< "state=" << state_index << " triple=" << p << r << t;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
TEST(DynamicRotation, NormalizedNullDerivativesIncludeNormalizationCurvature) {
|
|
const V3 initial{0, 0, 1};
|
|
const V3 direction = V3{1, -2, 3} / std::sqrt(14.0);
|
|
for (const V3 phi : {V3{}, V3{1.0e-6, -2.0e-6, 3.0e-6}, V3{0.2, -0.3, 0.4}}) {
|
|
const auto value =
|
|
fesa::EvaluateDynamicRotation(phi, initial, direction, {});
|
|
ASSERT_TRUE(value.HasValue());
|
|
const V3 first = Multiply(value.Value().null_gradient, direction);
|
|
EXPECT_NEAR(value.Value().drill_null.Norm(), 1.0, 1.0e-12);
|
|
EXPECT_NEAR(value.Value().drill_null.Dot(first), 0.0, 1.0e-12);
|
|
EXPECT_NEAR(
|
|
value.Value().drill_null.Dot(value.Value().null_directional_second),
|
|
-first.Dot(first), 1.0e-12);
|
|
bool previous = false;
|
|
bool consecutive = false;
|
|
for (const int exponent : {8, 10, 12, 14, 16, 18, 20}) {
|
|
const double step = std::ldexp(1.0, -exponent);
|
|
const auto plus = fesa::EvaluateDynamicRotation(phi + step * direction,
|
|
initial, direction, {});
|
|
const auto minus = fesa::EvaluateDynamicRotation(phi - step * direction,
|
|
initial, direction, {});
|
|
ASSERT_TRUE(plus.HasValue());
|
|
ASSERT_TRUE(minus.HasValue());
|
|
const V3 fd1 =
|
|
(plus.Value().drill_null - minus.Value().drill_null) / (2.0 * step);
|
|
const V3 fd2 = (Multiply(plus.Value().null_gradient, direction) -
|
|
Multiply(minus.Value().null_gradient, direction)) /
|
|
(2.0 * step);
|
|
const double error1 = (fd1 - first).Norm() / (fd1.Norm() + first.Norm());
|
|
const double error2 =
|
|
(fd2 - value.Value().null_directional_second).Norm() /
|
|
(fd2.Norm() + value.Value().null_directional_second.Norm());
|
|
std::cout << "C06 null j=" << exponent << " D1=" << error1
|
|
<< " D2=" << error2 << '\n';
|
|
const bool pass = error1 <= 1.0e-6 && error2 <= 1.0e-6 &&
|
|
exponent >= 10 && exponent <= 18;
|
|
consecutive = consecutive || (previous && pass);
|
|
previous = pass;
|
|
}
|
|
EXPECT_TRUE(consecutive);
|
|
}
|
|
EXPECT_FALSE(fesa::EvaluateDynamicRotation({}, {0, 0, 2}, {}, {}).HasValue());
|
|
EXPECT_FALSE(
|
|
fesa::EvaluateDynamicRotation(
|
|
{std::numeric_limits<double>::quiet_NaN(), 0, 0}, initial, {}, {})
|
|
.HasValue());
|
|
}
|
|
|
|
TEST(NonlinearMitc4Inertia, UnitAndGlobalFrameChangesPreservePhysicalMetrics) {
|
|
const Fixture fixture = Warped();
|
|
const State state = FiniteState();
|
|
const auto inertia = fixture.Create();
|
|
ASSERT_TRUE(inertia.HasValue());
|
|
const auto value = inertia.Value().Evaluate(state.q, state.v, state.a);
|
|
ASSERT_TRUE(value.HasValue());
|
|
constexpr double kLength = 1.0e-3;
|
|
constexpr double kMass = 1.0e-6;
|
|
constexpr double kTime = 1.0e2;
|
|
Fixture units = fixture;
|
|
State converted = state;
|
|
units.thickness *= kLength;
|
|
units.density *= kMass / (kLength * kLength * kLength);
|
|
units.modulus *= kMass / (kLength * kTime * kTime);
|
|
for (std::size_t node = 0; node < 4; ++node) {
|
|
for (double& coordinate : units.nodes[node].coordinates) {
|
|
coordinate *= kLength;
|
|
}
|
|
converted.q.displacement[node] = kLength * state.q.displacement[node];
|
|
for (std::size_t j = 0; j < 6; ++j) {
|
|
const double length_factor = j < 3U ? kLength : 1.0;
|
|
converted.v[6U * node + j] *= length_factor / kTime;
|
|
converted.a[6U * node + j] *= length_factor / (kTime * kTime);
|
|
}
|
|
}
|
|
const auto converted_inertia = units.Create();
|
|
ASSERT_TRUE(converted_inertia.HasValue());
|
|
const auto uv =
|
|
converted_inertia.Value().Evaluate(converted.q, converted.v, converted.a);
|
|
ASSERT_TRUE(uv.HasValue());
|
|
const double energy_scale = kMass * kLength * kLength / (kTime * kTime);
|
|
EXPECT_LE(std::abs(uv.Value().kinetic_energy / energy_scale -
|
|
value.Value().kinetic_energy) /
|
|
value.Value().kinetic_energy,
|
|
1.0e-12);
|
|
const auto original_scaled =
|
|
ScaledMatrix(value.Value().mass, fixture.Length());
|
|
const auto converted_scaled = ScaledMatrix(uv.Value().mass, units.Length());
|
|
double error = 0.0;
|
|
for (std::size_t i = 0; i < 24; ++i) {
|
|
for (std::size_t j = 0; j < 24; ++j) {
|
|
error = std::hypot(error,
|
|
converted_scaled(i, j) / (kMass * kLength * kLength) -
|
|
original_scaled(i, j));
|
|
}
|
|
}
|
|
EXPECT_LE(error / MatrixNorm(original_scaled), 1.0e-12);
|
|
const auto original_force = ApplyD(value.Value().force, fixture.Length());
|
|
auto converted_force = ApplyD(uv.Value().force, units.Length());
|
|
converted_force.Scale(1.0 / energy_scale);
|
|
EXPECT_LE(Difference(original_force, converted_force, 1.0).Norm() /
|
|
original_force.Norm(),
|
|
1.0e-10);
|
|
|
|
const auto qx = fesa::ExpSkew(V3{0.4, 0, 0});
|
|
const auto qz = fesa::ExpSkew(V3{0, 0, 0.7});
|
|
const auto rotate = [&](const V3& v) {
|
|
return Multiply(qz, Multiply(qx, v));
|
|
};
|
|
Fixture frame = fixture;
|
|
State rotated = state;
|
|
for (std::size_t node = 0; node < 4; ++node) {
|
|
frame.nodes[node].coordinates =
|
|
rotate(V3{fixture.nodes[node].coordinates}).Components();
|
|
frame.frames[node].director =
|
|
rotate(V3{fixture.frames[node].director}).Components();
|
|
frame.frames[node].tangent_a =
|
|
rotate(V3{fixture.frames[node].tangent_a}).Components();
|
|
frame.frames[node].tangent_b =
|
|
rotate(V3{fixture.frames[node].tangent_b}).Components();
|
|
rotated.q.displacement[node] = rotate(state.q.displacement[node]);
|
|
rotated.q.rotation_vector[node] = rotate(state.q.rotation_vector[node]);
|
|
for (std::size_t offset = 0; offset < 6; offset += 3) {
|
|
const std::size_t base = 6U * node + offset;
|
|
const V3 v =
|
|
rotate(V3{state.v[base], state.v[base + 1U], state.v[base + 2U]});
|
|
const V3 a =
|
|
rotate(V3{state.a[base], state.a[base + 1U], state.a[base + 2U]});
|
|
for (std::size_t j = 0; j < 3; ++j) {
|
|
rotated.v[base + j] = v[j];
|
|
rotated.a[base + j] = a[j];
|
|
}
|
|
}
|
|
}
|
|
const auto frame_inertia = frame.Create();
|
|
ASSERT_TRUE(frame_inertia.HasValue());
|
|
const auto fv =
|
|
frame_inertia.Value().Evaluate(rotated.q, rotated.v, rotated.a);
|
|
ASSERT_TRUE(fv.HasValue());
|
|
EXPECT_LE(std::abs(fv.Value().kinetic_energy - value.Value().kinetic_energy) /
|
|
value.Value().kinetic_energy,
|
|
1.0e-12);
|
|
EXPECT_LE((fv.Value().momentum - rotate(value.Value().momentum)).Norm() /
|
|
value.Value().momentum.Norm(),
|
|
1.0e-12);
|
|
EXPECT_LE(
|
|
(fv.Value().angular_momentum - rotate(value.Value().angular_momentum))
|
|
.Norm() /
|
|
value.Value().angular_momentum.Norm(),
|
|
1.0e-12);
|
|
for (std::size_t base = 0; base < 24; base += 3) {
|
|
const V3 expected =
|
|
rotate(V3{value.Value().force[base], value.Value().force[base + 1U],
|
|
value.Value().force[base + 2U]});
|
|
const V3 actual{fv.Value().force[base], fv.Value().force[base + 1U],
|
|
fv.Value().force[base + 2U]};
|
|
EXPECT_LE((actual - expected).Norm() / expected.Norm(), 1.0e-10);
|
|
}
|
|
// Full matrix congruence checks every translation/rotation cross-block.
|
|
double covariance_error = 0.0;
|
|
for (std::size_t i = 0; i < 24; i += 3) {
|
|
for (std::size_t j = 0; j < 24; j += 3) {
|
|
for (std::size_t column = 0; column < 3; ++column) {
|
|
std::array<double, 3> axis{};
|
|
axis[column] = 1.0;
|
|
const V3 global_axis = rotate(V3{axis});
|
|
V3 result{};
|
|
std::array<double, 3> components{};
|
|
for (std::size_t row = 0; row < 3; ++row) {
|
|
for (std::size_t k = 0; k < 3; ++k) {
|
|
components[row] += fv.Value().mass(i + row, j + k) * global_axis[k];
|
|
}
|
|
}
|
|
result = V3{components};
|
|
const V3 expected = rotate(V3{value.Value().mass(i, j + column),
|
|
value.Value().mass(i + 1U, j + column),
|
|
value.Value().mass(i + 2U, j + column)});
|
|
covariance_error =
|
|
std::hypot(covariance_error, (result - expected).Norm());
|
|
}
|
|
}
|
|
}
|
|
EXPECT_LE(covariance_error / MatrixNorm(value.Value().mass), 1.0e-12);
|
|
}
|
|
|
|
TEST(DynamicRotation, MatchesEightyDigitIndependentBlockExponentialAnchors) {
|
|
// Constants were evaluated with Python decimal precision 80 by applying
|
|
// exp(A) to (e3,0,...,0), with A the 24x24 subset-block matrix described
|
|
// above, summing 99 matrix-power terms. This oracle uses no Rodrigues/Jl
|
|
// coefficients and retains high precision even on MSVC (long double=double).
|
|
const std::array<std::array<V3, 3>, 5> expected{
|
|
std::array<V3, 3>{V3{0, 1, 0}, V3{0, 0, 0},
|
|
V3{-0.81649658092772603, 0.40824829046386302, 0}},
|
|
std::array<V3, 3>{
|
|
V3{-7.5000039999910002e-07, 0.9999999999982, 9.9999999999883338e-07},
|
|
V3{1.6666686666628888e-07, -8.3332933333111116e-08,
|
|
2.6666666666643096e-19},
|
|
V3{-0.8164969891729138, 0.40824706571842007,
|
|
-1.6329931618530299e-06}},
|
|
std::array<V3, 3>{
|
|
V3{-0.10876310154778675, 0.95485599102728924, 0.19478607292927094},
|
|
V3{0.029022554703521119, -0.0080127091261333588,
|
|
0.0010465349668405597},
|
|
V3{-0.80418976262207553, 0.24008088229171029, -0.21102407953866634}},
|
|
std::array<V3, 3>{V3{0, 0.99999999500976089, 9.9902343583821137e-05},
|
|
V3{0, -8.325195306960704e-06, 1.111111111020202e-108},
|
|
V3{-0.81648298485418402, 0.40825508659919563,
|
|
3.7386214252198052e-05}},
|
|
std::array<V3, 3>{V3{0, 0.99999999499022962, 0.00010009765608284457},
|
|
V3{0, -8.3414713485948191e-06, 8.5714285692063495e-99},
|
|
V3{-0.81648295827126682, 0.40825509988321224,
|
|
3.7459305579990838e-05}}};
|
|
const std::array<V3, 5> states{
|
|
V3{}, V3{1.0e-6, -2.0e-6, 3.0e-6}, V3{0.2, -0.3, 0.4},
|
|
V3{1.0e-4 * (1.0 - std::ldexp(1.0, -10)), 0.0, 0.0},
|
|
V3{1.0e-4 * (1.0 + std::ldexp(1.0, -10)), 0.0, 0.0}};
|
|
const V3 e1{1, 0, 0};
|
|
const V3 e2{0, 1, 0};
|
|
const V3 e3{0, 0, 1};
|
|
const V3 mixed = V3{1, 2, -1} / std::sqrt(6.0);
|
|
for (std::size_t i = 0; i < states.size(); ++i) {
|
|
const auto value = fesa::EvaluateDynamicRotation(states[i], e3, {}, {});
|
|
ASSERT_TRUE(value.HasValue());
|
|
EXPECT_LE((Third(value.Value(), e1, e1, e1) - expected[i][0]).Norm(),
|
|
1.0e-12);
|
|
EXPECT_LE((Third(value.Value(), e1, e2, e3) - expected[i][1]).Norm(),
|
|
1.0e-12);
|
|
EXPECT_LE(
|
|
(Third(value.Value(), mixed, mixed, mixed) - expected[i][2]).Norm(),
|
|
1.0e-12);
|
|
}
|
|
}
|
|
|
|
TEST(NonlinearMitc4Inertia, IndependentOracleDetectsEachRequiredOmission) {
|
|
const Fixture fixture = Warped();
|
|
const auto inertia = fixture.Create();
|
|
ASSERT_TRUE(inertia.HasValue());
|
|
const State state = FiniteState();
|
|
const auto value = inertia.Value().Evaluate(state.q, state.v, state.a);
|
|
ASSERT_TRUE(value.HasValue());
|
|
const Integral oracle = IntegrateFivePoints(fixture, state);
|
|
fesa::Vector p{24U};
|
|
for (std::size_t i = 0; i < 24; ++i) {
|
|
if (i % 6U >= 3U) {
|
|
p[i] = static_cast<double>(i + 1U) * (i % 2U == 0U ? 1.0 : -1.0);
|
|
}
|
|
}
|
|
p.Scale(1.0 / p.Norm());
|
|
std::array<V3, 4> first_acceleration{};
|
|
std::array<V3, 4> third{};
|
|
std::array<std::array<V3, 3>, 4> first{};
|
|
std::array<std::array<V3, 3>, 4> second{};
|
|
for (std::size_t node = 0; node < 4; ++node) {
|
|
const std::size_t b = 6U * node + 3U;
|
|
const V3 phi = state.q.rotation_vector[node];
|
|
const V3 d0{fixture.frames[node].director};
|
|
const V3 v{state.v[b], state.v[b + 1U], state.v[b + 2U]};
|
|
const V3 a{state.a[b], state.a[b + 1U], state.a[b + 2U]};
|
|
const V3 direction{p[b], p[b + 1U], p[b + 2U]};
|
|
first_acceleration[node] = BlockExponentialDerivative(phi, d0, {a}) +
|
|
BlockExponentialDerivative(phi, d0, {v, v});
|
|
third[node] = BlockExponentialDerivative(phi, d0, {direction, v, v});
|
|
for (std::size_t j = 0; j < 3; ++j) {
|
|
std::array<double, 3> axis{};
|
|
axis[j] = 1.0;
|
|
first[node][j] = BlockExponentialDerivative(phi, d0, {V3{axis}});
|
|
second[node][j] =
|
|
BlockExponentialDerivative(phi, d0, {direction, V3{axis}});
|
|
}
|
|
}
|
|
fesa::Vector transposed_h_derivative{24U};
|
|
fesa::Vector third_derivative{24U};
|
|
for (std::size_t i = 0; i < 4; ++i) {
|
|
for (std::size_t j = 0; j < 4; ++j) {
|
|
const V3 au{state.a[6U * j], state.a[6U * j + 1U], state.a[6U * j + 2U]};
|
|
for (std::size_t c = 0; c < 3; ++c) {
|
|
transposed_h_derivative[6U * i + 3U + c] +=
|
|
oracle.moments[1][i][j] * second[i][c].Dot(au) +
|
|
oracle.moments[2][i][j] * second[i][c].Dot(first_acceleration[j]);
|
|
third_derivative[6U * i + c] += oracle.moments[1][i][j] * third[j][c];
|
|
third_derivative[6U * i + 3U + c] +=
|
|
oracle.moments[2][i][j] * first[i][c].Dot(third[j]);
|
|
}
|
|
}
|
|
}
|
|
const double step = std::ldexp(1.0, -12);
|
|
const auto plus =
|
|
inertia.Value().Evaluate(AddRotation(state.q, p, step), state.v, state.a);
|
|
const auto minus = inertia.Value().Evaluate(AddRotation(state.q, p, -step),
|
|
state.v, state.a);
|
|
ASSERT_TRUE(plus.HasValue());
|
|
ASSERT_TRUE(minus.HasValue());
|
|
const auto fd =
|
|
ApplyD(Difference(plus.Value().force, minus.Value().force, 2.0 * step),
|
|
fixture.Length());
|
|
const auto correct =
|
|
ApplyD(value.Value().position_tangent.Multiply(p), fixture.Length());
|
|
for (const auto& omitted : {transposed_h_derivative, third_derivative}) {
|
|
auto mutant = correct;
|
|
mutant.Axpy(-1.0, ApplyD(omitted, fixture.Length()));
|
|
EXPECT_GT(Difference(mutant, fd, 1.0).Norm() / (mutant.Norm() + fd.Norm()),
|
|
1.0e-6);
|
|
}
|
|
auto no_convective = value.Value().force;
|
|
no_convective.Axpy(-1.0, value.Value().convective_force);
|
|
EXPECT_GT(Difference(ApplyD(no_convective, fixture.Length()),
|
|
ApplyD(oracle.force, fixture.Length()), 1.0)
|
|
.Norm() /
|
|
ApplyD(oracle.force, fixture.Length()).Norm(),
|
|
1.0e-6);
|
|
fesa::Vector vp = state.v;
|
|
fesa::Vector vm = state.v;
|
|
vp.Axpy(step, p);
|
|
vm.Axpy(-step, p);
|
|
const auto vplus = inertia.Value().Evaluate(state.q, vp, state.a);
|
|
const auto vminus = inertia.Value().Evaluate(state.q, vm, state.a);
|
|
ASSERT_TRUE(vplus.HasValue());
|
|
ASSERT_TRUE(vminus.HasValue());
|
|
const auto vfd =
|
|
ApplyD(Difference(vplus.Value().force, vminus.Value().force, 2.0 * step),
|
|
fixture.Length());
|
|
EXPECT_GT(vfd.Norm(), 0.0);
|
|
const fesa::Vector missing_dv{24U};
|
|
EXPECT_GT(Difference(missing_dv, vfd, 1.0).Norm() / vfd.Norm(), 1.0e-6);
|
|
}
|
|
|
|
} // namespace
|