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

src/robolib/trajectory/Velocity3d.java: Add cached getter for squared velocity magnitude

This commit is contained in:
Ray Healy 2026-02-10 15:59:00 -05:00
parent e9e111f1c4
commit 25e291091e
1 changed files with 17 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -11,6 +11,7 @@ public class Velocity3d {
private final LinearVelocity vY; private final LinearVelocity vY;
private final LinearVelocity vZ; private final LinearVelocity vZ;
private LinearVelocity cache_squareMagnitudeVelocity;
private LinearVelocity cache_magnitudeVelocity; private LinearVelocity cache_magnitudeVelocity;
private Angle cache_eleveationAngle; private Angle cache_eleveationAngle;
private Angle cache_azimuthAngle; 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 * @return The magnitude of the velocity vector
*/ */
public LinearVelocity getMagnitude() { public LinearVelocity getMagnitude() {
if(this.cache_magnitudeVelocity == null) { 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( this.cache_magnitudeVelocity = Units.MetersPerSecond.of(Math.pow(
Math.pow(vX_mps, 2) + Math.pow(vY_mps, 2) + Math.pow(vZ_mps, 2), this.getSquareMagnitude().in(Units.MetersPerSecond), 0.5
0.5
)); ));
} }
return this.cache_magnitudeVelocity; return this.cache_magnitudeVelocity;