src/robolib/trajectory/Velocity3dTest: Defined unit

tests for Velocity3d

Also, implement fixes for bugs found in test bringup

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 xVelocity;
-    private final LinearVelocity vY;
+    private final LinearVelocity yVelocity;
-    private final LinearVelocity vZ;
+    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) {
+    public Velocity3d(LinearVelocity xVelocity, LinearVelocity yVelocity, LinearVelocity zVelocity) {
-        this.vX = vX.copy();
+        this.xVelocity = xVelocity.copy();
-        this.vY = vY.copy();
+        this.yVelocity = yVelocity.copy();
-        this.vZ = vZ.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 vX_mps = this.xVelocity.in(Units.MetersPerSecond);
-            double vY_mps = this.vY.in(Units.MetersPerSecond);
+            double vY_mps = this.yVelocity.in(Units.MetersPerSecond);
-            double vZ_mps = this.vZ.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.xVelocity.minus(other.xVelocity),
-            this.vY.minus(other.vY),
+            this.yVelocity.minus(other.yVelocity),
-            this.vZ.minus(other.vZ)
+            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.xVelocity.equals(otherVelocity.getXVelocity()) &&
-                this.vY == otherVelocity.getYVelocity() &&
+                this.yVelocity.equals(otherVelocity.getYVelocity()) &&
-                this.vZ == otherVelocity.getZVelocity();
+                this.zVelocity.equals(otherVelocity.getZVelocity());
         }
         return false;
     }

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"
+        );
+    }
+}