card_path.cpp 19 KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015
  1. #include <stdio.h>
  2. #include <algorithm>
  3. #include <unordered_map>
  4. #include <set>
  5. #include <memory>
  6. #include <atomic>
  7. #include <thread>
  8. #include <list>
  9. #include "zlist.h"
  10. #include "line.h"
  11. #include "ant.h"
  12. #include "config_file.h"
  13. #include "card_path.h"
  14. #include "visit.h"
  15. namespace{
  16. static const double PI=3.1416;
  17. inline bool eq(double a,double b,double e=0.0001)
  18. {
  19. return fabs(a-b)<e;
  20. }
  21. struct vertex:point
  22. {
  23. float level;
  24. int sid0,sid1;
  25. vertex()
  26. :level(0)
  27. ,sid0(0)
  28. ,sid1(0)
  29. {}
  30. vertex(const point&pt,float level_=0,int site_id0=0,int site_id1=0)
  31. :point(pt)
  32. ,level(level_)
  33. ,sid0(site_id0)
  34. ,sid1(site_id1)
  35. {
  36. }
  37. std::string to_string()const
  38. {
  39. char buf[128];
  40. sprintf(buf,"%.2f,%.3lf,%d,%d,%.2lf",x,y,sid0,sid1,level/PI*180);
  41. return buf;
  42. }
  43. };
  44. struct vertex_list;
  45. struct base_path:std::array<int,2>
  46. {
  47. int sid;
  48. base_path(int i1=-1,int i2=-1)
  49. {
  50. at(0)=i1;
  51. at(1)=i2;
  52. }
  53. bool operator<(const base_path&o)const
  54. {
  55. int c=at(0)-o.at(0);
  56. if(c!=0)
  57. return c<0;
  58. return at(1)-o.at(1)<0;
  59. }
  60. point cross(const vertex_list&v_list, const base_path&o)const;
  61. bool contains(const vertex_list&v_list, const point&o, double e)const;
  62. double arg(const vertex_list&v)const;
  63. int type(const vertex_list&v)const;
  64. line_v as_line(const vertex_list&v)const;
  65. int level(const vertex_list&v)const;
  66. };
  67. struct vertex_list
  68. {
  69. std::vector<vertex> v;
  70. std::string to_string()const
  71. {
  72. std::string ret;
  73. ret.reserve(1024);
  74. char buf[128];
  75. for(int i=0,len=v.size();i<len;i++)
  76. {
  77. sprintf(buf,"vertex:%3d,%s\n",i,v[i].to_string().c_str());
  78. ret+=buf;
  79. }
  80. return std::move(ret);
  81. }
  82. void log()
  83. {
  84. log_info("%s",to_string().c_str());
  85. }
  86. static unsigned hash(const point&p)
  87. {
  88. return (((((unsigned)p.x)>>2)<<16) + (((unsigned)p.y)>>2));
  89. }
  90. int find(const point&p,double x=1)const
  91. {
  92. int rc=-1;
  93. double t;
  94. for(int i=0,len=v.size();i<len;i++)
  95. {
  96. t=p.dist(v[i]);
  97. if(t<x)
  98. {
  99. rc=i;
  100. x=t;
  101. }
  102. }
  103. return rc;
  104. }
  105. int add(const point&p,float level=0,int sid0=0,int sid1=0)
  106. {
  107. int i=find(p);
  108. if(i<0)
  109. {
  110. v.push_back(vertex(p,level,sid0,sid1));
  111. return v.size()-1;
  112. }
  113. vertex&v0=v[i];
  114. v0.level=std::max(v0.level,level);
  115. return i;
  116. }
  117. int size()const
  118. {
  119. return v.size();
  120. }
  121. const vertex&operator[](int i)const
  122. {
  123. return v[i];
  124. }
  125. vertex&operator[](int i)
  126. {
  127. return v[i];
  128. }
  129. };
  130. int base_path::level(const vertex_list&v)const
  131. {
  132. const vertex&p0=v[at(0)];
  133. const vertex&p1=v[at(1)];
  134. return p0.level+p1.level;
  135. }
  136. line_v base_path::as_line(const vertex_list&v)const
  137. {
  138. const vertex&p0=v[at(0)];
  139. const vertex&p1=v[at(1)];
  140. return line_v(p0,p1);
  141. }
  142. double base_path::arg(const vertex_list&v)const
  143. {
  144. return as_line(v).arg();
  145. }
  146. bool base_path::contains(const vertex_list&v, const point&o, double e)const
  147. {
  148. return as_line(v).contain(o,e);
  149. }
  150. point base_path::cross(const vertex_list&v, const base_path&o)const
  151. {
  152. line_v l0=as_line(v);
  153. line_v l1=o.as_line(v);
  154. return l0.crossing(l1);
  155. }
  156. void log_path(const std::vector<base_path>&path,const vertex_list&v_list,const char*add_info="")
  157. {
  158. log_info("%s\n","----------------------------------------------------------------");
  159. for(int i=0,len=path.size();i<len;i++)
  160. {
  161. double c=path[i].arg(v_list)/PI*180;
  162. log_info("base_path:%s %.6lf, %03d,(%d,%s),(%d,%s)",add_info,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());
  163. }
  164. }
  165. struct handle_path :visitor<std::shared_ptr<site>>
  166. {
  167. std::vector<base_path> ret;
  168. vertex_list v;
  169. handle_path()
  170. {
  171. v.add(point(0,0),0,-1);
  172. }
  173. bool visit(std::shared_ptr<site> sit)
  174. {
  175. //auto&s=sites[i];
  176. const auto &s = *sit;
  177. if(!s.have_valid_path())
  178. return false;
  179. if(s.path(0).empty()||s.path(1).empty())
  180. return false;
  181. if(s[0].size()<2)
  182. return false;
  183. line_v l000 = s[0][0][0];
  184. line_v l010 = s[0][1][0];
  185. if(l000.is_same(l010))
  186. {
  187. int p0=v.add(point::min(s.path(0),s.path(1)),0,s.m_id);
  188. int p1=v.add(point::max(s.path(0),s.path(1)),0,s.m_id);
  189. ret.push_back(base_path(p0,p1));
  190. ret.back().sid=s.m_id;
  191. }
  192. else
  193. {
  194. point px = l000.line::crossing(l010);
  195. for(int i=0;i<2;i++)
  196. {
  197. int p0=v.add(point::min(px,s.path(i)),0,s.m_id);
  198. int p1=v.add(point::max(px,s.path(i)),0,s.m_id);
  199. ret.push_back(base_path(p0,p1));
  200. ret.back().sid=s.m_id;
  201. }
  202. }
  203. for(int i=0;i<2;i++)
  204. {
  205. if(!s[0][i][1].empty())
  206. {
  207. int p0=v.add(point::min(s[0][i][1][0],s[0][i][1][1]),0,s.m_id);
  208. int p1=v.add(point::max(s[0][i][1][0],s[0][i][1][1]),0,s.m_id);
  209. ret.push_back(base_path(p0,p1));
  210. ret.back().sid=s.m_id;
  211. }
  212. }
  213. return true;
  214. }
  215. };
  216. static std::vector<base_path> init_path(std::vector<base_path> & ret,vertex_list&v)
  217. {
  218. do
  219. {
  220. config_file cfg;
  221. if(cfg.open("../etc/path-not-cross.ini"))
  222. break;
  223. auto kset=cfg.keys();
  224. if(kset.empty())
  225. break;
  226. for(auto k:kset)
  227. {
  228. if(strncmp(k,"add_path",8)!=0)
  229. continue;
  230. const char* vv=cfg.get(k,"");
  231. point p0;
  232. point p1;
  233. if(sscanf(vv,"%lf,%lf,%lf,%lf",&p0.x,&p0.y,&p1.x,&p1.y)==4)
  234. {
  235. int p00=v.add(p0,0,999);
  236. int p10=v.add(p1,0,999);
  237. ret.push_back(base_path(p00,p10));
  238. }
  239. }
  240. break;
  241. }while(1);
  242. #ifdef __DEBUG__
  243. log_path(ret,v);
  244. printf("++++++++++++++++++++++++++++++++++++++++++++");
  245. #endif
  246. std::sort(ret.begin(),ret.end());
  247. #ifdef __DEBUG__
  248. log_path(ret,v);
  249. printf("++++++++++++++++++++++++++++++++++++++++++++");
  250. #endif
  251. ret.erase(std::unique(ret.begin(),ret.end()),ret.end());
  252. #ifdef __DEBUG__
  253. log_path(ret,v);
  254. printf("+++++++++++++++++nnnnn+++++++++++++++++++++++++++");
  255. #endif
  256. std::sort(ret.begin(),ret.end(),[&v](const base_path&p1,const base_path&p2){
  257. double arg=p1.arg(v)-p2.arg(v);
  258. if(fabs(arg)<0.1)
  259. {
  260. return v[p1[0]]<v[p2[0]];
  261. }
  262. return arg<0;
  263. });
  264. log_path(ret,v,"合并前");
  265. //合并相同
  266. for(int i=0,len=ret.size();i<len;i++)
  267. {
  268. line_v li=ret[i].as_line(v);
  269. for(int j=i+1;j<len;j++)
  270. {
  271. line_v lj=ret[j].as_line(v);
  272. if(!lj.is_same(li,2.5))
  273. continue;
  274. line_v ij=lj.projection(li);
  275. if(ij.empty())
  276. continue;
  277. point p0=point::min(v[ret[j][0]],v[ret[i][0]]);
  278. point p1=point::max(v[ret[j][1]],v[ret[i][1]]);
  279. ret[j][0]=v.add(p0,0);
  280. ret[j][1]=v.add(p1,0);
  281. ret[i][0]=0;
  282. ret[i][1]=0;
  283. break;
  284. }
  285. }
  286. ret.erase(std::remove_if(ret.begin(),ret.end(),[&v](const base_path&p){
  287. return p[0]==0||p[1]==0;
  288. }),ret.end());
  289. std::sort(ret.begin(),ret.end(),[&v](const base_path&p1,const base_path&p2){
  290. double arg=p1.arg(v)-p2.arg(v);
  291. if(fabs(arg)<0.1)
  292. {
  293. return v[p1[0]]<v[p2[0]];
  294. }
  295. return arg<0;
  296. });
  297. log_path(ret,v,"合并后");
  298. //非路径交点配置
  299. std::vector<point> no_cross;
  300. auto in_no_cross=[&no_cross](const point&pt){
  301. for(auto p:no_cross)
  302. {
  303. if(p.dist(pt)<3)
  304. return true;
  305. }
  306. return false;
  307. };
  308. do
  309. {
  310. config_file cfg;
  311. if(cfg.open("../etc/path-not-cross.ini"))
  312. break;
  313. auto kset=cfg.keys();
  314. if(kset.empty())
  315. break;
  316. for(auto k:kset)
  317. {
  318. if(strncmp(k,"pt",2)!=0)
  319. continue;
  320. const char* v=cfg.get(k,"");
  321. point tmp;
  322. if(sscanf(v,"%lf,%lf",&tmp.x,&tmp.y)==2)
  323. no_cross.push_back(tmp);
  324. }
  325. for(auto p:no_cross)
  326. {
  327. log_info("no_cross_point:x=%lf,y=%lf",p.x,p.y);
  328. }
  329. break;
  330. }while(1);
  331. std::vector<std::vector<int>> p0(ret.size());
  332. std::vector<base_path> ret2;
  333. for(int i=0,len=ret.size();i<len;i++)
  334. {
  335. p0[i].push_back(ret[i][0]);
  336. for(int j=i+1;j<len;j++)
  337. {
  338. if(i==j) continue;
  339. point cr=ret[i].cross(v,ret[j]);
  340. if(cr.empty())
  341. continue;
  342. if(in_no_cross(cr))
  343. continue;
  344. double arg=fabs(ret[i].as_line(v).arg()-ret[j].as_line(v).arg());
  345. while(arg>PI/2)
  346. arg-=PI/2;
  347. if(arg/PI*180<5)//相交小于5度,不切分已有路径
  348. continue;
  349. int id=v.add(cr,arg,v[ret[i][0]].sid0,v[ret[j][0]].sid0);
  350. p0[i].push_back(id);
  351. p0[j].push_back(id);
  352. }
  353. p0[i].push_back(ret[i][1]);
  354. std::sort(p0[i].begin(),p0[i].end(),[&v](int i0,int i1){
  355. return v[i0]<v[i1];
  356. });
  357. auto it=std::unique(p0[i].begin(),p0[i].end());
  358. p0[i].erase(it,p0[i].end());
  359. for(int j=1,cnt=p0[i].size();j<cnt;j++)
  360. {
  361. ret2.push_back(base_path(p0[i][j - 1], p0[i][j]));
  362. ret2.back().sid = ret[i].sid;
  363. }
  364. }
  365. ret2.erase(std::remove_if(ret2.begin(),ret2.end(),[&v](base_path&p){
  366. point&p0=v[p[0]];
  367. point&p1=v[p[1]];
  368. //return p0.dist(p1)<0.1 || p0.empty() || p1.empty();
  369. return p0.dist(p1)<1;
  370. }),ret2.end());
  371. std::sort(ret2.begin(),ret2.end());
  372. ret2.erase(std::unique(ret2.begin(),ret2.end()),ret2.end());
  373. std::sort(ret2.begin(),ret2.end(),[&v](const base_path&p1,const base_path&p2){
  374. double arg=p1.arg(v)-p2.arg(v);
  375. if(fabs(arg)<0.1)
  376. {
  377. return v[p1[0]]<v[p2[0]];
  378. }
  379. return arg<0;
  380. });
  381. log_path(ret2,v,"拆分后");
  382. return std::move(ret2);
  383. }
  384. #if 0
  385. struct ghash
  386. {
  387. static std::tuple<int,int> decode(unsigned h)
  388. {
  389. const uint64_t S=0x8000000000000000;
  390. int x=0,y=0;
  391. for(int i=0;i<64;i++)
  392. {
  393. x<<=1;
  394. y<<=1;
  395. if(h&S)
  396. x|=1;
  397. h<<=1;
  398. if(h&S)
  399. y|=1;
  400. h<<=1;
  401. }
  402. return std::make_tuple(x-2147483648,y-2147483648);
  403. }
  404. static uint64_t encode(int64_t x, int64_t y)
  405. {
  406. return encode_(x+2147483648,y+2147483648);
  407. }
  408. public: //test
  409. static void test_code(int64_t x,int64_t y)
  410. {
  411. uint64_t h=ghash::encode(x,y);
  412. auto t=ghash::decode(h);
  413. 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));
  414. }
  415. static void test()
  416. {
  417. for(int i=0;i<10;i++)
  418. {
  419. test_code((4<<i)-1,(4<<i)-1);
  420. test_code((4<<i)-1,(4<<i));
  421. test_code((4<<i)-1,(4<<i)-1);
  422. test_code((4<<i),(4<<i)-1);
  423. }
  424. }
  425. private:
  426. static unsigned encode_(unsigned short x, unsigned short y)
  427. {
  428. const unsigned S=0x8000;
  429. unsigned r=0;
  430. for(int i=0;i<16;i++)
  431. {
  432. r<<=2;
  433. if(x&S)
  434. {
  435. r|=(y&S)?3:2;
  436. }
  437. else
  438. {
  439. if(y&S) r|=1;
  440. }
  441. x<<=1;
  442. y<<=1;
  443. }
  444. return r;
  445. }
  446. };
  447. #endif
  448. struct geolist
  449. {
  450. std::unordered_map<uint64_t,int> geo2path;
  451. public:
  452. static uint64_t encode(double x0,double y0)
  453. {
  454. uint64_t x=(uint32_t)(x0*10);
  455. uint64_t y=(uint32_t)(y0*10);
  456. return (x<<32)|y;
  457. }
  458. int find(double x,double y)const
  459. {
  460. uint64_t h=encode(x,y);
  461. auto it=geo2path.find(h);
  462. if(it==geo2path.end())
  463. return -1;
  464. return it->second;
  465. }
  466. void add(double x,double y,int id)
  467. {
  468. uint64_t h=encode(x,y);
  469. geo2path.insert(std::make_pair(h,id));
  470. }
  471. int size()
  472. {
  473. return geo2path.size();
  474. }
  475. void print()
  476. {
  477. for(auto it=geo2path.begin();it!=geo2path.end();++it)
  478. {
  479. //std::cout<<it->first<<"\n";
  480. printf("%ld\n",it->first);
  481. }
  482. }
  483. geolist()
  484. {
  485. }
  486. ~geolist()
  487. {
  488. }
  489. };
  490. struct graph
  491. {
  492. vertex_list v;
  493. std::vector<std::vector<int>> d; //索引为起始顶点,数组内容为目标点
  494. std::vector<std::array<int,2>> l; //bast_path list
  495. geolist geo;
  496. graph()
  497. {}
  498. void init(const vertex_list&v_,const std::vector<base_path>&bp)
  499. {
  500. for(auto&p:bp)
  501. {
  502. int from=v.add(v_[p[0]]);
  503. int to=v.add(v_[p[1]]);
  504. l.push_back({from,to});
  505. int id=l.size()-1;
  506. line_v lv(v[from],v[to]);
  507. log_info("graph::init() site:%d line:%s\n",p.sid, lv.to_string().c_str());
  508. double cos=lv.cos_k();
  509. double sin=lv.sin_k();
  510. double x,y;
  511. for(double r=0,len=lv.length();r<len+0.1;r+=0.05)
  512. {
  513. x=lv[0].x+r*cos;
  514. y=lv[0].y+r*sin;
  515. // printf("x=%lf,y=%lf\n",x,y);
  516. for(int i=-1;i<=1;i++)
  517. for(int j=-1;j<=1;j++)
  518. {
  519. geo.add(x+i/10.,y+j/10.,id);
  520. }
  521. }
  522. add(from,to);
  523. }
  524. }
  525. std::vector<line_v> find_possible_path(const point&from,double dist) const
  526. {
  527. std::vector<line_v> ret;
  528. int start_pt_id=v.find(from,dist);
  529. if(start_pt_id==-1)
  530. return std::move(ret);
  531. point start_pt=v[start_pt_id];
  532. for(uint32_t i=0;i<d[start_pt_id].size();i++)
  533. {
  534. ret.push_back(line_v(start_pt,v[d[start_pt_id][i]]));
  535. }
  536. return std::move(ret);
  537. }
  538. void add(int from,int to)
  539. {
  540. if((int)d.size()<=from) d.resize(from+1);
  541. if((int)d.size()<=to) d.resize(to+1);
  542. d[from].push_back(to);
  543. d[to].push_back(from);
  544. }
  545. bool find(std::vector<int>&ret,int from,std::array<int,2>&to,std::set<int>&ex,int level)
  546. {
  547. if(level--<0)
  548. return false;
  549. ret.push_back(from);
  550. for(int p:d[from]) //在当前路径
  551. {
  552. if(ex.find(p)!=ex.end()) //找过
  553. continue;
  554. if(p==to[0]||p==to[1])//找到
  555. return true;
  556. // {
  557. // ret.push_back(p);
  558. // return true;
  559. // }
  560. }
  561. ex.insert(from);
  562. for(int p:d[from])//在下级路径
  563. {
  564. if(ex.find(p)!=ex.end())//找过
  565. continue;
  566. if(find(ret,p,to,ex,level)) //找到
  567. {
  568. return true;
  569. }
  570. }
  571. ret.pop_back();
  572. return false;
  573. }
  574. bool is_at_path(const point&pt)const
  575. {
  576. return geo.find(pt.x,pt.y)>=0;
  577. }
  578. struct item
  579. {
  580. item(int _vid=-1, int _g=0,int _h=0,std::shared_ptr<item> _parent=nullptr)
  581. :vid(_vid)
  582. ,g(_g)
  583. ,h(_h)
  584. ,parent(_parent)
  585. {
  586. }
  587. int vid=-1,g=0,h=0;
  588. std::shared_ptr<item> parent;
  589. int cost()const {return g+h;}
  590. bool operator< (const item&i) const
  591. {
  592. return vid<i.vid;
  593. }
  594. };
  595. std::vector<point> find2(const point&from,const point&to)
  596. {
  597. int f=geo.find(from.x,from.y);
  598. int t=geo.find(to.x,to.y);
  599. if(f<0 || t<0 || f==t)
  600. return {};
  601. std::set<std::shared_ptr<item>> open_set;
  602. std::set<int> close_set;
  603. open_set.insert(std::make_shared<item>(l[f][0],from.dist(v[l[f][0]]),to.dist(v[l[f][0]])));
  604. open_set.insert(std::make_shared<item>(l[f][1],from.dist(v[l[f][1]]),to.dist(v[l[f][1]])));
  605. std::shared_ptr<item> cur;
  606. while(open_set.size()>0)
  607. {
  608. cur=*open_set.begin();
  609. for(auto&i:open_set)
  610. {
  611. if(i->cost()<cur->cost())
  612. cur=i;
  613. }
  614. open_set.erase(cur);
  615. close_set.insert(cur->vid);
  616. if(cur->vid==l[t][0]||cur->vid==l[t][1])
  617. break;
  618. for(auto vid:d[cur->vid])
  619. {
  620. if(close_set.find(vid)!=close_set.end())
  621. continue;
  622. open_set.insert(std::make_shared<item>(vid,cur->g+v[cur->vid].dist(v[vid]),to.dist(v[vid]),cur));
  623. }
  624. }
  625. if(open_set.size()==0)
  626. {
  627. return {};
  628. }
  629. std::vector<point> rc;
  630. while(cur.get())
  631. {
  632. rc.push_back(v[cur->vid]);
  633. cur=cur->parent;
  634. }
  635. std::reverse(rc.begin(),rc.end());
  636. rc.push_back(to);
  637. auto bp=from;
  638. auto it1=rc.begin();
  639. for(auto it2=it1+1;it1!=rc.end() && it2!=rc.end();it2++)
  640. {
  641. if(line(bp,*it1).dist(*it2)<0.1)
  642. {
  643. *it1=*it2;
  644. }
  645. else
  646. {
  647. bp=*it1++;
  648. *it1=*it2;
  649. }
  650. }
  651. rc.erase(it1,rc.end());
  652. if(rc.size() && from.dist(*rc.begin())<0.1)
  653. {
  654. rc.erase(rc.begin());
  655. }
  656. return std::move(rc);
  657. }
  658. std::vector<point> find(const point&from,const point&to)
  659. {
  660. int f=geo.find(from.x,from.y);
  661. int t=geo.find(to.x,to.y);
  662. if(f<0 || t<0 || f==t)
  663. return {};
  664. #if 0
  665. std::vector<point> rc[2];
  666. for(int i=0;i<2;i++)
  667. {
  668. std::vector<int> ret;
  669. std::set<int> ex({l[f][1-i]});
  670. if(find(ret,l[f][i],l[t],ex,2))
  671. {
  672. for(auto id:ret)
  673. rc[i].push_back(v[id]);
  674. }
  675. }
  676. if(!rc[0].empty() && (rc[1].empty() || rc[0].size() < rc[1].size()))
  677. return std::move(rc[0]);
  678. else if(!rc[1].empty())
  679. return std::move(rc[1]);
  680. #endif
  681. std::vector<int> ret;
  682. int size=v.size();
  683. std::vector<double> mat(size+1,-1);
  684. std::vector<double> trace(size+1,-1);
  685. std::vector<bool> val(size+2,false);
  686. mat[l[t][0]]=to.dist(v[l[t][0]]);
  687. mat[l[t][1]]=to.dist(v[l[t][1]]);
  688. int f0=l[f][0],f1=l[f][1];
  689. for(int i=0;i<2;i++)
  690. {
  691. if(mat[f0]!=-1 && mat[f1]!=-1)
  692. break;
  693. for(int j=0;j<size;j++)
  694. if(mat[j]!=-1)val[j]=true;
  695. for(int j=0;j<size;j++)
  696. {
  697. if(val[j]==false)continue;
  698. for(unsigned int k=0;k<d[j].size();k++)
  699. {
  700. double dist=v[j].dist(v[d[j][k]]);
  701. if(mat[d[j][k]]==-1 || mat[j]+dist < mat[d[j][k]])
  702. {
  703. mat[d[j][k]] = mat[j]+dist;
  704. trace[d[j][k]] = j;
  705. }
  706. }
  707. }
  708. }
  709. mat[f0]=mat[f0]+from.dist(v[l[f][0]]);
  710. mat[f1]=mat[f1]+from.dist(v[l[f][1]]);
  711. std::vector<point> rc;
  712. if(mat[f0]!=-1 && (mat[f1]==-1 || mat[f0] < mat[f1]))
  713. {
  714. int temp=f0;
  715. while(temp!=-1)
  716. {
  717. rc.push_back(v[temp]);
  718. temp=trace[temp];
  719. }
  720. return std::move(rc);
  721. }
  722. else if(mat[f1]!=-1)
  723. {
  724. int temp=f1;
  725. while(temp!=-1)
  726. {
  727. rc.push_back(v[temp]);
  728. temp=trace[temp];
  729. }
  730. return std::move(rc);
  731. }
  732. return {};
  733. }
  734. };
  735. safe_shared_ptr<graph> g_graph(std::make_shared<graph>());
  736. }//namespace
  737. void card_path::init()
  738. {
  739. std::shared_ptr<graph> local_graph=std::make_shared<graph>();
  740. handle_path hp;
  741. sit_list::instance()->accept(hp);
  742. std::vector<base_path> opath=init_path(hp.ret,hp.v);
  743. local_graph->init(hp.v,opath);
  744. g_graph.set(local_graph);
  745. }
  746. card_path&card_path::inst()
  747. {
  748. static card_path path;
  749. return path;
  750. }
  751. std::vector<point> card_path::find_path(const point&from,const point&to)const
  752. {
  753. return g_graph.get()->find2(from,to);
  754. }
  755. bool card_path::is_at_path(const point&pt)const
  756. {
  757. return g_graph.get()->is_at_path(pt);
  758. }
  759. std::vector<line_v> card_path::find_possible_path(const point&from,double dist) const
  760. {
  761. return std::move(g_graph.get()->find_possible_path(from,dist));
  762. }
  763. #ifdef __DEBUG__
  764. void test_at_path(card_path&cp,const point&pt)
  765. {
  766. if(cp.is_at_path(pt))
  767. log_info("test (%.3lf,%.3lf) is-at-path:true\n",pt.x,pt.y);
  768. else
  769. log_info("test (%.3lf,%.3lf) is-at-path:false\n",pt.x,pt.y);
  770. }
  771. void test_find_path(card_path&cp,const point&p1,const point&p2)
  772. {
  773. printf("\nfind-path: from=(%.3lf,%.3lf),to=(%.3lf,%.3lf)\n",p1.x,p1.y,p2.x,p2.y);
  774. std::vector<point> rc=cp.find_path(p1,p2);
  775. for(uint32_t i=0;i<rc.size();i++)
  776. log_info("x=%.3lf,y=%.3lf\n",rc[i].x,rc[i].y);
  777. }
  778. void test_find_poss_path(const card_path&cp ,const point&from)
  779. {
  780. log_info("\nfind-poss_path: from=(%.3lf,%.3lf)\n",from.x,from.y);
  781. std::vector<line_v> rc=cp.find_possible_path(from,3);
  782. for(uint32_t i=0;i<rc.size();i++)
  783. {
  784. log_info("poss path from=(%.3lf,%.3lf) to=(%.3lf,%.3lf)\n", rc[i][0].x,rc[i][0].y, rc[i][1].x,rc[i][1].y);
  785. }
  786. }
  787. int main()
  788. {
  789. // std::unique_ptr<sit_list> sites(new sit_list());
  790. //sit_list::instance()->load("data_reader_antenna.txt","path_tof.txt");
  791. sit_list::instance()->load("1.txt","2.txt");
  792. card_path cp;
  793. cp.init();
  794. #if 0
  795. {
  796. test_at_path(cp,point(4773.104654,-104.0887));
  797. test_at_path(cp,point(4727,-74.8));
  798. test_at_path(cp,point(4727,-75.0));
  799. test_at_path(cp,point(4727,-75.2));
  800. test_at_path(cp,point(4727,-75.8));
  801. test_at_path(cp,point(4727,-90));
  802. test_at_path(cp,point(4727,-89.6));
  803. test_at_path(cp,point(4727,-89.8));
  804. test_at_path(cp,point(4727,-90));
  805. test_at_path(cp,point(4727,-90.2));
  806. test_at_path(cp,point(4727,-90.4));
  807. }
  808. #endif
  809. //test_at_path(cp,point(4726.90,376.85));
  810. test_find_path(cp,point(-227.6,174.9),point(-201.3,175.5));
  811. test_find_path(cp,point(-227.625219,174.945670),point(-170.085722,176.101574));
  812. test_find_path(cp,point(-393.1944,262.5324),point(-366.0754,274.1253));
  813. //test_find_path(cp,point(4637.000000,-75),point(4727,135));
  814. #if 1
  815. {
  816. test_find_path(cp,point(4637.000000,-75),point(4727,135));
  817. test_find_path(cp,point(4637.000000,-75),point(4727,120));
  818. test_find_path(cp,point(4700,-75),point(4727,120));
  819. test_find_path(cp,point(4725,-75),point(4727,135));
  820. test_find_path(cp,point(4727,-89.7),point(6144.3,-523));
  821. }
  822. #endif
  823. #if 0
  824. {
  825. test_find_poss_path(cp, point(4727,-89));
  826. test_find_poss_path(cp, point(4727,-90));
  827. test_find_poss_path(cp, point(4724,-75));
  828. test_find_poss_path(cp, point(4725,-75));
  829. test_find_poss_path(cp, point(4726,-75));
  830. test_find_poss_path(cp, point(4727,-75));
  831. test_find_poss_path(cp, point(4728,-75));
  832. test_find_poss_path(cp, point(4727,-7));
  833. test_find_poss_path(cp, point(4726.6,33));
  834. }
  835. #endif
  836. return 0;
  837. }
  838. #endif