TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/RingPickupPathGenerator.java
/**
     * Enumerate all chosen ring possible pick up order and get quickest one
     * @return Array of RoadRunner Trajectory
     */
Enumerate all chosen ring possible pick up order and get quickest one
@return Array of RoadRunner Trajectory
Enumerate all chosen ring possible pick up order and get quickest one @return Array of RoadRunner Trajectory
public ArrayList<Trajectory> generatePath(ArrayList<Vector2d> rings) {
        Vector2D newStart = move(startPose, ROBOT_WIDTH);
        Vector2D newEnd = move(endPose, -ROBOT_WIDTH);
        Logger.logFile("GeneratePath rings - " + rings.size());
        if (rings.size()==2) {
            rings.add(new Vector2d(endPose.getX()-8, endPose.getY()));
        }
        else if (rings.size()==1) {
            return pickUpAndShoot(rings.get(0));
        }
        if (rings.size()==0) {
            rings.add(new Vector2d(endPose.getX()+5, endPose.getY()+5));
            rings.add(new Vector2d(endPose.getX()-5, endPose.getY()));
            rings.add(new Vector2d(endPose.getX()-8, endPose.getY()));
        }
        ArrayList<Vector2d> chosen = selectRings(rings);
        for(Vector2d v : chosen) {
            Logger.logFile("Ring:" + v.getX() + ", " + v.getY());
        }
        ArrayList<Trajectory> bestMove = new ArrayList<Trajectory>();
        int moveExamined = 0;
        long startTime = System.currentTimeMillis();
        double bestCost = 10000;
        int bestA = 1, bestB = 2, bestC = 3;
        for (int a = 0; a < chosen.size(); a++) {
            for (int b = 0; b < chosen.size(); b++) {
                if (a != b) {
                    for (int c = 0; c < chosen.size(); c++) {
                        if (a != b && a != c && b != c) {
                            double currCost = newStart.distance(toVector2D(chosen.get(a))) +
                                    chosen.get(a).distTo(chosen.get(b)) +
                                    chosen.get(b).distTo(chosen.get(c)) +
                                    newEnd.distance(toVector2D(chosen.get(c)));
                            if (isNearWall(chosen.get(a))>0) {
                                currCost += NEAR_WALL_EXTRA;
                            }
                            if (isNearWall(chosen.get(b))>0) {
                                currCost += NEAR_WALL_EXTRA;
                            }
                            if (isNearWall(chosen.get(c))>0) {
                                currCost += NEAR_WALL_EXTRA;
                            }
                            moveExamined++;
                            if (bestCost > currCost) {
                                bestCost = currCost;
                                bestA = a; bestB = b; bestC = c;
                            }
                        }
                    }
                }
            }
        }
        long timeSpent = System.currentTimeMillis() - startTime;
        Logger.logFile("Moves compared: " + moveExamined + " time: " + timeSpent + " avg: " + 1.0*timeSpent/moveExamined);
        Logger.logFile("rings.add(new Vector2d" + chosen.get(bestA) + ");");    
        Logger.logFile("rings.add(new Vector2d" + chosen.get(bestB) + ");");
        Logger.logFile("rings.add(new Vector2d" + chosen.get(bestC) + ");");
        bestMove = pickUpAndShoot(chosen.get(bestA), chosen.get(bestB), chosen.get(bestC));
        return bestMove;
    }
public ArrayList < Trajectory > generatePath ( ArrayList < Vector2d > rings ) { Vector2D newStart = move ( startPose , ROBOT_WIDTH ) ; Vector2D newEnd = move ( endPose , - ROBOT_WIDTH ) ; Logger . logFile ( "GeneratePath rings - " + rings . size ( ) ) ; if ( rings . size ( ) == 2 ) { rings . add ( new Vector2d ( endPose . getX ( ) - 8 , endPose . getY ( ) ) ) ; } else if ( rings . size ( ) == 1 ) { return pickUpAndShoot ( rings . get ( 0 ) ) ; } if ( rings . size ( ) == 0 ) { rings . add ( new Vector2d ( endPose . getX ( ) + 5 , endPose . getY ( ) + 5 ) ) ; rings . add ( new Vector2d ( endPose . getX ( ) - 5 , endPose . getY ( ) ) ) ; rings . add ( new Vector2d ( endPose . getX ( ) - 8 , endPose . getY ( ) ) ) ; } ArrayList < Vector2d > chosen = selectRings ( rings ) ; for ( Vector2d v : chosen ) { Logger . logFile ( "Ring:" + v . getX ( ) + ", " + v . getY ( ) ) ; } ArrayList < Trajectory > bestMove = new ArrayList < Trajectory > ( ) ; int moveExamined = 0 ; long startTime = System . currentTimeMillis ( ) ; double bestCost = 10000 ; int bestA = 1 , bestB = 2 , bestC = 3 ; for ( int a = 0 ; a < chosen . size ( ) ; a ++ ) { for ( int b = 0 ; b < chosen . size ( ) ; b ++ ) { if ( a != b ) { for ( int c = 0 ; c < chosen . size ( ) ; c ++ ) { if ( a != b && a != c && b != c ) { double currCost = newStart . distance ( toVector2D ( chosen . get ( a ) ) ) + chosen . get ( a ) . distTo ( chosen . get ( b ) ) + chosen . get ( b ) . distTo ( chosen . get ( c ) ) + newEnd . distance ( toVector2D ( chosen . get ( c ) ) ) ; if ( isNearWall ( chosen . get ( a ) ) > 0 ) { currCost += NEAR_WALL_EXTRA ; } if ( isNearWall ( chosen . get ( b ) ) > 0 ) { currCost += NEAR_WALL_EXTRA ; } if ( isNearWall ( chosen . get ( c ) ) > 0 ) { currCost += NEAR_WALL_EXTRA ; } moveExamined ++ ; if ( bestCost > currCost ) { bestCost = currCost ; bestA = a ; bestB = b ; bestC = c ; } } } } } } long timeSpent = System . currentTimeMillis ( ) - startTime ; Logger . logFile ( "Moves compared: " + moveExamined + " time: " + timeSpent + " avg: " + 1.0 * timeSpent / moveExamined ) ; Logger . logFile ( "rings.add(new Vector2d" + chosen . get ( bestA ) + ");" ) ; Logger . logFile ( "rings.add(new Vector2d" + chosen . get ( bestB ) + ");" ) ; Logger . logFile ( "rings.add(new Vector2d" + chosen . get ( bestC ) + ");" ) ; bestMove = pickUpAndShoot ( chosen . get ( bestA ) , chosen . get ( bestB ) , chosen . get ( bestC ) ) ; return bestMove ; }
Enumerate all chosen ring possible pick up order and get quickest one
 @return Array of RoadRunner Trajectory
Enumerate all chosen ring possible pick up order and get quickest one @return Array of RoadRunner Trajectory
// if number of rings seen is not 3 // there's only one right, special processing // just have it go around and do nothing if see nothing to waste some time // build a path for different pick up orders // to be used in simulator if needed