Examples with PointXYZ me.wobblyyyy.pathfinder2.geometry.PointXYZ used on opensource projects

Example 96 with PointXYZ

use of me.wobblyyyy.pathfinder2.geometry.PointXYZ in project Pathfinder2 by Wobblyyyy.

the class Pathfinder method splineTo.

/**
 * Create a spline trajectory to a certain target point, and then follow
 * that aforementioned trajectory.
 *
 * <p>
 * If this method is called on a set of points with non-monotonic Y
 * values, this will instead invoke
 * {@link #multiSplineTo(double, double, Angle, PointXYZ...)}, which
 * supports non-monotonic Y values.
 * </p>
 *
 * @param speed          the speed at which the robot should move. This
 *                       is a constant value.
 * @param tolerance      the tolerance used for determining whether the
 *                       robot is at the target point.
 * @param angleTolerance same thing as {@code tolerance}, but for the
 *                       robot's angle.
 * @param points         a set of control points for the spline. This
 *                       will automatically insert the robot's current
 *                       position into this array. This array must have
 *                       AT LEAST two points.
 * @return {@code this}, used for method chaining.
 */
public Pathfinder splineTo(double speed, double tolerance, Angle angleTolerance, PointXYZ... points) {
    if (points.length < 2)
        throw new IllegalArgumentException("At least two control points are required to use the " + "splineTo method.");
    InvalidSpeedException.throwIfInvalid("Invalid speed value provided! Speed must be between 0 and 1.", speed);
    InvalidToleranceException.throwIfInvalid("Invalid tolerance! Tolerance must be a positive number.", tolerance);
    if (angleTolerance.deg() < 0)
        throw new InvalidToleranceException("Invalid angle tolerance! " + "Angle tolerance must be greater than 0 degrees.");
    NotNull.throwExceptionIfNull("One or more points provided to splineTo was null!", (Object[]) points);
    // non-monotonic Y values means we need to use a multi spline instead
    if (!Spline.areMonotonicY(points))
        return multiSplineTo(speed, tolerance, angleTolerance, points);
    // length is the total distance of the spline (NOT all control points)
    double length = PointXY.distance(points[0], points[points.length - 1]);
    // step should be relatively small - by default, it's 1/20th of
    // the spline's length.
    double step = length / Core.pathfinderStepDivisor;
    AdvancedSplineTrajectoryBuilder builder = new AdvancedSplineTrajectoryBuilder().setSpeed(speed).setTolerance(tolerance).setAngleTolerance(angleTolerance).setStep(step);
    PointXYZ robotPosition = getPosition();
    // to the trajectory
    if (!robotPosition.equals(points[0]))
        builder.add(robotPosition);
    PointXYZ lastPoint = points[0];
    for (int i = 1; i < points.length; i++) {
        PointXYZ point = points[i];
        if (point == null)
            throw new NullPointException("Cannot use the splineTo method with a null " + "control point!");
        if (!point.equals(lastPoint)) {
            builder.add(point);
            lastPoint = point;
        } else {
            throw new SplineException("There were duplicate adjacent points in the set " + "of control points! This means there's the same " + "point, twice in a row. The points were: " + Arrays.toString(points));
        }
    }
    Trajectory trajectory = builder.build();
    return followTrajectory(trajectory);
}

Example 97 with PointXYZ

use of me.wobblyyyy.pathfinder2.geometry.PointXYZ in project Pathfinder2 by Wobblyyyy.

the class CircleSurrounder method fastSurround.

/**
 * Surround the circle. This will find the closest point, create a new
 * {@link me.wobblyyyy.pathfinder2.trajectory.FastTrajectory} to that
 * point, and then instruct Pathfinder to follow it.
 */
public void fastSurround() {
    PointXYZ robotPosition = pathfinder.getPosition();
    double speed = pathfinder.getSpeed();
    double tolerance = pathfinder.getTolerance();
    Angle angleTolerance = pathfinder.getAngleTolerance();
    Trajectory trajectory = CircleSurround.fastTrajectoryToClosestPoint(robotPosition, center, radius, speed);
    pathfinder.followTrajectory(trajectory);
}

Example 98 with PointXYZ

use of me.wobblyyyy.pathfinder2.geometry.PointXYZ in project Pathfinder2 by Wobblyyyy.

the class CircleSurrounder method surround.

/**
 * Surround the circle. This will find the closest point, create a new
 * {@link me.wobblyyyy.pathfinder2.trajectory.LinearTrajectory} to that
 * point, and then instruct Pathfinder to follow it.
 */
public void surround() {
    PointXYZ robotPosition = pathfinder.getPosition();
    double speed = pathfinder.getSpeed();
    double tolerance = pathfinder.getTolerance();
    Angle angleTolerance = pathfinder.getAngleTolerance();
    Trajectory trajectory = CircleSurround.trajectoryToClosestPoint(robotPosition, center, radius, speed, tolerance, angleTolerance);
    pathfinder.followTrajectory(trajectory);
}

Example 99 with PointXYZ

use of me.wobblyyyy.pathfinder2.geometry.PointXYZ in project Pathfinder2 by Wobblyyyy.

the class PathBuilder method fastPath.

public List<Trajectory> fastPath(PointXYZ start, double speed) {
    List<Trajectory> trajectories = new ArrayList<>(targets.size());
    PointXYZ previousPoint = start;
    for (PointXYZ target : targets) {
        trajectories.add(new FastTrajectory(previousPoint, target, speed));
        previousPoint = target;
    }
    return trajectories;
}

Example 100 with PointXYZ

use of me.wobblyyyy.pathfinder2.geometry.PointXYZ in project Pathfinder2 by Wobblyyyy.

the class CircleSurround method closestPoint.

/**
 * Get the point along a circle closest to the robot.
 *
 * @param robotPosition the robot's current position.
 * @param center        the circle's center point.
 * @param radius        the radius of the circle.
 * @return the point along a circle closest to the robot. This point's
 * heading will be facing towards the center of the circle.
 */
public static PointXYZ closestPoint(PointXYZ robotPosition, PointXY center, double radius) {
    if (robotPosition == null)
        throw new NullPointException("Robot position may not be null!");
    if (center == null)
        throw new NullPointException("Center point may not be null!");
    if (radius < 0)
        throw new IllegalArgumentException("Radius values must be greater than 0!");
    Angle centerToRobot = center.angleTo(robotPosition);
    Angle robotToCenter = robotPosition.angleTo(center);
    return center.inDirection(radius, centerToRobot).withHeading(robotToCenter);
}