trajectory/ballistics/models: Reorganize getters
Moved targetPosRelative and relativeHorazontalDistance calc and getters into BallisticModel.java
This commit is contained in:
parent
ef6a02cffc
commit
66150eea99
|
|
@ -3,15 +3,26 @@ package org.fairportrobotics.frc.robolib.trajectory.ballistics.models;
|
||||||
import org.fairportrobotics.frc.robolib.trajectory.Velocity3d;
|
import org.fairportrobotics.frc.robolib.trajectory.Velocity3d;
|
||||||
|
|
||||||
import edu.wpi.first.math.geometry.Translation3d;
|
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;
|
import edu.wpi.first.units.measure.Time;
|
||||||
|
|
||||||
public abstract class BallisticModel<P> {
|
public abstract class BallisticModel<P> {
|
||||||
protected Translation3d posLaunch;
|
protected Translation3d posLaunch;
|
||||||
protected Translation3d posTarget;
|
protected Translation3d posTarget;
|
||||||
|
|
||||||
|
protected final Translation3d targetPosRelative;
|
||||||
|
protected final Distance relativeHorizontalDistance;
|
||||||
|
|
||||||
public BallisticModel(Translation3d posLaunch, Translation3d posTarget) {
|
public BallisticModel(Translation3d posLaunch, Translation3d posTarget) {
|
||||||
this.posLaunch = posLaunch;
|
this.posLaunch = posLaunch;
|
||||||
this.posTarget = posTarget;
|
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<P> {
|
||||||
return this.posTarget;
|
return this.posTarget;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
public Translation3d getTargetPosRelative() {
|
||||||
|
return this.targetPosRelative;
|
||||||
|
}
|
||||||
|
|
||||||
|
public Distance getRelativeHorizontalDistance() {
|
||||||
|
return this.relativeHorizontalDistance;
|
||||||
|
}
|
||||||
|
|
||||||
//
|
//
|
||||||
// Parameter Manipulation
|
// Parameter Manipulation
|
||||||
//
|
//
|
||||||
|
|
|
||||||
|
|
@ -20,8 +20,6 @@ public class ParabolicTimeOfFlightBM extends ReversableRadialBM<Time> {
|
||||||
protected final Time tofMax;
|
protected final Time tofMax;
|
||||||
protected final Time tofStep;
|
protected final Time tofStep;
|
||||||
|
|
||||||
protected final Translation3d targetPosRelative;
|
|
||||||
protected final Distance relativeHorizontalDistance;
|
|
||||||
protected final Angle azimuth;
|
protected final Angle azimuth;
|
||||||
|
|
||||||
private HashSet<Double> computedSeconds;
|
private HashSet<Double> computedSeconds;
|
||||||
|
|
@ -38,13 +36,6 @@ public class ParabolicTimeOfFlightBM extends ReversableRadialBM<Time> {
|
||||||
this.tofStep = tofStep;
|
this.tofStep = tofStep;
|
||||||
|
|
||||||
// Pre-compute components
|
// 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(
|
this.azimuth = Units.Radians.of(
|
||||||
Math.atan2(targetPosRelative.getY(), targetPosRelative.getX()
|
Math.atan2(targetPosRelative.getY(), targetPosRelative.getX()
|
||||||
));
|
));
|
||||||
|
|
@ -61,21 +52,16 @@ public class ParabolicTimeOfFlightBM extends ReversableRadialBM<Time> {
|
||||||
return this.tofMin;
|
return this.tofMin;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
public Time getTOFMax() {
|
public Time getTOFMax() {
|
||||||
return this.tofMax;
|
return this.tofMax;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
public Time getTOFStep() {
|
public Time getTOFStep() {
|
||||||
return this.tofStep;
|
return this.tofStep;
|
||||||
}
|
}
|
||||||
|
|
||||||
public Translation3d getTargetPosRelative() {
|
|
||||||
return this.targetPosRelative;
|
|
||||||
}
|
|
||||||
|
|
||||||
public Distance getRelativeHorizontalDistance() {
|
|
||||||
return this.relativeHorizontalDistance;
|
|
||||||
}
|
|
||||||
|
|
||||||
public Angle getAzimuth() {
|
public Angle getAzimuth() {
|
||||||
return this.azimuth;
|
return this.azimuth;
|
||||||
|
|
|
||||||
|
|
@ -74,4 +74,5 @@ public abstract class ReversableRadialBM<P> extends BallisticModel<P> {
|
||||||
public Time getTimeAtTarget(Velocity3d candidate) {
|
public Time getTimeAtTarget(Velocity3d candidate) {
|
||||||
return this.getTimeAtX(candidate, posTarget.getMeasureX());
|
return this.getTimeAtX(candidate, posTarget.getMeasureX());
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue