Clean up Java style (#5990)

Also make equivalent changes in C++ where applicable.

Co-authored-by: Sriman Achanta <68172138+srimanachanta@users.noreply.github.com>

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

@@ -42,8 +42,7 @@ class DARETest extends UtilityClassTest<DARE> {
                     .times((B.transpose().times(X).times(B).plus(R)).inv())
                     .times(B.transpose().times(X).times(A)))
             .plus(Q);
-    assertMatrixEqual(
-        new Matrix<States, States>(new SimpleMatrix(Y.getNumRows(), Y.getNumCols())), Y);
+    assertMatrixEqual(new Matrix<>(new SimpleMatrix(Y.getNumRows(), Y.getNumCols())), Y);
   }
 
   <States extends Num, Inputs extends Num> void assertDARESolution(
@@ -63,8 +62,7 @@ class DARETest extends UtilityClassTest<DARE> {
                     .times((B.transpose().times(X).times(B).plus(R)).inv())
                     .times(B.transpose().times(X).times(A).plus(N.transpose())))
             .plus(Q);
-    assertMatrixEqual(
-        new Matrix<States, States>(new SimpleMatrix(Y.getNumRows(), Y.getNumCols())), Y);
+    assertMatrixEqual(new Matrix<>(new SimpleMatrix(Y.getNumRows(), Y.getNumCols())), Y);
   }
 
   @Test

wpimath/src/test/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforwardTest.java

