Examples with InterpolationMode me.wobblyyyy.pathfinder2.trajectory.spline.InterpolationMode used on opensource projects

Example 1 with InterpolationMode

use of me.wobblyyyy.pathfinder2.trajectory.spline.InterpolationMode in project Pathfinder2 by Wobblyyyy.

the class AdvancedSplineTrajectoryBuilder method build.

public AdvancedSplineTrajectory build() {
    boolean invalidStep = step == Double.MAX_VALUE;
    boolean invalidSpeed = speed == Double.MAX_VALUE;
    boolean invalidTolerance = tolerance == Double.MAX_VALUE;
    boolean invalidAngleTolerance = angleTolerance == null;
    DEFAULT_LOGGER.accept("--- BUILDING SPLINE ---");
    DEFAULT_LOGGER.accept(StringUtils.format("invalid step: %s\n" + "invalid speed: %s\n" + "invalid tolerance: %s\n" + "invalid angle tolerance: %s", invalidStep, invalidSpeed, invalidTolerance, invalidAngleTolerance));
    if (invalidStep && invalidSpeed && invalidTolerance && invalidAngleTolerance)
        throw new IllegalArgumentException("Did not set a step, speed, tolerance, and angle tolerance " + "value! You need to use setStep(), setSpeed(), " + "setTolerance(), and setAngleTolerance() before " + "calling the build() method.");
    if (invalidStep)
        throw new IllegalArgumentException("Did not set a step value - use setStep().");
    if (invalidSpeed)
        throw new InvalidSpeedException("Did not set a speed - use setSpeed().");
    if (invalidTolerance)
        throw new InvalidToleranceException("Did not set a tolerance - use setTolerance().");
    if (invalidAngleTolerance)
        throw new NullAngleException("Null angle tolerance while creating an " + "AdvancedSplineTrajectory.");
    int size = xValues.size();
    Double[] xBoxed = new Double[size];
    Double[] yBoxed = new Double[size];
    Double[] speedBoxed = new Double[size];
    Angle[] z = new Angle[size];
    xValues.toArray(xBoxed);
    yValues.toArray(yBoxed);
    angleTargets.toArray(z);
    speeds.toArray(speedBoxed);
    double[] x = new double[size];
    double[] y = new double[size];
    double[] speed = new double[size];
    boolean sameSpeedValue = true;
    int xDuplicates = 0;
    int yDuplicates = 0;
    for (int i = 0; i < xBoxed.length; i++) {
        x[i] = xBoxed[i];
        y[i] = yBoxed[i];
        speed[i] = speedBoxed[i];
        if (i != 0) {
            if (speed[i] != speed[i - 1])
                sameSpeedValue = false;
            // ensure adjacent X values are unique
            if (x[i] == x[i - 1])
                x[i] += Core.advancedSplineTrajectoryDuplicateOffset * ((xDuplicates++) + 1);
            // ensure adjacent Y values are unique
            if (y[i] == y[i - 1])
                y[i] += Core.advancedSplineTrajectoryDuplicateOffset * ((yDuplicates++) + 1);
        }
    }
    DEFAULT_LOGGER.accept(StringUtils.format("boxed x: %s\n" + "boxed y: %s\n" + "boxed z: %s\n" + "boxed speed: %s\n" + "unboxed x: %s\n" + "unboxed y: %s\n" + "unboxed speed: %s", Arrays.toString(xBoxed), Arrays.toString(yBoxed), Arrays.toString(z), Arrays.toString(speedBoxed), Arrays.toString(x), Arrays.toString(y), Arrays.toString(speed)));
    // add support for different types of spline interpolation!
    // this is a really bad way to implement support for multiple
    // types of spline interpolation, but... oh well.
    Spline spline;
    switch(interpolationMode) {
        case DEFAULT:
            spline = new MonotoneCubicSpline(x, y);
            break;
        case CUBIC:
            spline = new ApacheSpline(Interpolator.CUBIC, x, y);
            break;
        case AKIMA:
            spline = new ApacheSpline(Interpolator.AKIMA, x, y);
            break;
        case CUSTOM:
            if (customSplineGenerator != null)
                spline = customSplineGenerator.apply(xBoxed, yBoxed);
            else
                throw new NullPointerException("Tried to use custom " + "spline generator without having set it first: " + "use setCustomSplineGenerator() to do so. The " + "function you pass in should accept two arrays " + "of Double values (x and y).");
            break;
        default:
            throw new RuntimeException("How did you even get here?");
    }
    AngleSpline angleSpline = new AngleSpline(x, z);
    /*
        Spline speedSpline;
        if (sameSpeedValue) speedSpline =
            new LinearSpline(
                new SlopeIntercept(0, speed[0])
            ); else speedSpline = new MonotoneCubicSpline(x, speed);
        */
    Spline speedSpline = new MonotoneCubicSpline(x, speed);
    Logger.debug(AdvancedSplineTrajectoryBuilder.class, "spline: <%s> angleSpline: <%s> speedSpline: <%s> step: <%s> " + "tolerance: <%s> angleTolerance: <%s>", spline, angleSpline, speedSpline, step, tolerance, angleTolerance);
    return new AdvancedSplineTrajectory(spline, angleSpline, speedSpline, step, tolerance, angleTolerance);
}