[wpimath] Add cosineScale method to SwerveModuleState and instance optimize

wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java

@@ -83,6 +83,32 @@ public String toString() {
         "SwerveModuleState(Speed: %.2f m/s, Angle: %s)", speedMetersPerSecond, angle);
   }
 
+  /**
+   * Minimize the change in heading this 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 currentAngle The current module angle.
+   */
+  public void optimize(Rotation2d currentAngle) {
+    var delta = angle.minus(currentAngle);
+    if (Math.abs(delta.getDegrees()) > 90.0) {
+      speedMetersPerSecond *= -1;
+      angle = angle.rotateBy(Rotation2d.kPi);
+    }
+  }  
+
+  /**
+   * Scales 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.
+   *
+   * @param currentAngle The current module angle.
+   */  
+  public void cosineScale(Rotation2d currentAngle) {
+    speedMetersPerSecond *= angle.minus(currentAngle).getCos();
+  }
+
   /**
    * 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
@@ -91,7 +117,9 @@ public String toString() {
    * @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);