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

Example 26 with PointXY

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

the class TestHeadingLock method testHeadingLockAlongLine.

@Test
public void testHeadingLockAlongLine() {
    Pathfinder pathfinder = Pathfinder.newSimulatedPathfinder(0.01);
    SimulatedRobot odometry = (SimulatedRobot) pathfinder.getOdometry();
    pathfinder.loadBundledPlugins();
    pathfinder.lockHeading(new PointXY(10, 10));
    pathfinder.followTrajectory(new LinearTrajectory(new PointXYZ(0, 10, 0), 0.5, 2, Angle.fromDeg(5)));
    odometry.setPosition(new PointXYZ(0, 0, 0));
    pathfinder.tick();
    Assertions.assertEquals(-0.45, pathfinder.getTranslation().vz());
    odometry.setPosition(new PointXYZ(0, 0, 45));
    pathfinder.tick();
    Assertions.assertEquals(0, pathfinder.getTranslation().vz());
    odometry.setPosition(new PointXYZ(0, 0, 90));
    pathfinder.tick();
    Assertions.assertEquals(0.45, pathfinder.getTranslation().vz());
    odometry.setPosition(0, 5, 0);
    pathfinder.tick();
    Assertions.assertTrue(Equals.soft(-0.265, pathfinder.getTranslation().vz(), 0.01));
    odometry.setPosition(5, 5, 0);
    pathfinder.tick();
    Assertions.assertTrue(Equals.soft(-0.45, pathfinder.getTranslation().vz(), 0.01));
}

Example 27 with PointXY

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

the class Trajectory method shift.

/**
 * Shift a trajectory.
 *
 * @param origin the trajectory's base.
 * @param target the trajectory's target.
 * @return a shifted trajectory.
 */
default Trajectory shift(PointXY origin, PointXY target) {
    Function<PointXYZ, PointXYZ> nextMarkerFunction = InternalTrajectoryUtils.nextMarkerFunction(this);
    Function<PointXYZ, Boolean> isDoneFunction = InternalTrajectoryUtils.isDoneFunction(this);
    Function<PointXYZ, Double> speedFunction = InternalTrajectoryUtils.speedFunction(this);
    Supplier<String> _toString = this::toString;
    PointXYZ difference = origin.subtract(target).withHeading(Angle.fromDeg(0));
    return getTrajectory(difference, nextMarkerFunction, isDoneFunction, speedFunction, _toString);
}

Example 28 with PointXY

use of me.wobblyyyy.pathfinder2.geometry.PointXY 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);
}

Example 29 with PointXY

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

the class CircleSurround method closestPointBetweenAngles.

/**
 * Get the closest point along a circle between two given angles.
 * I don't really know how to explain this, so if you're confused,
 * just look at the code and try and figure it out.
 *
 * @param robotPosition the robot's current position.
 * @param center        the circle's center point.
 * @param radius        the radius of the circle.
 * @param minimumAngle  the minimum angle.
 * @param maximumAngle  the maximum angle.
 * @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 closestPointBetweenAngles(PointXYZ robotPosition, PointXY center, double radius, Angle minimumAngle, Angle maximumAngle) {
    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).angleWithMinAndMax(minimumAngle, maximumAngle);
    Angle robotToCenter = robotPosition.angleTo(center);
    return center.inDirection(radius, centerToRobot).withHeading(robotToCenter);
}

Example 30 with PointXY

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

the class AdvancedSplineTrajectory method nextMarker.

@Override
public PointXYZ nextMarker(PointXYZ current) {
    double x = clipX(current.x() + step);
    PointXY interpolatedPoint = spline.interpolate(x);
    Angle interpolatedAngle = angleSpline.getAngleTarget(x);
    return interpolatedPoint.withHeading(interpolatedAngle);
}