src/robolib/trajectory/ballistics/models: Define base classes for ballistic models

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

@@ -0,0 +1,44 @@
+package org.fairportrobotics.frc.robolib.trajectory.ballistics.models;
+
+import org.fairportrobotics.frc.robolib.trajectory.Velocity3d;
+
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.units.measure.Time;
+
+public abstract class BallisticModel<P> {
+    protected Translation3d posLaunch;
+    protected Translation3d posTarget;
+
+    public BallisticModel(Translation3d posLaunch, Translation3d posTarget) {
+        this.posLaunch      = posLaunch;
+        this.posTarget      = posTarget;
+    }
+
+    /**
+     * 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
+     * the next batch of candidates that (may) improve on this one)
+     *
+     * @param param The base parameter
+     * @return An array containing all neighbor params
+     */
+    public abstract P[] getNeighborParams(P param);
+
+    /**
+     * Get the position of the projectile at a given time offset
+     *
+     * @param velocity The velocity to check
+     * @param time The time to check
+     * @return The expected position at the given time
+     */
+    public abstract Translation3d positionAtTime(Velocity3d velocity, Time time);
+
+}

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

@@ -0,0 +1,59 @@
+package org.fairportrobotics.frc.robolib.trajectory.ballistics.models;
+
+import org.fairportrobotics.frc.robolib.trajectory.Velocity3d;
+
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.units.Units;
+import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.units.measure.Time;
+
+/**
+ * Base class for Ballistic models where translation is radial only (i.e. no side-slip) and
+ * each radial position corresponds to exactly one time (e.g. a simple parabolic trajectory)
+ */
+public abstract class ReversableRadialBM<P> extends BallisticModel<P> {
+    public ReversableRadialBM(Translation3d posLaunch, Translation3d posTarget) {
+        super(posLaunch, posTarget);
+    }
+
+    /**
+     * Get the time taken to travel a given radial distance from posLaunch towards posTarget
+     *
+     * @param candidate The generated velocity to compute against
+     * @param radialDistance The radial distance from posLaunch to posTarget to solve for
+     * @return The time it takes to travel radialDistance from posTarget towards posTarget
+     */
+    public abstract Time getTimeAtRadius(Velocity3d candidate, Distance radialDistance);
+
+
+    /**
+     * Get the time taken to travel a given X distance (relative to posLaunch) on a given candidate
+     * trajectory
+     *
+     * @param candidate The generated velocity to compute against
+     * @param xDistance The X distance relative to posLaunch to solve for
+     * @return The time it takes to travel a given X distance relative to posTarget
+     */
+    public Time getTimeAtX(Velocity3d candidate, Distance xDistance) {
+        Distance radialDistance = Units.Meters.of(
+            xDistance.in(Units.Meters)/Math.cos(candidate.getAzimuthAngle().in(Units.Radians))
+        );
+        return this.getTimeAtRadius(candidate, radialDistance);
+    }
+
+
+    /**
+     * Get the time taken to travel a given Y distance (relative to posLaunch) on a given candidate
+     * trajectory
+     *
+     * @param candidate The generated velocity to compute against
+     * @param yDistance The Y distance relative to posLaunch to solve for
+     * @return The time it takes to travel a given Y distance relative to posTarget
+     */
+    public Time getTimeAtY(Velocity3d candidate, Distance yDistance) {
+        Distance radialDistance = Units.Meters.of(
+            yDistance.in(Units.Meters)/Math.sin(candidate.getAzimuthAngle().in(Units.Radians))
+        );
+        return this.getTimeAtRadius(candidate, radialDistance);
+    }
+}