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

Example 1 with PointXY

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

the class TestLocalizedPathGen method testObstructedNegativePathfinding.

@Test
public void testObstructedNegativePathfinding() {
    List<Zone> zones = new ArrayList<Zone>() {

        {
            add(new Zone(new Rectangle(-3, -2, 8, 3)));
        }
    };
    LocalizedPathGen gen = new LocalizedPathGen(zones, 0.5, 0.5);
    PointXY start = new PointXY(-5, -5);
    PointXY end = new PointXY(5, 5);
    List<PointXY> path = gen.getPath(start, end);
    Assertions.assertNotNull(path);
}

Example 2 with PointXY

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

the class TestStatTracker method testTicksPerSecond.

public void testTicksPerSecond() {
    Pathfinder pathfinder = Pathfinder.newSimulatedPathfinder(0.01).setSpeed(0.75).setTolerance(2).setAngleTolerance(Angle.fixedDeg(10));
    pathfinder.loadPlugin(new StatTracker());
    pathfinder.goTo(new PointXYZ(10, 10, 0));
    pathfinder.splineTo(new PointXYZ(10, 10, 90), new PointXYZ(12, 11, 45), new PointXYZ(13, 15, 180));
    ElapsedTimer timer = new ElapsedTimer(true);
    SimulatedOdometry odometry = (SimulatedOdometry) pathfinder.getOdometry();
    AtomicInteger i = new AtomicInteger(0);
    try {
        while (timer.elapsedSeconds() < 5) {
            if (Math.random() > 0.5)
                Thread.sleep(2);
            pathfinder.tick();
            Translation translation = pathfinder.getTranslation();
            odometry.setRawPosition(odometry.getRawPosition().add(new PointXYZ(translation.vx() / 50, translation.vy() / 50, Angle.fixedRad(translation.vz() / 50))));
        }
    } catch (Exception ignored) {
    }
    pathfinder.tick();
    System.out.println("tps: " + pathfinder.ticksPerSecond());
    System.out.println("ticks: " + pathfinder.getData("pf_ticks"));
    System.out.println("position: " + pathfinder.getPosition());
    System.out.println("condition met: " + i.get());
    System.out.println("total PointXY: " + PointXY.COUNT);
    System.out.println("total PointXYZ: " + PointXYZ.COUNT);
    System.out.println("total Angle: " + Angle.COUNT);
}

Example 3 with PointXY

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

the class AngleSplineTrajectory method nextMarker.

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

Example 4 with PointXY

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

the class Velocity method nearestVelocities.

public List<Velocity> nearestVelocities(List<Velocity> velocities, Velocity velocity, int howMany) {
    if (howMany < velocities.size())
        throw new IllegalArgumentException("Cannot return more velocities than were provided!");
    PointXY velocityPoint = velocity.asPointXY();
    velocities.sort((v1, v2) -> {
        PointXY point1 = v1.asPointXY();
        PointXY point2 = v2.asPointXY();
        double distance1 = PointXY.distance(velocityPoint, point1);
        double distance2 = PointXY.distance(velocityPoint, point2);
        return Double.compare(distance1, distance2);
    });
    List<Velocity> nearestVelocities = new ArrayList<>(howMany);
    for (int i = 0; i < howMany; i++) {
        nearestVelocities.set(i, velocities.get(0));
    }
    return nearestVelocities;
}

Example 5 with PointXY

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

the class AutoRotator method apply.

@Override
public Translation apply(Translation translation) {
    if (!isActive)
        return translation;
    if (centerPoint == null)
        throw new NullPointerException("Attempted to use an AutoRotator without having previously " + "set a center point, make sure to use the" + "setCenterPoint(PointXY) method to do that.");
    PointXYZ position = pathfinder.getPosition();
    double delta = Angle.minimumDelta(position.z(), position.angleTo(centerPoint).add(angleOffset));
    return translation.withVz(turnController.calculate(delta));
}