Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Rotate traveling salesman solution so input and solution have same initial pose (#6015)

Browse Source
This commit is contained in:
Ashray._.g
2023-12-05 23:21:28 -08:00
committed by GitHub
parent 28deba20f5
commit 9d11544c18
4 changed files with 25 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
wpimath/src/test/native/cpp/path/TravelingSalesmanTest.cpp
View File

@@ -25,16 +25,10 @@ bool IsMatchingCycle(std::span<const frc::Pose2d> expected,
std::span<const frc::Pose2d> actual) {
assert(expected.size() == actual.size());
// Find first element in actual that matches expected
size_t actualStart = 0;
while (actual[actualStart] != expected[0]) {
++actualStart;
}
// Check actual has expected cycle (forward)
wpi::circular_buffer<frc::Pose2d> actualBufferForward{expected.size()};
for (size_t i = 0; i < actual.size(); ++i) {
actualBufferForward.push_back(actual[(actualStart + i) % actual.size()]);
actualBufferForward.push_back(actual[i % actual.size()]);
}
bool matchesExpectedForward = true;
for (size_t i = 0; i < expected.size(); ++i) {
@@ -44,8 +38,7 @@ bool IsMatchingCycle(std::span<const frc::Pose2d> expected,
// Check actual has expected cycle (reverse)
wpi::circular_buffer<frc::Pose2d> actualBufferReverse{expected.size()};
for (size_t i = 0; i < actual.size(); ++i) {
actualBufferReverse.push_front(
actual[(actualStart + 1 + i) % actual.size()]);
actualBufferReverse.push_front(actual[(1 + i) % actual.size()]);
}
bool matchesExpectedReverse = true;
for (size_t i = 0; i < expected.size(); ++i) {