PAPFController.java

package org.team340.lib.math;

import edu.wpi.first.epilogue.Logged;
import edu.wpi.first.epilogue.Logged.Strategy;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import java.util.ArrayList;
import java.util.Collections;
import java.util.List;
import org.team340.lib.tunable.TunableTable;
import org.team340.lib.tunable.Tunables.Tunable;

/**
 * Implements a predictive artificial potential field (P-APF),
 * capable of real-time motion planning with obstacle avoidance.
 *
 * <p>This implementation improves upon traditional APF algorithms by predicting the
 * future movement of the robot to determine intermediate setpoints, with the goal of
 * achieving smoother motion and preventing oscillation caused by suboptimal interactions
 * with potentials generated by obstacles.
 */
@Logged(strategy = Strategy.OPT_IN)
public final class PAPFController implements Tunable {

    private final GoalHeuristic goalHeuristic;
    private final Obstacle[] obstacles;

    private double resolution;
    private double horizon;
    private double tolerance;
    private boolean savePrediction;

    // spotless:off

    // Setpoint/goal are Pose2d instead of Translation2d
    // due to visualization options in AdvantageScope.
    @Logged private final List<Translation2d> prediction = new ArrayList<>();
    @Logged private Pose2d setpoint = Pose2d.kZero;
    @Logged private Pose2d goal = Pose2d.kZero;

    // spotless:on

    /**
     * Creates a new P-APF controller.
     * @param horizon The horizon at which to predict the robot's motion, in meters.
     *                Larger horizons increase computational cost, with the benefit
     *                of generally smoother motion.
     * @param resolution The resolution at which to sample the horizon in meters. Higher
     *                   resolutions (i.e. smaller values) will increase accuracy of
     *                   predicted motion, however are more computationally expensive.
     * @param tolerance The distance from the goal in meters at which to return a velocity output of 0.
     * @param savePrediction If the robot's predicted motion should be saved. Useful
     *                       for debugging, however does increase GC pressure.
     * @param obstacles The field's obstacles.
     */
    public PAPFController(
        double horizon,
        double resolution,
        double tolerance,
        boolean savePrediction,
        Obstacle[] obstacles
    ) {
        this(horizon, resolution, tolerance, savePrediction, GoalHeuristic.DEFAULT, obstacles);
    }

    /**
     * Creates a new P-APF controller.
     * @param horizon The horizon at which to predict the robot's motion, in meters.
     *                Larger horizons increase computational cost, with the benefit
     *                of generally smoother motion.
     * @param resolution The resolution at which to sample the horizon in meters. Higher
     *                   resolutions (i.e. smaller values) will increase accuracy of
     *                   predicted motion, however are more computationally expensive.
     * @param tolerance The distance from the goal in meters at which to return a velocity output of 0.
     * @param savePrediction If the robot's predicted motion should be saved. Useful
     *                       for debugging, however does increase GC pressure.
     * @param goalHeuristic A heuristic for calculating the strength of the
     *                      attractive force generated by the goal location.
     * @param obstacles The field's obstacles.
     */
    public PAPFController(
        double horizon,
        double resolution,
        double tolerance,
        boolean savePrediction,
        GoalHeuristic goalHeuristic,
        Obstacle[] obstacles
    ) {
        this.horizon = horizon;
        this.resolution = resolution;
        this.tolerance = tolerance;
        this.savePrediction = savePrediction;

        this.goalHeuristic = goalHeuristic;
        this.obstacles = obstacles;
    }

    /**
     * Returns the current setpoint of the controller from the last invocation of
     * {@link #calculate(Pose2d, Translation2d, double, double) PAPFController.calculate()}.
     */
    public Translation2d getSetpoint() {
        return setpoint.getTranslation();
    }

    /**
     * Returns the current goal of the controller from the last invocation of
     * {@link #calculate(Pose2d, Translation2d, double, double) PAPFController.calculate()}.
     */
    public Translation2d getGoal() {
        return goal.getTranslation();
    }

    /**
     * Returns the current predicted motion of the robot from the last invocation of
     * {@link #calculate(Pose2d, Translation2d, double, double) PAPFController.calculate()}.
     * Note that this list will be empty if {@code savePrediction} is {@code false}.
     */
    public List<Translation2d> getPrediction() {
        return Collections.unmodifiableList(prediction);
    }

