1
0

card_path.cpp 19 KB

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