[wpimath] Move struct/proto classes to separate files (#5918)

Also add unit tests.

wpimath/src/main/native/cpp/geometry/Pose2d.cpp

@@ -9,7 +9,6 @@
 #include <wpi/json.h>
 
 #include "frc/MathUtil.h"
-#include "geometry2d.pb.h"
 
 using namespace frc;
 
@@ -109,23 +108,3 @@ void frc::from_json(const wpi::json& json, Pose2d& pose) {
   pose = Pose2d{json.at("translation").get<Translation2d>(),
                 json.at("rotation").get<Rotation2d>()};
 }
-
-google::protobuf::Message* wpi::Protobuf<frc::Pose2d>::New(
-    google::protobuf::Arena* arena) {
-  return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufPose2d>(
-      arena);
-}
-
-frc::Pose2d wpi::Protobuf<frc::Pose2d>::Unpack(
-    const google::protobuf::Message& msg) {
-  auto m = static_cast<const wpi::proto::ProtobufPose2d*>(&msg);
-  return Pose2d{wpi::UnpackProtobuf<frc::Translation2d>(m->translation()),
-                wpi::UnpackProtobuf<frc::Rotation2d>(m->rotation())};
-}
-
-void wpi::Protobuf<frc::Pose2d>::Pack(google::protobuf::Message* msg,
-                                      const frc::Pose2d& value) {
-  auto m = static_cast<wpi::proto::ProtobufPose2d*>(msg);
-  wpi::PackProtobuf(m->mutable_translation(), value.Translation());
-  wpi::PackProtobuf(m->mutable_rotation(), value.Rotation());
-}

wpimath/src/main/native/cpp/geometry/Pose3d.cpp

@@ -189,23 +189,3 @@ void frc::from_json(const wpi::json& json, Pose3d& pose) {
   pose = Pose3d{json.at("translation").get<Translation3d>(),
                 json.at("rotation").get<Rotation3d>()};
 }
-
-google::protobuf::Message* wpi::Protobuf<frc::Pose3d>::New(
-    google::protobuf::Arena* arena) {
-  return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufPose3d>(
-      arena);
-}
-
-frc::Pose3d wpi::Protobuf<frc::Pose3d>::Unpack(
-    const google::protobuf::Message& msg) {
-  auto m = static_cast<const wpi::proto::ProtobufPose3d*>(&msg);
-  return Pose3d{wpi::UnpackProtobuf<frc::Translation3d>(m->translation()),
-                wpi::UnpackProtobuf<frc::Rotation3d>(m->rotation())};
-}
-
-void wpi::Protobuf<frc::Pose3d>::Pack(google::protobuf::Message* msg,
-                                      const frc::Pose3d& value) {
-  auto m = static_cast<wpi::proto::ProtobufPose3d*>(msg);
-  wpi::PackProtobuf(m->mutable_translation(), value.Translation());
-  wpi::PackProtobuf(m->mutable_rotation(), value.Rotation());
-}

wpimath/src/main/native/cpp/geometry/Quaternion.cpp

@@ -232,24 +232,3 @@ void frc::from_json(const wpi::json& json, Quaternion& quaternion) {
       Quaternion{json.at("W").get<double>(), json.at("X").get<double>(),
                  json.at("Y").get<double>(), json.at("Z").get<double>()};
 }
-
-google::protobuf::Message* wpi::Protobuf<frc::Quaternion>::New(
-    google::protobuf::Arena* arena) {
-  return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufQuaternion>(
-      arena);
-}
-
-frc::Quaternion wpi::Protobuf<frc::Quaternion>::Unpack(
-    const google::protobuf::Message& msg) {
-  auto m = static_cast<const wpi::proto::ProtobufQuaternion*>(&msg);
-  return frc::Quaternion{m->w(), m->x(), m->y(), m->z()};
-}
-
-void wpi::Protobuf<frc::Quaternion>::Pack(google::protobuf::Message* msg,
-                                          const frc::Quaternion& value) {
-  auto m = static_cast<wpi::proto::ProtobufQuaternion*>(msg);
-  m->set_w(value.W());
-  m->set_x(value.X());
-  m->set_y(value.Y());
-  m->set_z(value.Z());
-}

wpimath/src/main/native/cpp/geometry/Rotation2d.cpp

@@ -20,21 +20,3 @@ void frc::to_json(wpi::json& json, const Rotation2d& rotation) {
 void frc::from_json(const wpi::json& json, Rotation2d& rotation) {
   rotation = Rotation2d{units::radian_t{json.at("radians").get<double>()}};
 }
-
-google::protobuf::Message* wpi::Protobuf<frc::Rotation2d>::New(
-    google::protobuf::Arena* arena) {
-  return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufRotation2d>(
-      arena);
-}
-
-frc::Rotation2d wpi::Protobuf<frc::Rotation2d>::Unpack(
-    const google::protobuf::Message& msg) {
-  auto m = static_cast<const wpi::proto::ProtobufRotation2d*>(&msg);
-  return frc::Rotation2d{units::radian_t{m->value()}};
-}
-
-void wpi::Protobuf<frc::Rotation2d>::Pack(google::protobuf::Message* msg,
-                                          const frc::Rotation2d& value) {
-  auto m = static_cast<wpi::proto::ProtobufRotation2d*>(msg);
-  m->set_value(value.Radians().value());
-}

wpimath/src/main/native/cpp/geometry/Rotation3d.cpp

@@ -262,21 +262,3 @@ void frc::to_json(wpi::json& json, const Rotation3d& rotation) {
 void frc::from_json(const wpi::json& json, Rotation3d& rotation) {
   rotation = Rotation3d{json.at("quaternion").get<Quaternion>()};
 }
-
-google::protobuf::Message* wpi::Protobuf<frc::Rotation3d>::New(
-    google::protobuf::Arena* arena) {
-  return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufRotation3d>(
-      arena);
-}
-
-frc::Rotation3d wpi::Protobuf<frc::Rotation3d>::Unpack(
-    const google::protobuf::Message& msg) {
-  auto m = static_cast<const wpi::proto::ProtobufRotation3d*>(&msg);
-  return Rotation3d{wpi::UnpackProtobuf<frc::Quaternion>(m->q())};
-}
-
-void wpi::Protobuf<frc::Rotation3d>::Pack(google::protobuf::Message* msg,
-                                          const frc::Rotation3d& value) {
-  auto m = static_cast<wpi::proto::ProtobufRotation3d*>(msg);
-  wpi::PackProtobuf(m->mutable_q(), value.GetQuaternion());
-}

wpimath/src/main/native/cpp/geometry/Transform2d.cpp

@@ -22,23 +22,3 @@ Transform2d::Transform2d(Pose2d initial, Pose2d final) {
 Transform2d Transform2d::operator+(const Transform2d& other) const {
   return Transform2d{Pose2d{}, Pose2d{}.TransformBy(*this).TransformBy(other)};
 }
-
-google::protobuf::Message* wpi::Protobuf<frc::Transform2d>::New(
-    google::protobuf::Arena* arena) {
-  return google::protobuf::Arena::CreateMessage<
-      wpi::proto::ProtobufTransform2d>(arena);
-}
-
-frc::Transform2d wpi::Protobuf<frc::Transform2d>::Unpack(
-    const google::protobuf::Message& msg) {
-  auto m = static_cast<const wpi::proto::ProtobufTransform2d*>(&msg);
-  return Transform2d{wpi::UnpackProtobuf<frc::Translation2d>(m->translation()),
-                     wpi::UnpackProtobuf<frc::Rotation2d>(m->rotation())};
-}
-
-void wpi::Protobuf<frc::Transform2d>::Pack(google::protobuf::Message* msg,
-                                           const frc::Transform2d& value) {
-  auto m = static_cast<wpi::proto::ProtobufTransform2d*>(msg);
-  wpi::PackProtobuf(m->mutable_translation(), value.Translation());
-  wpi::PackProtobuf(m->mutable_rotation(), value.Rotation());
-}

wpimath/src/main/native/cpp/geometry/Transform3d.cpp

@@ -5,7 +5,6 @@
 #include "frc/geometry/Transform3d.h"
 
 #include "frc/geometry/Pose3d.h"
-#include "geometry3d.pb.h"
 
 using namespace frc;
 
@@ -36,23 +35,3 @@ Transform3d Transform3d::Inverse() const {
 Transform3d Transform3d::operator+(const Transform3d& other) const {
   return Transform3d{Pose3d{}, Pose3d{}.TransformBy(*this).TransformBy(other)};
 }
-
-google::protobuf::Message* wpi::Protobuf<frc::Transform3d>::New(
-    google::protobuf::Arena* arena) {
-  return google::protobuf::Arena::CreateMessage<
-      wpi::proto::ProtobufTransform3d>(arena);
-}
-
-frc::Transform3d wpi::Protobuf<frc::Transform3d>::Unpack(
-    const google::protobuf::Message& msg) {
-  auto m = static_cast<const wpi::proto::ProtobufTransform3d*>(&msg);
-  return Transform3d{wpi::UnpackProtobuf<frc::Translation3d>(m->translation()),
-                     wpi::UnpackProtobuf<frc::Rotation3d>(m->rotation())};
-}
-
-void wpi::Protobuf<frc::Transform3d>::Pack(google::protobuf::Message* msg,
-                                           const frc::Transform3d& value) {
-  auto m = static_cast<wpi::proto::ProtobufTransform3d*>(msg);
-  wpi::PackProtobuf(m->mutable_translation(), value.Translation());
-  wpi::PackProtobuf(m->mutable_rotation(), value.Rotation());
-}

wpimath/src/main/native/cpp/geometry/Translation2d.cpp

@@ -49,22 +49,3 @@ void frc::from_json(const wpi::json& json, Translation2d& translation) {
   translation = Translation2d{units::meter_t{json.at("x").get<double>()},
                               units::meter_t{json.at("y").get<double>()}};
 }
-
-google::protobuf::Message* wpi::Protobuf<frc::Translation2d>::New(
-    google::protobuf::Arena* arena) {
-  return google::protobuf::Arena::CreateMessage<
-      wpi::proto::ProtobufTranslation2d>(arena);
-}
-
-frc::Translation2d wpi::Protobuf<frc::Translation2d>::Unpack(
-    const google::protobuf::Message& msg) {
-  auto m = static_cast<const wpi::proto::ProtobufTranslation2d*>(&msg);
-  return frc::Translation2d{units::meter_t{m->x()}, units::meter_t{m->y()}};
-}
-
-void wpi::Protobuf<frc::Translation2d>::Pack(google::protobuf::Message* msg,
-                                             const frc::Translation2d& value) {
-  auto m = static_cast<wpi::proto::ProtobufTranslation2d*>(msg);
-  m->set_x(value.X().value());
-  m->set_y(value.Y().value());
-}

wpimath/src/main/native/cpp/geometry/Translation3d.cpp

@@ -53,24 +53,3 @@ void frc::from_json(const wpi::json& json, Translation3d& translation) {
                               units::meter_t{json.at("y").get<double>()},
                               units::meter_t{json.at("z").get<double>()}};
 }
-
-google::protobuf::Message* wpi::Protobuf<frc::Translation3d>::New(
-    google::protobuf::Arena* arena) {
-  return google::protobuf::Arena::CreateMessage<
-      wpi::proto::ProtobufTranslation3d>(arena);
-}
-
-frc::Translation3d wpi::Protobuf<frc::Translation3d>::Unpack(
-    const google::protobuf::Message& msg) {
-  auto m = static_cast<const wpi::proto::ProtobufTranslation3d*>(&msg);
-  return frc::Translation3d{units::meter_t{m->x()}, units::meter_t{m->y()},
-                            units::meter_t{m->z()}};
-}
-
-void wpi::Protobuf<frc::Translation3d>::Pack(google::protobuf::Message* msg,
-                                             const frc::Translation3d& value) {
-  auto m = static_cast<wpi::proto::ProtobufTranslation3d*>(msg);
-  m->set_x(value.X().value());
-  m->set_y(value.Y().value());
-  m->set_z(value.Z().value());
-}

wpimath/src/main/native/cpp/geometry/proto/Pose2dProto.cpp

@@ -0,0 +1,29 @@
+// 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 "frc/geometry/proto/Pose2dProto.h"
+
+#include "geometry2d.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::Pose2d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufPose2d>(
+      arena);
+}
+
+frc::Pose2d wpi::Protobuf<frc::Pose2d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufPose2d*>(&msg);
+  return frc::Pose2d{
+      wpi::UnpackProtobuf<frc::Translation2d>(m->translation()),
+      wpi::UnpackProtobuf<frc::Rotation2d>(m->rotation()),
+  };
+}
+
+void wpi::Protobuf<frc::Pose2d>::Pack(google::protobuf::Message* msg,
+                                      const frc::Pose2d& value) {
+  auto m = static_cast<wpi::proto::ProtobufPose2d*>(msg);
+  wpi::PackProtobuf(m->mutable_translation(), value.Translation());
+  wpi::PackProtobuf(m->mutable_rotation(), value.Rotation());
+}

wpimath/src/main/native/cpp/geometry/proto/Pose3dProto.cpp

@@ -0,0 +1,29 @@
+// 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 "frc/geometry/proto/Pose3dProto.h"
+
+#include "geometry3d.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::Pose3d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufPose3d>(
+      arena);
+}
+
+frc::Pose3d wpi::Protobuf<frc::Pose3d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufPose3d*>(&msg);
+  return frc::Pose3d{
+      wpi::UnpackProtobuf<frc::Translation3d>(m->translation()),
+      wpi::UnpackProtobuf<frc::Rotation3d>(m->rotation()),
+  };
+}
+
+void wpi::Protobuf<frc::Pose3d>::Pack(google::protobuf::Message* msg,
+                                      const frc::Pose3d& value) {
+  auto m = static_cast<wpi::proto::ProtobufPose3d*>(msg);
+  wpi::PackProtobuf(m->mutable_translation(), value.Translation());
+  wpi::PackProtobuf(m->mutable_rotation(), value.Rotation());
+}