    /**
     * Returns the next velocity output of the potential field. Note
     * that while deceleration is accounted for, acceleration from
     * rest is not. If acceleration limiting is desired, it must be
     * implemented downstream.
     * @param currentPose The current blue origin relative pose of the robot.
     * @param goal The desired position of the robot.
     * @param maxVelocity The desired cruise velocity of the robot, in m/s.
     * @param maxDeceleration The desired deceleration rate of the robot, in m/s/s.
     * @return Blue origin relative chassis speeds.
     */
    public ChassisSpeeds calculate(Pose2d currentPose, Translation2d goal, double maxVelocity, double maxDeceleration) {
        return calculate(currentPose, goal, maxVelocity, maxDeceleration, (Obstacle[]) null);
    }

    /**
     * Returns the next velocity output of the potential field. Note
     * that while deceleration is accounted for, acceleration from
     * rest is not. If acceleration limiting is desired, it must be
     * implemented downstream.
     * @param currentPose The current blue origin relative pose of the robot.
     * @param goal The desired position of the robot.
     * @param maxVelocity The desired cruise velocity of the robot, in m/s.
     * @param maxDeceleration The desired deceleration rate of the robot, in m/s/s.
     * @param tempObstacles Additional temporary obstacles. Can be null.
     * @return Blue origin relative chassis speeds.
     */
    public ChassisSpeeds calculate(
        Pose2d currentPose,
        Translation2d goal,
        double maxVelocity,
        double maxDeceleration,
        Obstacle... tempObstacles
    ) {
        this.goal = new Pose2d(goal, currentPose.getRotation());
        prediction.clear();

        // To future readers, apologies for the notation crimes.

        double e_x = goal.getX() - currentPose.getX();
        double e_y = goal.getY() - currentPose.getY();
        double error = Math.hypot(e_x, e_y);

        if (error <= tolerance) {
            setpoint = this.goal;
            return new ChassisSpeeds();
        }

        double rk_x = goal.getX();
        double rk_y = goal.getY();

        if (horizon > 0.0 && resolution <= error) {
            double net_x = currentPose.getX();
            double net_y = currentPose.getY();
            NetForce netForce = new NetForce();

            double d_max = 0.0;
            double seg_c = rk_x * net_y - rk_y * net_x;
            for (int i = 0; i < Math.ceil(horizon / resolution); i++) {
                netForce.reset();
                forceAt(net_x, net_y, goal, tempObstacles, netForce);
                double norm = Math.hypot(netForce.x, netForce.y);
                if (norm < 1e-6) break;

                double alpha = resolution / norm;
                net_x += netForce.x * alpha;
                net_y += netForce.y * alpha;

                if (savePrediction) {
                    prediction.add(new Translation2d(net_x, net_y));
                }

                double d = Math.abs(e_y * net_x - e_x * net_y + seg_c) / error;
                if (d > resolution && d >= d_max) {
                    rk_x = net_x;
                    rk_y = net_y;
                    d_max = d;
                }

                double ex_k_n = goal.getX() - net_x;
                double ey_k_n = goal.getY() - net_y;
                if (ex_k_n * ex_k_n + ey_k_n * ey_k_n <= resolution * resolution) {
                    break;
                }
            }
        }

        setpoint = new Pose2d(rk_x, rk_y, currentPose.getRotation());

        NetForce force = new NetForce();
        forceAt(currentPose.getX(), currentPose.getY(), setpoint.getTranslation(), tempObstacles, force);

        if (error < (maxVelocity * maxVelocity) / (2.0 * maxDeceleration)) {
            maxVelocity = Math.sqrt(2.0 * maxDeceleration * error);
        }

        double norm = Math.hypot(force.x, force.y);
        return norm > 1e-6
            ? new ChassisSpeeds(maxVelocity * (force.x / norm), maxVelocity * (force.y / norm), 0.0)
            : new ChassisSpeeds();
    }

    /**
     * Applies the the force generated by all obstacles as well as the
     * attraction potential of the goal to the provided accumulator,
     * based on the robot's current position and goal.
     * @param x The robot's current X position on the field.
     * @param y The robot's current Y position on the field.
     * @param goal The current target field location.
     * @param tempObstacles Additional temporary obstacles. Can be null.
     * @param netForce The force accumulator to apply the force to.
     */
    private void forceAt(double x, double y, Translation2d goal, Obstacle[] tempObstacles, NetForce netForce) {
        double e_x = goal.getX() - x;
        double e_y = goal.getY() - y;
        double error = Math.hypot(e_x, e_y);

        if (error < 1e-6) return;

        double goalForce = goalHeuristic.apply(error);
        netForce.x += goalForce * (e_x / error);
        netForce.y += goalForce * (e_y / error);

        for (Obstacle obstacle : obstacles) {
            obstacle.applyForce(x, y, netForce);
        }

        if (tempObstacles != null) {
            for (Obstacle obstacle : tempObstacles) {
                obstacle.applyForce(x, y, netForce);
            }
        }
    }

