feat(euler-beam-3d): add global transform recovery

2026-06-12 18:10:35 +09:00
parent 95ca95180a
commit 7845ebec68
4 changed files with 323 additions and 9 deletions
@@ -83,7 +83,8 @@
    {
      "step": 8,
      "name": "global-transform-recovery",
-      "status": "pending",
+      "status": "completed",
+      "summary": "global transform and global end-force recovery added for 3D Euler beam",
      "allowed_paths": [
        "src/fesa/elements/",
        "tests/unit/euler_beam_3d_*_test.cpp"
@@ -1,12 +1,14 @@
 #include <fesa/elements/euler_beam_3d.hpp>

 #include <cmath>
+#include <cstddef>
 #include <stdexcept>

 namespace fesa::elements {
 namespace {

 constexpr int matrix_size = 12;
+constexpr double geometry_tolerance = 1.0e-12;

 std::size_t index(int row, int column)
 {
@@ -37,6 +39,139 @@ void set_symmetric(Matrix12& matrix, int row, int column, double value)
    matrix[index(column, row)] = value;
 }

+Vector3 subtract(const Vector3& lhs, const Vector3& rhs)
+{
+    return {
+        lhs[0] - rhs[0],
+        lhs[1] - rhs[1],
+        lhs[2] - rhs[2]
+    };
+}
+
+double dot(const Vector3& lhs, const Vector3& rhs)
+{
+    return lhs[0] * rhs[0] + lhs[1] * rhs[1] + lhs[2] * rhs[2];
+}
+
+double norm(const Vector3& vector)
+{
+    return std::sqrt(dot(vector, vector));
+}
+
+Vector3 scale(const Vector3& vector, double scalar)
+{
+    return {
+        vector[0] * scalar,
+        vector[1] * scalar,
+        vector[2] * scalar
+    };
+}
+
+Vector3 normalize(const Vector3& vector, const char* message)
+{
+    const double vector_norm = norm(vector);
+    if (!std::isfinite(vector_norm) || vector_norm <= geometry_tolerance) {
+        throw std::invalid_argument(message);
+    }
+    return scale(vector, 1.0 / vector_norm);
+}
+
+Vector3 cross(const Vector3& lhs, const Vector3& rhs)
+{
+    return {
+        lhs[1] * rhs[2] - lhs[2] * rhs[1],
+        lhs[2] * rhs[0] - lhs[0] * rhs[2],
+        lhs[0] * rhs[1] - lhs[1] * rhs[0]
+    };
+}
+
+double geometry_length(const EulerBeam3DGeometry& geometry)
+{
+    return norm(subtract(geometry.node2, geometry.node1));
+}
+
+std::array<Vector3, 3> local_basis(const EulerBeam3DGeometry& geometry)
+{
+    const auto axis = subtract(geometry.node2, geometry.node1);
+    const auto local_x = normalize(axis, "Euler beam geometry length must be positive and finite");
+
+    const double orientation_along_x = dot(geometry.orientation, local_x);
+    const auto projected_orientation = subtract(
+        geometry.orientation,
+        scale(local_x, orientation_along_x));
+    const auto local_y = normalize(
+        projected_orientation,
+        "Euler beam orientation must be nonzero and not parallel to the element axis");
+    const auto local_z = cross(local_x, local_y);
+
+    return {local_x, local_y, local_z};
+}
+
+Matrix12 transform_matrix(const EulerBeam3DGeometry& geometry)
+{
+    const auto basis = local_basis(geometry);
+    Matrix12 transform{};
+    for (int block_offset : {0, 3, 6, 9}) {
+        for (int row = 0; row < 3; ++row) {
+            for (int column = 0; column < 3; ++column) {
+                transform[index(block_offset + row, block_offset + column)] =
+                    basis[static_cast<std::size_t>(row)][static_cast<std::size_t>(column)];
+            }
+        }
+    }
+    return transform;
+}
+
+Vector12 multiply(const Matrix12& matrix, const Vector12& vector)
+{
+    Vector12 result{};
+    for (int row = 0; row < matrix_size; ++row) {
+        for (int column = 0; column < matrix_size; ++column) {
+            result[static_cast<std::size_t>(row)] +=
+                matrix[index(row, column)] * vector[static_cast<std::size_t>(column)];
+        }
+    }
+    return result;
+}
+
+Vector12 multiply_transpose(const Matrix12& matrix, const Vector12& vector)
+{
+    Vector12 result{};
+    for (int row = 0; row < matrix_size; ++row) {
+        for (int column = 0; column < matrix_size; ++column) {
+            result[static_cast<std::size_t>(row)] +=
+                matrix[index(column, row)] * vector[static_cast<std::size_t>(column)];
+        }
+    }
+    return result;
+}
+
+Matrix12 multiply(const Matrix12& lhs, const Matrix12& rhs)
+{
+    Matrix12 result{};
+    for (int row = 0; row < matrix_size; ++row) {
+        for (int column = 0; column < matrix_size; ++column) {
+            for (int inner = 0; inner < matrix_size; ++inner) {
+                result[index(row, column)] += lhs[index(row, inner)] * rhs[index(inner, column)];
+            }
+        }
+    }
+    return result;
+}
+
+Matrix12 multiply_transpose_left(const Matrix12& lhs, const Matrix12& rhs)
+{
+    Matrix12 result{};
+    for (int row = 0; row < matrix_size; ++row) {
+        for (int column = 0; column < matrix_size; ++column) {
+            for (int inner = 0; inner < matrix_size; ++inner) {
+                result[index(row, column)] += lhs[index(inner, row)] * rhs[index(inner, column)];
+            }
+        }
+    }
+    return result;
+}
+
 } // namespace

 Matrix12 euler_beam_3d_local_stiffness(double length, const EulerBeam3DSection& section)
@@ -100,14 +235,27 @@ Vector12 euler_beam_3d_local_end_forces(double length,
                                        const Vector12& local_displacements)
 {
    const auto stiffness = euler_beam_3d_local_stiffness(length, section);
-    Vector12 forces{};
-    for (int row = 0; row < matrix_size; ++row) {
-        for (int column = 0; column < matrix_size; ++column) {
-            forces[static_cast<std::size_t>(row)] +=
-                stiffness[index(row, column)] * local_displacements[static_cast<std::size_t>(column)];
-        }
-    }
-    return forces;
+    return multiply(stiffness, local_displacements);
+}
+
+Matrix12 euler_beam_3d_global_stiffness(const EulerBeam3DGeometry& geometry,
+                                        const EulerBeam3DSection& section)
+{
+    const double length = geometry_length(geometry);
+    const auto local_stiffness = euler_beam_3d_local_stiffness(length, section);
+    const auto transform = transform_matrix(geometry);
+    return multiply_transpose_left(transform, multiply(local_stiffness, transform));
+}
+
+Vector12 euler_beam_3d_global_end_forces(const EulerBeam3DGeometry& geometry,
+                                         const EulerBeam3DSection& section,
+                                         const Vector12& global_displacements)
+{
+    const double length = geometry_length(geometry);
+    const auto transform = transform_matrix(geometry);
+    const auto local_displacements = multiply(transform, global_displacements);
+    const auto local_forces = euler_beam_3d_local_end_forces(length, section, local_displacements);
+    return multiply_transpose(transform, local_forces);
 }

 } // namespace fesa::elements
@@ -6,6 +6,7 @@ namespace fesa::elements {

 using Vector12 = std::array<double, 12>;
 using Matrix12 = std::array<double, 144>;
+using Vector3 = std::array<double, 3>;

 struct EulerBeam3DSection {
    double young_modulus;
@@ -16,10 +17,23 @@ struct EulerBeam3DSection {
    double second_moment_z;
 };

+struct EulerBeam3DGeometry {
+    Vector3 node1;
+    Vector3 node2;
+    Vector3 orientation;
+};
+
 Matrix12 euler_beam_3d_local_stiffness(double length, const EulerBeam3DSection& section);

 Vector12 euler_beam_3d_local_end_forces(double length,
                                        const EulerBeam3DSection& section,
                                        const Vector12& local_displacements);

+Matrix12 euler_beam_3d_global_stiffness(const EulerBeam3DGeometry& geometry,
+                                        const EulerBeam3DSection& section);
+
+Vector12 euler_beam_3d_global_end_forces(const EulerBeam3DGeometry& geometry,
+                                         const EulerBeam3DSection& section,
+                                         const Vector12& global_displacements);
+
 } // namespace fesa::elements
@@ -0,0 +1,151 @@
+#include <fesa/elements/euler_beam_3d.hpp>
+
+#include <cmath>
+#include <cstddef>
+#include <stdexcept>
+
+namespace {
+
+constexpr double tolerance = 1.0e-9;
+
+bool close(double actual, double expected, double tol = tolerance)
+{
+    return std::abs(actual - expected) <= tol;
+}
+
+double entry(const fesa::elements::Matrix12& matrix, int row, int column)
+{
+    return matrix[static_cast<std::size_t>(row * 12 + column)];
+}
+
+fesa::elements::EulerBeam3DSection section()
+{
+    return fesa::elements::EulerBeam3DSection{
+        210.0,
+        80.0,
+        3.0,
+        4.0,
+        5.0,
+        6.0
+    };
+}
+
+bool matrices_close(const fesa::elements::Matrix12& lhs, const fesa::elements::Matrix12& rhs)
+{
+    for (std::size_t index = 0; index < lhs.size(); ++index) {
+        if (!close(lhs[index], rhs[index])) {
+            return false;
+        }
+    }
+    return true;
+}
+
+bool symmetric(const fesa::elements::Matrix12& matrix)
+{
+    for (int row = 0; row < 12; ++row) {
+        for (int column = 0; column < 12; ++column) {
+            if (!close(entry(matrix, row, column), entry(matrix, column, row))) {
+                return false;
+            }
+        }
+    }
+    return true;
+}
+
+bool throws_parallel_orientation()
+{
+    const fesa::elements::EulerBeam3DGeometry geometry{
+        {0.0, 0.0, 0.0},
+        {2.0, 0.0, 0.0},
+        {1.0, 0.0, 0.0}
+    };
+
+    try {
+        (void)fesa::elements::euler_beam_3d_global_stiffness(geometry, section());
+    } catch (const std::invalid_argument&) {
+        return true;
+    }
+    return false;
+}
+
+bool throws_zero_orientation()
+{
+    const fesa::elements::EulerBeam3DGeometry geometry{
+        {0.0, 0.0, 0.0},
+        {2.0, 0.0, 0.0},
+        {0.0, 0.0, 0.0}
+    };
+
+    try {
+        (void)fesa::elements::euler_beam_3d_global_stiffness(geometry, section());
+    } catch (const std::invalid_argument&) {
+        return true;
+    }
+    return false;
+}
+
+} // namespace
+
+int main()
+{
+    const auto beam_section = section();
+    const fesa::elements::EulerBeam3DGeometry axis_aligned{
+        {0.0, 0.0, 0.0},
+        {2.0, 0.0, 0.0},
+        {0.0, 1.0, 0.0}
+    };
+
+    const auto local = fesa::elements::euler_beam_3d_local_stiffness(2.0, beam_section);
+    const auto global_identity =
+        fesa::elements::euler_beam_3d_global_stiffness(axis_aligned, beam_section);
+    if (!matrices_close(global_identity, local)) {
+        return 1;
+    }
+
+    const fesa::elements::EulerBeam3DGeometry rotated{
+        {0.0, 0.0, 0.0},
+        {0.0, 2.0, 0.0},
+        {1.0, 0.0, 0.0}
+    };
+    const auto rotated_stiffness =
+        fesa::elements::euler_beam_3d_global_stiffness(rotated, beam_section);
+    if (!symmetric(rotated_stiffness)) {
+        return 2;
+    }
+
+    fesa::elements::Vector12 rigid_translation{};
+    rigid_translation[0] = 0.2;
+    rigid_translation[1] = -0.1;
+    rigid_translation[2] = 0.3;
+    rigid_translation[6] = 0.2;
+    rigid_translation[7] = -0.1;
+    rigid_translation[8] = 0.3;
+    const auto rigid_forces =
+        fesa::elements::euler_beam_3d_global_end_forces(rotated, beam_section, rigid_translation);
+    for (double force : rigid_forces) {
+        if (!close(force, 0.0)) {
+            return 3;
+        }
+    }
+
+    fesa::elements::Vector12 axial_extension{};
+    axial_extension[6] = 0.1;
+    const auto axial_forces = fesa::elements::euler_beam_3d_global_end_forces(
+        axis_aligned,
+        beam_section,
+        axial_extension);
+    if (!close(axial_forces[0], -31.5) ||
+        !close(axial_forces[6], 31.5) ||
+        !close(axial_forces[0] + axial_forces[6], 0.0)) {
+        return 4;
+    }
+
+    if (!throws_parallel_orientation()) {
+        return 5;
+    }
+    if (!throws_zero_orientation()) {
+        return 6;
+    }
+
+    return 0;
+}