wpimath/src/main/native/cpp/geometry/proto/QuaternionProto.cpp

@@ -0,0 +1,33 @@
+// 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 "frc/geometry/proto/QuaternionProto.h"
+
+#include "geometry3d.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::Quaternion>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufQuaternion>(
+      arena);
+}
+
+frc::Quaternion wpi::Protobuf<frc::Quaternion>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufQuaternion*>(&msg);
+  return frc::Quaternion{
+      m->w(),
+      m->x(),
+      m->y(),
+      m->z(),
+  };
+}
+
+void wpi::Protobuf<frc::Quaternion>::Pack(google::protobuf::Message* msg,
+                                          const frc::Quaternion& value) {
+  auto m = static_cast<wpi::proto::ProtobufQuaternion*>(msg);
+  m->set_w(value.W());
+  m->set_x(value.X());
+  m->set_y(value.Y());
+  m->set_z(value.Z());
+}

wpimath/src/main/native/cpp/geometry/proto/Rotation2dProto.cpp

@@ -0,0 +1,27 @@
+// 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 "frc/geometry/proto/Rotation2dProto.h"
+
+#include "geometry2d.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::Rotation2d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufRotation2d>(
+      arena);
+}
+
+frc::Rotation2d wpi::Protobuf<frc::Rotation2d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufRotation2d*>(&msg);
+  return frc::Rotation2d{
+      units::radian_t{m->value()},
+  };
+}
+
+void wpi::Protobuf<frc::Rotation2d>::Pack(google::protobuf::Message* msg,
+                                          const frc::Rotation2d& value) {
+  auto m = static_cast<wpi::proto::ProtobufRotation2d*>(msg);
+  m->set_value(value.Radians().value());
+}

