ant.h 6.2 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290
  1. #ifndef _ANT_LIST_HPP_
  2. #define _ANT_LIST_HPP_
  3. #include <math.h>
  4. #include <array>
  5. #include <deque>
  6. #include <tuple>
  7. #include <memory>
  8. #include <algorithm>
  9. #include <sstream>
  10. #include "log.h"
  11. #include "line.h"
  12. #include "point.h"
  13. struct path
  14. {
  15. std::array<line_v,2> m_line;
  16. std::array<double,2> m_slope;
  17. path()
  18. {
  19. for(auto &d:m_slope)
  20. d=0;
  21. }
  22. std::string to_str() const
  23. {
  24. std::stringstream ss;
  25. for(int i=0;i<2;i++)
  26. {
  27. ss<<"line:" <<m_line[i].to_string()<<"slope:"<<m_slope[i]<< " cos:"<<m_line[i].cos()<<" sin:"<<m_line[i].sin()<<" | ";
  28. }
  29. return ss.str();
  30. }
  31. bool vaild() const
  32. {
  33. return !m_line[0].empty();
  34. }
  35. line_v & operator[](int i)
  36. {
  37. return m_line[i];
  38. }
  39. const line_v & operator[](int i) const
  40. {
  41. return m_line[i];
  42. }
  43. };
  44. //?
  45. struct algo_config
  46. {
  47. const char*desc;
  48. int min_msg_cnt;
  49. int best_msg_cnt;
  50. double min_wait_time;
  51. double max_wait_time;
  52. };
  53. //
  54. struct ant :point
  55. {
  56. std::vector<path> m_path;
  57. path & operator[](int i)
  58. {
  59. return m_path[i];
  60. }
  61. const path & operator[](int i) const
  62. {
  63. return m_path[i];
  64. }
  65. size_t size() const
  66. {
  67. return m_path.size();
  68. }
  69. std::vector<point> getsol(const double &dist) const
  70. {
  71. std::vector<point> v;
  72. for(const auto & p : m_path)
  73. {
  74. double d = dist;
  75. if(p.vaild())
  76. {
  77. point pt;
  78. if(dist <= p.m_line[0].length() || (dist > p.m_line[0].length() && p.m_line[1].empty()))
  79. {
  80. d += d*p.m_slope[0];
  81. pt = point(p.m_line[0][0].x + d*p.m_line[0].cos() , p.m_line[0][0].y + d*p.m_line[0].sin());
  82. }
  83. else
  84. {
  85. d -= p.m_line[0].length();
  86. d += d*p.m_slope[1];
  87. pt = point(p.m_line[1][0].x+d*p.m_line[1].cos(),p.m_line[1][0].y+d*p.m_line[1].sin());
  88. }
  89. v.push_back(pt);
  90. std_info("get_sol:x:%.2f,y:%.2f",pt.x,pt.y);
  91. }
  92. else
  93. std_error("%s","ant::getsol empty path..");
  94. }
  95. return std::move(v);
  96. }
  97. };
  98. struct site:point
  99. {
  100. static algo_config g_config[];
  101. int m_algo; //TOF:0,TDOA:1
  102. int m_num_dims; //1维:0,2维:1,3维:2
  103. double m_scale = 2.0; // 地图比例尺
  104. point m_position;
  105. int index()const;
  106. const algo_config&config()const;
  107. site(int id=-1);
  108. mutable double m_height=1.5;
  109. int m_id;
  110. bool m_path_empty;
  111. std::array<ant,2> m_ant;
  112. mutable double m_ant_dist=0;
  113. mutable double m_ant_dist_sum_new=0;
  114. mutable int m_ant_dist_cnt_new=0;
  115. point get_dstp(const point pt) const
  116. {
  117. point tmp;
  118. for(const auto & p : m_ant[0].m_path)
  119. {
  120. for(int i=0;i<2;i++)
  121. {
  122. if(!p[i].empty())
  123. {
  124. if(p[i].contain(pt,0.01))
  125. {
  126. //if(i==0)
  127. // return *this;
  128. //else
  129. tmp = p[i][0];
  130. }
  131. }
  132. }
  133. }
  134. return tmp;
  135. }
  136. void count_ant_dist(double dist_tof1, double dist_tof2)const
  137. {
  138. if(dist_tof1<10 || dist_tof2<10)
  139. return;
  140. double dist = fabs(dist_tof1 - dist_tof2);
  141. if(dist>5)
  142. return;
  143. m_ant_dist_sum_new += dist;
  144. m_ant_dist_cnt_new++;
  145. if(m_ant_dist_cnt_new >= 2500)
  146. {
  147. m_ant_dist = m_ant_dist_sum_new / m_ant_dist_cnt_new;
  148. m_ant_dist_sum_new = 0;
  149. m_ant_dist_cnt_new = 0;
  150. }
  151. }
  152. void swap()
  153. {
  154. auto v0 = m_ant[0].m_path;
  155. auto v1 = m_ant[1].m_path;
  156. std::copy (std::begin(v0),std::end(v0),std::back_inserter(m_ant[1].m_path));
  157. std::copy (std::begin(v1),std::end(v1),std::back_inserter(m_ant[0].m_path));
  158. }
  159. double ant_dist()const
  160. {
  161. return m_ant[0].dist(m_ant[1]);
  162. }
  163. bool is_path_empty()const
  164. {
  165. return m_path_empty;
  166. }
  167. bool have_valid_path()const
  168. {
  169. return m_id != -1 && ant_dist() > 0.1;
  170. }
  171. std::string to_string()const
  172. {
  173. std::stringstream ss;
  174. ss<<"site_id:"<<m_id<<"x:"<<x<<" y: "<<y;
  175. for(const auto a:m_ant)
  176. {
  177. ss<<"<";
  178. for(const auto p:a.m_path)
  179. {
  180. ss<<"{"<<p.to_str()<<"}";
  181. }
  182. ss<<">";
  183. }
  184. return ss.str();
  185. }
  186. const point&path(int i)const
  187. {
  188. static point p;
  189. if(i>=(int)m_ant[0].m_path.size())
  190. return p ;
  191. return m_ant[0].m_path[i].m_line[0][1];
  192. }
  193. std::vector<point> solving(int ant_id, double dist)const
  194. {
  195. const ant &a = m_ant[ant_id];
  196. if(dist<50 && dist>0)
  197. {
  198. if(dist<m_height)
  199. {
  200. m_height=dist;
  201. dist=0;
  202. }
  203. else
  204. {
  205. dist=sqrt(dist*dist-m_height*m_height);
  206. }
  207. }
  208. return std::move(a.getsol(dist));
  209. }
  210. ant operator[](int i)
  211. {
  212. return m_ant[i];
  213. }
  214. const ant operator[](int i) const
  215. {
  216. return m_ant[i];
  217. }
  218. };
  219. struct sit_list
  220. {
  221. std::array<site,1000> m_list;
  222. const site& operator[](int id) const;
  223. void load(const char*ant_file,const char*path_file)
  224. {
  225. read_sit_list(ant_file);
  226. read_ant_path(path_file);
  227. }
  228. void load_from_db()
  229. {
  230. load("data_reader_antenna.txt","path_tof.txt");
  231. }
  232. void read_sit_list(const char*fname);
  233. void read_ant_path(const char*fname);
  234. static sit_list*instance();
  235. };
  236. struct loc_message
  237. {
  238. site m_sit;
  239. uint64_t m_num_ticks; //tof时间片m_tof或tdoa相对root时间
  240. uint64_t m_loc_time;
  241. uint32_t m_card_id;
  242. int32_t m_card_ct;
  243. int8_t m_card_type;
  244. int8_t m_ant_id;
  245. int16_t m_rav;
  246. int16_t m_acc;
  247. uint16_t m_sync_ct;
  248. uint16_t m_rssi;
  249. loc_message();
  250. loc_message(site s,uint64_t num_ticks,uint64_t timestamp,uint32_t cardid,int32_t ct,int8_t type,int8_t antid,int16_t rav,int16_t acc,uint16_t sync_ct,uint16_t rssi)
  251. :m_sit(s)
  252. ,m_num_ticks(num_ticks)
  253. ,m_loc_time(timestamp)
  254. ,m_card_id(cardid)
  255. ,m_card_ct(ct)
  256. ,m_card_type(type)
  257. ,m_ant_id(antid)
  258. ,m_rav(rav)
  259. ,m_acc(acc)
  260. ,m_sync_ct(sync_ct)
  261. ,m_rssi(rssi)
  262. {}
  263. int tool_index()const;
  264. };
  265. #endif