From 2056de0a1a4b11990c8e91ef741e2f30b54a727b Mon Sep 17 00:00:00 2001
From: Ray Healy <raymond.m.healy@gmail.com>
Date: Tue, 10 Feb 2026 19:36:51 -0500
Subject: [PATCH] src/robolib/trajectory/ballistics/models: Implemented a
 simple parabolic ballistic model with ToF as a param

Also require a function that provides an initial
param in BallisticModel
---
 .../ballistics/models/BallisticModel.java     |  5 +
 .../ballistics/models/SimpleTOFBM.java        | 96 +++++++++++++++++++
 2 files changed, 101 insertions(+)
 create mode 100644 lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/models/SimpleTOFBM.java

diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/models/BallisticModel.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/models/BallisticModel.java
index ae9f874..d1da39a 100644
--- a/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/models/BallisticModel.java
+++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/models/BallisticModel.java
@@ -14,6 +14,11 @@ public abstract class BallisticModel<P> {
         this.posTarget      = posTarget;
     }
 
+    /**
+     * @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
diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/models/SimpleTOFBM.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/models/SimpleTOFBM.java
new file mode 100644
index 0000000..7f3c01c
--- /dev/null
+++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/models/SimpleTOFBM.java
@@ -0,0 +1,96 @@
+package org.fairportrobotics.frc.robolib.trajectory.ballistics.models;
+
+import java.util.ArrayList;
+
+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.Angle;
+import edu.wpi.first.units.measure.Distance;
+import edu.wpi.first.units.measure.LinearAcceleration;
+import edu.wpi.first.units.measure.Time;
+
+
+public class SimpleTOFBM extends ReversableRadialBM<Time> {
+
+    private final LinearAcceleration k_gravity = Units.MetersPerSecondPerSecond.of(-9.81);
+
+    protected final Time tofMin;
+    protected final Time tofMax;
+    protected final Time tofStep;
+
+    protected final Translation3d   targetPosRelative;
+    protected final Distance        relativeHorizontalDistance;
+    protected final Angle           azimuth;
+
+    public SimpleTOFBM(
+        Translation3d posLaunch, Translation3d posTarget,
+        Time tofMin, Time tofMax, Time tofStep
+    ) {
+        super(posLaunch, posTarget);
+
+        // Get time of flight params
+        this.tofMin     = tofMin;
+        this.tofMax     = tofMax;
+        this.tofStep    = tofStep;
+
+        // Pre-compute components
+        this.targetPosRelative = this.posTarget.minus(this.posLaunch);
+        this.relativeHorizontalDistance = Units.Meters.of(
+            Math.hypot(
+                this.targetPosRelative.getMeasureX().in(Units.Meters),
+                this.targetPosRelative.getMeasureY().in(Units.Meters)
+            )
+        );
+        this.azimuth = Units.Radians.of(
+            Math.atan2(targetPosRelative.getY(), targetPosRelative.getX()
+        ));
+    }
+
+
+    public Time getInitParam() {
+        return this.tofMin.plus(this.tofMax).div(2);
+    }
+
+
+    public Velocity3d getCandidateVelocity(Time param) {
+        return new Velocity3d(
+            this.relativeHorizontalDistance.div(param),
+            this.targetPosRelative.getMeasureZ().div(param)
+                .plus(k_gravity.times(param).times(0.5)),
+            azimuth
+        );
+    }
+
+
+    public Time[] getNeighborParams(Time param) {
+        ArrayList<Time> neighbors = new ArrayList<>();
+
+        Time            lower  = param.minus(tofStep);
+        if(lower.gte(tofMin))   neighbors.add(lower);
+
+        Time            higher = param.plus(tofStep);
+        if(higher.lte(tofMax))  neighbors.add(higher);
+
+        return neighbors.toArray(new Time[0]);
+    }
+
+
+    public Translation3d positionAtTime(Velocity3d velocity, Time time) {
+        return new Translation3d(
+            this.posLaunch.getMeasureX()
+                .plus(velocity.getXVelocity().times(time)),
+            this.posLaunch.getMeasureY()
+                .plus(velocity.getYVelocity().times(time)),
+            this.posLaunch.getMeasureZ()
+                .plus(velocity.getZVelocity().times(time))
+                .plus(k_gravity.times(time).times(time).times(0.5))
+        );
+    }
+
+
+    public Time getTimeAtRadius(Velocity3d candidate, Distance radialDistance) {
+        return radialDistance.div(candidate.getHorizontalVelocity());
+    }
+}