Change getDistanceFromPoint to work on Translation2d

Galaxia5987 · Feb 3, 2024 · 95cc921 · 95cc921
1 parent 45fb03d
commit 95cc921
Showing 1 changed file with 5 additions and 3 deletions.
diff --git a/src/main/java/lib/Utils.java b/src/main/java/lib/Utils.java
@@ -49,13 +49,15 @@ public static Rotation2d normalize(Rotation2d angle) {
         return Rotation2d.fromRadians(normalize(angle.getRadians()));
     }
 
-    public static double getDistanceFromPoint(Pose2d point, Pose2d robotPose) {
-        return robotPose.getTranslation().getDistance(point.getTranslation());
+    public static double getDistanceFromPoint(Translation2d point, Pose2d robotPose) {
+        return robotPose.getTranslation().getDistance(point);
     }
 
     public static Pose2d calcOptimalPose(List<Pose2d> points, Pose2d robotPose) {
         return points.stream()
-                .min(Comparator.comparingDouble(point -> getDistanceFromPoint(point, robotPose)))
+                .min(
+                        Comparator.comparingDouble(
+                                point -> getDistanceFromPoint(point.getTranslation(), robotPose)))
                 .orElse(null);
     }
 }