src/robolib/trajectory/ballistics: Basic impl of calculator base class

- BallisticCalculator.java: The base class for
calculating solutions
- BCResult.java: Holds computed solution and
related values
- BCEvalParams.java: Stores and computes values
used in candidate evaluation
- BallisticConstraint.java: evaluate() takes
BCEvalParams

lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/calculators/BCResult.java

@@ -0,0 +1,27 @@
+package org.fairportrobotics.frc.robolib.trajectory.ballistics.calculators;
+
+import org.fairportrobotics.frc.robolib.trajectory.Velocity3d;
+
+public class BCResult<P> {
+    public final Double     velPenalty;     /// The penalty for the computed launcher velocity (null if invalid)
+    public final Velocity3d velGlobal;      /// The computed launcher velocity (in the global coorinate space)
+    public final Velocity3d velRelative;    /// The computed launcher velocity (in the robot-relative coordinate space)
+    public final P          velParam;       /// The model parameter that yielded velGlobal
+
+
+    public BCResult(
+        Double      velPenalty,
+        Velocity3d  velGlobal,
+        Velocity3d  velRelative,
+        P           velParam
+    ) {
+        this.velPenalty     = velPenalty;
+        this.velGlobal      = velGlobal;
+        this.velRelative    = velRelative;
+        this.velParam       = velParam;
+    }
+
+    public boolean isValid() {
+        return this.velPenalty != null;
+    }
+}

lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/calculators/BallisticCalculator.java

@@ -0,0 +1,78 @@
+package org.fairportrobotics.frc.robolib.trajectory.ballistics.calculators;
+
+import java.util.ArrayList;
+
+import org.fairportrobotics.frc.robolib.trajectory.Velocity3d;
+import org.fairportrobotics.frc.robolib.trajectory.ballistics.constraints.BCEvalParams;
+import org.fairportrobotics.frc.robolib.trajectory.ballistics.constraints.BallisticConstraint;
+import org.fairportrobotics.frc.robolib.trajectory.ballistics.models.BallisticModel;
+
+import edu.wpi.first.units.measure.Angle;
+
+public abstract class BallisticCalculator {
+    private ArrayList<BallisticConstraint> constraints;
+    private double velocityDiffWeight;
+
+    //
+    //  Setup code
+    //
+
+    public BallisticCalculator() {
+        constraints = new ArrayList<>();
+        this.velocityDiffWeight = 1.0;
+    }
+
+    public BallisticCalculator addConstraint(BallisticConstraint constraint) {
+        constraints.add(constraint);
+        return this;
+    }
+
+    public BallisticCalculator setVelocityDiffWeight(double weight) {
+        this.velocityDiffWeight = weight;
+        return this;
+    }
+
+    //
+    //  Computation
+    //
+
+    /**
+     * Public solution
+     * @param <M> The type of model to compute against
+     * @param <P> The candidate parameter type of the model
+     * @param model The model to compute against
+     * @param initParam The seed parameter (to start computation)
+     * @param velRobot The velocity of the robot (in the model's coordinate system)
+     * @param velShooter The velocity currently imparted by the shooter (in the model's coordinate system)
+     * @param robotHeading The heading of the robot (in the model's coordinate system)
+     * @return The generated BCResult
+     */
+    public abstract <M extends BallisticModel<P>, P> BCResult<P> computeSolution(
+        M model, Velocity3d velRobot, Velocity3d velShooter,
+        Angle robotHeading, boolean quickMode
+    );
+
+
+    protected <M extends BallisticModel<P>, P> Double evaluateCandidate(
+        M model, BCEvalParams evalParams
+    ) {
+        // Get the weight component for velocity difference
+        Velocity3d velShooterDiff =
+            evalParams.getCandidateGlobalShooterVelocity()
+            .minus(evalParams.getCurrentGlobalShooterVelocity());
+        double numerator    = velShooterDiff.getSquareMagnitude() * this.velocityDiffWeight;
+        double denominator  = this.velocityDiffWeight;
+
+        // Evaluate against all constraints
+        for (BallisticConstraint constraint : this.constraints) {
+            Double penaltyPercent = constraint.evaluate(model, evalParams);
+            if(penaltyPercent == null) return null;
+
+            numerator   += penaltyPercent * constraint.getWeight();
+            denominator += constraint.getWeight();
+        }
+
+        // Get the final sum
+        return numerator/denominator;
+    }
+}

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

