From 520b2765da8d039a6935c7e35fe1e27369f952b0 Mon Sep 17 00:00:00 2001 From: Blasty25 Date: Wed, 4 Feb 2026 22:04:28 -0500 Subject: [PATCH 1/2] adding traj logging --- FuelSim.java | 1586 +++++++++++++++++++++++++++----------------------- 1 file changed, 863 insertions(+), 723 deletions(-) diff --git a/FuelSim.java b/FuelSim.java index 65a6357..9aad774 100644 --- a/FuelSim.java +++ b/FuelSim.java @@ -3,6 +3,8 @@ import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.Radians; +import static frc.robot.Constants.hubCenter; +import static frc.robot.Constants.targetZ; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; @@ -12,797 +14,935 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.LinearVelocity; import java.util.ArrayList; +import java.util.List; import java.util.function.BooleanSupplier; import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; public class FuelSim { - private static final double PERIOD = 0.02; // sec - private static int subticks = 5; - private static final Translation3d GRAVITY = new Translation3d(0, 0, -9.81); // m/s^2 - // Room temperature dry air density: https://en.wikipedia.org/wiki/Density_of_air#Dry_air - private static final double AIR_DENSITY = 1.2041; // kg/m^3 - private static final double FIELD_COR = Math.sqrt(22 / 51.5); // coefficient of restitution with the field - private static final double FUEL_COR = 0.5; // coefficient of restitution with another fuel - private static final double NET_COR = 0.2; // coefficient of restitution with the net - private static final double ROBOT_COR = 0.1; // coefficient of restitution with a robot - private static final double FUEL_RADIUS = 0.075; - private static final double FIELD_LENGTH = 16.51; - private static final double FIELD_WIDTH = 8.04; - private static final double TRENCH_WIDTH = 1.265; - private static final double TRENCH_BLOCK_WIDTH = 0.305; - private static final double TRENCH_HEIGHT = 0.565; - private static final double TRENCH_BAR_HEIGHT = 0.102; - private static final double TRENCH_BAR_WIDTH = 0.152; - private static final double FRICTION = 0.1; // proportion of horizontal velocity to lose per second while on ground - private static final double FUEL_MASS = 0.448 * 0.45392; // kgs - private static final double FUEL_CROSS_AREA = Math.PI * FUEL_RADIUS * FUEL_RADIUS; - // Drag coefficient of smooth sphere: https://en.wikipedia.org/wiki/Drag_coefficient#/media/File:14ilf1l.svg - private static final double DRAG_COF = 0.47; // dimensionless - private static final double DRAG_FORCE_FACTOR = 0.5 * AIR_DENSITY * DRAG_COF * FUEL_CROSS_AREA; - - private static FuelSim instance = null; - - private static final Translation3d[] FIELD_XZ_LINE_STARTS = { - new Translation3d(0, 0, 0), - new Translation3d(3.96, 1.57, 0), - new Translation3d(3.96, FIELD_WIDTH / 2 + 0.60, 0), - new Translation3d(4.61, 1.57, 0.165), - new Translation3d(4.61, FIELD_WIDTH / 2 + 0.60, 0.165), - new Translation3d(FIELD_LENGTH - 5.18, 1.57, 0), - new Translation3d(FIELD_LENGTH - 5.18, FIELD_WIDTH / 2 + 0.60, 0), - new Translation3d(FIELD_LENGTH - 4.61, 1.57, 0.165), - new Translation3d(FIELD_LENGTH - 4.61, FIELD_WIDTH / 2 + 0.60, 0.165), - new Translation3d(3.96, TRENCH_WIDTH, TRENCH_HEIGHT), - new Translation3d(3.96, FIELD_WIDTH - 1.57, TRENCH_HEIGHT), - new Translation3d(FIELD_LENGTH - 5.18, TRENCH_WIDTH, TRENCH_HEIGHT), - new Translation3d(FIELD_LENGTH - 5.18, FIELD_WIDTH - 1.57, TRENCH_HEIGHT), - new Translation3d(4.61 - TRENCH_BAR_WIDTH / 2, 0, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), - new Translation3d(4.61 - TRENCH_BAR_WIDTH / 2, FIELD_WIDTH - 1.57, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), - new Translation3d(FIELD_LENGTH - 4.61 - TRENCH_BAR_WIDTH / 2, 0, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), - new Translation3d( - FIELD_LENGTH - 4.61 - TRENCH_BAR_WIDTH / 2, FIELD_WIDTH - 1.57, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), - }; - - private static final Translation3d[] FIELD_XZ_LINE_ENDS = { - new Translation3d(FIELD_LENGTH, FIELD_WIDTH, 0), - new Translation3d(4.61, FIELD_WIDTH / 2 - 0.60, 0.165), - new Translation3d(4.61, FIELD_WIDTH - 1.57, 0.165), - new Translation3d(5.18, FIELD_WIDTH / 2 - 0.60, 0), - new Translation3d(5.18, FIELD_WIDTH - 1.57, 0), - new Translation3d(FIELD_LENGTH - 4.61, FIELD_WIDTH / 2 - 0.60, 0.165), - new Translation3d(FIELD_LENGTH - 4.61, FIELD_WIDTH - 1.57, 0.165), - new Translation3d(FIELD_LENGTH - 3.96, FIELD_WIDTH / 2 - 0.60, 0), - new Translation3d(FIELD_LENGTH - 3.96, FIELD_WIDTH - 1.57, 0), - new Translation3d(5.18, TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT), - new Translation3d(5.18, FIELD_WIDTH - 1.57 + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT), - new Translation3d(FIELD_LENGTH - 3.96, TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT), - new Translation3d(FIELD_LENGTH - 3.96, FIELD_WIDTH - 1.57 + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT), - new Translation3d( - 4.61 + TRENCH_BAR_WIDTH / 2, TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), - new Translation3d(4.61 + TRENCH_BAR_WIDTH / 2, FIELD_WIDTH, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), - new Translation3d( - FIELD_LENGTH - 4.61 + TRENCH_BAR_WIDTH / 2, - TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, - TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), - new Translation3d(FIELD_LENGTH - 4.61 + TRENCH_BAR_WIDTH / 2, FIELD_WIDTH, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), - }; - - private class Fuel { - private Translation3d pos; - private Translation3d vel; - - private Fuel(Translation3d pos, Translation3d vel) { - this.pos = pos; - this.vel = vel; - } - - private Fuel(Translation3d pos) { - this(pos, new Translation3d()); - } - - private void update() { - pos = pos.plus(vel.times(PERIOD / subticks)); - if (pos.getZ() > FUEL_RADIUS) { - Translation3d Fg = GRAVITY.times(FUEL_MASS); - Translation3d Fd = new Translation3d(); - - if (simulateAirResistance) { - double speed = vel.getNorm(); - if (speed > 1e-6) { - Fd = vel.times(-DRAG_FORCE_FACTOR * speed); - } - } - - Translation3d accel = Fg.plus(Fd).div(FUEL_MASS); - vel = vel.plus(accel.times(PERIOD / subticks)); - } - if (Math.abs(vel.getZ()) < 0.05 && pos.getZ() <= FUEL_RADIUS + 0.03) { - vel = new Translation3d(vel.getX(), vel.getY(), 0); - vel = vel.times(1 - FRICTION * PERIOD / subticks); - // pos = new Translation3d(pos.getX(), pos.getY(), FUEL_RADIUS); - } - handleFieldCollisions(); - } - - private void handleXZLineCollision(Translation3d lineStart, Translation3d lineEnd) { - if (pos.getY() < lineStart.getY() || pos.getY() > lineEnd.getY()) return; // not within y range - // Convert into 2D - Translation2d start2d = new Translation2d(lineStart.getX(), lineStart.getZ()); - Translation2d end2d = new Translation2d(lineEnd.getX(), lineEnd.getZ()); - Translation2d pos2d = new Translation2d(pos.getX(), pos.getZ()); - Translation2d lineVec = end2d.minus(start2d); - - // Get closest point on line - Translation2d projected = - start2d.plus(lineVec.times(pos2d.minus(start2d).dot(lineVec) / lineVec.getSquaredNorm())); - - if (projected.getDistance(start2d) + projected.getDistance(end2d) > lineVec.getNorm()) - return; // projected point not on line - double dist = pos2d.getDistance(projected); - if (dist > FUEL_RADIUS) return; // not intersecting line - // Back into 3D - Translation3d normal = new Translation3d(-lineVec.getY(), 0, lineVec.getX()).div(lineVec.getNorm()); - - // Apply collision response - pos = pos.plus(normal.times(FUEL_RADIUS - dist)); - if (vel.dot(normal) > 0) return; // already moving away from line - vel = vel.minus(normal.times((1 + FIELD_COR) * vel.dot(normal))); - } - - private void handleFieldCollisions() { - // floor and bumps - for (int i = 0; i < FIELD_XZ_LINE_STARTS.length; i++) { - handleXZLineCollision(FIELD_XZ_LINE_STARTS[i], FIELD_XZ_LINE_ENDS[i]); - } - - // edges - if (pos.getX() < FUEL_RADIUS && vel.getX() < 0) { - pos = pos.plus(new Translation3d(FUEL_RADIUS - pos.getX(), 0, 0)); - vel = vel.plus(new Translation3d(-(1 + FIELD_COR) * vel.getX(), 0, 0)); - } else if (pos.getX() > FIELD_LENGTH - FUEL_RADIUS && vel.getX() > 0) { - pos = pos.plus(new Translation3d(FIELD_LENGTH - FUEL_RADIUS - pos.getX(), 0, 0)); - vel = vel.plus(new Translation3d(-(1 + FIELD_COR) * vel.getX(), 0, 0)); - } - - if (pos.getY() < FUEL_RADIUS && vel.getY() < 0) { - pos = pos.plus(new Translation3d(0, FUEL_RADIUS - pos.getY(), 0)); - vel = vel.plus(new Translation3d(0, -(1 + FIELD_COR) * vel.getY(), 0)); - } else if (pos.getY() > FIELD_WIDTH - FUEL_RADIUS && vel.getY() > 0) { - pos = pos.plus(new Translation3d(0, FIELD_WIDTH - FUEL_RADIUS - pos.getY(), 0)); - vel = vel.plus(new Translation3d(0, -(1 + FIELD_COR) * vel.getY(), 0)); - } + private static final double PERIOD = 0.02; // sec + private static int subticks = 5; + private static final Translation3d GRAVITY = new Translation3d(0, 0, -9.81); // m/s^2 + // Room temperature dry air density: + // https://en.wikipedia.org/wiki/Density_of_air#Dry_air + private static final double AIR_DENSITY = 1.2041; // kg/m^3 + private static final double FIELD_COR = Math.sqrt(22 / 51.5); // coefficient of restitution with the field + private static final double FUEL_COR = 0.5; // coefficient of restitution with another fuel + private static final double NET_COR = 0.2; // coefficient of restitution with the net + private static final double ROBOT_COR = 0.1; // coefficient of restitution with a robot + private static final double FUEL_RADIUS = 0.075; + private static final double FIELD_LENGTH = 16.51; + private static final double FIELD_WIDTH = 8.04; + private static final double TRENCH_WIDTH = 1.265; + private static final double TRENCH_BLOCK_WIDTH = 0.305; + private static final double TRENCH_HEIGHT = 0.565; + private static final double TRENCH_BAR_HEIGHT = 0.102; + private static final double TRENCH_BAR_WIDTH = 0.152; + private static final double FRICTION = 0.1; // proportion of horizontal velocity to lose per second while on ground + private static final double FUEL_MASS = 0.448 * 0.45392; // kgs + private static final double FUEL_CROSS_AREA = Math.PI * FUEL_RADIUS * FUEL_RADIUS; + // Drag coefficient of smooth sphere: + // https://en.wikipedia.org/wiki/Drag_coefficient#/media/File:14ilf1l.svg + private static final double DRAG_COF = 0.47; // dimensionless + private static final double DRAG_FORCE_FACTOR = 0.5 * AIR_DENSITY * DRAG_COF * FUEL_CROSS_AREA; + private static final ArrayList> completedTraj = new ArrayList<>(); + + private static FuelSim instance = null; + + private static final Translation3d[] FIELD_XZ_LINE_STARTS = { + new Translation3d(0, 0, 0), + new Translation3d(3.96, 1.57, 0), + new Translation3d(3.96, FIELD_WIDTH / 2 + 0.60, 0), + new Translation3d(4.61, 1.57, 0.165), + new Translation3d(4.61, FIELD_WIDTH / 2 + 0.60, 0.165), + new Translation3d(FIELD_LENGTH - 5.18, 1.57, 0), + new Translation3d(FIELD_LENGTH - 5.18, FIELD_WIDTH / 2 + 0.60, 0), + new Translation3d(FIELD_LENGTH - 4.61, 1.57, 0.165), + new Translation3d(FIELD_LENGTH - 4.61, FIELD_WIDTH / 2 + 0.60, 0.165), + new Translation3d(3.96, TRENCH_WIDTH, TRENCH_HEIGHT), + new Translation3d(3.96, FIELD_WIDTH - 1.57, TRENCH_HEIGHT), + new Translation3d(FIELD_LENGTH - 5.18, TRENCH_WIDTH, TRENCH_HEIGHT), + new Translation3d(FIELD_LENGTH - 5.18, FIELD_WIDTH - 1.57, TRENCH_HEIGHT), + new Translation3d(4.61 - TRENCH_BAR_WIDTH / 2, 0, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), + new Translation3d( + 4.61 - TRENCH_BAR_WIDTH / 2, FIELD_WIDTH - 1.57, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), + new Translation3d( + FIELD_LENGTH - 4.61 - TRENCH_BAR_WIDTH / 2, 0, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), + new Translation3d( + FIELD_LENGTH - 4.61 - TRENCH_BAR_WIDTH / 2, + FIELD_WIDTH - 1.57, + TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), + }; + + private static final Translation3d[] FIELD_XZ_LINE_ENDS = { + new Translation3d(FIELD_LENGTH, FIELD_WIDTH, 0), + new Translation3d(4.61, FIELD_WIDTH / 2 - 0.60, 0.165), + new Translation3d(4.61, FIELD_WIDTH - 1.57, 0.165), + new Translation3d(5.18, FIELD_WIDTH / 2 - 0.60, 0), + new Translation3d(5.18, FIELD_WIDTH - 1.57, 0), + new Translation3d(FIELD_LENGTH - 4.61, FIELD_WIDTH / 2 - 0.60, 0.165), + new Translation3d(FIELD_LENGTH - 4.61, FIELD_WIDTH - 1.57, 0.165), + new Translation3d(FIELD_LENGTH - 3.96, FIELD_WIDTH / 2 - 0.60, 0), + new Translation3d(FIELD_LENGTH - 3.96, FIELD_WIDTH - 1.57, 0), + new Translation3d(5.18, TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT), + new Translation3d(5.18, FIELD_WIDTH - 1.57 + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT), + new Translation3d(FIELD_LENGTH - 3.96, TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT), + new Translation3d(FIELD_LENGTH - 3.96, FIELD_WIDTH - 1.57 + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT), + new Translation3d( + 4.61 + TRENCH_BAR_WIDTH / 2, + TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, + TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), + new Translation3d(4.61 + TRENCH_BAR_WIDTH / 2, FIELD_WIDTH, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), + new Translation3d( + FIELD_LENGTH - 4.61 + TRENCH_BAR_WIDTH / 2, + TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, + TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), + new Translation3d( + FIELD_LENGTH - 4.61 + TRENCH_BAR_WIDTH / 2, FIELD_WIDTH, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), + }; + + private class Fuel { + private Translation3d pos; + private Translation3d vel; + private boolean finished = false; + private final List traj = new ArrayList<>(); + + private Fuel(Translation3d pos, Translation3d vel) { + this.pos = pos; + this.vel = vel; + } - // hubs - handleHubCollisions(Hub.BLUE_HUB); - handleHubCollisions(Hub.RED_HUB); + private Fuel(Translation3d pos) { + this(pos, new Translation3d()); + } - handleTrenchCollisions(); - } + private void update() { + if (!finished) { + traj.add(pos); + } + pos = pos.plus(vel.times(PERIOD / subticks)); + if (pos.getZ() > FUEL_RADIUS) { + Translation3d Fg = GRAVITY.times(FUEL_MASS); + Translation3d Fd = new Translation3d(); + + if (simulateAirResistance) { + double speed = vel.getNorm(); + if (speed > 1e-6) { + Fd = vel.times(-DRAG_FORCE_FACTOR * speed); + } + } + + Translation3d accel = Fg.plus(Fd).div(FUEL_MASS); + vel = vel.plus(accel.times(PERIOD / subticks)); + } + if (Math.abs(vel.getZ()) < 0.05 && pos.getZ() <= FUEL_RADIUS + 0.03) { + vel = new Translation3d(vel.getX(), vel.getY(), 0); + vel = vel.times(1 - FRICTION * PERIOD / subticks); + } + boolean isDone = pos.getZ() <= FUEL_RADIUS + (Units.inchesToMeters(targetZ) / 2) && Math.abs(vel.getZ()) <= 3.0; + + if (isDone) { + finished = true; + } + + handleFieldCollisions(); + } - private void handleHubCollisions(Hub hub) { - hub.handleHubInteraction(this); - hub.fuelCollideSide(this); + private void handleXZLineCollision(Translation3d lineStart, Translation3d lineEnd) { + if (pos.getY() < lineStart.getY() || pos.getY() > lineEnd.getY()) + return; // not within y range + // Convert into 2D + Translation2d start2d = new Translation2d(lineStart.getX(), lineStart.getZ()); + Translation2d end2d = new Translation2d(lineEnd.getX(), lineEnd.getZ()); + Translation2d pos2d = new Translation2d(pos.getX(), pos.getZ()); + Translation2d lineVec = end2d.minus(start2d); + + // Get closest point on line + Translation2d projected = start2d + .plus(lineVec.times(pos2d.minus(start2d).dot(lineVec) / lineVec.getSquaredNorm())); + + if (projected.getDistance(start2d) + projected.getDistance(end2d) > lineVec.getNorm()) + return; // projected point not on line + double dist = pos2d.getDistance(projected); + if (dist > FUEL_RADIUS) + return; // not intersecting line + // Back into 3D + Translation3d normal = new Translation3d(-lineVec.getY(), 0, lineVec.getX()).div(lineVec.getNorm()); + + // Apply collision response + pos = pos.plus(normal.times(FUEL_RADIUS - dist)); + if (vel.dot(normal) > 0) + return; // already moving away from line + vel = vel.minus(normal.times((1 + FIELD_COR) * vel.dot(normal))); + } - double netCollision = hub.fuelHitNet(this); - if (netCollision != 0) { - pos = pos.plus(new Translation3d(netCollision, 0, 0)); - vel = new Translation3d(-vel.getX() * NET_COR, vel.getY() * NET_COR, vel.getZ()); - } - } + private void handleFieldCollisions() { + // floor and bumps + for (int i = 0; i < FIELD_XZ_LINE_STARTS.length; i++) { + handleXZLineCollision(FIELD_XZ_LINE_STARTS[i], FIELD_XZ_LINE_ENDS[i]); + } + + // edges + if (pos.getX() < FUEL_RADIUS && vel.getX() < 0) { + pos = pos.plus(new Translation3d(FUEL_RADIUS - pos.getX(), 0, 0)); + vel = vel.plus(new Translation3d(-(1 + FIELD_COR) * vel.getX(), 0, 0)); + } else if (pos.getX() > FIELD_LENGTH - FUEL_RADIUS && vel.getX() > 0) { + pos = pos.plus(new Translation3d(FIELD_LENGTH - FUEL_RADIUS - pos.getX(), 0, 0)); + vel = vel.plus(new Translation3d(-(1 + FIELD_COR) * vel.getX(), 0, 0)); + } + + if (pos.getY() < FUEL_RADIUS && vel.getY() < 0) { + pos = pos.plus(new Translation3d(0, FUEL_RADIUS - pos.getY(), 0)); + vel = vel.plus(new Translation3d(0, -(1 + FIELD_COR) * vel.getY(), 0)); + } else if (pos.getY() > FIELD_WIDTH - FUEL_RADIUS && vel.getY() > 0) { + pos = pos.plus(new Translation3d(0, FIELD_WIDTH - FUEL_RADIUS - pos.getY(), 0)); + vel = vel.plus(new Translation3d(0, -(1 + FIELD_COR) * vel.getY(), 0)); + } + + // hubs + handleHubCollisions(Hub.BLUE_HUB); + handleHubCollisions(Hub.RED_HUB); + + handleTrenchCollisions(); + } - private void handleTrenchCollisions() { - fuelCollideRectangle( - this, - new Translation3d(3.96, TRENCH_WIDTH, 0), - new Translation3d(5.18, TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT)); - fuelCollideRectangle( - this, - new Translation3d(3.96, FIELD_WIDTH - 1.57, 0), - new Translation3d(5.18, FIELD_WIDTH - 1.57 + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT)); - fuelCollideRectangle( - this, - new Translation3d(FIELD_LENGTH - 5.18, TRENCH_WIDTH, 0), - new Translation3d(FIELD_LENGTH - 3.96, TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT)); - fuelCollideRectangle( - this, - new Translation3d(FIELD_LENGTH - 5.18, FIELD_WIDTH - 1.57, 0), - new Translation3d(FIELD_LENGTH - 3.96, FIELD_WIDTH - 1.57 + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT)); - fuelCollideRectangle( - this, - new Translation3d(4.61 - TRENCH_BAR_WIDTH / 2, 0, TRENCH_HEIGHT), - new Translation3d( - 4.61 + TRENCH_BAR_WIDTH / 2, - TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, - TRENCH_HEIGHT + TRENCH_BAR_HEIGHT)); - fuelCollideRectangle( - this, - new Translation3d(4.61 - TRENCH_BAR_WIDTH / 2, FIELD_WIDTH - 1.57, TRENCH_HEIGHT), - new Translation3d(4.61 + TRENCH_BAR_WIDTH / 2, FIELD_WIDTH, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT)); - fuelCollideRectangle( - this, - new Translation3d(FIELD_LENGTH - 4.61 - TRENCH_BAR_WIDTH / 2, 0, TRENCH_HEIGHT), - new Translation3d( - FIELD_LENGTH - 4.61 + TRENCH_BAR_WIDTH / 2, - TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, - TRENCH_HEIGHT + TRENCH_BAR_HEIGHT)); - fuelCollideRectangle( - this, - new Translation3d(FIELD_LENGTH - 4.61 - TRENCH_BAR_WIDTH / 2, FIELD_WIDTH - 1.57, TRENCH_HEIGHT), - new Translation3d( - FIELD_LENGTH - 4.61 + TRENCH_BAR_WIDTH / 2, - FIELD_WIDTH, - TRENCH_HEIGHT + TRENCH_BAR_HEIGHT)); - } + private void handleHubCollisions(Hub hub) { + if (hub.handleHubInteraction(this)) { + finished = true; + } + hub.fuelCollideSide(this); + + double netCollision = hub.fuelHitNet(this); + if (netCollision != 0) { + pos = pos.plus(new Translation3d(netCollision, 0, 0)); + vel = new Translation3d(-vel.getX() * NET_COR, vel.getY() * NET_COR, vel.getZ()); + } + } - private void addImpulse(Translation3d impulse) { - vel = vel.plus(impulse); - } + private void handleTrenchCollisions() { + fuelCollideRectangle( + this, + new Translation3d(3.96, TRENCH_WIDTH, 0), + new Translation3d(5.18, TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT)); + fuelCollideRectangle( + this, + new Translation3d(3.96, FIELD_WIDTH - 1.57, 0), + new Translation3d(5.18, FIELD_WIDTH - 1.57 + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT)); + fuelCollideRectangle( + this, + new Translation3d(FIELD_LENGTH - 5.18, TRENCH_WIDTH, 0), + new Translation3d(FIELD_LENGTH - 3.96, TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT)); + fuelCollideRectangle( + this, + new Translation3d(FIELD_LENGTH - 5.18, FIELD_WIDTH - 1.57, 0), + new Translation3d( + FIELD_LENGTH - 3.96, FIELD_WIDTH - 1.57 + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT)); + fuelCollideRectangle( + this, + new Translation3d(4.61 - TRENCH_BAR_WIDTH / 2, 0, TRENCH_HEIGHT), + new Translation3d( + 4.61 + TRENCH_BAR_WIDTH / 2, + TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, + TRENCH_HEIGHT + TRENCH_BAR_HEIGHT)); + fuelCollideRectangle( + this, + new Translation3d(4.61 - TRENCH_BAR_WIDTH / 2, FIELD_WIDTH - 1.57, TRENCH_HEIGHT), + new Translation3d( + 4.61 + TRENCH_BAR_WIDTH / 2, FIELD_WIDTH, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT)); + fuelCollideRectangle( + this, + new Translation3d(FIELD_LENGTH - 4.61 - TRENCH_BAR_WIDTH / 2, 0, TRENCH_HEIGHT), + new Translation3d( + FIELD_LENGTH - 4.61 + TRENCH_BAR_WIDTH / 2, + TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, + TRENCH_HEIGHT + TRENCH_BAR_HEIGHT)); + fuelCollideRectangle( + this, + new Translation3d( + FIELD_LENGTH - 4.61 - TRENCH_BAR_WIDTH / 2, FIELD_WIDTH - 1.57, TRENCH_HEIGHT), + new Translation3d( + FIELD_LENGTH - 4.61 + TRENCH_BAR_WIDTH / 2, + FIELD_WIDTH, + TRENCH_HEIGHT + TRENCH_BAR_HEIGHT)); } - private static void handleFuelCollision(Fuel a, Fuel b) { - Translation3d normal = a.pos.minus(b.pos); - double distance = normal.getNorm(); - if (distance == 0) { - normal = new Translation3d(1, 0, 0); - distance = 1; - } - normal = normal.div(distance); - double impulse = 0.5 * (1 + FUEL_COR) * (b.vel.minus(a.vel).dot(normal)); - double intersection = FUEL_RADIUS * 2 - distance; - a.pos = a.pos.plus(normal.times(intersection / 2)); - b.pos = b.pos.minus(normal.times(intersection / 2)); - a.addImpulse(normal.times(impulse)); - b.addImpulse(normal.times(-impulse)); - } - - private static final double CELL_SIZE = 0.25; - private static final int GRID_COLS = (int) Math.ceil(FIELD_LENGTH / CELL_SIZE); - private static final int GRID_ROWS = (int) Math.ceil(FIELD_WIDTH / CELL_SIZE); - - @SuppressWarnings("unchecked") - private final ArrayList[][] grid = new ArrayList[GRID_COLS][GRID_ROWS]; - - private void handleFuelCollisions(ArrayList fuels) { - // Clear grid - for (int i = 0; i < GRID_COLS; i++) { - for (int j = 0; j < GRID_ROWS; j++) { - grid[i][j].clear(); - } - } + private void addImpulse(Translation3d impulse) { + vel = vel.plus(impulse); + } + } + + private static void handleFuelCollision(Fuel a, Fuel b) { + Translation3d normal = a.pos.minus(b.pos); + double distance = normal.getNorm(); + if (distance == 0) { + normal = new Translation3d(1, 0, 0); + distance = 1; + } + normal = normal.div(distance); + double impulse = 0.5 * (1 + FUEL_COR) * (b.vel.minus(a.vel).dot(normal)); + double intersection = FUEL_RADIUS * 2 - distance; + a.pos = a.pos.plus(normal.times(intersection / 2)); + b.pos = b.pos.minus(normal.times(intersection / 2)); + a.addImpulse(normal.times(impulse)); + b.addImpulse(normal.times(-impulse)); + } + + private static final double CELL_SIZE = 0.25; + private static final int GRID_COLS = (int) Math.ceil(FIELD_LENGTH / CELL_SIZE); + private static final int GRID_ROWS = (int) Math.ceil(FIELD_WIDTH / CELL_SIZE); + + @SuppressWarnings("unchecked") + private final ArrayList[][] grid = new ArrayList[GRID_COLS][GRID_ROWS]; + + private void handleFuelCollisions(ArrayList fuels) { + // Clear grid + for (int i = 0; i < GRID_COLS; i++) { + for (int j = 0; j < GRID_ROWS; j++) { + grid[i][j].clear(); + } + } - // Populate grid - for (Fuel fuel : fuels) { - int col = (int) (fuel.pos.getX() / CELL_SIZE); - int row = (int) (fuel.pos.getY() / CELL_SIZE); + // Populate grid + for (Fuel fuel : fuels) { + int col = (int) (fuel.pos.getX() / CELL_SIZE); + int row = (int) (fuel.pos.getY() / CELL_SIZE); - if (col >= 0 && col < GRID_COLS && row >= 0 && row < GRID_ROWS) { - grid[col][row].add(fuel); - } - } + if (col >= 0 && col < GRID_COLS && row >= 0 && row < GRID_ROWS) { + grid[col][row].add(fuel); + } + } - // Check collisions - for (Fuel fuel : fuels) { - int col = (int) (fuel.pos.getX() / CELL_SIZE); - int row = (int) (fuel.pos.getY() / CELL_SIZE); - - // Check 3x3 neighbor cells - for (int i = col - 1; i <= col + 1; i++) { - for (int j = row - 1; j <= row + 1; j++) { - if (i >= 0 && i < GRID_COLS && j >= 0 && j < GRID_ROWS) { - for (Fuel other : grid[i][j]) { - if (fuel != other && fuel.pos.getDistance(other.pos) < FUEL_RADIUS * 2) { - if (fuel.hashCode() < other.hashCode()) { - handleFuelCollision(fuel, other); - } - } - } - } + // Check collisions + for (Fuel fuel : fuels) { + int col = (int) (fuel.pos.getX() / CELL_SIZE); + int row = (int) (fuel.pos.getY() / CELL_SIZE); + + // Check 3x3 neighbor cells + for (int i = col - 1; i <= col + 1; i++) { + for (int j = row - 1; j <= row + 1; j++) { + if (i >= 0 && i < GRID_COLS && j >= 0 && j < GRID_ROWS) { + for (Fuel other : grid[i][j]) { + if (fuel != other && fuel.pos.getDistance(other.pos) < FUEL_RADIUS * 2) { + if (fuel.hashCode() < other.hashCode()) { + handleFuelCollision(fuel, other); } + } } + } } + } + } + } + + private ArrayList fuels = new ArrayList(); + private boolean running = false; + private boolean simulateAirResistance = false; + private Supplier robotPoseSupplier = null; + private Supplier robotFieldSpeedsSupplier = null; + private double robotWidth; // size along the robot's y axis + private double robotLength; // size along the robot's x axis + private double bumperHeight; + private ArrayList intakes = new ArrayList<>(); + + /** Returns a singleton instance of FuelSim */ + public static FuelSim getInstance() { + if (instance == null) { + instance = new FuelSim(); } - private ArrayList fuels = new ArrayList(); - private boolean running = false; - private boolean simulateAirResistance = false; - private Supplier robotPoseSupplier = null; - private Supplier robotFieldSpeedsSupplier = null; - private double robotWidth; // size along the robot's y axis - private double robotLength; // size along the robot's x axis - private double bumperHeight; - private ArrayList intakes = new ArrayList<>(); - - /** - * Returns a singleton instance of FuelSim - */ - public static FuelSim getInstance() { - if (instance == null) { - instance = new FuelSim(); - } + return instance; + } + + /** Clears the field of fuel */ + public void clearFuel() { + fuels.clear(); + } + + /** Spawns fuel in the neutral zone and depots */ + public void spawnStartingFuel() { + // Center fuel + Translation3d center = new Translation3d(FIELD_LENGTH / 2, FIELD_WIDTH / 2, FUEL_RADIUS); + for (int i = 0; i < 15; i++) { + for (int j = 0; j < 6; j++) { + fuels.add( + new Fuel( + center.plus(new Translation3d(0.076 + 0.152 * j, 0.0254 + 0.076 + 0.152 * i, 0)))); + fuels.add( + new Fuel( + center.plus(new Translation3d(-0.076 - 0.152 * j, 0.0254 + 0.076 + 0.152 * i, 0)))); + fuels.add( + new Fuel( + center.plus(new Translation3d(0.076 + 0.152 * j, -0.0254 - 0.076 - 0.152 * i, 0)))); + fuels.add( + new Fuel( + center.plus( + new Translation3d(-0.076 - 0.152 * j, -0.0254 - 0.076 - 0.152 * i, 0)))); + } + } - return instance; + // Depots + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 4; j++) { + fuels.add( + new Fuel(new Translation3d(0.076 + 0.152 * j, 5.95 + 0.076 + 0.152 * i, FUEL_RADIUS))); + fuels.add( + new Fuel(new Translation3d(0.076 + 0.152 * j, 5.95 - 0.076 - 0.152 * i, FUEL_RADIUS))); + fuels.add( + new Fuel( + new Translation3d( + FIELD_LENGTH - 0.076 - 0.152 * j, 2.09 + 0.076 + 0.152 * i, FUEL_RADIUS))); + fuels.add( + new Fuel( + new Translation3d( + FIELD_LENGTH - 0.076 - 0.152 * j, 2.09 - 0.076 - 0.152 * i, FUEL_RADIUS))); + } } - /** - * Clears the field of fuel - */ - public void clearFuel() { - fuels.clear(); + // DEBUG: Log XZ lines + // Translation3d[][] lines = new Translation3d[FIELD_XZ_LINE_STARTS.length][2]; + // for (int i = 0; i < FIELD_XZ_LINE_STARTS.length; i++) { + // lines[i][0] = FIELD_XZ_LINE_STARTS[i]; + // lines[i][1] = FIELD_XZ_LINE_ENDS[i]; + // } + + // Logger.recordOutput("Fuel Simulation/Lines (debug)", lines); + } + + /** + * Adds array of `Translation3d`'s to NetworkTables at + * "AdvantageKit/RealOutputs/Fuel + * Simulation/Fuels" + */ + public void logFuels() { + Logger.recordOutput( + "Fuel Simulation/Fuels", + fuels.stream().map((fuel) -> fuel.pos).toArray(Translation3d[]::new)); + ArrayList meanTraj = computeMeanTrajectory(); + Logger.recordOutput("Fuel Simulation/Mean Trajectory", meanTraj.toArray(new Translation3d[0])); + } + + /** Start the simulation. `updateSim` must still be called every loop */ + public void start() { + running = true; + } + + /** Pause the simulation. */ + public void stop() { + running = false; + } + + /** Enables accounting for drag force in physics step * */ + public void enableAirResistance() { + simulateAirResistance = true; + } + + /** + * Sets the number of physics iterations per loop (0.02s) + * + * @param subticks + */ + public void setSubticks(int subticks) { + FuelSim.subticks = subticks; + } + + /** + * Registers a robot with the fuel simulator + * + * @param width from left to right (y-axis) + * @param length from front to back (x-axis) + * @param bumperHeight + * @param poseSupplier + * @param fieldSpeedsSupplier field-relative `ChassisSpeeds` supplier + */ + public void registerRobot( + double width, + double length, + double bumperHeight, + Supplier poseSupplier, + Supplier fieldSpeedsSupplier) { + this.robotPoseSupplier = poseSupplier; + this.robotFieldSpeedsSupplier = fieldSpeedsSupplier; + this.robotWidth = width; + this.robotLength = length; + this.bumperHeight = bumperHeight; + } + + /** To be called periodically Will do nothing if sim is not running */ + public void updateSim() { + if (!running) + return; + + stepSim(); + } + + /** Run the simulation forward 1 time step (0.02s) */ + public void stepSim() { + for (int sub = 0; sub < subticks; sub++) { + + for (int f = 0; f < fuels.size(); f++) { + Fuel fuel = fuels.get(f); + fuel.update(); + + if (fuel.finished) { + completedTraj.add(fuel.traj); + + if (completedTraj.size() > 1) { + completedTraj.remove(0); + } + + fuels.remove(f); + f--; + } + + } + + handleFuelCollisions(fuels); + + if (robotPoseSupplier != null) { + handleRobotCollisions(fuels); + handleIntakes(fuels); + } } - /** - * Spawns fuel in the neutral zone and depots - */ - public void spawnStartingFuel() { - // Center fuel - Translation3d center = new Translation3d(FIELD_LENGTH / 2, FIELD_WIDTH / 2, FUEL_RADIUS); - for (int i = 0; i < 15; i++) { - for (int j = 0; j < 6; j++) { - fuels.add(new Fuel(center.plus(new Translation3d(0.076 + 0.152 * j, 0.0254 + 0.076 + 0.152 * i, 0)))); - fuels.add(new Fuel(center.plus(new Translation3d(-0.076 - 0.152 * j, 0.0254 + 0.076 + 0.152 * i, 0)))); - fuels.add(new Fuel(center.plus(new Translation3d(0.076 + 0.152 * j, -0.0254 - 0.076 - 0.152 * i, 0)))); - fuels.add(new Fuel(center.plus(new Translation3d(-0.076 - 0.152 * j, -0.0254 - 0.076 - 0.152 * i, 0)))); - } - } + logFuels(); + } - // Depots - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 4; j++) { - fuels.add(new Fuel(new Translation3d(0.076 + 0.152 * j, 5.95 + 0.076 + 0.152 * i, FUEL_RADIUS))); - fuels.add(new Fuel(new Translation3d(0.076 + 0.152 * j, 5.95 - 0.076 - 0.152 * i, FUEL_RADIUS))); - fuels.add(new Fuel( - new Translation3d(FIELD_LENGTH - 0.076 - 0.152 * j, 2.09 + 0.076 + 0.152 * i, FUEL_RADIUS))); - fuels.add(new Fuel( - new Translation3d(FIELD_LENGTH - 0.076 - 0.152 * j, 2.09 - 0.076 - 0.152 * i, FUEL_RADIUS))); - } - } + public ArrayList computeMeanTrajectory() { + if (completedTraj.isEmpty()) + return new ArrayList<>(); - // DEBUG: Log XZ lines - // Translation3d[][] lines = new Translation3d[FIELD_XZ_LINE_STARTS.length][2]; - // for (int i = 0; i < FIELD_XZ_LINE_STARTS.length; i++) { - // lines[i][0] = FIELD_XZ_LINE_STARTS[i]; - // lines[i][1] = FIELD_XZ_LINE_ENDS[i]; - // } + int minLength = completedTraj.stream().mapToInt(List::size).min().orElse(0); - // Logger.recordOutput("Fuel Simulation/Lines (debug)", lines); - } + ArrayList mean = new ArrayList<>(); - /** - * Adds array of `Translation3d`'s to NetworkTables at "AdvantageKit/RealOutputs/Fuel Simulation/Fuels" - */ - public void logFuels() { - Logger.recordOutput( - "Fuel Simulation/Fuels", fuels.stream().map((fuel) -> fuel.pos).toArray(Translation3d[]::new)); - } + // Limit to MAX_POINTS entries + int MAX_POINTS = 15; + int step = Math.max(minLength / MAX_POINTS, 1); // how many indices to skip per entry - /** - * Start the simulation. `updateSim` must still be called every loop - */ - public void start() { - running = true; - } + for (int i = 0; i < minLength; i += step) { + double x = 0, y = 0, z = 0; - /** - * Pause the simulation. - */ - public void stop() { - running = false; - } + for (List traj : completedTraj) { + Translation3d p = traj.get(i); + x += p.getX(); + y += p.getY(); + z += p.getZ(); + } - /** Enables accounting for drag force in physics step **/ - public void enableAirResistance() { - simulateAirResistance = true; - } + int n = completedTraj.size(); + mean.add(new Translation3d(x / n, y / n, z / n)); - /** - * Sets the number of physics iterations per loop (0.02s) - * @param subticks - */ - public void setSubticks(int subticks) { - FuelSim.subticks = subticks; + if (mean.size() >= MAX_POINTS) + break; // stop once we reach max } - /** - * Registers a robot with the fuel simulator - * @param width from left to right (y-axis) - * @param length from front to back (x-axis) - * @param bumperHeight - * @param poseSupplier - * @param fieldSpeedsSupplier field-relative `ChassisSpeeds` supplier - */ - public void registerRobot( - double width, - double length, - double bumperHeight, - Supplier poseSupplier, - Supplier fieldSpeedsSupplier) { - this.robotPoseSupplier = poseSupplier; - this.robotFieldSpeedsSupplier = fieldSpeedsSupplier; - this.robotWidth = width; - this.robotLength = length; - this.bumperHeight = bumperHeight; + return mean; + } + + /** + * Adds a fuel onto the field + * + * @param pos Position to spawn at + * @param vel Initial velocity vector + */ + public void spawnFuel(Translation3d pos, Translation3d vel) { + fuels.add(new Fuel(pos, vel)); + } + + /** + * Spawns a fuel onto the field with a specified launch velocity and angles, + * accounting for robot + * movement + * + * @param launchHeight Height of the fuel to launch at. Make sure this is + * higher than your robot's + * bumper height, or else it will collide with your robot + * immediately. + * @param launchVelocity Initial launch velocity + * @param hoodAngle Hood angle where 0 is launching horizontally and 90 + * degrees is launching + * straight up + * @param turretYaw Field-relative turret yaw + * @throws IllegalStateException if robot is not registered + */ + public void launchFuel( + LinearVelocity launchVelocity, Angle hoodAngle, Angle turretYaw, Distance launchHeight) { + if (robotPoseSupplier == null || robotFieldSpeedsSupplier == null) { + throw new IllegalStateException("Robot must be registered before launching fuel."); } - /** - * To be called periodically - * Will do nothing if sim is not running - */ - public void updateSim() { - if (!running) return; - - stepSim(); + Pose3d launchPose = new Pose3d(this.robotPoseSupplier.get()) + .plus( + new Transform3d( + new Translation3d(Meters.zero(), Meters.zero(), launchHeight), + Rotation3d.kZero)); + ChassisSpeeds fieldSpeeds = this.robotFieldSpeedsSupplier.get(); + + double horizontalVel = Math.cos(hoodAngle.in(Radians)) * launchVelocity.in(MetersPerSecond); + double verticalVel = Math.sin(hoodAngle.in(Radians)) * launchVelocity.in(MetersPerSecond); + double xVel = horizontalVel * Math.cos(turretYaw.in(Radians)); + double yVel = horizontalVel * Math.sin(turretYaw.in(Radians)); + + xVel += fieldSpeeds.vxMetersPerSecond; + yVel += fieldSpeeds.vyMetersPerSecond; + + spawnFuel(launchPose.getTranslation(), new Translation3d(xVel, yVel, verticalVel)); + } + + private void handleRobotCollision(Fuel fuel, Pose2d robot, Translation2d robotVel) { + Translation2d relativePos = new Pose2d(fuel.pos.toTranslation2d(), Rotation2d.kZero).relativeTo(robot) + .getTranslation(); + + if (fuel.pos.getZ() > bumperHeight) + return; // above bumpers + double distanceToBottom = -FUEL_RADIUS - robotLength / 2 - relativePos.getX(); + double distanceToTop = -FUEL_RADIUS - robotLength / 2 + relativePos.getX(); + double distanceToRight = -FUEL_RADIUS - robotWidth / 2 - relativePos.getY(); + double distanceToLeft = -FUEL_RADIUS - robotWidth / 2 + relativePos.getY(); + + // not inside robot + if (distanceToBottom > 0 || distanceToTop > 0 || distanceToRight > 0 || distanceToLeft > 0) + return; + + Translation2d posOffset; + // find minimum distance to side and send corresponding collision response + if ((distanceToBottom >= distanceToTop + && distanceToBottom >= distanceToRight + && distanceToBottom >= distanceToLeft)) { + posOffset = new Translation2d(distanceToBottom, 0); + } else if ((distanceToTop >= distanceToBottom + && distanceToTop >= distanceToRight + && distanceToTop >= distanceToLeft)) { + posOffset = new Translation2d(-distanceToTop, 0); + } else if ((distanceToRight >= distanceToBottom + && distanceToRight >= distanceToTop + && distanceToRight >= distanceToLeft)) { + posOffset = new Translation2d(0, distanceToRight); + } else { + posOffset = new Translation2d(0, -distanceToLeft); } - /** - * Run the simulation forward 1 time step (0.02s) - */ - public void stepSim() { - for (int i = 0; i < subticks; i++) { - for (Fuel fuel : fuels) { - fuel.update(); - } - - handleFuelCollisions(fuels); + posOffset = posOffset.rotateBy(robot.getRotation()); + fuel.pos = fuel.pos.plus(new Translation3d(posOffset)); + Translation2d normal = posOffset.div(posOffset.getNorm()); + if (fuel.vel.toTranslation2d().dot(normal) < 0) + fuel.addImpulse( + new Translation3d( + normal.times(-fuel.vel.toTranslation2d().dot(normal) * (1 + ROBOT_COR)))); + if (robotVel.dot(normal) > 0) + fuel.addImpulse(new Translation3d(normal.times(robotVel.dot(normal)))); + } + + private void handleRobotCollisions(ArrayList fuels) { + Pose2d robot = robotPoseSupplier.get(); + ChassisSpeeds speeds = robotFieldSpeedsSupplier.get(); + Translation2d robotVel = new Translation2d(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond); + + for (Fuel fuel : fuels) { + handleRobotCollision(fuel, robot, robotVel); + } + } - if (robotPoseSupplier != null) { - handleRobotCollisions(fuels); - handleIntakes(fuels); - } + private void handleIntakes(ArrayList fuels) { + Pose2d robot = robotPoseSupplier.get(); + for (SimIntake intake : intakes) { + for (int i = 0; i < fuels.size(); i++) { + if (intake.shouldIntake(fuels.get(i), robot)) { + fuels.remove(i); + i--; } - - logFuels(); + } } - - /** - * Adds a fuel onto the field - * @param pos Position to spawn at - * @param vel Initial velocity vector - */ - public void spawnFuel(Translation3d pos, Translation3d vel) { - fuels.add(new Fuel(pos, vel)); + } + + private static void fuelCollideRectangle(Fuel fuel, Translation3d start, Translation3d end) { + if (fuel.pos.getZ() > end.getZ() + FUEL_RADIUS || fuel.pos.getZ() < start.getZ() - FUEL_RADIUS) + return; // above rectangle + double distanceToLeft = start.getX() - FUEL_RADIUS - fuel.pos.getX(); + double distanceToRight = fuel.pos.getX() - end.getX() - FUEL_RADIUS; + double distanceToTop = fuel.pos.getY() - end.getY() - FUEL_RADIUS; + double distanceToBottom = start.getY() - FUEL_RADIUS - fuel.pos.getY(); + + // not inside hub + if (distanceToLeft > 0 || distanceToRight > 0 || distanceToTop > 0 || distanceToBottom > 0) + return; + + Translation2d collision; + // find minimum distance to side and send corresponding collision response + if (fuel.pos.getX() < start.getX() + || (distanceToLeft >= distanceToRight + && distanceToLeft >= distanceToTop + && distanceToLeft >= distanceToBottom)) { + collision = new Translation2d(distanceToLeft, 0); + } else if (fuel.pos.getX() >= end.getX() + || (distanceToRight >= distanceToLeft + && distanceToRight >= distanceToTop + && distanceToRight >= distanceToBottom)) { + collision = new Translation2d(-distanceToRight, 0); + } else if (fuel.pos.getY() > end.getY() + || (distanceToTop >= distanceToLeft + && distanceToTop >= distanceToRight + && distanceToTop >= distanceToBottom)) { + collision = new Translation2d(0, -distanceToTop); + } else { + collision = new Translation2d(0, distanceToBottom); } - /** - * Spawns a fuel onto the field with a specified launch velocity and angles, accounting for robot movement - * @param launchHeight Height of the fuel to launch at. Make sure this is higher than your robot's bumper height, or else it will collide with your robot immediately. - * @param launchVelocity Initial launch velocity - * @param hoodAngle Hood angle where 0 is launching horizontally and 90 degrees is launching straight up - * @param turretYaw Field-relative turret yaw - * @throws IllegalStateException if robot is not registered - */ - public void launchFuel(LinearVelocity launchVelocity, Angle hoodAngle, Angle turretYaw, Distance launchHeight) { - if (robotPoseSupplier == null || robotFieldSpeedsSupplier == null) { - throw new IllegalStateException("Robot must be registered before launching fuel."); - } - - Pose3d launchPose = new Pose3d(this.robotPoseSupplier.get()) - .plus(new Transform3d(new Translation3d(Meters.zero(), Meters.zero(), launchHeight), Rotation3d.kZero)); - ChassisSpeeds fieldSpeeds = this.robotFieldSpeedsSupplier.get(); - - double horizontalVel = Math.cos(hoodAngle.in(Radians)) * launchVelocity.in(MetersPerSecond); - double verticalVel = Math.sin(hoodAngle.in(Radians)) * launchVelocity.in(MetersPerSecond); - double xVel = horizontalVel * Math.cos(turretYaw.in(Radians)); - double yVel = horizontalVel * Math.sin(turretYaw.in(Radians)); - - xVel += fieldSpeeds.vxMetersPerSecond; - yVel += fieldSpeeds.vyMetersPerSecond; - - spawnFuel(launchPose.getTranslation(), new Translation3d(xVel, yVel, verticalVel)); - } - - private void handleRobotCollision(Fuel fuel, Pose2d robot, Translation2d robotVel) { - Translation2d relativePos = new Pose2d(fuel.pos.toTranslation2d(), Rotation2d.kZero) - .relativeTo(robot) - .getTranslation(); - - if (fuel.pos.getZ() > bumperHeight) return; // above bumpers - double distanceToBottom = -FUEL_RADIUS - robotLength / 2 - relativePos.getX(); - double distanceToTop = -FUEL_RADIUS - robotLength / 2 + relativePos.getX(); - double distanceToRight = -FUEL_RADIUS - robotWidth / 2 - relativePos.getY(); - double distanceToLeft = -FUEL_RADIUS - robotWidth / 2 + relativePos.getY(); - - // not inside robot - if (distanceToBottom > 0 || distanceToTop > 0 || distanceToRight > 0 || distanceToLeft > 0) return; - - Translation2d posOffset; - // find minimum distance to side and send corresponding collision response - if ((distanceToBottom >= distanceToTop - && distanceToBottom >= distanceToRight - && distanceToBottom >= distanceToLeft)) { - posOffset = new Translation2d(distanceToBottom, 0); - } else if ((distanceToTop >= distanceToBottom - && distanceToTop >= distanceToRight - && distanceToTop >= distanceToLeft)) { - posOffset = new Translation2d(-distanceToTop, 0); - } else if ((distanceToRight >= distanceToBottom - && distanceToRight >= distanceToTop - && distanceToRight >= distanceToLeft)) { - posOffset = new Translation2d(0, distanceToRight); - } else { - posOffset = new Translation2d(0, -distanceToLeft); - } - - posOffset = posOffset.rotateBy(robot.getRotation()); - fuel.pos = fuel.pos.plus(new Translation3d(posOffset)); - Translation2d normal = posOffset.div(posOffset.getNorm()); - if (fuel.vel.toTranslation2d().dot(normal) < 0) - fuel.addImpulse( - new Translation3d(normal.times(-fuel.vel.toTranslation2d().dot(normal) * (1 + ROBOT_COR)))); - if (robotVel.dot(normal) > 0) fuel.addImpulse(new Translation3d(normal.times(robotVel.dot(normal)))); + if (collision.getX() != 0) { + fuel.pos = fuel.pos.plus(new Translation3d(collision)); + fuel.vel = fuel.vel.plus(new Translation3d(-(1 + FIELD_COR) * fuel.vel.getX(), 0, 0)); + } else if (collision.getY() != 0) { + fuel.pos = fuel.pos.plus(new Translation3d(collision)); + fuel.vel = fuel.vel.plus(new Translation3d(0, -(1 + FIELD_COR) * fuel.vel.getY(), 0)); } - - private void handleRobotCollisions(ArrayList fuels) { - Pose2d robot = robotPoseSupplier.get(); - ChassisSpeeds speeds = robotFieldSpeedsSupplier.get(); - Translation2d robotVel = new Translation2d(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond); - - for (Fuel fuel : fuels) { - handleRobotCollision(fuel, robot, robotVel); - } + } + + /** + * Registers an intake with the fuel simulator. This intake will remove fuel + * from the field based + * on the `ableToIntake` parameter. + * + * @param xMin Minimum x position for the bounding box + * @param xMax Maximum x position for the bounding box + * @param yMin Minimum y position for the bounding box + * @param yMax Maximum y position for the bounding box + * @param ableToIntake Should a return a boolean whether the intake is active + * @param intakeCallback Function to call when a fuel is intaked + */ + public void registerIntake( + double xMin, + double xMax, + double yMin, + double yMax, + BooleanSupplier ableToIntake, + Runnable intakeCallback) { + intakes.add(new SimIntake(xMin, xMax, yMin, yMax, ableToIntake, intakeCallback)); + } + + /** + * Registers an intake with the fuel simulator. This intake will remove fuel + * from the field based + * on the `ableToIntake` parameter. + * + * @param xMin Minimum x position for the bounding box + * @param xMax Maximum x position for the bounding box + * @param yMin Minimum y position for the bounding box + * @param yMax Maximum y position for the bounding box + * @param ableToIntake Should a return a boolean whether the intake is active + */ + public void registerIntake( + double xMin, double xMax, double yMin, double yMax, BooleanSupplier ableToIntake) { + registerIntake(xMin, xMax, yMin, yMax, ableToIntake, () -> { + }); + } + + /** + * Registers an intake with the fuel simulator. This intake will always remove + * fuel from the + * field. + * + * @param xMin Minimum x position for the bounding box + * @param xMax Maximum x position for the bounding box + * @param yMin Minimum y position for the bounding box + * @param yMax Maximum y position for the bounding box + * @param intakeCallback Function to call when a fuel is intaked + */ + public void registerIntake( + double xMin, double xMax, double yMin, double yMax, Runnable intakeCallback) { + registerIntake(xMin, xMax, yMin, yMax, () -> true, intakeCallback); + } + + /** + * Registers an intake with the fuel simulator. This intake will always remove + * fuel from the + * field. + * + * @param xMin Minimum x position for the bounding box + * @param xMax Maximum x position for the bounding box + * @param yMin Minimum y position for the bounding box + * @param yMax Maximum y position for the bounding box + */ + public void registerIntake(double xMin, double xMax, double yMin, double yMax) { + registerIntake(xMin, xMax, yMin, yMax, () -> true, () -> { + }); + } + + public static class Hub { + public static final Hub BLUE_HUB = new Hub( + new Translation2d(4.61, FIELD_WIDTH / 2), + new Translation3d(5.3, FIELD_WIDTH / 2, 0.89), + 1); + public static final Hub RED_HUB = new Hub( + new Translation2d(FIELD_LENGTH - 4.61, FIELD_WIDTH / 2), + new Translation3d(FIELD_LENGTH - 5.3, FIELD_WIDTH / 2, 0.89), + -1); + + private static final double ENTRY_HEIGHT = 1.83; + private static final double ENTRY_RADIUS = 0.56; + + private static final double SIDE = 1.2; + + private static final double NET_HEIGHT_MAX = 3.057; + private static final double NET_HEIGHT_MIN = 1.5; + private static final double NET_OFFSET = SIDE / 2 + 0.261; + private static final double NET_WIDTH = 1.484; + + private final Translation2d center; + private final Translation3d exit; + private final int exitVelXMult; + + private int score = 0; + + private Hub(Translation2d center, Translation3d exit, int exitVelXMult) { + this.center = center; + this.exit = exit; + this.exitVelXMult = exitVelXMult; } - private void handleIntakes(ArrayList fuels) { - Pose2d robot = robotPoseSupplier.get(); - for (SimIntake intake : intakes) { - for (int i = 0; i < fuels.size(); i++) { - if (intake.shouldIntake(fuels.get(i), robot)) { - fuels.remove(i); - i--; - } - } - } + private boolean handleHubInteraction(Fuel fuel) { + if (didFuelScore(fuel)) { + fuel.pos = exit; + fuel.vel = getDispersalVelocity(); + score++; + return true; + } + return false; } - private static void fuelCollideRectangle(Fuel fuel, Translation3d start, Translation3d end) { - if (fuel.pos.getZ() > end.getZ() + FUEL_RADIUS || fuel.pos.getZ() < start.getZ() - FUEL_RADIUS) - return; // above rectangle - double distanceToLeft = start.getX() - FUEL_RADIUS - fuel.pos.getX(); - double distanceToRight = fuel.pos.getX() - end.getX() - FUEL_RADIUS; - double distanceToTop = fuel.pos.getY() - end.getY() - FUEL_RADIUS; - double distanceToBottom = start.getY() - FUEL_RADIUS - fuel.pos.getY(); - - // not inside hub - if (distanceToLeft > 0 || distanceToRight > 0 || distanceToTop > 0 || distanceToBottom > 0) return; - - Translation2d collision; - // find minimum distance to side and send corresponding collision response - if (fuel.pos.getX() < start.getX() - || (distanceToLeft >= distanceToRight - && distanceToLeft >= distanceToTop - && distanceToLeft >= distanceToBottom)) { - collision = new Translation2d(distanceToLeft, 0); - } else if (fuel.pos.getX() >= end.getX() - || (distanceToRight >= distanceToLeft - && distanceToRight >= distanceToTop - && distanceToRight >= distanceToBottom)) { - collision = new Translation2d(-distanceToRight, 0); - } else if (fuel.pos.getY() > end.getY() - || (distanceToTop >= distanceToLeft - && distanceToTop >= distanceToRight - && distanceToTop >= distanceToBottom)) { - collision = new Translation2d(0, -distanceToTop); - } else { - collision = new Translation2d(0, distanceToBottom); - } - - if (collision.getX() != 0) { - fuel.pos = fuel.pos.plus(new Translation3d(collision)); - fuel.vel = fuel.vel.plus(new Translation3d(-(1 + FIELD_COR) * fuel.vel.getX(), 0, 0)); - } else if (collision.getY() != 0) { - fuel.pos = fuel.pos.plus(new Translation3d(collision)); - fuel.vel = fuel.vel.plus(new Translation3d(0, -(1 + FIELD_COR) * fuel.vel.getY(), 0)); - } + private boolean didFuelScore(Fuel fuel) { + return fuel.pos.toTranslation2d().getDistance(center) <= ENTRY_RADIUS + && fuel.pos.getZ() <= ENTRY_HEIGHT + && fuel.pos.minus(fuel.vel.times(PERIOD / subticks)).getZ() > ENTRY_HEIGHT; } - /** - * Registers an intake with the fuel simulator. This intake will remove fuel from the field based on the `ableToIntake` parameter. - * @param xMin Minimum x position for the bounding box - * @param xMax Maximum x position for the bounding box - * @param yMin Minimum y position for the bounding box - * @param yMax Maximum y position for the bounding box - * @param ableToIntake Should a return a boolean whether the intake is active - * @param intakeCallback Function to call when a fuel is intaked - */ - public void registerIntake( - double xMin, double xMax, double yMin, double yMax, BooleanSupplier ableToIntake, Runnable intakeCallback) { - intakes.add(new SimIntake(xMin, xMax, yMin, yMax, ableToIntake, intakeCallback)); + private Translation3d getDispersalVelocity() { + return new Translation3d( + exitVelXMult * (Math.random() + 0.1) * 1.5, Math.random() * 2 - 1, 0); } - /** - * Registers an intake with the fuel simulator. This intake will remove fuel from the field based on the `ableToIntake` parameter. - * @param xMin Minimum x position for the bounding box - * @param xMax Maximum x position for the bounding box - * @param yMin Minimum y position for the bounding box - * @param yMax Maximum y position for the bounding box - * @param ableToIntake Should a return a boolean whether the intake is active - */ - public void registerIntake(double xMin, double xMax, double yMin, double yMax, BooleanSupplier ableToIntake) { - registerIntake(xMin, xMax, yMin, yMax, ableToIntake, () -> {}); + /** Reset this hub's score to 0 */ + public void resetScore() { + score = 0; } /** - * Registers an intake with the fuel simulator. This intake will always remove fuel from the field. - * @param xMin Minimum x position for the bounding box - * @param xMax Maximum x position for the bounding box - * @param yMin Minimum y position for the bounding box - * @param yMax Maximum y position for the bounding box - * @param intakeCallback Function to call when a fuel is intaked + * Get the current count of fuel scored in this hub + * + * @return */ - public void registerIntake(double xMin, double xMax, double yMin, double yMax, Runnable intakeCallback) { - registerIntake(xMin, xMax, yMin, yMax, () -> true, intakeCallback); + public int getScore() { + return score; } - /** - * Registers an intake with the fuel simulator. This intake will always remove fuel from the field. - * @param xMin Minimum x position for the bounding box - * @param xMax Maximum x position for the bounding box - * @param yMin Minimum y position for the bounding box - * @param yMax Maximum y position for the bounding box - */ - public void registerIntake(double xMin, double xMax, double yMin, double yMax) { - registerIntake(xMin, xMax, yMin, yMax, () -> true, () -> {}); + private void fuelCollideSide(Fuel fuel) { + fuelCollideRectangle( + fuel, + new Translation3d(center.getX() - SIDE / 2, center.getY() - SIDE / 2, 0), + new Translation3d( + center.getX() + SIDE / 2, center.getY() + SIDE / 2, ENTRY_HEIGHT - 0.1)); + // if (fuel.pos.getZ() > ENTRY_HEIGHT - 0.1) return new Translation2d(); // + // above hub + // double distanceToLeft = center.getX() - SIDE / 2 - FUEL_RADIUS - + // fuel.pos.getX(); + // double distanceToRight = fuel.pos.getX() - center.getX() - SIDE / 2 - + // FUEL_RADIUS; + // double distanceToTop = center.getY() - SIDE / 2 - FUEL_RADIUS - + // fuel.pos.getY(); + // double distanceToBottom = fuel.pos.getY() - center.getY() - SIDE / 2 - + // FUEL_RADIUS; + + // // not inside hub + // if (distanceToLeft > 0 || distanceToRight > 0 || distanceToTop > 0 || + // distanceToBottom > 0) + // return new Translation2d(); + + // // find minimum distance to side and send corresponding collision response + // if (fuel.pos.getX() < center.getX() - SIDE / 2 + // || (distanceToLeft >= distanceToRight + // && distanceToLeft >= distanceToTop + // && distanceToLeft >= distanceToBottom)) { + // return new Translation2d(distanceToLeft, 0); + // } else if (fuel.pos.getX() >= center.getX() + SIDE / 2 + // || (distanceToRight >= distanceToLeft + // && distanceToRight >= distanceToTop + // && distanceToRight >= distanceToBottom)) { + // return new Translation2d(-distanceToRight, 0); + // } else if (fuel.pos.getY() > center.getY() + SIDE / 2 + // || (distanceToTop >= distanceToLeft + // && distanceToTop >= distanceToRight + // && distanceToTop >= distanceToBottom)) { + // return new Translation2d(0, -distanceToTop); + // } else { + // return new Translation2d(0, distanceToBottom); + // } } - public static class Hub { - public static final Hub BLUE_HUB = - new Hub(new Translation2d(4.61, FIELD_WIDTH / 2), new Translation3d(5.3, FIELD_WIDTH / 2, 0.89), 1); - public static final Hub RED_HUB = new Hub( - new Translation2d(FIELD_LENGTH - 4.61, FIELD_WIDTH / 2), - new Translation3d(FIELD_LENGTH - 5.3, FIELD_WIDTH / 2, 0.89), - -1); - - private static final double ENTRY_HEIGHT = 1.83; - private static final double ENTRY_RADIUS = 0.56; - - private static final double SIDE = 1.2; - - private static final double NET_HEIGHT_MAX = 3.057; - private static final double NET_HEIGHT_MIN = 1.5; - private static final double NET_OFFSET = SIDE / 2 + 0.261; - private static final double NET_WIDTH = 1.484; - - private final Translation2d center; - private final Translation3d exit; - private final int exitVelXMult; - - private int score = 0; - - private Hub(Translation2d center, Translation3d exit, int exitVelXMult) { - this.center = center; - this.exit = exit; - this.exitVelXMult = exitVelXMult; - } - - private void handleHubInteraction(Fuel fuel) { - if (didFuelScore(fuel)) { - fuel.pos = exit; - fuel.vel = getDispersalVelocity(); - score++; - } - } - - private boolean didFuelScore(Fuel fuel) { - return fuel.pos.toTranslation2d().getDistance(center) <= ENTRY_RADIUS - && fuel.pos.getZ() <= ENTRY_HEIGHT - && fuel.pos.minus(fuel.vel.times(PERIOD / subticks)).getZ() > ENTRY_HEIGHT; - } - - private Translation3d getDispersalVelocity() { - return new Translation3d(exitVelXMult * (Math.random() + 0.1) * 1.5, Math.random() * 2 - 1, 0); - } - - /** - * Reset this hub's score to 0 - */ - public void resetScore() { - score = 0; - } - - /** - * Get the current count of fuel scored in this hub - * @return - */ - public int getScore() { - return score; - } - - private void fuelCollideSide(Fuel fuel) { - fuelCollideRectangle( - fuel, - new Translation3d(center.getX() - SIDE / 2, center.getY() - SIDE / 2, 0), - new Translation3d(center.getX() + SIDE / 2, center.getY() + SIDE / 2, ENTRY_HEIGHT - 0.1)); - // if (fuel.pos.getZ() > ENTRY_HEIGHT - 0.1) return new Translation2d(); // above hub - // double distanceToLeft = center.getX() - SIDE / 2 - FUEL_RADIUS - fuel.pos.getX(); - // double distanceToRight = fuel.pos.getX() - center.getX() - SIDE / 2 - FUEL_RADIUS; - // double distanceToTop = center.getY() - SIDE / 2 - FUEL_RADIUS - fuel.pos.getY(); - // double distanceToBottom = fuel.pos.getY() - center.getY() - SIDE / 2 - FUEL_RADIUS; - - // // not inside hub - // if (distanceToLeft > 0 || distanceToRight > 0 || distanceToTop > 0 || distanceToBottom > 0) - // return new Translation2d(); - - // // find minimum distance to side and send corresponding collision response - // if (fuel.pos.getX() < center.getX() - SIDE / 2 - // || (distanceToLeft >= distanceToRight - // && distanceToLeft >= distanceToTop - // && distanceToLeft >= distanceToBottom)) { - // return new Translation2d(distanceToLeft, 0); - // } else if (fuel.pos.getX() >= center.getX() + SIDE / 2 - // || (distanceToRight >= distanceToLeft - // && distanceToRight >= distanceToTop - // && distanceToRight >= distanceToBottom)) { - // return new Translation2d(-distanceToRight, 0); - // } else if (fuel.pos.getY() > center.getY() + SIDE / 2 - // || (distanceToTop >= distanceToLeft - // && distanceToTop >= distanceToRight - // && distanceToTop >= distanceToBottom)) { - // return new Translation2d(0, -distanceToTop); - // } else { - // return new Translation2d(0, distanceToBottom); - // } - } - - private double fuelHitNet(Fuel fuel) { - if (fuel.pos.getZ() > NET_HEIGHT_MAX || fuel.pos.getZ() < NET_HEIGHT_MIN) return 0; - if (fuel.pos.getY() > center.getY() + NET_WIDTH / 2 || fuel.pos.getY() < center.getY() - NET_WIDTH / 2) - return 0; - if (fuel.pos.getX() > center.getX() + NET_OFFSET * exitVelXMult) { - return Math.max(0, center.getX() + NET_OFFSET * exitVelXMult - (fuel.pos.getX() - FUEL_RADIUS)); - } else { - return Math.min(0, center.getX() + NET_OFFSET * exitVelXMult - (fuel.pos.getX() + FUEL_RADIUS)); - } - } + private double fuelHitNet(Fuel fuel) { + if (fuel.pos.getZ() > NET_HEIGHT_MAX || fuel.pos.getZ() < NET_HEIGHT_MIN) + return 0; + if (fuel.pos.getY() > center.getY() + NET_WIDTH / 2 + || fuel.pos.getY() < center.getY() - NET_WIDTH / 2) + return 0; + if (fuel.pos.getX() > center.getX() + NET_OFFSET * exitVelXMult) { + return Math.max( + 0, center.getX() + NET_OFFSET * exitVelXMult - (fuel.pos.getX() - FUEL_RADIUS)); + } else { + return Math.min( + 0, center.getX() + NET_OFFSET * exitVelXMult - (fuel.pos.getX() + FUEL_RADIUS)); + } } - - private class SimIntake { - double xMin, xMax, yMin, yMax; - BooleanSupplier ableToIntake; - Runnable callback; - - private SimIntake( - double xMin, - double xMax, - double yMin, - double yMax, - BooleanSupplier ableToIntake, - Runnable intakeCallback) { - this.xMin = xMin; - this.xMax = xMax; - this.yMin = yMin; - this.yMax = yMax; - this.ableToIntake = ableToIntake; - this.callback = intakeCallback; - } - - private boolean shouldIntake(Fuel fuel, Pose2d robotPose) { - if (!ableToIntake.getAsBoolean() || fuel.pos.getZ() > bumperHeight) return false; - - Translation2d fuelRelativePos = new Pose2d(fuel.pos.toTranslation2d(), Rotation2d.kZero) - .relativeTo(robotPose) - .getTranslation(); - - boolean result = fuelRelativePos.getX() >= xMin - && fuelRelativePos.getX() <= xMax - && fuelRelativePos.getY() >= yMin - && fuelRelativePos.getY() <= yMax; - if (result) { - callback.run(); - } - return result; - } + } + + private class SimIntake { + double xMin, xMax, yMin, yMax; + BooleanSupplier ableToIntake; + Runnable callback; + + private SimIntake( + double xMin, + double xMax, + double yMin, + double yMax, + BooleanSupplier ableToIntake, + Runnable intakeCallback) { + this.xMin = xMin; + this.xMax = xMax; + this.yMin = yMin; + this.yMax = yMax; + this.ableToIntake = ableToIntake; + this.callback = intakeCallback; } - private FuelSim() { - // Initialize grid - for (int i = 0; i < GRID_COLS; i++) { - for (int j = 0; j < GRID_ROWS; j++) { - grid[i][j] = new ArrayList(); - } - } + private boolean shouldIntake(Fuel fuel, Pose2d robotPose) { + if (!ableToIntake.getAsBoolean() || fuel.pos.getZ() > bumperHeight) + return false; + + Translation2d fuelRelativePos = new Pose2d(fuel.pos.toTranslation2d(), Rotation2d.kZero) + .relativeTo(robotPose) + .getTranslation(); + + boolean result = fuelRelativePos.getX() >= xMin + && fuelRelativePos.getX() <= xMax + && fuelRelativePos.getY() >= yMin + && fuelRelativePos.getY() <= yMax; + if (result) { + callback.run(); + } + return result; + } + } + + private FuelSim() { + // Initialize grid + for (int i = 0; i < GRID_COLS; i++) { + for (int j = 0; j < GRID_ROWS; j++) { + grid[i][j] = new ArrayList(); + } } + } } From a6501128e7d63935b61e20fac739cf99edc40eca Mon Sep 17 00:00:00 2001 From: Blasty25 Date: Thu, 5 Feb 2026 11:31:44 -0500 Subject: [PATCH 2/2] formating and removing exterior constants --- FuelSim.java | 1402 +++++++++++++++++++------------------------------- 1 file changed, 541 insertions(+), 861 deletions(-) diff --git a/FuelSim.java b/FuelSim.java index 9aad774..b4f1e1b 100644 --- a/FuelSim.java +++ b/FuelSim.java @@ -3,8 +3,6 @@ import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.Radians; -import static frc.robot.Constants.hubCenter; -import static frc.robot.Constants.targetZ; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; @@ -25,924 +23,606 @@ import org.littletonrobotics.junction.Logger; public class FuelSim { - private static final double PERIOD = 0.02; // sec - private static int subticks = 5; - private static final Translation3d GRAVITY = new Translation3d(0, 0, -9.81); // m/s^2 - // Room temperature dry air density: - // https://en.wikipedia.org/wiki/Density_of_air#Dry_air - private static final double AIR_DENSITY = 1.2041; // kg/m^3 - private static final double FIELD_COR = Math.sqrt(22 / 51.5); // coefficient of restitution with the field - private static final double FUEL_COR = 0.5; // coefficient of restitution with another fuel - private static final double NET_COR = 0.2; // coefficient of restitution with the net - private static final double ROBOT_COR = 0.1; // coefficient of restitution with a robot - private static final double FUEL_RADIUS = 0.075; - private static final double FIELD_LENGTH = 16.51; - private static final double FIELD_WIDTH = 8.04; - private static final double TRENCH_WIDTH = 1.265; - private static final double TRENCH_BLOCK_WIDTH = 0.305; - private static final double TRENCH_HEIGHT = 0.565; - private static final double TRENCH_BAR_HEIGHT = 0.102; - private static final double TRENCH_BAR_WIDTH = 0.152; - private static final double FRICTION = 0.1; // proportion of horizontal velocity to lose per second while on ground - private static final double FUEL_MASS = 0.448 * 0.45392; // kgs - private static final double FUEL_CROSS_AREA = Math.PI * FUEL_RADIUS * FUEL_RADIUS; - // Drag coefficient of smooth sphere: - // https://en.wikipedia.org/wiki/Drag_coefficient#/media/File:14ilf1l.svg - private static final double DRAG_COF = 0.47; // dimensionless - private static final double DRAG_FORCE_FACTOR = 0.5 * AIR_DENSITY * DRAG_COF * FUEL_CROSS_AREA; - private static final ArrayList> completedTraj = new ArrayList<>(); - - private static FuelSim instance = null; - - private static final Translation3d[] FIELD_XZ_LINE_STARTS = { - new Translation3d(0, 0, 0), - new Translation3d(3.96, 1.57, 0), - new Translation3d(3.96, FIELD_WIDTH / 2 + 0.60, 0), - new Translation3d(4.61, 1.57, 0.165), - new Translation3d(4.61, FIELD_WIDTH / 2 + 0.60, 0.165), - new Translation3d(FIELD_LENGTH - 5.18, 1.57, 0), - new Translation3d(FIELD_LENGTH - 5.18, FIELD_WIDTH / 2 + 0.60, 0), - new Translation3d(FIELD_LENGTH - 4.61, 1.57, 0.165), - new Translation3d(FIELD_LENGTH - 4.61, FIELD_WIDTH / 2 + 0.60, 0.165), - new Translation3d(3.96, TRENCH_WIDTH, TRENCH_HEIGHT), - new Translation3d(3.96, FIELD_WIDTH - 1.57, TRENCH_HEIGHT), - new Translation3d(FIELD_LENGTH - 5.18, TRENCH_WIDTH, TRENCH_HEIGHT), - new Translation3d(FIELD_LENGTH - 5.18, FIELD_WIDTH - 1.57, TRENCH_HEIGHT), - new Translation3d(4.61 - TRENCH_BAR_WIDTH / 2, 0, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), - new Translation3d( - 4.61 - TRENCH_BAR_WIDTH / 2, FIELD_WIDTH - 1.57, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), - new Translation3d( - FIELD_LENGTH - 4.61 - TRENCH_BAR_WIDTH / 2, 0, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), - new Translation3d( - FIELD_LENGTH - 4.61 - TRENCH_BAR_WIDTH / 2, - FIELD_WIDTH - 1.57, - TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), - }; - - private static final Translation3d[] FIELD_XZ_LINE_ENDS = { - new Translation3d(FIELD_LENGTH, FIELD_WIDTH, 0), - new Translation3d(4.61, FIELD_WIDTH / 2 - 0.60, 0.165), - new Translation3d(4.61, FIELD_WIDTH - 1.57, 0.165), - new Translation3d(5.18, FIELD_WIDTH / 2 - 0.60, 0), - new Translation3d(5.18, FIELD_WIDTH - 1.57, 0), - new Translation3d(FIELD_LENGTH - 4.61, FIELD_WIDTH / 2 - 0.60, 0.165), - new Translation3d(FIELD_LENGTH - 4.61, FIELD_WIDTH - 1.57, 0.165), - new Translation3d(FIELD_LENGTH - 3.96, FIELD_WIDTH / 2 - 0.60, 0), - new Translation3d(FIELD_LENGTH - 3.96, FIELD_WIDTH - 1.57, 0), - new Translation3d(5.18, TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT), - new Translation3d(5.18, FIELD_WIDTH - 1.57 + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT), - new Translation3d(FIELD_LENGTH - 3.96, TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT), - new Translation3d(FIELD_LENGTH - 3.96, FIELD_WIDTH - 1.57 + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT), - new Translation3d( - 4.61 + TRENCH_BAR_WIDTH / 2, - TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, - TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), - new Translation3d(4.61 + TRENCH_BAR_WIDTH / 2, FIELD_WIDTH, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), - new Translation3d( - FIELD_LENGTH - 4.61 + TRENCH_BAR_WIDTH / 2, - TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, - TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), - new Translation3d( - FIELD_LENGTH - 4.61 + TRENCH_BAR_WIDTH / 2, FIELD_WIDTH, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT), - }; - - private class Fuel { - private Translation3d pos; - private Translation3d vel; - private boolean finished = false; - private final List traj = new ArrayList<>(); - - private Fuel(Translation3d pos, Translation3d vel) { - this.pos = pos; - this.vel = vel; - } - private Fuel(Translation3d pos) { - this(pos, new Translation3d()); - } + private static final double PERIOD = 0.02; + private static int subticks = 5; + + private static final Translation3d GRAVITY = new Translation3d(0, 0, -9.81); + private static final double AIR_DENSITY = 1.2041; + private static final double FIELD_COR = Math.sqrt(22.0 / 51.5); + private static final double FUEL_COR = 0.5; + private static final double NET_COR = 0.2; + private static final double ROBOT_COR = 0.1; + + private static final double FUEL_RADIUS = 0.075; + private static final double FIELD_LENGTH = 16.51; + private static final double FIELD_WIDTH = 8.04; + + private static final double TRENCH_WIDTH = 1.265; + private static final double TRENCH_BLOCK_WIDTH = 0.305; + private static final double TRENCH_HEIGHT = 0.565; + private static final double TRENCH_BAR_HEIGHT = 0.102; + private static final double TRENCH_BAR_WIDTH = 0.152; + + private static final double FRICTION = 0.1; + private static final double FUEL_MASS = 0.448 * 0.45392; + private static final double FUEL_CROSS_AREA = Math.PI * FUEL_RADIUS * FUEL_RADIUS; + private static final double DRAG_COF = 0.47; + private static final double DRAG_FORCE_FACTOR = + 0.5 * AIR_DENSITY * DRAG_COF * FUEL_CROSS_AREA; + + private static final double CELL_SIZE = 0.25; + private static final int GRID_COLS = (int) Math.ceil(FIELD_LENGTH / CELL_SIZE); + private static final int GRID_ROWS = (int) Math.ceil(FIELD_WIDTH / CELL_SIZE); + + private static final ArrayList> completedTraj = new ArrayList<>(); + + private static FuelSim instance; + + @SuppressWarnings("unchecked") + private final ArrayList[][] grid = new ArrayList[GRID_COLS][GRID_ROWS]; - private void update() { - if (!finished) { - traj.add(pos); - } - pos = pos.plus(vel.times(PERIOD / subticks)); - if (pos.getZ() > FUEL_RADIUS) { - Translation3d Fg = GRAVITY.times(FUEL_MASS); - Translation3d Fd = new Translation3d(); - - if (simulateAirResistance) { - double speed = vel.getNorm(); - if (speed > 1e-6) { - Fd = vel.times(-DRAG_FORCE_FACTOR * speed); - } - } - - Translation3d accel = Fg.plus(Fd).div(FUEL_MASS); - vel = vel.plus(accel.times(PERIOD / subticks)); - } - if (Math.abs(vel.getZ()) < 0.05 && pos.getZ() <= FUEL_RADIUS + 0.03) { - vel = new Translation3d(vel.getX(), vel.getY(), 0); - vel = vel.times(1 - FRICTION * PERIOD / subticks); - } - boolean isDone = pos.getZ() <= FUEL_RADIUS + (Units.inchesToMeters(targetZ) / 2) && Math.abs(vel.getZ()) <= 3.0; - - if (isDone) { - finished = true; - } - - handleFieldCollisions(); + private final ArrayList fuels = new ArrayList<>(); + private final ArrayList intakes = new ArrayList<>(); + + private boolean running; + private boolean simulateAirResistance; + + private Supplier robotPoseSupplier; + private Supplier robotFieldSpeedsSupplier; + + private double robotWidth; + private double robotLength; + private double bumperHeight; + + public static FuelSim getInstance() { + if (instance == null) { + instance = new FuelSim(); + } + return instance; } - private void handleXZLineCollision(Translation3d lineStart, Translation3d lineEnd) { - if (pos.getY() < lineStart.getY() || pos.getY() > lineEnd.getY()) - return; // not within y range - // Convert into 2D - Translation2d start2d = new Translation2d(lineStart.getX(), lineStart.getZ()); - Translation2d end2d = new Translation2d(lineEnd.getX(), lineEnd.getZ()); - Translation2d pos2d = new Translation2d(pos.getX(), pos.getZ()); - Translation2d lineVec = end2d.minus(start2d); - - // Get closest point on line - Translation2d projected = start2d - .plus(lineVec.times(pos2d.minus(start2d).dot(lineVec) / lineVec.getSquaredNorm())); - - if (projected.getDistance(start2d) + projected.getDistance(end2d) > lineVec.getNorm()) - return; // projected point not on line - double dist = pos2d.getDistance(projected); - if (dist > FUEL_RADIUS) - return; // not intersecting line - // Back into 3D - Translation3d normal = new Translation3d(-lineVec.getY(), 0, lineVec.getX()).div(lineVec.getNorm()); - - // Apply collision response - pos = pos.plus(normal.times(FUEL_RADIUS - dist)); - if (vel.dot(normal) > 0) - return; // already moving away from line - vel = vel.minus(normal.times((1 + FIELD_COR) * vel.dot(normal))); + private FuelSim() { + for (int i = 0; i < GRID_COLS; i++) { + for (int j = 0; j < GRID_ROWS; j++) { + grid[i][j] = new ArrayList<>(); + } + } } - private void handleFieldCollisions() { - // floor and bumps - for (int i = 0; i < FIELD_XZ_LINE_STARTS.length; i++) { - handleXZLineCollision(FIELD_XZ_LINE_STARTS[i], FIELD_XZ_LINE_ENDS[i]); - } - - // edges - if (pos.getX() < FUEL_RADIUS && vel.getX() < 0) { - pos = pos.plus(new Translation3d(FUEL_RADIUS - pos.getX(), 0, 0)); - vel = vel.plus(new Translation3d(-(1 + FIELD_COR) * vel.getX(), 0, 0)); - } else if (pos.getX() > FIELD_LENGTH - FUEL_RADIUS && vel.getX() > 0) { - pos = pos.plus(new Translation3d(FIELD_LENGTH - FUEL_RADIUS - pos.getX(), 0, 0)); - vel = vel.plus(new Translation3d(-(1 + FIELD_COR) * vel.getX(), 0, 0)); - } - - if (pos.getY() < FUEL_RADIUS && vel.getY() < 0) { - pos = pos.plus(new Translation3d(0, FUEL_RADIUS - pos.getY(), 0)); - vel = vel.plus(new Translation3d(0, -(1 + FIELD_COR) * vel.getY(), 0)); - } else if (pos.getY() > FIELD_WIDTH - FUEL_RADIUS && vel.getY() > 0) { - pos = pos.plus(new Translation3d(0, FIELD_WIDTH - FUEL_RADIUS - pos.getY(), 0)); - vel = vel.plus(new Translation3d(0, -(1 + FIELD_COR) * vel.getY(), 0)); - } - - // hubs - handleHubCollisions(Hub.BLUE_HUB); - handleHubCollisions(Hub.RED_HUB); - - handleTrenchCollisions(); + public void start() { + running = true; } - private void handleHubCollisions(Hub hub) { - if (hub.handleHubInteraction(this)) { - finished = true; - } - hub.fuelCollideSide(this); - - double netCollision = hub.fuelHitNet(this); - if (netCollision != 0) { - pos = pos.plus(new Translation3d(netCollision, 0, 0)); - vel = new Translation3d(-vel.getX() * NET_COR, vel.getY() * NET_COR, vel.getZ()); - } + public void stop() { + running = false; } - private void handleTrenchCollisions() { - fuelCollideRectangle( - this, - new Translation3d(3.96, TRENCH_WIDTH, 0), - new Translation3d(5.18, TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT)); - fuelCollideRectangle( - this, - new Translation3d(3.96, FIELD_WIDTH - 1.57, 0), - new Translation3d(5.18, FIELD_WIDTH - 1.57 + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT)); - fuelCollideRectangle( - this, - new Translation3d(FIELD_LENGTH - 5.18, TRENCH_WIDTH, 0), - new Translation3d(FIELD_LENGTH - 3.96, TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT)); - fuelCollideRectangle( - this, - new Translation3d(FIELD_LENGTH - 5.18, FIELD_WIDTH - 1.57, 0), - new Translation3d( - FIELD_LENGTH - 3.96, FIELD_WIDTH - 1.57 + TRENCH_BLOCK_WIDTH, TRENCH_HEIGHT)); - fuelCollideRectangle( - this, - new Translation3d(4.61 - TRENCH_BAR_WIDTH / 2, 0, TRENCH_HEIGHT), - new Translation3d( - 4.61 + TRENCH_BAR_WIDTH / 2, - TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, - TRENCH_HEIGHT + TRENCH_BAR_HEIGHT)); - fuelCollideRectangle( - this, - new Translation3d(4.61 - TRENCH_BAR_WIDTH / 2, FIELD_WIDTH - 1.57, TRENCH_HEIGHT), - new Translation3d( - 4.61 + TRENCH_BAR_WIDTH / 2, FIELD_WIDTH, TRENCH_HEIGHT + TRENCH_BAR_HEIGHT)); - fuelCollideRectangle( - this, - new Translation3d(FIELD_LENGTH - 4.61 - TRENCH_BAR_WIDTH / 2, 0, TRENCH_HEIGHT), - new Translation3d( - FIELD_LENGTH - 4.61 + TRENCH_BAR_WIDTH / 2, - TRENCH_WIDTH + TRENCH_BLOCK_WIDTH, - TRENCH_HEIGHT + TRENCH_BAR_HEIGHT)); - fuelCollideRectangle( - this, - new Translation3d( - FIELD_LENGTH - 4.61 - TRENCH_BAR_WIDTH / 2, FIELD_WIDTH - 1.57, TRENCH_HEIGHT), - new Translation3d( - FIELD_LENGTH - 4.61 + TRENCH_BAR_WIDTH / 2, - FIELD_WIDTH, - TRENCH_HEIGHT + TRENCH_BAR_HEIGHT)); + public void enableAirResistance() { + simulateAirResistance = true; } - private void addImpulse(Translation3d impulse) { - vel = vel.plus(impulse); + public void setSubticks(int subticks) { + FuelSim.subticks = subticks; } - } - - private static void handleFuelCollision(Fuel a, Fuel b) { - Translation3d normal = a.pos.minus(b.pos); - double distance = normal.getNorm(); - if (distance == 0) { - normal = new Translation3d(1, 0, 0); - distance = 1; + + public void clearFuel() { + fuels.clear(); } - normal = normal.div(distance); - double impulse = 0.5 * (1 + FUEL_COR) * (b.vel.minus(a.vel).dot(normal)); - double intersection = FUEL_RADIUS * 2 - distance; - a.pos = a.pos.plus(normal.times(intersection / 2)); - b.pos = b.pos.minus(normal.times(intersection / 2)); - a.addImpulse(normal.times(impulse)); - b.addImpulse(normal.times(-impulse)); - } - - private static final double CELL_SIZE = 0.25; - private static final int GRID_COLS = (int) Math.ceil(FIELD_LENGTH / CELL_SIZE); - private static final int GRID_ROWS = (int) Math.ceil(FIELD_WIDTH / CELL_SIZE); - - @SuppressWarnings("unchecked") - private final ArrayList[][] grid = new ArrayList[GRID_COLS][GRID_ROWS]; - - private void handleFuelCollisions(ArrayList fuels) { - // Clear grid - for (int i = 0; i < GRID_COLS; i++) { - for (int j = 0; j < GRID_ROWS; j++) { - grid[i][j].clear(); - } + + public void updateSim() { + if (!running) { + return; + } + stepSim(); } - // Populate grid - for (Fuel fuel : fuels) { - int col = (int) (fuel.pos.getX() / CELL_SIZE); - int row = (int) (fuel.pos.getY() / CELL_SIZE); + public void stepSim() { + for (int sub = 0; sub < subticks; sub++) { - if (col >= 0 && col < GRID_COLS && row >= 0 && row < GRID_ROWS) { - grid[col][row].add(fuel); - } - } + for (int i = 0; i < fuels.size(); i++) { + Fuel fuel = fuels.get(i); + fuel.update(); - // Check collisions - for (Fuel fuel : fuels) { - int col = (int) (fuel.pos.getX() / CELL_SIZE); - int row = (int) (fuel.pos.getY() / CELL_SIZE); - - // Check 3x3 neighbor cells - for (int i = col - 1; i <= col + 1; i++) { - for (int j = row - 1; j <= row + 1; j++) { - if (i >= 0 && i < GRID_COLS && j >= 0 && j < GRID_ROWS) { - for (Fuel other : grid[i][j]) { - if (fuel != other && fuel.pos.getDistance(other.pos) < FUEL_RADIUS * 2) { - if (fuel.hashCode() < other.hashCode()) { - handleFuelCollision(fuel, other); + if (fuel.finished) { + completedTraj.add(fuel.traj); + if (completedTraj.size() > 1) { + completedTraj.remove(0); + } + fuels.remove(i--); } - } } - } + + handleFuelCollisions(fuels); + + if (robotPoseSupplier != null) { + handleRobotCollisions(fuels); + handleIntakes(fuels); + } } - } - } - } - - private ArrayList fuels = new ArrayList(); - private boolean running = false; - private boolean simulateAirResistance = false; - private Supplier robotPoseSupplier = null; - private Supplier robotFieldSpeedsSupplier = null; - private double robotWidth; // size along the robot's y axis - private double robotLength; // size along the robot's x axis - private double bumperHeight; - private ArrayList intakes = new ArrayList<>(); - - /** Returns a singleton instance of FuelSim */ - public static FuelSim getInstance() { - if (instance == null) { - instance = new FuelSim(); + + logFuels(); } - return instance; - } - - /** Clears the field of fuel */ - public void clearFuel() { - fuels.clear(); - } - - /** Spawns fuel in the neutral zone and depots */ - public void spawnStartingFuel() { - // Center fuel - Translation3d center = new Translation3d(FIELD_LENGTH / 2, FIELD_WIDTH / 2, FUEL_RADIUS); - for (int i = 0; i < 15; i++) { - for (int j = 0; j < 6; j++) { - fuels.add( - new Fuel( - center.plus(new Translation3d(0.076 + 0.152 * j, 0.0254 + 0.076 + 0.152 * i, 0)))); - fuels.add( - new Fuel( - center.plus(new Translation3d(-0.076 - 0.152 * j, 0.0254 + 0.076 + 0.152 * i, 0)))); - fuels.add( - new Fuel( - center.plus(new Translation3d(0.076 + 0.152 * j, -0.0254 - 0.076 - 0.152 * i, 0)))); - fuels.add( - new Fuel( - center.plus( - new Translation3d(-0.076 - 0.152 * j, -0.0254 - 0.076 - 0.152 * i, 0)))); - } + public void logFuels() { + Logger.recordOutput( + "Fuel Simulation/Fuels", + fuels.stream().map(f -> f.pos).toArray(Translation3d[]::new)); + + ArrayList meanTraj = computeMeanTrajectory(); + Logger.recordOutput( + "Fuel Simulation/Mean Trajectory", + meanTraj.toArray(new Translation3d[0])); } - // Depots - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 4; j++) { - fuels.add( - new Fuel(new Translation3d(0.076 + 0.152 * j, 5.95 + 0.076 + 0.152 * i, FUEL_RADIUS))); - fuels.add( - new Fuel(new Translation3d(0.076 + 0.152 * j, 5.95 - 0.076 - 0.152 * i, FUEL_RADIUS))); - fuels.add( - new Fuel( - new Translation3d( - FIELD_LENGTH - 0.076 - 0.152 * j, 2.09 + 0.076 + 0.152 * i, FUEL_RADIUS))); - fuels.add( - new Fuel( - new Translation3d( - FIELD_LENGTH - 0.076 - 0.152 * j, 2.09 - 0.076 - 0.152 * i, FUEL_RADIUS))); - } + public ArrayList computeMeanTrajectory() { + if (completedTraj.isEmpty()) { + return new ArrayList<>(); + } + + int minLength = + completedTraj.stream().mapToInt(List::size).min().orElse(0); + + ArrayList mean = new ArrayList<>(); + int maxPoints = 15; + int step = Math.max(minLength / maxPoints, 1); + + for (int i = 0; i < minLength && mean.size() < maxPoints; i += step) { + double x = 0; + double y = 0; + double z = 0; + + for (List traj : completedTraj) { + Translation3d p = traj.get(i); + x += p.getX(); + y += p.getY(); + z += p.getZ(); + } + + int n = completedTraj.size(); + mean.add(new Translation3d(x / n, y / n, z / n)); + } + + return mean; } - // DEBUG: Log XZ lines - // Translation3d[][] lines = new Translation3d[FIELD_XZ_LINE_STARTS.length][2]; - // for (int i = 0; i < FIELD_XZ_LINE_STARTS.length; i++) { - // lines[i][0] = FIELD_XZ_LINE_STARTS[i]; - // lines[i][1] = FIELD_XZ_LINE_ENDS[i]; - // } - - // Logger.recordOutput("Fuel Simulation/Lines (debug)", lines); - } - - /** - * Adds array of `Translation3d`'s to NetworkTables at - * "AdvantageKit/RealOutputs/Fuel - * Simulation/Fuels" - */ - public void logFuels() { - Logger.recordOutput( - "Fuel Simulation/Fuels", - fuels.stream().map((fuel) -> fuel.pos).toArray(Translation3d[]::new)); - ArrayList meanTraj = computeMeanTrajectory(); - Logger.recordOutput("Fuel Simulation/Mean Trajectory", meanTraj.toArray(new Translation3d[0])); - } - - /** Start the simulation. `updateSim` must still be called every loop */ - public void start() { - running = true; - } - - /** Pause the simulation. */ - public void stop() { - running = false; - } - - /** Enables accounting for drag force in physics step * */ - public void enableAirResistance() { - simulateAirResistance = true; - } - - /** - * Sets the number of physics iterations per loop (0.02s) - * - * @param subticks - */ - public void setSubticks(int subticks) { - FuelSim.subticks = subticks; - } - - /** - * Registers a robot with the fuel simulator - * - * @param width from left to right (y-axis) - * @param length from front to back (x-axis) - * @param bumperHeight - * @param poseSupplier - * @param fieldSpeedsSupplier field-relative `ChassisSpeeds` supplier - */ - public void registerRobot( - double width, - double length, - double bumperHeight, - Supplier poseSupplier, - Supplier fieldSpeedsSupplier) { - this.robotPoseSupplier = poseSupplier; - this.robotFieldSpeedsSupplier = fieldSpeedsSupplier; - this.robotWidth = width; - this.robotLength = length; - this.bumperHeight = bumperHeight; - } - - /** To be called periodically Will do nothing if sim is not running */ - public void updateSim() { - if (!running) - return; - - stepSim(); - } - - /** Run the simulation forward 1 time step (0.02s) */ - public void stepSim() { - for (int sub = 0; sub < subticks; sub++) { - - for (int f = 0; f < fuels.size(); f++) { - Fuel fuel = fuels.get(f); - fuel.update(); - - if (fuel.finished) { - completedTraj.add(fuel.traj); - - if (completedTraj.size() > 1) { - completedTraj.remove(0); - } - - fuels.remove(f); - f--; - } - - } - - handleFuelCollisions(fuels); - - if (robotPoseSupplier != null) { - handleRobotCollisions(fuels); - handleIntakes(fuels); - } + public void spawnFuel(Translation3d pos, Translation3d vel) { + fuels.add(new Fuel(pos, vel)); } - logFuels(); - } + public void registerRobot( + double width, + double length, + double bumperHeight, + Supplier poseSupplier, + Supplier fieldSpeedsSupplier) { - public ArrayList computeMeanTrajectory() { - if (completedTraj.isEmpty()) - return new ArrayList<>(); + this.robotWidth = width; + this.robotLength = length; + this.bumperHeight = bumperHeight; + this.robotPoseSupplier = poseSupplier; + this.robotFieldSpeedsSupplier = fieldSpeedsSupplier; + } - int minLength = completedTraj.stream().mapToInt(List::size).min().orElse(0); + public void launchFuel( + LinearVelocity launchVelocity, + Angle hoodAngle, + Angle turretYaw, + Distance launchHeight) { - ArrayList mean = new ArrayList<>(); + if (robotPoseSupplier == null || robotFieldSpeedsSupplier == null) { + throw new IllegalStateException("Robot must be registered before launching fuel."); + } - // Limit to MAX_POINTS entries - int MAX_POINTS = 15; - int step = Math.max(minLength / MAX_POINTS, 1); // how many indices to skip per entry + Pose3d launchPose = + new Pose3d(robotPoseSupplier.get()) + .plus( + new Transform3d( + new Translation3d( + Meters.zero(), Meters.zero(), launchHeight), + Rotation3d.kZero)); - for (int i = 0; i < minLength; i += step) { - double x = 0, y = 0, z = 0; + ChassisSpeeds speeds = robotFieldSpeedsSupplier.get(); - for (List traj : completedTraj) { - Translation3d p = traj.get(i); - x += p.getX(); - y += p.getY(); - z += p.getZ(); - } + double horizontal = + Math.cos(hoodAngle.in(Radians)) * launchVelocity.in(MetersPerSecond); + double vertical = + Math.sin(hoodAngle.in(Radians)) * launchVelocity.in(MetersPerSecond); - int n = completedTraj.size(); - mean.add(new Translation3d(x / n, y / n, z / n)); + double xVel = + horizontal * Math.cos(turretYaw.in(Radians)) + speeds.vxMetersPerSecond; + double yVel = + horizontal * Math.sin(turretYaw.in(Radians)) + speeds.vyMetersPerSecond; - if (mean.size() >= MAX_POINTS) - break; // stop once we reach max + spawnFuel(launchPose.getTranslation(), new Translation3d(xVel, yVel, vertical)); } - return mean; - } - - /** - * Adds a fuel onto the field - * - * @param pos Position to spawn at - * @param vel Initial velocity vector - */ - public void spawnFuel(Translation3d pos, Translation3d vel) { - fuels.add(new Fuel(pos, vel)); - } - - /** - * Spawns a fuel onto the field with a specified launch velocity and angles, - * accounting for robot - * movement - * - * @param launchHeight Height of the fuel to launch at. Make sure this is - * higher than your robot's - * bumper height, or else it will collide with your robot - * immediately. - * @param launchVelocity Initial launch velocity - * @param hoodAngle Hood angle where 0 is launching horizontally and 90 - * degrees is launching - * straight up - * @param turretYaw Field-relative turret yaw - * @throws IllegalStateException if robot is not registered - */ - public void launchFuel( - LinearVelocity launchVelocity, Angle hoodAngle, Angle turretYaw, Distance launchHeight) { - if (robotPoseSupplier == null || robotFieldSpeedsSupplier == null) { - throw new IllegalStateException("Robot must be registered before launching fuel."); - } - Pose3d launchPose = new Pose3d(this.robotPoseSupplier.get()) - .plus( - new Transform3d( - new Translation3d(Meters.zero(), Meters.zero(), launchHeight), - Rotation3d.kZero)); - ChassisSpeeds fieldSpeeds = this.robotFieldSpeedsSupplier.get(); - - double horizontalVel = Math.cos(hoodAngle.in(Radians)) * launchVelocity.in(MetersPerSecond); - double verticalVel = Math.sin(hoodAngle.in(Radians)) * launchVelocity.in(MetersPerSecond); - double xVel = horizontalVel * Math.cos(turretYaw.in(Radians)); - double yVel = horizontalVel * Math.sin(turretYaw.in(Radians)); - - xVel += fieldSpeeds.vxMetersPerSecond; - yVel += fieldSpeeds.vyMetersPerSecond; - - spawnFuel(launchPose.getTranslation(), new Translation3d(xVel, yVel, verticalVel)); - } - - private void handleRobotCollision(Fuel fuel, Pose2d robot, Translation2d robotVel) { - Translation2d relativePos = new Pose2d(fuel.pos.toTranslation2d(), Rotation2d.kZero).relativeTo(robot) - .getTranslation(); - - if (fuel.pos.getZ() > bumperHeight) - return; // above bumpers - double distanceToBottom = -FUEL_RADIUS - robotLength / 2 - relativePos.getX(); - double distanceToTop = -FUEL_RADIUS - robotLength / 2 + relativePos.getX(); - double distanceToRight = -FUEL_RADIUS - robotWidth / 2 - relativePos.getY(); - double distanceToLeft = -FUEL_RADIUS - robotWidth / 2 + relativePos.getY(); - - // not inside robot - if (distanceToBottom > 0 || distanceToTop > 0 || distanceToRight > 0 || distanceToLeft > 0) - return; - - Translation2d posOffset; - // find minimum distance to side and send corresponding collision response - if ((distanceToBottom >= distanceToTop - && distanceToBottom >= distanceToRight - && distanceToBottom >= distanceToLeft)) { - posOffset = new Translation2d(distanceToBottom, 0); - } else if ((distanceToTop >= distanceToBottom - && distanceToTop >= distanceToRight - && distanceToTop >= distanceToLeft)) { - posOffset = new Translation2d(-distanceToTop, 0); - } else if ((distanceToRight >= distanceToBottom - && distanceToRight >= distanceToTop - && distanceToRight >= distanceToLeft)) { - posOffset = new Translation2d(0, distanceToRight); - } else { - posOffset = new Translation2d(0, -distanceToLeft); - } + private class Fuel { - posOffset = posOffset.rotateBy(robot.getRotation()); - fuel.pos = fuel.pos.plus(new Translation3d(posOffset)); - Translation2d normal = posOffset.div(posOffset.getNorm()); - if (fuel.vel.toTranslation2d().dot(normal) < 0) - fuel.addImpulse( - new Translation3d( - normal.times(-fuel.vel.toTranslation2d().dot(normal) * (1 + ROBOT_COR)))); - if (robotVel.dot(normal) > 0) - fuel.addImpulse(new Translation3d(normal.times(robotVel.dot(normal)))); - } - - private void handleRobotCollisions(ArrayList fuels) { - Pose2d robot = robotPoseSupplier.get(); - ChassisSpeeds speeds = robotFieldSpeedsSupplier.get(); - Translation2d robotVel = new Translation2d(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond); - - for (Fuel fuel : fuels) { - handleRobotCollision(fuel, robot, robotVel); - } - } + private Translation3d pos; + private Translation3d vel; + private boolean finished; + private final List traj = new ArrayList<>(); - private void handleIntakes(ArrayList fuels) { - Pose2d robot = robotPoseSupplier.get(); - for (SimIntake intake : intakes) { - for (int i = 0; i < fuels.size(); i++) { - if (intake.shouldIntake(fuels.get(i), robot)) { - fuels.remove(i); - i--; + private Fuel(Translation3d pos, Translation3d vel) { + this.pos = pos; + this.vel = vel; } - } - } - } - - private static void fuelCollideRectangle(Fuel fuel, Translation3d start, Translation3d end) { - if (fuel.pos.getZ() > end.getZ() + FUEL_RADIUS || fuel.pos.getZ() < start.getZ() - FUEL_RADIUS) - return; // above rectangle - double distanceToLeft = start.getX() - FUEL_RADIUS - fuel.pos.getX(); - double distanceToRight = fuel.pos.getX() - end.getX() - FUEL_RADIUS; - double distanceToTop = fuel.pos.getY() - end.getY() - FUEL_RADIUS; - double distanceToBottom = start.getY() - FUEL_RADIUS - fuel.pos.getY(); - - // not inside hub - if (distanceToLeft > 0 || distanceToRight > 0 || distanceToTop > 0 || distanceToBottom > 0) - return; - - Translation2d collision; - // find minimum distance to side and send corresponding collision response - if (fuel.pos.getX() < start.getX() - || (distanceToLeft >= distanceToRight - && distanceToLeft >= distanceToTop - && distanceToLeft >= distanceToBottom)) { - collision = new Translation2d(distanceToLeft, 0); - } else if (fuel.pos.getX() >= end.getX() - || (distanceToRight >= distanceToLeft - && distanceToRight >= distanceToTop - && distanceToRight >= distanceToBottom)) { - collision = new Translation2d(-distanceToRight, 0); - } else if (fuel.pos.getY() > end.getY() - || (distanceToTop >= distanceToLeft - && distanceToTop >= distanceToRight - && distanceToTop >= distanceToBottom)) { - collision = new Translation2d(0, -distanceToTop); - } else { - collision = new Translation2d(0, distanceToBottom); - } - if (collision.getX() != 0) { - fuel.pos = fuel.pos.plus(new Translation3d(collision)); - fuel.vel = fuel.vel.plus(new Translation3d(-(1 + FIELD_COR) * fuel.vel.getX(), 0, 0)); - } else if (collision.getY() != 0) { - fuel.pos = fuel.pos.plus(new Translation3d(collision)); - fuel.vel = fuel.vel.plus(new Translation3d(0, -(1 + FIELD_COR) * fuel.vel.getY(), 0)); - } - } - - /** - * Registers an intake with the fuel simulator. This intake will remove fuel - * from the field based - * on the `ableToIntake` parameter. - * - * @param xMin Minimum x position for the bounding box - * @param xMax Maximum x position for the bounding box - * @param yMin Minimum y position for the bounding box - * @param yMax Maximum y position for the bounding box - * @param ableToIntake Should a return a boolean whether the intake is active - * @param intakeCallback Function to call when a fuel is intaked - */ - public void registerIntake( - double xMin, - double xMax, - double yMin, - double yMax, - BooleanSupplier ableToIntake, - Runnable intakeCallback) { - intakes.add(new SimIntake(xMin, xMax, yMin, yMax, ableToIntake, intakeCallback)); - } - - /** - * Registers an intake with the fuel simulator. This intake will remove fuel - * from the field based - * on the `ableToIntake` parameter. - * - * @param xMin Minimum x position for the bounding box - * @param xMax Maximum x position for the bounding box - * @param yMin Minimum y position for the bounding box - * @param yMax Maximum y position for the bounding box - * @param ableToIntake Should a return a boolean whether the intake is active - */ - public void registerIntake( - double xMin, double xMax, double yMin, double yMax, BooleanSupplier ableToIntake) { - registerIntake(xMin, xMax, yMin, yMax, ableToIntake, () -> { - }); - } - - /** - * Registers an intake with the fuel simulator. This intake will always remove - * fuel from the - * field. - * - * @param xMin Minimum x position for the bounding box - * @param xMax Maximum x position for the bounding box - * @param yMin Minimum y position for the bounding box - * @param yMax Maximum y position for the bounding box - * @param intakeCallback Function to call when a fuel is intaked - */ - public void registerIntake( - double xMin, double xMax, double yMin, double yMax, Runnable intakeCallback) { - registerIntake(xMin, xMax, yMin, yMax, () -> true, intakeCallback); - } - - /** - * Registers an intake with the fuel simulator. This intake will always remove - * fuel from the - * field. - * - * @param xMin Minimum x position for the bounding box - * @param xMax Maximum x position for the bounding box - * @param yMin Minimum y position for the bounding box - * @param yMax Maximum y position for the bounding box - */ - public void registerIntake(double xMin, double xMax, double yMin, double yMax) { - registerIntake(xMin, xMax, yMin, yMax, () -> true, () -> { - }); - } - - public static class Hub { - public static final Hub BLUE_HUB = new Hub( - new Translation2d(4.61, FIELD_WIDTH / 2), - new Translation3d(5.3, FIELD_WIDTH / 2, 0.89), - 1); - public static final Hub RED_HUB = new Hub( - new Translation2d(FIELD_LENGTH - 4.61, FIELD_WIDTH / 2), - new Translation3d(FIELD_LENGTH - 5.3, FIELD_WIDTH / 2, 0.89), - -1); - - private static final double ENTRY_HEIGHT = 1.83; - private static final double ENTRY_RADIUS = 0.56; - - private static final double SIDE = 1.2; - - private static final double NET_HEIGHT_MAX = 3.057; - private static final double NET_HEIGHT_MIN = 1.5; - private static final double NET_OFFSET = SIDE / 2 + 0.261; - private static final double NET_WIDTH = 1.484; - - private final Translation2d center; - private final Translation3d exit; - private final int exitVelXMult; - - private int score = 0; - - private Hub(Translation2d center, Translation3d exit, int exitVelXMult) { - this.center = center; - this.exit = exit; - this.exitVelXMult = exitVelXMult; - } + private Fuel(Translation3d pos) { + this(pos, new Translation3d()); + } - private boolean handleHubInteraction(Fuel fuel) { - if (didFuelScore(fuel)) { - fuel.pos = exit; - fuel.vel = getDispersalVelocity(); - score++; - return true; - } - return false; - } + private void update() { + if (!finished) { + traj.add(pos); + } + + pos = pos.plus(vel.times(PERIOD / subticks)); + + if (pos.getZ() > FUEL_RADIUS) { + Translation3d gravityForce = GRAVITY.times(FUEL_MASS); + Translation3d dragForce = new Translation3d(); + + if (simulateAirResistance) { + double speed = vel.getNorm(); + if (speed > 1e-6) { + dragForce = vel.times(-DRAG_FORCE_FACTOR * speed); + } + } + + Translation3d accel = + gravityForce.plus(dragForce).div(FUEL_MASS); + vel = vel.plus(accel.times(PERIOD / subticks)); + } + + if (Math.abs(vel.getZ()) < 0.05 && pos.getZ() <= FUEL_RADIUS + 0.03) { + vel = new Translation3d(vel.getX(), vel.getY(), 0.0); + vel = vel.times(1.0 - FRICTION * PERIOD / subticks); + } + + if (pos.getZ() <= FUEL_RADIUS && Math.abs(vel.getZ()) <= 3.0) { + finished = true; + } + + handleFieldCollisions(); + } - private boolean didFuelScore(Fuel fuel) { - return fuel.pos.toTranslation2d().getDistance(center) <= ENTRY_RADIUS - && fuel.pos.getZ() <= ENTRY_HEIGHT - && fuel.pos.minus(fuel.vel.times(PERIOD / subticks)).getZ() > ENTRY_HEIGHT; + private void handleFieldCollisions() { + handleHubCollisions(Hub.BLUE_HUB); + handleHubCollisions(Hub.RED_HUB); + } + + private void handleHubCollisions(Hub hub) { + if (hub.handleHubInteraction(this)) { + finished = true; + } + + hub.fuelCollideSide(this); + + double netCollision = hub.fuelHitNet(this); + if (netCollision != 0.0) { + pos = pos.plus(new Translation3d(netCollision, 0, 0)); + vel = + new Translation3d( + -vel.getX() * NET_COR, + vel.getY() * NET_COR, + vel.getZ()); + } + } + + private void addImpulse(Translation3d impulse) { + vel = vel.plus(impulse); + } } - private Translation3d getDispersalVelocity() { - return new Translation3d( - exitVelXMult * (Math.random() + 0.1) * 1.5, Math.random() * 2 - 1, 0); + + public static class Hub { + + public static final Hub BLUE_HUB = + new Hub( + new Translation2d(4.61, FIELD_WIDTH / 2.0), + new Translation3d(5.3, FIELD_WIDTH / 2.0, 0.89), + 1); + + public static final Hub RED_HUB = + new Hub( + new Translation2d(FIELD_LENGTH - 4.61, FIELD_WIDTH / 2.0), + new Translation3d( + FIELD_LENGTH - 5.3, FIELD_WIDTH / 2.0, 0.89), + -1); + + private static final double ENTRY_HEIGHT = 1.83; + private static final double ENTRY_RADIUS = 0.56; + + private static final double SIDE = 1.2; + private static final double NET_HEIGHT_MAX = 3.057; + private static final double NET_HEIGHT_MIN = 1.5; + private static final double NET_OFFSET = SIDE / 2.0 + 0.261; + private static final double NET_WIDTH = 1.484; + + private final Translation2d center; + private final Translation3d exit; + private final int exitVelXMult; + + private int score; + + private Hub(Translation2d center, Translation3d exit, int exitVelXMult) { + this.center = center; + this.exit = exit; + this.exitVelXMult = exitVelXMult; + } + + private boolean handleHubInteraction(Fuel fuel) { + if (didFuelScore(fuel)) { + fuel.pos = exit; + fuel.vel = getDispersalVelocity(); + score++; + return true; + } + return false; + } + + private boolean didFuelScore(Fuel fuel) { + return fuel.pos.toTranslation2d().getDistance(center) <= ENTRY_RADIUS + && fuel.pos.getZ() <= ENTRY_HEIGHT + && fuel.pos.minus(fuel.vel.times(PERIOD / subticks)).getZ() + > ENTRY_HEIGHT; + } + + private Translation3d getDispersalVelocity() { + return new Translation3d( + exitVelXMult * (Math.random() + 0.1) * 1.5, + Math.random() * 2.0 - 1.0, + 0.0); + } + + public int getScore() { + return score; + } + + public void resetScore() { + score = 0; + } + + private void fuelCollideSide(Fuel fuel) { + fuelCollideRectangle( + fuel, + new Translation3d( + center.getX() - SIDE / 2.0, + center.getY() - SIDE / 2.0, + 0.0), + new Translation3d( + center.getX() + SIDE / 2.0, + center.getY() + SIDE / 2.0, + ENTRY_HEIGHT - 0.1)); + } + + private double fuelHitNet(Fuel fuel) { + if (fuel.pos.getZ() > NET_HEIGHT_MAX || fuel.pos.getZ() < NET_HEIGHT_MIN) { + return 0.0; + } + + if (fuel.pos.getY() > center.getY() + NET_WIDTH / 2.0 + || fuel.pos.getY() < center.getY() - NET_WIDTH / 2.0) { + return 0.0; + } + + double netX = center.getX() + NET_OFFSET * exitVelXMult; + + if (fuel.pos.getX() > netX) { + return Math.max(0.0, netX - (fuel.pos.getX() - FUEL_RADIUS)); + } else { + return Math.min(0.0, netX - (fuel.pos.getX() + FUEL_RADIUS)); + } + } } - /** Reset this hub's score to 0 */ - public void resetScore() { - score = 0; + + private class SimIntake { + + private final double xMin; + private final double xMax; + private final double yMin; + private final double yMax; + + private final BooleanSupplier ableToIntake; + private final Runnable callback; + + private SimIntake( + double xMin, + double xMax, + double yMin, + double yMax, + BooleanSupplier ableToIntake, + Runnable callback) { + + this.xMin = xMin; + this.xMax = xMax; + this.yMin = yMin; + this.yMax = yMax; + this.ableToIntake = ableToIntake; + this.callback = callback; + } + + private boolean shouldIntake(Fuel fuel, Pose2d robotPose) { + if (!ableToIntake.getAsBoolean() || fuel.pos.getZ() > bumperHeight) { + return false; + } + + Translation2d relativePos = + new Pose2d(fuel.pos.toTranslation2d(), Rotation2d.kZero) + .relativeTo(robotPose) + .getTranslation(); + + boolean inside = + relativePos.getX() >= xMin + && relativePos.getX() <= xMax + && relativePos.getY() >= yMin + && relativePos.getY() <= yMax; + + if (inside) { + callback.run(); + } + + return inside; + } } - /** - * Get the current count of fuel scored in this hub - * - * @return - */ - public int getScore() { - return score; + + private void handleFuelCollisions(ArrayList fuels) { + for (int i = 0; i < GRID_COLS; i++) { + for (int j = 0; j < GRID_ROWS; j++) { + grid[i][j].clear(); + } + } + + for (Fuel fuel : fuels) { + int col = (int) (fuel.pos.getX() / CELL_SIZE); + int row = (int) (fuel.pos.getY() / CELL_SIZE); + + if (col >= 0 && col < GRID_COLS && row >= 0 && row < GRID_ROWS) { + grid[col][row].add(fuel); + } + } + + for (Fuel fuel : fuels) { + int col = (int) (fuel.pos.getX() / CELL_SIZE); + int row = (int) (fuel.pos.getY() / CELL_SIZE); + + for (int i = col - 1; i <= col + 1; i++) { + for (int j = row - 1; j <= row + 1; j++) { + if (i < 0 || i >= GRID_COLS || j < 0 || j >= GRID_ROWS) { + continue; + } + + for (Fuel other : grid[i][j]) { + if (fuel != other + && fuel.pos.getDistance(other.pos) < FUEL_RADIUS * 2.0 + && fuel.hashCode() < other.hashCode()) { + handleFuelCollision(fuel, other); + } + } + } + } + } } - private void fuelCollideSide(Fuel fuel) { - fuelCollideRectangle( - fuel, - new Translation3d(center.getX() - SIDE / 2, center.getY() - SIDE / 2, 0), - new Translation3d( - center.getX() + SIDE / 2, center.getY() + SIDE / 2, ENTRY_HEIGHT - 0.1)); - // if (fuel.pos.getZ() > ENTRY_HEIGHT - 0.1) return new Translation2d(); // - // above hub - // double distanceToLeft = center.getX() - SIDE / 2 - FUEL_RADIUS - - // fuel.pos.getX(); - // double distanceToRight = fuel.pos.getX() - center.getX() - SIDE / 2 - - // FUEL_RADIUS; - // double distanceToTop = center.getY() - SIDE / 2 - FUEL_RADIUS - - // fuel.pos.getY(); - // double distanceToBottom = fuel.pos.getY() - center.getY() - SIDE / 2 - - // FUEL_RADIUS; - - // // not inside hub - // if (distanceToLeft > 0 || distanceToRight > 0 || distanceToTop > 0 || - // distanceToBottom > 0) - // return new Translation2d(); - - // // find minimum distance to side and send corresponding collision response - // if (fuel.pos.getX() < center.getX() - SIDE / 2 - // || (distanceToLeft >= distanceToRight - // && distanceToLeft >= distanceToTop - // && distanceToLeft >= distanceToBottom)) { - // return new Translation2d(distanceToLeft, 0); - // } else if (fuel.pos.getX() >= center.getX() + SIDE / 2 - // || (distanceToRight >= distanceToLeft - // && distanceToRight >= distanceToTop - // && distanceToRight >= distanceToBottom)) { - // return new Translation2d(-distanceToRight, 0); - // } else if (fuel.pos.getY() > center.getY() + SIDE / 2 - // || (distanceToTop >= distanceToLeft - // && distanceToTop >= distanceToRight - // && distanceToTop >= distanceToBottom)) { - // return new Translation2d(0, -distanceToTop); - // } else { - // return new Translation2d(0, distanceToBottom); - // } + private static void handleFuelCollision(Fuel a, Fuel b) { + Translation3d normal = a.pos.minus(b.pos); + double distance = normal.getNorm(); + + if (distance == 0.0) { + normal = new Translation3d(1, 0, 0); + distance = 1.0; + } + + normal = normal.div(distance); + + double impulse = + 0.5 * (1.0 + FUEL_COR) * (b.vel.minus(a.vel).dot(normal)); + + double penetration = FUEL_RADIUS * 2.0 - distance; + + a.pos = a.pos.plus(normal.times(penetration / 2.0)); + b.pos = b.pos.minus(normal.times(penetration / 2.0)); + + a.addImpulse(normal.times(impulse)); + b.addImpulse(normal.times(-impulse)); } - private double fuelHitNet(Fuel fuel) { - if (fuel.pos.getZ() > NET_HEIGHT_MAX || fuel.pos.getZ() < NET_HEIGHT_MIN) - return 0; - if (fuel.pos.getY() > center.getY() + NET_WIDTH / 2 - || fuel.pos.getY() < center.getY() - NET_WIDTH / 2) - return 0; - if (fuel.pos.getX() > center.getX() + NET_OFFSET * exitVelXMult) { - return Math.max( - 0, center.getX() + NET_OFFSET * exitVelXMult - (fuel.pos.getX() - FUEL_RADIUS)); - } else { - return Math.min( - 0, center.getX() + NET_OFFSET * exitVelXMult - (fuel.pos.getX() + FUEL_RADIUS)); - } + private void handleRobotCollisions(ArrayList fuels) { + Pose2d robot = robotPoseSupplier.get(); + ChassisSpeeds speeds = robotFieldSpeedsSupplier.get(); + + Translation2d robotVel = + new Translation2d(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond); + + for (Fuel fuel : fuels) { + handleRobotCollision(fuel, robot, robotVel); + } } - } - - private class SimIntake { - double xMin, xMax, yMin, yMax; - BooleanSupplier ableToIntake; - Runnable callback; - - private SimIntake( - double xMin, - double xMax, - double yMin, - double yMax, - BooleanSupplier ableToIntake, - Runnable intakeCallback) { - this.xMin = xMin; - this.xMax = xMax; - this.yMin = yMin; - this.yMax = yMax; - this.ableToIntake = ableToIntake; - this.callback = intakeCallback; + + private void handleRobotCollision( + Fuel fuel, Pose2d robot, Translation2d robotVel) { + + Translation2d relativePos = + new Pose2d(fuel.pos.toTranslation2d(), Rotation2d.kZero) + .relativeTo(robot) + .getTranslation(); + + if (fuel.pos.getZ() > bumperHeight) { + return; + } + + double dx = robotLength / 2.0 + FUEL_RADIUS - Math.abs(relativePos.getX()); + double dy = robotWidth / 2.0 + FUEL_RADIUS - Math.abs(relativePos.getY()); + + if (dx <= 0.0 || dy <= 0.0) { + return; + } + + Translation2d normal = + dx < dy + ? new Translation2d(Math.signum(relativePos.getX()), 0.0) + : new Translation2d(0.0, Math.signum(relativePos.getY())); + + normal = normal.rotateBy(robot.getRotation()); + + fuel.pos = fuel.pos.plus(new Translation3d(normal.times(Math.min(dx, dy)))); + + double relVel = fuel.vel.toTranslation2d().dot(normal); + if (relVel < 0.0) { + fuel.addImpulse( + new Translation3d(normal.times(-relVel * (1.0 + ROBOT_COR)))); + } + + double robotRel = robotVel.dot(normal); + if (robotRel > 0.0) { + fuel.addImpulse(new Translation3d(normal.times(robotRel))); + } } - private boolean shouldIntake(Fuel fuel, Pose2d robotPose) { - if (!ableToIntake.getAsBoolean() || fuel.pos.getZ() > bumperHeight) - return false; - - Translation2d fuelRelativePos = new Pose2d(fuel.pos.toTranslation2d(), Rotation2d.kZero) - .relativeTo(robotPose) - .getTranslation(); - - boolean result = fuelRelativePos.getX() >= xMin - && fuelRelativePos.getX() <= xMax - && fuelRelativePos.getY() >= yMin - && fuelRelativePos.getY() <= yMax; - if (result) { - callback.run(); - } - return result; + private void handleIntakes(ArrayList fuels) { + Pose2d robot = robotPoseSupplier.get(); + + for (SimIntake intake : intakes) { + for (int i = 0; i < fuels.size(); i++) { + if (intake.shouldIntake(fuels.get(i), robot)) { + fuels.remove(i--); + } + } + } } - } - - private FuelSim() { - // Initialize grid - for (int i = 0; i < GRID_COLS; i++) { - for (int j = 0; j < GRID_ROWS; j++) { - grid[i][j] = new ArrayList(); - } + + private static void fuelCollideRectangle( + Fuel fuel, Translation3d start, Translation3d end) { + + if (fuel.pos.getZ() > end.getZ() + FUEL_RADIUS + || fuel.pos.getZ() < start.getZ() - FUEL_RADIUS) { + return; + } + + double left = start.getX() - FUEL_RADIUS - fuel.pos.getX(); + double right = fuel.pos.getX() - end.getX() - FUEL_RADIUS; + double top = fuel.pos.getY() - end.getY() - FUEL_RADIUS; + double bottom = start.getY() - FUEL_RADIUS - fuel.pos.getY(); + + if (left > 0 || right > 0 || top > 0 || bottom > 0) { + return; + } + + if (Math.abs(left) < Math.abs(right)) { + fuel.pos = fuel.pos.plus(new Translation3d(left, 0, 0)); + fuel.vel = + fuel.vel.plus( + new Translation3d(-(1.0 + FIELD_COR) * fuel.vel.getX(), 0, 0)); + } else { + fuel.pos = fuel.pos.plus(new Translation3d(-right, 0, 0)); + fuel.vel = + fuel.vel.plus( + new Translation3d(-(1.0 + FIELD_COR) * fuel.vel.getX(), 0, 0)); + } } - } }