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

Also add unit tests.

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