    /**
     * Utilizes the potential field to alter an arbitrary input chassis speed and minimize collisions
     * with obstacles. Typically, this method is used for manipulating driver input such that the robot
     * doesn't crash into field walls or game elements.
     * @param input Blue origin relative input speeds, typically from the driver.
     * @param currentPose The current blue origin relative pose of the robot.
     * @return The new blue origin relative chassis speeds.
     */
    public ChassisSpeeds repulsorDrive(ChassisSpeeds input, Pose2d currentPose) {
        NetForce sample = new NetForce();
        for (Obstacle obstacle : obstacles) {
            obstacle.applyForce(currentPose.getX(), currentPose.getY(), sample);
        }

        double repulsion_x =
            sample.x * input.vxMetersPerSecond < 0.0 ? sample.x * Math.abs(input.vxMetersPerSecond) : 0.0;
        double repulsion_y =
            sample.y * input.vyMetersPerSecond < 0.0 ? sample.y * Math.abs(input.vyMetersPerSecond) : 0.0;

        return new ChassisSpeeds(
            input.vxMetersPerSecond + repulsion_x,
            input.vyMetersPerSecond + repulsion_y,
            input.omegaRadiansPerSecond
        );
    }

    /**
     * Returns a list of samples from the potential field for visualization in
     * AdvantageScope. Represented as {@link Pose2d}s, the translation of the
     * pose is the sample location, and the rotation is the direction of force
     * at the sample. Useful for debugging purposes, but is not recommended for
     * regular use due to performance implications.
     * @param rows The number of rows (X+ direction) to sample. Columns are sampled
     *             relative to the ratio of {@code fieldWidth / fieldLength}.
     * @param padding The distance past the field perimeter to sample in meters.
     * @param fieldLength The length of the field in meters.
     * @param fieldWidth The width of the field in meters.
     */
    public List<Pose2d> visualizeField(int rows, double padding, double fieldLength, double fieldWidth) {
        fieldLength += (padding * 2.0);
        fieldWidth += (padding * 2.0);

        List<Pose2d> field = new ArrayList<>();
        NetForce sample = new NetForce();
        int columns = (int) Math.ceil(rows * (fieldWidth / fieldLength));
        for (double x = -padding; x < fieldLength + 1e-3; x += fieldLength / rows) {
            for (double y = -padding; y < fieldWidth + 1e-3; y += fieldWidth / columns) {
                sample.reset();
                forceAt(x, y, setpoint.getTranslation(), null, sample);
                if (sample.x * sample.x + sample.y * sample.y > 1e-6) {
                    field.add(new Pose2d(x, y, new Rotation2d(sample.x, sample.y)));
                }
            }
        }

        return field;
    }

    @Override
    public void initTunable(TunableTable table) {
        table.value("horizon", horizon, v -> horizon = v);
        table.value("resolution", resolution, v -> resolution = v);
        table.value("tolerance", tolerance, v -> tolerance = v);
        table.value("savePrediction", savePrediction, v -> savePrediction = v);
    }

    /**
     * Represents a sum of forces.
     */
    private static final class NetForce {

        // TODO (2027 MRC)
        // NetForce exists as an optimization to reduce GC pressure, over
        // the alternative of returning immutable container objects. With
        // the release of the 2027 MRC, this will not be necessary.

        public double x = 0.0;
        public double y = 0.0;

        public void reset() {
            x = 0.0;
            y = 0.0;
        }
    }

    /**
     * Represents a heuristic for calculating the strength of
     * the attractive force generated by the goal location.
     */
    @FunctionalInterface
    public static interface GoalHeuristic {
        /**
         * The default heuristic. Returns a static base strength of {@code 1.0} in
         * addition to an increasing strength of {@code 2.0 / distance} to have
         * greater influence on the robot as it approaches the goal location.
         */
        public static final GoalHeuristic DEFAULT = d -> 1.0 + (2.0 / d);

