|
@@ -1,484 +0,0 @@
|
|
|
-#include "select_tool.h"
|
|
|
-#include "ant.h"
|
|
|
-#include "card_path.h"
|
|
|
-#include "log.h"
|
|
|
-void select_point_object::att_initiate()
|
|
|
-{
|
|
|
- m_fitk.add_tool(30,5,5);
|
|
|
- m_fitk.add_tool(30,5,6);
|
|
|
- m_fitk.add_tool(30,10,10);
|
|
|
- m_fitk.add_tool(30,10,15);
|
|
|
-
|
|
|
- m_fitk.add_tool(20,5,6);
|
|
|
- m_fitk.add_tool(30,4,6);
|
|
|
- m_fitk.add_tool(40,4,6);
|
|
|
-
|
|
|
-
|
|
|
- m_fita.add_tool(15,5,5);
|
|
|
- m_begin=m_last=0;
|
|
|
-
|
|
|
-}
|
|
|
-loc_point select_point_object::select_solution_impl(const std::vector<point> vp,const std::vector<loc_message>&lm)
|
|
|
-{
|
|
|
- bool flag=false;
|
|
|
- loc_point lp;
|
|
|
- if(vp.size()==4)
|
|
|
- {
|
|
|
- m_d.grow().reset().set_source(lm[0],lm[1]);
|
|
|
- flag = true;
|
|
|
- }
|
|
|
- if(vp.size()==2 && lm[0].m_card_ct - last_ct() == 1)
|
|
|
- {
|
|
|
- m_d.grow().reset().set_source(lm[0]);
|
|
|
- flag = true;
|
|
|
- }
|
|
|
- if(flag)
|
|
|
- {
|
|
|
- if(!select_solution(vp,lm[0].m_sit.get(),lp))
|
|
|
- {
|
|
|
- m_cur_fit.reset();
|
|
|
- return lp;
|
|
|
- }
|
|
|
- }
|
|
|
- m_ct = lm[0].m_card_ct;
|
|
|
- return lp;
|
|
|
-}
|
|
|
-
|
|
|
-int select_point_object::find_last(int start)
|
|
|
-{
|
|
|
- for(int i=start,len=m_d.size();i<len;i++)
|
|
|
- {
|
|
|
- if(m_d(i).cl()>0)
|
|
|
- return i;
|
|
|
- }
|
|
|
-
|
|
|
- return -1;
|
|
|
-}
|
|
|
-
|
|
|
-int select_point_object::find_first(int start)
|
|
|
-{
|
|
|
- for(int i=start,len=m_d.size();i<len;i++)
|
|
|
- {
|
|
|
- if(m_d[i].cl()>0)
|
|
|
- return i;
|
|
|
- }
|
|
|
-
|
|
|
- return -1;
|
|
|
-}
|
|
|
-bool select_point_object::filter_by_fit(loc_point & c,const std::vector<point> & vp,const double scale)
|
|
|
-{
|
|
|
- fit_result * fit = get_best_fit();
|
|
|
- if(fit==nullptr || m_begin==nullptr || fit->ke>2)
|
|
|
- return false;
|
|
|
- loc_point &f = *m_begin;
|
|
|
- std::array<solpoint,4> v;
|
|
|
- int cnt = vp.size();
|
|
|
- for(int i=0;i<cnt;i++)
|
|
|
- {
|
|
|
- v[i].set_sol(vp[i],fabs(fit->testk(c.m_time/1000.,f.dist_direct(vp[i]))));
|
|
|
- }
|
|
|
- std::sort(&v[0],&v[0]+cnt);
|
|
|
- double a = getA(fit,scale,v[1].score()-v[0].score());
|
|
|
- if(a<0)
|
|
|
- return false;
|
|
|
- if(!filter_by_acc(c,v,a))
|
|
|
- {
|
|
|
- c.set_cl(0);
|
|
|
- return true; //false?
|
|
|
- }
|
|
|
- reset_fit(vp[0].dist(v[0]));
|
|
|
- c[0]=v[0];
|
|
|
- c[1]=v[1];
|
|
|
- return true;
|
|
|
-
|
|
|
-}
|
|
|
-bool select_point_object::filter_by_acc(loc_point&c,std::array<solpoint,4>&v,double a)
|
|
|
-{
|
|
|
- if(!m_last->is_same_site(c))
|
|
|
- return true;
|
|
|
-
|
|
|
- double td=m_last->time_off(c);
|
|
|
-
|
|
|
- if(v[0].score()>a*td*td)
|
|
|
- return false;
|
|
|
-
|
|
|
- return true;
|
|
|
-}
|
|
|
-
|
|
|
-
|
|
|
-void select_point_object::select_one_ant(loc_point &c,const std::vector<point> & vp)
|
|
|
-{
|
|
|
- int last=find_last(1);
|
|
|
- if(last>0)
|
|
|
- {
|
|
|
- loc_point&p=m_d(last);
|
|
|
-
|
|
|
- int cnt=vp.size();
|
|
|
- std::array<solpoint,4> res;
|
|
|
-
|
|
|
- //find the shortest dis
|
|
|
- for(int i=0;i<cnt;i++)
|
|
|
- res[i].set_sol(vp[i],vp[i].dist(p[0]));
|
|
|
-
|
|
|
- std::sort(&res[0],&res[0]+cnt);
|
|
|
-
|
|
|
- c[1].set(res[1]);
|
|
|
- c[0].set(res[0]);
|
|
|
- c.inc_cl(5);
|
|
|
- }
|
|
|
-}
|
|
|
-
|
|
|
-point select_point_object::select_solution0(std::vector<point> &tvp,const double scale)
|
|
|
-{
|
|
|
- //log_info("lemon test 2 :cardid:%d sit:%f sitid:%d",m_d(0).m_cid,scale,m_d(0).m_sid);
|
|
|
- loc_point&c=m_d(0);
|
|
|
- if(m_d.size()==1)
|
|
|
- {
|
|
|
- //first point ,only accpet two ants good data.
|
|
|
- if(c.cl()>0 && tvp.size()<4)
|
|
|
- {
|
|
|
- c[1]=tvp[1];
|
|
|
- return c[0]=tvp[0];
|
|
|
- }
|
|
|
-
|
|
|
- m_d.skip(1);
|
|
|
- return point(0,0);
|
|
|
- }
|
|
|
- select_solution1(c,tvp,scale);
|
|
|
- return c[0];
|
|
|
-}
|
|
|
-bool select_point_object::select_solution(const std::vector<point> &vp,const site*sit,loc_point &p)
|
|
|
-{
|
|
|
- //log_info("lemon test 1 :cardid:%d sit:%d sitid:%d",m_d(0).m_cid,sit->m_id,m_d(0).m_sid);
|
|
|
- remove_history();
|
|
|
- std::vector<point> tvp(vp.begin(),vp.end());
|
|
|
- if(vp.size()==4)
|
|
|
- {
|
|
|
- int c=0;
|
|
|
- std::array<solpoint,4> res;
|
|
|
- for(int i=0;i<2;i++)
|
|
|
- {
|
|
|
- int x = i+2;
|
|
|
- double d=vp[i].dist(vp[x]);
|
|
|
- if(d<sit->ant_dist()*3)
|
|
|
- {
|
|
|
- res[c++].set_sol(vp[i].middle(vp[x]),d);
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- res[c++].set_sol(vp[i]);
|
|
|
- res[c++].set_sol(vp[x]);
|
|
|
- }
|
|
|
- }
|
|
|
-
|
|
|
- std::sort(&res[0],&res[0]+c);
|
|
|
-
|
|
|
- tvp.clear();
|
|
|
- for(int i=0;i<c;i++)
|
|
|
- tvp.push_back(res[i]);
|
|
|
-
|
|
|
- m_d(0).inc_cl(10);
|
|
|
- }
|
|
|
- point pt = select_solution0(tvp,sit->m_scale);
|
|
|
- if(pt.empty() || m_d.empty())
|
|
|
- return false;
|
|
|
- m_d(0).set(pt);
|
|
|
- if(!m_d(0).empty())
|
|
|
- m_d(0).m_dist=sit->dist_direct(pt);
|
|
|
- else
|
|
|
- {
|
|
|
- m_d(0).set_cl(0);
|
|
|
- if(m_last) m_d(0).m_dist=0.01*m_last->m_dist>0?1:-1;
|
|
|
- }
|
|
|
-
|
|
|
- m_d(0).m_dist1=sit->dist_direct(pt);
|
|
|
- //
|
|
|
- //std_info("revise_by_history:::%llu",m_d(0).m_time);
|
|
|
- bool fg=revise_by_history(pt,sit,m_d(0).m_time);
|
|
|
- if(!fg)
|
|
|
- {
|
|
|
- log_warn("out of site path:t=%ld,sit=%d,card_id=%d,ct=%d,tof1=%d,tof2=%d,pt=(%f,%f)\n", m_d(0).m_time, m_d(0).m_sid,m_d(0).m_cid,m_d(0).m_ct, m_d(0).m_tof[0], m_d(0).m_tof[1], pt.x, pt.y);
|
|
|
-
|
|
|
- }
|
|
|
-
|
|
|
- if(!card_path::inst().is_at_path(pt))
|
|
|
- {
|
|
|
- m_d(0).set_cl(0);
|
|
|
-
|
|
|
- log_warn("out of path:t=%ld,sit=%d,card_id=%d,ct=%d,tof1=%d,tof2=%d,pt=(%f,%f)\n", m_d(0).m_time, m_d(0).m_sid,m_d(0).m_cid,m_d(0).m_ct, m_d(0).m_tof[0], m_d(0).m_tof[1], pt.x, pt.y);
|
|
|
- return false;
|
|
|
- }
|
|
|
-
|
|
|
-
|
|
|
- m_last=&m_d(0);
|
|
|
-
|
|
|
- if(m_line.empty() && m_d.size()>=2 && !make_line())
|
|
|
- return false;
|
|
|
- if(!m_line.contain(m_d(0),0.01) || (m_begin && !m_line.contain(m_begin->m_sol[0], 0.01)))
|
|
|
- {
|
|
|
- m_fitk.reset_data();
|
|
|
- m_fita.reset_data();
|
|
|
- m_begin=m_last=nullptr;
|
|
|
-
|
|
|
- int i0=find_last(1);
|
|
|
- if(i0==-1)
|
|
|
- return false;
|
|
|
-
|
|
|
- std::vector<point> path=card_path::inst().find_path(m_d(i0),m_d(0));
|
|
|
-
|
|
|
- m_d.skip(m_d.size()-i0);
|
|
|
-
|
|
|
- if(path.empty())
|
|
|
- {
|
|
|
- m_line.clear();
|
|
|
- return false;
|
|
|
- }
|
|
|
-
|
|
|
- m_line.set(path.back(),m_d(0));
|
|
|
- }
|
|
|
- if(!m_begin)
|
|
|
- {
|
|
|
- int idx=find_first(0);
|
|
|
- if(idx>=0) m_begin=&m_d[idx];
|
|
|
- }
|
|
|
-
|
|
|
- if(!m_last)
|
|
|
- {
|
|
|
- int idx=find_last(0);
|
|
|
- if(idx>=0) m_last=&m_d(idx);
|
|
|
- }
|
|
|
-
|
|
|
- if(m_begin && m_d(0).cl())
|
|
|
- {
|
|
|
- //implemented by li
|
|
|
- int lastID1=find_last(1);
|
|
|
- int lastID2=-1;
|
|
|
- if(lastID1!=-1)
|
|
|
- lastID2=find_last(lastID1+1);
|
|
|
- if(lastID2!=-1)
|
|
|
- {
|
|
|
- double t0=m_last->m_time/1000., t1=m_d(lastID1).m_time/1000., t2=m_d(lastID2).m_time/1000.;
|
|
|
- double d0=m_begin->loc_dist(*m_last), d1=m_begin->loc_dist(m_d(lastID1)), d2=m_begin->loc_dist(m_d(lastID2));
|
|
|
- double k1=(d1-d0)/(t1-t0), k2=(d2-d1)/(t2-t1);
|
|
|
- if(t0-t1<5 && t1-t2<5 && fabs(k2-k1)<0.5)
|
|
|
- {
|
|
|
- double tbegin = t0-3;
|
|
|
- while(tbegin<t1+0.5)
|
|
|
- tbegin=tbegin+1;
|
|
|
- double tk=(d0-d1)/(t0-t1), tb=d1-tk*t1; //d1+(d0-d1)/(t0-t1)*(t-t1)
|
|
|
- for(double ti=tbegin;ti<t0;ti=ti+1)
|
|
|
- {
|
|
|
- //std_info("add ..seelct");
|
|
|
- m_fitk.add(ti, tk*ti+tb);
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
- m_fitk.add(m_last->m_time/1000.,m_begin->loc_dist(*m_last));
|
|
|
-
|
|
|
- if(m_d.size()>1)
|
|
|
- {
|
|
|
- int pre=find_last(1);
|
|
|
- if(pre>0)
|
|
|
- {
|
|
|
- m_fita.add(m_d(0).m_time/1000.,m_begin->loc_dist(m_d(0))-m_begin->loc_dist(m_d(pre)));
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
-
|
|
|
- p.set(pt);
|
|
|
- save_k();
|
|
|
- p.m_useless=fg;
|
|
|
-
|
|
|
- return true;
|
|
|
-
|
|
|
-}
|
|
|
-
|
|
|
-bool select_point_object::make_line()
|
|
|
-{
|
|
|
- int i0=-1,i1=-1;
|
|
|
-
|
|
|
- if(-1==(i0=find_last(0)))
|
|
|
- return false;
|
|
|
-
|
|
|
- if(-1==(i1=find_last(i0+1)))
|
|
|
- return false;
|
|
|
-
|
|
|
- m_line.set(m_d(i0),m_d(i1));
|
|
|
-
|
|
|
- return true;
|
|
|
-}
|
|
|
-
|
|
|
-void select_point_object::remove_history()
|
|
|
-{
|
|
|
- loc_point&b=m_d(0);
|
|
|
-
|
|
|
- if(m_d.size()>120 || (m_d.size()>2 && m_d(1).time_off(b)>max_histime))
|
|
|
- {
|
|
|
- m_d.skip_if([&b,this](loc_point&p){ return p.time_off(b)>max_histime;});
|
|
|
- m_fitk.reset_data();
|
|
|
- m_fita.reset_data();
|
|
|
- m_cur_fit.reset();
|
|
|
- m_begin=m_last=nullptr;
|
|
|
- int idx=find_first(0);
|
|
|
- if(idx<0)
|
|
|
- return;
|
|
|
- m_begin=&m_d[idx];
|
|
|
- idx=find_last(1);
|
|
|
- m_last=&m_d(idx);
|
|
|
-
|
|
|
- double dist=0,dist2=0;
|
|
|
- for(int len=std::min(15,m_d.size()-1),i=len;i>0;i--)
|
|
|
- {
|
|
|
- if(!m_d(i).cl())
|
|
|
- continue;
|
|
|
- int lastID1=find_last(i+1);
|
|
|
- int lastID2=-1;
|
|
|
- if(lastID1!=-1)
|
|
|
- lastID2=find_last(lastID1+1);
|
|
|
- if(lastID2!=-1)
|
|
|
- {
|
|
|
- double t0=m_d(i).m_time/1000., t1=m_d(lastID1).m_time/1000., t2=m_d(lastID2).m_time/1000.;
|
|
|
- double d0=m_begin->loc_dist(m_d(i)[0]), d1=m_begin->loc_dist(m_d(lastID1)[0]),
|
|
|
- d2=m_begin->loc_dist(m_d(lastID2)[0]);
|
|
|
- double k1=(d1-d0)/(t1-t0), k2=(d2-d1)/(t2-t1);
|
|
|
- if(t0-t1<5 && t1-t2<5 && fabs(k2-k1)<0.5)
|
|
|
- {
|
|
|
- double tbegin = t0-3;
|
|
|
- while(tbegin<t1+0.5)
|
|
|
- tbegin=tbegin+1;
|
|
|
- double tk=(d0-d1)/(t0-t1), tb=d1-tk*t1; //d1+(d0-d1)/(t0-t1)*(t-t1)
|
|
|
- for(double ti=tbegin;ti<t0;ti=ti+1)
|
|
|
- {
|
|
|
- //std_info("add remove..");
|
|
|
- m_fitk.add(ti, tk*ti+tb);
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
-
|
|
|
- dist=m_begin->loc_dist(m_d(i)[0]);
|
|
|
- //std_info("add remove..");
|
|
|
- m_fitk.add(m_d(i).m_time/1000.,dist);
|
|
|
-
|
|
|
- if(i==len)
|
|
|
- continue;
|
|
|
-
|
|
|
- m_fita.add(m_d(i).m_time/1000.,dist-dist2);
|
|
|
- dist2=dist;
|
|
|
- }
|
|
|
- save_k();
|
|
|
- }
|
|
|
-
|
|
|
-}
|
|
|
-void select_point_object::save_k()
|
|
|
-{
|
|
|
- const fit_result*fk=best_fit_raw(0,4);
|
|
|
- if(!fk)
|
|
|
- {
|
|
|
- m_cur_fit.reset();
|
|
|
- return;
|
|
|
- }
|
|
|
- m_cur_fit=*fk;
|
|
|
-
|
|
|
- fit_result&r=m_cur_fit;
|
|
|
- card_fit*fa=&m_fita[0];
|
|
|
- if(fa->is_valid() && fa->ke<0.1 && fk->k*fa->k<0)
|
|
|
- {
|
|
|
- double dk=fa->k*fa->num_point;
|
|
|
- r.ka=fa->k;
|
|
|
- if((fk->k+dk)*fk->k<0)
|
|
|
- r.k=0;
|
|
|
- else
|
|
|
- r.k+=dk;
|
|
|
-
|
|
|
- double y=fk->k*m_fitk(0).x+fk->kb;
|
|
|
- r.kb=y-m_fitk(0).x*r.k;
|
|
|
- }
|
|
|
-
|
|
|
-
|
|
|
-}
|
|
|
-fit_result* select_point_object::best_fit_raw(int num_point,int start,int last)
|
|
|
-{
|
|
|
- card_fit*fit=nullptr;
|
|
|
-
|
|
|
- start=std::max(start,0);
|
|
|
- last =std::min(last,m_fitk.tool_size());
|
|
|
- if(last==-1)
|
|
|
- last=m_fitk.tool_size();
|
|
|
- //std_info("best_fit_raw :%d:%d",start,last);
|
|
|
- for(int i=start;i<last;i++)
|
|
|
- {
|
|
|
- if(!m_fitk[i].is_valid())
|
|
|
- continue;
|
|
|
-
|
|
|
- if(m_fitk[i].num_point<num_point)
|
|
|
- continue;
|
|
|
-
|
|
|
- if(fit==nullptr)
|
|
|
- {
|
|
|
- fit=&m_fitk[i];
|
|
|
- continue;
|
|
|
- }
|
|
|
-
|
|
|
- if(fit->ke>m_fitk[i].ke)
|
|
|
- {
|
|
|
- fit=&m_fitk[i];
|
|
|
- }
|
|
|
- }
|
|
|
-
|
|
|
- return fit;
|
|
|
-}
|
|
|
-
|
|
|
-select_tool::~select_tool()
|
|
|
-{
|
|
|
- if(m_spo !=nullptr)
|
|
|
- delete m_spo;
|
|
|
-}
|
|
|
-
|
|
|
-loc_point select_tool_person_1::select_solution(const std::vector<point> vp,const std::vector<loc_message>&lm)
|
|
|
-{
|
|
|
- loc_point lp;
|
|
|
- //select point.
|
|
|
- if(lm[0].tool_index() == 0)
|
|
|
- {
|
|
|
- if(m_spo==nullptr)
|
|
|
- m_spo = new person_point_filter(this);
|
|
|
- lp=m_spo->select_solution_impl(vp,lm);
|
|
|
- }
|
|
|
- else if (lm[0].tool_index() == 2)
|
|
|
- {
|
|
|
- //for now..
|
|
|
- //m_spo = new person_point_filter();
|
|
|
- lp=m_spo->select_solution_impl(vp,lm);
|
|
|
- }
|
|
|
- else
|
|
|
- {}
|
|
|
- return lp;
|
|
|
-}
|
|
|
-
|
|
|
-loc_point select_tool_car_1::select_solution(const std::vector<point> vp,const std::vector<loc_message>&lm)
|
|
|
-{
|
|
|
- loc_point lp;
|
|
|
- //select point.
|
|
|
- if(lm[0].tool_index() == 0)
|
|
|
- {
|
|
|
- if(m_spo==nullptr)
|
|
|
- m_spo = new car_point_filter(this);
|
|
|
- lp=m_spo->select_solution_impl(vp,lm);
|
|
|
- }
|
|
|
- else if (lm[0].tool_index() == 2)
|
|
|
- {
|
|
|
- //for now..
|
|
|
- //m_spo = new car_point_filter();
|
|
|
- lp=m_spo->select_solution_impl(vp,lm);
|
|
|
- }
|
|
|
- else
|
|
|
- {}
|
|
|
- return lp;
|
|
|
-}
|
|
|
-
|
|
|
-
|
|
|
-select_tool_manage * select_tool_manage::instance()
|
|
|
-{
|
|
|
- static select_tool_manage stm;
|
|
|
- return &stm;
|
|
|
-}
|