Use more idiomatic functional interfaces in NumericalIntegration

wpilibsuite · Dec 12, 2024 · cb741b0 · cb741b0
1 parent 5038c1a
commit cb741b0
Showing 1 changed file with 14 additions and 14 deletions.
diff --git a/wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java b/wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java
@@ -8,8 +8,9 @@
 import edu.wpi.first.math.Num;
 import edu.wpi.first.math.numbers.N1;
 import java.util.function.BiFunction;
-import java.util.function.DoubleFunction;
-import java.util.function.Function;
+import java.util.function.DoubleBinaryOperator;
+import java.util.function.DoubleUnaryOperator;
+import java.util.function.UnaryOperator;
 
 /** Numerical integration utilities. */
 public final class NumericalIntegration {
@@ -25,12 +26,12 @@ private NumericalIntegration() {
    * @param dtSeconds The time over which to integrate.
    * @return the integration of dx/dt = f(x) for dt.
    */
-  public static double rk4(DoubleFunction<Double> f, double x, double dtSeconds) {
+  public static double rk4(DoubleUnaryOperator f, double x, double dtSeconds) {
     final var h = dtSeconds;
-    final var k1 = f.apply(x);
-    final var k2 = f.apply(x + h * k1 * 0.5);
-    final var k3 = f.apply(x + h * k2 * 0.5);
-    final var k4 = f.apply(x + h * k3);
+    final var k1 = f.applyAsDouble(x);
+    final var k2 = f.applyAsDouble(x + h * k1 * 0.5);
+    final var k3 = f.applyAsDouble(x + h * k2 * 0.5);
+    final var k4 = f.applyAsDouble(x + h * k3);
 
     return x + h / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
   }
@@ -44,14 +45,13 @@ public static double rk4(DoubleFunction<Double> f, double x, double dtSeconds) {
    * @param dtSeconds The time over which to integrate.
    * @return The result of Runge Kutta integration (4th order).
    */
-  public static double rk4(
-      BiFunction<Double, Double, Double> f, double x, double u, double dtSeconds) {
+  public static double rk4(DoubleBinaryOperator f, double x, double u, double dtSeconds) {
     final var h = dtSeconds;
 
-    final var k1 = f.apply(x, u);
-    final var k2 = f.apply(x + h * k1 * 0.5, u);
-    final var k3 = f.apply(x + h * k2 * 0.5, u);
-    final var k4 = f.apply(x + h * k3, u);
+    final var k1 = f.applyAsDouble(x, u);
+    final var k2 = f.applyAsDouble(x + h * k1 * 0.5, u);
+    final var k3 = f.applyAsDouble(x + h * k2 * 0.5, u);
+    final var k4 = f.applyAsDouble(x + h * k3, u);
 
     return x + h / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
   }
@@ -92,7 +92,7 @@ public static <States extends Num, Inputs extends Num> Matrix<States, N1> rk4(
    * @return 4th order Runge-Kutta integration of dx/dt = f(x) for dt.
    */
   public static <States extends Num> Matrix<States, N1> rk4(
-      Function<Matrix<States, N1>, Matrix<States, N1>> f, Matrix<States, N1> x, double dtSeconds) {
+      UnaryOperator<Matrix<States, N1>> f, Matrix<States, N1> x, double dtSeconds) {
     final var h = dtSeconds;
 
     Matrix<States, N1> k1 = f.apply(x);