        /**
         * Applies the heuristic to a specified distance from the
         * goal location, returning an arbitrary strength value.
         * @param d The robot's current distance from the goal location,
         *          in meters. This value will always be positive and non-zero.
         * @return A strength value, relative to the strength specified for obstacles.
         */
        public double apply(double d);
    }

    /**
     * Represents a heuristic for transforming the force profile of an obstacle.
     */
    @FunctionalInterface
    public static interface ForceHeuristic {
        /**
         * The default heuristic. Squares the input value.
         */
        public static final ForceHeuristic DEFAULT = t -> t * t;

        /**
         * Applies the heuristic to a specified distance within the obstacle's range.
         * @param t A normalized value {@code [0.0, 1.0]} that represents the robot's
         *          distance to the obstacle as a percent of the obstacle's range.
         *          For repulsive obstacles, this value is {@code 1.0} when the robot's
         *          distance to the obstacle is zero. For attractive obstacles, this value
         *          is {@code 1.0} when the robot is at the edge of the obstacle's range.
         * @return The transformed output from the heuristic, within the range {@code [0.0, 1.0]}.
         */
        public double apply(double t);
    }

    /**
     * A generic obstacle.
     */
    public abstract static class Obstacle {

        private final ForceHeuristic heuristic;
        private final double strength;
        private final double range;
        private final boolean repulsive;

        /**
         * Creates an obstacle.
         * @param strength The strength of the obstacle's force. Positive values
         *                 represent a repulsive potential (pushes away), negative
         *                 values represent an attractive potential (pulls towards).
         * @param range The range of the obstacle's potential, in meters.
         */
        public Obstacle(double strength, double range) {
            this(strength, range, ForceHeuristic.DEFAULT);
        }

        /**
         * Creates an obstacle.
         * @param strength The strength of the obstacle's force. Positive values
         *                 represent a repulsive potential (pushes away), negative
         *                 values represent an attractive potential (pulls towards).
         * @param range The range of the obstacle's potential, in meters.
         * @param heuristic A heuristic for transforming the obstacle's force profile.
         */
        public Obstacle(double strength, double range, ForceHeuristic heuristic) {
            this.heuristic = heuristic;
            this.strength = strength;
            this.range = range;
            repulsive = strength > 0.0;
        }

        /**
         * Applies the force generated by the obstacle to the provided
         * accumulator, based on the robot's current position and goal.
         * @param x The robot's current X position on the field.
         * @param y The robot's current Y position on the field.
         * @param netForce The force accumulator to apply the force to.
         */
        public abstract void applyForce(double x, double y, NetForce netForce);

        /**
         * Converts a distance from the obstacle to the strength of the force.
         * @param dist The distance from the obstacle.
         */
        protected final double getForceMagnitude(double dist) {
            dist = Math.abs(dist);
            if (dist > range) return 0.0;

            double t = (repulsive ? (range - dist) : dist) / range;
            return strength * heuristic.apply(t);
        }
    }

    /**
     * A simple point obstacle. Applies a force from the specified field location.
     */
    public static final class PointObstacle extends Obstacle {

        private final Translation2d location;

        /**
         * Creates a point obstacle.
         * @param location The point's location on the field.
         * @param strength The strength of the obstacle's force. Positive values
         *                 represent a repulsive potential (pushes away), negative
         *                 values represent an attractive potential (pulls towards).
         * @param range The range of the obstacle's potential, in meters.
         */
        public PointObstacle(Translation2d location, double strength, double range) {
            this(location, strength, range, ForceHeuristic.DEFAULT);
        }

        /**
         * Creates a point obstacle.
         * @param location The point's location on the field.
         * @param strength The strength of the obstacle's force. Positive values
         *                 represent a repulsive potential (pushes away), negative
         *                 values represent an attractive potential (pulls towards).
         * @param range The range of the obstacle's potential, in meters.
         * @param heuristic A heuristic for transforming the obstacle's force profile.
         */
        public PointObstacle(Translation2d location, double strength, double range, ForceHeuristic heuristic) {
            super(strength, range, heuristic);
            this.location = location;
        }

        @Override
        public void applyForce(double x, double y, NetForce netForce) {
            double d_x = x - location.getX();
            double d_y = y - location.getY();
            double d_norm = Math.max(1e-6, Math.hypot(d_x, d_y));
            double force = getForceMagnitude(d_norm);

            netForce.x += force * (d_x / d_norm);
            netForce.y += force * (d_y / d_norm);
        }
    }

