/* * INPUT : * command : x_start;y_start;x_end;y_end\n * * OUTPUT: * if valid: * x_start;y_start;x1;y1;...;xn;xy;x_end;y_end;path_length\n * all coordinates are integers, path_length is a float * if invalid (no path found or parsing error): * FAILED\n * * if start point is not valid, find the nearest valid point * if end point is not valid, do the same, except if * the end point is in a dynamic barrier (aka a robot) * then return \n immediatly */ string command_calc_path(string& command, MAP &map) { int x_s, y_s, x_e, y_e; vertex_descriptor start, end, start_valid, end_valid; vector<vertex_descriptor> path; double distance; stringstream answer; command = command.substr(2); if (sscanf(command.c_str(), "%i;%i;%i;%i", &x_s, &y_s, &x_e, &y_e) < 4) { cerr << "Did not parse the input correctly" << endl; return FAILED_STR; } if (!(is_valid(x_s, y_s, map) && is_valid(x_e, y_e, map))) { cerr << "Start or end point is not in the map" << endl; return FAILED_STR; } end = map.get_vertex(x_e, y_e); if (map.has_dynamic_barrier(end)) { cerr << "End point on a dynamic object" << endl; return FAILED_STR; } end_valid = map.find_nearest_valid(end); start = map.get_vertex(x_s, y_s); start_valid = map.find_nearest_valid(start); #if DEBUG cerr << "Start : " << start_valid[0] << ":" << start_valid[1] << endl; cerr << "End : " << end_valid[0] << ":" << end_valid[1] << endl; clock_t t = clock(); #endif map.solve(start_valid, end_valid); if (!map.solved()) { cerr << "Could not find any path" << endl; return FAILED_STR; } map.solve_smooth(); path = map.get_smooth_solution(); distance = map.get_smooth_solution_length(); if (start != start_valid) { path.insert(path.begin(), start); distance += euclidean_heuristic(start)(start_valid); } if (end != end_valid) { path.push_back(end); distance += euclidean_heuristic(end)(end_valid); } for (auto &point: path) { answer << point[0] << ";" << point[1] << ";"; } answer << distance << endl; #if DEBUG cerr << "Path contains " << path.size() << " points, total distance = " << distance << endl; cerr << "Computing time : " << double(clock() - t)/CLOCKS_PER_SEC << endl; #if RENDER_BMP map.generate_bmp("tmp.bmp"); #endif #endif return answer.str(); }