[wpimath] Fix potential divide-by-zero in RKDP (#5242)

If f(x, u) has no dynamics, the truncation error can be zero.

wpimath/src/test/java/edu/wpi/first/math/system/NumericalIntegrationTest.java

@@ -30,6 +30,20 @@ class NumericalIntegrationTest {
     assertEquals(Math.exp(0.1) - Math.exp(0.0), y1.get(0, 0), 1e-3);
   }
 
+  @Test
+  void testZeroRKDP() {
+    var y1 =
+        NumericalIntegration.rkdp(
+            (x, u) -> {
+              return VecBuilder.fill(0);
+            },
+            VecBuilder.fill(0),
+            VecBuilder.fill(0),
+            0.1);
+
+    assertEquals(0.0, y1.get(0, 0), 1e-3);
+  }
+
   @Test
   void testExponentialRKDP() {
     Matrix<N1, N1> y0 = VecBuilder.fill(0.0);

wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp

@@ -30,6 +30,16 @@ TEST(NumericalIntegrationTest, ExponentialWithU) {
   EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
 }
 
+// Tests that integrating dx/dt = 0 works with RKDP
+TEST(NumericalIntegrationTest, ZeroRKDP) {
+  frc::Vectord<1> y1 = frc::RKDP(
+      [](const frc::Vectord<1>& x, const frc::Vectord<1>& u) {
+        return frc::Vectord<1>::Zero();
+      },
+      frc::Vectord<1>{0.0}, frc::Vectord<1>{0.0}, 0.1_s);
+  EXPECT_NEAR(y1(0), 0.0, 1e-3);
+}
+
 // Tests that integrating dx/dt = e^x works with RKDP
 TEST(NumericalIntegrationTest, ExponentialRKDP) {
   frc::Vectord<1> y0{0.0};