Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-23 01:21:42 +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

10
wpimath/src/test/java/edu/wpi/first/math/path/TravelingSalesmanTest.java
View File

@@ -22,16 +22,10 @@ class TravelingSalesmanTest {
private boolean isMatchingCycle(Pose2d[] expected, Pose2d[] actual) {
assertEquals(expected.length, actual.length);
// Find first element in actual that matches expected
int actualStart = 0;
while (!actual[actualStart].equals(expected[0])) {
++actualStart;
}
// Check actual has expected cycle (forward)
var actualBufferForward = new CircularBuffer<Pose2d>(actual.length);
for (int i = 0; i < actual.length; ++i) {
actualBufferForward.addLast(actual[(actualStart + i) % actual.length]);
actualBufferForward.addLast(actual[i % actual.length]);
}
boolean matchesExpectedForward = true;
for (int i = 0; i < expected.length; ++i) {
@@ -41,7 +35,7 @@ class TravelingSalesmanTest {
// Check actual has expected cycle (reverse)
var actualBufferReverse = new CircularBuffer<Pose2d>(actual.length);
for (int i = 0; i < actual.length; ++i) {
actualBufferReverse.addFirst(actual[(actualStart + 1 + i) % actual.length]);
actualBufferReverse.addFirst(actual[(1 + i) % actual.length]);
}
boolean matchesExpectedReverse = true;