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 73e8c77..d4fb710 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 @@ -1,15 +1,13 @@ package org.fairportrobotics.frc.robolib.trajectory; -import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.LinearVelocity; -import edu.wpi.first.units.measure.Time; public class Velocity3d { - private final LinearVelocity vX; - private final LinearVelocity vY; - private final LinearVelocity vZ; + private final LinearVelocity xVelocity; + private final LinearVelocity yVelocity; + private final LinearVelocity zVelocity; private Double cache_squareMagnitudeVelocity; private LinearVelocity cache_magnitudeVelocity; @@ -17,10 +15,10 @@ public class Velocity3d { private Angle cache_azimuthAngle; private LinearVelocity cache_horizontalVelocity; - public Velocity3d(LinearVelocity vX, LinearVelocity vY, LinearVelocity vZ) { - this.vX = vX.copy(); - this.vY = vY.copy(); - this.vZ = vZ.copy(); + public Velocity3d(LinearVelocity xVelocity, LinearVelocity yVelocity, LinearVelocity zVelocity) { + this.xVelocity = xVelocity.copy(); + this.yVelocity = yVelocity.copy(); + this.zVelocity = zVelocity.copy(); this.cache_squareMagnitudeVelocity = null; this.cache_magnitudeVelocity = null; @@ -69,7 +67,7 @@ public class Velocity3d { * @return The x-axis component of the velocity */ public LinearVelocity getXVelocity() { - return vX; + return xVelocity; } @@ -77,7 +75,7 @@ public class Velocity3d { * @return The y-axis component of the velocity */ public LinearVelocity getYVelocity() { - return vY; + return yVelocity; } @@ -85,7 +83,7 @@ public class Velocity3d { * @return The z-axis component of the velocity */ public LinearVelocity getZVelocity() { - return vZ; + return zVelocity; } @@ -94,9 +92,9 @@ public class Velocity3d { */ public double 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); + double vX_mps = this.xVelocity.in(Units.MetersPerSecond); + double vY_mps = this.yVelocity.in(Units.MetersPerSecond); + double vZ_mps = this.zVelocity.in(Units.MetersPerSecond); this.cache_squareMagnitudeVelocity = Math.pow(vX_mps, 2) + Math.pow(vY_mps, 2) + Math.pow(vZ_mps, 2); @@ -178,7 +176,7 @@ public class Velocity3d { * @return The elevation angle of the velocity, with respect to the horizontal (XY) components * of the velocity. */ - public Angle getElevation() { + public Angle getElevationAngle() { if(this.cache_eleveationAngle == null) { double vZ_mps = this.getZVelocity().in(Units.MetersPerSecond); double vHori_mps = this.getHorizontalVelocity().in(Units.MetersPerSecond); @@ -207,9 +205,9 @@ public class Velocity3d { */ public Velocity3d minus(Velocity3d other) { return new Velocity3d( - this.vX.minus(other.vX), - this.vY.minus(other.vY), - this.vZ.minus(other.vZ) + this.xVelocity.minus(other.xVelocity), + this.yVelocity.minus(other.yVelocity), + this.zVelocity.minus(other.zVelocity) ); } @@ -227,33 +225,19 @@ public class Velocity3d { } - /** - * Multiply the velocity by time in order to get a displacement - * @param time The time to compute over - * @return The resulting displacement - */ - public Translation3d times(Time time) { - return new Translation3d( - this.vX.times(time), - this.vY.times(time), - this.vZ.times(time) - ); - } - - // // Overrides // @Override public int hashCode() { - return this.vX.hashCode() ^ this.vY.hashCode() ^ this.vZ.hashCode(); + return this.xVelocity.hashCode() ^ this.yVelocity.hashCode() ^ this.zVelocity.hashCode(); } @Override public String toString() { - return String.format("Velocity3d(X: %s, Y: %s, Z: %s)", vX, vY, vZ); + return String.format("Velocity3d(X: %s, Y: %s, Z: %s)", xVelocity, yVelocity, zVelocity); } @@ -262,9 +246,9 @@ public class Velocity3d { if(Velocity3d.class.isInstance(obj)) { Velocity3d otherVelocity = (Velocity3d) obj; return - this.vX == otherVelocity.getXVelocity() && - this.vY == otherVelocity.getYVelocity() && - this.vZ == otherVelocity.getZVelocity(); + this.xVelocity.equals(otherVelocity.getXVelocity()) && + this.yVelocity.equals(otherVelocity.getYVelocity()) && + this.zVelocity.equals(otherVelocity.getZVelocity()); } return false; } 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 new file mode 100644 index 0000000..fbff77b --- /dev/null +++ b/lib/src/test/java/org/fairportrobotics/frc/robolib/trajectory/Velocity3dTest.java @@ -0,0 +1,105 @@ +package org.fairportrobotics.frc.robolib.trajectory; + + +import org.junit.jupiter.api.Test; + +import edu.wpi.first.units.Units; + +import static org.junit.jupiter.api.Assertions.*; + +public class Velocity3dTest { + private static final double epsilon=1e-10; + + // + // Test base create and coordinate space transform + // + + void testConstructorBase(Velocity3d test) { + // Check cartesian + assertEquals(Math.cos(Math.toRadians(45)), test.getXVelocity().in(Units.MetersPerSecond), epsilon, "X"); + assertEquals(Math.sin(Math.toRadians(45)), test.getYVelocity().in(Units.MetersPerSecond), epsilon, "Y"); + assertEquals(1.0, test.getZVelocity().in(Units.MetersPerSecond), epsilon, "Z"); + + // Check cylindrical + assertEquals(1.0, test.getVeritcalVelocity().in(Units.MetersPerSecond), epsilon, "Vertical"); + assertEquals(1.0, test.getHorizontalVelocity().in(Units.MetersPerSecond), epsilon, "Horizontal"); + assertEquals(45, test.getAzimuthAngle().in(Units.Degrees), epsilon, "Azimuth"); + + // Check spherical + assertEquals(45, test.getElevationAngle().in(Units.Degrees), epsilon, "Elevation"); + assertEquals(Math.sqrt(2.0), test.getSphericalVelocity().in(Units.MetersPerSecond), epsilon, "Magnitude"); + + } + + @Test void testCartesianConstructor() { + Velocity3d test = new Velocity3d( + Units.MetersPerSecond.of(Math.cos(Math.toRadians(45))), + Units.MetersPerSecond.of(Math.sin(Math.toRadians(45))), + Units.MetersPerSecond.of(1.0) + ); + + testConstructorBase(test); + + // Check implicit cast to LinearVelocity + assertEquals(test, new Velocity3d( + Math.cos(Math.toRadians(45)), + Math.sin(Math.toRadians(45)), + 1.0 + ), "Implicit convert" + ); + } + + @Test void testCylindicalConstructor() { + testConstructorBase(new Velocity3d( + Units.MetersPerSecond.of(1.0), + Units.MetersPerSecond.of(1.0), + Units.Degrees.of(45) + )); + } + + @Test void testSphericalConstructor() { + testConstructorBase(new Velocity3d( + Units.MetersPerSecond.of(Math.sqrt(2)), + Units.Degrees.of(45), + Units.Degrees.of(45) + )); + } + + // + // Test additional getters + // + + @Test void testMagnitudes() { + Velocity3d test = new Velocity3d(1.0, 1.0, 1.0); + assertEquals(3.0, test.getSquareMagnitude(), epsilon, "Square magnitude"); + assertEquals(Math.sqrt(3.0), test.getMagnitude().in(Units.MetersPerSecond), epsilon, "Magnitude"); + } + + // + // Test transforms + // + + @Test void testMinus() { + Velocity3d test = new Velocity3d(1.0, 1.0, 1.0).minus( + new Velocity3d(2.0, 2.0, 2.0) + ); + + assertEquals(new Velocity3d(-1.0, -1.0, -1.0), test, "Subtract"); + } + + @Test void testRotate() { + Velocity3d test = new Velocity3d( + Units.MetersPerSecond.of(1.0), + Units.Degrees.of(45), + Units.Degrees.of(45) + ).rotate(Units.Degrees.of(45)); + + assertEquals( + new Velocity3d( + Units.MetersPerSecond.of(1.0), + Units.Degrees.of(45), + Units.Degrees.of(90) + ), test, "Rotate" + ); + } +}