Exemple #1
0
/*
 * 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();
}