diff --git a/CMakeLists.txt b/CMakeLists.txt
index bd75892648..6670ec04f9 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -287,6 +287,10 @@ set(WPIUNITS_DEP_REPLACE_IMPL "find_dependency(wpiunits)")
 set(WPIANNOTATIONS_DEP_REPLACE_IMPL "find_dependency(wpiannotations)")
 set(WPIUTIL_DEP_REPLACE "find_dependency(wpiutil)")
 set(DATALOG_DEP_REPLACE "find_dependency(datalog)")
+if(WITH_JAVA)
+    add_subdirectory(wpiannotations)
+endif()
+
 add_subdirectory(wpiutil)
 
 add_subdirectory(datalog)
@@ -307,10 +311,6 @@ if(WITH_WPIMATH)
     add_subdirectory(wpimath)
 endif()
 
-if(WITH_JAVA)
-    add_subdirectory(wpiannotations)
-endif()
-
 if(WITH_WPIUNITS AND NOT WITH_WPIMATH)
     # In case of building wpiunits standalone
     set(WPIUNITS_DEP_REPLACE ${WPIUNITS_DEP_REPLACE_IMPL})
diff --git a/robotpyExamples/SwerveBot/drivetrain.py b/robotpyExamples/SwerveBot/drivetrain.py
index 2121100bb1..0a6d463c00 100644
--- a/robotpyExamples/SwerveBot/drivetrain.py
+++ b/robotpyExamples/SwerveBot/drivetrain.py
@@ -67,22 +67,23 @@ class Drivetrain:
         :param fieldRelative: Whether the provided x and y velocities are relative to the field.
         :param periodSeconds: Time
         """
-        robot_velocities = wpimath.ChassisVelocities(xVelocity, yVelocity, rot)
+        chassisVelocities = wpimath.ChassisVelocities(xVelocity, yVelocity, rot)
         if fieldRelative:
-            robot_velocities = robot_velocities.toRobotRelative(
+            chassisVelocities = chassisVelocities.toRobotRelative(
                 self.imu.getRotation2d()
             )
 
-        swerveModuleStates = self.kinematics.toSwerveModuleVelocities(
-            wpimath.ChassisVelocities.discretize(robot_velocities, periodSeconds)
+        chassisVelocities = chassisVelocities.discretize(periodSeconds)
+
+        velocities = wpimath.SwerveDrive4Kinematics.desaturateWheelVelocities(
+            self.kinematics.toSwerveModuleVelocities(chassisVelocities),
+            kMaxVelocity,
         )
-        wpimath.SwerveDrive4Kinematics.desaturateWheelVelocities(
-            swerveModuleStates, kMaxVelocity
-        )
-        self.frontLeft.setDesiredVelocity(swerveModuleStates[0])
-        self.frontRight.setDesiredVelocity(swerveModuleStates[1])
-        self.backLeft.setDesiredVelocity(swerveModuleStates[2])
-        self.backRight.setDesiredVelocity(swerveModuleStates[3])
+
+        self.frontLeft.setDesiredVelocity(velocities[0])
+        self.frontRight.setDesiredVelocity(velocities[1])
+        self.backLeft.setDesiredVelocity(velocities[2])
+        self.backRight.setDesiredVelocity(velocities[3])
 
     def updateOdometry(self) -> None:
         """Updates the field relative position of the robot."""
diff --git a/robotpyExamples/SwerveBot/swervemodule.py b/robotpyExamples/SwerveBot/swervemodule.py
index a890f8549c..68060b3472 100644
--- a/robotpyExamples/SwerveBot/swervemodule.py
+++ b/robotpyExamples/SwerveBot/swervemodule.py
@@ -104,24 +104,24 @@ class SwerveModule:
 
         encoderRotation = wpimath.Rotation2d(self.turningEncoder.getDistance())
 
-        # Optimize the reference state to avoid spinning further than 90 degrees
-        desiredVelocity.optimize(encoderRotation)
-
-        # Scale velocity by cosine of angle error. This scales down movement perpendicular to the desired
-        # direction of travel that can occur when modules change directions. This results in smoother
-        # driving.
-        desiredVelocity.cosineScale(encoderRotation)
+        # Optimize the desired velocity to avoid spinning further than 90 degrees, then scale
+        # velocity by cosine of angle error. This scales down movement perpendicular to the desired
+        # direction of travel that can occur when modules change directions. This results in
+        # smoother driving.
+        velocity = desiredVelocity.optimize(encoderRotation).cosineScale(
+            encoderRotation
+        )
 
         # Calculate the drive output from the drive PID controller.
         driveOutput = self.drivePIDController.calculate(
-            self.driveEncoder.getRate(), desiredVelocity.velocity
+            self.driveEncoder.getRate(), velocity.velocity
         )
 
-        driveFeedforward = self.driveFeedforward.calculate(desiredVelocity.velocity)
+        driveFeedforward = self.driveFeedforward.calculate(velocity.velocity)
 
         # Calculate the turning motor output from the turning PID controller.
         turnOutput = self.turningPIDController.calculate(
-            self.turningEncoder.getDistance(), desiredVelocity.angle.radians()
+            self.turningEncoder.getDistance(), velocity.angle.radians()
         )
 
         turnFeedforward = self.turnFeedforward.calculate(
diff --git a/robotpyExamples/SwerveDrivePoseEstimator/drivetrain.py b/robotpyExamples/SwerveDrivePoseEstimator/drivetrain.py
index e211978831..f43634d4f2 100644
--- a/robotpyExamples/SwerveDrivePoseEstimator/drivetrain.py
+++ b/robotpyExamples/SwerveDrivePoseEstimator/drivetrain.py
@@ -81,15 +81,15 @@ class Drivetrain:
 
         chassisVelocities = chassisVelocities.discretize(period)
 
-        states = self.kinematics.toSwerveModuleVelocities(chassisVelocities)
-        wpimath.SwerveDrive4Kinematics.desaturateWheelVelocities(
-            states, self.kMaxVelocity
+        velocities = wpimath.SwerveDrive4Kinematics.desaturateWheelVelocities(
+            self.kinematics.toSwerveModuleVelocities(chassisVelocities),
+            self.kMaxVelocity,
         )
 
-        self.frontLeft.setDesiredVelocity(states[0])
-        self.frontRight.setDesiredVelocity(states[1])
-        self.backLeft.setDesiredVelocity(states[2])
-        self.backRight.setDesiredVelocity(states[3])
+        self.frontLeft.setDesiredVelocity(velocities[0])
+        self.frontRight.setDesiredVelocity(velocities[1])
+        self.backLeft.setDesiredVelocity(velocities[2])
+        self.backRight.setDesiredVelocity(velocities[3])
 
     def updateOdometry(self) -> None:
         """Updates the field relative position of the robot."""
diff --git a/robotpyExamples/SwerveDrivePoseEstimator/swervemodule.py b/robotpyExamples/SwerveDrivePoseEstimator/swervemodule.py
index 828cbd4875..5f0aa9e210 100644
--- a/robotpyExamples/SwerveDrivePoseEstimator/swervemodule.py
+++ b/robotpyExamples/SwerveDrivePoseEstimator/swervemodule.py
@@ -104,24 +104,24 @@ class SwerveModule:
         """
         encoderRotation = wpimath.Rotation2d(self.turningEncoder.getDistance())
 
-        # Optimize the reference state to avoid spinning further than 90 degrees
-        desiredVelocity.optimize(encoderRotation)
-
-        # Scale velocity by cosine of angle error. This scales down movement perpendicular to the
-        # desired direction of travel that can occur when modules change directions. This results
-        # in smoother driving.
-        desiredVelocity.cosineScale(encoderRotation)
+        # Optimize the desired velocity to avoid spinning further than 90 degrees, then scale
+        # velocity by cosine of angle error. This scales down movement perpendicular to the desired
+        # direction of travel that can occur when modules change directions. This results in
+        # smoother driving.
+        velocity = desiredVelocity.optimize(encoderRotation).cosineScale(
+            encoderRotation
+        )
 
         # Calculate the drive output from the drive PID controller.
         driveOutput = self.drivePIDController.calculate(
-            self.driveEncoder.getRate(), desiredVelocity.velocity
+            self.driveEncoder.getRate(), velocity.velocity
         )
 
-        driveFeedforward = self.driveFeedforward.calculate(desiredVelocity.velocity)
+        driveFeedforward = self.driveFeedforward.calculate(velocity.velocity)
 
         # Calculate the turning motor output from the turning PID controller.
         turnOutput = self.turningPIDController.calculate(
-            self.turningEncoder.getDistance(), desiredVelocity.angle.radians()
+            self.turningEncoder.getDistance(), velocity.angle.radians()
         )
 
         turnFeedforward = self.turnFeedforward.calculate(
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp
index 4fc55b7509..150c4e86e5 100644
--- a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp
@@ -15,10 +15,8 @@ void Drivetrain::Drive(wpi::units::meters_per_second_t xVelocity,
   }
   chassisVelocities = chassisVelocities.Discretize(period);
 
-  auto states = m_kinematics.ToSwerveModuleVelocities(chassisVelocities);
-  m_kinematics.DesaturateWheelVelocities(&states, kMaxVelocity);
-
-  auto [fl, fr, bl, br] = states;
+  auto [fl, fr, bl, br] = m_kinematics.DesaturateWheelVelocities(
+      m_kinematics.ToSwerveModuleVelocities(chassisVelocities), kMaxVelocity);
   m_frontLeft.SetDesiredVelocity(fl);
   m_frontRight.SetDesiredVelocity(fr);
   m_backLeft.SetDesiredVelocity(bl);
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp
index 26c917c73e..319b407096 100644
--- a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp
@@ -52,25 +52,23 @@ void SwerveModule::SetDesiredVelocity(
   wpi::math::Rotation2d encoderRotation{
       wpi::units::radian_t{m_turningEncoder.GetDistance()}};
 
-  // Optimize the desired velocity to avoid spinning further than 90 degrees
-  desiredVelocity.Optimize(encoderRotation);
-
-  // Scale velocity by cosine of angle error. This scales down movement
+  // Optimize the desired velocity to avoid spinning further than 90 degrees,
+  // then scale velocity by cosine of angle error. This scales down movement
   // perpendicular to the desired direction of travel that can occur when
   // modules change directions. This results in smoother driving.
-  desiredVelocity.CosineScale(encoderRotation);
+  auto velocity =
+      desiredVelocity.Optimize(encoderRotation).CosineScale(encoderRotation);
 
   // Calculate the drive output from the drive PID controller.
   const auto driveOutput = m_drivePIDController.Calculate(
-      m_driveEncoder.GetRate(), desiredVelocity.velocity.value());
+      m_driveEncoder.GetRate(), velocity.velocity.value());
 
-  const auto driveFeedforward =
-      m_driveFeedforward.Calculate(desiredVelocity.velocity);
+  const auto driveFeedforward = m_driveFeedforward.Calculate(velocity.velocity);
 
   // Calculate the turning motor output from the turning PID controller.
   const auto turnOutput = m_turningPIDController.Calculate(
       wpi::units::radian_t{m_turningEncoder.GetDistance()},
-      desiredVelocity.angle.Radians());
+      velocity.angle.Radians());
 
   const auto turnFeedforward = m_turnFeedforward.Calculate(
       m_turningPIDController.GetSetpoint().velocity);
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp
index 1e53ce686f..b29e456794 100644
--- a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp
@@ -18,10 +18,8 @@ void Drivetrain::Drive(wpi::units::meters_per_second_t xVelocity,
   }
   chassisVelocities = chassisVelocities.Discretize(period);
 
