feat(fem-and-beam-kernel): step 2 — beam-local-frame

src/fesa/fem/beam_frame.cpp

@@ -0,0 +1,146 @@
+#include <fesa/fem/beam_frame.hpp>
+
+#include <algorithm>
+#include <array>
+#include <cmath>
+#include <cstddef>
+#include <limits>
+#include <string>
+#include <utility>
+
+namespace fesa {
+namespace {
+
+constexpr double kParallelTolerance =
+    64.0 * std::numeric_limits<double>::epsilon();
+
+double length(const Vec3 value) {
+    return std::hypot(value.x, value.y, value.z);
+}
+
+double max_abs_component(const Vec3 value) {
+    return std::max(
+        std::abs(value.x),
+        std::max(std::abs(value.y), std::abs(value.z)));
+}
+
+double dot(const Vec3 first, const Vec3 second) {
+    return first.x * second.x + first.y * second.y + first.z * second.z;
+}
+
+Vec3 cross(const Vec3 first, const Vec3 second) {
+    return {
+        first.y * second.z - first.z * second.y,
+        first.z * second.x - first.x * second.z,
+        first.x * second.y - first.y * second.x,
+    };
+}
+
+Vec3 normalized(const Vec3 value) {
+    const double scale = max_abs_component(value);
+    const Vec3 scaled{
+        value.x / scale,
+        value.y / scale,
+        value.z / scale,
+    };
+    const double scaled_length = length(scaled);
+    return {
+        scaled.x / scaled_length,
+        scaled.y / scaled_length,
+        scaled.z / scaled_length,
+    };
+}
+
+BeamFrameResult error_result(std::string code, std::string message) {
+    BeamFrameResult result;
+    result.diagnostics.push_back({
+        DiagnosticStage::model,
+        Severity::error,
+        std::move(code),
+        std::move(message),
+        std::nullopt,
+    });
+    return result;
+}
+
+}  // namespace
+
+BeamFrameResult make_beam_frame(
+    const Vec3& first,
+    const Vec3& second,
+    const Vec3& orientation) {
+    if (!is_finite(first) || !is_finite(second)) {
+        return error_result(
+            "model.nonfinite_value",
+            "Beam frame requires finite node coordinates.");
+    }
+
+    const Vec3 axis{
+        second.x - first.x,
+        second.y - first.y,
+        second.z - first.z,
+    };
+    if (!is_finite(axis)) {
+        return error_result(
+            "model.nonfinite_value",
+            "Beam frame axis is not representable as a finite vector.");
+    }
+
+    if (max_abs_component(axis) == 0.0) {
+        return error_result(
+            "model.zero_length_element",
+            "Beam frame requires distinct node coordinates.");
+    }
+
+    if (!is_finite(orientation)) {
+        return error_result(
+            "model.nonfinite_value",
+            "Beam frame requires a finite orientation vector.");
+    }
+
+    if (max_abs_component(orientation) == 0.0) {
+        return error_result(
+            "model.invalid_orientation",
+            "Beam frame requires a nonzero orientation vector.");
+    }
+
+    const Vec3 ex = normalized(axis);
+    const Vec3 orientation_unit = normalized(orientation);
+    const double axial_projection = dot(orientation_unit, ex);
+    const Vec3 transverse{
+        orientation_unit.x - axial_projection * ex.x,
+        orientation_unit.y - axial_projection * ex.y,
+        orientation_unit.z - axial_projection * ex.z,
+    };
+    const double transverse_length = length(transverse);
+    if (transverse_length <= kParallelTolerance) {
+        return error_result(
+            "model.invalid_orientation",
+            "Beam orientation is parallel to the element axis.");
+    }
+
+    const Vec3 ey = normalized(transverse);
+    const Vec3 ez = cross(ex, ey);
+    return {
+        BeamFrame{ex, ey, ez},
+        {},
+    };
+}
+
+Matrix12 beam_transformation(const BeamFrame& frame) {
+    Matrix12 transformation{};
+    const std::array<Vec3, 3> basis{frame.ex, frame.ey, frame.ez};
+    constexpr std::array<std::size_t, 4> block_offsets{0, 3, 6, 9};
+
+    for (const std::size_t offset : block_offsets) {
+        for (std::size_t row = 0; row < basis.size(); ++row) {
+            transformation[offset + row][offset] = basis[row].x;
+            transformation[offset + row][offset + 1] = basis[row].y;
+            transformation[offset + row][offset + 2] = basis[row].z;
+        }
+    }
+
+    return transformation;
+}
+
+}  // namespace fesa