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 224af83..2f98288 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 @@ -3,15 +3,26 @@ 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; public abstract class BallisticModel
{ protected Translation3d posLaunch; protected Translation3d posTarget; + protected final Translation3d targetPosRelative; + protected final Distance relativeHorizontalDistance; + public BallisticModel(Translation3d posLaunch, Translation3d posTarget) { this.posLaunch = posLaunch; this.posTarget = posTarget; + + 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) + )); } // @@ -26,6 +37,14 @@ public abstract class BallisticModel
{ return this.posTarget; } + public Translation3d getTargetPosRelative() { + return this.targetPosRelative; + } + + public Distance getRelativeHorizontalDistance() { + return this.relativeHorizontalDistance; + } + // // Parameter Manipulation // diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/models/ParabolicTimeOfFlightBM.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/models/ParabolicTimeOfFlightBM.java index ba4f365..5b0d247 100644 --- a/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/models/ParabolicTimeOfFlightBM.java +++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/trajectory/ballistics/models/ParabolicTimeOfFlightBM.java @@ -20,8 +20,6 @@ public class ParabolicTimeOfFlightBM extends ReversableRadialBM