# 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 velocity = initialVelocity;
double distanceRemaining = fromLocation.distance(toLocation);;
long time = 0;
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