double g = 20;
double v = speed.asDouble();
Vector relative = destination.clone().subtract(originLocation).toVector();
double testAng = Velocity.launchAngle(originLocation, destination.toVector(), v, relative.getY(), g);
double hangTime = Velocity.hangtime(testAng, v, relative.getY(), g);
Vector to = destination.clone().add(lead.clone().multiply(hangTime)).toVector();
relative = to.clone().subtract(originLocation.toVector());
Double dist = Math.sqrt(relative.getX() * relative.getX() + relative.getZ() * relative.getZ());
if (dist == 0) dist = 0.1d;
testAng = Velocity.launchAngle(originLocation, to, v, relative.getY(), g);
relative.setY(Math.tan(testAng) * dist);