Skilgannon/CodeSnippets

NB! Initial velocity is signed, and initialHeading is the result of getHeadingRadians()! This code takes care of BackAsFront!

      public static boolean reachable(Point2D.Double fromLocation, Point2D.Double toLocation, long timeAvailable, double initialVelocity, double initialHeading){
      
         double wantedHeading = absoluteBearing(fromLocation,toLocation);
         double velocity = initialVelocity;
         double distanceRemaining = fromLocation.distance(toLocation);;
         long time = 0;
         double theta = Math.abs(Utils.normalRelativeAngle(wantedHeading - initialHeading));
         if(theta > Math.PI/2){
            theta = Math.PI - theta;
            velocity = -velocity;
         }
         
         do{
            if(velocity >= 0 && distanceRemaining >= decelDistance(velocity))
               velocity = limit(0,velocity + 1, 8);
            
            else
               velocity = limit(-1, Math.abs(velocity) - Math.min(distanceRemaining,2), 6)*(velocity<0?-1:1);
               
            double maxTurn = Math.PI/180*(10 - 0.75*velocity);
            theta = limit(0,theta - maxTurn, Math.PI/2);
         		
            if(theta == 0)
               distanceRemaining -= velocity;
            else //rule of cosines
               distanceRemaining = Math.sqrt(velocity*velocity + distanceRemaining*distanceRemaining - 2*velocity*distanceRemaining*Math.cos(theta));
            
            time++;
            	
         }while(time < timeAvailable && distanceRemaining >= 1);
      
      
         return  distanceRemaining < 1;
      }

       static double decelDistance(double vel){
         int intVel = (int)Math.round(vel);
         switch(intVel){  
            case 8:
               return 6 + 4 + 2;
            case 7:
               return 5 + 3 + 1;
            case 6:
               return 4 + 2;
            case 5:
               return 3 + 1;
            case 4:
               return 2;
            case 3:
               return 1;
            case 2:
            case 1:
            case 0:
               return 0;
               
            default:
               return 6 + 4 + 2;
         
         
         }
      }