diff --git a/wpimath/src/main/native/include/frc/struct/MatrixStruct.h b/wpimath/src/main/native/include/frc/struct/MatrixStruct.h new file mode 100644 index 00000000000..cd655e1cfb3 --- /dev/null +++ b/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 +#include +#include + +#include "frc/EigenCore.h" + +template + requires(Cols != 1) +struct wpi::Struct> { + static constexpr ct_string kTypeString = + wpi::Concat("struct:Matrix__"_ct_string, wpi::NumToCtString(), + "_"_ct_string, wpi::NumToCtString()); + 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(), + "]"_ct_string); + static constexpr std::string_view GetSchema() { return kSchema; } + + static frc::Matrixd Unpack( + std::span data); + static void Pack( + std::span data, + const frc::Matrixd& value); +}; + +static_assert(wpi::StructSerializable>); +static_assert(wpi::StructSerializable>); + +#include "frc/struct/MatrixStruct.inc" diff --git a/wpimath/src/main/native/include/frc/struct/MatrixStruct.inc b/wpimath/src/main/native/include/frc/struct/MatrixStruct.inc new file mode 100644 index 00000000000..2c8e23c2300 --- /dev/null +++ b/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 + requires(Cols != 1) +frc::Matrixd +wpi::Struct>::Unpack( + std::span data) { + wpi::array mat_data = + wpi::UnpackStructArray(data); + frc::Matrixd mat; + for (int i = 0; i < Rows * Cols; i++) { + mat(i) = mat_data[i]; + } + return mat; +} + +template + requires(Cols != 1) +void wpi::Struct>::Pack( + std::span data, + const frc::Matrixd& value) { + wpi::array mat_data(wpi::empty_array); + for (int i = 0; i < Rows * Cols; i++) { + mat_data[i] = value(i); + } + wpi::PackStructArray(data, mat_data); +} diff --git a/wpimath/src/main/native/include/frc/struct/VectorStruct.h b/wpimath/src/main/native/include/frc/struct/VectorStruct.h new file mode 100644 index 00000000000..65c3000d237 --- /dev/null +++ b/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 +#include +#include + +#include "frc/EigenCore.h" + +template +struct wpi::Struct> { + static constexpr ct_string kTypeString = + wpi::Concat("struct:Vector__"_ct_string, wpi::NumToCtString()); + 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(), + "]"_ct_string); + static constexpr std::string_view GetSchema() { return kSchema; } + + static frc::Matrixd Unpack( + std::span data); + static void Pack( + std::span data, + const frc::Matrixd& value); +}; + +static_assert(wpi::StructSerializable>); +static_assert(wpi::StructSerializable>); + +#include "frc/struct/VectorStruct.inc" diff --git a/wpimath/src/main/native/include/frc/struct/VectorStruct.inc b/wpimath/src/main/native/include/frc/struct/VectorStruct.inc new file mode 100644 index 00000000000..ee2a81893f5 --- /dev/null +++ b/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 +frc::Matrixd +wpi::Struct>::Unpack( + std::span data) { + wpi::array vec_data = + wpi::UnpackStructArray(data); + frc::Matrixd vec; + for (int i = 0; i < Size; i++) { + vec(i) = vec_data[i]; + } + return vec; +} + +template +void wpi::Struct>::Pack( + std::span data, + const frc::Matrixd& value) { + wpi::array vec_data(wpi::empty_array); + for (int i = 0; i < Size; i++) { + vec_data[i] = value(i); + } + wpi::PackStructArray(data, vec_data); +} diff --git a/wpimath/src/test/native/cpp/struct/MatrixStructTest.cpp b/wpimath/src/test/native/cpp/struct/MatrixStructTest.cpp new file mode 100644 index 00000000000..5445b3b0d1b --- /dev/null +++ b/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 + +#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); diff --git a/wpimath/src/test/native/cpp/struct/VectorStructTest.cpp b/wpimath/src/test/native/cpp/struct/VectorStructTest.cpp new file mode 100644 index 00000000000..cae75d8a560 --- /dev/null +++ b/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 + +#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);