wpimath/src/main/native/cpp/geometry/proto/Rotation3dProto.cpp

@@ -0,0 +1,27 @@
+// 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 "frc/geometry/proto/Rotation3dProto.h"
+
+#include "geometry3d.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::Rotation3d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufRotation3d>(
+      arena);
+}
+
+frc::Rotation3d wpi::Protobuf<frc::Rotation3d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufRotation3d*>(&msg);
+  return frc::Rotation3d{
+      wpi::UnpackProtobuf<frc::Quaternion>(m->q()),
+  };
+}
+
+void wpi::Protobuf<frc::Rotation3d>::Pack(google::protobuf::Message* msg,
+                                          const frc::Rotation3d& value) {
+  auto m = static_cast<wpi::proto::ProtobufRotation3d*>(msg);
+  wpi::PackProtobuf(m->mutable_q(), value.GetQuaternion());
+}

wpimath/src/main/native/cpp/geometry/proto/Transform2dProto.cpp

@@ -0,0 +1,29 @@
+// 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 "frc/geometry/proto/Transform2dProto.h"
+
+#include "geometry2d.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::Transform2d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<
+      wpi::proto::ProtobufTransform2d>(arena);
+}
+
+frc::Transform2d wpi::Protobuf<frc::Transform2d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufTransform2d*>(&msg);
+  return frc::Transform2d{
+      wpi::UnpackProtobuf<frc::Translation2d>(m->translation()),
+      wpi::UnpackProtobuf<frc::Rotation2d>(m->rotation()),
+  };
+}
+
+void wpi::Protobuf<frc::Transform2d>::Pack(google::protobuf::Message* msg,
+                                           const frc::Transform2d& value) {
+  auto m = static_cast<wpi::proto::ProtobufTransform2d*>(msg);
+  wpi::PackProtobuf(m->mutable_translation(), value.Translation());
+  wpi::PackProtobuf(m->mutable_rotation(), value.Rotation());
+}

