Remove copy methods and remove duplicate Interpolate methods

wpilibsuite · Jun 24, 2024 · c65f239 · c65f239
1 parent 9cb2987
commit c65f239
Show file tree

Hide file tree

Showing 6 changed files with 4 additions and 25 deletions.
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java
@@ -8,7 +8,6 @@
 
 import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.math.MathUsageId;
-import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Twist2d;
 import edu.wpi.first.math.kinematics.proto.DifferentialDriveKinematicsProto;
 import edu.wpi.first.math.kinematics.struct.DifferentialDriveKinematicsStruct;
@@ -125,8 +124,6 @@ public DifferentialDriveWheelPositions interpolate(
       DifferentialDriveWheelPositions startValue,
       DifferentialDriveWheelPositions endValue,
       double t) {
-    return new DifferentialDriveWheelPositions(
-        MathUtil.interpolate(startValue.leftMeters, endValue.leftMeters, t),
-        MathUtil.interpolate(startValue.rightMeters, endValue.rightMeters, t));
+    return startValue.interpolate(endValue, t);
   }
 }
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java
@@ -70,10 +70,6 @@ public String toString() {
         "DifferentialDriveWheelPositions(Left: %.2f m, Right: %.2f m", leftMeters, rightMeters);
   }
 
-  public DifferentialDriveWheelPositions copy() {
-    return new DifferentialDriveWheelPositions(leftMeters, rightMeters);
-  }
-
   @Override
   public DifferentialDriveWheelPositions interpolate(
       DifferentialDriveWheelPositions endValue, double t) {

diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java
@@ -6,7 +6,6 @@
 
 import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.math.MathUsageId;
-import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Twist2d;
 import edu.wpi.first.math.kinematics.proto.MecanumDriveKinematicsProto;
@@ -269,10 +268,6 @@ public MecanumDriveWheelPositions copy(MecanumDriveWheelPositions positions) {
   @Override
   public MecanumDriveWheelPositions interpolate(
       MecanumDriveWheelPositions startValue, MecanumDriveWheelPositions endValue, double t) {
-    return new MecanumDriveWheelPositions(
-        MathUtil.interpolate(startValue.frontLeftMeters, endValue.frontLeftMeters, t),
-        MathUtil.interpolate(startValue.frontRightMeters, endValue.frontRightMeters, t),
-        MathUtil.interpolate(startValue.rearLeftMeters, endValue.rearLeftMeters, t),
-        MathUtil.interpolate(startValue.rearRightMeters, endValue.rearRightMeters, t));
+    return startValue.interpolate(endValue, t);
   }
 }
diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java
@@ -100,11 +100,6 @@ public String toString() {
         frontLeftMeters, frontRightMeters, rearLeftMeters, rearRightMeters);
   }
 
-  public MecanumDriveWheelPositions copy() {
-    return new MecanumDriveWheelPositions(
-        frontLeftMeters, frontRightMeters, rearLeftMeters, rearRightMeters);
-  }
-
   @Override
   public MecanumDriveWheelPositions interpolate(MecanumDriveWheelPositions endValue, double t) {
     return new MecanumDriveWheelPositions(

diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
@@ -91,8 +91,7 @@ class WPILIB_DLLEXPORT DifferentialDriveKinematics
   DifferentialDriveWheelPositions Interpolate(
       const DifferentialDriveWheelPositions& start,
       const DifferentialDriveWheelPositions& end, double t) const override {
-    return {wpi::Lerp(start.left, end.left, t),
-            wpi::Lerp(start.right, end.right, t)};
+    return start.Interpolate(end, t);
   }
 
   /// Differential drive trackwidth.

diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h
@@ -169,10 +169,7 @@ class WPILIB_DLLEXPORT MecanumDriveKinematics
   MecanumDriveWheelPositions Interpolate(
       const MecanumDriveWheelPositions& start,
       const MecanumDriveWheelPositions& end, double t) const override {
-    return {wpi::Lerp(start.frontLeft, end.frontLeft, t),
-            wpi::Lerp(start.frontRight, end.frontRight, t),
-            wpi::Lerp(start.rearLeft, end.rearLeft, t),
-            wpi::Lerp(start.rearRight, end.rearRight, t)};
+    return start.Interpolate(end, t);
   }
 
  private: