#include #include #include #include #include #include #include #include "zlist.h" #include "line.h" #include "ant.h" #include "card_path.h" #include "visit.h" namespace{ static const double PI=3.1416; inline bool eq(double a,double b,double e=0.0001) { return fabs(a-b) { int sid; base_path(int i1=-1,int i2=-1) { at(0)=i1; at(1)=i2; } bool operator<(const base_path&o)const { int c=at(0)-o.at(0); if(c!=0) return c<0; return at(1)-o.at(1)<0; } point cross(const vertex_list&v_list, const base_path&o)const; bool contains(const vertex_list&v_list, const point&o, double e)const; double arg(const vertex_list&v)const; int type(const vertex_list&v)const; line_v as_line(const vertex_list&v)const; int level(const vertex_list&v)const; }; struct vertex_list { std::vector v; std::string to_string()const { std::string ret; ret.reserve(1024); char buf[128]; for(int i=0,len=v.size();i>2)<<16) + (((unsigned)p.y)>>2)); } int find(const point&p,double x=1)const { int rc=-1; double t; for(int i=0,len=v.size();i&path,const vertex_list&v_list) { printf("%s\n","----------------------------------------------------------------"); for(int i=0,len=path.size();i> { std::vector ret; vertex_list v; handle_path() { v.add(point(0,0),0,-1); } bool visit(std::shared_ptr sit) { //auto&s=sites[i]; const auto &s = *sit; if(!s.have_valid_path()) return false; if(s.path(0).empty()||s.path(1).empty()) return false; if(s[0].size()<2) return false; line_v l000 = s[0][0][0]; line_v l010 = s[0][1][0]; if(l000.is_same(l010)) { int p0=v.add(point::min(s.path(0),s.path(1)),0,s.m_id); int p1=v.add(point::max(s.path(0),s.path(1)),0,s.m_id); ret.push_back(base_path(p0,p1)); ret.back().sid=s.m_id; } else { point px = l000.line::crossing(l010); for(int i=0;i<2;i++) { int p0=v.add(point::min(px,s.path(i)),0,s.m_id); int p1=v.add(point::max(px,s.path(i)),0,s.m_id); ret.push_back(base_path(p0,p1)); ret.back().sid=s.m_id; } } for(int i=0;i<2;i++) { if(!s[0][i][1].empty()) { int p0=v.add(point::min(s[0][i][1][0],s[0][i][1][1]),0,s.m_id); int p1=v.add(point::max(s[0][i][1][0],s[0][i][1][1]),0,s.m_id); ret.push_back(base_path(p0,p1)); ret.back().sid=s.m_id; } } return true; } }; static std::vector init_path(std::vector & ret,vertex_list&v) { // for(uint32_t i=0;i> p0(ret.size()); std::vector ret2; for(int i=0,len=ret.size();iPI/2) arg-=PI/2; if(arg/PI*180<5)//相交小于5度,不切分已有路径 continue; int id=v.add(cr,arg,v[ret[i][0]].sid0,v[ret[j][0]].sid0); p0[i].push_back(id); p0[j].push_back(id); } p0[i].push_back(ret[i][1]); std::sort(p0[i].begin(),p0[i].end(),[&v](int i0,int i1){ return v[i0] tmp; tmp.push_back(ret2[i].as_line(v)); for(int j=0;j decode(unsigned h) { const uint64_t S=0x8000000000000000; int x=0,y=0; for(int i=0;i<64;i++) { x<<=1; y<<=1; if(h&S) x|=1; h<<=1; if(h&S) y|=1; h<<=1; } return std::make_tuple(x-2147483648,y-2147483648); } static uint64_t encode(int64_t x, int64_t y) { return encode_(x+2147483648,y+2147483648); } public: //test static void test_code(int64_t x,int64_t y) { uint64_t h=ghash::encode(x,y); auto t=ghash::decode(h); printf("src x=%X,y=%X hash=%X,check x=%X,y=%X\n",x,y,h,std::get<0>(t),std::get<1>(t)); } static void test() { for(int i=0;i<10;i++) { test_code((4< geo2path; public: static uint64_t encode(double x0,double y0) { uint64_t x=(uint32_t)(x0*10); uint64_t y=(uint32_t)(y0*10); return (x<<32)|y; } int find(double x,double y)const { uint64_t h=encode(x,y); auto it=geo2path.find(h); if(it==geo2path.end()) return -1; return it->second; } void add(double x,double y,int id) { uint64_t h=encode(x,y); geo2path.insert(std::make_pair(h,id)); } int size() { return geo2path.size(); } void print() { for(auto it=geo2path.begin();it!=geo2path.end();++it) { //std::cout<first<<"\n"; printf("%ld\n",it->first); } } geolist() { } ~geolist() { } }; struct graph { vertex_list v; std::vector> d; //索引为起始顶点,数组内容为目标点 std::vector> l; //bast_path list geolist geo; graph() {} void init(const vertex_list&v_,const std::vector&bp) { for(auto&p:bp) { int from=v.add(v_[p[0]]); int to=v.add(v_[p[1]]); l.push_back({from,to}); int id=l.size()-1; line_v lv(v[from],v[to]); #ifdef __DEBUG__ printf("line:%s\n",lv.to_string().c_str()); #endif double cos=lv.cos_k(); double sin=lv.sin_k(); double x,y; for(double r=0,len=lv.length();r find_possible_path(const point&from,double dist) const { std::vector ret; int start_pt_id=v.find(from,dist); if(start_pt_id==-1) return std::move(ret); point start_pt=v[start_pt_id]; for(uint32_t i=0;i&ret,int from,std::array&to,std::set&ex,int level) { if(level--<0) return false; ret.push_back(from); for(int p:d[from]) //在当前路径 { if(ex.find(p)!=ex.end()) //找过 continue; if(p==to[0]||p==to[1])//找到 return true; // { // ret.push_back(p); // return true; // } } ex.insert(from); for(int p:d[from])//在下级路径 { if(ex.find(p)!=ex.end())//找过 continue; if(find(ret,p,to,ex,level)) //找到 { return true; } } ret.pop_back(); return false; } bool is_at_path(const point&pt)const { return geo.find(pt.x,pt.y)>=0; } std::vector find(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 {}; #if 0 std::vector rc[2]; for(int i=0;i<2;i++) { std::vector ret; std::set ex({l[f][1-i]}); if(find(ret,l[f][i],l[t],ex,2)) { for(auto id:ret) rc[i].push_back(v[id]); } } if(!rc[0].empty() && (rc[1].empty() || rc[0].size() < rc[1].size())) return std::move(rc[0]); else if(!rc[1].empty()) return std::move(rc[1]); #endif std::vector ret; int size=v.size(); std::vector mat(size+1,-1); std::vector trace(size+1,-1); std::vector val(size+2,false); mat[l[t][0]]=to.dist(v[l[t][0]]); mat[l[t][1]]=to.dist(v[l[t][1]]); int f0=l[f][0],f1=l[f][1]; for(int i=0;i<2;i++) { if(mat[f0]!=-1 && mat[f1]!=-1) break; for(int j=0;j rc; if(mat[f0]!=-1 && (mat[f1]==-1 || mat[f0] < mat[f1])) { int temp=f0; while(temp!=-1) { rc.push_back(v[temp]); temp=trace[temp]; } return std::move(rc); } else if(mat[f1]!=-1) { int temp=f1; while(temp!=-1) { rc.push_back(v[temp]); temp=trace[temp]; } return std::move(rc); } return {}; } }; std::atomic g_init_flag(0); graph g_graph; }//namespace void card_path::init() { //Ensure only ont thread can init path. int expect=0; if(g_init_flag.compare_exchange_strong(expect,1)) { handle_path hp; //std::vector opath=init_path(sites,v_list); sit_list::instance()->accept(hp); std::vector opath=init_path(hp.ret,hp.v); g_graph.init(hp.v,opath); ++g_init_flag; } //if != 2 then wait here until init over. while(g_init_flag.load()!=2) { std::this_thread::sleep_for (std::chrono::seconds(1)); } } card_path&card_path::inst() { static card_path path; return path; } std::vector card_path::find_path(const point&from,const point&to)const { return g_graph.find(from,to); } bool card_path::is_at_path(const point&pt)const { return g_graph.is_at_path(pt); } std::vector card_path::find_possible_path(const point&from,double dist) const { return std::move(g_graph.find_possible_path(from,dist)); } #ifdef __DEBUG__ void test_at_path(card_path&cp,const point&pt) { if(cp.is_at_path(pt)) printf("test (%.3lf,%.3lf) is-at-path:true\n",pt.x,pt.y); else printf("test (%.3lf,%.3lf) is-at-path:false\n",pt.x,pt.y); } void test_find_path(card_path&cp,const point&p1,const point&p2) { printf("\nfind-path: from=(%.3lf,%.3lf),to=(%.3lf,%.3lf)\n",p1.x,p1.y,p2.x,p2.y); std::vector rc=cp.find_path(p1,p2); for(uint32_t i=0;i rc=cp.find_possible_path(from,3); for(uint32_t i=0;i sites(new sit_list()); //sit_list::instance()->load("data_reader_antenna.txt","path_tof.txt"); sit_list::instance()->load("1.txt","2.txt"); card_path cp; cp.init(); #if 0 { test_at_path(cp,point(4773.104654,-104.0887)); test_at_path(cp,point(4727,-74.8)); test_at_path(cp,point(4727,-75.0)); test_at_path(cp,point(4727,-75.2)); test_at_path(cp,point(4727,-75.8)); test_at_path(cp,point(4727,-90)); test_at_path(cp,point(4727,-89.6)); test_at_path(cp,point(4727,-89.8)); test_at_path(cp,point(4727,-90)); test_at_path(cp,point(4727,-90.2)); test_at_path(cp,point(4727,-90.4)); } #endif //test_at_path(cp,point(4726.90,376.85)); test_find_path(cp,point(-227.6,174.9),point(-201.3,175.5)); test_find_path(cp,point(-227.625219,174.945670),point(-170.085722,176.101574)); test_find_path(cp,point(-393.1944,262.5324),point(-366.0754,274.1253)); //test_find_path(cp,point(4637.000000,-75),point(4727,135)); #if 1 { test_find_path(cp,point(4637.000000,-75),point(4727,135)); test_find_path(cp,point(4637.000000,-75),point(4727,120)); test_find_path(cp,point(4700,-75),point(4727,120)); test_find_path(cp,point(4725,-75),point(4727,135)); test_find_path(cp,point(4727,-89.7),point(6144.3,-523)); } #endif #if 0 { test_find_poss_path(cp, point(4727,-89)); test_find_poss_path(cp, point(4727,-90)); test_find_poss_path(cp, point(4724,-75)); test_find_poss_path(cp, point(4725,-75)); test_find_poss_path(cp, point(4726,-75)); test_find_poss_path(cp, point(4727,-75)); test_find_poss_path(cp, point(4728,-75)); test_find_poss_path(cp, point(4727,-7)); test_find_poss_path(cp, point(4726.6,33)); } #endif return 0; } #endif