A_star类

源码如下所示：

class A_star
{
	public:
		A_star() {};
		~A_star();
		void Astar_algorithm();
		bool judge_env_legal(const Point&);
		int calcu_Hn(const Point&);
		int calcu_Gn(const Point&);
		int calcu_cost(const Point&, const Point&);
		void set_start_end_obs_map(Graph&);
		void add_openset(Point* p) { openset.push_back(p); };
		void add_fatherset(Point* p) { fatherset.push_back(p); };
		void delete_openset(const int& idx) {openset.erase(openset.begin() + idx);};
		void search_env_block(Point*);
		void calcu_type_selection(int&, const int&, const int&);
		void show_para_below(Point*);
		bool is_alread_set(std::vector<Point*>&, Point*);
		void get_trajectory(const Point*);
		UserPara get_astar_userpara();
		Point* find_min_Fn(int& idx);

	private:
		int cross_cost = 14;
		int plane_cost = 10;
		std::vector<Point*> openset;
		std::vector<Point*> fatherset;
		//std::vector<Point*> trajectory;
		std::vector<Point*> obstacleset;		
		Point start;
		Point end;
		UserPara user_trajectory;
		const int env_nums = 8;
		//int offset_x[4] = {0, 1, 0, -1};
		//int offset_y[4] = {-1, 0, 1, 0};
		int offset_x[8] = {-1, 0, 1, 1, 1, 0, -1, -1};
		int offset_y[8] = { -1, -1, -1, 0, 1, 1, 1, 0};
		
};

这个就是A_star类定义，下面是Astar_algorithm函数主体：

void A_star::Astar_algorithm()
{
	int min_Fn_idx(0);
	Point* min_Fn_p = new Point(start.x, start.y);

	memcpy(min_Fn_p, &start, sizeof(start));
	add_openset(min_Fn_p);
	
	while (!openset.empty()) {
		Graph::show_map(user_trajectory);
		min_Fn_p = find_min_Fn(min_Fn_idx);
		show_para_below(min_Fn_p);
		if (*min_Fn_p == end) { break; }
		search_env_block(min_Fn_p);
		
		add_fatherset(min_Fn_p);
		delete_openset(min_Fn_idx);
		
		user_trajectory.map[min_Fn_p->x][min_Fn_p->y] = PASS_SEARCH;
	}

	get_trajectory(min_Fn_p);
}

可以看到，这个这个函数主体就是这篇文章里的伪代码的具体实现：[Astar_algorithm01]A*算法伪代码以及思路