diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp
index 39d711b6229..bd6ca6524dd 100644
--- a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp
@@ -14,10 +14,9 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed,
   }
   chassisSpeeds = chassisSpeeds.Discretize(period);
 
-  auto states = m_kinematics.ToSwerveModuleStates(chassisSpeeds);
-  m_kinematics.DesaturateWheelSpeeds(&states, kMaxSpeed);
+  auto [fl, fr, bl, br] = m_kinematics.DesaturateWheelSpeeds(
+      m_kinematics.ToSwerveModuleStates(chassisSpeeds), kMaxSpeed);
 
-  auto [fl, fr, bl, br] = states;
   m_frontLeft.SetDesiredState(fl);
   m_frontRight.SetDesiredState(fr);
   m_backLeft.SetDesiredState(bl);
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp
index 7c902b88e69..f1276c3bc8f 100644
--- a/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp
@@ -50,25 +50,23 @@ void SwerveModule::SetDesiredState(frc::SwerveModuleState& referenceState) {
   frc::Rotation2d encoderRotation{
       units::radian_t{m_turningEncoder.GetDistance()}};
 
-  // Optimize the reference state to avoid spinning further than 90 degrees
-  referenceState.Optimize(encoderRotation);
-
-  // Scale speed by cosine of angle error. This scales down movement
+  // Optimize the reference state to avoid spinning further than 90 degrees,
+  // then scale speed 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.
-  referenceState.CosineScale(encoderRotation);
+  // Optimize the reference state to avoid spinning further than 90 degrees
+  auto state =
+      referenceState.Optimize(encoderRotation).CosineScale(encoderRotation);
 
   // Calculate the drive output from the drive PID controller.
   const auto driveOutput = m_drivePIDController.Calculate(
-      m_driveEncoder.GetRate(), referenceState.speed.value());
+      m_driveEncoder.GetRate(), state.speed.value());
 
-  const auto driveFeedforward =
-      m_driveFeedforward.Calculate(referenceState.speed);
+  const auto driveFeedforward = m_driveFeedforward.Calculate(state.speed);
 
   // Calculate the turning motor output from the turning PID controller.
   const auto turnOutput = m_turningPIDController.Calculate(
-      units::radian_t{m_turningEncoder.GetDistance()},
-      referenceState.angle.Radians());
+      units::radian_t{m_turningEncoder.GetDistance()}, state.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 be7f3592f57..50bd08eb908 100644
--- a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp
@@ -19,10 +19,9 @@ void Drivetrain::Drive(units::meters_per_second_t xSpeed,
   }
   chassisSpeeds = chassisSpeeds.Discretize(period);
 
-  auto states = m_kinematics.ToSwerveModuleStates(chassisSpeeds);
-  m_kinematics.DesaturateWheelSpeeds(&states, kMaxSpeed);
+  auto [fl, fr, bl, br] = m_kinematics.DesaturateWheelSpeeds(
+      m_kinematics.ToSwerveModuleStates(chassisSpeeds), kMaxSpeed);
 
-  auto [fl, fr, bl, br] = states;
   m_frontLeft.SetDesiredState(fl);
   m_frontRight.SetDesiredState(fr);
   m_backLeft.SetDesiredState(bl);
diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/SwerveModule.cpp b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/SwerveModule.cpp
index ae75e3af9d3..9519036c84d 100644
--- a/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/SwerveModule.cpp
+++ b/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/SwerveModule.cpp
@@ -50,25 +50,22 @@ void SwerveModule::SetDesiredState(frc::SwerveModuleState& referenceState) {
   frc::Rotation2d encoderRotation{
       units::radian_t{m_turningEncoder.GetDistance()}};
 
-  // Optimize the reference state to avoid spinning further than 90 degrees
-  referenceState.Optimize(encoderRotation);
-
-  // Scale speed by cosine of angle error. This scales down movement
+  // Optimize the reference state to avoid spinning further than 90 degrees,
+  // then scale speed 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.
-  referenceState.CosineScale(encoderRotation);
+  auto state =
+      referenceState.Optimize(encoderRotation).CosineScale(encoderRotation);
 
   // Calculate the drive output from the drive PID controller.
   const auto driveOutput = m_drivePIDController.Calculate(
-      m_driveEncoder.GetRate(), referenceState.speed.value());
+      m_driveEncoder.GetRate(), state.speed.value());
 
-  const auto driveFeedforward =
-      m_driveFeedforward.Calculate(referenceState.speed);
+  const auto driveFeedforward = m_driveFeedforward.Calculate(state.speed);
 
   // Calculate the turning motor output from the turning PID controller.
   const auto turnOutput = m_turningPIDController.Calculate(
-      units::radian_t{m_turningEncoder.GetDistance()},
-      referenceState.angle.Radians());
+      units::radian_t{m_turningEncoder.GetDistance()}, state.angle.Radians());
 
   const auto turnFeedforward = m_turnFeedforward.Calculate(
       m_turningPIDController.GetSetpoint().velocity);
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java
index 2cfbd938df1..ac965791d1c 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java
@@ -63,8 +63,9 @@ public void drive(
     }
     chassisSpeeds = chassisSpeeds.discretize(period);
 
