Skilgannon/CodeSnippets

public static boolean reachable(Point2D.Double fromLocation, Point2D.Double toLocation, long timeAvailable, double initialVelocity, double previousHeading){ double wantedHeading = absoluteBearing(fromLocation,toLocation); double velocity = Math.abs(initialVelocity); double distanceRemaining = fromLocation.distance(toLocation);; long time = 0; double theta = Math.abs(Utils.normalRelativeAngle(wantedHeading - previousHeading)); if(theta > Math.PI/2){ theta = Math.PI - theta; } do{ if(distanceRemaining >= decelDistance(velocity)) velocity = limit(0,velocity + 1, 8); else velocity = limit(0, velocity - 2,8); double maxTurn = Math.PI/180*(10 - 0.75*velocity); theta = limit(0,theta - maxTurn, Math.PI/2); if(theta == 0) distanceRemaining -= velocity; else //rule of cosines distanceRemaining = Math.sqrt(velocity*velocity + distanceRemaining*distanceRemaining - 2*velocity*distanceRemaining*Math.cos(theta)); time++; }while(time < timeAvailable); return distanceRemaining < 1; }