A* 最短路径算法

ant.cpp

@@ -373,6 +373,7 @@ void sit_list::read_ant_path(int id)
 
 	card_path::init();
 
+	test_find_path(card_path::inst(),point(4727,-8.06),point(5427.04,-304.02));
 	test_find_path(card_path::inst(),point(4714.70,-8.06),point(5427.04,-304.02));
 }
 

card_path.cpp

@@ -5,6 +5,7 @@
 #include <memory>
 #include <atomic>
 #include <thread>
+#include <list>
 #include "zlist.h"
 #include "line.h"
 #include "ant.h"
@@ -189,7 +190,6 @@ point  base_path::cross(const vertex_list&v, const base_path&o)const
 
 	return l0.crossing(l1);
 }
-#ifdef __DEBUG__
 void log_path(const std::vector<base_path>&path,const vertex_list&v_list)
 {
 	log_info("%s\n","----------------------------------------------------------------");
@@ -199,7 +199,6 @@ void log_path(const std::vector<base_path>&path,const vertex_list&v_list)
 		log_info("base_path %.6lf, %03d,(%d,%s),(%d,%s)\n",c,i,path[i][0],v_list[path[i][0]].to_string().c_str(),path[i][1],v_list[path[i][1]].to_string().c_str());
 	}
 }
-#endif
 struct handle_path :visitor<std::shared_ptr<site>>
 {
 	std::vector<base_path> ret;
@@ -490,8 +489,8 @@ static std::vector<base_path> init_path(std::vector<base_path> & ret,vertex_list
 		return arg<0;
 	});
 
-	log_path(ret2,v);
 #endif
+	log_path(ret2,v);
 
 	return std::move(ret2);
 }
@@ -743,6 +742,71 @@ struct graph
 		return geo.find(pt.x,pt.y)>=0;
 	}
 
+	struct item
+	{
+		item(int _vid=-1, int _g=0,int _h=0,std::shared_ptr<item> _parent=nullptr)
+			:vid(_vid)
+			,g(_g)
+			,h(_h)
+			,parent(_parent)
+		{
+		}
+		int vid=-1,g=0,h=0;
+		std::shared_ptr<item> parent;
+
+		int cost()const {return g+h;}
+
+		bool operator< (const item&i) const
+		{
+			return vid<i.vid;
+		}
+	};
+
+	std::vector<point> find2(const point&from,const point&to)
+	{
+		int f=geo.find(from.x,from.y);
+		int t=geo.find(to.x,to.y);
+
+		if(f<0 || t<0 || f==t)
+			return {};
+
+		std::set<std::shared_ptr<item>> open_set;
+		std::set<int> close_set;
+		open_set.insert(std::make_shared<item>(l[f][0],from.dist(v[l[f][0]]),to.dist(v[l[f][0]])));
+		open_set.insert(std::make_shared<item>(l[f][1],from.dist(v[l[f][1]]),to.dist(v[l[f][1]])));
+
+		std::shared_ptr<item> cur;
+		while(open_set.size()>0)
+		{
+			cur=*open_set.begin();
+			for(auto&i:open_set)
+			{
+				if(i->cost()<cur->cost())
+					cur=i;
+			}
+			open_set.erase(cur);
+			close_set.insert(cur->vid);
+			if(cur->vid==l[t][0]||cur->vid==l[t][1])
+				break;
+
+			for(auto vid:d[cur->vid])
+			{
+				if(close_set.find(vid)!=close_set.end())
+					continue;
+
+				open_set.insert(std::make_shared<item>(vid,cur->g+v[cur->vid].dist(v[vid]),to.dist(v[vid]),cur));
+			}
+		}
+
+		std::vector<point> rc;
+		while(cur.get())
+		{
+			rc.push_back(v[cur->vid]);
+			cur=cur->parent;
+		}
+		return std::move(rc);
+	}
+
 	std::vector<point> find(const point&from,const point&to)
 	{
 		int f=geo.find(from.x,from.y);
@@ -845,7 +909,7 @@ card_path&card_path::inst()
 
 std::vector<point> card_path::find_path(const point&from,const point&to)const
 {
-	return g_graph.get()->find(from,to);
+	return g_graph.get()->find2(from,to);
 }
 
 bool card_path::is_at_path(const point&pt)const