|
@@ -1,293 +0,0 @@
|
|
|
-#ifndef _ANT_LIST_HPP_
|
|
|
-#define _ANT_LIST_HPP_
|
|
|
-
|
|
|
-#include <math.h>
|
|
|
-#include <array>
|
|
|
-#include <deque>
|
|
|
-#include <tuple>
|
|
|
-#include <memory>
|
|
|
-#include <algorithm>
|
|
|
-#include <sstream>
|
|
|
-#include "log.h"
|
|
|
-#include "line.h"
|
|
|
-#include "point.h"
|
|
|
-#include "write-copy.h"
|
|
|
-
|
|
|
-struct path
|
|
|
-{
|
|
|
- std::array<line_v,2> m_line;
|
|
|
- std::array<double,2> m_slope;
|
|
|
- path()
|
|
|
- {
|
|
|
- for(auto &d:m_slope)
|
|
|
- d=0;
|
|
|
- }
|
|
|
- std::string to_str() const
|
|
|
- {
|
|
|
- std::stringstream ss;
|
|
|
- for(int i=0;i<2;i++)
|
|
|
- {
|
|
|
- ss<<"line:" <<m_line[i].to_string()<<"slope:"<<m_slope[i]<< " cos:"<<m_line[i].cos()<<" sin:"<<m_line[i].sin()<<" | ";
|
|
|
- }
|
|
|
- return ss.str();
|
|
|
- }
|
|
|
- bool vaild() const
|
|
|
- {
|
|
|
- return !m_line[0].empty();
|
|
|
- }
|
|
|
- line_v & operator[](int i)
|
|
|
- {
|
|
|
- return m_line[i];
|
|
|
- }
|
|
|
- const line_v & operator[](int i) const
|
|
|
- {
|
|
|
- return m_line[i];
|
|
|
- }
|
|
|
-
|
|
|
-};
|
|
|
-//?
|
|
|
-struct algo_config
|
|
|
-{
|
|
|
- const char*desc;
|
|
|
- int min_msg_cnt;
|
|
|
- int best_msg_cnt;
|
|
|
- double min_wait_time;
|
|
|
- double max_wait_time;
|
|
|
-};
|
|
|
-//
|
|
|
-struct ant :point
|
|
|
-{
|
|
|
- std::vector<path> m_path;
|
|
|
- path & operator[](int i)
|
|
|
- {
|
|
|
- return m_path[i];
|
|
|
- }
|
|
|
- const path & operator[](int i) const
|
|
|
- {
|
|
|
- return m_path[i];
|
|
|
- }
|
|
|
- size_t size() const
|
|
|
- {
|
|
|
- return m_path.size();
|
|
|
- }
|
|
|
- std::vector<point> getsol(const double &dist) const
|
|
|
- {
|
|
|
- std::vector<point> v;
|
|
|
- for(const auto & p : m_path)
|
|
|
- {
|
|
|
- double d = dist;
|
|
|
- if(p.vaild())
|
|
|
- {
|
|
|
- point pt;
|
|
|
- if(dist <= p.m_line[0].length() || (dist > p.m_line[0].length() && p.m_line[1].empty()))
|
|
|
- {
|
|
|
- d += d*p.m_slope[0];
|
|
|
- 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());
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- d -= p.m_line[0].length();
|
|
|
- d += d*p.m_slope[1];
|
|
|
- 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());
|
|
|
- }
|
|
|
- v.push_back(pt);
|
|
|
- std_info("get_sol:x:%.2f,y:%.2f",pt.x,pt.y);
|
|
|
- }
|
|
|
- else
|
|
|
- std_error("%s","ant::getsol empty path..");
|
|
|
- }
|
|
|
- return std::move(v);
|
|
|
- }
|
|
|
-
|
|
|
-};
|
|
|
-
|
|
|
-struct site:point
|
|
|
-{
|
|
|
- static algo_config g_config[];
|
|
|
- int m_algo; //TOF:0,TDOA:1
|
|
|
- int m_num_dims; //1维:0,2维:1,3维:2
|
|
|
- double m_scale = 2.0; // 地图比例尺
|
|
|
-
|
|
|
- point m_position;
|
|
|
- int index()const;
|
|
|
- const algo_config&config()const;
|
|
|
-
|
|
|
- site(int id=-1);
|
|
|
-
|
|
|
- mutable double m_height=1.5;
|
|
|
- int m_id;
|
|
|
- bool m_path_empty;
|
|
|
- std::array<ant,2> m_ant;
|
|
|
-
|
|
|
- mutable double m_ant_dist=0;
|
|
|
- mutable double m_ant_dist_sum_new=0;
|
|
|
- mutable int m_ant_dist_cnt_new=0;
|
|
|
-
|
|
|
- point get_dstp(const point pt) const
|
|
|
- {
|
|
|
- point tmp;
|
|
|
- for(const auto & p : m_ant[0].m_path)
|
|
|
- {
|
|
|
- for(int i=0;i<2;i++)
|
|
|
- {
|
|
|
- if(!p[i].empty())
|
|
|
- {
|
|
|
- if(p[i].contain(pt,0.01))
|
|
|
- {
|
|
|
- //if(i==0)
|
|
|
- // return *this;
|
|
|
- //else
|
|
|
- tmp = p[i][0];
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
- return tmp;
|
|
|
- }
|
|
|
-
|
|
|
- void count_ant_dist(double dist_tof1, double dist_tof2)const
|
|
|
- {
|
|
|
- if(dist_tof1<10 || dist_tof2<10)
|
|
|
- return;
|
|
|
- double dist = fabs(dist_tof1 - dist_tof2);
|
|
|
- if(dist>5)
|
|
|
- return;
|
|
|
- m_ant_dist_sum_new += dist;
|
|
|
- m_ant_dist_cnt_new++;
|
|
|
-
|
|
|
- if(m_ant_dist_cnt_new >= 2500)
|
|
|
- {
|
|
|
- m_ant_dist = m_ant_dist_sum_new / m_ant_dist_cnt_new;
|
|
|
- m_ant_dist_sum_new = 0;
|
|
|
- m_ant_dist_cnt_new = 0;
|
|
|
- }
|
|
|
- }
|
|
|
- void swap()
|
|
|
- {
|
|
|
- auto v0 = m_ant[0].m_path;
|
|
|
- auto v1 = m_ant[1].m_path;
|
|
|
- std::copy (std::begin(v0),std::end(v0),std::back_inserter(m_ant[1].m_path));
|
|
|
- std::copy (std::begin(v1),std::end(v1),std::back_inserter(m_ant[0].m_path));
|
|
|
- }
|
|
|
- double ant_dist()const
|
|
|
- {
|
|
|
- return m_ant[0].dist(m_ant[1]);
|
|
|
- }
|
|
|
-
|
|
|
- bool is_path_empty()const
|
|
|
- {
|
|
|
- return m_path_empty;
|
|
|
- }
|
|
|
-
|
|
|
- bool have_valid_path()const
|
|
|
- {
|
|
|
- return m_id != -1 && ant_dist() > 0.1;
|
|
|
- }
|
|
|
-
|
|
|
- std::string to_string()const
|
|
|
- {
|
|
|
- std::stringstream ss;
|
|
|
- ss<<"site_id:"<<m_id<<"x:"<<x<<" y: "<<y;
|
|
|
- for(const auto a:m_ant)
|
|
|
- {
|
|
|
- ss<<"<";
|
|
|
- for(const auto p:a.m_path)
|
|
|
- {
|
|
|
- ss<<"{"<<p.to_str()<<"}";
|
|
|
- }
|
|
|
- ss<<">";
|
|
|
- }
|
|
|
- return ss.str();
|
|
|
- }
|
|
|
-
|
|
|
- const point&path(int i)const
|
|
|
- {
|
|
|
- static point p;
|
|
|
- if(i>=(int)m_ant[0].m_path.size())
|
|
|
- return p ;
|
|
|
- return m_ant[0].m_path[i].m_line[0][1];
|
|
|
- }
|
|
|
-
|
|
|
- std::vector<point> solving(int ant_id, double dist)const
|
|
|
- {
|
|
|
- const ant &a = m_ant[ant_id];
|
|
|
- if(dist<50 && dist>0)
|
|
|
- {
|
|
|
- if(dist<m_height)
|
|
|
- {
|
|
|
- m_height=dist;
|
|
|
- dist=0;
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- dist=sqrt(dist*dist-m_height*m_height);
|
|
|
- }
|
|
|
- }
|
|
|
- return std::move(a.getsol(dist));
|
|
|
- }
|
|
|
- ant operator[](int i)
|
|
|
- {
|
|
|
- return m_ant[i];
|
|
|
- }
|
|
|
- const ant operator[](int i) const
|
|
|
- {
|
|
|
- return m_ant[i];
|
|
|
- }
|
|
|
-};
|
|
|
-
|
|
|
-
|
|
|
-struct sit_list:single_base<sit_list,int,std::shared_ptr<site>>
|
|
|
-{
|
|
|
-
|
|
|
- const site& operator[](int id) const;
|
|
|
- static void load(const char*ant_file,const char*path_file)
|
|
|
- {
|
|
|
- read_sit_list(ant_file);
|
|
|
- read_ant_path(path_file);
|
|
|
- }
|
|
|
-
|
|
|
- static void load_from_db()
|
|
|
- {
|
|
|
- load("data_reader_antenna.txt","path_tof.txt");
|
|
|
- }
|
|
|
-
|
|
|
- static void read_sit_list(const char*fname);
|
|
|
- static void read_ant_path(const char*fname);
|
|
|
-};
|
|
|
-
|
|
|
-struct loc_message
|
|
|
-{
|
|
|
- site m_sit;
|
|
|
- uint64_t m_num_ticks; //tof时间片m_tof或tdoa相对root时间
|
|
|
- uint64_t m_loc_time;
|
|
|
- uint32_t m_card_id;
|
|
|
- int32_t m_card_ct;
|
|
|
- int8_t m_card_type;
|
|
|
- int8_t m_ant_id;
|
|
|
- int16_t m_rav;
|
|
|
- int16_t m_acc;
|
|
|
- uint16_t m_sync_ct;
|
|
|
- uint16_t m_rssi;
|
|
|
-
|
|
|
-
|
|
|
- loc_message();
|
|
|
- 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)
|
|
|
- :m_sit(s)
|
|
|
- ,m_num_ticks(num_ticks)
|
|
|
- ,m_loc_time(timestamp)
|
|
|
- ,m_card_id(cardid)
|
|
|
- ,m_card_ct(ct)
|
|
|
- ,m_card_type(type)
|
|
|
- ,m_ant_id(antid)
|
|
|
- ,m_rav(rav)
|
|
|
- ,m_acc(acc)
|
|
|
- ,m_sync_ct(sync_ct)
|
|
|
- ,m_rssi(rssi)
|
|
|
- {}
|
|
|
- int tool_index()const;
|
|
|
-};
|
|
|
-
|
|
|
-#endif
|
|
|
-
|