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{
         
            double maxTurn = Math.PI/180*(10 - 0.75*Math.abs(velocity));
            theta = limit(0,theta - maxTurn, Math.PI/2);
         
            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);
               
            if(theta == 0)
               distanceRemaining -= velocity;
            else{ //rule of cosines
               double oldDistRem = distanceRemaining;
               distanceRemaining = Math.sqrt(velocity*velocity + distanceRemaining*distanceRemaining - 2*velocity*distanceRemaining*Math.cos(theta));
             
               double acosVal = (velocity*velocity + distanceRemaining*distanceRemaining - oldDistRem*oldDistRem)/(2*velocity*distanceRemaining);
               if(Math.abs(acosVal) <= 1){
                  theta =  Math.abs(Math.acos(acosVal));
                  if(theta > Math.PI/2)
                     theta = Math.PI - theta;  
               }
            }
            
            time++;
            	
         }while(time < timeAvailable && distanceRemaining > 0.000001);
      
      
         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;
         
         
         }
      }