lib/RoboLib/ballistics/models: Added unit test

for ParabolicTimeOfFlightBM

- Added getters for internal state
- Fixed ReversableRadialBM.getTimeAt[XY]() (was
missing correction for launch position)
- Fixed
ParabolicTimeOfFlightBM.getCandidateVelocity()
(was adding gravity factor instead of subtracting)

lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/models/BallisticModel.java

@@ -14,19 +14,27 @@ public abstract class BallisticModel<P> {
         this.posTarget      = posTarget;
     }
 
+    //
+    //  Base getters
+    //
+
+    public Translation3d getPosLaunch() {
+        return this.posLaunch;
+    }
+
+    public Translation3d getPosTarget() {
+        return this.posTarget;
+    }
+
+    //
+    //  Parameter Manipulation
+    //
+
     /**
      * @return Returns a recommended starting parameter
      */
     public abstract P getInitParam();
 
-    /**
-     * Get an abstract velocity (in the same coordinate space as posLaunch and
-     * posTarget) which will pass thru
-     *
-     * @param param The parameters used to determine this candidate
-     * @return The computed result for the given parameter
-     */
-    public abstract Velocity3d getCandidateVelocity(P param);
 
     /**
      * Get parameters that are "adjacent" to the input paramter (i.e. generate
@@ -37,8 +45,25 @@ public abstract class BallisticModel<P> {
      */
     public abstract P[] getNeighborParams(P param);
 
+
+    //
+    //  Velocity modelling
+    //
+
     /**
-     * Get the position of the projectile at a given time offset
+     * Get an initial velocity (in the same coordinate space as posLaunch and
+     * posTarget) which will pass thru posLaunch and posTarget with the given
+     * model configurations and parameters
+     *
+     * @param param The parameters used to determine this candidate
+     * @return The computed result for the given parameter
+     */
+    public abstract Velocity3d getCandidateVelocity(P param);
+
+
+    /**
+     * Get the position of the projectile at a given time offset for a given
+     * starting velocity
      *
      * @param velocity The velocity to check
      * @param time The time to check
@@ -46,6 +71,10 @@ public abstract class BallisticModel<P> {
      */
     public abstract Translation3d positionAtTime(Velocity3d velocity, Time time);
 
+    //
+    //  Computed parameter check
+    //
+
     /**
      * Check if a given parameter has had a computed velocity
      * @param param The param to check

lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/models/ParabolicTimeOfFlightBM.java

@@ -14,8 +14,7 @@ import edu.wpi.first.units.measure.Time;
 
 
 public class ParabolicTimeOfFlightBM extends ReversableRadialBM<Time> {
-
+    public static final LinearAcceleration k_gravity = Units.MetersPerSecondPerSecond.of(-9.81);
-    private final LinearAcceleration k_gravity = Units.MetersPerSecondPerSecond.of(-9.81);
 
     protected final Time tofMin;
     protected final Time tofMax;
@@ -54,20 +53,41 @@ public class ParabolicTimeOfFlightBM extends ReversableRadialBM<Time> {
         this.computedSeconds = new HashSet<>();
     }
 
+    //
+    //  Base getters
+    //
 
-    public Time getInitParam() {
+    public Time getTOFMin() {
-        return this.tofMin.plus(this.tofMax).div(2);
+        return this.tofMin;
+    }
+
+    public Time getTOFMax() {
+        return this.tofMax;
+    }
+
+    public Time getTOFStep() {
+        return this.tofStep;
+    }
+
+    public Translation3d getTargetPosRelative() {
+        return this.targetPosRelative;
+    }
+
+    public Distance getRelativeHorizontalDistance() {
+        return this.relativeHorizontalDistance;
+    }
+
+    public Angle getAzimuth() {
+        return this.azimuth;
     }
 
 
-    public Velocity3d getCandidateVelocity(Time param) {
+    //
-        this.computedSeconds.add(param.in(Units.Seconds));
+    //  Parameter manipulation
-        return new Velocity3d(
+    //
-            this.relativeHorizontalDistance.div(param),
+
-            this.targetPosRelative.getMeasureZ().div(param)
+    public Time getInitParam() {
-                .plus(k_gravity.times(param).times(0.5)),
+        return this.tofMin.plus(this.tofMax).div(2);
-            azimuth
-        );
     }
 
 
@@ -84,6 +104,21 @@ public class ParabolicTimeOfFlightBM extends ReversableRadialBM<Time> {
     }
 
 
+    //
+    //  Velocity modelling
+    //
+
+    public Velocity3d getCandidateVelocity(Time param) {
+        this.computedSeconds.add(param.in(Units.Seconds));
+        return new Velocity3d(
+            this.relativeHorizontalDistance.div(param),
+            this.targetPosRelative.getMeasureZ().div(param)
+                .minus(k_gravity.times(param).times(0.5)),
+            azimuth
+        );
+    }
+
+
     public Translation3d positionAtTime(Velocity3d velocity, Time time) {
         return new Translation3d(
             this.posLaunch.getMeasureX()
@@ -97,11 +132,19 @@ public class ParabolicTimeOfFlightBM extends ReversableRadialBM<Time> {
     }
 
 
+    //
+    //  Reverse functions
+    //
+
     public Time getTimeAtRadius(Velocity3d candidate, Distance radialDistance) {
         return radialDistance.div(candidate.getHorizontalVelocity());
     }
 
 
+    //
+    //  Computed parameter check
+    //
+
     public boolean paramComputed(Time param) {
         return this.computedSeconds.contains(param.in(Units.Seconds));
     }

lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/models/ReversableRadialBM.java

@@ -16,6 +16,11 @@ public abstract class ReversableRadialBM<P> extends BallisticModel<P> {
         super(posLaunch, posTarget);
     }
 
+
+    //
+    //  Reverse functions
+    //
+
     /**
      * Get the time taken to travel a given radial distance from posLaunch towards posTarget
      *
@@ -36,7 +41,8 @@ public abstract class ReversableRadialBM<P> extends BallisticModel<P> {
      */
     public Time getTimeAtX(Velocity3d candidate, Distance xDistance) {
         Distance radialDistance = Units.Meters.of(
-            xDistance.in(Units.Meters)/Math.cos(candidate.getAzimuthAngle().in(Units.Radians))
+            xDistance.minus(posLaunch.getMeasureX()).in(Units.Meters)
+                / Math.cos(candidate.getAzimuthAngle().in(Units.Radians))
         );
         return this.getTimeAtRadius(candidate, radialDistance);
     }
