Improve OTOSLocalizer and PinpointLocalizer performance

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java

@@ -111,10 +111,7 @@ public Pose getPose() {
         SparkFunOTOS.Pose2D rawPose = otos.getPosition();
         Pose pose = new Pose(rawPose.x, rawPose.y, rawPose.h);
 
-        Vector vec = pose.getVector();
-        vec.rotateVector(startPose.getHeading());
-
-        return MathFunctions.addPoses(startPose, new Pose(vec.getXComponent(), vec.getYComponent(), pose.getHeading()));
+        return MathFunctions.addPoses(startPose, MathFunctions.rotatePose(pose, startPose.getHeading(), false));
     }
 
     /**

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/PinpointLocalizer.java

@@ -99,10 +99,7 @@ public Pose getPose() {
         Pose2D rawPose = odo.getPosition();
         Pose pose = new Pose(rawPose.getX(DistanceUnit.INCH), rawPose.getY(DistanceUnit.INCH), rawPose.getHeading(AngleUnit.RADIANS));
 
-        Vector vec = pose.getVector();
-        vec.rotateVector(startPose.getHeading());
-
-        return MathFunctions.addPoses(startPose, new Pose(vec.getXComponent(), vec.getYComponent(), pose.getHeading()));
+        return MathFunctions.addPoses(startPose, MathFunctions.rotatePose(pose, startPose.getHeading(), false));
     }
 
     /**

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/pathGeneration/MathFunctions.java

@@ -172,6 +172,22 @@ public static Pose subtractPoses(Pose one, Pose two) {
         return new Pose(one.getX() - two.getX(), one.getY() - two.getY(), one.getHeading() - two.getHeading());
     }
 
+    /**
+     * This rotates the given pose by the given theta,
+     *
+     * @param pose          the Pose to rotate.
+     * @param theta         the angle to rotate by.
+     * @param rotateHeading whether to adjust the Pose heading too.
+     * @return the rotated Pose.
+     */
+    public static Pose rotatePose(Pose pose, double theta, boolean rotateHeading) {
+        double x = pose.getX() * Math.cos(theta) - pose.getY() * Math.sin(theta);
+        double y = pose.getX() * Math.sin(theta) + pose.getY() * Math.cos(theta);
+        double heading = rotateHeading ? normalizeAngle(pose.getHeading() + theta) : pose.getHeading();
+
+        return new Pose(x, y, heading);
+    }
+
     /**
      * This multiplies a Point by a scalar and returns the result as a Point
      *