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

Example 31 with PointXYZ

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

the class TestAdvancedSplineTrajectory method testInvalidAkimaSpline.

@Test
public void testInvalidAkimaSpline() {
    mode = InterpolationMode.AKIMA;
    Assertions.assertThrows(SplineException.class, () -> {
        testSplineTo(new PointXYZ(0, 0));
    });
    mode = InterpolationMode.DEFAULT;
}

Example 32 with PointXYZ

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

the class TestAdvancedSplineTrajectory method testSplineTo.

private void testSplineTo(PointXYZ... points) {
    if (points.length < 1)
        throw new IllegalArgumentException();
    AdvancedSplineTrajectoryBuilder builder = new AdvancedSplineTrajectoryBuilder().setStep(0.1).setTolerance(tolerance).setSpeed(speed).setAngleTolerance(angleTolerance);
    builder.setInterpolationMode(mode);
    for (PointXYZ point : points) builder.add(point);
    builder.setInterpolationMode(InterpolationMode.DEFAULT);
    Trajectory trajectory = builder.build();
    PointXYZ target = points[points.length - 1];
    testTrajectory(trajectory, target);
}

Example 33 with PointXYZ

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

the class TestAdvancedSplineTrajectory method testAkimaSplineInterpolation.

@Test
public void testAkimaSplineInterpolation() {
    mode = InterpolationMode.AKIMA;
    testSplineTo(new PointXYZ(0, 0), new PointXYZ(5, 5), new PointXYZ(10, 15), new PointXYZ(15, 30));
    testSplineTo(new PointXYZ(0, 0, 45), new PointXYZ(5, 5, 65), new PointXYZ(10, 15, 90), new PointXYZ(15, 30, 180));
    mode = InterpolationMode.DEFAULT;
}

Example 34 with PointXYZ

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

the class TestableRobot method testTrajectory.

public void testTrajectory(Trajectory trajectory, PointXYZ target, double maxTimeMs) {
    if (!hasBeforeEachBeenCalled)
        throw new RuntimeException("Did not call beforeEach method! Try super.beforeEach()?");
    ValidationUtils.validate(trajectory, "trajectory");
    ValidationUtils.validate(target, "target");
    pathfinder.followTrajectory(trajectory);
    ElapsedTimer timer = new ElapsedTimer(true);
    pathfinder.tickUntil(maxTimeMs);
    if (timer.elapsedMs() >= maxTimeMs)
        throw new RuntimeException(StringUtils.format("Trajectory <%s> to target <%s> took <%s> milliseconds " + "to execute indicating there was an issue " + "following the trajectory.", trajectory, target, timer.elapsedMs()));
    AssertionUtils.assertIsNear(target, pathfinder.getPosition(), tolerance, angleTolerance);
}

Example 35 with PointXYZ

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

the class Average method of.

public static PointXYZ of(PointXYZ... points) {
    double x = 0;
    double y = 0;
    Angle z = Angle.fromDeg(0);
    for (PointXYZ point : points) {
        x += point.x();
        y += point.y();
        z = z.add(point.z());
    }
    x /= points.length;
    y /= points.length;
    z = Angle.fromDeg(z.deg() / points.length);
    return new PointXYZ(x, y, z);
}