Add struct implementations for Matrix and Vector

wpimath/src/main/native/include/frc/struct/MatrixStruct.h

@@ -0,0 +1,36 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <fmt/format.h>
+#include <wpi/ct_string.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/EigenCore.h"
+
+template <int Rows, int Cols, int Options, int MaxRows, int MaxCols>
+  requires(Cols != 1)
+struct wpi::Struct<frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>> {
+  static constexpr ct_string kTypeString =
+      wpi::Concat("struct:Matrix__"_ct_string, wpi::NumToCtString<Rows>(),
+                  "_"_ct_string, wpi::NumToCtString<Cols>());
+  static constexpr std::string_view GetTypeString() { return kTypeString; }
+  static constexpr size_t GetSize() { return Rows * Cols * 8; }
+  static constexpr ct_string kSchema =
+      wpi::Concat("double data["_ct_string, wpi::NumToCtString<Rows * Cols>(),
+                  "]"_ct_string);
+  static constexpr std::string_view GetSchema() { return kSchema; }
+
+  static frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols> Unpack(
+      std::span<const uint8_t> data);
+  static void Pack(
+      std::span<uint8_t> data,
+      const frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>& value);
+};
+
+static_assert(wpi::StructSerializable<frc::Matrixd<1, 2>>);
+static_assert(wpi::StructSerializable<frc::Matrixd<3, 3>>);
+
+#include "frc/struct/MatrixStruct.inc"

wpimath/src/main/native/include/frc/struct/MatrixStruct.inc

@@ -0,0 +1,37 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "frc/struct/MatrixStruct.h"
+
+namespace {
+constexpr size_t kDataOff = 0;
+}  // namespace
+
+template <int Rows, int Cols, int Options, int MaxRows, int MaxCols>
+  requires(Cols != 1)
+frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>
+wpi::Struct<frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>>::Unpack(
+    std::span<const uint8_t> data) {
+  wpi::array<double, Rows * Cols> mat_data =
+      wpi::UnpackStructArray<double, kDataOff, Rows * Cols>(data);
+  frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols> mat;
+  for (int i = 0; i < Rows * Cols; i++) {
+    mat(i) = mat_data[i];
+  }
+  return mat;
+}
+
+template <int Rows, int Cols, int Options, int MaxRows, int MaxCols>
+  requires(Cols != 1)
+void wpi::Struct<frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>>::Pack(
+    std::span<uint8_t> data,
+    const frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>& value) {
+  wpi::array<double, Rows * Cols> mat_data(wpi::empty_array);
+  for (int i = 0; i < Rows * Cols; i++) {
+    mat_data[i] = value(i);
+  }
+  wpi::PackStructArray<kDataOff, Rows * Cols>(data, mat_data);
+}

wpimath/src/main/native/include/frc/struct/VectorStruct.h

@@ -0,0 +1,34 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <fmt/format.h>
+#include <wpi/ct_string.h>
+#include <wpi/struct/Struct.h>
+
+#include "frc/EigenCore.h"
+
+template <int Size, int Options, int MaxRows, int MaxCols>
+struct wpi::Struct<frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>> {
+  static constexpr ct_string kTypeString =
+      wpi::Concat("struct:Vector__"_ct_string, wpi::NumToCtString<Size>());
+  static constexpr std::string_view GetTypeString() { return kTypeString; }
+  static constexpr size_t GetSize() { return Size * 8; }
+  static constexpr ct_string kSchema =
+      wpi::Concat("double data["_ct_string, wpi::NumToCtString<Size>(),
+                  "]"_ct_string);
+  static constexpr std::string_view GetSchema() { return kSchema; }
+
+  static frc::Matrixd<Size, 1, Options, MaxRows, MaxCols> Unpack(
+      std::span<const uint8_t> data);
+  static void Pack(
+      std::span<uint8_t> data,
+      const frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>& value);
+};
+
+static_assert(wpi::StructSerializable<frc::Vectord<1>>);
+static_assert(wpi::StructSerializable<frc::Vectord<3>>);
+
+#include "frc/struct/VectorStruct.inc"

wpimath/src/main/native/include/frc/struct/VectorStruct.inc

@@ -0,0 +1,35 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "frc/struct/VectorStruct.h"
+
+namespace {
+constexpr size_t kDataOff = 0;
+}  // namespace
+
+template <int Size, int Options, int MaxRows, int MaxCols>
+frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>
+wpi::Struct<frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>>::Unpack(
+    std::span<const uint8_t> data) {
+  wpi::array<double, Size> vec_data =
+      wpi::UnpackStructArray<double, kDataOff, Size>(data);
+  frc::Matrixd<Size, 1, Options, MaxRows, MaxCols> vec;
+  for (int i = 0; i < Size; i++) {
+    vec(i) = vec_data[i];
+  }
+  return vec;
+}
+
+template <int Size, int Options, int MaxRows, int MaxCols>
+void wpi::Struct<frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>>::Pack(
+    std::span<uint8_t> data,
+    const frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>& value) {
+  wpi::array<double, Size> vec_data(wpi::empty_array);
+  for (int i = 0; i < Size; i++) {
+    vec_data[i] = value(i);
+  }
+  wpi::PackStructArray<kDataOff, Size>(data, vec_data);
+}

wpimath/src/test/native/cpp/struct/MatrixStructTest.cpp

@@ -0,0 +1,23 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+
+#include "../StructTestBase.h"
+#include "frc/EigenCore.h"
+#include "frc/struct/MatrixStruct.h"
+
+using namespace frc;
+
+struct MatrixStructTestData {
+  using Type = Matrixd<2, 3>;
+
+  inline static const Type kTestData{{1.1, 1.2, 1.3}, {1.4, 1.5, 1.6}};
+
+  static void CheckEq(const Type& testData, const Type& data) {
+    EXPECT_EQ(testData, data);
+  }
+};
+
+INSTANTIATE_TYPED_TEST_SUITE_P(Matrix, StructTest, MatrixStructTestData);

wpimath/src/test/native/cpp/struct/VectorStructTest.cpp

@@ -0,0 +1,23 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+
+#include "../StructTestBase.h"
+#include "frc/EigenCore.h"
+#include "frc/struct/VectorStruct.h"
+
+using namespace frc;
+
+struct VectorStructTestData {
+  using Type = Vectord<2>;
+
+  inline static const Type kTestData{1.1, 1.2};
+
+  static void CheckEq(const Type& testData, const Type& data) {
+    EXPECT_EQ(testData, data);
+  }
+};
+
+INSTANTIATE_TYPED_TEST_SUITE_P(Vector, StructTest, VectorStructTestData);