[wpimath] Add methods to concatenate trajectories (#3139)

Co-authored-by: Tyler Veness <calcmogul@gmail.com>

wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryConcatenateTest.java

@@ -0,0 +1,52 @@
+// 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.
+
+package edu.wpi.first.wpilibj.trajectory;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import java.util.List;
+import org.junit.jupiter.api.Test;
+
+class TrajectoryConcatenateTest {
+  @Test
+  void testStates() {
+    var t1 =
+        TrajectoryGenerator.generateTrajectory(
+            new Pose2d(),
+            List.of(),
+            new Pose2d(1, 1, new Rotation2d()),
+            new TrajectoryConfig(2, 2));
+
+    var t2 =
+        TrajectoryGenerator.generateTrajectory(
+            new Pose2d(1, 1, new Rotation2d()),
+            List.of(),
+            new Pose2d(2, 2, Rotation2d.fromDegrees(45)),
+            new TrajectoryConfig(2, 2));
+
+    var t = t1.concatenate(t2);
+
+    double time = -1.0;
+    for (int i = 0; i < t.getStates().size(); ++i) {
+      var state = t.getStates().get(i);
+
+      // Make sure that the timestamps are strictly increasing.
+      assertTrue(state.timeSeconds > time);
+      time = state.timeSeconds;
+
+      // Ensure that the states in t are the same as those in t1 and t2.
+      if (i < t1.getStates().size()) {
+        assertEquals(state, t1.getStates().get(i));
+      } else {
+        var st = t2.getStates().get(i - t1.getStates().size() + 1);
+        st.timeSeconds += t1.getTotalTimeSeconds();
+        assertEquals(state, st);
+      }
+    }
+  }
+}