[Home]Skilgannon/CodeSnippets

Robo Home | Skilgannon | Changes | Preferences | AllPages

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;
         
         
         }
      } 

Robo Home | Skilgannon | Changes | Preferences | AllPages
Edit text of this page | View other revisions
Last edited October 30, 2007 17:56 EST by Skilgannon (diff)
Search: