FairportRobotics
/
RoboLib
mirror of https://github.com/FairportRobotics/RoboLib.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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.
This commit is contained in:
Ray Healy 2026-02-21 16:31:40 -05:00
parent dfefcd4c1a
commit f0835e88d5
1 changed files with 4 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/Velocity3d.java
View File

@ -45,8 +45,6 @@ public class Velocity3d {
horizontalVelocity.times(Math.sin(azimuthAngle.in(Units.Radians))), horizontalVelocity.times(Math.sin(azimuthAngle.in(Units.Radians))),
verticalVelocity verticalVelocity
); );
this.cache_azimuthAngle = azimuthAngle.copy();
this.cache_horizontalVelocity = horizontalVelocity.copy();
} }
public static Velocity3d genSpherical(double radialVelocityMPS, double elevationAngleDeg, double azimuthAngleDeg) { 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))), radialVelocity.times(Math.sin(elevationAngle.in(Units.Radians))),
azimuthAngle azimuthAngle
); );
this.cache_eleveationAngle = elevationAngle.copy();
this.cache_magnitudeVelocity = radialVelocity.copy();
} }
@ -259,8 +255,10 @@ public class Velocity3d {
public String toStringSpherical() { public String toStringSpherical() {
return String.format( return String.format(
"Velocity3d(Speed: %s, Elevation: %s, Azimuth: %s)", "Velocity3d(Speed: %s m/s, Elevation: %s deg, Azimuth: %s deg)",
this.getSpeed(), this.getElevationAngle(), this.getAzimuthAngle() this.getSpeed().in(Units.MetersPerSecond),
this.getElevationAngle().in(Units.Degrees),
this.getAzimuthAngle().in(Units.Degrees)
); );
} }