diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/Velocity3d.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/Velocity3d.java
index 32b0b60..535fe12 100644
--- a/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/Velocity3d.java
+++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/Velocity3d.java
@@ -11,6 +11,7 @@ public class Velocity3d {
     private final LinearVelocity vY;
     private final LinearVelocity vZ;
 
+    private LinearVelocity  cache_squareMagnitudeVelocity;
     private LinearVelocity  cache_magnitudeVelocity;
     private Angle           cache_eleveationAngle;
     private Angle           cache_azimuthAngle;
@@ -87,17 +88,28 @@ public class Velocity3d {
     }
 
 
+    /**
+     * @return The squared magnitude of the velocity vector
+     */
+    public LinearVelocity getSquareMagnitude() {
+        if(this.cache_squareMagnitudeVelocity == null) {
+            double vX_mps = this.vX.in(Units.MetersPerSecond);
+            double vY_mps = this.vY.in(Units.MetersPerSecond);
+            double vZ_mps = this.vZ.in(Units.MetersPerSecond);
+            this.cache_magnitudeVelocity = Units.MetersPerSecond.of(
+                Math.pow(vX_mps, 2) + Math.pow(vY_mps, 2) + Math.pow(vZ_mps, 2)
+            );
+        }
+        return this.cache_squareMagnitudeVelocity;
+    }
+
     /**
      * @return  The magnitude of the velocity vector
      */
     public LinearVelocity getMagnitude() {
         if(this.cache_magnitudeVelocity == null) {
-            double vX_mps = this.vX.in(Units.MetersPerSecond);
-            double vY_mps = this.vY.in(Units.MetersPerSecond);
-            double vZ_mps = this.vZ.in(Units.MetersPerSecond);
             this.cache_magnitudeVelocity = Units.MetersPerSecond.of(Math.pow(
-                Math.pow(vX_mps, 2) + Math.pow(vY_mps, 2) + Math.pow(vZ_mps, 2),
-                0.5
+                this.getSquareMagnitude().in(Units.MetersPerSecond), 0.5
             ));
         }
         return this.cache_magnitudeVelocity;