code: private void optimize(NavPath path) {
  		int pt = 0;
  		
  		while (pt < path.length()-2) {
  			float sx = path.getX(pt);
  			float sy = path.getY(pt);
  			float nx = path.getX(pt+2);
  			float ny = path.getY(pt+2);
  			
  			if (isClear(sx,sy,nx,ny,0.1f)) {
  				path.remove(pt+1);
  			} else {
  				pt++;
  			}
  		}
  	}
docs: Optimize a path by removing segments that arn't required
  to reach the end point
  
  @param path The path to optimize. Redundant segments will be removed