Editing Skilgannon/CodeSnippets

Robo Home | Changes | Preferences | AllPages

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

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

Summary:
Consciousness check: 5 * 3 =
This change is a minor edit. Send email notification that Skilgannon/CodeSnippets has been changed.
(Visit Preferences to set your user name.)

View other revisions
Robo Home | Skilgannon | Changes | Preferences | AllPages