-  auto states = m_kinematics.ToSwerveModuleVelocities(chassisVelocities);
-  m_kinematics.DesaturateWheelVelocities(&states, kMaxVelocity);
-
-  auto [fl, fr, bl, br] = states;
+  auto [fl, fr, bl, br] = m_kinematics.DesaturateWheelVelocities(
+      m_kinematics.ToSwerveModuleVelocities(chassisVelocities), kMaxVelocity);
   m_frontLeft.SetDesiredVelocity(fl);
   m_frontRight.SetDesiredVelocity(fr);
   m_backLeft.SetDesiredVelocity(bl);
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/SwerveModule.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/SwerveModule.cpp
index 0daab13e93..c6c025958b 100644
--- a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/SwerveModule.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/SwerveModule.cpp
@@ -52,25 +52,23 @@ void SwerveModule::SetDesiredVelocity(
   wpi::math::Rotation2d encoderRotation{
       wpi::units::radian_t{m_turningEncoder.GetDistance()}};
 
-  // Optimize the desired velocity to avoid spinning further than 90 degrees
-  desiredVelocity.Optimize(encoderRotation);
-
-  // Scale velocity by cosine of angle error. This scales down movement
+  // Optimize the desired velocity to avoid spinning further than 90 degrees,
+  // then scale velocity by cosine of angle error. This scales down movement
   // perpendicular to the desired direction of travel that can occur when
   // modules change directions. This results in smoother driving.
-  desiredVelocity.CosineScale(encoderRotation);
+  auto velocity =
+      desiredVelocity.Optimize(encoderRotation).CosineScale(encoderRotation);
 
   // Calculate the drive output from the drive PID controller.
   const auto driveOutput = m_drivePIDController.Calculate(
-      m_driveEncoder.GetRate(), desiredVelocity.velocity.value());
+      m_driveEncoder.GetRate(), velocity.velocity.value());
 
-  const auto driveFeedforward =
-      m_driveFeedforward.Calculate(desiredVelocity.velocity);
+  const auto driveFeedforward = m_driveFeedforward.Calculate(velocity.velocity);
 
   // Calculate the turning motor output from the turning PID controller.
   const auto turnOutput = m_turningPIDController.Calculate(
       wpi::units::radian_t{m_turningEncoder.GetDistance()},
-      desiredVelocity.angle.Radians());
+      velocity.angle.Radians());
 
   const auto turnFeedforward = m_turnFeedforward.Calculate(
       m_turningPIDController.GetSetpoint().velocity);
diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/swervebot/Drivetrain.java b/wpilibjExamples/src/main/java/org/wpilib/examples/swervebot/Drivetrain.java
index 607f96d7af..4c0d28d03e 100644
--- a/wpilibjExamples/src/main/java/org/wpilib/examples/swervebot/Drivetrain.java
+++ b/wpilibjExamples/src/main/java/org/wpilib/examples/swervebot/Drivetrain.java
@@ -63,13 +63,14 @@ public class Drivetrain {
     }
     chassisVelocities = chassisVelocities.discretize(period);
 
-    var states = m_kinematics.toWheelVelocities(chassisVelocities);
-    SwerveDriveKinematics.desaturateWheelVelocities(states, kMaxVelocity);
+    var velocities =
+        SwerveDriveKinematics.desaturateWheelVelocities(
+            m_kinematics.toWheelVelocities(chassisVelocities), kMaxVelocity);
 
-    m_frontLeft.setDesiredVelocity(states[0]);
-    m_frontRight.setDesiredVelocity(states[1]);
-    m_backLeft.setDesiredVelocity(states[2]);
-    m_backRight.setDesiredVelocity(states[3]);
+    m_frontLeft.setDesiredVelocity(velocities[0]);
+    m_frontRight.setDesiredVelocity(velocities[1]);
+    m_backLeft.setDesiredVelocity(velocities[2]);
+    m_backRight.setDesiredVelocity(velocities[3]);
   }
 
   /** Updates the field relative position of the robot. */
diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/swervebot/SwerveModule.java b/wpilibjExamples/src/main/java/org/wpilib/examples/swervebot/SwerveModule.java
index aa5f69dedd..bcf68dc27f 100644
--- a/wpilibjExamples/src/main/java/org/wpilib/examples/swervebot/SwerveModule.java
+++ b/wpilibjExamples/src/main/java/org/wpilib/examples/swervebot/SwerveModule.java
@@ -110,26 +110,23 @@ public class SwerveModule {
   public void setDesiredVelocity(SwerveModuleVelocity desiredVelocity) {
     var encoderRotation = new Rotation2d(m_turningEncoder.getDistance());
 
-    // Optimize the desired velocity to avoid spinning further than 90 degrees
-    desiredVelocity.optimize(encoderRotation);
-
-    // Scale velocity by cosine of angle error. This scales down movement perpendicular to the
-    // desired
-    // direction of travel that can occur when modules change directions. This results in smoother
-    // driving.
-    desiredVelocity.cosineScale(encoderRotation);
+    // Optimize the desired velocity to avoid spinning further than 90 degrees, then scale velocity
+    // by cosine of angle error. This scales down movement perpendicular to the desired direction of
+    // travel that can occur when modules change directions. This results in smoother driving.
+    SwerveModuleVelocity velocity =
+        desiredVelocity.optimize(encoderRotation).cosineScale(encoderRotation);
 
     // Calculate the drive output from the drive PID controller.
 
     final double driveOutput =
-        m_drivePIDController.calculate(m_driveEncoder.getRate(), desiredVelocity.velocity);
+        m_drivePIDController.calculate(m_driveEncoder.getRate(), velocity.velocity);
 
-    final double driveFeedforward = m_driveFeedforward.calculate(desiredVelocity.velocity);
+    final double driveFeedforward = m_driveFeedforward.calculate(velocity.velocity);
 
     // Calculate the turning motor output from the turning PID controller.
     final double turnOutput =
         m_turningPIDController.calculate(
-            m_turningEncoder.getDistance(), desiredVelocity.angle.getRadians());
+            m_turningEncoder.getDistance(), velocity.angle.getRadians());
 
     final double turnFeedforward =
         m_turnFeedforward.calculate(m_turningPIDController.getSetpoint().velocity);
diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/swervedriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/org/wpilib/examples/swervedriveposeestimator/Drivetrain.java
index d587eefc23..bceaeb58ca 100644
--- a/wpilibjExamples/src/main/java/org/wpilib/examples/swervedriveposeestimator/Drivetrain.java
+++ b/wpilibjExamples/src/main/java/org/wpilib/examples/swervedriveposeestimator/Drivetrain.java
@@ -74,13 +74,14 @@ public class Drivetrain {
 
     chassisVelocities = chassisVelocities.discretize(period);
 
-    var states = m_kinematics.toWheelVelocities(chassisVelocities);
-    SwerveDriveKinematics.desaturateWheelVelocities(states, kMaxVelocity);
+    var velocities =
+        SwerveDriveKinematics.desaturateWheelVelocities(
+            m_kinematics.toWheelVelocities(chassisVelocities), kMaxVelocity);
 
-    m_frontLeft.setDesiredVelocity(states[0]);
-    m_frontRight.setDesiredVelocity(states[1]);
-    m_backLeft.setDesiredVelocity(states[2]);
-    m_backRight.setDesiredVelocity(states[3]);
+    m_frontLeft.setDesiredVelocity(velocities[0]);
+    m_frontRight.setDesiredVelocity(velocities[1]);
+    m_backLeft.setDesiredVelocity(velocities[2]);
+    m_backRight.setDesiredVelocity(velocities[3]);
   }
 
   /** Updates the field relative position of the robot. */
diff --git a/wpilibjExamples/src/main/java/org/wpilib/examples/swervedriveposeestimator/SwerveModule.java b/wpilibjExamples/src/main/java/org/wpilib/examples/swervedriveposeestimator/SwerveModule.java
index 975c476a0f..2b4df5008e 100644
--- a/wpilibjExamples/src/main/java/org/wpilib/examples/swervedriveposeestimator/SwerveModule.java
+++ b/wpilibjExamples/src/main/java/org/wpilib/examples/swervedriveposeestimator/SwerveModule.java
@@ -110,25 +110,22 @@ public class SwerveModule {
   public void setDesiredVelocity(SwerveModuleVelocity desiredVelocity) {
     var encoderRotation = new Rotation2d(m_turningEncoder.getDistance());
 
-    // Optimize the reference velocity to avoid spinning further than 90 degrees
-    desiredVelocity.optimize(encoderRotation);
-
-    // Scale velocity by cosine of angle error. This scales down movement perpendicular to the
-    // desired
-    // direction of travel that can occur when modules change directions. This results in smoother
-    // driving.
-    desiredVelocity.cosineScale(encoderRotation);
+    // Optimize the desired velocity to avoid spinning further than 90 degrees, then scale velocity
+    // by cosine of angle error. This scales down movement perpendicular to the desired direction of
+    // travel that can occur when modules change directions. This results in smoother driving.
+    SwerveModuleVelocity velocity =
+        desiredVelocity.optimize(encoderRotation).cosineScale(encoderRotation);
 
     // Calculate the drive output from the drive PID controller.
     final double driveOutput =
-        m_drivePIDController.calculate(m_driveEncoder.getRate(), desiredVelocity.velocity);
+        m_drivePIDController.calculate(m_driveEncoder.getRate(), velocity.velocity);
 
-    final double driveFeedforward = m_driveFeedforward.calculate(desiredVelocity.velocity);
+    final double driveFeedforward = m_driveFeedforward.calculate(velocity.velocity);
 
     // Calculate the turning motor output from the turning PID controller.
     final double turnOutput =
         m_turningPIDController.calculate(
-            m_turningEncoder.getDistance(), desiredVelocity.angle.getRadians());
+            m_turningEncoder.getDistance(), velocity.angle.getRadians());
 
     final double turnFeedforward =
         m_turnFeedforward.calculate(m_turningPIDController.getSetpoint().velocity);
diff --git a/wpimath/BUILD.bazel b/wpimath/BUILD.bazel
index c318446a95..378f8cdf1b 100644
--- a/wpimath/BUILD.bazel
+++ b/wpimath/BUILD.bazel
@@ -236,6 +236,7 @@ wpilib_cc_shared_library(
 wpilib_jni_java_library(
     name = "wpimath-java",
     srcs = [":generated_java"] + glob(["src/main/java/**/*.java"]),
+    exported_plugins = ["//javacPlugin:plugin"],
     javacopts = [
         "-Xep:UnicodeInCode:OFF",
     ],
@@ -249,6 +250,7 @@ wpilib_jni_java_library(
     resources = glob(["src/main/proto/**"]),
     visibility = ["//visibility:public"],
     deps = [
+        "//wpiannotations",
         "//wpiunits:wpiunits-java",
         "//wpiutil:wpiutil-java",
         "@maven//:io_avaje_avaje_json_core",
diff --git a/wpimath/CMakeLists.txt b/wpimath/CMakeLists.txt
index 5160275080..daeb98d33d 100644
--- a/wpimath/CMakeLists.txt
+++ b/wpimath/CMakeLists.txt
@@ -74,7 +74,13 @@ if(WITH_JAVA)
     add_jar(
         wpimath_jar
         ${JAVA_SOURCES}
-        INCLUDE_JARS wpiutil_jar wpiunits_jar ${EJML_JARS} ${AVAJE_JARS} ${QUICKBUF_JAR}
+        INCLUDE_JARS
+            wpiutil_jar
+            wpiunits_jar
+            wpiannotations_jar
+            ${EJML_JARS}
+            ${AVAJE_JARS}
+            ${QUICKBUF_JAR}
         OUTPUT_NAME wpimath
         OUTPUT_DIR ${WPILIB_BINARY_DIR}/${java_lib_dest}
         GENERATE_NATIVE_HEADERS wpimath_jni_headers
diff --git a/wpimath/build.gradle b/wpimath/build.gradle
index 92fb447fd7..8a766b7c3c 100644
--- a/wpimath/build.gradle
+++ b/wpimath/build.gradle
@@ -93,6 +93,9 @@ dependencies {
     api libs.avaje.jsonb
     api libs.bundles.ejml
     api libs.quickbuf.runtime
+    implementation project(":wpiannotations")
+    annotationProcessor project(':javacPlugin')
+    testAnnotationProcessor project(':javacPlugin')
 }
 
 sourceSets.main.java.srcDir "${projectDir}/src/generated/main/java"
diff --git a/wpimath/src/main/java/org/wpilib/math/kinematics/DifferentialDriveWheelVelocities.java b/wpimath/src/main/java/org/wpilib/math/kinematics/DifferentialDriveWheelVelocities.java
index 166c40f286..92a83d07f3 100644
--- a/wpimath/src/main/java/org/wpilib/math/kinematics/DifferentialDriveWheelVelocities.java
+++ b/wpimath/src/main/java/org/wpilib/math/kinematics/DifferentialDriveWheelVelocities.java
@@ -6,6 +6,7 @@ package org.wpilib.math.kinematics;
 
 import static org.wpilib.units.Units.MetersPerSecond;
 
+import org.wpilib.annotation.NoDiscard;
 import org.wpilib.math.interpolation.Interpolatable;
 import org.wpilib.math.kinematics.proto.DifferentialDriveWheelVelocitiesProto;
 import org.wpilib.math.kinematics.struct.DifferentialDriveWheelVelocitiesStruct;
@@ -14,6 +15,7 @@ import org.wpilib.util.protobuf.ProtobufSerializable;
 import org.wpilib.util.struct.StructSerializable;
 
 /** Represents the wheel velocities for a differential drive drivetrain. */
+@NoDiscard
 public class DifferentialDriveWheelVelocities
     implements Interpolatable<DifferentialDriveWheelVelocities>,
         ProtobufSerializable,
@@ -66,14 +68,17 @@ public class DifferentialDriveWheelVelocities
    *
    * @param attainableMaxVelocity The absolute max velocity in meters per second that a wheel can
    *     reach.
+   * @return The desaturated DifferentialDriveWheelVelocities.
    */
-  public void desaturate(double attainableMaxVelocity) {
+  public DifferentialDriveWheelVelocities desaturate(double attainableMaxVelocity) {
     double realMaxVelocity = Math.max(Math.abs(left), Math.abs(right));
 
     if (realMaxVelocity > attainableMaxVelocity) {
-      left = left / realMaxVelocity * attainableMaxVelocity;
-      right = right / realMaxVelocity * attainableMaxVelocity;
+      return new DifferentialDriveWheelVelocities(
+          left / realMaxVelocity * attainableMaxVelocity,
+          right / realMaxVelocity * attainableMaxVelocity);
     }
+    return new DifferentialDriveWheelVelocities(left, right);
   }
 
   /**
@@ -86,9 +91,10 @@ public class DifferentialDriveWheelVelocities
    *
    * @param attainableMaxVelocity The absolute max velocity in meters per second that a wheel can
    *     reach.
+   * @return The desaturated DifferentialDriveWheelVelocities.
    */
-  public void desaturate(LinearVelocity attainableMaxVelocity) {
-    desaturate(attainableMaxVelocity.in(MetersPerSecond));
+  public DifferentialDriveWheelVelocities desaturate(LinearVelocity attainableMaxVelocity) {
+    return desaturate(attainableMaxVelocity.in(MetersPerSecond));
   }
 
   /**
diff --git a/wpimath/src/main/java/org/wpilib/math/kinematics/MecanumDriveWheelVelocities.java b/wpimath/src/main/java/org/wpilib/math/kinematics/MecanumDriveWheelVelocities.java
index d274c42b03..7d8b028729 100644
--- a/wpimath/src/main/java/org/wpilib/math/kinematics/MecanumDriveWheelVelocities.java
+++ b/wpimath/src/main/java/org/wpilib/math/kinematics/MecanumDriveWheelVelocities.java
@@ -6,6 +6,7 @@ package org.wpilib.math.kinematics;
 
 import static org.wpilib.units.Units.MetersPerSecond;
 
+import org.wpilib.annotation.NoDiscard;
 import org.wpilib.math.interpolation.Interpolatable;
 import org.wpilib.math.kinematics.proto.MecanumDriveWheelVelocitiesProto;
 import org.wpilib.math.kinematics.struct.MecanumDriveWheelVelocitiesStruct;
@@ -14,6 +15,7 @@ import org.wpilib.util.protobuf.ProtobufSerializable;
 import org.wpilib.util.struct.StructSerializable;
 
 /** Represents the wheel velocities for a mecanum drive drivetrain. */
+@NoDiscard
 public class MecanumDriveWheelVelocities
     implements Interpolatable<MecanumDriveWheelVelocities>,
         ProtobufSerializable,
@@ -87,7 +89,7 @@ public class MecanumDriveWheelVelocities
    *
    * @param attainableMaxVelocity The absolute max velocity in meters per second that a wheel can
    *     reach.
-   * @return Desaturated MecanumDriveWheelVelocities.
+   * @return The desaturated MecanumDriveWheelVelocities.
    */
   public MecanumDriveWheelVelocities desaturate(double attainableMaxVelocity) {
     double realMaxVelocity = Math.max(Math.abs(frontLeft), Math.abs(frontRight));
@@ -114,7 +116,7 @@ public class MecanumDriveWheelVelocities
    * at-or-below the absolute threshold, while maintaining the ratio of velocities between wheels.
    *
    * @param attainableMaxVelocity The absolute max velocity that a wheel can reach.
-   * @return Desaturated MecanumDriveWheelVelocities.
+   * @return The desaturated MecanumDriveWheelVelocities.
    */
   public MecanumDriveWheelVelocities desaturate(LinearVelocity attainableMaxVelocity) {
     return desaturate(attainableMaxVelocity.in(MetersPerSecond));
diff --git a/wpimath/src/main/java/org/wpilib/math/kinematics/SwerveDriveKinematics.java b/wpimath/src/main/java/org/wpilib/math/kinematics/SwerveDriveKinematics.java
index 364931ca21..67692c2915 100644
--- a/wpimath/src/main/java/org/wpilib/math/kinematics/SwerveDriveKinematics.java
+++ b/wpimath/src/main/java/org/wpilib/math/kinematics/SwerveDriveKinematics.java
@@ -9,6 +9,7 @@ import static org.wpilib.units.Units.RadiansPerSecond;
 
 import java.util.Arrays;
 import org.ejml.simple.SimpleMatrix;
+import org.wpilib.annotation.NoDiscard;
 import org.wpilib.math.geometry.Rotation2d;
 import org.wpilib.math.geometry.Translation2d;
 import org.wpilib.math.geometry.Twist2d;
@@ -261,22 +262,35 @@ public class SwerveDriveKinematics
    * direction of rotational velocity, which makes discretizing the chassis velocities inaccurate
    * because the discretization did not account for this translational skew.
    *
-   * @param moduleVelocities Reference to array of module states. The array will be mutated with the
-   *     normalized velocities!
+   * @param moduleVelocities The array of module velocities.
    * @param attainableMaxVelocity The absolute max velocity in meters per second that a module can
    *     reach.
+   * @return The array of desaturated module velocities.
    */
-  public static void desaturateWheelVelocities(
+  @NoDiscard
+  public static SwerveModuleVelocity[] desaturateWheelVelocities(
       SwerveModuleVelocity[] moduleVelocities, double attainableMaxVelocity) {
     double realMaxVelocity = 0;
     for (SwerveModuleVelocity moduleVelocity : moduleVelocities) {
       realMaxVelocity = Math.max(realMaxVelocity, Math.abs(moduleVelocity.velocity));
     }
+    var velocities = new SwerveModuleVelocity[moduleVelocities.length];
     if (realMaxVelocity > attainableMaxVelocity) {
-      for (SwerveModuleVelocity moduleVelocity : moduleVelocities) {
-        moduleVelocity.velocity = moduleVelocity.velocity / realMaxVelocity * attainableMaxVelocity;
+      for (int i = 0; i < velocities.length; i++) {
+        velocities[i] =
+            new SwerveModuleVelocity(
+                moduleVelocities[i].velocity / realMaxVelocity * attainableMaxVelocity,
+                moduleVelocities[i].angle);
+      }
+    } else {
+      // Copy in the event someone wants to mutate the desaturated velocities but also wants the
+      // original velocities
+      for (int i = 0; i < velocities.length; i++) {
+        velocities[i] =
+            new SwerveModuleVelocity(moduleVelocities[i].velocity, moduleVelocities[i].angle);
       }
     }
+    return velocities;
   }
 
   /**
@@ -291,14 +305,15 @@ public class SwerveDriveKinematics
    * direction of rotational velocity, which makes discretizing the chassis velocities inaccurate
    * because the discretization did not account for this translational skew.
    *
-   * @param moduleVelocities Reference to array of module states. The array will be mutated with the
-   *     normalized velocities!
+   * @param moduleVelocities The array of module velocities.
    * @param attainableMaxVelocity The absolute max velocity in meters per second that a module can
    *     reach.
+   * @return The array of desaturated module velocities.
    */
-  public static void desaturateWheelVelocities(
+  @NoDiscard
+  public static SwerveModuleVelocity[] desaturateWheelVelocities(
       SwerveModuleVelocity[] moduleVelocities, LinearVelocity attainableMaxVelocity) {
-    desaturateWheelVelocities(moduleVelocities, attainableMaxVelocity.in(MetersPerSecond));
+    return desaturateWheelVelocities(moduleVelocities, attainableMaxVelocity.in(MetersPerSecond));
   }
 
   /**
@@ -314,8 +329,7 @@ public class SwerveDriveKinematics
    * direction of rotational velocity, which makes discretizing the chassis velocities inaccurate
    * because the discretization did not account for this translational skew.
    *
-   * @param moduleVelocities Reference to array of module states. The array will be mutated with the
-   *     normalized velocities!
+   * @param moduleVelocities The array of module velocities
    * @param desiredChassisVelocity The desired velocity of the robot
    * @param attainableMaxModuleVelocity The absolute max velocity in meters per second that a module
    *     can reach
@@ -323,8 +337,10 @@ public class SwerveDriveKinematics
    *     your robot can reach while translating
    * @param attainableMaxRotationalVelocity The absolute max velocity in radians per second the
    *     robot can reach while rotating
+   * @return The array of desaturated module velocities
    */
-  public static void desaturateWheelVelocities(
+  @NoDiscard
+  public static SwerveModuleVelocity[] desaturateWheelVelocities(
       SwerveModuleVelocity[] moduleVelocities,
       ChassisVelocities desiredChassisVelocity,
       double attainableMaxModuleVelocity,
@@ -335,10 +351,17 @@ public class SwerveDriveKinematics
       realMaxVelocity = Math.max(realMaxVelocity, Math.abs(moduleVelocity.velocity));
     }
 
+    var velocities = new SwerveModuleVelocity[moduleVelocities.length];
     if (attainableMaxTranslationalVelocity == 0
         || attainableMaxRotationalVelocity == 0
         || realMaxVelocity == 0) {
-      return;
+      // Copy in the event someone wants to mutate the desaturated velocities but also wants the
+      // original velocities
+      for (int i = 0; i < velocities.length; i++) {
+        velocities[i] =
+            new SwerveModuleVelocity(moduleVelocities[i].velocity, moduleVelocities[i].angle);
+      }
+      return velocities;
     }
     double translationalK =
         Math.hypot(desiredChassisVelocity.vx, desiredChassisVelocity.vy)
@@ -346,9 +369,11 @@ public class SwerveDriveKinematics
     double rotationalK = Math.abs(desiredChassisVelocity.omega) / attainableMaxRotationalVelocity;
     double k = Math.max(translationalK, rotationalK);
     double scale = Math.min(k * attainableMaxModuleVelocity / realMaxVelocity, 1);
-    for (SwerveModuleVelocity moduleVelocity : moduleVelocities) {
-      moduleVelocity.velocity *= scale;
+    for (int i = 0; i < velocities.length; i++) {
+      velocities[i] =
+          new SwerveModuleVelocity(moduleVelocities[i].velocity * scale, moduleVelocities[i].angle);
     }
+    return velocities;
   }
 
   /**
@@ -364,22 +389,23 @@ public class SwerveDriveKinematics
    * direction of rotational velocity, which makes discretizing the chassis velocities inaccurate
    * because the discretization did not account for this translational skew.
    *
-   * @param moduleVelocities Reference to array of module states. The array will be mutated with the
-   *     normalized velocities!
+   * @param moduleVelocities The array of module velocities
    * @param desiredChassisVelocity The desired velocity of the robot
    * @param attainableMaxModuleVelocity The absolute max velocity that a module can reach
    * @param attainableMaxTranslationalVelocity The absolute max velocity that your robot can reach
    *     while translating
    * @param attainableMaxRotationalVelocity The absolute max velocity the robot can reach while
    *     rotating
+   * @return The array of desaturated module velocities
    */
-  public static void desaturateWheelVelocities(
+  @NoDiscard
+  public static SwerveModuleVelocity[] desaturateWheelVelocities(
       SwerveModuleVelocity[] moduleVelocities,
       ChassisVelocities desiredChassisVelocity,
       LinearVelocity attainableMaxModuleVelocity,
       LinearVelocity attainableMaxTranslationalVelocity,
       AngularVelocity attainableMaxRotationalVelocity) {
-    desaturateWheelVelocities(
+    return desaturateWheelVelocities(
         moduleVelocities,
         desiredChassisVelocity,
         attainableMaxModuleVelocity.in(MetersPerSecond),
diff --git a/wpimath/src/main/java/org/wpilib/math/kinematics/SwerveModuleVelocity.java b/wpimath/src/main/java/org/wpilib/math/kinematics/SwerveModuleVelocity.java
index f2ebfc9495..b27905d38a 100644
--- a/wpimath/src/main/java/org/wpilib/math/kinematics/SwerveModuleVelocity.java
+++ b/wpimath/src/main/java/org/wpilib/math/kinematics/SwerveModuleVelocity.java
@@ -7,6 +7,7 @@ package org.wpilib.math.kinematics;
 import static org.wpilib.units.Units.MetersPerSecond;
 
 import java.util.Objects;
+import org.wpilib.annotation.NoDiscard;
 import org.wpilib.math.geometry.Rotation2d;
 import org.wpilib.math.interpolation.Interpolatable;
 import org.wpilib.math.kinematics.proto.SwerveModuleVelocityProto;
@@ -16,6 +17,7 @@ import org.wpilib.util.protobuf.ProtobufSerializable;
 import org.wpilib.util.struct.StructSerializable;
 
 /** Represents the velocity of one swerve module. */
+@NoDiscard
 public class SwerveModuleVelocity
     implements Interpolatable<SwerveModuleVelocity>,
         Comparable<SwerveModuleVelocity>,
@@ -92,34 +94,14 @@ public class SwerveModuleVelocity
    * continuous input functionality, the furthest a wheel will ever rotate is 90 degrees.
    *
    * @param currentAngle The current module angle.
+   * @return The optimized SwerveModuleVelocity.
    */
-  public void optimize(Rotation2d currentAngle) {
+  public SwerveModuleVelocity optimize(Rotation2d currentAngle) {
     var delta = angle.minus(currentAngle);
     if (Math.abs(delta.getDegrees()) > 90.0) {
-      velocity *= -1;
-      angle = angle.rotateBy(Rotation2d.kPi);
-    }
-  }
-
-  /**
-   * Minimize the change in heading the desired swerve module velocity would require by potentially
-   * reversing the direction the wheel spins. If this is used with the PIDController class's
-   * continuous input functionality, the furthest a wheel will ever rotate is 90 degrees.
-   *
-   * @param desiredVelocity The desired velocity.
-   * @param currentAngle The current module angle.
-   * @return Optimized swerve module velocity.
-   * @deprecated Use the instance method instead.
-   */
-  @Deprecated
-  public static SwerveModuleVelocity optimize(
-      SwerveModuleVelocity desiredVelocity, Rotation2d currentAngle) {
-    var delta = desiredVelocity.angle.minus(currentAngle);
-    if (Math.abs(delta.getDegrees()) > 90.0) {
-      return new SwerveModuleVelocity(
-          -desiredVelocity.velocity, desiredVelocity.angle.rotateBy(Rotation2d.kPi));
+      return new SwerveModuleVelocity(-velocity, angle.rotateBy(Rotation2d.kPi));
     } else {
-      return new SwerveModuleVelocity(desiredVelocity.velocity, desiredVelocity.angle);
+      return new SwerveModuleVelocity(velocity, angle);
     }
   }
 
@@ -151,8 +133,9 @@ public class SwerveModuleVelocity
    * smoother driving.
    *
    * @param currentAngle The current module angle.
+   * @return The scaled SwerveModuleVelocity.
    */
-  public void cosineScale(Rotation2d currentAngle) {
-    velocity *= angle.minus(currentAngle).getCos();
+  public SwerveModuleVelocity cosineScale(Rotation2d currentAngle) {
+    return new SwerveModuleVelocity(velocity * angle.minus(currentAngle).getCos(), angle);
   }
 }
diff --git a/wpimath/src/main/java/org/wpilib/math/trajectory/constraint/DifferentialDriveKinematicsConstraint.java b/wpimath/src/main/java/org/wpilib/math/trajectory/constraint/DifferentialDriveKinematicsConstraint.java
index 6e07200ba5..127761aee2 100644
--- a/wpimath/src/main/java/org/wpilib/math/trajectory/constraint/DifferentialDriveKinematicsConstraint.java
+++ b/wpimath/src/main/java/org/wpilib/math/trajectory/constraint/DifferentialDriveKinematicsConstraint.java
@@ -44,8 +44,8 @@ public class DifferentialDriveKinematicsConstraint implements TrajectoryConstrai
     var chassisVelocities = new ChassisVelocities(velocity, 0, velocity * curvature);
 
     // Get the wheel velocities and normalize them to within the max velocity.
-    var wheelVelocities = m_kinematics.toWheelVelocities(chassisVelocities);
-    wheelVelocities.desaturate(m_maxVelocity);
+    var wheelVelocities =
+        m_kinematics.toWheelVelocities(chassisVelocities).desaturate(m_maxVelocity);
 
     // Return the new linear chassis velocity.
     return m_kinematics.toChassisVelocities(wheelVelocities).vx;
diff --git a/wpimath/src/main/java/org/wpilib/math/trajectory/constraint/SwerveDriveKinematicsConstraint.java b/wpimath/src/main/java/org/wpilib/math/trajectory/constraint/SwerveDriveKinematicsConstraint.java
index 5102274ace..42f9350269 100644
--- a/wpimath/src/main/java/org/wpilib/math/trajectory/constraint/SwerveDriveKinematicsConstraint.java
+++ b/wpimath/src/main/java/org/wpilib/math/trajectory/constraint/SwerveDriveKinematicsConstraint.java
@@ -50,8 +50,9 @@ public class SwerveDriveKinematicsConstraint implements TrajectoryConstraint {
     var chassisVelocities = new ChassisVelocities(xdVelocity, ydVelocity, velocity * curvature);
 
     // Get the wheel velocities and normalize them to within the max velocity.
-    var wheelVelocities = m_kinematics.toSwerveModuleVelocities(chassisVelocities);
-    SwerveDriveKinematics.desaturateWheelVelocities(wheelVelocities, m_maxVelocity);
+    var wheelVelocities =
+        SwerveDriveKinematics.desaturateWheelVelocities(
+            m_kinematics.toSwerveModuleVelocities(chassisVelocities), m_maxVelocity);
 
     // Convert normalized wheel velocities back to chassis velocities
     var normVelocities = m_kinematics.toChassisVelocities(wheelVelocities);
diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveWheelVelocities.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveWheelVelocities.hpp
index 317f0ba79c..a20627213a 100644
--- a/wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveWheelVelocities.hpp
+++ b/wpimath/src/main/native/include/wpi/math/kinematics/DifferentialDriveWheelVelocities.hpp
@@ -36,16 +36,19 @@ struct WPILIB_DLLEXPORT DifferentialDriveWheelVelocities {
    *
    * @param attainableMaxVelocity The absolute max velocity that a wheel can
    *     reach.
+   * @return The desaturated DifferentialDriveWheelVelocities.
    */
-  constexpr void Desaturate(
+  [[nodiscard]]
+  constexpr DifferentialDriveWheelVelocities Desaturate(
       wpi::units::meters_per_second_t attainableMaxVelocity) {
     auto realMaxVelocity = wpi::units::math::max(wpi::units::math::abs(left),
                                                  wpi::units::math::abs(right));
 
     if (realMaxVelocity > attainableMaxVelocity) {
-      left = left / realMaxVelocity * attainableMaxVelocity;
-      right = right / realMaxVelocity * attainableMaxVelocity;
+      return {left / realMaxVelocity * attainableMaxVelocity,
+              right / realMaxVelocity * attainableMaxVelocity};
     }
+    return *this;
   }
 
   /**
diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveWheelVelocities.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveWheelVelocities.hpp
index b296d64dd7..6f9b23c32c 100644
--- a/wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveWheelVelocities.hpp
+++ b/wpimath/src/main/native/include/wpi/math/kinematics/MecanumDriveWheelVelocities.hpp
@@ -49,8 +49,9 @@ struct WPILIB_DLLEXPORT MecanumDriveWheelVelocities {
    *
    * @param attainableMaxVelocity The absolute max velocity that a wheel can
    *     reach.
-   * @return Desaturated MecanumDriveWheelVelocities.
+   * @return The desaturated MecanumDriveWheelVelocities.
    */
+  [[nodiscard]]
   constexpr MecanumDriveWheelVelocities Desaturate(
       wpi::units::meters_per_second_t attainableMaxVelocity) const {
     std::array<wpi::units::meters_per_second_t, 4> wheelVelocities{
diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveKinematics.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveKinematics.hpp
index 8267f623fa..16f2a6ed47 100644
--- a/wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveKinematics.hpp
+++ b/wpimath/src/main/native/include/wpi/math/kinematics/SwerveDriveKinematics.hpp
@@ -168,7 +168,7 @@ class SwerveDriveKinematics
    *     module states are not normalized. Sometimes, a user input may cause one
    *     of the module velocities to go above the attainable max velocity. Use
    *     the DesaturateWheelVelocities(wpi::util::array<SwerveModuleVelocity,
-   *     NumModules>*, wpi::units::meters_per_second_t) function to rectify this
+   *     NumModules>, wpi::units::meters_per_second_t) function to rectify this
    *     issue. In addition, you can leverage the power of C++17 to directly
    *     assign the module states to variables:
    *
@@ -357,17 +357,18 @@ class SwerveDriveKinematics
    * chassis velocities inaccurate because the discretization did not account
    * for this translational skew.
    *
-   * @param moduleVelocities Reference to array of module states. The array will
-   *     be mutated with the normalized velocities!
+   * @param moduleVelocities The array of module velocities.
    * @param attainableMaxVelocity The absolute max velocity that a module can
    *     reach.
+   * @return The array of desaturated module velocities.
    */
-  static void DesaturateWheelVelocities(
-      wpi::util::array<SwerveModuleVelocity, NumModules>* moduleVelocities,
+  [[nodiscard]]
+  static wpi::util::array<SwerveModuleVelocity, NumModules>
+  DesaturateWheelVelocities(
+      wpi::util::array<SwerveModuleVelocity, NumModules> moduleVelocities,
       wpi::units::meters_per_second_t attainableMaxVelocity) {
-    auto& states = *moduleVelocities;
     auto realMaxVelocity = wpi::units::math::abs(
-        std::max_element(states.begin(), states.end(),
+        std::max_element(moduleVelocities.begin(), moduleVelocities.end(),
                          [](const auto& a, const auto& b) {
                            return wpi::units::math::abs(a.velocity) <
                                   wpi::units::math::abs(b.velocity);
@@ -375,11 +376,16 @@ class SwerveDriveKinematics
             ->velocity);
 
     if (realMaxVelocity > attainableMaxVelocity) {
-      for (auto& module : states) {
-        module.velocity =
-            module.velocity / realMaxVelocity * attainableMaxVelocity;
+      wpi::util::array<SwerveModuleVelocity, NumModules> velocities(
+          wpi::util::empty_array);
+      for (size_t i = 0; i < NumModules; ++i) {
+        velocities[i] = {moduleVelocities[i].velocity / realMaxVelocity *
+                             attainableMaxVelocity,
+                         moduleVelocities[i].angle};
       }
+      return velocities;
     }
+    return moduleVelocities;
   }
 
   /**
@@ -399,8 +405,7 @@ class SwerveDriveKinematics
    * chassis velocities inaccurate because the discretization did not account
    * for this translational skew.
    *
-   * @param moduleVelocities Reference to array of module states. The array will
-   *     be mutated with the normalized velocities!
+   * @param moduleVelocities The array of module velocities.
    * @param desiredChassisVelocity The desired velocity of the robot
    * @param attainableMaxModuleVelocity The absolute max velocity a module can
    *     reach
@@ -408,17 +413,18 @@ class SwerveDriveKinematics
    *     robot can reach while translating
    * @param attainableMaxRobotRotationVelocity The absolute max velocity the
    *     robot can reach while rotating
+   * @return The array of desaturated module velocities
    */
-  static void DesaturateWheelVelocities(
-      wpi::util::array<SwerveModuleVelocity, NumModules>* moduleVelocities,
+  [[nodiscard]]
+  static wpi::util::array<SwerveModuleVelocity, NumModules>
+  DesaturateWheelVelocities(
+      wpi::util::array<SwerveModuleVelocity, NumModules> moduleVelocities,
       ChassisVelocities desiredChassisVelocity,
       wpi::units::meters_per_second_t attainableMaxModuleVelocity,
       wpi::units::meters_per_second_t attainableMaxRobotTranslationVelocity,
       wpi::units::radians_per_second_t attainableMaxRobotRotationVelocity) {
-    auto& states = *moduleVelocities;
-
     auto realMaxVelocity = wpi::units::math::abs(
-        std::max_element(states.begin(), states.end(),
+        std::max_element(moduleVelocities.begin(), moduleVelocities.end(),
                          [](const auto& a, const auto& b) {
                            return wpi::units::math::abs(a.velocity) <
                                   wpi::units::math::abs(b.velocity);
@@ -428,7 +434,7 @@ class SwerveDriveKinematics
     if (attainableMaxRobotTranslationVelocity == 0_mps ||
         attainableMaxRobotRotationVelocity == 0_rad_per_s ||
         realMaxVelocity == 0_mps) {
-      return;
+      return moduleVelocities;
     }
 
     auto translationalK = wpi::units::math::hypot(desiredChassisVelocity.vx,
@@ -443,9 +449,13 @@ class SwerveDriveKinematics
     auto scale =
         wpi::units::math::min(k * attainableMaxModuleVelocity / realMaxVelocity,
                               wpi::units::scalar_t{1});
-    for (auto& module : states) {
-      module.velocity = module.velocity * scale;
+    wpi::util::array<SwerveModuleVelocity, NumModules> velocities(
+        wpi::util::empty_array);
+    for (size_t i = 0; i < NumModules; ++i) {
+      velocities[i] = {moduleVelocities[i].velocity * scale,
+                       moduleVelocities[i].angle};
     }
+    return velocities;
   }
 
   wpi::util::array<SwerveModulePosition, NumModules> Interpolate(
diff --git a/wpimath/src/main/native/include/wpi/math/kinematics/SwerveModuleVelocity.hpp b/wpimath/src/main/native/include/wpi/math/kinematics/SwerveModuleVelocity.hpp
index 1879f17694..7fff7e6867 100644
--- a/wpimath/src/main/native/include/wpi/math/kinematics/SwerveModuleVelocity.hpp
+++ b/wpimath/src/main/native/include/wpi/math/kinematics/SwerveModuleVelocity.hpp
@@ -43,34 +43,15 @@ struct WPILIB_DLLEXPORT SwerveModuleVelocity {
    * furthest a wheel will ever rotate is 90 degrees.
    *
    * @param currentAngle The current module angle.
+   * @return The optimized SwerveModuleVelocity.
    */
-  constexpr void Optimize(const Rotation2d& currentAngle) {
+  [[nodiscard]]
+  constexpr SwerveModuleVelocity Optimize(const Rotation2d& currentAngle) {
     auto delta = angle - currentAngle;
     if (wpi::units::math::abs(delta.Degrees()) > 90_deg) {
-      velocity *= -1;
-      angle = angle + Rotation2d{180_deg};
-    }
-  }
-
-  /**
-   * Minimize the change in heading the desired swerve module velocity would
-   * require by potentially reversing the direction the wheel spins. If this is
-   * used with the PIDController class's continuous input functionality, the
-   * furthest a wheel will ever rotate is 90 degrees.
-   *
-   * @param desiredVelocity The desired velocity.
-   * @param currentAngle The current module angle.
-   */
-  [[deprecated("Use instance method instead.")]]
-  constexpr static SwerveModuleVelocity Optimize(
-      const SwerveModuleVelocity& desiredVelocity,
-      const Rotation2d& currentAngle) {
-    auto delta = desiredVelocity.angle - currentAngle;
-    if (wpi::units::math::abs(delta.Degrees()) > 90_deg) {
-      return {-desiredVelocity.velocity,
-              desiredVelocity.angle + Rotation2d{180_deg}};
+      return {-velocity, angle + Rotation2d{180_deg}};
     } else {
-      return {desiredVelocity.velocity, desiredVelocity.angle};
+      return {velocity, angle};
     }
   }
 
@@ -80,9 +61,11 @@ struct WPILIB_DLLEXPORT SwerveModuleVelocity {
    * modules change directions. This results in smoother driving.
    *
    * @param currentAngle The current module angle.
+   * @return The scaled SwerveModuleVelocity.
    */
-  constexpr void CosineScale(const Rotation2d& currentAngle) {
-    velocity *= (angle - currentAngle).Cos();
+  [[nodiscard]]
+  constexpr SwerveModuleVelocity CosineScale(const Rotation2d& currentAngle) {
+    return {velocity * (angle - currentAngle).Cos(), angle};
   }
 };
 }  // namespace wpi::math
diff --git a/wpimath/src/main/native/include/wpi/math/trajectory/constraint/DifferentialDriveKinematicsConstraint.hpp b/wpimath/src/main/native/include/wpi/math/trajectory/constraint/DifferentialDriveKinematicsConstraint.hpp
index 157380b1ec..0e7429ab4e 100644
--- a/wpimath/src/main/native/include/wpi/math/trajectory/constraint/DifferentialDriveKinematicsConstraint.hpp
+++ b/wpimath/src/main/native/include/wpi/math/trajectory/constraint/DifferentialDriveKinematicsConstraint.hpp
@@ -30,8 +30,8 @@ class WPILIB_DLLEXPORT DifferentialDriveKinematicsConstraint
       const Pose2d& pose, wpi::units::curvature_t curvature,
       wpi::units::meters_per_second_t velocity) const override {
     auto wheelVelocities =
-        m_kinematics.ToWheelVelocities({velocity, 0_mps, velocity * curvature});
-    wheelVelocities.Desaturate(m_maxVelocity);
+        m_kinematics.ToWheelVelocities({velocity, 0_mps, velocity * curvature})
+            .Desaturate(m_maxVelocity);
 
     return m_kinematics.ToChassisVelocities(wheelVelocities).vx;
   }
diff --git a/wpimath/src/main/native/include/wpi/math/trajectory/constraint/SwerveDriveKinematicsConstraint.hpp b/wpimath/src/main/native/include/wpi/math/trajectory/constraint/SwerveDriveKinematicsConstraint.hpp
index 1aebde4e59..a24b344014 100644
--- a/wpimath/src/main/native/include/wpi/math/trajectory/constraint/SwerveDriveKinematicsConstraint.hpp
+++ b/wpimath/src/main/native/include/wpi/math/trajectory/constraint/SwerveDriveKinematicsConstraint.hpp
@@ -31,9 +31,9 @@ class SwerveDriveKinematicsConstraint : public TrajectoryConstraint {
     auto yVelocity = velocity * pose.Rotation().Sin();
     auto wheelVelocities = m_kinematics.ToSwerveModuleVelocities(
         {xVelocity, yVelocity, velocity * curvature});
-    m_kinematics.DesaturateWheelVelocities(&wheelVelocities, m_maxVelocity);
 
-    auto normVelocities = m_kinematics.ToChassisVelocities(wheelVelocities);
+    auto normVelocities = m_kinematics.ToChassisVelocities(
+        m_kinematics.DesaturateWheelVelocities(wheelVelocities, m_maxVelocity));
 
     return wpi::units::math::hypot(normVelocities.vx, normVelocities.vy);
   }
diff --git a/wpimath/src/main/python/semiwrap/SwerveDriveKinematics.yml b/wpimath/src/main/python/semiwrap/SwerveDriveKinematics.yml
index 87af033508..78c0b4f139 100644
--- a/wpimath/src/main/python/semiwrap/SwerveDriveKinematics.yml
+++ b/wpimath/src/main/python/semiwrap/SwerveDriveKinematics.yml
@@ -58,23 +58,9 @@ classes:
           :
       DesaturateWheelVelocities:
         overloads:
-          wpi::util::array<SwerveModuleVelocity, NumModules>*, wpi::units::meters_per_second_t:
-            cpp_code: |
-              [](wpi::util::array<SwerveModuleVelocity, NumModules> moduleVelocities, wpi::units::meters_per_second_t attainableMaxVelocity) {
-                wpi::math::SwerveDriveKinematics<NumModules>::DesaturateWheelVelocities(&moduleVelocities, attainableMaxVelocity);
-                return moduleVelocities;
-              }
-          ? wpi::util::array<SwerveModuleVelocity, NumModules>*, ChassisVelocities, wpi::units::meters_per_second_t, wpi::units::meters_per_second_t, wpi::units::radians_per_second_t
-          : cpp_code: |
-              [](wpi::util::array<SwerveModuleVelocity, NumModules> moduleVelocities,
-                  ChassisVelocities currentChassisVelocity,
-                  wpi::units::meters_per_second_t attainableMaxModuleVelocity,
-                  wpi::units::meters_per_second_t attainableMaxRobotTranslationVelocity,
-                  wpi::units::radians_per_second_t attainableMaxRobotRotationVelocity) {
-                wpi::math::SwerveDriveKinematics<NumModules>::DesaturateWheelVelocities(&moduleVelocities, currentChassisVelocity, attainableMaxModuleVelocity, attainableMaxRobotTranslationVelocity, attainableMaxRobotRotationVelocity);
-                return moduleVelocities;
-              }
-
+          wpi::util::array<SwerveModuleVelocity, NumModules>, wpi::units::meters_per_second_t:
+          ? wpi::util::array<SwerveModuleVelocity, NumModules>, ChassisVelocities, wpi::units::meters_per_second_t, wpi::units::meters_per_second_t, wpi::units::radians_per_second_t
+          :
       Interpolate:
       GetModules:
       ToSwerveModuleAccelerations:
diff --git a/wpimath/src/test/java/org/wpilib/math/kinematics/SwerveDriveKinematicsTest.java b/wpimath/src/test/java/org/wpilib/math/kinematics/SwerveDriveKinematicsTest.java
index 1c4a6c16b7..d9198f00fb 100644
--- a/wpimath/src/test/java/org/wpilib/math/kinematics/SwerveDriveKinematicsTest.java
+++ b/wpimath/src/test/java/org/wpilib/math/kinematics/SwerveDriveKinematicsTest.java
@@ -350,15 +350,15 @@ class SwerveDriveKinematicsTest {
     SwerveModuleVelocity br = new SwerveModuleVelocity(7, Rotation2d.kZero);
 
     SwerveModuleVelocity[] arr = {fl, fr, bl, br};
-    SwerveDriveKinematics.desaturateWheelVelocities(arr, 5.5);
+    var desaturatedArr = SwerveDriveKinematics.desaturateWheelVelocities(arr, 5.5);
 
     double factor = 5.5 / 7.0;
 
     assertAll(
-        () -> assertEquals(5.0 * factor, arr[0].velocity, kEpsilon),
-        () -> assertEquals(6.0 * factor, arr[1].velocity, kEpsilon),
-        () -> assertEquals(4.0 * factor, arr[2].velocity, kEpsilon),
-        () -> assertEquals(7.0 * factor, arr[3].velocity, kEpsilon));
+        () -> assertEquals(5.0 * factor, desaturatedArr[0].velocity, kEpsilon),
+        () -> assertEquals(6.0 * factor, desaturatedArr[1].velocity, kEpsilon),
+        () -> assertEquals(4.0 * factor, desaturatedArr[2].velocity, kEpsilon),
+        () -> assertEquals(7.0 * factor, desaturatedArr[3].velocity, kEpsilon));
   }
 
   @Test
@@ -369,16 +369,17 @@ class SwerveDriveKinematicsTest {
     SwerveModuleVelocity br = new SwerveModuleVelocity(7, Rotation2d.kZero);
 
     SwerveModuleVelocity[] arr = {fl, fr, bl, br};
-    SwerveDriveKinematics.desaturateWheelVelocities(
-        arr, m_kinematics.toChassisVelocities(arr), 5.5, 5.5, 3.5);
+    var desaturatedArr =
+        SwerveDriveKinematics.desaturateWheelVelocities(
+            arr, m_kinematics.toChassisVelocities(arr), 5.5, 5.5, 3.5);
 
     double factor = 5.5 / 7.0;
 
     assertAll(
-        () -> assertEquals(5.0 * factor, arr[0].velocity, kEpsilon),
-        () -> assertEquals(6.0 * factor, arr[1].velocity, kEpsilon),
-        () -> assertEquals(4.0 * factor, arr[2].velocity, kEpsilon),
-        () -> assertEquals(7.0 * factor, arr[3].velocity, kEpsilon));
+        () -> assertEquals(5.0 * factor, desaturatedArr[0].velocity, kEpsilon),
+        () -> assertEquals(6.0 * factor, desaturatedArr[1].velocity, kEpsilon),
+        () -> assertEquals(4.0 * factor, desaturatedArr[2].velocity, kEpsilon),
+        () -> assertEquals(7.0 * factor, desaturatedArr[3].velocity, kEpsilon));
   }
 
   @Test
@@ -389,13 +390,13 @@ class SwerveDriveKinematicsTest {
     SwerveModuleVelocity br = new SwerveModuleVelocity(-2, Rotation2d.kZero);
 
     SwerveModuleVelocity[] arr = {fl, fr, bl, br};
-    SwerveDriveKinematics.desaturateWheelVelocities(arr, 1);
+    var desaturatedArr = SwerveDriveKinematics.desaturateWheelVelocities(arr, 1);
 
     assertAll(
-        () -> assertEquals(0.5, arr[0].velocity, kEpsilon),
-        () -> assertEquals(0.5, arr[1].velocity, kEpsilon),
-        () -> assertEquals(-1.0, arr[2].velocity, kEpsilon),
-        () -> assertEquals(-1.0, arr[3].velocity, kEpsilon));
+        () -> assertEquals(0.5, desaturatedArr[0].velocity, kEpsilon),
+        () -> assertEquals(0.5, desaturatedArr[1].velocity, kEpsilon),
+        () -> assertEquals(-1.0, desaturatedArr[2].velocity, kEpsilon),
+        () -> assertEquals(-1.0, desaturatedArr[3].velocity, kEpsilon));
   }
 
   @Test
diff --git a/wpimath/src/test/java/org/wpilib/math/kinematics/SwerveModuleVelocityTest.java b/wpimath/src/test/java/org/wpilib/math/kinematics/SwerveModuleVelocityTest.java
index 1489b8e558..16bb1ed446 100644
--- a/wpimath/src/test/java/org/wpilib/math/kinematics/SwerveModuleVelocityTest.java
+++ b/wpimath/src/test/java/org/wpilib/math/kinematics/SwerveModuleVelocityTest.java
@@ -17,137 +17,137 @@ class SwerveModuleVelocityTest {
   void testOptimize() {
     var angleA = Rotation2d.fromDegrees(45);
     var refA = new SwerveModuleVelocity(-2.0, Rotation2d.kPi);
-    refA.optimize(angleA);
+    var optimizedA = refA.optimize(angleA);
 
     assertAll(
-        () -> assertEquals(2.0, refA.velocity, kEpsilon),
-        () -> assertEquals(0.0, refA.angle.getDegrees(), kEpsilon));
+        () -> assertEquals(2.0, optimizedA.velocity, kEpsilon),
+        () -> assertEquals(0.0, optimizedA.angle.getDegrees(), kEpsilon));
 
     var angleB = Rotation2d.fromDegrees(-50);
     var refB = new SwerveModuleVelocity(4.7, Rotation2d.fromDegrees(41));
-    refB.optimize(angleB);
+    var optimizedB = refB.optimize(angleB);
 
     assertAll(
-        () -> assertEquals(-4.7, refB.velocity, kEpsilon),
-        () -> assertEquals(-139.0, refB.angle.getDegrees(), kEpsilon));
+        () -> assertEquals(-4.7, optimizedB.velocity, kEpsilon),
+        () -> assertEquals(-139.0, optimizedB.angle.getDegrees(), kEpsilon));
   }
 
   @Test
   void testNoOptimize() {
     var angleA = Rotation2d.kZero;
     var refA = new SwerveModuleVelocity(2.0, Rotation2d.fromDegrees(89));
-    refA.optimize(angleA);
+    var optimizedA = refA.optimize(angleA);
 
     assertAll(
-        () -> assertEquals(2.0, refA.velocity, kEpsilon),
-        () -> assertEquals(89.0, refA.angle.getDegrees(), kEpsilon));
+        () -> assertEquals(2.0, optimizedA.velocity, kEpsilon),
+        () -> assertEquals(89.0, optimizedA.angle.getDegrees(), kEpsilon));
 
     var angleB = Rotation2d.kZero;
     var refB = new SwerveModuleVelocity(-2.0, Rotation2d.fromDegrees(-2));
-    refB.optimize(angleB);
+    var optimizedB = refB.optimize(angleB);
 
     assertAll(
-        () -> assertEquals(-2.0, refB.velocity, kEpsilon),
-        () -> assertEquals(-2.0, refB.angle.getDegrees(), kEpsilon));
+        () -> assertEquals(-2.0, optimizedB.velocity, kEpsilon),
+        () -> assertEquals(-2.0, optimizedB.angle.getDegrees(), kEpsilon));
   }
 
   @Test
   void testCosineScale() {
     var angleA = Rotation2d.fromDegrees(0.0);
     var refA = new SwerveModuleVelocity(2.0, Rotation2d.fromDegrees(45.0));
-    refA.cosineScale(angleA);
+    var optimizedA = refA.cosineScale(angleA);
 
     assertAll(
-        () -> assertEquals(Math.sqrt(2.0), refA.velocity, kEpsilon),
-        () -> assertEquals(45.0, refA.angle.getDegrees(), kEpsilon));
+        () -> assertEquals(Math.sqrt(2.0), optimizedA.velocity, kEpsilon),
+        () -> assertEquals(45.0, optimizedA.angle.getDegrees(), kEpsilon));
 
     var angleB = Rotation2d.fromDegrees(45.0);
     var refB = new SwerveModuleVelocity(2.0, Rotation2d.fromDegrees(45.0));
-    refB.cosineScale(angleB);
+    var optimizedB = refB.cosineScale(angleB);
 
     assertAll(
-        () -> assertEquals(2.0, refB.velocity, kEpsilon),
-        () -> assertEquals(45.0, refB.angle.getDegrees(), kEpsilon));
+        () -> assertEquals(2.0, optimizedB.velocity, kEpsilon),
+        () -> assertEquals(45.0, optimizedB.angle.getDegrees(), kEpsilon));
 
     var angleC = Rotation2d.fromDegrees(-45.0);
     var refC = new SwerveModuleVelocity(2.0, Rotation2d.fromDegrees(45.0));
-    refC.cosineScale(angleC);
+    var optimizedC = refC.cosineScale(angleC);
 
     assertAll(
-        () -> assertEquals(0.0, refC.velocity, kEpsilon),
-        () -> assertEquals(45.0, refC.angle.getDegrees(), kEpsilon));
+        () -> assertEquals(0.0, optimizedC.velocity, kEpsilon),
+        () -> assertEquals(45.0, optimizedC.angle.getDegrees(), kEpsilon));
 
     var angleD = Rotation2d.fromDegrees(135.0);
     var refD = new SwerveModuleVelocity(2.0, Rotation2d.fromDegrees(45.0));
-    refD.cosineScale(angleD);
+    var optimizedD = refD.cosineScale(angleD);
 
     assertAll(
-        () -> assertEquals(0.0, refD.velocity, kEpsilon),
-        () -> assertEquals(45.0, refD.angle.getDegrees(), kEpsilon));
+        () -> assertEquals(0.0, optimizedD.velocity, kEpsilon),
+        () -> assertEquals(45.0, optimizedD.angle.getDegrees(), kEpsilon));
 
     var angleE = Rotation2d.fromDegrees(-135.0);
     var refE = new SwerveModuleVelocity(2.0, Rotation2d.fromDegrees(45.0));
-    refE.cosineScale(angleE);
+    var optimizedE = refE.cosineScale(angleE);
 
     assertAll(
-        () -> assertEquals(-2.0, refE.velocity, kEpsilon),
-        () -> assertEquals(45.0, refE.angle.getDegrees(), kEpsilon));
+        () -> assertEquals(-2.0, optimizedE.velocity, kEpsilon),
+        () -> assertEquals(45.0, optimizedE.angle.getDegrees(), kEpsilon));
 
     var angleF = Rotation2d.fromDegrees(180.0);
     var refF = new SwerveModuleVelocity(2.0, Rotation2d.fromDegrees(45.0));
-    refF.cosineScale(angleF);
+    var optimizedF = refF.cosineScale(angleF);
 
     assertAll(
-        () -> assertEquals(-Math.sqrt(2.0), refF.velocity, kEpsilon),
-        () -> assertEquals(45.0, refF.angle.getDegrees(), kEpsilon));
+        () -> assertEquals(-Math.sqrt(2.0), optimizedF.velocity, kEpsilon),
+        () -> assertEquals(45.0, optimizedF.angle.getDegrees(), kEpsilon));
 
     var angleG = Rotation2d.fromDegrees(0.0);
     var refG = new SwerveModuleVelocity(-2.0, Rotation2d.fromDegrees(45.0));
-    refG.cosineScale(angleG);
+    var optimizedG = refG.cosineScale(angleG);
 
     assertAll(
-        () -> assertEquals(-Math.sqrt(2.0), refG.velocity, kEpsilon),
-        () -> assertEquals(45.0, refG.angle.getDegrees(), kEpsilon));
+        () -> assertEquals(-Math.sqrt(2.0), optimizedG.velocity, kEpsilon),
+        () -> assertEquals(45.0, optimizedG.angle.getDegrees(), kEpsilon));
 
     var angleH = Rotation2d.fromDegrees(45.0);
     var refH = new SwerveModuleVelocity(-2.0, Rotation2d.fromDegrees(45.0));
-    refH.cosineScale(angleH);
+    var optimizedH = refH.cosineScale(angleH);
 
     assertAll(
-        () -> assertEquals(-2.0, refH.velocity, kEpsilon),
-        () -> assertEquals(45.0, refH.angle.getDegrees(), kEpsilon));
+        () -> assertEquals(-2.0, optimizedH.velocity, kEpsilon),
+        () -> assertEquals(45.0, optimizedH.angle.getDegrees(), kEpsilon));
 
     var angleI = Rotation2d.fromDegrees(-45.0);
     var refI = new SwerveModuleVelocity(-2.0, Rotation2d.fromDegrees(45.0));
-    refI.cosineScale(angleI);
+    var optimizedI = refI.cosineScale(angleI);
 
     assertAll(
-        () -> assertEquals(0.0, refI.velocity, kEpsilon),
-        () -> assertEquals(45.0, refI.angle.getDegrees(), kEpsilon));
+        () -> assertEquals(0.0, optimizedI.velocity, kEpsilon),
+        () -> assertEquals(45.0, optimizedI.angle.getDegrees(), kEpsilon));
 
     var angleJ = Rotation2d.fromDegrees(135.0);
     var refJ = new SwerveModuleVelocity(-2.0, Rotation2d.fromDegrees(45.0));
-    refJ.cosineScale(angleJ);
+    var optimizedJ = refJ.cosineScale(angleJ);
 
     assertAll(
-        () -> assertEquals(0.0, refJ.velocity, kEpsilon),
-        () -> assertEquals(45.0, refJ.angle.getDegrees(), kEpsilon));
+        () -> assertEquals(0.0, optimizedJ.velocity, kEpsilon),
+        () -> assertEquals(45.0, optimizedJ.angle.getDegrees(), kEpsilon));
 
     var angleK = Rotation2d.fromDegrees(-135.0);
     var refK = new SwerveModuleVelocity(-2.0, Rotation2d.fromDegrees(45.0));
-    refK.cosineScale(angleK);
+    var optimizedK = refK.cosineScale(angleK);
 
     assertAll(
-        () -> assertEquals(2.0, refK.velocity, kEpsilon),
-        () -> assertEquals(45.0, refK.angle.getDegrees(), kEpsilon));
+        () -> assertEquals(2.0, optimizedK.velocity, kEpsilon),
+        () -> assertEquals(45.0, optimizedK.angle.getDegrees(), kEpsilon));
 
     var angleL = Rotation2d.fromDegrees(180.0);
     var refL = new SwerveModuleVelocity(-2.0, Rotation2d.fromDegrees(45.0));
-    refL.cosineScale(angleL);
+    var optimizedL = refL.cosineScale(angleL);
 
     assertAll(
-        () -> assertEquals(Math.sqrt(2.0), refL.velocity, kEpsilon),
-        () -> assertEquals(45.0, refL.angle.getDegrees(), kEpsilon));
+        () -> assertEquals(Math.sqrt(2.0), optimizedL.velocity, kEpsilon),
+        () -> assertEquals(45.0, optimizedL.angle.getDegrees(), kEpsilon));
   }
 
   @Test
diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
index a9c1731bd2..ad800263f1 100644
--- a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
@@ -272,7 +272,7 @@ TEST_F(SwerveDriveKinematicsTest, Desaturate) {
   SwerveModuleVelocity state4{7.0_mps, 0_deg};
 
   wpi::util::array<SwerveModuleVelocity, 4> arr{state1, state2, state3, state4};
-  SwerveDriveKinematics<4>::DesaturateWheelVelocities(&arr, 5.5_mps);
+  arr = SwerveDriveKinematics<4>::DesaturateWheelVelocities(arr, 5.5_mps);
 
   double kFactor = 5.5 / 7.0;
 
@@ -289,8 +289,8 @@ TEST_F(SwerveDriveKinematicsTest, DesaturateSmooth) {
   SwerveModuleVelocity state4{7.0_mps, 0_deg};
 
   wpi::util::array<SwerveModuleVelocity, 4> arr{state1, state2, state3, state4};
-  SwerveDriveKinematics<4>::DesaturateWheelVelocities(
-      &arr, m_kinematics.ToChassisVelocities(arr), 5.5_mps, 5.5_mps,
+  arr = SwerveDriveKinematics<4>::DesaturateWheelVelocities(
+      arr, m_kinematics.ToChassisVelocities(arr), 5.5_mps, 5.5_mps,
       3.5_rad_per_s);
 
   double kFactor = 5.5 / 7.0;
@@ -308,7 +308,7 @@ TEST_F(SwerveDriveKinematicsTest, DesaturateNegativeVelocity) {
   SwerveModuleVelocity state4{-2.0_mps, 0_deg};
 
   wpi::util::array<SwerveModuleVelocity, 4> arr{state1, state2, state3, state4};
-  SwerveDriveKinematics<4>::DesaturateWheelVelocities(&arr, 1.0_mps);
+  arr = SwerveDriveKinematics<4>::DesaturateWheelVelocities(arr, 1.0_mps);
 
   EXPECT_NEAR(arr[0].velocity.value(), 0.5, kEpsilon);
   EXPECT_NEAR(arr[1].velocity.value(), 0.5, kEpsilon);
diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveModuleVelocityTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveModuleVelocityTest.cpp
index fc99b2f1f9..8c3a7b541f 100644
--- a/wpimath/src/test/native/cpp/kinematics/SwerveModuleVelocityTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/SwerveModuleVelocityTest.cpp
@@ -13,119 +13,119 @@ static constexpr double kEpsilon = 1E-9;
 TEST(SwerveModuleVelocityTest, Optimize) {
   wpi::math::Rotation2d angleA{45_deg};
   wpi::math::SwerveModuleVelocity refA{-2_mps, 180_deg};
-  refA.Optimize(angleA);
+  auto optimizedA = refA.Optimize(angleA);
 
-  EXPECT_NEAR(refA.velocity.value(), 2.0, kEpsilon);
-  EXPECT_NEAR(refA.angle.Degrees().value(), 0.0, kEpsilon);
+  EXPECT_NEAR(optimizedA.velocity.value(), 2.0, kEpsilon);
+  EXPECT_NEAR(optimizedA.angle.Degrees().value(), 0.0, kEpsilon);
 
   wpi::math::Rotation2d angleB{-50_deg};
   wpi::math::SwerveModuleVelocity refB{4.7_mps, 41_deg};
-  refB.Optimize(angleB);
+  auto optimizedB = refB.Optimize(angleB);
 
-  EXPECT_NEAR(refB.velocity.value(), -4.7, kEpsilon);
-  EXPECT_NEAR(refB.angle.Degrees().value(), -139.0, kEpsilon);
+  EXPECT_NEAR(optimizedB.velocity.value(), -4.7, kEpsilon);
+  EXPECT_NEAR(optimizedB.angle.Degrees().value(), -139.0, kEpsilon);
 }
 
 TEST(SwerveModuleVelocityTest, NoOptimize) {
   wpi::math::Rotation2d angleA{0_deg};
   wpi::math::SwerveModuleVelocity refA{2_mps, 89_deg};
-  refA.Optimize(angleA);
+  auto optimizedA = refA.Optimize(angleA);
 
-  EXPECT_NEAR(refA.velocity.value(), 2.0, kEpsilon);
-  EXPECT_NEAR(refA.angle.Degrees().value(), 89.0, kEpsilon);
+  EXPECT_NEAR(optimizedA.velocity.value(), 2.0, kEpsilon);
+  EXPECT_NEAR(optimizedA.angle.Degrees().value(), 89.0, kEpsilon);
 
   wpi::math::Rotation2d angleB{0_deg};
   wpi::math::SwerveModuleVelocity refB{-2_mps, -2_deg};
-  refB.Optimize(angleB);
+  auto optimizedB = refB.Optimize(angleB);
 
-  EXPECT_NEAR(refB.velocity.value(), -2.0, kEpsilon);
-  EXPECT_NEAR(refB.angle.Degrees().value(), -2.0, kEpsilon);
+  EXPECT_NEAR(optimizedB.velocity.value(), -2.0, kEpsilon);
+  EXPECT_NEAR(optimizedB.angle.Degrees().value(), -2.0, kEpsilon);
 }
 
 TEST(SwerveModuleVelocityTest, CosineScaling) {
   wpi::math::Rotation2d angleA{0_deg};
   wpi::math::SwerveModuleVelocity refA{2_mps, 45_deg};
-  refA.CosineScale(angleA);
+  auto optimizedA = refA.CosineScale(angleA);
 
-  EXPECT_NEAR(refA.velocity.value(), std::sqrt(2.0), kEpsilon);
-  EXPECT_NEAR(refA.angle.Degrees().value(), 45.0, kEpsilon);
+  EXPECT_NEAR(optimizedA.velocity.value(), std::sqrt(2.0), kEpsilon);
+  EXPECT_NEAR(optimizedA.angle.Degrees().value(), 45.0, kEpsilon);
 
   wpi::math::Rotation2d angleB{45_deg};
   wpi::math::SwerveModuleVelocity refB{2_mps, 45_deg};
-  refB.CosineScale(angleB);
+  auto optimizedB = refB.CosineScale(angleB);
 
-  EXPECT_NEAR(refB.velocity.value(), 2.0, kEpsilon);
-  EXPECT_NEAR(refB.angle.Degrees().value(), 45.0, kEpsilon);
+  EXPECT_NEAR(optimizedB.velocity.value(), 2.0, kEpsilon);
+  EXPECT_NEAR(optimizedB.angle.Degrees().value(), 45.0, kEpsilon);
 
   wpi::math::Rotation2d angleC{-45_deg};
   wpi::math::SwerveModuleVelocity refC{2_mps, 45_deg};
-  refC.CosineScale(angleC);
+  auto optimizedC = refC.CosineScale(angleC);
 
-  EXPECT_NEAR(refC.velocity.value(), 0.0, kEpsilon);
-  EXPECT_NEAR(refC.angle.Degrees().value(), 45.0, kEpsilon);
+  EXPECT_NEAR(optimizedC.velocity.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(optimizedC.angle.Degrees().value(), 45.0, kEpsilon);
 
   wpi::math::Rotation2d angleD{135_deg};
   wpi::math::SwerveModuleVelocity refD{2_mps, 45_deg};
-  refD.CosineScale(angleD);
+  auto optimizedD = refD.CosineScale(angleD);
 
-  EXPECT_NEAR(refD.velocity.value(), 0.0, kEpsilon);
-  EXPECT_NEAR(refD.angle.Degrees().value(), 45.0, kEpsilon);
+  EXPECT_NEAR(optimizedD.velocity.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(optimizedD.angle.Degrees().value(), 45.0, kEpsilon);
 
   wpi::math::Rotation2d angleE{-135_deg};
   wpi::math::SwerveModuleVelocity refE{2_mps, 45_deg};
-  refE.CosineScale(angleE);
+  auto optimizedE = refE.CosineScale(angleE);
 
-  EXPECT_NEAR(refE.velocity.value(), -2.0, kEpsilon);
-  EXPECT_NEAR(refE.angle.Degrees().value(), 45.0, kEpsilon);
+  EXPECT_NEAR(optimizedE.velocity.value(), -2.0, kEpsilon);
+  EXPECT_NEAR(optimizedE.angle.Degrees().value(), 45.0, kEpsilon);
 
   wpi::math::Rotation2d angleF{180_deg};
   wpi::math::SwerveModuleVelocity refF{2_mps, 45_deg};
-  refF.CosineScale(angleF);
+  auto optimizedF = refF.CosineScale(angleF);
 
-  EXPECT_NEAR(refF.velocity.value(), -std::sqrt(2.0), kEpsilon);
-  EXPECT_NEAR(refF.angle.Degrees().value(), 45.0, kEpsilon);
+  EXPECT_NEAR(optimizedF.velocity.value(), -std::sqrt(2.0), kEpsilon);
+  EXPECT_NEAR(optimizedF.angle.Degrees().value(), 45.0, kEpsilon);
 
   wpi::math::Rotation2d angleG{0_deg};
   wpi::math::SwerveModuleVelocity refG{-2_mps, 45_deg};
-  refG.CosineScale(angleG);
+  auto optimizedG = refG.CosineScale(angleG);
 
-  EXPECT_NEAR(refG.velocity.value(), -std::sqrt(2.0), kEpsilon);
-  EXPECT_NEAR(refG.angle.Degrees().value(), 45.0, kEpsilon);
+  EXPECT_NEAR(optimizedG.velocity.value(), -std::sqrt(2.0), kEpsilon);
+  EXPECT_NEAR(optimizedG.angle.Degrees().value(), 45.0, kEpsilon);
 
   wpi::math::Rotation2d angleH{45_deg};
   wpi::math::SwerveModuleVelocity refH{-2_mps, 45_deg};
-  refH.CosineScale(angleH);
+  auto optimizedH = refH.CosineScale(angleH);
 
-  EXPECT_NEAR(refH.velocity.value(), -2.0, kEpsilon);
-  EXPECT_NEAR(refH.angle.Degrees().value(), 45.0, kEpsilon);
+  EXPECT_NEAR(optimizedH.velocity.value(), -2.0, kEpsilon);
+  EXPECT_NEAR(optimizedH.angle.Degrees().value(), 45.0, kEpsilon);
 
   wpi::math::Rotation2d angleI{-45_deg};
   wpi::math::SwerveModuleVelocity refI{-2_mps, 45_deg};
-  refI.CosineScale(angleI);
+  auto optimizedI = refI.CosineScale(angleI);
 
-  EXPECT_NEAR(refI.velocity.value(), 0.0, kEpsilon);
-  EXPECT_NEAR(refI.angle.Degrees().value(), 45.0, kEpsilon);
+  EXPECT_NEAR(optimizedI.velocity.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(optimizedI.angle.Degrees().value(), 45.0, kEpsilon);
 
   wpi::math::Rotation2d angleJ{135_deg};
   wpi::math::SwerveModuleVelocity refJ{-2_mps, 45_deg};
-  refJ.CosineScale(angleJ);
+  auto optimizedJ = refJ.CosineScale(angleJ);
 
-  EXPECT_NEAR(refJ.velocity.value(), 0.0, kEpsilon);
-  EXPECT_NEAR(refJ.angle.Degrees().value(), 45.0, kEpsilon);
+  EXPECT_NEAR(optimizedJ.velocity.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(optimizedJ.angle.Degrees().value(), 45.0, kEpsilon);
 
   wpi::math::Rotation2d angleK{-135_deg};
   wpi::math::SwerveModuleVelocity refK{-2_mps, 45_deg};
-  refK.CosineScale(angleK);
+  auto optimizedK = refK.CosineScale(angleK);
 
-  EXPECT_NEAR(refK.velocity.value(), 2.0, kEpsilon);
-  EXPECT_NEAR(refK.angle.Degrees().value(), 45.0, kEpsilon);
+  EXPECT_NEAR(optimizedK.velocity.value(), 2.0, kEpsilon);
+  EXPECT_NEAR(optimizedK.angle.Degrees().value(), 45.0, kEpsilon);
 
   wpi::math::Rotation2d angleL{180_deg};
   wpi::math::SwerveModuleVelocity refL{-2_mps, 45_deg};
-  refL.CosineScale(angleL);
+  auto optimizedL = refL.CosineScale(angleL);
 
-  EXPECT_NEAR(refL.velocity.value(), std::sqrt(2.0), kEpsilon);
-  EXPECT_NEAR(refL.angle.Degrees().value(), 45.0, kEpsilon);
+  EXPECT_NEAR(optimizedL.velocity.value(), std::sqrt(2.0), kEpsilon);
+  EXPECT_NEAR(optimizedL.angle.Degrees().value(), 45.0, kEpsilon);
 }
 
 TEST(SwerveModuleVelocityTest, Equality) {
diff --git a/wpimath/src/test/python/kinematics/test_swerve_module_velocity.py b/wpimath/src/test/python/kinematics/test_swerve_module_velocity.py
index 8b7b55323c..37daf7ba7b 100644
--- a/wpimath/src/test/python/kinematics/test_swerve_module_velocity.py
+++ b/wpimath/src/test/python/kinematics/test_swerve_module_velocity.py
@@ -9,119 +9,119 @@ kEpsilon = 1e-9
 def test_optimize():
     angle_a = Rotation2d.fromDegrees(45)
     ref_a = SwerveModuleVelocity(-2, Rotation2d.fromDegrees(180))
-    ref_a.optimize(angle_a)
+    optimized_a = ref_a.optimize(angle_a)
 
-    assert ref_a.velocity == pytest.approx(2.0, abs=kEpsilon)
-    assert ref_a.angle.degrees() == pytest.approx(0.0, abs=kEpsilon)
+    assert optimized_a.velocity == pytest.approx(2.0, abs=kEpsilon)
+    assert optimized_a.angle.degrees() == pytest.approx(0.0, abs=kEpsilon)
 
     angle_b = Rotation2d.fromDegrees(-50)
     ref_b = SwerveModuleVelocity(4.7, Rotation2d.fromDegrees(41))
-    ref_b.optimize(angle_b)
+    optimized_b = ref_b.optimize(angle_b)
 
-    assert ref_b.velocity == pytest.approx(-4.7, abs=kEpsilon)
-    assert ref_b.angle.degrees() == pytest.approx(-139.0, abs=kEpsilon)
+    assert optimized_b.velocity == pytest.approx(-4.7, abs=kEpsilon)
+    assert optimized_b.angle.degrees() == pytest.approx(-139.0, abs=kEpsilon)
 
 
 def test_no_optimize():
     angle_a = Rotation2d.fromDegrees(0)
     ref_a = SwerveModuleVelocity(2, Rotation2d.fromDegrees(89))
-    ref_a.optimize(angle_a)
+    optimized_a = ref_a.optimize(angle_a)
 
-    assert ref_a.velocity == pytest.approx(2.0, abs=kEpsilon)
-    assert ref_a.angle.degrees() == pytest.approx(89.0, abs=kEpsilon)
+    assert optimized_a.velocity == pytest.approx(2.0, abs=kEpsilon)
+    assert optimized_a.angle.degrees() == pytest.approx(89.0, abs=kEpsilon)
 
     angle_b = Rotation2d.fromDegrees(0)
     ref_b = SwerveModuleVelocity(-2, Rotation2d.fromDegrees(-2))
-    ref_b.optimize(angle_b)
+    optimized_b = ref_b.optimize(angle_b)
 
-    assert ref_b.velocity == pytest.approx(-2.0, abs=kEpsilon)
-    assert ref_b.angle.degrees() == pytest.approx(-2.0, abs=kEpsilon)
+    assert optimized_b.velocity == pytest.approx(-2.0, abs=kEpsilon)
+    assert optimized_b.angle.degrees() == pytest.approx(-2.0, abs=kEpsilon)
 
 
 def test_cosine_scaling():
     angle_a = Rotation2d.fromDegrees(0)
     ref_a = SwerveModuleVelocity(2, Rotation2d.fromDegrees(45))
-    ref_a.cosineScale(angle_a)
+    optimized_a = ref_a.cosineScale(angle_a)
 
-    assert ref_a.velocity == pytest.approx(math.sqrt(2.0), abs=kEpsilon)
-    assert ref_a.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
+    assert optimized_a.velocity == pytest.approx(math.sqrt(2.0), abs=kEpsilon)
+    assert optimized_a.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
 
     angle_b = Rotation2d.fromDegrees(45)
     ref_b = SwerveModuleVelocity(2, Rotation2d.fromDegrees(45))
-    ref_b.cosineScale(angle_b)
+    optimized_b = ref_b.cosineScale(angle_b)
 
-    assert ref_b.velocity == pytest.approx(2.0, abs=kEpsilon)
-    assert ref_b.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
+    assert optimized_b.velocity == pytest.approx(2.0, abs=kEpsilon)
+    assert optimized_b.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
 
     angle_c = Rotation2d.fromDegrees(-45)
     ref_c = SwerveModuleVelocity(2, Rotation2d.fromDegrees(45))
-    ref_c.cosineScale(angle_c)
+    optimized_c = ref_c.cosineScale(angle_c)
 
-    assert ref_c.velocity == pytest.approx(0.0, abs=kEpsilon)
-    assert ref_c.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
+    assert optimized_c.velocity == pytest.approx(0.0, abs=kEpsilon)
+    assert optimized_c.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
 
     angle_d = Rotation2d.fromDegrees(135)
     ref_d = SwerveModuleVelocity(2, Rotation2d.fromDegrees(45))
-    ref_d.cosineScale(angle_d)
+    optimized_d = ref_d.cosineScale(angle_d)
 
-    assert ref_d.velocity == pytest.approx(0.0, abs=kEpsilon)
-    assert ref_d.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
+    assert optimized_d.velocity == pytest.approx(0.0, abs=kEpsilon)
+    assert optimized_d.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
 
     angle_e = Rotation2d.fromDegrees(-135)
     ref_e = SwerveModuleVelocity(2, Rotation2d.fromDegrees(45))
-    ref_e.cosineScale(angle_e)
+    optimized_e = ref_e.cosineScale(angle_e)
 
-    assert ref_e.velocity == pytest.approx(-2.0, abs=kEpsilon)
-    assert ref_e.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
+    assert optimized_e.velocity == pytest.approx(-2.0, abs=kEpsilon)
+    assert optimized_e.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
 
     angle_f = Rotation2d.fromDegrees(180)
     ref_f = SwerveModuleVelocity(2, Rotation2d.fromDegrees(45))
-    ref_f.cosineScale(angle_f)
+    optimized_f = ref_f.cosineScale(angle_f)
 
-    assert ref_f.velocity == pytest.approx(-math.sqrt(2.0), abs=kEpsilon)
-    assert ref_f.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
+    assert optimized_f.velocity == pytest.approx(-math.sqrt(2.0), abs=kEpsilon)
+    assert optimized_f.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
 
     angle_g = Rotation2d.fromDegrees(0)
     ref_g = SwerveModuleVelocity(-2, Rotation2d.fromDegrees(45))
-    ref_g.cosineScale(angle_g)
+    optimized_g = ref_g.cosineScale(angle_g)
 
-    assert ref_g.velocity == pytest.approx(-math.sqrt(2.0), abs=kEpsilon)
-    assert ref_g.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
+    assert optimized_g.velocity == pytest.approx(-math.sqrt(2.0), abs=kEpsilon)
+    assert optimized_g.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
 
     angle_h = Rotation2d.fromDegrees(45)
     ref_h = SwerveModuleVelocity(-2, Rotation2d.fromDegrees(45))
-    ref_h.cosineScale(angle_h)
+    optimized_h = ref_h.cosineScale(angle_h)
 
-    assert ref_h.velocity == pytest.approx(-2.0, abs=kEpsilon)
-    assert ref_h.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
+    assert optimized_h.velocity == pytest.approx(-2.0, abs=kEpsilon)
+    assert optimized_h.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
 
     angle_i = Rotation2d.fromDegrees(-45)
     ref_i = SwerveModuleVelocity(-2, Rotation2d.fromDegrees(45))
-    ref_i.cosineScale(angle_i)
+    optimized_i = ref_i.cosineScale(angle_i)
 
-    assert ref_i.velocity == pytest.approx(0.0, abs=kEpsilon)
-    assert ref_i.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
+    assert optimized_i.velocity == pytest.approx(0.0, abs=kEpsilon)
+    assert optimized_i.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
 
     angle_j = Rotation2d.fromDegrees(135)
     ref_j = SwerveModuleVelocity(-2, Rotation2d.fromDegrees(45))
-    ref_j.cosineScale(angle_j)
+    optimized_j = ref_j.cosineScale(angle_j)
 
-    assert ref_j.velocity == pytest.approx(0.0, abs=kEpsilon)
-    assert ref_j.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
+    assert optimized_j.velocity == pytest.approx(0.0, abs=kEpsilon)
+    assert optimized_j.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
 
     angle_k = Rotation2d.fromDegrees(-135)
     ref_k = SwerveModuleVelocity(-2, Rotation2d.fromDegrees(45))
-    ref_k.cosineScale(angle_k)
+    optimized_k = ref_k.cosineScale(angle_k)
 
-    assert ref_k.velocity == pytest.approx(2.0, abs=kEpsilon)
-    assert ref_k.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
+    assert optimized_k.velocity == pytest.approx(2.0, abs=kEpsilon)
+    assert optimized_k.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
 
     angle_l = Rotation2d.fromDegrees(180)
     ref_l = SwerveModuleVelocity(-2, Rotation2d.fromDegrees(45))
-    ref_l.cosineScale(angle_l)
+    optimized_l = ref_l.cosineScale(angle_l)
 
-    assert ref_l.velocity == pytest.approx(math.sqrt(2.0), abs=kEpsilon)
-    assert ref_l.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
+    assert optimized_l.velocity == pytest.approx(math.sqrt(2.0), abs=kEpsilon)
+    assert optimized_l.angle.degrees() == pytest.approx(45.0, abs=kEpsilon)
 
 
 def test_equality():