diff --git a/src/architecture/utilities/eigenSupport.h b/src/architecture/utilities/eigenSupport.h index 54f8b21f28..5e9ba7bd87 100644 --- a/src/architecture/utilities/eigenSupport.h +++ b/src/architecture/utilities/eigenSupport.h @@ -11,6 +11,70 @@ #include #include +// ============================================================================= +// Design goals for this header +// ============================================================================= +// +// This header bridges Eigen types and plain C arrays at the boundary with +// Ada/SWIG/message-buffer code. The functions below already follow a small +// set of conventions; the rules are stated here so that future contributions +// stay consistent and so deviations are easy to spot. +// +// 1. Two directions, two parameter conventions. +// Output-side functions (Eigen -> C array) take the destination as a sized +// array reference `T (&out)[N]` so the buffer length is deduced and bounds +// are checked at compile time. Input-side functions (C array -> Eigen) +// take a `const ScalarT*` because callers commonly hand them stride-indexed +// offsets into larger buffers (e.g. `&GsMatrix_B[i * 3]`) or struct member +// arrays - array-reference parameters would force those callers into +// awkward casts. The `cArrayToEigenVector` overload is the exception (its +// size is part of the contract) and accepts `const ScalarT (&)[size]`. +// +// 2. Compile-time shape enforcement when the type carries it. +// Fixed-size variants (`eigenMatrixToCArray`, `eigenMatrixToCArray2D`, +// `eigenVectorToCArray`, `cArrayToEigenMatrix`, `cArrayToEigenVector3`, +// `cArrayToEigenMatrix3`, `c2DArrayToEigenMatrix3`, `eigenTilde`) use +// `static_assert` on `RowsAtCompileTime` / `ColsAtCompileTime` and on +// destination size. Compile-time fixed sizes are regularly valuable in +// embedded / flight-software contexts - they enable static stack +// reasoning, eliminate heap allocation, and surface shape mismatches +// before the binary leaves the developer's machine - and the FSW side +// of this codebase prefers them by default. Dynamic-size variants (named +// with an `X` infix: `eigenMatrixXToCArray`, `eigenMatrixXToCArray2D`, +// `eigenMatrixXInsertCArray`, `cArrayToEigenMatrixX`) fall back to runtime +// checks and `std::terminate()` on shape or capacity violations. The +// dynamic variants exist for host-PC / Xmera simulation modules where +// shapes legitimately depend on runtime configuration (variable-length +// sensor arrays, scenario-driven reaction wheel counts, etc.) and the +// FSW compile-time guarantees aren't applicable. +// +// 3. Row-major C array convention, regardless of Eigen storage order. +// All matrix <-> C-array conversions read and write row-major. Column- +// major Eigen inputs are transposed internally; row-major inputs go +// through unchanged. Callers don't need to reason about Eigen's default +// storage order. +// +// 4. Accept any Eigen expression on the output side. +// Output-side functions take `const Eigen::MatrixBase&`, not +// concrete `Eigen::Matrix`/`Vector`. This admits `Zero()`, `Ones()`, +// `Constant(...)`, `transpose()`, `block<...>()`, and segment views in +// addition to plain matrix/vector variables. Internal evaluation to a +// `PlainObject` handles non-contiguous expressions safely. +// +// 5. Const correctness on inputs. +// Input parameters are const-qualified (`const ScalarT*`, +// `const ScalarT (&)[N]`, `const Eigen::MatrixBase&`). +// `const`-qualified C arrays must be acceptable - regression tests in +// `tests/test_eigenSupport.cpp` enforce this for each input-side +// function, so removing `const` somewhere will fail to compile. +// +// 6. Fail loudly. +// Compile-time violations use `static_assert` with a message identifying +// which constraint failed. Runtime violations on dynamic variants call +// `std::terminate()` rather than throwing or silently truncating. +// +// ============================================================================= + template inline constexpr bool is_row_major_v = (Eigen::internal::traits::Flags & Eigen::RowMajorBit) != 0; @@ -189,16 +253,28 @@ void eigenMatrixXInsertCArray(const Eigen::MatrixBase& inMat, } /** - * @brief Copy a fixed-size Eigen vector into a contiguous C array. + * @brief Copy a fixed-size Eigen column-vector expression into a C array. * - * @tparam ScalarT Scalar type stored in the vector. - * @tparam size Compile-time number of elements. - * @param inVec Vector whose contents should be copied. - * @param outArray Pointer to a C array with at least `size` elements. + * Accepts any fixed-size Eigen expression that resolves to a column vector + * (concrete `Eigen::Vector`, `Vector::Zero()`, `block()`, etc.). + * The destination is a sized C array; its length is enforced at compile time + * to match the vector length. + * + * @tparam Derived Fixed-size Eigen column-vector expression type. + * @tparam size Extent of the destination array (must equal vector length). + * @param inVec Vector expression whose contents should be copied. + * @param out Destination array that receives the entries. */ -template -void eigenVectorToCArray(const Eigen::Vector& inVec, ScalarT* outArray) { - std::copy(inVec.data(), inVec.data() + size, outArray); +template +void eigenVectorToCArray(const Eigen::MatrixBase& inVec, typename Derived::Scalar (&out)[size]) { + static_assert(Derived::RowsAtCompileTime != Eigen::Dynamic && Derived::ColsAtCompileTime != Eigen::Dynamic, + "Input must be a fixed-size Eigen type."); + static_assert(Derived::ColsAtCompileTime == 1, "Input must be a column vector."); + static_assert(static_cast(Derived::RowsAtCompileTime) == size, + "Output array size must equal vector length."); + + const typename Derived::PlainObject evaluated = inVec; + std::copy(evaluated.data(), evaluated.data() + size, out); } /** @@ -286,14 +362,17 @@ Eigen::Matrix3 cArrayToEigenMatrix3(const ScalarT* inArray) { /** * @brief Copy a 3×3 C two-dimensional array into an Eigen 3×3 matrix. * + * Both dimensions are enforced at compile time via the array reference + * parameter, and the input is const-qualified for consistency with the rest + * of the input-side conversion functions. + * * @tparam ScalarT Scalar type of the matrix. * @param in2DArray Source array with bounds `[3][3]`. * @return Eigen::Matrix3 containing the same entries. */ template -Eigen::Matrix3 c2DArrayToEigenMatrix3(ScalarT in2DArray[3][3]) { - Eigen::Matrix3 outMat = Eigen::Map>(&in2DArray[0][0]); - return outMat; +Eigen::Matrix3 c2DArrayToEigenMatrix3(const ScalarT (&in2DArray)[3][3]) { + return Eigen::Map>(&in2DArray[0][0]); } /** @@ -356,12 +435,28 @@ Eigen::Matrix3 eigenM3(ScalarT angle) { /** * @brief Construct the skew-symmetric matrix such that `[tilde(vec)] * b = vec × b`. * - * @tparam Derived Eigen vector expression type. + * Accepts either a fixed-size 3-element column vector (shape checked at + * compile time via `static_assert`) or a dynamic-size expression that is + * 3×1 at runtime (shape checked via `std::terminate()`). This mirrors the + * suite-wide split between fixed and dynamic variants documented at the + * top of this header. + * + * @tparam Derived Eigen 3-vector expression type. * @param vec Vector whose associated tilde matrix is requested. * @return Eigen::Matrix3 representing the skew-symmetric cross-product matrix. */ template Eigen::Matrix3::Scalar> eigenTilde(const Eigen::MatrixBase& vec) { + static_assert((Derived::RowsAtCompileTime == 3 || Derived::RowsAtCompileTime == Eigen::Dynamic) && + (Derived::ColsAtCompileTime == 1 || Derived::ColsAtCompileTime == Eigen::Dynamic), + "eigenTilde requires a 3-element column vector (fixed-size or dynamic)."); + + if constexpr (Derived::RowsAtCompileTime == Eigen::Dynamic || Derived::ColsAtCompileTime == Eigen::Dynamic) { + if (vec.rows() != 3 || vec.cols() != 1) { + std::terminate(); + } + } + using Scalar = Eigen::MatrixBase::Scalar; const Scalar vx = vec(0); diff --git a/src/architecture/utilities/tests/test_eigenSupport.cpp b/src/architecture/utilities/tests/test_eigenSupport.cpp index 287fedd122..a595e63bbd 100644 --- a/src/architecture/utilities/tests/test_eigenSupport.cpp +++ b/src/architecture/utilities/tests/test_eigenSupport.cpp @@ -121,14 +121,36 @@ TYPED_TEST(EigenSupportConversionsTest, EigenVectorToCArrayCopiesElements) { input(i) = static_cast(i - 1); } - std::array output{}; - eigenVectorToCArray(input, output.data()); + Scalar output[size] = {}; + eigenVectorToCArray(input, output); for (int i = 0; i < size; ++i) { EXPECT_EQ(output[i], input(i)); } } +TYPED_TEST(EigenSupportConversionsTest, EigenVectorToCArrayAcceptsExpression) { + using Scalar = TypeParam; + + // Constant expression (Zero) - the original failure mode that prompted + // widening the signature to MatrixBase. + Scalar zeroOut[3] = {static_cast(7), static_cast(7), static_cast(7)}; + eigenVectorToCArray(Eigen::Vector3::Zero(), zeroOut); + for (int i = 0; i < 3; ++i) { + EXPECT_EQ(zeroOut[i], static_cast(0)); + } + + // Block expression - exercises the PlainObject evaluation path. + Eigen::Matrix source; + source << static_cast(1), static_cast(2), static_cast(3), static_cast(4); + + Scalar blockOut[3] = {}; + eigenVectorToCArray(source.template head<3>(), blockOut); + for (int i = 0; i < 3; ++i) { + EXPECT_EQ(blockOut[i], source(i)); + } +} + TYPED_TEST(EigenSupportConversionsTest, cArrayToEigenMatrixPreservesColumnMajorOrdering) { using Scalar = TypeParam; constexpr int rows = 3; @@ -281,6 +303,21 @@ TYPED_TEST(EigenSupportConversionsTest, C2DArrayToEigenMatrix3CopiesEntries) { } } +TYPED_TEST(EigenSupportConversionsTest, C2DArrayToEigenMatrix3AcceptsConstInput) { + using Scalar = TypeParam; + const Scalar raw[3][3] = {{static_cast(1), static_cast(2), static_cast(3)}, + {static_cast(4), static_cast(5), static_cast(6)}, + {static_cast(7), static_cast(8), static_cast(9)}}; + + Eigen::Matrix3 reconstructed = c2DArrayToEigenMatrix3(raw); + + for (int i = 0; i < 3; ++i) { + for (int j = 0; j < 3; ++j) { + EXPECT_EQ(reconstructed(i, j), raw[i][j]); + } + } +} + TYPED_TEST(EigenSupportConversionsTest, EigenMatrixToCArray2DColumnMajorInput) { using Scalar = TypeParam; using MatrixType = Eigen::Matrix;