Skilgannon/CodeSnippets

       public static boolean reachable(Point2D.Double fromLocation, Point2D.Double toLocation, long timeAvailable, double initialVelocity, double previousHeading){
      
         double wantedHeading = absoluteBearing(fromLocation,toLocation);
         double velocity = Math.abs(initialVelocity);
         double distanceRemaining = fromLocation.distance(toLocation);;
         long time = 0;
         double theta = Math.abs(Utils.normalRelativeAngle(wantedHeading - previousHeading));
         if(theta > Math.PI/2){
            theta = Math.PI - theta;
         }
         
         do{
            if(distanceRemaining >= decelDistance(velocity))
               velocity = limit(0,velocity + 1, 8);
            
            else
               velocity = limit(0, velocity - 2,8);
               
            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);
      
         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;
         
         
         }
      }