[wpimath] Add remaining struct and protobuf implementations (#5953)

wpimath/src/main/native/include/frc/proto/MatrixProto.h

@@ -0,0 +1,22 @@
+// 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 <wpi/protobuf/Protobuf.h>
+
+#include "frc/EigenCore.h"
+
+template <int Rows, int Cols, int Options, int MaxRows, int MaxCols>
+  requires(Cols != 1)
+struct wpi::Protobuf<frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols> Unpack(
+      const google::protobuf::Message& msg);
+  static void Pack(
+      google::protobuf::Message* msg,
+      const frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>& value);
+};
+
+#include "frc/proto/MatrixProto.inc"

wpimath/src/main/native/include/frc/proto/MatrixProto.inc

@@ -0,0 +1,60 @@
+// 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 <stdexcept>
+
+#include <fmt/format.h>
+#include <wpi/ProtoHelper.h>
+
+#include "frc/proto/MatrixProto.h"
+#include "wpimath.pb.h"
+
+template <int Rows, int Cols, int Options, int MaxRows, int MaxCols>
+  requires(Cols != 1)
+google::protobuf::Message*
+wpi::Protobuf<frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>>::New(
+    google::protobuf::Arena* arena) {
+  return wpi::CreateMessage<wpi::proto::ProtobufMatrix>(arena);
+}
+
+template <int Rows, int Cols, int Options, int MaxRows, int MaxCols>
+  requires(Cols != 1)
+frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>
+wpi::Protobuf<frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufMatrix*>(&msg);
+  if (m->num_rows() != Rows || m->num_cols() != Cols) {
+    throw std::invalid_argument(
+        fmt::format("Tried to unpack message with {} rows and {} columns into "
+                    "Matrix with {} rows and {} columns",
+                    m->num_rows(), m->num_cols(), Rows, Cols));
+  }
+  if (m->data_size() != Rows * Cols) {
+    throw std::invalid_argument(
+        fmt::format("Tried to unpack message with {} elements in data into "
+                    "Matrix with {} elements",
+                    m->data_size(), Rows * Cols));
+  }
+  frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols> mat;
+  for (int i = 0; i < Rows * Cols; i++) {
+    mat(i) = m->data(i);
+  }
+  return mat;
+}
+
+template <int Rows, int Cols, int Options, int MaxRows, int MaxCols>
+  requires(Cols != 1)
+void wpi::Protobuf<frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>>::Pack(
+    google::protobuf::Message* msg,
+    const frc::Matrixd<Rows, Cols, Options, MaxRows, MaxCols>& value) {
+  auto m = static_cast<wpi::proto::ProtobufMatrix*>(msg);
+  m->set_num_rows(Rows);
+  m->set_num_cols(Cols);
+  m->clear_data();
+  for (int i = 0; i < Rows * Cols; i++) {
+    m->add_data(value(i));
+  }
+}

wpimath/src/main/native/include/frc/proto/VectorProto.h

@@ -0,0 +1,21 @@
+// 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 <wpi/protobuf/Protobuf.h>
+
+#include "frc/EigenCore.h"
+
+template <int Size, int Options, int MaxRows, int MaxCols>
+struct wpi::Protobuf<frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>> {
+  static google::protobuf::Message* New(google::protobuf::Arena* arena);
+  static frc::Matrixd<Size, 1, Options, MaxRows, MaxCols> Unpack(
+      const google::protobuf::Message& msg);
+  static void Pack(
+      google::protobuf::Message* msg,
+      const frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>& value);
+};
+
+#include "frc/proto/VectorProto.inc"

wpimath/src/main/native/include/frc/proto/VectorProto.inc

@@ -0,0 +1,49 @@
+// 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 <stdexcept>
+
+#include <fmt/format.h>
+#include <wpi/ProtoHelper.h>
+
+#include "frc/proto/VectorProto.h"
+#include "wpimath.pb.h"
+
+template <int Size, int Options, int MaxRows, int MaxCols>
+google::protobuf::Message*
+wpi::Protobuf<frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>>::New(
+    google::protobuf::Arena* arena) {
+  return wpi::CreateMessage<wpi::proto::ProtobufVector>(arena);
+}
+
+template <int Size, int Options, int MaxRows, int MaxCols>
+frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>
+wpi::Protobuf<frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufVector*>(&msg);
+  if (m->rows_size() != Size) {
+    throw std::invalid_argument(
+        fmt::format("Tried to unpack message with {} elements in rows into "
+                    "Vector with {} rows",
+                    m->rows_size(), Size));
+  }
+  frc::Matrixd<Size, 1, Options, MaxRows, MaxCols> vec;
+  for (int i = 0; i < Size; i++) {
+    vec(i) = m->rows(i);
+  }
+  return vec;
+}
+
+template <int Size, int Options, int MaxRows, int MaxCols>
+void wpi::Protobuf<frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>>::Pack(
+    google::protobuf::Message* msg,
+    const frc::Matrixd<Size, 1, Options, MaxRows, MaxCols>& value) {
+  auto m = static_cast<wpi::proto::ProtobufVector*>(msg);
+  m->clear_rows();
+  for (int i = 0; i < Size; i++) {
+    m->add_rows(value(i));
+  }
+}