trajectory/ballistics/constraints: Add tests for

computed vectors

lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/constraints/BCEvalParams.java

@@ -26,7 +26,7 @@ public class BCEvalParams {
 
     //
     // Setup
-    //
+    //)
 
     public BCEvalParams(
         Velocity3d robotVelocity,
@@ -34,13 +34,13 @@ public class BCEvalParams {
         Velocity3d currentShooterVelocity,
         Velocity3d candidateVelocity
     ) {
-        this.robotVelocity          = robotVelocity;
+        this.robotVelocity                          = robotVelocity;
-        this.robotAngle             = robotAngle;
+        this.robotAngle                             = robotAngle;
-        this.currentShooterVelocity = currentShooterVelocity;
+        this.currentShooterVelocity                 = currentShooterVelocity;
 
-        this.candidateVelocity      = candidateVelocity;
+        this.candidateVelocity                      = candidateVelocity;
 
-        this.cache_candidateShooterVelocity   = null;
+        this.cache_candidateShooterVelocity         = null;
         this.cache_candidateShooterVelocityRelative = null;
     }
 
@@ -109,4 +109,16 @@ public class BCEvalParams {
         return this.cache_candidateShooterVelocityRelative;
     }
 
+    public boolean equals(BCEvalParams other) {
+        return
+            this.robotVelocity.equals(other.getRobotVelocity()) &&
+            this.robotAngle.equals(other.getRobotAngle()) &&
+            this.currentShooterVelocity.equals(other.getCurrentShooterVelocity()) &&
+            this.candidateVelocity.equals(other.candidateVelocity);
+    }
+
+    public boolean equals(Object other) {
+        return false;
+    }
+
 }

lib/src/test/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/constraints/BCEvalParamsTest.java

@@ -0,0 +1,46 @@
+package org.fairportrobotics.frc.robolib.trajectory.ballistics.constraints;
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.units.Units;
+
+import static org.junit.jupiter.api.Assertions.*;
+
+import org.fairportrobotics.frc.robolib.trajectory.Velocity3d;
+
+public class BCEvalParamsTest {
+    @Test void testCandidateShooterVelocity() {
+        assertEquals(
+            new Velocity3d(1.0, 2.0, 3.0),
+            new BCEvalParams(
+                new Velocity3d(1.0, 1.0, 1.0),
+                Units.Degrees.of(0),
+                Velocity3d.zero,
+                new Velocity3d(2.0, 3.0, 4.0)
+            ).getCandidateShooterVelocity(),
+            "Robot Velocity Only"
+        );
+
+        assertEquals(
+            new Velocity3d(1.0, 0.0, 0.0),
+            new BCEvalParams(
+                Velocity3d.zero,
+                Units.Degrees.of(90),
+                Velocity3d.zero,
+                new Velocity3d(0.0, 1.0, 0.0)
+            ).getCandidateShooterVelocityRelative(),
+            "Robot Angle Only"
+        );
+
+        assertEquals(
+            new Velocity3d(1.0, 0.0, 0.0),
+            new BCEvalParams(
+                new Velocity3d(1.0, 1.0, 0.0),
+                Units.Degrees.of(90),
+                Velocity3d.zero,
+                new Velocity3d(1.0, 2.0, 0.0)
+            ).getCandidateShooterVelocityRelative(),
+            "Robot Velocity and Angle"
+        );
+    }
+}