wpimath/src/main/native/cpp/geometry/proto/Transform3dProto.cpp

@@ -0,0 +1,29 @@
+// 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 "frc/geometry/proto/Transform3dProto.h"
+
+#include "geometry3d.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::Transform3d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<
+      wpi::proto::ProtobufTransform3d>(arena);
+}
+
+frc::Transform3d wpi::Protobuf<frc::Transform3d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufTransform3d*>(&msg);
+  return frc::Transform3d{
+      wpi::UnpackProtobuf<frc::Translation3d>(m->translation()),
+      wpi::UnpackProtobuf<frc::Rotation3d>(m->rotation()),
+  };
+}
+
+void wpi::Protobuf<frc::Transform3d>::Pack(google::protobuf::Message* msg,
+                                           const frc::Transform3d& value) {
+  auto m = static_cast<wpi::proto::ProtobufTransform3d*>(msg);
+  wpi::PackProtobuf(m->mutable_translation(), value.Translation());
+  wpi::PackProtobuf(m->mutable_rotation(), value.Rotation());
+}

wpimath/src/main/native/cpp/geometry/proto/Translation2dProto.cpp

@@ -0,0 +1,29 @@
+// 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 "frc/geometry/proto/Translation2dProto.h"
+
+#include "geometry2d.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::Translation2d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<
+      wpi::proto::ProtobufTranslation2d>(arena);
+}
+
+frc::Translation2d wpi::Protobuf<frc::Translation2d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufTranslation2d*>(&msg);
+  return frc::Translation2d{
+      units::meter_t{m->x()},
+      units::meter_t{m->y()},
+  };
+}
+
+void wpi::Protobuf<frc::Translation2d>::Pack(google::protobuf::Message* msg,
+                                             const frc::Translation2d& value) {
+  auto m = static_cast<wpi::proto::ProtobufTranslation2d*>(msg);
+  m->set_x(value.X().value());
+  m->set_y(value.Y().value());
+}

