[Home]Skilgannon/CodeSnippets

Robo Home | Skilgannon | Changes | Preferences | AllPages

Difference (from prior major revision) (no other diffs)

Changed: 18c18,22
if(velocity >= 0 && distanceRemaining >= decelDistance(velocity))

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))

Removed: 20d23


Removed: 24,26d26
double maxTurn = Math.PI/180*(10 - 0.75*velocity);
theta = limit(0,theta - maxTurn, Math.PI/2);


Changed: 29c29,30
else //rule of cosines
else{ //rule of cosines
double oldDistRem? = distanceRemaining;

Added: 30a32,39

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

Changed: 34c43
}while(time < timeAvailable && distanceRemaining >= 1);
}while(time < timeAvailable && distanceRemaining > 0.000001);

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 18:56 EST by Skilgannon (diff)
Search: