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: Updates 

- Rename getV[XYZ]() to get[XYZ]Velocity()
- Added a rotation transform
This commit is contained in:
Ray Healy 2026-02-10 19:34:36 -05:00
parent f1c80233f0
commit 167fa8dfd6
1 changed files with 26 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -68,7 +68,7 @@ public class Velocity3d {
/** /**
* @return The x-axis component of the velocity * @return The x-axis component of the velocity
*/ */
public LinearVelocity getVX() { public LinearVelocity getXVelocity() {
return vX; return vX;
} }
@ -76,7 +76,7 @@ public class Velocity3d {
/** /**
* @return The y-axis component of the velocity * @return The y-axis component of the velocity
*/ */
public LinearVelocity getVY() { public LinearVelocity getYVelocity() {
return vY; return vY;
} }
@ -84,7 +84,7 @@ public class Velocity3d {
/** /**
* @return The z-axis component of the velocity * @return The z-axis component of the velocity
*/ */
public LinearVelocity getVZ() { public LinearVelocity getZVelocity() {
return vZ; return vZ;
} }
@ -125,7 +125,7 @@ public class Velocity3d {
* @return The magnitude of the vertical component of the velocity (alias of this.getVZ()) * @return The magnitude of the vertical component of the velocity (alias of this.getVZ())
*/ */
public LinearVelocity getVeritcalVelocity() { public LinearVelocity getVeritcalVelocity() {
return this.getVZ(); return this.getZVelocity();
} }
@ -134,8 +134,8 @@ public class Velocity3d {
*/ */
public LinearVelocity getHorizontalVelocity() { public LinearVelocity getHorizontalVelocity() {
if (this.cache_horizontalVelocity == null) { if (this.cache_horizontalVelocity == null) {
double vX_mps = this.getVX().in(Units.MetersPerSecond); double vX_mps = this.getXVelocity().in(Units.MetersPerSecond);
double vY_mps = this.getVY().in(Units.MetersPerSecond); double vY_mps = this.getYVelocity().in(Units.MetersPerSecond);
this.cache_horizontalVelocity = Units.MetersPerSecond.of( this.cache_horizontalVelocity = Units.MetersPerSecond.of(
Math.pow( Math.pow(
Math.pow(vX_mps, 2) + Math.pow(vY_mps, 2), Math.pow(vX_mps, 2) + Math.pow(vY_mps, 2),
@ -156,8 +156,8 @@ public class Velocity3d {
*/ */
public Angle getAzimuthAngle() { public Angle getAzimuthAngle() {
if (this.cache_azimuthAngle == null) { if (this.cache_azimuthAngle == null) {
double vX_mps = this.getVX().in(Units.MetersPerSecond); double vX_mps = this.getXVelocity().in(Units.MetersPerSecond);
double vY_mps = this.getVY().in(Units.MetersPerSecond); double vY_mps = this.getYVelocity().in(Units.MetersPerSecond);
this.cache_azimuthAngle = Units.Radians.of(Math.atan2(vY_mps, vX_mps)); this.cache_azimuthAngle = Units.Radians.of(Math.atan2(vY_mps, vX_mps));
} }
return this.cache_azimuthAngle; return this.cache_azimuthAngle;
@ -180,7 +180,7 @@ public class Velocity3d {
*/ */
public Angle getElevation() { public Angle getElevation() {
if(this.cache_eleveationAngle == null) { if(this.cache_eleveationAngle == null) {
double vZ_mps = this.getVZ().in(Units.MetersPerSecond); double vZ_mps = this.getZVelocity().in(Units.MetersPerSecond);
double vHori_mps = this.getHorizontalVelocity().in(Units.MetersPerSecond); double vHori_mps = this.getHorizontalVelocity().in(Units.MetersPerSecond);
this.cache_eleveationAngle = Units.Radians.of(Math.atan2(vZ_mps, vHori_mps)); this.cache_eleveationAngle = Units.Radians.of(Math.atan2(vZ_mps, vHori_mps));
} }
@ -197,7 +197,7 @@ public class Velocity3d {
// //
// Mathematical operations // Transforms and Conversions
// //
/** /**
@ -213,6 +213,19 @@ public class Velocity3d {
); );
} }
/**
* Rotate the vector by adding a given angle to the azimuth
* @param rotAngle The angle to add to the azimuth
* @return A new, rotated Velocity3d instance
*/
public Velocity3d rotate(Angle rotAngle) {
return new Velocity3d(
this.getHorizontalVelocity(),
this.getVeritcalVelocity(),
this.getAzimuthAngle().plus(rotAngle)
);
}
/** /**
* Multiply the velocity by time in order to get a displacement * Multiply the velocity by time in order to get a displacement
@ -249,9 +262,9 @@ public class Velocity3d {
if(Velocity3d.class.isInstance(obj)) { if(Velocity3d.class.isInstance(obj)) {
Velocity3d otherVelocity = (Velocity3d) obj; Velocity3d otherVelocity = (Velocity3d) obj;
return return
this.vX == otherVelocity.getVX() && this.vX == otherVelocity.getXVelocity() &&
this.vY == otherVelocity.getVY() && this.vY == otherVelocity.getYVelocity() &&
this.vZ == otherVelocity.getVZ(); this.vZ == otherVelocity.getZVelocity();
} }
return false; return false;
} }