-    var states = m_kinematics.toWheelSpeeds(chassisSpeeds);
-    SwerveDriveKinematics.desaturateWheelSpeeds(states, kMaxSpeed);
+    var states =
+        SwerveDriveKinematics.desaturateWheelSpeeds(
+            m_kinematics.toWheelSpeeds(chassisSpeeds), kMaxSpeed);
 
     m_frontLeft.setDesiredState(states[0]);
     m_frontRight.setDesiredState(states[1]);
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java
index 1590dfaa805..21ba12b7df6 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java
@@ -110,25 +110,22 @@ public SwerveModulePosition getPosition() {
   public void setDesiredState(SwerveModuleState desiredState) {
     var encoderRotation = new Rotation2d(m_turningEncoder.getDistance());
 
-    // Optimize the reference state to avoid spinning further than 90 degrees
-    desiredState.optimize(encoderRotation);
-
-    // Scale speed by cosine of angle error. This scales down movement perpendicular to the desired
+    // Optimize the reference state to avoid spinning further than 90 degrees, then
+    // scale speed 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.
-    desiredState.cosineScale(encoderRotation);
+    SwerveModuleState state = desiredState.optimize(encoderRotation).cosineScale(encoderRotation);
 
     // Calculate the drive output from the drive PID controller.
 
     final double driveOutput =
-        m_drivePIDController.calculate(m_driveEncoder.getRate(), desiredState.speed);
+        m_drivePIDController.calculate(m_driveEncoder.getRate(), state.speed);
 
-    final double driveFeedforward = m_driveFeedforward.calculate(desiredState.speed);
+    final double driveFeedforward = m_driveFeedforward.calculate(state.speed);
 
     // Calculate the turning motor output from the turning PID controller.
     final double turnOutput =
-        m_turningPIDController.calculate(
-            m_turningEncoder.getDistance(), desiredState.angle.getRadians());
+        m_turningPIDController.calculate(m_turningEncoder.getDistance(), state.angle.getRadians());
 
     final double turnFeedforward =
         m_turnFeedforward.calculate(m_turningPIDController.getSetpoint().velocity);
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java
index 0f155e6eede..b27025a5585 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java
@@ -74,8 +74,9 @@ public void drive(
 
     chassisSpeeds = chassisSpeeds.discretize(period);
 
-    var states = m_kinematics.toWheelSpeeds(chassisSpeeds);
-    SwerveDriveKinematics.desaturateWheelSpeeds(states, kMaxSpeed);
+    var states =
+        SwerveDriveKinematics.desaturateWheelSpeeds(
+            m_kinematics.toWheelSpeeds(chassisSpeeds), kMaxSpeed);
 
     m_frontLeft.setDesiredState(states[0]);
     m_frontRight.setDesiredState(states[1]);
diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java
index 45111f1275a..911f1d230dd 100644
--- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java
+++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java
@@ -110,24 +110,21 @@ public SwerveModulePosition getPosition() {
   public void setDesiredState(SwerveModuleState desiredState) {
     var encoderRotation = new Rotation2d(m_turningEncoder.getDistance());
 
-    // Optimize the reference state to avoid spinning further than 90 degrees
-    desiredState.optimize(encoderRotation);
-
-    // Scale speed by cosine of angle error. This scales down movement perpendicular to the desired
+    // Optimize the reference state to avoid spinning further than 90 degrees, then
+    // scale speed 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.
-    desiredState.cosineScale(encoderRotation);
+    SwerveModuleState state = desiredState.optimize(encoderRotation).cosineScale(encoderRotation);
 
     // Calculate the drive output from the drive PID controller.
     final double driveOutput =
-        m_drivePIDController.calculate(m_driveEncoder.getRate(), desiredState.speed);
+        m_drivePIDController.calculate(m_driveEncoder.getRate(), state.speed);
 
-    final double driveFeedforward = m_driveFeedforward.calculate(desiredState.speed);
+    final double driveFeedforward = m_driveFeedforward.calculate(state.speed);
 
     // Calculate the turning motor output from the turning PID controller.
     final double turnOutput =
-        m_turningPIDController.calculate(
-            m_turningEncoder.getDistance(), desiredState.angle.getRadians());
+        m_turningPIDController.calculate(m_turningEncoder.getDistance(), state.angle.getRadians());
 
     final double turnFeedforward =
         m_turnFeedforward.calculate(m_turningPIDController.getSetpoint().velocity);
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java
index 2e69777af85..3080544dd33 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java
@@ -261,21 +261,31 @@ public Twist2d toTwist2d(SwerveModulePosition[] start, SwerveModulePosition[] en
    * of rotational velocity, which makes discretizing the chassis speeds inaccurate because the
    * discretization did not account for this translational skew.
    *
-   * @param moduleStates Reference to array of module states. The array will be mutated with the
-   *     normalized speeds!
+   * @param moduleStates The array of module states.
    * @param attainableMaxSpeed The absolute max speed in meters per second that a module can reach.
+   * @return The array of desaturated module states.
    */
-  public static void desaturateWheelSpeeds(
+  public static SwerveModuleState[] desaturateWheelSpeeds(
       SwerveModuleState[] moduleStates, double attainableMaxSpeed) {
     double realMaxSpeed = 0;
     for (SwerveModuleState moduleState : moduleStates) {
       realMaxSpeed = Math.max(realMaxSpeed, Math.abs(moduleState.speed));
     }
+    var states = new SwerveModuleState[moduleStates.length];
     if (realMaxSpeed > attainableMaxSpeed) {
-      for (SwerveModuleState moduleState : moduleStates) {
-        moduleState.speed = moduleState.speed / realMaxSpeed * attainableMaxSpeed;
+      for (int i = 0; i < states.length; i++) {
+        states[i] =
+            new SwerveModuleState(
+                moduleStates[i].speed / realMaxSpeed * attainableMaxSpeed, moduleStates[i].angle);
+      }
+    } else {
+      // Copy in the event someone wants to mutate the desaturated states but also wants the
+      // original states
+      for (int i = 0; i < states.length; i++) {
+        states[i] = new SwerveModuleState(moduleStates[i].speed, moduleStates[i].angle);
       }
     }
+    return states;
   }
 
   /**
@@ -290,13 +300,13 @@ public static void desaturateWheelSpeeds(
    * of rotational velocity, which makes discretizing the chassis speeds inaccurate because the
    * discretization did not account for this translational skew.
    *
-   * @param moduleStates Reference to array of module states. The array will be mutated with the
-   *     normalized speeds!
+   * @param moduleStates The array of module states.
    * @param attainableMaxSpeed The absolute max speed in meters per second that a module can reach.
+   * @return The array of desaturated module states.
    */
-  public static void desaturateWheelSpeeds(
+  public static SwerveModuleState[] desaturateWheelSpeeds(
       SwerveModuleState[] moduleStates, LinearVelocity attainableMaxSpeed) {
-    desaturateWheelSpeeds(moduleStates, attainableMaxSpeed.in(MetersPerSecond));
+    return desaturateWheelSpeeds(moduleStates, attainableMaxSpeed.in(MetersPerSecond));
   }
 
   /**
@@ -312,8 +322,7 @@ public static void desaturateWheelSpeeds(
    * of rotational velocity, which makes discretizing the chassis speeds inaccurate because the
    * discretization did not account for this translational skew.
    *
-   * @param moduleStates Reference to array of module states. The array will be mutated with the
-   *     normalized speeds!
+   * @param moduleStates The array of module states
    * @param desiredChassisSpeed The desired speed of the robot
    * @param attainableMaxModuleSpeed The absolute max speed in meters per second that a module can
    *     reach
@@ -321,8 +330,9 @@ public static void desaturateWheelSpeeds(
    *     robot can reach while translating
    * @param attainableMaxRotationalVelocity The absolute max speed in radians per second the robot
    *     can reach while rotating
+   * @return The array of desaturated module states
    */
-  public static void desaturateWheelSpeeds(
+  public static SwerveModuleState[] desaturateWheelSpeeds(
       SwerveModuleState[] moduleStates,
       ChassisSpeeds desiredChassisSpeed,
       double attainableMaxModuleSpeed,
@@ -333,10 +343,16 @@ public static void desaturateWheelSpeeds(
       realMaxSpeed = Math.max(realMaxSpeed, Math.abs(moduleState.speed));
     }
 
+    var states = new SwerveModuleState[moduleStates.length];
     if (attainableMaxTranslationalSpeed == 0
         || attainableMaxRotationalVelocity == 0
         || realMaxSpeed == 0) {
-      return;
+      // Copy in the event someone wants to mutate the desaturated states but also wants the
+      // original states
+      for (int i = 0; i < states.length; i++) {
+        states[i] = new SwerveModuleState(moduleStates[i].speed, moduleStates[i].angle);
+      }
+      return states;
     }
     double translationalK =
         Math.hypot(desiredChassisSpeed.vx, desiredChassisSpeed.vy)
@@ -344,9 +360,10 @@ public static void desaturateWheelSpeeds(
     double rotationalK = Math.abs(desiredChassisSpeed.omega) / attainableMaxRotationalVelocity;
     double k = Math.max(translationalK, rotationalK);
     double scale = Math.min(k * attainableMaxModuleSpeed / realMaxSpeed, 1);
-    for (SwerveModuleState moduleState : moduleStates) {
-      moduleState.speed *= scale;
+    for (int i = 0; i < states.length; i++) {
+      states[i] = new SwerveModuleState(moduleStates[i].speed * scale, moduleStates[i].angle);
     }
+    return states;
   }
 
   /**
@@ -362,22 +379,22 @@ public static void desaturateWheelSpeeds(
    * of rotational velocity, which makes discretizing the chassis speeds inaccurate because the
    * discretization did not account for this translational skew.
    *
-   * @param moduleStates Reference to array of module states. The array will be mutated with the
-   *     normalized speeds!
+   * @param moduleStates The array of module states
    * @param desiredChassisSpeed The desired speed of the robot
    * @param attainableMaxModuleSpeed The absolute max speed that a module can reach
    * @param attainableMaxTranslationalSpeed The absolute max speed that your robot can reach while
    *     translating
    * @param attainableMaxRotationalVelocity The absolute max speed the robot can reach while
    *     rotating
+   * @return The array of desaturated module states
    */
-  public static void desaturateWheelSpeeds(
+  public static SwerveModuleState[] desaturateWheelSpeeds(
       SwerveModuleState[] moduleStates,
       ChassisSpeeds desiredChassisSpeed,
       LinearVelocity attainableMaxModuleSpeed,
       LinearVelocity attainableMaxTranslationalSpeed,
       AngularVelocity attainableMaxRotationalVelocity) {
-    desaturateWheelSpeeds(
+    return desaturateWheelSpeeds(
         moduleStates,
         desiredChassisSpeed,
         attainableMaxModuleSpeed.in(MetersPerSecond),
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java
index 8855f980bea..c2f50245c60 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java
@@ -88,34 +88,14 @@ public String toString() {
    * functionality, the furthest a wheel will ever rotate is 90 degrees.
    *
    * @param currentAngle The current module angle.
+   * @return The optimized module state.
    */
-  public void optimize(Rotation2d currentAngle) {
+  public SwerveModuleState optimize(Rotation2d currentAngle) {
     var delta = angle.minus(currentAngle);
     if (Math.abs(delta.getDegrees()) > 90.0) {
-      speed *= -1;
-      angle = angle.rotateBy(Rotation2d.kPi);
-    }
-  }
-
-  /**
-   * Minimize the change in heading the desired swerve module state 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 desiredState The desired state.
-   * @param currentAngle The current module angle.
-   * @return Optimized swerve module state.
-   * @deprecated Use the instance method instead.
-   */
-  @Deprecated
-  public static SwerveModuleState optimize(
-      SwerveModuleState desiredState, Rotation2d currentAngle) {
-    var delta = desiredState.angle.minus(currentAngle);
-    if (Math.abs(delta.getDegrees()) > 90.0) {
-      return new SwerveModuleState(
-          -desiredState.speed, desiredState.angle.rotateBy(Rotation2d.kPi));
+      return new SwerveModuleState(-speed, angle.rotateBy(Rotation2d.kPi));
     } else {
-      return new SwerveModuleState(desiredState.speed, desiredState.angle);
+      return new SwerveModuleState(speed, angle);
     }
   }
 
@@ -125,8 +105,9 @@ public static SwerveModuleState optimize(
    * driving.
    *
    * @param currentAngle The current module angle.
+   * @return The scaled module state.
    */
-  public void cosineScale(Rotation2d currentAngle) {
-    speed *= angle.minus(currentAngle).getCos();
+  public SwerveModuleState cosineScale(Rotation2d currentAngle) {
+    return new SwerveModuleState(speed * angle.minus(currentAngle).getCos(), angle);
   }
 }
diff --git a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/SwerveDriveKinematicsConstraint.java b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/SwerveDriveKinematicsConstraint.java
index 0f3867de9b9..ac30bff17e7 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/SwerveDriveKinematicsConstraint.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/SwerveDriveKinematicsConstraint.java
@@ -49,8 +49,9 @@ public double getMaxVelocity(Pose2d pose, double curvature, double velocity) {
     var chassisSpeeds = new ChassisSpeeds(xdVelocity, ydVelocity, velocity * curvature);
 
     // Get the wheel speeds and normalize them to within the max velocity.
-    var wheelSpeeds = m_kinematics.toSwerveModuleStates(chassisSpeeds);
-    SwerveDriveKinematics.desaturateWheelSpeeds(wheelSpeeds, m_maxSpeed);
+    var wheelSpeeds =
+        SwerveDriveKinematics.desaturateWheelSpeeds(
+            m_kinematics.toSwerveModuleStates(chassisSpeeds), m_maxSpeed);
 
     // Convert normalized wheel speeds back to chassis speeds
     var normSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
index 2aa2a885236..e97f7e6642f 100644
--- a/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
+++ b/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
@@ -144,7 +144,7 @@ class SwerveDriveKinematics
    * @return An array containing the module states. Use caution because these
    * module states are not normalized. Sometimes, a user input may cause one of
    * the module speeds to go above the attainable max velocity. Use the
-   * DesaturateWheelSpeeds(wpi::array<SwerveModuleState, NumModules>*,
+   * DesaturateWheelSpeeds(wpi::array<SwerveModuleState, NumModules>,
    * 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:
@@ -338,27 +338,31 @@ class SwerveDriveKinematics
    * chassis speeds inaccurate because the discretization did not account for
    * this translational skew.
    *
-   * @param moduleStates Reference to array of module states. The array will be
-   * mutated with the normalized speeds!
+   * @param moduleStates The array of module states.
    * @param attainableMaxSpeed The absolute max speed that a module can reach.
+   * @return The array of desaturated module states.
    */
-  static void DesaturateWheelSpeeds(
-      wpi::array<SwerveModuleState, NumModules>* moduleStates,
+  [[nodiscard]]
+  static wpi::array<SwerveModuleState, NumModules> DesaturateWheelSpeeds(
+      wpi::array<SwerveModuleState, NumModules> moduleStates,
       units::meters_per_second_t attainableMaxSpeed) {
-    auto& states = *moduleStates;
-    auto realMaxSpeed =
-        units::math::abs(std::max_element(states.begin(), states.end(),
-                                          [](const auto& a, const auto& b) {
-                                            return units::math::abs(a.speed) <
-                                                   units::math::abs(b.speed);
-                                          })
-                             ->speed);
+    auto realMaxSpeed = units::math::abs(
+        std::max_element(moduleStates.begin(), moduleStates.end(),
+                         [](const auto& a, const auto& b) {
+                           return units::math::abs(a.speed) <
+                                  units::math::abs(b.speed);
+                         })
+            ->speed);
 
     if (realMaxSpeed > attainableMaxSpeed) {
-      for (auto& module : states) {
-        module.speed = module.speed / realMaxSpeed * attainableMaxSpeed;
+      wpi::array<SwerveModuleState, NumModules> states(wpi::empty_array);
+      for (size_t i = 0; i < NumModules; ++i) {
+        states[i] = {moduleStates[i].speed / realMaxSpeed * attainableMaxSpeed,
+                     moduleStates[i].angle};
       }
+      return states;
     }
+    return moduleStates;
   }
 
   /**
@@ -378,35 +382,34 @@ class SwerveDriveKinematics
    * chassis speeds inaccurate because the discretization did not account for
    * this translational skew.
    *
-   * @param moduleStates Reference to array of module states. The array will be
-   * mutated with the normalized speeds!
+   * @param moduleStates The array of module states.
    * @param desiredChassisSpeed The desired speed of the robot
    * @param attainableMaxModuleSpeed The absolute max speed a module can reach
    * @param attainableMaxRobotTranslationSpeed The absolute max speed the robot
    * can reach while translating
    * @param attainableMaxRobotRotationSpeed The absolute max speed the robot can
    * reach while rotating
+   * @return The array of desaturated module states
    */
-  static void DesaturateWheelSpeeds(
-      wpi::array<SwerveModuleState, NumModules>* moduleStates,
+  [[nodiscard]]
+  static wpi::array<SwerveModuleState, NumModules> DesaturateWheelSpeeds(
+      wpi::array<SwerveModuleState, NumModules> moduleStates,
       ChassisSpeeds desiredChassisSpeed,
       units::meters_per_second_t attainableMaxModuleSpeed,
       units::meters_per_second_t attainableMaxRobotTranslationSpeed,
       units::radians_per_second_t attainableMaxRobotRotationSpeed) {
-    auto& states = *moduleStates;
-
-    auto realMaxSpeed =
-        units::math::abs(std::max_element(states.begin(), states.end(),
-                                          [](const auto& a, const auto& b) {
-                                            return units::math::abs(a.speed) <
-                                                   units::math::abs(b.speed);
-                                          })
-                             ->speed);
+    auto realMaxSpeed = units::math::abs(
+        std::max_element(moduleStates.begin(), moduleStates.end(),
+                         [](const auto& a, const auto& b) {
+                           return units::math::abs(a.speed) <
+                                  units::math::abs(b.speed);
+                         })
+            ->speed);
 
     if (attainableMaxRobotTranslationSpeed == 0_mps ||
         attainableMaxRobotRotationSpeed == 0_rad_per_s ||
         realMaxSpeed == 0_mps) {
-      return;
+      return moduleStates;
     }
 
     auto translationalK =
@@ -420,9 +423,11 @@ class SwerveDriveKinematics
 
     auto scale = units::math::min(k * attainableMaxModuleSpeed / realMaxSpeed,
                                   units::scalar_t{1});
-    for (auto& module : states) {
-      module.speed = module.speed * scale;
+    wpi::array<SwerveModuleState, NumModules> states(wpi::empty_array);
+    for (size_t i = 0; i < NumModules; ++i) {
+      states[i] = {moduleStates[i].speed * scale, moduleStates[i].angle};
     }
+    return states;
   }
 
   wpi::array<SwerveModulePosition, NumModules> Interpolate(
diff --git a/wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h b/wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h
index 9c005c24797..bbc378c6a6d 100644
--- a/wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h
+++ b/wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h
@@ -44,32 +44,15 @@ struct WPILIB_DLLEXPORT SwerveModuleState {
    * furthest a wheel will ever rotate is 90 degrees.
    *
    * @param currentAngle The current module angle.
+   * @return The optimized module state.
    */
-  constexpr void Optimize(const Rotation2d& currentAngle) {
+  [[nodiscard]]
+  constexpr SwerveModuleState Optimize(const Rotation2d& currentAngle) {
     auto delta = angle - currentAngle;
     if (units::math::abs(delta.Degrees()) > 90_deg) {
-      speed *= -1;
-      angle = angle + Rotation2d{180_deg};
-    }
-  }
-
-  /**
-   * Minimize the change in heading the desired swerve module state 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 desiredState The desired state.
-   * @param currentAngle The current module angle.
-   */
-  [[deprecated("Use instance method instead.")]]
-  constexpr static SwerveModuleState Optimize(
-      const SwerveModuleState& desiredState, const Rotation2d& currentAngle) {
-    auto delta = desiredState.angle - currentAngle;
-    if (units::math::abs(delta.Degrees()) > 90_deg) {
-      return {-desiredState.speed, desiredState.angle + Rotation2d{180_deg}};
+      return {-speed, angle + Rotation2d{180_deg}};
     } else {
-      return {desiredState.speed, desiredState.angle};
+      return {speed, angle};
     }
   }
 
@@ -79,9 +62,11 @@ struct WPILIB_DLLEXPORT SwerveModuleState {
    * modules change directions. This results in smoother driving.
    *
    * @param currentAngle The current module angle.
+   * @return The scaled module state.
    */
-  constexpr void CosineScale(const Rotation2d& currentAngle) {
-    speed *= (angle - currentAngle).Cos();
+  [[nodiscard]]
+  constexpr SwerveModuleState CosineScale(const Rotation2d& currentAngle) {
+    return {speed * (angle - currentAngle).Cos(), angle};
   }
 };
 }  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h b/wpimath/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h
index ac05bcfeb2f..83502e772dc 100644
--- a/wpimath/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h
+++ b/wpimath/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h
@@ -33,9 +33,9 @@ class SwerveDriveKinematicsConstraint : public TrajectoryConstraint {
     auto yVelocity = velocity * pose.Rotation().Sin();
     auto wheelSpeeds = m_kinematics.ToSwerveModuleStates(
         {xVelocity, yVelocity, velocity * curvature});
-    m_kinematics.DesaturateWheelSpeeds(&wheelSpeeds, m_maxSpeed);
 
-    auto normSpeeds = m_kinematics.ToChassisSpeeds(wheelSpeeds);
+    auto normSpeeds = m_kinematics.ToChassisSpeeds(
+        m_kinematics.DesaturateWheelSpeeds(wheelSpeeds, m_maxSpeed));
 
     return units::math::hypot(normSpeeds.vx, normSpeeds.vy);
   }
diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java
index fc3921f52f4..e6073da35eb 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java
@@ -359,15 +359,15 @@ void testDesaturate() {
     SwerveModuleState br = new SwerveModuleState(7, Rotation2d.kZero);
 
     SwerveModuleState[] arr = {fl, fr, bl, br};
-    SwerveDriveKinematics.desaturateWheelSpeeds(arr, 5.5);
+    var desaturatedArr = SwerveDriveKinematics.desaturateWheelSpeeds(arr, 5.5);
 
     double factor = 5.5 / 7.0;
 
     assertAll(
-        () -> assertEquals(5.0 * factor, arr[0].speed, kEpsilon),
-        () -> assertEquals(6.0 * factor, arr[1].speed, kEpsilon),
-        () -> assertEquals(4.0 * factor, arr[2].speed, kEpsilon),
-        () -> assertEquals(7.0 * factor, arr[3].speed, kEpsilon));
+        () -> assertEquals(5.0 * factor, desaturatedArr[0].speed, kEpsilon),
+        () -> assertEquals(6.0 * factor, desaturatedArr[1].speed, kEpsilon),
+        () -> assertEquals(4.0 * factor, desaturatedArr[2].speed, kEpsilon),
+        () -> assertEquals(7.0 * factor, desaturatedArr[3].speed, kEpsilon));
   }
 
   @Test
@@ -378,16 +378,17 @@ void testDesaturateSmooth() {
     SwerveModuleState br = new SwerveModuleState(7, Rotation2d.kZero);
 
     SwerveModuleState[] arr = {fl, fr, bl, br};
-    SwerveDriveKinematics.desaturateWheelSpeeds(
-        arr, m_kinematics.toChassisSpeeds(arr), 5.5, 5.5, 3.5);
+    var desaturatedArr =
+        SwerveDriveKinematics.desaturateWheelSpeeds(
+            arr, m_kinematics.toChassisSpeeds(arr), 5.5, 5.5, 3.5);
 
     double factor = 5.5 / 7.0;
 
     assertAll(
-        () -> assertEquals(5.0 * factor, arr[0].speed, kEpsilon),
-        () -> assertEquals(6.0 * factor, arr[1].speed, kEpsilon),
-        () -> assertEquals(4.0 * factor, arr[2].speed, kEpsilon),
-        () -> assertEquals(7.0 * factor, arr[3].speed, kEpsilon));
+        () -> assertEquals(5.0 * factor, desaturatedArr[0].speed, kEpsilon),
+        () -> assertEquals(6.0 * factor, desaturatedArr[1].speed, kEpsilon),
+        () -> assertEquals(4.0 * factor, desaturatedArr[2].speed, kEpsilon),
+        () -> assertEquals(7.0 * factor, desaturatedArr[3].speed, kEpsilon));
   }
 
   @Test
@@ -398,12 +399,12 @@ void testDesaturateNegativeSpeed() {
     SwerveModuleState br = new SwerveModuleState(-2, Rotation2d.kZero);
 
     SwerveModuleState[] arr = {fl, fr, bl, br};
-    SwerveDriveKinematics.desaturateWheelSpeeds(arr, 1);
+    var desaturatedArr = SwerveDriveKinematics.desaturateWheelSpeeds(arr, 1);
 
     assertAll(
-        () -> assertEquals(0.5, arr[0].speed, kEpsilon),
-        () -> assertEquals(0.5, arr[1].speed, kEpsilon),
-        () -> assertEquals(-1.0, arr[2].speed, kEpsilon),
-        () -> assertEquals(-1.0, arr[3].speed, kEpsilon));
+        () -> assertEquals(0.5, desaturatedArr[0].speed, kEpsilon),
+        () -> assertEquals(0.5, desaturatedArr[1].speed, kEpsilon),
+        () -> assertEquals(-1.0, desaturatedArr[2].speed, kEpsilon),
+        () -> assertEquals(-1.0, desaturatedArr[3].speed, kEpsilon));
   }
 }
diff --git a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveModuleStateTest.java b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveModuleStateTest.java
index fdccdccb08b..6341864e6bc 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveModuleStateTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveModuleStateTest.java
@@ -17,136 +17,136 @@ class SwerveModuleStateTest {
   void testOptimize() {
     var angleA = Rotation2d.fromDegrees(45);
     var refA = new SwerveModuleState(-2.0, Rotation2d.kPi);
-    refA.optimize(angleA);
+    var optimizedA = refA.optimize(angleA);
 
     assertAll(
-        () -> assertEquals(2.0, refA.speed, kEpsilon),
-        () -> assertEquals(0.0, refA.angle.getDegrees(), kEpsilon));
+        () -> assertEquals(2.0, optimizedA.speed, kEpsilon),
+        () -> assertEquals(0.0, optimizedA.angle.getDegrees(), kEpsilon));
 
     var angleB = Rotation2d.fromDegrees(-50);
     var refB = new SwerveModuleState(4.7, Rotation2d.fromDegrees(41));
-    refB.optimize(angleB);
+    var optimizedB = refB.optimize(angleB);
 
     assertAll(
-        () -> assertEquals(-4.7, refB.speed, kEpsilon),
-        () -> assertEquals(-139.0, refB.angle.getDegrees(), kEpsilon));
+        () -> assertEquals(-4.7, optimizedB.speed, kEpsilon),
+        () -> assertEquals(-139.0, optimizedB.angle.getDegrees(), kEpsilon));
   }
 
   @Test
   void testNoOptimize() {
     var angleA = Rotation2d.kZero;
     var refA = new SwerveModuleState(2.0, Rotation2d.fromDegrees(89));
-    refA.optimize(angleA);
+    var optimizedA = refA.optimize(angleA);
 
     assertAll(
-        () -> assertEquals(2.0, refA.speed, kEpsilon),
-        () -> assertEquals(89.0, refA.angle.getDegrees(), kEpsilon));
+        () -> assertEquals(2.0, optimizedA.speed, kEpsilon),
+        () -> assertEquals(89.0, optimizedA.angle.getDegrees(), kEpsilon));
 
     var angleB = Rotation2d.kZero;
     var refB = new SwerveModuleState(-2.0, Rotation2d.fromDegrees(-2));
-    refB.optimize(angleB);
+    var optimizedB = refB.optimize(angleB);
 
     assertAll(
-        () -> assertEquals(-2.0, refB.speed, kEpsilon),
-        () -> assertEquals(-2.0, refB.angle.getDegrees(), kEpsilon));
+        () -> assertEquals(-2.0, optimizedB.speed, kEpsilon),
+        () -> assertEquals(-2.0, optimizedB.angle.getDegrees(), kEpsilon));
   }
 
   @Test
   void testCosineScale() {
     var angleA = Rotation2d.fromDegrees(0.0);
     var refA = new SwerveModuleState(2.0, Rotation2d.fromDegrees(45.0));
-    refA.cosineScale(angleA);
+    var optimizedA = refA.cosineScale(angleA);
 
     assertAll(
-        () -> assertEquals(Math.sqrt(2.0), refA.speed, kEpsilon),
-        () -> assertEquals(45.0, refA.angle.getDegrees(), kEpsilon));
+        () -> assertEquals(Math.sqrt(2.0), optimizedA.speed, kEpsilon),
+        () -> assertEquals(45.0, optimizedA.angle.getDegrees(), kEpsilon));
 
     var angleB = Rotation2d.fromDegrees(45.0);
     var refB = new SwerveModuleState(2.0, Rotation2d.fromDegrees(45.0));
-    refB.cosineScale(angleB);
+    var optimizedB = refB.cosineScale(angleB);
 
     assertAll(
-        () -> assertEquals(2.0, refB.speed, kEpsilon),
-        () -> assertEquals(45.0, refB.angle.getDegrees(), kEpsilon));
+        () -> assertEquals(2.0, optimizedB.speed, kEpsilon),
+        () -> assertEquals(45.0, optimizedB.angle.getDegrees(), kEpsilon));
 
     var angleC = Rotation2d.fromDegrees(-45.0);
     var refC = new SwerveModuleState(2.0, Rotation2d.fromDegrees(45.0));
-    refC.cosineScale(angleC);
+    var optimizedC = refC.cosineScale(angleC);
 
     assertAll(
-        () -> assertEquals(0.0, refC.speed, kEpsilon),
-        () -> assertEquals(45.0, refC.angle.getDegrees(), kEpsilon));
+        () -> assertEquals(0.0, optimizedC.speed, kEpsilon),
+        () -> assertEquals(45.0, optimizedC.angle.getDegrees(), kEpsilon));
 
     var angleD = Rotation2d.fromDegrees(135.0);
     var refD = new SwerveModuleState(2.0, Rotation2d.fromDegrees(45.0));
-    refD.cosineScale(angleD);
+    var optimizedD = refD.cosineScale(angleD);
 
     assertAll(
-        () -> assertEquals(0.0, refD.speed, kEpsilon),
-        () -> assertEquals(45.0, refD.angle.getDegrees(), kEpsilon));
+        () -> assertEquals(0.0, optimizedD.speed, kEpsilon),
+        () -> assertEquals(45.0, optimizedD.angle.getDegrees(), kEpsilon));
 
     var angleE = Rotation2d.fromDegrees(-135.0);
     var refE = new SwerveModuleState(2.0, Rotation2d.fromDegrees(45.0));
-    refE.cosineScale(angleE);
+    var optimizedE = refE.cosineScale(angleE);
 
     assertAll(
-        () -> assertEquals(-2.0, refE.speed, kEpsilon),
-        () -> assertEquals(45.0, refE.angle.getDegrees(), kEpsilon));
+        () -> assertEquals(-2.0, optimizedE.speed, kEpsilon),
+        () -> assertEquals(45.0, optimizedE.angle.getDegrees(), kEpsilon));
 
     var angleF = Rotation2d.fromDegrees(180.0);
     var refF = new SwerveModuleState(2.0, Rotation2d.fromDegrees(45.0));
-    refF.cosineScale(angleF);
+    var optimizedF = refF.cosineScale(angleF);
 
     assertAll(
-        () -> assertEquals(-Math.sqrt(2.0), refF.speed, kEpsilon),
-        () -> assertEquals(45.0, refF.angle.getDegrees(), kEpsilon));
+        () -> assertEquals(-Math.sqrt(2.0), optimizedF.speed, kEpsilon),
+        () -> assertEquals(45.0, optimizedF.angle.getDegrees(), kEpsilon));
 
     var angleG = Rotation2d.fromDegrees(0.0);
     var refG = new SwerveModuleState(-2.0, Rotation2d.fromDegrees(45.0));
-    refG.cosineScale(angleG);
+    var optimizedG = refG.cosineScale(angleG);
 
     assertAll(
-        () -> assertEquals(-Math.sqrt(2.0), refG.speed, kEpsilon),
-        () -> assertEquals(45.0, refG.angle.getDegrees(), kEpsilon));
+        () -> assertEquals(-Math.sqrt(2.0), optimizedG.speed, kEpsilon),
+        () -> assertEquals(45.0, optimizedG.angle.getDegrees(), kEpsilon));
 
     var angleH = Rotation2d.fromDegrees(45.0);
     var refH = new SwerveModuleState(-2.0, Rotation2d.fromDegrees(45.0));
-    refH.cosineScale(angleH);
+    var optimizedH = refH.cosineScale(angleH);
 
     assertAll(
-        () -> assertEquals(-2.0, refH.speed, kEpsilon),
-        () -> assertEquals(45.0, refH.angle.getDegrees(), kEpsilon));
+        () -> assertEquals(-2.0, optimizedH.speed, kEpsilon),
+        () -> assertEquals(45.0, optimizedH.angle.getDegrees(), kEpsilon));
 
     var angleI = Rotation2d.fromDegrees(-45.0);
     var refI = new SwerveModuleState(-2.0, Rotation2d.fromDegrees(45.0));
-    refI.cosineScale(angleI);
+    var optimizedI = refI.cosineScale(angleI);
 
     assertAll(
-        () -> assertEquals(0.0, refI.speed, kEpsilon),
-        () -> assertEquals(45.0, refI.angle.getDegrees(), kEpsilon));
+        () -> assertEquals(0.0, optimizedI.speed, kEpsilon),
+        () -> assertEquals(45.0, optimizedI.angle.getDegrees(), kEpsilon));
 
     var angleJ = Rotation2d.fromDegrees(135.0);
     var refJ = new SwerveModuleState(-2.0, Rotation2d.fromDegrees(45.0));
-    refJ.cosineScale(angleJ);
+    var optimizedJ = refJ.cosineScale(angleJ);
 
     assertAll(
-        () -> assertEquals(0.0, refJ.speed, kEpsilon),
-        () -> assertEquals(45.0, refJ.angle.getDegrees(), kEpsilon));
+        () -> assertEquals(0.0, optimizedJ.speed, kEpsilon),
+        () -> assertEquals(45.0, optimizedJ.angle.getDegrees(), kEpsilon));
 
     var angleK = Rotation2d.fromDegrees(-135.0);
     var refK = new SwerveModuleState(-2.0, Rotation2d.fromDegrees(45.0));
-    refK.cosineScale(angleK);
+    var optimizedK = refK.cosineScale(angleK);
 
     assertAll(
-        () -> assertEquals(2.0, refK.speed, kEpsilon),
-        () -> assertEquals(45.0, refK.angle.getDegrees(), kEpsilon));
+        () -> assertEquals(2.0, optimizedK.speed, kEpsilon),
+        () -> assertEquals(45.0, optimizedK.angle.getDegrees(), kEpsilon));
 
     var angleL = Rotation2d.fromDegrees(180.0);
     var refL = new SwerveModuleState(-2.0, Rotation2d.fromDegrees(45.0));
-    refL.cosineScale(angleL);
+    var optimizedL = refL.cosineScale(angleL);
 
     assertAll(
-        () -> assertEquals(Math.sqrt(2.0), refL.speed, kEpsilon),
-        () -> assertEquals(45.0, refL.angle.getDegrees(), kEpsilon));
+        () -> assertEquals(Math.sqrt(2.0), optimizedL.speed, kEpsilon),
+        () -> assertEquals(45.0, optimizedL.angle.getDegrees(), kEpsilon));
   }
 }
diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
index a851e562c22..4e68840ddc7 100644
--- a/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
@@ -267,7 +267,7 @@ TEST_F(SwerveDriveKinematicsTest, Desaturate) {
   SwerveModuleState state4{7.0_mps, 0_deg};
 
   wpi::array<SwerveModuleState, 4> arr{state1, state2, state3, state4};
-  SwerveDriveKinematics<4>::DesaturateWheelSpeeds(&arr, 5.5_mps);
+  arr = SwerveDriveKinematics<4>::DesaturateWheelSpeeds(arr, 5.5_mps);
 
   double kFactor = 5.5 / 7.0;
 
@@ -284,8 +284,8 @@ TEST_F(SwerveDriveKinematicsTest, DesaturateSmooth) {
   SwerveModuleState state4{7.0_mps, 0_deg};
 
   wpi::array<SwerveModuleState, 4> arr{state1, state2, state3, state4};
-  SwerveDriveKinematics<4>::DesaturateWheelSpeeds(
-      &arr, m_kinematics.ToChassisSpeeds(arr), 5.5_mps, 5.5_mps, 3.5_rad_per_s);
+  arr = SwerveDriveKinematics<4>::DesaturateWheelSpeeds(
+      arr, m_kinematics.ToChassisSpeeds(arr), 5.5_mps, 5.5_mps, 3.5_rad_per_s);
 
   double kFactor = 5.5 / 7.0;
 
@@ -302,7 +302,7 @@ TEST_F(SwerveDriveKinematicsTest, DesaturateNegativeSpeed) {
   SwerveModuleState state4{-2.0_mps, 0_deg};
 
   wpi::array<SwerveModuleState, 4> arr{state1, state2, state3, state4};
-  SwerveDriveKinematics<4>::DesaturateWheelSpeeds(&arr, 1.0_mps);
+  arr = SwerveDriveKinematics<4>::DesaturateWheelSpeeds(arr, 1.0_mps);
 
   EXPECT_NEAR(arr[0].speed.value(), 0.5, kEpsilon);
   EXPECT_NEAR(arr[1].speed.value(), 0.5, kEpsilon);
diff --git a/wpimath/src/test/native/cpp/kinematics/SwerveModuleStateTest.cpp b/wpimath/src/test/native/cpp/kinematics/SwerveModuleStateTest.cpp
index 9d13eeca4e3..abbf93f7aea 100644
--- a/wpimath/src/test/native/cpp/kinematics/SwerveModuleStateTest.cpp
+++ b/wpimath/src/test/native/cpp/kinematics/SwerveModuleStateTest.cpp
@@ -12,119 +12,119 @@ static constexpr double kEpsilon = 1E-9;
 TEST(SwerveModuleStateTest, Optimize) {
   frc::Rotation2d angleA{45_deg};
   frc::SwerveModuleState refA{-2_mps, 180_deg};
-  refA.Optimize(angleA);
+  auto optimizedA = refA.Optimize(angleA);
 
-  EXPECT_NEAR(refA.speed.value(), 2.0, kEpsilon);
-  EXPECT_NEAR(refA.angle.Degrees().value(), 0.0, kEpsilon);
+  EXPECT_NEAR(optimizedA.speed.value(), 2.0, kEpsilon);
+  EXPECT_NEAR(optimizedA.angle.Degrees().value(), 0.0, kEpsilon);
 
   frc::Rotation2d angleB{-50_deg};
   frc::SwerveModuleState refB{4.7_mps, 41_deg};
-  refB.Optimize(angleB);
+  auto optimizedB = refB.Optimize(angleB);
 
-  EXPECT_NEAR(refB.speed.value(), -4.7, kEpsilon);
-  EXPECT_NEAR(refB.angle.Degrees().value(), -139.0, kEpsilon);
+  EXPECT_NEAR(optimizedB.speed.value(), -4.7, kEpsilon);
+  EXPECT_NEAR(optimizedB.angle.Degrees().value(), -139.0, kEpsilon);
 }
 
 TEST(SwerveModuleStateTest, NoOptimize) {
   frc::Rotation2d angleA{0_deg};
   frc::SwerveModuleState refA{2_mps, 89_deg};
-  refA.Optimize(angleA);
+  auto optimizedA = refA.Optimize(angleA);
 
-  EXPECT_NEAR(refA.speed.value(), 2.0, kEpsilon);
-  EXPECT_NEAR(refA.angle.Degrees().value(), 89.0, kEpsilon);
+  EXPECT_NEAR(optimizedA.speed.value(), 2.0, kEpsilon);
+  EXPECT_NEAR(optimizedA.angle.Degrees().value(), 89.0, kEpsilon);
 
   frc::Rotation2d angleB{0_deg};
   frc::SwerveModuleState refB{-2_mps, -2_deg};
-  refB.Optimize(angleB);
+  auto optimizedB = refB.Optimize(angleB);
 
-  EXPECT_NEAR(refB.speed.value(), -2.0, kEpsilon);
-  EXPECT_NEAR(refB.angle.Degrees().value(), -2.0, kEpsilon);
+  EXPECT_NEAR(optimizedB.speed.value(), -2.0, kEpsilon);
+  EXPECT_NEAR(optimizedB.angle.Degrees().value(), -2.0, kEpsilon);
 }
 
 TEST(SwerveModuleStateTest, CosineScaling) {
   frc::Rotation2d angleA{0_deg};
   frc::SwerveModuleState refA{2_mps, 45_deg};
-  refA.CosineScale(angleA);
+  auto optimizedA = refA.CosineScale(angleA);
 
-  EXPECT_NEAR(refA.speed.value(), std::sqrt(2.0), kEpsilon);
-  EXPECT_NEAR(refA.angle.Degrees().value(), 45.0, kEpsilon);
+  EXPECT_NEAR(optimizedA.speed.value(), std::sqrt(2.0), kEpsilon);
+  EXPECT_NEAR(optimizedA.angle.Degrees().value(), 45.0, kEpsilon);
 
   frc::Rotation2d angleB{45_deg};
   frc::SwerveModuleState refB{2_mps, 45_deg};
-  refB.CosineScale(angleB);
+  auto optimizedB = refB.CosineScale(angleB);
 
-  EXPECT_NEAR(refB.speed.value(), 2.0, kEpsilon);
-  EXPECT_NEAR(refB.angle.Degrees().value(), 45.0, kEpsilon);
+  EXPECT_NEAR(optimizedB.speed.value(), 2.0, kEpsilon);
+  EXPECT_NEAR(optimizedB.angle.Degrees().value(), 45.0, kEpsilon);
 
   frc::Rotation2d angleC{-45_deg};
   frc::SwerveModuleState refC{2_mps, 45_deg};
-  refC.CosineScale(angleC);
+  auto optimizedC = refC.CosineScale(angleC);
 
-  EXPECT_NEAR(refC.speed.value(), 0.0, kEpsilon);
-  EXPECT_NEAR(refC.angle.Degrees().value(), 45.0, kEpsilon);
+  EXPECT_NEAR(optimizedC.speed.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(optimizedC.angle.Degrees().value(), 45.0, kEpsilon);
 
   frc::Rotation2d angleD{135_deg};
   frc::SwerveModuleState refD{2_mps, 45_deg};
-  refD.CosineScale(angleD);
+  auto optimizedD = refD.CosineScale(angleD);
 
-  EXPECT_NEAR(refD.speed.value(), 0.0, kEpsilon);
-  EXPECT_NEAR(refD.angle.Degrees().value(), 45.0, kEpsilon);
+  EXPECT_NEAR(optimizedD.speed.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(optimizedD.angle.Degrees().value(), 45.0, kEpsilon);
 
   frc::Rotation2d angleE{-135_deg};
   frc::SwerveModuleState refE{2_mps, 45_deg};
-  refE.CosineScale(angleE);
+  auto optimizedE = refE.CosineScale(angleE);
 
-  EXPECT_NEAR(refE.speed.value(), -2.0, kEpsilon);
-  EXPECT_NEAR(refE.angle.Degrees().value(), 45.0, kEpsilon);
+  EXPECT_NEAR(optimizedE.speed.value(), -2.0, kEpsilon);
+  EXPECT_NEAR(optimizedE.angle.Degrees().value(), 45.0, kEpsilon);
 
   frc::Rotation2d angleF{180_deg};
   frc::SwerveModuleState refF{2_mps, 45_deg};
-  refF.CosineScale(angleF);
+  auto optimizedF = refF.CosineScale(angleF);
 
-  EXPECT_NEAR(refF.speed.value(), -std::sqrt(2.0), kEpsilon);
-  EXPECT_NEAR(refF.angle.Degrees().value(), 45.0, kEpsilon);
+  EXPECT_NEAR(optimizedF.speed.value(), -std::sqrt(2.0), kEpsilon);
+  EXPECT_NEAR(optimizedF.angle.Degrees().value(), 45.0, kEpsilon);
 
   frc::Rotation2d angleG{0_deg};
   frc::SwerveModuleState refG{-2_mps, 45_deg};
-  refG.CosineScale(angleG);
+  auto optimizedG = refG.CosineScale(angleG);
 
-  EXPECT_NEAR(refG.speed.value(), -std::sqrt(2.0), kEpsilon);
-  EXPECT_NEAR(refG.angle.Degrees().value(), 45.0, kEpsilon);
+  EXPECT_NEAR(optimizedG.speed.value(), -std::sqrt(2.0), kEpsilon);
+  EXPECT_NEAR(optimizedG.angle.Degrees().value(), 45.0, kEpsilon);
 
   frc::Rotation2d angleH{45_deg};
   frc::SwerveModuleState refH{-2_mps, 45_deg};
-  refH.CosineScale(angleH);
+  auto optimizedH = refH.CosineScale(angleH);
 
-  EXPECT_NEAR(refH.speed.value(), -2.0, kEpsilon);
-  EXPECT_NEAR(refH.angle.Degrees().value(), 45.0, kEpsilon);
+  EXPECT_NEAR(optimizedH.speed.value(), -2.0, kEpsilon);
+  EXPECT_NEAR(optimizedH.angle.Degrees().value(), 45.0, kEpsilon);
 
   frc::Rotation2d angleI{-45_deg};
   frc::SwerveModuleState refI{-2_mps, 45_deg};
-  refI.CosineScale(angleI);
+  auto optimizedI = refI.CosineScale(angleI);
 
-  EXPECT_NEAR(refI.speed.value(), 0.0, kEpsilon);
-  EXPECT_NEAR(refI.angle.Degrees().value(), 45.0, kEpsilon);
+  EXPECT_NEAR(optimizedI.speed.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(optimizedI.angle.Degrees().value(), 45.0, kEpsilon);
 
   frc::Rotation2d angleJ{135_deg};
   frc::SwerveModuleState refJ{-2_mps, 45_deg};
-  refJ.CosineScale(angleJ);
+  auto optimizedJ = refJ.CosineScale(angleJ);
 
-  EXPECT_NEAR(refJ.speed.value(), 0.0, kEpsilon);
-  EXPECT_NEAR(refJ.angle.Degrees().value(), 45.0, kEpsilon);
+  EXPECT_NEAR(optimizedJ.speed.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(optimizedJ.angle.Degrees().value(), 45.0, kEpsilon);
 
   frc::Rotation2d angleK{-135_deg};
   frc::SwerveModuleState refK{-2_mps, 45_deg};
-  refK.CosineScale(angleK);
+  auto optimizedK = refK.CosineScale(angleK);
 
-  EXPECT_NEAR(refK.speed.value(), 2.0, kEpsilon);
-  EXPECT_NEAR(refK.angle.Degrees().value(), 45.0, kEpsilon);
+  EXPECT_NEAR(optimizedK.speed.value(), 2.0, kEpsilon);
+  EXPECT_NEAR(optimizedK.angle.Degrees().value(), 45.0, kEpsilon);
 
   frc::Rotation2d angleL{180_deg};
   frc::SwerveModuleState refL{-2_mps, 45_deg};
-  refL.CosineScale(angleL);
+  auto optimizedL = refL.CosineScale(angleL);
 
-  EXPECT_NEAR(refL.speed.value(), std::sqrt(2.0), kEpsilon);
-  EXPECT_NEAR(refL.angle.Degrees().value(), 45.0, kEpsilon);
+  EXPECT_NEAR(optimizedL.speed.value(), std::sqrt(2.0), kEpsilon);
+  EXPECT_NEAR(optimizedL.angle.Degrees().value(), 45.0, kEpsilon);
 }
 
 TEST(SwerveModuleStateTest, Equality) {