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

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));
+  }
+}