{ + 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); + +} diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/models/ReversableRadialBM.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/models/ReversableRadialBM.java new file mode 100644 index 0000000..513d2df --- /dev/null +++ b/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
extends BallisticModel
{ + 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); + } +}