[wpimath] Rotate traveling salesman solution so input and solution ha…

…ve same initial pose (wpilibsuite#6015)

wpimath/src/main/java/edu/wpi/first/math/path/TravelingSalesman.java

@@ -8,6 +8,8 @@
 import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.optimization.SimulatedAnnealing;
+import java.util.Arrays;
+import java.util.Collections;
 import java.util.function.ToDoubleBiFunction;
 
 /**
@@ -40,7 +42,8 @@ public TravelingSalesman(ToDoubleBiFunction<Pose2d, Pose2d> cost) {
   }
 
   /**
-   * Finds the path through every pose that minimizes the cost.
+   * Finds the path through every pose that minimizes the cost. The first pose in the returned array
+   * is the first pose that was passed in.
    *
    * @param <Poses> A Num defining the length of the path and the number of poses.
    * @param poses An array of Pose2ds the path must pass through.
@@ -76,6 +79,9 @@ public <Poses extends Num> Pose2d[] solve(Pose2d[] poses, int iterations) {
       solution[i] = poses[(int) indices.get(i, 0)];
     }
 
+    // Rotate solution list until solution[0] = poses[0]
+    Collections.rotate(Arrays.asList(solution), -Arrays.asList(solution).indexOf(poses[0]));
+
     return solution;
   }
 

wpimath/src/main/native/include/frc/path/TravelingSalesman.h

@@ -44,7 +44,8 @@ class TravelingSalesman {
       : m_cost{std::move(cost)} {}
 
   /**
-   * Finds the path through every pose that minimizes the cost.
+   * Finds the path through every pose that minimizes the cost. The first pose
+   * in the returned array is the first pose that was passed in.
    *
    * This overload supports a statically-sized list of poses.
    *
@@ -81,11 +82,17 @@ class TravelingSalesman {
       solution[i] = poses[static_cast<int>(indices[i])];
     }
 
+    // Rotate solution list until solution[0] = poses[0]
+    std::rotate(solution.begin(),
+                std::find(solution.begin(), solution.end(), poses[0]),
+                solution.end());
+
     return solution;
   }
 
   /**
-   * Finds the path through every pose that minimizes the cost.
+   * Finds the path through every pose that minimizes the cost. The first pose
+   * in the returned array is the first pose that was passed in.
    *
    * This overload supports a dynamically-sized list of poses for Python to use.
    *
@@ -119,6 +126,11 @@ class TravelingSalesman {
       solution.emplace_back(poses[static_cast<int>(indices[i])]);
     }
 
+    // Rotate solution list until solution[0] = poses[0]
+    std::rotate(solution.begin(),
+                std::find(solution.begin(), solution.end(), poses[0]),
+                solution.end());
+
     return solution;
   }
 

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

@@ -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 @@ private boolean isMatchingCycle(Pose2d[] expected, Pose2d[] actual) {
     // 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;

wpimath/src/test/native/cpp/path/TravelingSalesmanTest.cpp

@@ -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) {