From a0eb653afdbacff0d1a84836879db8c293829b69 Mon Sep 17 00:00:00 2001
From: Ray Healy <raymond.m.healy@gmail.com>
Date: Sat, 14 Feb 2026 15:04:00 -0500
Subject: [PATCH] trajectory/ballistics/constraints: Added HeightAtPositionBC

Checks that z value is above a given minimum at
a given X, Y, or target radial position
---
 .../constraints/HeightAtPositionBC.java       | 93 +++++++++++++++++++
 .../exceptions/UnhandledEnumException.java    | 14 +++
 2 files changed, 107 insertions(+)
 create mode 100644 lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/constraints/HeightAtPositionBC.java
 create mode 100644 lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/exceptions/UnhandledEnumException.java

diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/constraints/HeightAtPositionBC.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/constraints/HeightAtPositionBC.java
new file mode 100644
index 0000000..05e49d5
--- /dev/null
+++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/constraints/HeightAtPositionBC.java
@@ -0,0 +1,93 @@
+package org.fairportrobotics.frc.robolib.trajectory.ballistics.constraints;
+
+import org.fairportrobotics.frc.robolib.Util;
+import org.fairportrobotics.frc.robolib.trajectory.ballistics.exceptions.UnhandledEnumException;
+import org.fairportrobotics.frc.robolib.trajectory.ballistics.models.BallisticModel;
+import org.fairportrobotics.frc.robolib.trajectory.ballistics.models.ReversableRadialBM;
+
+import edu.wpi.first.units.Units;
+import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.units.measure.Time;
+
+/**
+ * Ballistic constraint requiring ball be a minimum height at a given
+ * radius from the target
+ */
+public class HeightAtPositionBC extends BallisticConstraint {
+
+    enum PositionType {
+        TargetRadius,
+        AbsoluteX,
+        AbsoluteY,
+        RelativeX,
+        RelativeY
+    };
+
+    private final double minHeightMeters;
+    private final double safeHeightMeters;
+    private final Distance testPosition;
+    private final PositionType positionType;
+
+    /**
+     * @param minHeight The minimum passing height
+     * @param safeHeight The height above which no penalty is applied
+     * @param testPosition The position to test at
+     * @param positionType The type of position check to run
+     */
+    public HeightAtPositionBC(
+        Distance minHeight, Distance safeHeight, Distance testRadius, PositionType positionType
+    ) {
+        this.minHeightMeters    = minHeight.in(Units.Meters);
+        this.safeHeightMeters   = safeHeight.in(Units.Meters);
+        this.testPosition       = testRadius;
+        this.positionType       = positionType;
+    }
+
+
+    public <M extends ReversableRadialBM<P>, P> Double evaluate(
+        M model, BCEvalParams evalParams
+    ) throws UnhandledEnumException{
+        Time timeAtTarget;
+        switch(this.positionType) {
+            case TargetRadius:
+                timeAtTarget = model.getTimeAtRadius(
+                    evalParams.getCandidateVelocity(),
+                    model.getRelativeHorizontalDistance().minus(testPosition)
+                );
+                break;
+            case AbsoluteX:
+                timeAtTarget = model.getTimeAtX(evalParams.getCandidateVelocity(), testPosition);
+                break;
+            case AbsoluteY:
+                timeAtTarget = model.getTimeAtY(evalParams.getCandidateVelocity(), testPosition);
+                break;
+            case RelativeX:
+                timeAtTarget = model.getTimeAtXRelative(evalParams.getCandidateVelocity(), testPosition);
+                break;
+            case RelativeY:
+                timeAtTarget = model.getTimeAtYRelative(evalParams.getCandidateVelocity(), testPosition);
+                break;
+            default:
+                throw new UnhandledEnumException(this.positionType.name());
+        }
+        double zAtTargetMeters = model.positionAtTime(
+            evalParams.getCandidateVelocity(),
+            timeAtTarget
+        ).getMeasureZ().in(Units.Meters);
+
+        if(zAtTargetMeters > safeHeightMeters) {
+            return 0.0;
+        } else if(zAtTargetMeters < minHeightMeters) {
+            return null;
+        } else {
+            return Util.rangePercent(minHeightMeters, safeHeightMeters, zAtTargetMeters);
+        }
+    }
+
+
+    public <M extends BallisticModel<P>, P> Double evaluate(
+        M model, BCEvalParams evalParams
+    ) {
+        return null;
+    }
+}
diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/exceptions/UnhandledEnumException.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/exceptions/UnhandledEnumException.java
new file mode 100644
index 0000000..168f750
--- /dev/null
+++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/exceptions/UnhandledEnumException.java
@@ -0,0 +1,14 @@
+package org.fairportrobotics.frc.robolib.trajectory.ballistics.exceptions;
+
+/**
+ * Base exception for the ballistic calculator
+ */
+public class UnhandledEnumException extends BallisticException {
+    public UnhandledEnumException() {
+        super();
+    }
+
+    public UnhandledEnumException(String message) {
+        super(message);
+    }
+}