[wpimath] Add Translation2d/Translation3d slew rate limiter (#7806)

Co-authored-by: Tyler Veness <calcmogul@gmail.com>
Co-authored-by: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com>

wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java

@@ -8,6 +8,8 @@ import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.wpilibj.UtilityClassTest;
 import org.junit.jupiter.api.Test;
 
@@ -167,4 +169,55 @@ class MathUtilTest extends UtilityClassTest<MathUtil> {
     assertFalse(MathUtil.isNear(400, -315, 5, 0, 360));
     assertFalse(MathUtil.isNear(400, 395, 5, 0, 360));
   }
+
+  @Test
+  void testSlewRateTranslation2dUnchanged() {
+    Translation2d translation1 = new Translation2d(0, 0);
+    Translation2d translation2 = new Translation2d(2, 2);
+
+    Translation2d result1 = MathUtil.slewRateLimit(translation1, translation2, 1, 50);
+
+    Translation2d expected1 = new Translation2d(2, 2);
+
+    assertEquals(expected1, result1);
+  }
+
+  @Test
+  void testSlewRateTranslation2dChanged() {
+    Translation2d translation3 = new Translation2d(1, 1);
+    Translation2d translation4 = new Translation2d(3, 3);
+
+    Translation2d result2 = MathUtil.slewRateLimit(translation3, translation4, 0.25, 2);
+
+    Translation2d expected2 =
+        new Translation2d(1.0 + 1.0 / Math.sqrt(8.0), 1.0 + 1.0 / Math.sqrt(8.0));
+
+    assertEquals(expected2, result2);
+  }
+
+  @Test
+  void testSlewRateTranslation3dUnchanged() {
+    Translation3d translation1 = new Translation3d(0, 0, 0);
+    Translation3d translation2 = new Translation3d(2, 2, 2);
+
+    Translation3d result1 = MathUtil.slewRateLimit(translation1, translation2, 1, 50);
+
+    Translation3d expected1 = new Translation3d(2, 2, 2);
+
+    assertEquals(expected1, result1);
+  }
+
+  @Test
+  void testSlewRateTranslation3dChanged() {
+    Translation3d translation3 = new Translation3d(1, 1, 1);
+    Translation3d translation4 = new Translation3d(3, 3, 3);
+
+    Translation3d result2 = MathUtil.slewRateLimit(translation3, translation4, 0.25, 2);
+
+    Translation3d expected2 =
+        new Translation3d(
+            1.0 + 1.0 / Math.sqrt(12.0), 1.0 + 1.0 / Math.sqrt(12.0), 1.0 + 1.0 / Math.sqrt(12.0));
+
+    assertEquals(expected2, result2);
+  }
 }

wpimath/src/test/native/cpp/MathUtilTest.cpp

@@ -3,11 +3,17 @@
 // the WPILib BSD license file in the root directory of this project.
 
 #include <limits>
+#include <numbers>
 
 #include <gtest/gtest.h>
 
 #include "frc/MathUtil.h"
+#include "frc/geometry/Translation2d.h"
+#include "frc/geometry/Translation3d.h"
 #include "units/angle.h"
+#include "units/length.h"
+#include "units/time.h"
+#include "units/velocity.h"
 
 #define EXPECT_UNITS_EQ(a, b) EXPECT_DOUBLE_EQ((a).value(), (b).value())
 
@@ -164,3 +170,56 @@ TEST(MathUtilTest, IsNear) {
   EXPECT_FALSE(frc::IsNear(400, 395, 5, 0, 360));
   EXPECT_FALSE(frc::IsNear(0_deg, -4_deg, 2.5_deg, 0_deg, 360_deg));
 }
+
+TEST(MathUtilTest, Translation2dSlewRateLimitUnchanged) {
+  const frc::Translation2d translation1{0_m, 0_m};
+  const frc::Translation2d translation2{2_m, 2_m};
+
+  const frc::Translation2d result1 =
+      frc::SlewRateLimit(translation1, translation2, 1_s, 50_mps);
+
+  const frc::Translation2d expected1{2_m, 2_m};
+
+  EXPECT_EQ(result1, expected1);
+}
+
+TEST(MathUtilTest, Translation2dSlewRateLimitChanged) {
+  const frc::Translation2d translation3{1_m, 1_m};
+  const frc::Translation2d translation4{3_m, 3_m};
+
+  const frc::Translation2d result2 =
+      frc::SlewRateLimit(translation3, translation4, 0.25_s, 2_mps);
+
+  const frc::Translation2d expected2{
+      units::meter_t{1.0 + 0.5 * (std::numbers::sqrt2 / 2)},
+      units::meter_t{1.0 + 0.5 * (std::numbers::sqrt2 / 2)}};
+
+  EXPECT_EQ(result2, expected2);
+}
+
+TEST(MathUtilTest, Translation3dSlewRateLimitUnchanged) {
+  const frc::Translation3d translation1{0_m, 0_m, 0_m};
+  const frc::Translation3d translation2{2_m, 2_m, 2_m};
+
+  const frc::Translation3d result1 =
+      frc::SlewRateLimit(translation1, translation2, 1_s, 50.0_mps);
+
+  const frc::Translation3d expected1{2_m, 2_m, 2_m};
+
+  EXPECT_EQ(result1, expected1);
+}
+
+TEST(MathUtilTest, Translation3dSlewRateLimitChanged) {
+  const frc::Translation3d translation3{1_m, 1_m, 1_m};
+  const frc::Translation3d translation4{3_m, 3_m, 3_m};
+
+  const frc::Translation3d result2 =
+      frc::SlewRateLimit(translation3, translation4, 0.25_s, 2.0_mps);
+
+  const frc::Translation3d expected2{
+      units::meter_t{1.0 + 0.5 * std::numbers::inv_sqrt3},
+      units::meter_t{1.0 + 0.5 * std::numbers::inv_sqrt3},
+      units::meter_t{1.0 + 0.5 * std::numbers::inv_sqrt3}};
+
+  EXPECT_EQ(result2, expected2);
+}