@@ -18,8 +18,7 @@ class ControlAffinePlantInversionFeedforwardTest {
   @Test
   void testCalculate() {
     ControlAffinePlantInversionFeedforward<N2, N1> feedforward =
-        new ControlAffinePlantInversionFeedforward<N2, N1>(
-            Nat.N2(), Nat.N1(), this::getDynamics, 0.02);
+        new ControlAffinePlantInversionFeedforward<>(Nat.N2(), Nat.N1(), this::getDynamics, 0.02);
 
     assertEquals(
         48.0, feedforward.calculate(VecBuilder.fill(2, 2), VecBuilder.fill(3, 3)).get(0, 0), 1e-6);
@@ -28,8 +27,7 @@ class ControlAffinePlantInversionFeedforwardTest {
   @Test
   void testCalculateState() {
     ControlAffinePlantInversionFeedforward<N2, N1> feedforward =
-        new ControlAffinePlantInversionFeedforward<N2, N1>(
-            Nat.N2(), Nat.N1(), this::getDynamics, 0.02);
+        new ControlAffinePlantInversionFeedforward<>(Nat.N2(), Nat.N1(), this::getDynamics, 0.02);
 
     assertEquals(
         48.0, feedforward.calculate(VecBuilder.fill(2, 2), VecBuilder.fill(3, 3)).get(0, 0), 1e-6);

wpimath/src/test/java/edu/wpi/first/math/controller/ImplicitModelFollowerTest.java

@@ -21,7 +21,7 @@ class ImplicitModelFollowerTest {
 
     var plant = LinearSystemId.identifyDrivetrainSystem(1.0, 1.0, 1.0, 1.0);
 
-    var imf = new ImplicitModelFollower<N2, N2, N2>(plant, plant);
+    var imf = new ImplicitModelFollower<>(plant, plant);
 
     Matrix<N2, N1> x = VecBuilder.fill(0.0, 0.0);
     Matrix<N2, N1> xImf = VecBuilder.fill(0.0, 0.0);
@@ -66,7 +66,7 @@ class ImplicitModelFollowerTest {
     // Linear acceleration is slower, but angular acceleration is the same
     var plantRef = LinearSystemId.identifyDrivetrainSystem(1.0, 2.0, 1.0, 1.0);
 
-    var imf = new ImplicitModelFollower<N2, N2, N2>(plant, plantRef);
+    var imf = new ImplicitModelFollower<>(plant, plantRef);
 
     Matrix<N2, N1> x = VecBuilder.fill(0.0, 0.0);
     Matrix<N2, N1> xImf = VecBuilder.fill(0.0, 0.0);

wpimath/src/test/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforwardTest.java

@@ -21,7 +21,7 @@ class LinearPlantInversionFeedforwardTest {
     Matrix<N2, N1> B = VecBuilder.fill(0, 1);
 
     LinearPlantInversionFeedforward<N2, N1, N1> feedforward =
-        new LinearPlantInversionFeedforward<N2, N1, N1>(A, B, 0.02);
+        new LinearPlantInversionFeedforward<>(A, B, 0.02);
 
     assertEquals(
         47.502599,

wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java

@@ -5,6 +5,7 @@
 package edu.wpi.first.math.estimator;
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
 
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -254,7 +255,7 @@ class DifferentialDrivePoseEstimatorTest {
           "Estimator converged to one vision measurement: "
               + estimator.getEstimatedPosition().toString()
               + " -> "
-              + measurement.toString();
+              + measurement;
 
       var dx = Math.abs(measurement.getX() - estimator.getEstimatedPosition().getX());
       var dy = Math.abs(measurement.getY() - estimator.getEstimatedPosition().getY());
@@ -263,7 +264,7 @@ class DifferentialDrivePoseEstimatorTest {
               measurement.getRotation().getDegrees()
                   - estimator.getEstimatedPosition().getRotation().getDegrees());
 
-      assertEquals(dx > 0.08 || dy > 0.08 || dtheta > 0.08, true, errorLog);
+      assertTrue(dx > 0.08 || dy > 0.08 || dtheta > 0.08, errorLog);
     }
   }
 

wpimath/src/test/java/edu/wpi/first/math/estimator/ExtendedKalmanFilterTest.java

@@ -22,7 +22,6 @@ import edu.wpi.first.math.system.NumericalJacobian;
 import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.trajectory.TrajectoryConfig;
 import edu.wpi.first.math.trajectory.TrajectoryGenerator;
-import java.util.Arrays;
 import java.util.List;
 import org.junit.jupiter.api.Test;
 
@@ -114,7 +113,7 @@ class ExtendedKalmanFilterTest {
             dtSeconds);
 
     List<Pose2d> waypoints =
-        Arrays.asList(
+        List.of(
             new Pose2d(2.75, 22.521, new Rotation2d()),
             new Pose2d(24.73, 19.68, Rotation2d.fromDegrees(5.846)));
     var trajectory =

wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java

@@ -5,6 +5,7 @@
 package edu.wpi.first.math.estimator;
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
 
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -267,7 +268,7 @@ class MecanumDrivePoseEstimatorTest {
           "Estimator converged to one vision measurement: "
               + estimator.getEstimatedPosition().toString()
               + " -> "
-              + measurement.toString();
+              + measurement;
 
       var dx = Math.abs(measurement.getX() - estimator.getEstimatedPosition().getX());
       var dy = Math.abs(measurement.getY() - estimator.getEstimatedPosition().getY());
@@ -276,7 +277,7 @@ class MecanumDrivePoseEstimatorTest {
               measurement.getRotation().getDegrees()
                   - estimator.getEstimatedPosition().getRotation().getDegrees());
 
-      assertEquals(dx > 0.08 || dy > 0.08 || dtheta > 0.08, true, errorLog);
+      assertTrue(dx > 0.08 || dy > 0.08 || dtheta > 0.08, errorLog);
     }
   }
 

wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java

@@ -5,6 +5,7 @@
 package edu.wpi.first.math.estimator;
 
 import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
 
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -288,7 +289,7 @@ class SwerveDrivePoseEstimatorTest {
           "Estimator converged to one vision measurement: "
               + estimator.getEstimatedPosition().toString()
               + " -> "
-              + measurement.toString();
+              + measurement;
 
       var dx = Math.abs(measurement.getX() - estimator.getEstimatedPosition().getX());
       var dy = Math.abs(measurement.getY() - estimator.getEstimatedPosition().getY());
@@ -297,7 +298,7 @@ class SwerveDrivePoseEstimatorTest {
               measurement.getRotation().getDegrees()
                   - estimator.getEstimatedPosition().getRotation().getDegrees());
 
-      assertEquals(dx > 0.08 || dy > 0.08 || dtheta > 0.08, true, errorLog);
+      assertTrue(dx > 0.08 || dy > 0.08 || dtheta > 0.08, errorLog);
     }
   }
 

wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java

@@ -26,7 +26,6 @@ import edu.wpi.first.math.system.plant.DCMotor;
 import edu.wpi.first.math.system.plant.LinearSystemId;
 import edu.wpi.first.math.trajectory.TrajectoryConfig;
 import edu.wpi.first.math.trajectory.TrajectoryGenerator;
-import java.util.Arrays;
 import java.util.List;
 import org.junit.jupiter.api.Test;
 
@@ -137,7 +136,7 @@ class UnscentedKalmanFilterTest {
             dtSeconds);
 
     List<Pose2d> waypoints =
-        Arrays.asList(
+        List.of(
             new Pose2d(2.75, 22.521, new Rotation2d()),
             new Pose2d(24.73, 19.68, Rotation2d.fromDegrees(5.846)));
     var trajectory =

wpimath/src/test/java/edu/wpi/first/math/filter/LinearFilterTest.java

@@ -129,9 +129,9 @@ class LinearFilterTest {
         1,
         3,
         // f(x) = sin(x)
-        (double x) -> Math.sin(x),
+        Math::sin,
         // df/dx = cos(x)
-        (double x) -> Math.cos(x),
+        Math::cos,
         h,
         -20.0,
         20.0);
@@ -140,7 +140,7 @@ class LinearFilterTest {
         1,
         3,
         // f(x) = ln(x)
-        (double x) -> Math.log(x),
+        Math::log,
         // df/dx = 1/x
         (double x) -> 1.0 / x,
         h,
@@ -162,7 +162,7 @@ class LinearFilterTest {
         2,
         5,
         // f(x) = sin(x)
-        (double x) -> Math.sin(x),
+        Math::sin,
         // d²f/dx² = -sin(x)
         (double x) -> -Math.sin(x),
         h,
@@ -173,7 +173,7 @@ class LinearFilterTest {
         2,
         5,
         // f(x) = ln(x)
-        (double x) -> Math.log(x),
+        Math::log,
         // d²f/dx² = -1/x²
         (double x) -> -1.0 / (x * x),
         h,
@@ -201,9 +201,9 @@ class LinearFilterTest {
         1,
         2,
         // f(x) = sin(x)
-        (double x) -> Math.sin(x),
+        Math::sin,
         // df/dx = cos(x)
-        (double x) -> Math.cos(x),
+        Math::cos,
         h,
         -20.0,
         20.0);
@@ -212,7 +212,7 @@ class LinearFilterTest {
         1,
         2,
         // f(x) = ln(x)
-        (double x) -> Math.log(x),
+        Math::log,
         // df/dx = 1/x
         (double x) -> 1.0 / x,
         h,
@@ -234,7 +234,7 @@ class LinearFilterTest {
         2,
         4,
         // f(x) = sin(x)
-        (double x) -> Math.sin(x),
+        Math::sin,
         // d²f/dx² = -sin(x)
         (double x) -> -Math.sin(x),
         h,
@@ -245,7 +245,7 @@ class LinearFilterTest {
         2,
         4,
         // f(x) = ln(x)
-        (double x) -> Math.log(x),
+        Math::log,
         // d²f/dx² = -1/x²
         (double x) -> -1.0 / (x * x),
         h,

wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java

@@ -11,7 +11,7 @@ import static org.junit.jupiter.api.Assertions.assertNotEquals;
 
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.util.Units;
-import java.util.Arrays;
+import java.util.List;
 import org.junit.jupiter.api.Test;
 
 class Pose3dTest {
@@ -194,7 +194,7 @@ class Pose3dTest {
   @Test
   void testComplexTwists() {
     var initial_poses =
-        Arrays.asList(
+        List.of(
             new Pose3d(
                 new Translation3d(0.698303, -0.959096, 0.271076),
                 new Rotation3d(new Quaternion(0.86403, -0.076866, 0.147234, 0.475254))),
@@ -212,7 +212,7 @@ class Pose3dTest {
                 new Rotation3d(new Quaternion(0.807886, 0.029298, 0.257788, 0.529157))));
 
     var final_poses =
-        Arrays.asList(
+        List.of(
             new Pose3d(
                 new Translation3d(-0.230448, -0.511957, 0.198406),
                 new Rotation3d(new Quaternion(0.753984, 0.347016, 0.409105, 0.379106))),
@@ -267,7 +267,7 @@ class Pose3dTest {
   @Test
   void testTwistNaN() {
     var initial_poses =
-        Arrays.asList(
+        List.of(
             new Pose3d(
                 new Translation3d(6.32, 4.12, 0.00),
                 new Rotation3d(
@@ -277,7 +277,7 @@ class Pose3dTest {
                 new Rotation3d(
                     new Quaternion(0.9999999999999793, 0.0, 0.0, 2.0352360299846772E-7))));
     var final_poses =
-        Arrays.asList(
+        List.of(
             new Pose3d(
                 new Translation3d(6.33, 4.15, 0.00),
                 new Rotation3d(

wpimath/src/test/java/edu/wpi/first/math/optimization/SimulatedAnnealingTest.java

@@ -21,7 +21,7 @@ class SimulatedAnnealingTest {
         new SimulatedAnnealing<Double>(
             2.0,
             x -> MathUtil.clamp(x + (Math.random() - 0.5) * stepSize, -3, 3),
-            x -> function.applyAsDouble(x));
+            function::applyAsDouble);
 
     double solution = simulatedAnnealing.solve(-1.0, 5000);
 
@@ -38,7 +38,7 @@ class SimulatedAnnealingTest {
         new SimulatedAnnealing<Double>(
             2.0,
             x -> MathUtil.clamp(x + (Math.random() - 0.5) * stepSize, 0, 7),
-            x -> function.applyAsDouble(x));
+            function::applyAsDouble);
 
     double solution = simulatedAnnealing.solve(-1.0, 5000);
 

wpimath/src/test/java/edu/wpi/first/math/spline/CubicHermiteSplineTest.java

@@ -14,7 +14,6 @@ import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.spline.SplineParameterizer.MalformedSplineException;
 import java.util.ArrayList;
-import java.util.Arrays;
 import java.util.List;
 import org.junit.jupiter.api.Test;
 
@@ -151,7 +150,7 @@ class CubicHermiteSplineTest {
         () ->
             run(
                 new Pose2d(10, 10, Rotation2d.fromDegrees(90)),
-                Arrays.asList(new Translation2d(10, 10.5)),
+                List.of(new Translation2d(10, 10.5)),
                 new Pose2d(10, 11, Rotation2d.fromDegrees(-90))));
   }
 }

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

@@ -34,12 +34,7 @@ class NumericalIntegrationTest {
   void testZeroRKDP() {
     var y1 =
         NumericalIntegration.rkdp(
-            (x, u) -> {
-              return VecBuilder.fill(0);
-            },
-            VecBuilder.fill(0),
-            VecBuilder.fill(0),
-            0.1);
+            (x, u) -> VecBuilder.fill(0), VecBuilder.fill(0), VecBuilder.fill(0), 0.1);
 
     assertEquals(0.0, y1.get(0, 0), 1e-3);
   }

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

@@ -31,10 +31,9 @@ class RungeKuttaTimeVaryingTest {
 
     final var y1 =
         RungeKuttaTimeVarying.rungeKuttaTimeVarying(
-            (Double t, Matrix<N1, N1> x) -> {
-              return MatBuilder.fill(
-                  Nat.N1(), Nat.N1(), x.get(0, 0) * (2.0 / (Math.exp(t) + 1.0) - 1.0));
-            },
+            (Double t, Matrix<N1, N1> x) ->
+                MatBuilder.fill(
+                    Nat.N1(), Nat.N1(), x.get(0, 0) * (2.0 / (Math.exp(t) + 1.0) - 1.0)),
             5.0,
             y0,
             1.0);

wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java

@@ -11,7 +11,6 @@ import static org.junit.jupiter.api.Assertions.assertTrue;
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
 import edu.wpi.first.math.trajectory.constraint.DifferentialDriveVoltageConstraint;
@@ -89,7 +88,7 @@ class DifferentialDriveVoltageConstraintTest {
         () ->
             TrajectoryGenerator.generateTrajectory(
                 new Pose2d(1, 0, Rotation2d.fromDegrees(90)),
-                new ArrayList<Translation2d>(),
+                new ArrayList<>(),
                 new Pose2d(0, 1, Rotation2d.fromDegrees(180)),
                 config));
 
@@ -97,7 +96,7 @@ class DifferentialDriveVoltageConstraintTest {
         () ->
             TrajectoryGenerator.generateTrajectory(
                 new Pose2d(0, 1, Rotation2d.fromDegrees(180)),
-                new ArrayList<Translation2d>(),
+                new ArrayList<>(),
                 new Pose2d(1, 0, Rotation2d.fromDegrees(90)),
                 config.setReversed(true)));
   }

wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryGeneratorTest.java

@@ -16,7 +16,6 @@ import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint;
 import java.util.ArrayList;
-import java.util.Arrays;
 import java.util.List;
 import org.junit.jupiter.api.Test;
 
@@ -73,7 +72,7 @@ class TrajectoryGeneratorTest {
   void testMalformedTrajectory() {
     var traj =
         TrajectoryGenerator.generateTrajectory(
-            Arrays.asList(
+            List.of(
                 new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
                 new Pose2d(1, 0, Rotation2d.fromDegrees(180))),
             new TrajectoryConfig(feetToMeters(12), feetToMeters(12)));
@@ -86,7 +85,7 @@ class TrajectoryGeneratorTest {
   void testQuinticCurvatureOptimization() {
     Trajectory t =
         TrajectoryGenerator.generateTrajectory(
-            Arrays.asList(
+            List.of(
                 new Pose2d(1, 0, Rotation2d.fromDegrees(90)),
                 new Pose2d(0, 1, Rotation2d.fromDegrees(180)),
                 new Pose2d(-1, 0, Rotation2d.fromDegrees(270)),