wpimath/src/main/native/cpp/geometry/proto/Translation3dProto.cpp

@@ -0,0 +1,31 @@
+// 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 "frc/geometry/proto/Translation3dProto.h"
+
+#include "geometry3d.pb.h"
+
+google::protobuf::Message* wpi::Protobuf<frc::Translation3d>::New(
+    google::protobuf::Arena* arena) {
+  return google::protobuf::Arena::CreateMessage<
+      wpi::proto::ProtobufTranslation3d>(arena);
+}
+
+frc::Translation3d wpi::Protobuf<frc::Translation3d>::Unpack(
+    const google::protobuf::Message& msg) {
+  auto m = static_cast<const wpi::proto::ProtobufTranslation3d*>(&msg);
+  return frc::Translation3d{
+      units::meter_t{m->x()},
+      units::meter_t{m->y()},
+      units::meter_t{m->z()},
+  };
+}
+
+void wpi::Protobuf<frc::Translation3d>::Pack(google::protobuf::Message* msg,
+                                             const frc::Translation3d& value) {
+  auto m = static_cast<wpi::proto::ProtobufTranslation3d*>(msg);
+  m->set_x(value.X().value());
+  m->set_y(value.Y().value());
+  m->set_z(value.Z().value());
+}

wpimath/src/main/native/cpp/geometry/proto/Twist2dProto.cpp

@@ -2,12 +2,10 @@
 // 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 "frc/geometry/Twist2d.h"
+#include "frc/geometry/proto/Twist2dProto.h"
 
 #include "geometry2d.pb.h"
 