    /**
     * A circle with a specified radius.
     */
    public static final class CircleObstacle extends Obstacle {

        private final Translation2d location;
        private final double radius;

        /**
         * Creates a circle obstacle.
         * @param location The location of the circle's center.
         * @param radius The radius of the circle.
         * @param strength The strength of the obstacle's force. Positive values
         *                 represent a repulsive potential (pushes away), negative
         *                 values represent an attractive potential (pulls towards).
         * @param range The range of the obstacle's potential, in meters.
         */
        public CircleObstacle(Translation2d location, double radius, double strength, double range) {
            this(location, radius, strength, range, ForceHeuristic.DEFAULT);
        }

        /**
         * Creates a circle obstacle.
         * @param location The location of the circle's center.
         * @param radius The radius of the circle.
         * @param strength The strength of the obstacle's force. Positive values
         *                 represent a repulsive potential (pushes away), negative
         *                 values represent an attractive potential (pulls towards).
         * @param range The range of the obstacle's potential, in meters.
         * @param heuristic A heuristic for transforming the obstacle's force profile.
         */
        public CircleObstacle(
            Translation2d location,
            double radius,
            double strength,
            double range,
            ForceHeuristic heuristic
        ) {
            super(strength, range, heuristic);
            this.location = location;
            this.radius = radius;
        }

        @Override
        public void applyForce(double x, double y, NetForce netForce) {
            double d_x = x - location.getX();
            double d_y = y - location.getY();
            double d_norm = Math.max(1e-6, Math.hypot(d_x, d_y));
            double force = getForceMagnitude(Math.max(0.0, d_norm - radius));

            netForce.x += force * (d_x / d_norm);
            netForce.y += force * (d_y / d_norm);
        }
    }

    /**
     * An infinite line that pushes parallel to the X axis, to
     * constrain forwards/backwards field-relative movement.
     */
    public static final class LongitudinalObstacle extends Obstacle {

        private final double x;

        /**
         * Creates a longitudinal obstacle.
         * @param x The X coordinate of the line.
         * @param strength The strength of the obstacle's force. Positive values
         *                 represent a repulsive potential (pushes away), negative
         *                 values represent an attractive potential (pulls towards).
         * @param range The range of the obstacle's potential, in meters.
         */
        public LongitudinalObstacle(double x, double strength, double range) {
            this(x, strength, range, ForceHeuristic.DEFAULT);
        }

        /**
         * Creates a longitudinal obstacle.
         * @param x The X coordinate of the line.
         * @param strength The strength of the obstacle's force. Positive values
         *                 represent a repulsive potential (pushes away), negative
         *                 values represent an attractive potential (pulls towards).
         * @param range The range of the obstacle's potential, in meters.
         * @param heuristic A heuristic for transforming the obstacle's force profile.
         */
        public LongitudinalObstacle(double x, double strength, double range, ForceHeuristic heuristic) {
            super(strength, range);
            this.x = x;
        }

        @Override
        public void applyForce(double x, double y, NetForce netForce) {
            double difference = x - this.x;
            netForce.x += getForceMagnitude(difference) * Math.signum(difference);
        }
    }

    /**
     * An infinite line that pushes parallel to the Y axis, to
     * constrain left/right field-relative movement.
     */
    public static final class LateralObstacle extends Obstacle {

        private final double y;

        /**
         * Creates a lateral obstacle.
         * @param y The Y coordinate of the line.
         * @param strength The strength of the obstacle's force. Positive values
         *                 represent a repulsive potential (pushes away), negative
         *                 values represent an attractive potential (pulls towards).
         * @param range The range of the obstacle's potential, in meters.
         */
        public LateralObstacle(double y, double strength, double range) {
            this(y, strength, range, ForceHeuristic.DEFAULT);
        }

        /**
         * Creates a lateral obstacle.
         * @param y The Y coordinate of the line.
         * @param strength The strength of the obstacle's force. Positive values
         *                 represent a repulsive potential (pushes away), negative
         *                 values represent an attractive potential (pulls towards).
         * @param range The range of the obstacle's potential, in meters.
         * @param heuristic A heuristic for transforming the obstacle's force profile.
         */
        public LateralObstacle(double y, double strength, double range, ForceHeuristic heuristic) {
            super(strength, range);
            this.y = y;
        }