@@ -52,11 +58,13 @@ public abstract class ReversableRadialBM<P> extends BallisticModel<P> {
      */
     public Time getTimeAtY(Velocity3d candidate, Distance yDistance) {
         Distance radialDistance = Units.Meters.of(
-            yDistance.in(Units.Meters)/Math.sin(candidate.getAzimuthAngle().in(Units.Radians))
+            yDistance.minus(posLaunch.getMeasureY()).in(Units.Meters)
+                / Math.sin(candidate.getAzimuthAngle().in(Units.Radians))
         );
         return this.getTimeAtRadius(candidate, radialDistance);
     }
 
+
     /**
      * Get the time to target on a given candidate trajectory
      * 

lib/src/test/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/models/ParabolicTimeOfFlightBMTest.java

@@ -0,0 +1,139 @@
+package org.fairportrobotics.frc.robolib.trajectory.ballistics.models;
+
+
+import org.junit.jupiter.api.Test;
+
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.units.Units;
+import edu.wpi.first.units.measure.Time;
+
+import static org.junit.jupiter.api.Assertions.*;
+
+import org.fairportrobotics.frc.robolib.trajectory.Velocity3d;
+
+public class ParabolicTimeOfFlightBMTest {
+    private ParabolicTimeOfFlightBM getModel() {
+        return new ParabolicTimeOfFlightBM(
+            new Translation3d(Units.Meters.of(1), Units.Meters.of(1), Units.Meters.of(1)),
+            new Translation3d(Units.Meters.of(4), Units.Meters.of(5), Units.Meters.of(2)),
+            Units.Seconds.of(1.0), Units.Seconds.of(3), Units.Seconds.of(1.0)
+        );
+    }
+
+    @Test void testParamIterator() {
+        ParabolicTimeOfFlightBM model = getModel();
+
+        // Define the initial param
+        Time initParam = model.getInitParam();
+        assertEquals(2.0, initParam.in(Units.Seconds), "Init param");
+        assertArrayEquals(
+            new Time[]{Units.Seconds.of(1.0), Units.Seconds.of(3.0)},
+            model.getNeighborParams(initParam), "Neighbors"
+        );
+    }
+
+    @Test void testSolutionVelocity() {
+        ParabolicTimeOfFlightBM model = getModel();
+        Time tof = Units.Seconds.of(1.0);
+
+        // Compute the solution
+        Velocity3d result = model.getCandidateVelocity(tof);
+        assertEquals(new Velocity3d(
+            Units.MetersPerSecond.of(5),
+            Units.MetersPerSecond.of(1).minus(
+                ParabolicTimeOfFlightBM.k_gravity.times(0.5).times(tof)
+            ),
+            Units.Radians.of(Math.atan2(4, 3))
+        ), result, "Compute solution");
+
+        // Check that positions match expected values
+        assertEquals(
+            model.getPosLaunch(),
+            model.positionAtTime(result, Units.Seconds.of(0)),
+            "Launch position"
+        );
+        assertEquals(
+            model.getPosTarget(),
+            model.positionAtTime(result, tof),
+            "Target position check"
+        );
+
+        Time midTime = Units.Seconds.of(0.5);
+        assertEquals(
+            new Translation3d(
+                Units.Meters.of(2.5),   // 1+3*0.5
+                Units.Meters.of(3),     // 1+4*0.5
+                Units.Meters.of(1.0)
+                    .plus(result.getZVelocity().times(midTime))
+                    .plus(
+                        ParabolicTimeOfFlightBM.k_gravity
+                            .times(0.5).times(midTime).times(midTime)
+                    )
+            ),
+            model.positionAtTime(result, midTime),
+            "Mid-flight position check"
+        );
+    }
+
+    @Test void testReverseFunctions() {
+        ParabolicTimeOfFlightBM model   = getModel();
+        Time tof                        = Units.Seconds.of(1.0);
+        Velocity3d solution             = model.getCandidateVelocity(tof);
+
+        // Test initial position
+        Time t0 = Units.Seconds.of(0.0);
+        assertEquals(t0, model.getTimeAtRadius(solution, Units.Meters.of(0.0)), "Init Position (Rad)");
+        assertEquals(t0, model.getTimeAtX(solution, model.getPosLaunch().getMeasureX()), "Init Position (X)");
+        assertEquals(t0, model.getTimeAtY(solution, model.getPosLaunch().getMeasureY()), "Init Position (Y)");
+
+        // Test target position
+        assertEquals(tof, model.getTimeAtRadius(solution, model.getRelativeHorizontalDistance()), "Final Position (Rad)");
+        assertEquals(tof, model.getTimeAtX(solution, model.getPosTarget().getMeasureX()), "Final Position (X)");
+        assertEquals(tof, model.getTimeAtY(solution, model.getPosTarget().getMeasureY()), "Final Position (Y)");
+        assertEquals(tof, model.getTimeAtTarget(solution));
+
+        // Test mid-flight position
+        Time tMid = Units.Seconds.of(0.5);
+        assertEquals(tMid, model.getTimeAtRadius(solution, model.getRelativeHorizontalDistance().div(2)));
+        assertEquals(tMid, model.getTimeAtX(solution, Units.Meters.of(2.5)));
+        assertEquals(tMid, model.getTimeAtY(solution, Units.Meters.of(3.0)));
+    }
+
+    @Test void testCandidateTracking() {
+        ParabolicTimeOfFlightBM model = getModel();
+        Time[] candidates = {
+            Units.Seconds.of(1.0),
+            Units.Seconds.of(1.5),
+            Units.Seconds.of(2.0),
+            Units.Seconds.of(2.5),
+            Units.Seconds.of(3.0)
+        };
+
+        for(int cur_candidate = 0; cur_candidate < candidates.length; ++cur_candidate) {
+            // Check that existing candidates have been checked
+            for(int seen_candidate = 0; seen_candidate < cur_candidate; ++seen_candidate) {
+                assertTrue(
+                    model.paramComputed(candidates[seen_candidate]),
+                    String.format(
+                        "Cur Candidate: %d, Seen Candidate: %d",
+                        cur_candidate, seen_candidate
+                    )
+                );
+            }
+
+            // Check that unseen candidates haven't been checked
+            for(int unseen_candidate = cur_candidate; unseen_candidate < candidates.length; ++unseen_candidate) {
+                assertFalse(
+                    model.paramComputed(candidates[unseen_candidate]),
+                    String.format(
+                        "Cur Candidate: %d, Unseen Candidate: %d",
+                        cur_candidate, unseen_candidate
+                    )
+                );
+            }
+
+            // Compute for the candidate
+            model.getCandidateVelocity(candidates[cur_candidate]);
+        }
+    }
+}