#ifndef __SELECT_TOOL__H
#define __SELECT_TOOL__H
#include <mutex>
#include "loc_point.h"
#include "line.h"
#include <memory>
#include "ant.h"
#include "card_path.h"
#include "loc_message.h"
#include <zexception.h>
struct solpoint:point
{
solpoint()
:m_score(100)
{
}
double m_score;
bool operator<(const solpoint&p)const
{
return m_score<p.m_score;
}
void set_sol(const point&p,double score=100)
{
set(p);
this->m_score=score;
}
double score()const
{
return m_score;
}
};
struct push_data_point:point
{
push_data_point()
:ct(0)
,valid(false)
,stop_cnt(0)
{
}
point dstp;
int ct;
bool valid; // if valid
int stop_cnt; // if calculate speed
loc_point lp;
};
class select_point_object;
struct select_tool
{
std::mutex m_mtx;
zlist<push_data_point,16> m_push_list;
select_point_object *m_spo=nullptr;
virtual loc_point select_solution(const std::vector<point> p,const std::vector<loc_message>&lm)=0;
push_data_point getDpt()
{
push_data_point dpt;
std::lock_guard<std::mutex> lock(m_mtx);
if (m_push_list.empty()) //empty
{
return dpt;
}
else if(m_push_list.size()==1) //size=1
{
dpt = m_push_list[0];
m_push_list[0].valid=false;
if(m_push_list[0].stop_cnt>5)m_push_list[0].stop_cnt=5;
if(m_push_list[0].stop_cnt>0)m_push_list[0].stop_cnt--;
}
else{ //size>1
dpt = m_push_list[0];
m_push_list.skip(1);
}
return dpt;
}
virtual ~select_tool();
};
//--------------person------solution one--------
struct select_tool_person_1:select_tool
{
virtual loc_point select_solution(const std::vector<point> p,const std::vector<loc_message>&lm);
~select_tool_person_1()
{
}
};
struct select_tool_person_2:select_tool
{
virtual loc_point select_solution(const std::vector<point> p,const std::vector<loc_message>&lm)
{
loc_point lp;
return lp;
}
};
//----------------------car----------
struct select_tool_car_1:select_tool
{
virtual loc_point select_solution(const std::vector<point> p,const std::vector<loc_message>&lm);
};
//---------------------drivingfaceCar
struct select_tool_drivingface_car_1:select_tool
{
virtual loc_point select_solution(const std::vector<point> p,const std::vector<loc_message>&lm)
{
loc_point lp;
return lp;
}
};
//----------------------------------------
struct select_point_object
{
select_tool * m_owner;
public:
select_point_object(select_tool * owner)
:m_owner(owner)
,m_ct(-10)
{
att_initiate();
}
inline int last_ct(){return m_ct;}
public:
const static int max_histime=60; //±£Áô×îºó60sµÄÊý¾Ý
zlist<loc_point,128> m_d;
line m_line;
int m_ct = -10;
fit_batch m_fitk,m_fita;
fit_result m_cur_fit;
loc_point* m_begin;
loc_point* m_last;
public:
int find_last(int start);
int find_first(int start);
void save_k();
void att_initiate();
void remove_history();
bool make_line();
point select_solution0(std::vector<point> &vp,const double scale);
bool select_solution(const std::vector<point> &vp,const site*sit,loc_point &p);
loc_point select_solution_impl(const std::vector<point> p,const std::vector<loc_message>&lm);
fit_result* best_fit_raw(int num_point=0,int start=0,int end=-1);
bool filter_by_acc(loc_point&c,std::array<solpoint,4>&v,double a);
bool filter_by_fit(loc_point & c,const std::vector<point> & vp,const double scale);
void select_one_ant(loc_point &c,const std::vector<point> & vp);
public:
virtual void select_solution1(loc_point &c,const std::vector<point> &vp,const double scale)=0;
virtual fit_result * get_best_fit()=0;
virtual double getA(const fit_result * fit,const double scale,const double dt)=0;
virtual void reset_fit(double d)=0;
virtual bool revise_by_history(point & pt, const site*sit, int64_t m_time)=0;
virtual ~select_point_object(){}
};
struct person_point_filter:select_point_object
{
person_point_filter(select_tool * owner)
:select_point_object(owner)
,m_filter_man_count(0)
{}
int m_filter_man_count=0;
virtual void reset_fit(double d)
{
if(d>1)
m_filter_man_count++;
else
m_filter_man_count = 0;
if(m_filter_man_count==3)
{
m_fitk.reset_data();
m_fita.reset_data();
m_cur_fit.reset();
m_begin=m_last=nullptr;
m_filter_man_count = 0;
}
}
virtual void select_solution1(loc_point &c,const std::vector<point> &vp,const double scale)
{
if(filter_by_fit(c,vp,scale))
{
c.inc_cl(40);
}
else if(c.cl()>0 && vp.size()==2)
{
c[0]=vp[0];
c[1]=vp[1];
c.inc_cl(10);
}
else
{
select_one_ant(c,vp);
}
}
virtual fit_result * get_best_fit()
{
return best_fit_raw(4,4);
}
virtual double getA(const fit_result * fit,const double scale,const double dt)
{
double a=2.5;
if(fabs(fit->k)<2/(3.6*scale) && fabs(fit->k)>0.5/(3.6*scale) && dt<10)
{
a=0.3;
}
return a;
}
virtual bool revise_by_history(point & pt, const site*sit, int64_t timestamp)
{
//log_info("lemon test revise preson:cardid:%d sit:%d sitid:%d %0X",m_d(0).m_cid,sit->m_id,m_d(0).m_sid,sit);
point dstp = sit->get_dstp(pt);
if(dstp.empty())
log_error("person.dstp empty()");
push_data_point dp;
dp.dstp.set(dstp);
dp.ct=m_d(0).m_ct;
dp.valid=true;
dp.stop_cnt=5;
dp.lp=m_d(0);
dp.set(pt);
{ //don't delete the braces
std::lock_guard<std::mutex> lock(m_owner->m_mtx);
m_owner->m_push_list.clear();
m_owner->m_push_list.push(dp);
//dp.lp.debug_out("person");
}
return true;
}
};
struct car_point_filter:select_point_object
{
car_point_filter(select_tool * owner)
:select_point_object(owner)
,m_last_fit_valid(false)
,m_last_fit_k(0)
,m_last_fit_kb(0)
,m_last_fit_ka(0)
,m_last_fit_xo(0)
,m_last_fit_yo(0)
,m_last_fit_md_x(0)
,m_last_fit_md_y(0)
,m_last_fit_time_sec(0)
,m_last_fit_nearest_time_sec(0)
,m_last_time_sec(0)
,m_if_turning(false)
{}
bool m_last_fit_valid = false;
double m_last_fit_k ;
double m_last_fit_kb;
double m_last_fit_ka;
double m_last_fit_xo ;
double m_last_fit_yo;
double m_last_fit_md_x;
double m_last_fit_md_y;
double m_last_fit_time_sec; //x(0) of latest valid fit
double m_last_fit_nearest_time_sec;
double m_last_time_sec=0;
bool m_if_turning=false;
point m_turning_pt;
point m_turning_ept;
double m_simple_rec_kx,m_simple_rec_ky;
const double m_fit_differ=4;
const double m_pos_differ=8;
virtual void reset_fit(double d){}
virtual void select_solution1(loc_point &c,const std::vector<point> &vp,const double scale)
{
//two ants.
if(c.cl()>0 && vp.size()==2)//m_card->smoothFlag() )
{
c[0]=vp[0];
c[1]=vp[1];
c.inc_cl(50);
}
else if(filter_by_fit(c,vp,scale))
{
c.inc_cl(40);
}
else
{
select_one_ant(c,vp);
}
}
const fit_result* best_fit()const
{
if(m_cur_fit.k==0 && m_cur_fit.ke==0)
return nullptr;
return &m_cur_fit;
}
virtual fit_result * get_best_fit()
{
return best_fit_raw(5);
}
virtual double getA(const fit_result * fit,const double scale,const double dt)
{
double a=2.5;
if(fabs(fit->k)<10/(3.6*scale) && fabs(fit->k)>1/(3.6*scale) && dt<20)
{
a=1;
}
if(fabs(fit->k)<=1/(3.6*scale) && dt<20)
{
a=-1;
}
return a;
}
void reset_turning()
{
m_if_turning=false;
m_turning_pt.set(0,0);
m_turning_ept.set(0,0);
}
void generate_list(point &pt, const site*sit, bool is_whole_list)
{
//log_info("lemon test 4 :cardid:%d sit:%d sitid:%d",m_d(0).m_cid,sit->m_id,m_d(0).m_sid);
if(is_whole_list)
{
put_loc_point(pt, sit, m_d(0).m_ct, m_d(0));
}
else
{
put_single_loc_point(pt, sit, m_d(0).m_ct, m_d(0));
}
}
void turning_mapping(point &rpt,const site*sit)
{
if(!m_if_turning)return;
point sit_location; //projection
//sit_location.set(sit->x,sit->y);
sit_location = sit->get_dstp(m_turning_pt);
if(sit_location.empty())
std_error("get_dstp point error.....");
double dist1=sit_location.dist(rpt);
double dist2=sit_location.dist(m_turning_pt);
double dist3=m_turning_pt.dist(rpt); // dist1 is supposed to be = dist2+dist3
if(dist1<=dist2 || dist1<=dist3)
{
if(dist2-dist1>3||dist1<=dist3)
{
//printf("reset turning\n");
reset_turning(); // may encounter problems
}
return;
}
if(dist3>10)dist3=10; // turning distance no more than 10
double turning_x,turning_y;
double dist4=m_turning_pt.dist(m_turning_ept);
turning_x=m_turning_pt.x + dist3 * (m_turning_ept.x - m_turning_pt.x) / dist4;
turning_y=m_turning_pt.y + dist3 * (m_turning_ept.y - m_turning_pt.y) / dist4;
//printf("turning mapping:(%.2f,%.2f)->(%.2f,%.2f),d1:%f,d2:%f,d3:%f\n",
// rpt.x,rpt.y,turning_x,turning_y,dist1,dist2,dist3);
rpt.set(turning_x,turning_y);
}
void put_single_loc_point(point &pt, const site*sit, int ct, loc_point &lp)
{
//log_info("lemon test 6 :cardid:%d sit:%d sitid:%d",m_d(0).m_cid,sit->m_id,m_d(0).m_sid);
point rpt;
rpt.set(pt);
turning_mapping(rpt,sit);
if(!card_path::inst().is_at_path(rpt)) //if point not on the path
{
lp.debug_out();
// printf("out of path:t=%ld,sit=%d,card=l,ct=%d,"
// "tof1=%d,tof2=%d,pt=(%.2lf,%.2lf)\n",
// m_d(0).m_time, m_d(0).m_sid,m_d(0).m_ct,
// m_d(0).m_tof[0], m_d(0).m_tof[1], pt.x, pt.y);
return;
}
point dstp; //projection
if(!m_if_turning)
{
//dstp.set(sit->x,sit->y);
dstp = sit->get_dstp(pt);
if(dstp.empty())
log_error("error.here....here.");
}
else
{
dstp.set(m_turning_pt);
}
push_data_point dp;
dp.dstp.set(dstp);
dp.ct=ct;
dp.valid=true;
dp.stop_cnt=5;
dp.lp=lp;
dp.set(rpt);
{ //don't delete the braces
std::lock_guard<std::mutex> lock(m_owner->m_mtx);
m_owner->m_push_list.clear();
m_owner->m_push_list.push(dp);
//dp.lp.debug_out("single point .");
}
}
double estimate_point_by_history(const site*sit, double m_time_sec)
{
double estimate_dist = m_last_fit_k * (m_time_sec-m_last_fit_xo) + m_last_fit_kb + m_last_fit_yo;
point pt(m_last_fit_md_x + estimate_dist * m_simple_rec_kx, m_last_fit_md_y + estimate_dist * m_simple_rec_ky);
int fidx=find_first(0);
if(fidx>=0)
{
loc_point&f=m_d[fidx];
estimate_dist = f.loc_dist(pt);
}
else estimate_dist = 0;
return estimate_dist;
}
point convert_dist_to_pt(double dist, const site*sit)
{
int fidx=find_first(0);
if(fidx<0)return point(0, 0);
loc_point&f=m_d[fidx];
return point(f.m_sol[0].x + dist * m_simple_rec_kx, f.m_sol[0].y + dist * m_simple_rec_ky);
}
void put_loc_point(point &pt, const site*sit, int ct, loc_point &lp)
{
//log_info("lemon test 5 :cardid:%d sit:%d sitid:%d",m_d(0).m_cid,sit->m_id,m_d(0).m_sid);
point rpt;
rpt.set(pt);
turning_mapping(rpt,sit);
if(!card_path::inst().is_at_path(pt)) //if point not on the path
{
lp.debug_out();
// printf("out of path:t=%ld,sit=%d,card=0,ct=%d,"
// "tof1=%d,tof2=%d,pt=(%.2lf,%.2lf)\n",
// m_d(0).m_time, m_d(0).m_sid,m_d(0).m_ct,
// m_d(0).m_tof[0], m_d(0).m_tof[1], pt.x, pt.y);
return;
}
point dstp; //projection
if(!m_if_turning)
{
dstp = sit->get_dstp(pt);
if(dstp.empty())
log_error("dstp.empty()..");
//dstp.set(sit->x,sit->y);
}
else
{
dstp.set(m_turning_pt);
}
int size = 0;
push_data_point dp[13];
dp[size].dstp.set(dstp);
dp[size].ct=ct;
dp[size].valid=true;
dp[size].stop_cnt=5;
dp[size].lp=lp;
dp[size].set(rpt);
double missing_time = m_d(0).m_time/1000.;
size++;
for(;size<13;size++)
{
dp[size].dstp.set(dstp);
dp[size].ct = ct;
dp[size].valid=true;
dp[size].stop_cnt=5;
double mt = missing_time + size;
double missing_dist = estimate_point_by_history(sit, mt);
point missing_point = convert_dist_to_pt(missing_dist, sit);
if(!card_path::inst().is_at_path(missing_point)) //if point not on the path
{
break;
}
turning_mapping(missing_point,sit); //turning
dp[size].set(missing_point);
dp[size].lp.set(missing_point);
dp[size].lp.m_time=(int64_t)(missing_time * 1000);
dp[size].lp.m_sid = sit->m_id;
//dp[size].lp.m_cid = m_d(0).m_cid;
}
{
std::lock_guard<std::mutex> lock(m_owner->m_mtx);
m_owner->m_push_list.clear();
for(int i=0;i<size;i++)
{
m_owner->m_push_list.push(dp[i]);
//dp[i].lp.debug_out("push_list");
}
}
}
double convert_pt_to_dist(point &pt, const site*sit)
{
double dist=0;
int fidx=find_first(0);
if(fidx>=0)
{
loc_point&f=m_d[fidx];
//printf("find_first:(%.2f,%.2f)(%.2f,%.2f)\n",f.m_sol[0].x,f.m_sol[0].y,pt.x,pt.y);
//dist = f.dist(pt) * (pt<f?-1:1);
dist = f.loc_dist(pt);
if(dist!=0)
{
m_simple_rec_kx = (pt.x - f.m_sol[0].x) / dist;
m_simple_rec_ky = (pt.y - f.m_sol[0].y) / dist;
}
}
if(fidx<0 || dist==0)
{
m_simple_rec_kx = 0;
m_simple_rec_ky = 0;
}
//printf("convert_pt_to_dist:(%f,%f),%f\n",pt.x,pt.y,dist);
//double dist = sit->dist_direct(pt);
//if(dist == 0)return 0;
//m_simple_rec_kx = (pt.x - (*sit).x) / dist;
//m_simple_rec_ky = (pt.y - (*sit).y) / dist;
return dist;
}
virtual bool revise_by_history(point & pt, const site*sit, int64_t timestamp)
{
//log_info("lemon test 3 :cardid:%d sit:%d sitid:%d",m_d(0).m_cid,sit->m_id,m_d(0).m_sid);
bool flag =false;
if(m_line.empty() || !m_line.contain(m_d(0),0.1))
{
m_last_fit_valid = false;
m_last_fit_time_sec = 0;
m_last_fit_k = 0;
m_last_fit_kb = 0;
m_last_fit_ka = 0;
m_last_fit_xo = 0;
m_last_fit_yo = 0;
m_last_fit_nearest_time_sec = 0;
m_last_time_sec = 0;
reset_turning();
generate_list(pt, sit, false);
return true;
}
// convert pt to distance
double dist = convert_pt_to_dist(pt, sit);
double m_time_sec = timestamp / 1000.; //second
//if(m_time_sec - m_last_fit_nearest_time_sec > 30)m_last_fit_valid = false;
if(m_time_sec - m_last_fit_time_sec > 60)m_last_fit_valid = false;
// update acc
//m_accumulate_acc = m_d(0).m_acc;
// choose data by fit
const fit_result*fit=best_fit();
bool if_change_fit=false;
if(fit!=nullptr && fit->ke<=1 && m_time_sec - m_fitk.x(0) <= 15 && fabs(fit->k) < m_pos_differ)
{ //printf("change fit time:%f,%f,%f\n",m_time_sec, fit->d.x(0), m_time_sec - fit->d.x(0));
// put m_acccumulate_acc into consideration
// fit->k - m_last_fit_k < m_accumulate_acc
if(m_last_fit_valid == true && m_last_fit_k * fit->k > -0.6)
//if((sit->dist(pt)<20 ||m_last_fit_k * fit->k > -0.6))
{ //if point is too near the sit: do not not judge the backwards
double est1 = estimate_point_by_history(sit, m_last_fit_time_sec);
double est2 = fit->k * (m_time_sec-fit->xo) + fit->kb + fit->yo;
//printf("change fit:1(%f,%f),2(%f,%f),differ:(%f,%f)\n",
// m_last_fit_nearest_time_sec,est1,m_time_sec,est2,m_time_sec-m_last_fit_nearest_time_sec,est2-est1);
//if(fabs(est1-est2)>40)printf("change fit:%f,%f,%f\n",fabs(est1-est2),est1,est2);
if(fabs(est1-est2)< (m_time_sec - m_last_fit_time_sec) * 5) // large jump is not allowed
if_change_fit=true;
}
else if(m_last_fit_valid==false)
if_change_fit=true;
}
if(if_change_fit)
{
m_last_fit_valid = true;
m_last_fit_time_sec = m_fitk.x(0);
m_last_fit_k = fit->k;
m_last_fit_kb = fit->kb;
m_last_fit_ka = fit->ka;
m_last_fit_xo = fit->xo;
m_last_fit_yo = fit->yo;
m_last_fit_nearest_time_sec = m_fitk.x(0);
int fidx=find_first(0);
if(fidx<0)
{
m_last_fit_md_x = 0;
m_last_fit_md_y = 0;
}
else{
loc_point&f=m_d[fidx];
m_last_fit_md_x = f.m_sol[0].x;
m_last_fit_md_y = f.m_sol[0].y;
}
// update acc
//m_accumulate_acc=0
//printf("change line------------, k=%f,ke=%f\n",fit->k,fit->ke);
}
// revise
double estimate_dist = estimate_point_by_history(sit, m_time_sec);
//printf("revise:est:%f, d:%f, fitvalid:%d, timesecdiffer:%f\n",
//estimate_dist, dist, m_last_fit_valid, m_time_sec - m_last_fit_time_sec);
if(m_last_fit_valid && m_time_sec - m_last_fit_time_sec < 20)
{
if(fabs(m_last_fit_k) > 0.5 && fabs(estimate_dist-dist)>m_fit_differ)
dist=estimate_dist;
else if(fabs(m_last_fit_k) <= 0.5 && fabs(estimate_dist-dist)>m_fit_differ * 2)
dist=estimate_dist;
else flag = true;
//m_last_fit_nearest_time_sec = m_time_sec; //need more tests to uncomment this sentence
}
else m_last_fit_nearest_time_sec = m_time_sec;
m_last_time_sec = m_time_sec;
// convert the estimated dist to pt
point mpt = convert_dist_to_pt(dist, sit);
// judging turning
detect_turning(mpt, sit);
// create the list
//if(m_accumulate_acc<-10)generate(mpt, sit,false); generate single point
if(m_last_fit_valid && timestamp/1000. - m_last_fit_time_sec < 20 && fabs(m_last_fit_k) > 0.5)
generate_list(mpt, sit, true); //generate the whole list
else
generate_list(mpt, sit, false); //generate single point
//turning map
turning_mapping(mpt, sit);
pt = mpt;
return flag;
}
void detect_turning(point &mpt, const site*sit)
{
if(m_if_turning)return;
//IMPORTANT: only car-1121 and car-1136 have accurate rav values currently. May delete this sentence in the future.
//if(m_id!=1121 && m_id!=1136)return;
double detect_area = 4;
double detect_angle = 15; //15
double detect_para = 0.25; //0.25
// check angle
double angle=-m_d(0).m_rav; // right+ left-
if(fabs(angle)>180)return; // invalid data
if(fabs(angle)<detect_angle || !m_last_fit_valid || fabs(m_last_fit_k)<0.01)return;
// find turning point
std::vector<line_v> turning_list=card_path::inst().find_possible_path(mpt, detect_area);;
//angle1
int fidx=find_first(0);
if(fidx<0)return;
double dist=m_d[fidx].loc_dist(mpt);
point pt1;
pt1.set(m_d[fidx].m_sol[0]);
double angle1;
if(m_last_fit_k * dist>0)
angle1=calc_turning_angle(pt1, mpt);
else
angle1=calc_turning_angle(mpt, pt1);
if(angle1<0)return;
//finding
for(unsigned int i=0;i<turning_list.size();i++)
{
line_v &l=turning_list[i];
// get map angle
double angle2=calc_turning_angle(l.v[0], l.v[1]);
double delta=angle1-angle2;
if(delta>180)delta=delta-360;
if(delta<-180)delta=delta+360;
if(fabs(delta)<5)continue;
if(fabs(delta)>175)continue;
//printf("angle:%f, delta:%f. mul:%f\n",angle, delta, delta*detect_para);
// turning angle must be correct
if(angle*delta>0 && fabs(angle)>fabs(delta)*detect_para)
{
printf("turning:(%.5f,%.5f)(%.5f,%.5f)(%.5f,%.5f),a1:%f,a2:%f,delta:%f,angle:%f\n",
pt1.x,pt1.y,l.v[0].x,l.v[0].y,l.v[1].x,l.v[1].y,angle1,angle2,delta,angle);
m_if_turning=true;
m_turning_pt.set(l.v[0]);
m_turning_ept.set(l.v[1]);
break;
}
}
}
double calc_turning_angle(point &a, point &b)
{
if(fabs(a.x-b.x)<0.001)
{
if(fabs(a.y-b.y)<0.001)return -1;
return b.y>a.y?90:270;
}
double angle=std::atan((b.y-a.y)/(b.x-a.x))*180/3.1415926;
if(a.x>b.x)angle=angle+180;
if(angle<0)angle=angle+360;
return angle;
}
};
//---------------------smooth
struct smooth_tool
{
std::unique_ptr<select_tool>&m_st;
bool smooth_initial_setting;
double smooth_speed;
double smooth_speed_presentation;
int smooth_speed_presentation_cnt;
point smooth_last_position;
double smooth_last_time_sec;
point smooth_last_true_position;
line smooth_line;
bool smooth_line_reset; //if line reset
bool smooth_halt_condition; //if halting
int smooth_halt_count; //halting count
point smooth_halt_position; //position while begin halting
point smooth_halt_position_plus;
point smooth_halt_position_minus;
smooth_tool()=default;
smooth_tool(std::unique_ptr<select_tool>&st)
:m_st(st)
,smooth_initial_setting(false)
,smooth_speed(0)
,smooth_speed_presentation(0)
,smooth_speed_presentation_cnt(0)
,smooth_last_time_sec(0)
,smooth_halt_condition(0)
,smooth_halt_count(0)
{}
void smooth_set_loc_point(double t, int ct, const site*sit, loc_point *lp)
{
point pt;
if(smooth_halt_condition)
pt.set(smooth_halt_position);
else
pt.set(smooth_last_position);
lp->m_dist2=sit->dist_direct(pt);
lp->m_smooth_x=pt.x;
lp->m_smooth_y=pt.y;
lp->m_time=(int64_t)(t*1000);
lp->m_ct=ct;
if(smooth_halt_condition)
{
lp->m_speed=0;
lp->m_stat=0;
}
else
{
lp->m_speed=smooth_speed_presentation * (3.6*sit->m_scale) ; //(m/s) to (km/h)
if(smooth_speed<0)
lp->m_speed = -lp->m_speed;
if(std::isnan(lp->m_speed))
lp->m_speed=0;
lp->m_stat=1;
//if(fabs(smooth_speed_presentation) < 0.1)
// lp->m_speed=0;
}
}
void smooth_reset()
{
smooth_initial_setting=false;
smooth_speed=0; //smoothed speed
//smooth_speed_presentation=0;
smooth_speed_presentation_cnt=0;
smooth_last_position=point(0,0); //last position of smoothed point
smooth_last_true_position=point(0,0); //last position of true point
smooth_last_time_sec=0; //last time second
smooth_halt_condition = false;
smooth_halt_count=0;
}
bool smooth_initiate(point &pt, double t, const site*sit)
{
smooth_initial_setting=true;
smooth_speed=0;
//smooth_speed_presentation=0;
smooth_speed_presentation_cnt=0;
smooth_last_position = pt;
smooth_last_true_position = pt;
smooth_last_time_sec = t;
smooth_halt_condition=false;
smooth_halt_count=0;
smooth_halt_position=pt;
smooth_halt_position_plus=pt;
smooth_halt_position_minus=pt;
return true;
}
virtual void set(point &pt,loc_point &lp)=0;
loc_point smooth_strategy()
{
loc_point lp;
push_data_point dpt = m_st->getDpt();
if(dpt.empty())
return lp;
point pt;
pt.set(dpt);
double current_t=time(NULL);
const auto & sit = sit_list::instance()->get(dpt.lp.m_sid);
if(dpt.valid)
{
smooth_dist(pt, current_t, dpt.ct, sit.get(), dpt.dstp, &lp);
set(pt,lp);
dpt.lp.m_dist2=lp.m_dist2;
dpt.lp.m_time=lp.m_time;
dpt.lp.m_ct=lp.m_ct;
dpt.lp.set(lp);
dpt.lp.m_dist=sit->dist_direct(dpt);
dpt.lp.m_speed=lp.m_speed;
dpt.lp.m_stat=lp.m_stat;
dpt.lp.debug_out("get_point");
}
else
{
smooth_set_loc_point(current_t, dpt.ct, sit.get(), &lp);
if(dpt.stop_cnt<=0)
{
lp.m_speed=0;
lp.m_stat=0;
}
set(pt,lp);
}
return lp;
}
virtual void smooth_dist(point &pt, double t, int ct, const site*sit, point dstp, loc_point *m_lp = nullptr)=0;
virtual ~smooth_tool(){}
};
struct smooth_tool_person_1:smooth_tool
{
smooth_tool_person_1(std::unique_ptr<select_tool>&m)
:smooth_tool(m)
{}
virtual void set(point &pt,loc_point &lp)
{
lp.set(pt);
}
void smooth_dist(point &pt, double t, int ct, const site*sit, point dstp, loc_point *m_lp = nullptr)
{
if(smooth_line.empty() || !smooth_line.contain(pt,0.1))
{
if(!smooth_line_reset)
{
smooth_reset();
smooth_line_reset=true;
}
else
{
std::vector<point> path=card_path::inst().find_path(smooth_last_true_position, pt);
if(!path.empty() && smooth_last_true_position.dist(path[0])>200)
path.clear();
if(path.empty())
{
smooth_line.set(smooth_last_true_position, pt);
smooth_line_reset=false;
}
else
{
smooth_reset();
}
}
}
else
smooth_line_reset=false;
if(!smooth_initial_setting)
{
smooth_initiate(pt, t, sit);
}
else
{
double current_dist = dstp.dist_direct(pt);
double last_true_position = dstp.dist_direct(smooth_last_true_position);
double max_span = 100;
if(fabs(current_dist-last_true_position)<max_span && t - smooth_last_time_sec < 10)
{
double new_speed = (current_dist-last_true_position) / (t - smooth_last_time_sec);
double speed_differ = fabs(new_speed-smooth_speed);
if(speed_differ>1)new_speed=smooth_speed +1*(new_speed>smooth_speed?1:-1);
smooth_speed = smooth_speed * 0.4 + new_speed * 0.6;
smooth_last_true_position = pt;
smooth_last_position = pt;
smooth_last_time_sec = t;
if(fabs(smooth_speed_presentation)<1e-6 || std::isnan(smooth_speed_presentation))
{
smooth_speed_presentation=fabs(smooth_speed);
}
else
smooth_speed_presentation = smooth_speed_presentation * 0.4 + fabs(smooth_speed) * 0.6;
if(fabs(smooth_speed)<0.1)smooth_speed_presentation=0;
}
else
{
smooth_reset();
smooth_initiate(pt, t, sit);
}
}
if(m_lp != nullptr)//m_time,m_ct,x,y,m_speed,m_stat
{
smooth_set_loc_point(t, ct, sit, m_lp);
}
}
};
struct smooth_tool_car_1:smooth_tool
{
smooth_tool_car_1(std::unique_ptr<select_tool>&m)
:smooth_tool(m)
{}
virtual void set(point &pt,loc_point &lp)
{
lp.set(lp.m_smooth_x,lp.m_smooth_y);
}
void smooth_dist(point &pt, double t, int ct, const site*sit, point dstp, loc_point *m_lp = nullptr)
{
point init_pt(pt.x, pt.y);
if(smooth_line.empty() || !smooth_line.contain(pt,0.1) || smooth_halt_count>6)
{
if(!smooth_line_reset)
{
if(!smooth_line.empty() && !smooth_line.contain(pt,0.1) && !smooth_last_true_position.empty())
{
std::vector<point> path=card_path::inst().find_path(smooth_last_true_position, pt);
if(!path.empty() && smooth_last_true_position.dist(path[0])>200)
path.clear();
printf("generating critical point in smooth(car):(%.2f,%.2f)->(%.2f,%.2f)\n",
smooth_last_true_position.x, smooth_last_true_position.y, pt.x, pt.y);
if(!path.empty())
{
point critical_point=path[0];
printf("critical point generated in smooth(car):pt=(%.2f,%.2f),(%.2f,%.2f)->(%.2f,%.2f)\n",
critical_point.x, critical_point.y, smooth_last_true_position.x, smooth_last_true_position.y,
pt.x, pt.y);
init_pt.set(critical_point);
}
}
smooth_reset();
smooth_line_reset=true;
}
else
{
std::vector<point> path=card_path::inst().find_path(smooth_last_true_position, pt);
if(!path.empty() && smooth_last_true_position.dist(path[0])>200)
path.clear();
if(path.empty())
{
smooth_line.set(smooth_last_true_position, pt);
smooth_line_reset=false;
}
else
{
smooth_reset();
}
}
}
else
smooth_line_reset=false;
if(!smooth_initial_setting)
{
smooth_initiate(init_pt, t, sit);
}
else
{
double current_dist = dstp.dist_direct(pt);
double last_position = dstp.dist_direct(smooth_last_position);
double last_true_position = dstp.dist_direct(smooth_last_true_position);
double rec_kx=0;
double rec_ky=0;
if(current_dist!=0)
{
rec_kx = (pt.x - dstp.x)/current_dist;
rec_ky = (pt.y - dstp.y)/current_dist;
}
double next_dist = last_position + smooth_speed * (t-smooth_last_time_sec);
double max_span = 200;
//printf("smooth dist:%f,%f,%f\n",next_dist,current_dist,next_dist-current_dist);
if(fabs(next_dist-current_dist)<max_span && t - smooth_last_time_sec < 10)
{
double new_speed = (current_dist-last_true_position) / (t - smooth_last_time_sec);
// judge halting
if(fabs(new_speed)<0.1)smooth_halt_count++;
else{
smooth_halt_count=0;
}
if(!smooth_halt_condition && smooth_halt_count>=3 && fabs(smooth_speed) < 0.2)
{
smooth_halt_condition=true;
smooth_halt_position=smooth_last_position;
smooth_halt_position_plus=pt;
smooth_halt_position_minus=pt;
}
// handle speed
if(smooth_halt_condition)
{
double halt_position = dstp.dist_direct(smooth_halt_position);
double halt_position_plus = dstp.dist_direct(smooth_halt_position_plus);
double halt_position_minus = dstp.dist_direct(smooth_halt_position_minus);
if(halt_position_plus<current_dist)halt_position_plus=current_dist;
if(halt_position_minus>current_dist)halt_position_minus=current_dist;
smooth_halt_position_plus = point(dstp.x + halt_position_plus * rec_kx, dstp.y + halt_position_plus * rec_ky);
smooth_halt_position_minus = point(dstp.x + halt_position_minus * rec_kx, dstp.y + halt_position_minus * rec_ky);
if(fabs(halt_position_plus - halt_position_minus)>1)
{
smooth_halt_condition=false;
last_position = halt_position;
smooth_speed = 0;
//printf("smooth stop halting\n");
}
}
else
{
if(fabs(smooth_speed)<1e-6 || std::isnan(smooth_speed))
{
smooth_speed=new_speed;
if(smooth_speed>2.5)smooth_speed=2.5;
if(smooth_speed<-2.5)smooth_speed=-2.5;
}
else
{
double speed_differ = fabs(new_speed-smooth_speed);
if(speed_differ>1)
new_speed=smooth_speed +1*(new_speed>smooth_speed?1:-1);
smooth_speed = smooth_speed * 0.4 + new_speed * 0.6;
}
if(fabs(smooth_speed_presentation)<1e-6 || std::isnan(smooth_speed_presentation))
{
smooth_speed_presentation=fabs(smooth_speed);
}
else
smooth_speed_presentation = smooth_speed_presentation * 0.4 + fabs(smooth_speed) * 0.6;
//printf(",%f,%f\n",new_speed,smooth_speed);
// must obey speed direction
if(smooth_speed * (current_dist-last_position) > 0)
{
last_position = last_position+smooth_speed*(t-smooth_last_time_sec);
if(smooth_speed * (current_dist-last_position) < 0)
{
last_position = current_dist;
}
smooth_speed_presentation_cnt=0;
}
else
{
if(smooth_speed_presentation_cnt<3)
smooth_speed_presentation_cnt++;
else
smooth_speed_presentation=0;
}
if(fabs(smooth_speed)<0.1)smooth_speed_presentation=0;
double revise_para = 0.2;
if(fabs(smooth_speed) < 0.01 || smooth_speed * (current_dist-last_position) < 0)
revise_para=0;
last_position=last_position+(current_dist-last_position)*revise_para;
}
smooth_last_position = point(dstp.x + last_position * rec_kx, dstp.y + last_position * rec_ky);
smooth_last_true_position = pt;
smooth_last_time_sec = t;
}
else
{
smooth_reset();
smooth_initiate(pt, t, sit);
}
}
if(m_lp != nullptr)//m_time,m_ct,x,y,m_speed,m_stat
{
smooth_set_loc_point(t, ct, sit, m_lp);
}
}
};
struct smooth_tool_drivingface_car_1:smooth_tool
{
smooth_tool_drivingface_car_1(std::unique_ptr<select_tool>&m)
:smooth_tool(m)
{}
virtual void set(point&p,loc_point&lp){}
virtual void smooth_dist(point &pt, double t, int ct, const site*sit, point dstp, loc_point *m_lp = nullptr){}
};
//---------------------------------
DEF_EXCEPTION (no_tool);
struct select_tool_manage
{
void create_tool(const std::string &s,std::unique_ptr <select_tool> &set,std::unique_ptr <smooth_tool> &smt)
{
if(!s.compare(std::string{"PS_1"}))
{
set.reset(new select_tool_person_1());
smt.reset(new smooth_tool_person_1(set));
}
else if(!s.compare(std::string{"PS_2"}))
{
set.reset(new select_tool_person_2());
smt.reset(new smooth_tool_person_1(set));
}
else if(!s.compare(std::string{"CS_1"}))
{
set.reset(new select_tool_car_1());
smt.reset(new smooth_tool_car_1(set));
}
else if(!s.compare(std::string{"WS_1"}))
{
set.reset(new select_tool_person_1());
smt.reset(new smooth_tool_person_1(set));
//set.reset(new select_tool_drivingface_car_1());
//smt.reset(new smooth_tool_drivingface_car_1(set));
}
else
{
THROW_EXCEPTION(no_tool,"未定义%s的策略,需要在config.ini中定义",s.c_str());
}
}
static select_tool_manage * instance();
};
#endif