-using namespace frc;
-
 google::protobuf::Message* wpi::Protobuf<frc::Twist2d>::New(
     google::protobuf::Arena* arena) {
   return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufTwist2d>(
@@ -17,8 +15,11 @@ google::protobuf::Message* wpi::Protobuf<frc::Twist2d>::New(
 frc::Twist2d wpi::Protobuf<frc::Twist2d>::Unpack(
     const google::protobuf::Message& msg) {
   auto m = static_cast<const wpi::proto::ProtobufTwist2d*>(&msg);
-  return frc::Twist2d{units::meter_t{m->dx()}, units::meter_t{m->dy()},
-                      units::radian_t{m->dtheta()}};
+  return frc::Twist2d{
+      units::meter_t{m->dx()},
+      units::meter_t{m->dy()},
+      units::radian_t{m->dtheta()},
+  };
 }
 
 void wpi::Protobuf<frc::Twist2d>::Pack(google::protobuf::Message* msg,

wpimath/src/main/native/cpp/geometry/proto/Twist3dProto.cpp

@@ -2,12 +2,10 @@
 // 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 "frc/geometry/Twist3d.h"
+#include "frc/geometry/proto/Twist3dProto.h"
 
 #include "geometry3d.pb.h"
 
-using namespace frc;
-
 google::protobuf::Message* wpi::Protobuf<frc::Twist3d>::New(
     google::protobuf::Arena* arena) {
   return google::protobuf::Arena::CreateMessage<wpi::proto::ProtobufTwist3d>(
@@ -17,9 +15,11 @@ google::protobuf::Message* wpi::Protobuf<frc::Twist3d>::New(
 frc::Twist3d wpi::Protobuf<frc::Twist3d>::Unpack(
     const google::protobuf::Message& msg) {
   auto m = static_cast<const wpi::proto::ProtobufTwist3d*>(&msg);
-  return frc::Twist3d{units::meter_t{m->dx()},  units::meter_t{m->dy()},
-                      units::meter_t{m->dz()},  units::radian_t{m->rx()},
-                      units::radian_t{m->ry()}, units::radian_t{m->rz()}};
+  return frc::Twist3d{
+      units::meter_t{m->dx()},  units::meter_t{m->dy()},
+      units::meter_t{m->dz()},  units::radian_t{m->rx()},
+      units::radian_t{m->ry()}, units::radian_t{m->rz()},
+  };
 }
 
 void wpi::Protobuf<frc::Twist3d>::Pack(google::protobuf::Message* msg,

wpimath/src/main/native/cpp/geometry/struct/Pose2dStruct.cpp

@@ -0,0 +1,32 @@
+// 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 "frc/geometry/struct/Pose2dStruct.h"
+
+namespace {
+constexpr size_t kTranslationOff = 0;
+constexpr size_t kRotationOff =
+    kTranslationOff + wpi::Struct<frc::Translation2d>::kSize;
+}  // namespace
+
+using StructType = wpi::Struct<frc::Pose2d>;
+
+frc::Pose2d StructType::Unpack(std::span<const uint8_t, kSize> data) {
+  return frc::Pose2d{
+      wpi::UnpackStruct<frc::Translation2d, kTranslationOff>(data),
+      wpi::UnpackStruct<frc::Rotation2d, kRotationOff>(data),
+  };
+}
+
+void StructType::Pack(std::span<uint8_t, kSize> data,
+                      const frc::Pose2d& value) {
+  wpi::PackStruct<kTranslationOff>(data, value.Translation());
+  wpi::PackStruct<kRotationOff>(data, value.Rotation());
+}
+
+void StructType::ForEachNested(
+    std::invocable<std::string_view, std::string_view> auto fn) {
+  wpi::ForEachStructSchema<frc::Translation2d>(fn);
+  wpi::ForEachStructSchema<frc::Rotation2d>(fn);
+}

wpimath/src/main/native/cpp/geometry/struct/Pose3dStruct.cpp

@@ -0,0 +1,32 @@
+// 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 "frc/geometry/struct/Pose3dStruct.h"
+
+namespace {
+constexpr size_t kTranslationOff = 0;
+constexpr size_t kRotationOff =
+    kTranslationOff + wpi::Struct<frc::Translation3d>::kSize;
+}  // namespace
+
+using StructType = wpi::Struct<frc::Pose3d>;
+
+frc::Pose3d StructType::Unpack(std::span<const uint8_t, kSize> data) {
+  return frc::Pose3d{
+      wpi::UnpackStruct<frc::Translation3d, kTranslationOff>(data),
+      wpi::UnpackStruct<frc::Rotation3d, kRotationOff>(data),
+  };
+}
+
+void StructType::Pack(std::span<uint8_t, kSize> data,
+                      const frc::Pose3d& value) {
+  wpi::PackStruct<kTranslationOff>(data, value.Translation());
+  wpi::PackStruct<kRotationOff>(data, value.Rotation());
+}
+
+void StructType::ForEachNested(
+    std::invocable<std::string_view, std::string_view> auto fn) {
+  wpi::ForEachStructSchema<frc::Translation3d>(fn);
+  wpi::ForEachStructSchema<frc::Rotation3d>(fn);
+}

wpimath/src/main/native/cpp/geometry/struct/QuaternionStruct.cpp

@@ -0,0 +1,31 @@
+// 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 "frc/geometry/struct/QuaternionStruct.h"
+
+namespace {
+constexpr size_t kWOff = 0;
+constexpr size_t kXOff = kWOff + 8;
+constexpr size_t kYOff = kXOff + 8;
+constexpr size_t kZOff = kYOff + 8;
+}  // namespace
+
+using StructType = wpi::Struct<frc::Quaternion>;
+
+frc::Quaternion StructType::Unpack(std::span<const uint8_t, kSize> data) {
+  return frc::Quaternion{
+      wpi::UnpackStruct<double, kWOff>(data),
+      wpi::UnpackStruct<double, kXOff>(data),
+      wpi::UnpackStruct<double, kYOff>(data),
+      wpi::UnpackStruct<double, kZOff>(data),
+  };
+}
+
+void StructType::Pack(std::span<uint8_t, kSize> data,
+                      const frc::Quaternion& value) {
+  wpi::PackStruct<kWOff>(data, value.W());
+  wpi::PackStruct<kXOff>(data, value.X());
+  wpi::PackStruct<kYOff>(data, value.Y());
+  wpi::PackStruct<kZOff>(data, value.Z());
+}

wpimath/src/main/native/cpp/geometry/struct/Rotation2dStruct.cpp

@@ -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.
+
+#include "frc/geometry/struct/Rotation2dStruct.h"
+
+namespace {
+constexpr size_t kValueOff = 0;
+}  // namespace
+
+using StructType = wpi::Struct<frc::Rotation2d>;
+
+frc::Rotation2d StructType::Unpack(std::span<const uint8_t, kSize> data) {
+  return frc::Rotation2d{
+      units::radian_t{wpi::UnpackStruct<double, kValueOff>(data)},
+  };
+}
+
+void StructType::Pack(std::span<uint8_t, kSize> data,
+                      const frc::Rotation2d& value) {
+  wpi::PackStruct<kValueOff>(data, value.Radians().value());
+}

wpimath/src/main/native/cpp/geometry/struct/Rotation3dStruct.cpp

@@ -0,0 +1,27 @@
+// 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 "frc/geometry/struct/Rotation3dStruct.h"
+
+namespace {
+constexpr size_t kQOff = 0;
+}  // namespace
+
+using StructType = wpi::Struct<frc::Rotation3d>;
+
+frc::Rotation3d StructType::Unpack(std::span<const uint8_t, kSize> data) {
+  return frc::Rotation3d{
+      wpi::UnpackStruct<frc::Quaternion, kQOff>(data),
+  };
+}
+
+void StructType::Pack(std::span<uint8_t, kSize> data,
+                      const frc::Rotation3d& value) {
+  wpi::PackStruct<kQOff>(data, value.GetQuaternion());
+}
+
+void StructType::ForEachNested(
+    std::invocable<std::string_view, std::string_view> auto fn) {
+  wpi::ForEachStructSchema<frc::Quaternion>(fn);
+}

wpimath/src/main/native/cpp/geometry/struct/Transform2dStruct.cpp

@@ -0,0 +1,32 @@
+// 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 "frc/geometry/struct/Transform2dStruct.h"
+
+namespace {
+constexpr size_t kTranslationOff = 0;
+constexpr size_t kRotationOff =
+    kTranslationOff + wpi::Struct<frc::Translation2d>::kSize;
+}  // namespace
+
+using StructType = wpi::Struct<frc::Transform2d>;
+
+frc::Transform2d StructType::Unpack(std::span<const uint8_t, kSize> data) {
+  return frc::Transform2d{
+      wpi::UnpackStruct<frc::Translation2d, kTranslationOff>(data),
+      wpi::UnpackStruct<frc::Rotation2d, kRotationOff>(data),
+  };
+}
+
+void StructType::Pack(std::span<uint8_t, kSize> data,
+                      const frc::Transform2d& value) {
+  wpi::PackStruct<kTranslationOff>(data, value.Translation());
+  wpi::PackStruct<kRotationOff>(data, value.Rotation());
+}
+
+void StructType::ForEachNested(
+    std::invocable<std::string_view, std::string_view> auto fn) {
+  wpi::ForEachStructSchema<frc::Translation2d>(fn);
+  wpi::ForEachStructSchema<frc::Rotation2d>(fn);
+}

wpimath/src/main/native/cpp/geometry/struct/Transform3dStruct.cpp

@@ -0,0 +1,32 @@
+// 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 "frc/geometry/struct/Transform3dStruct.h"
+
+namespace {
+constexpr size_t kTranslationOff = 0;
+constexpr size_t kRotationOff =
+    kTranslationOff + wpi::Struct<frc::Translation3d>::kSize;
+}  // namespace
+
+using StructType = wpi::Struct<frc::Transform3d>;
+
+frc::Transform3d StructType::Unpack(std::span<const uint8_t, kSize> data) {
+  return frc::Transform3d{
+      wpi::UnpackStruct<frc::Translation3d, kTranslationOff>(data),
+      wpi::UnpackStruct<frc::Rotation3d, kRotationOff>(data),
+  };
+}
+
+void StructType::Pack(std::span<uint8_t, kSize> data,
+                      const frc::Transform3d& value) {
+  wpi::PackStruct<kTranslationOff>(data, value.Translation());
+  wpi::PackStruct<kRotationOff>(data, value.Rotation());
+}
+
+void StructType::ForEachNested(
+    std::invocable<std::string_view, std::string_view> auto fn) {
+  wpi::ForEachStructSchema<frc::Translation3d>(fn);
+  wpi::ForEachStructSchema<frc::Rotation3d>(fn);
+}

wpimath/src/main/native/cpp/geometry/struct/Translation2dStruct.cpp

@@ -0,0 +1,25 @@
+// 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 "frc/geometry/struct/Translation2dStruct.h"
+
+namespace {
+constexpr size_t kXOff = 0;
+constexpr size_t kYOff = kXOff + 8;
+}  // namespace
+
+using StructType = wpi::Struct<frc::Translation2d>;
+
+frc::Translation2d StructType::Unpack(std::span<const uint8_t, kSize> data) {
+  return frc::Translation2d{
+      units::meter_t{wpi::UnpackStruct<double, kXOff>(data)},
+      units::meter_t{wpi::UnpackStruct<double, kYOff>(data)},
+  };
+}
+
+void StructType::Pack(std::span<uint8_t, kSize> data,
+                      const frc::Translation2d& value) {
+  wpi::PackStruct<kXOff>(data, value.X().value());
+  wpi::PackStruct<kYOff>(data, value.Y().value());
+}

wpimath/src/main/native/cpp/geometry/struct/Translation3dStruct.cpp

@@ -0,0 +1,28 @@
+// 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 "frc/geometry/struct/Translation3dStruct.h"
+
+namespace {
+constexpr size_t kXOff = 0;
+constexpr size_t kYOff = kXOff + 8;
+constexpr size_t kZOff = kYOff + 8;
+}  // namespace
+
+using StructType = wpi::Struct<frc::Translation3d>;
+
+frc::Translation3d StructType::Unpack(std::span<const uint8_t, kSize> data) {
+  return frc::Translation3d{
+      units::meter_t{wpi::UnpackStruct<double, kXOff>(data)},
+      units::meter_t{wpi::UnpackStruct<double, kYOff>(data)},
+      units::meter_t{wpi::UnpackStruct<double, kZOff>(data)},
+  };
+}
+
+void StructType::Pack(std::span<uint8_t, kSize> data,
+                      const frc::Translation3d& value) {
+  wpi::PackStruct<kXOff>(data, value.X().value());
+  wpi::PackStruct<kYOff>(data, value.Y().value());
+  wpi::PackStruct<kZOff>(data, value.Z().value());
+}

wpimath/src/main/native/cpp/geometry/struct/Twist2dStruct.cpp

@@ -0,0 +1,28 @@
+// 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 "frc/geometry/struct/Twist2dStruct.h"
+
+namespace {
+constexpr size_t kDxOff = 0;
+constexpr size_t kDyOff = kDxOff + 8;
+constexpr size_t kDthetaOff = kDyOff + 8;
+}  // namespace
+
+using StructType = wpi::Struct<frc::Twist2d>;
+
+frc::Twist2d StructType::Unpack(std::span<const uint8_t, kSize> data) {
+  return frc::Twist2d{
+      units::meter_t{wpi::UnpackStruct<double, kDxOff>(data)},
+      units::meter_t{wpi::UnpackStruct<double, kDyOff>(data)},
+      units::radian_t{wpi::UnpackStruct<double, kDthetaOff>(data)},
+  };
+}
+
+void StructType::Pack(std::span<uint8_t, kSize> data,
+                      const frc::Twist2d& value) {
+  wpi::PackStruct<kDxOff>(data, value.dx.value());
+  wpi::PackStruct<kDyOff>(data, value.dy.value());
+  wpi::PackStruct<kDthetaOff>(data, value.dtheta.value());
+}

wpimath/src/main/native/cpp/geometry/struct/Twist3dStruct.cpp

@@ -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.
+
+#include "frc/geometry/struct/Twist3dStruct.h"
+
+namespace {
+constexpr size_t kDxOff = 0;
+constexpr size_t kDyOff = kDxOff + 8;
+constexpr size_t kDzOff = kDyOff + 8;
+constexpr size_t kRxOff = kDzOff + 8;
+constexpr size_t kRyOff = kRxOff + 8;
+constexpr size_t kRzOff = kRyOff + 8;
+}  // namespace
+
+using StructType = wpi::Struct<frc::Twist3d>;
+
+frc::Twist3d StructType::Unpack(std::span<const uint8_t, kSize> data) {
+  return frc::Twist3d{
+      units::meter_t{wpi::UnpackStruct<double, kDxOff>(data)},
+      units::meter_t{wpi::UnpackStruct<double, kDyOff>(data)},
+      units::meter_t{wpi::UnpackStruct<double, kDzOff>(data)},
+      units::radian_t{wpi::UnpackStruct<double, kRxOff>(data)},
+      units::radian_t{wpi::UnpackStruct<double, kRyOff>(data)},
+      units::radian_t{wpi::UnpackStruct<double, kRzOff>(data)},
+  };
+}
+
+void StructType::Pack(std::span<uint8_t, kSize> data,
+                      const frc::Twist3d& value) {
+  wpi::PackStruct<kDxOff>(data, value.dx.value());
+  wpi::PackStruct<kDyOff>(data, value.dy.value());
+  wpi::PackStruct<kDzOff>(data, value.dz.value());
+  wpi::PackStruct<kRxOff>(data, value.rx.value());
+  wpi::PackStruct<kRyOff>(data, value.ry.value());
+  wpi::PackStruct<kRzOff>(data, value.rz.value());
+}