@@ -0,0 +1,115 @@
+package org.fairportrobotics.frc.robolib.trajectory.ballistics.constraints;
+
+import org.fairportrobotics.frc.robolib.trajectory.Velocity3d;
+
+import edu.wpi.first.units.measure.Angle;
+
+/**
+ * Param block for ballistic constraint and calculator evaluation
+ */
+public class BCEvalParams {
+    //
+    //  Members
+    //
+
+    // Robot motion params
+    private final Velocity3d currentGlobalRobotVelocity;
+    private final Angle      currentGlobalRobotAngle;
+    private final Velocity3d currentGlobalShooterVelocity;
+
+    // Candidate params
+    private final Velocity3d candidateGlobalTotalVelocity;
+
+    // Composite value cache
+    private Velocity3d cache_candidateGlobalShooterVelocity;
+    private Velocity3d cache_candidateRelativeShooterVelocity;
+
+    //
+    // Setup
+    //
+
+    public BCEvalParams(
+        Velocity3d currentGlobalRobotVelocity,
+        Angle      currentGlobalRobotAngle,
+        Velocity3d currentGlobalShooterVelocity,
+        Velocity3d candidateGlobalTotalVelocity
+    ) {
+        this.currentGlobalRobotVelocity     = currentGlobalRobotVelocity;
+        this.currentGlobalRobotAngle        = currentGlobalRobotAngle;
+        this.currentGlobalShooterVelocity   = currentGlobalShooterVelocity;
+        this.candidateGlobalTotalVelocity   = candidateGlobalTotalVelocity;
+
+        this.cache_candidateGlobalShooterVelocity   = null;
+        this.cache_candidateRelativeShooterVelocity = null;
+    }
+
+
+    //
+    //  Base attribute getters
+    //
+
+    /**
+     * @return The robot's current velocity (in field coordinates)
+     */
+    public Velocity3d getCurrentGlobalRobotVelocity() {
+        return this.currentGlobalRobotVelocity;
+    }
+
+
+    /**
+     * @return The robot's current heading angle (in field coordinates)
+     */
+    public Angle      getCurrentGlobalRobotAngle() {
+        return this.currentGlobalRobotAngle;
+    }
+
+
+    /**
+     * @return The shooter's current launch velocity (in field coordinates)
+     */
+    public Velocity3d getCurrentGlobalShooterVelocity() {
+        return this.currentGlobalShooterVelocity;
+    }
+
+    /**
+     * @return The candidate total (robot + shooter) velocity (in field coordinates)
+     */
+    public Velocity3d getCandidateGlobalTotalVelocity() {
+        return this.candidateGlobalTotalVelocity;
+    }
+
+    //
+    //  Composite value getters
+    //
+
+    /**
+     * @return The candidate shooter velocity (in field coordinates) at current
+     *  robot velocity
+     */
+    public Velocity3d getCandidateGlobalShooterVelocity() {
+        if(this.cache_candidateGlobalShooterVelocity == null) {
+            this.cache_candidateGlobalShooterVelocity =
+                this.candidateGlobalTotalVelocity
+                    .minus(this.currentGlobalRobotVelocity);
+        }
+        return this.cache_candidateGlobalShooterVelocity;
+    }
+
+    /**
+     * @return The candidate shooter velocity (in robot coordinates) at current
+     *  robot velocity and heading
+     */
+    public Velocity3d getCandidateRelativeShooterVelocity() {
+        if(this.cache_candidateRelativeShooterVelocity == null) {
+            this.cache_candidateRelativeShooterVelocity =
+                new Velocity3d(
+                    this.getCandidateGlobalShooterVelocity().getHorizontalVelocity(),
+                    this.getCandidateGlobalShooterVelocity().getVeritcalVelocity(),
+                    this.getCandidateGlobalShooterVelocity().getAzimuthAngle()
+                        .minus(this.getCurrentGlobalRobotAngle())
+                );
+        }
+        return this.cache_candidateRelativeShooterVelocity;
+    }
+
+}

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

@@ -1,8 +1,8 @@
 package org.fairportrobotics.frc.robolib.trajectory.ballistics.constraints;
 
-import org.fairportrobotics.frc.robolib.trajectory.Velocity3d;
 import org.fairportrobotics.frc.robolib.trajectory.ballistics.models.BallisticModel;
 
+
 public abstract class BallisticConstraint {
     protected double weight;
 
@@ -42,14 +42,14 @@ public abstract class BallisticConstraint {
     /**
      * Evaluate a given solution's fitness against this constraint
      *
-     * TODO: Expand this with additional parameters
-     *
      * @param <M> The type of BallisticModel
      * @param <P> The parameter type of M
-     * @param model The model used to compute candidate
+     * @param model The model used to compute the candidate
-     * @param candidate The candidate velocity computed by model
+     * @param evalParams The candidate parameters used in evaluation
      * @return A penalty value (from 0 to 1.0) for this match (null if candidate
      *  is invalid). Used with weight to determine candidate fitness
      */
-    public abstract <M extends BallisticModel<P>, P> Double evaluate(M model, Velocity3d candidate);
+    public abstract <M extends BallisticModel<P>, P> Double evaluate(
+        M model, BCEvalParams evalParams
+    );
 }