commit 2739765ce40ba481c467c953e75beed3ef83673c
author James Kuszmaul <jabukuszmaul+collab@gmail.com> Fri Mar 15 21:44:52 2024 -0700
committer James Kuszmaul <jabukuszmaul+collab@gmail.com> Fri Mar 15 21:56:39 2024 -0700
tree d7f6bcffbd7ad6d37238a569f7f15301326bdadc
parent 07131d93b963996eb4b4f2d715a892f82bd228e5

Support FlatBufferBuilder in matrix library

Change-Id: I01e8638edd2794cd2fcbade5e754360058d999dd
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>

frc971/math/flatbuffers_matrix.h

diff --git a/frc971/math/flatbuffers_matrix.h b/frc971/math/flatbuffers_matrix.h
index 57013c9..cfca897 100644
--- a/frc971/math/flatbuffers_matrix.h
+++ b/frc971/math/flatbuffers_matrix.h
@@ -121,6 +121,22 @@
   return true;
 }
 
+template <int Rows, int Cols,
+          fbs::StorageOrder StorageOrder = fbs::StorageOrder::ColMajor>
+flatbuffers::Offset<fbs::Matrix> FromEigen(
+    const typename EigenMatrix<Rows, Cols, StorageOrder>::type &matrix,
+    flatbuffers::FlatBufferBuilder *fbb) {
+  constexpr size_t kSize = Rows * Cols;
+  flatbuffers::Offset<flatbuffers::Vector<double>> data_offset =
+      fbb->CreateVector(matrix.data(), kSize);
+  fbs::Matrix::Builder builder(*fbb);
+  builder.add_rows(Rows);
+  builder.add_cols(Cols);
+  builder.add_storage_order(StorageOrder);
+  builder.add_data(data_offset);
+  return builder.Finish();
+}
+
 template <typename T>
 bool FromEigen(const T &matrix, fbs::MatrixStatic *flatbuffer) {
   return FromEigen<T::RowsAtCompileTime, T::ColsAtCompileTime,

frc971/math/flatbuffers_matrix_test.cc

diff --git a/frc971/math/flatbuffers_matrix_test.cc b/frc971/math/flatbuffers_matrix_test.cc
index 2807309..b2587f3 100644
--- a/frc971/math/flatbuffers_matrix_test.cc
+++ b/frc971/math/flatbuffers_matrix_test.cc
@@ -24,11 +24,18 @@
   const Eigen::Matrix<double, 3, 4> expected{
       {0, 1, 2, 3}, {4, 5, 6, 7}, {8, 9, 10, 11}};
   aos::fbs::Builder<fbs::MatrixStatic> builder;
+  flatbuffers::FlatBufferBuilder fbb;
   ASSERT_TRUE(FromEigen(expected, builder.get()));
+  fbb.Finish(FromEigen<3, 4>(expected, &fbb));
   EXPECT_EQ(
       "{ \"rows\": 3, \"cols\": 4, \"storage_order\": \"ColMajor\", \"data\": "
       "[ 0.0, 4.0, 8.0, 1.0, 5.0, 9.0, 2.0, 6.0, 10.0, 3.0, 7.0, 11.0 ] }",
       aos::FlatbufferToJson(builder.AsFlatbufferSpan()));
+  EXPECT_EQ(
+      "{ \"rows\": 3, \"cols\": 4, \"storage_order\": \"ColMajor\", \"data\": "
+      "[ 0.0, 4.0, 8.0, 1.0, 5.0, 9.0, 2.0, 6.0, 10.0, 3.0, 7.0, 11.0 ] }",
+      aos::FlatbufferToJson(
+          aos::FlatbufferDetachedBuffer<fbs::Matrix>(fbb.Release())));
 
   const Eigen::Matrix<double, 3, 4> result =
       ToEigenOrDie<3, 4>(builder->AsFlatbuffer());