Applied same fix to cost_path_search and a_star_find_path.