From 5617a3a91bce898d02e1d605011aa1563b3188d6 Mon Sep 17 00:00:00 2001
From: Ray Healy <raymond.m.healy@gmail.com>
Date: Sat, 21 Feb 2026 15:09:12 -0500
Subject: [PATCH] trajectory/Velocity3d: New (tested) features

- Define plus() transform function
- rename getSphericalVelocity() to getSpeed()
- Define genSpherical() generator (mapper to
spherical constructor from doubles).
- Define toStringSpherical() (Outputs Velocity3d
string w/ spherical coordinates)
---
 .../frc/robolib/trajectory/Velocity3d.java    | 23 ++++++++++++++++++-
 .../robolib/trajectory/Velocity3dTest.java    | 21 ++++++++++++++---
 2 files changed, 40 insertions(+), 4 deletions(-)

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 55b0935..1fb68e9 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
@@ -49,6 +49,13 @@ public class Velocity3d {
         this.cache_horizontalVelocity   = horizontalVelocity.copy();
     }
 
+    public static Velocity3d genSpherical(double radialVelocityMPS, double elevationAngleDeg, double azimuthAngleDeg) {
+        return new Velocity3d(
+            Units.MetersPerSecond.of(radialVelocityMPS),
+            Units.Degrees.of(elevationAngleDeg),
+            Units.Degrees.of(azimuthAngleDeg)
+        );
+    }
 
     public Velocity3d(LinearVelocity radialVelocity, Angle elevationAngle, Angle azimuthAngle) {
         this(
@@ -191,7 +198,7 @@ public class Velocity3d {
     /**
      * @return The radial component of the spherical velocity (alias of this.getMagnitude())
      */
-    public LinearVelocity getSphericalVelocity() {
+    public LinearVelocity getSpeed() {
         return this.getMagnitude();
     }
 
@@ -200,6 +207,14 @@ public class Velocity3d {
     // Transforms and Conversions
     //
 
+    public Velocity3d plus(Velocity3d other) {
+        return new Velocity3d(
+            this.xVelocity.plus(other.xVelocity),
+            this.yVelocity.plus(other.yVelocity),
+            this.zVelocity.plus(other.zVelocity)
+        );
+    }
+
     /**
      * Subtract other from this vector and return the resulting vector
      * @param other The vector to subtract from this one
@@ -242,6 +257,12 @@ public class Velocity3d {
         return String.format("Velocity3d(X: %s, Y: %s, Z: %s)", xVelocity, yVelocity, zVelocity);
     }
 
+    public String toStringSpherical() {
+        return String.format(
+            "Velocity3d(Speed: %s, Elevation: %s, Azimuth: %s)",
+            this.getSpeed(), this.getElevationAngle(), this.getAzimuthAngle()
+        );
+    }
 
     @Override
     public boolean equals(Object obj) {
diff --git a/lib/src/test/java/org/fairportrobotics/frc/robolib/trajectory/Velocity3dTest.java b/lib/src/test/java/org/fairportrobotics/frc/robolib/trajectory/Velocity3dTest.java
index fbff77b..5aab792 100644
--- a/lib/src/test/java/org/fairportrobotics/frc/robolib/trajectory/Velocity3dTest.java
+++ b/lib/src/test/java/org/fairportrobotics/frc/robolib/trajectory/Velocity3dTest.java
@@ -27,7 +27,7 @@ public class Velocity3dTest {
 
         // Check spherical
         assertEquals(45, test.getElevationAngle().in(Units.Degrees), epsilon, "Elevation");
-        assertEquals(Math.sqrt(2.0), test.getSphericalVelocity().in(Units.MetersPerSecond), epsilon, "Magnitude");
+        assertEquals(Math.sqrt(2.0), test.getSpeed().in(Units.MetersPerSecond), epsilon, "Magnitude");
 
     }
 
@@ -58,11 +58,17 @@ public class Velocity3dTest {
     }
 
     @Test void testSphericalConstructor() {
-        testConstructorBase(new Velocity3d(
+        Velocity3d test = new Velocity3d(
             Units.MetersPerSecond.of(Math.sqrt(2)),
             Units.Degrees.of(45),
             Units.Degrees.of(45)
-        ));
+        );
+        testConstructorBase(test);
+        assertEquals(
+            test,
+            Velocity3d.genSpherical(Math.sqrt(2), 45, 45),
+            "genSpherical Test"
+        );
     }
 
     //
@@ -79,6 +85,15 @@ public class Velocity3dTest {
     //  Test transforms
     //
 
+    @Test void testPlus() {
+        assertEquals(
+            new Velocity3d(5.0, 6.0, 7.0),
+            new Velocity3d(4.0, 5.0, 6.0).plus(
+                new Velocity3d(1.0, 1.0, 1.0)
+            )
+        );
+    }
+
     @Test void testMinus() {
         Velocity3d test = new Velocity3d(1.0, 1.0, 1.0).minus(
             new Velocity3d(2.0, 2.0, 2.0)