From f0835e88d5c78a07f944d9be16dfe0885eab19fc Mon Sep 17 00:00:00 2001 From: Ray Healy Date: Sat, 21 Feb 2026 16:31:40 -0500 Subject: [PATCH] trajectory/Velocity3d: Remove caching of cylindrical and spherical coordinates from non-cartesian constructors Effectively bounds azimuth to (-180, 180) and elevation to (-90, 90) due to order of operations and the definition of atan2. --- .../frc/robolib/trajectory/Velocity3d.java | 10 ++++------ 1 file changed, 4 insertions(+), 6 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 1fb68e9..61fc973 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 @@ -45,8 +45,6 @@ public class Velocity3d { horizontalVelocity.times(Math.sin(azimuthAngle.in(Units.Radians))), verticalVelocity ); - this.cache_azimuthAngle = azimuthAngle.copy(); - this.cache_horizontalVelocity = horizontalVelocity.copy(); } public static Velocity3d genSpherical(double radialVelocityMPS, double elevationAngleDeg, double azimuthAngleDeg) { @@ -63,8 +61,6 @@ public class Velocity3d { radialVelocity.times(Math.sin(elevationAngle.in(Units.Radians))), azimuthAngle ); - this.cache_eleveationAngle = elevationAngle.copy(); - this.cache_magnitudeVelocity = radialVelocity.copy(); } @@ -259,8 +255,10 @@ public class Velocity3d { public String toStringSpherical() { return String.format( - "Velocity3d(Speed: %s, Elevation: %s, Azimuth: %s)", - this.getSpeed(), this.getElevationAngle(), this.getAzimuthAngle() + "Velocity3d(Speed: %s m/s, Elevation: %s deg, Azimuth: %s deg)", + this.getSpeed().in(Units.MetersPerSecond), + this.getElevationAngle().in(Units.Degrees), + this.getAzimuthAngle().in(Units.Degrees) ); }