        @Override
        public void applyForce(double x, double y, NetForce netForce) {
            double difference = y - this.y;
            netForce.y += getForceMagnitude(difference) * Math.signum(difference);
        }
    }

    /**
     * A line segment that pushes perpendicular from its length.
     */
    public static final class LineObstacle extends Obstacle {

        private final Translation2d start;
        private final Translation2d end;
        private final boolean sidesOnly;

        private final double length;
        private final Rotation2d inverse;
        private final Rotation2d perpendicular;

        /**
         * Creates a line obstacle.
         * @param start The start of the line segment.
         * @param end The end of the line segment.
         * @param strength The strength of the obstacle's force. Positive values
         *                 represent a repulsive potential (pushes away), negative
         *                 values represent an attractive potential (pulls towards).
         * @param range The range of the obstacle's potential, in meters.
         */
        public LineObstacle(Translation2d start, Translation2d end, double strength, double range) {
            this(start, end, strength, range, ForceHeuristic.DEFAULT);
        }

        /**
         * Creates a line obstacle.
         * @param start The start of the line segment.
         * @param end The end of the line segment.
         * @param sidesOnly If the line obstacle should only apply force when
         *                  a projection perpendicular to the line from the
         *                  robot's position intersects the line.
         * @param strength The strength of the obstacle's force. Positive values
         *                 represent a repulsive potential (pushes away), negative
         *                 values represent an attractive potential (pulls towards).
         * @param range The range of the obstacle's potential, in meters.
         */
        public LineObstacle(Translation2d start, Translation2d end, boolean sidesOnly, double strength, double range) {
            this(start, end, sidesOnly, strength, range, ForceHeuristic.DEFAULT);
        }

        /**
         * Creates a line obstacle.
         * @param start The start of the line segment.
         * @param end The end of the line segment.
         * @param strength The strength of the obstacle's force. Positive values
         *                 represent a repulsive potential (pushes away), negative
         *                 values represent an attractive potential (pulls towards).
         * @param range The range of the obstacle's potential, in meters.
         * @param heuristic A heuristic for transforming the obstacle's force profile.
         */
        public LineObstacle(
            Translation2d start,
            Translation2d end,
            double strength,
            double range,
            ForceHeuristic heuristic
        ) {
            this(start, end, false, strength, range, heuristic);
        }

        /**
         * Creates a line obstacle.
         * @param start The start of the line segment.
         * @param end The end of the line segment.
         * @param sidesOnly If the line obstacle should only apply force when
         *                  a projection perpendicular to the line from the
         *                  robot's position intersects the line.
         * @param strength The strength of the obstacle's force. Positive values
         *                 represent a repulsive potential (pushes away), negative
         *                 values represent an attractive potential (pulls towards).
         * @param range The range of the obstacle's potential, in meters.
         * @param heuristic A heuristic for transforming the obstacle's force profile.
         */
        public LineObstacle(
            Translation2d start,
            Translation2d end,
            boolean sidesOnly,
            double strength,
            double range,
            ForceHeuristic heuristic
        ) {
            super(strength, range, heuristic);
            this.start = start;
            this.end = end;
            this.sidesOnly = sidesOnly;

            Translation2d difference = end.minus(start);
            length = difference.getNorm();
            inverse = difference.getAngle().unaryMinus();
            perpendicular = difference.getAngle().rotateBy(Rotation2d.kCCW_Pi_2);
        }

        @Override
        public void applyForce(double x, double y, NetForce netForce) {
            double startDist_x = x - start.getX();
            double startDist_y = y - start.getY();

            double proj_x = startDist_x * inverse.getCos() - startDist_y * inverse.getSin();
            double proj_y = startDist_x * inverse.getSin() + startDist_y * inverse.getCos();

            if (proj_x > 0.0 && proj_x < length) {
                double magnitude = getForceMagnitude(proj_y) * Math.signum(proj_y);
                netForce.x += magnitude * perpendicular.getCos();
                netForce.y += magnitude * perpendicular.getSin();
            } else if (!sidesOnly) {
                Translation2d closest = proj_x <= 0.0 ? start : end;
                double d_x = x - closest.getX();
                double d_y = y - closest.getY();
                double d_norm = Math.max(1e-6, Math.hypot(d_x, d_y));
                double magnitude = getForceMagnitude(d_norm);

                netForce.x += magnitude * (d_x / d_norm);
                netForce.y += magnitude * (d_y / d_norm);
            }
        }
    }
}