algo/loc_tool.cpp

#include <vector>
#include <log.h>
#include <ant.h>
#include <loc_tool.h>
#include <message.h>
#include "loc_common.h"

// pdoa三维定位
std::vector<point> loc_tool_pdoa_3_base::calc_location(std::vector<loc_message>& loc)
{
	return std::vector<point>();
}
int loc_tool_pdoa_3_base::index() 
{
	return 8;
}

// pdoa二维定位
std::vector<point> loc_tool_pdoa_2_base::calc_location(std::vector<loc_message>& loc)
{
    if( 1 > loc.size())
    {
        logn_info(3, "[pdoa] loc_tool_pdoa_2_base::calc_location, the nums of location data is less than 2");
        return std::vector<point>();
    }
    logn_info(3, "[pdoa] loc_tool_pdoa_2_base::calc_location, loc.size=%d", loc.size());
    std::vector<pdoa_msg_ptr> vps;

    for(auto it = loc.begin(); it != loc.end(); ++it)
    {
        double d = (*it).m_num_ticks*15.65*2.996*1e-4;
        double angle = (*it).m_sit->m_ant[(*it).m_ant_id].m_angle;
        double x = (*it).m_sit->m_ant[(*it).m_ant_id].x;
        double y = (*it).m_sit->m_ant[(*it).m_ant_id].y;
	    double p = (*it).get_pdoa(0); 

        logn_info(3, "[pdoa] calc_position's param, d=%.2f, tof=%ld, angle=%.2f, x=%.2f, y=%.2f, pdoa=%.4f, m_scale=%.2f", d, (*it).m_num_ticks , angle, x, y, p, (*it).m_sit->m_scale);
            
        pdoa_param pp(p, d, angle, x, y);
        if(!cal_position_pdoa(vps, pp)){
            break;
        }
    }

    if(2 > vps.size()){
        logn_info(3, "[pdoa] solutions too less, size=%d", vps.size());
        return std::vector<point>();
    }

    for(auto it = vps.cbegin();it != vps.cend(); ++it){
        logn_info(3, "[pdoa] possible solution: x=%.4f, y=%.4f", (*it)->x, (*it)->y);
    }

    pdoa_msg_ptr p_msg;
    std::size_t count = vps.size();
    if(count <= 0){
        return std::vector<point>();
    }
    for(std::size_t i = 0; i < count; ++i){
        if(vps[i]->y > 0){
            p_msg = vps[i];
            p_msg->x /= loc[0].m_sit->m_scale;
            p_msg->y /= loc[0].m_sit->m_scale;
        }
    }

    double _angle = 3.1415926*p_msg->angle/180.0;
    double x = p_msg->x_ant + p_msg->x*cos(_angle) - p_msg->y*sin(_angle);
    double y = p_msg->y_ant + p_msg->x*sin(_angle) + p_msg->y*cos(_angle);
    logn_info(3, "[pdoa] save,card_id=%d, x=%.4f, y=%.4f, ant_x=%.4f, ant_y=%.4f, angle = %.3f, site_angle=%.3f", loc[0].m_card_id, x, y, p_msg->x_ant, p_msg->y_ant, _angle, p_msg->angle);

    std::vector<point> vp;
    vp.push_back(point(x, y));

    return vp;
}
int loc_tool_pdoa_2_base::index() 
{
	return 7;
}

/* 
 * 计算pdoa定位坐标，并将定位结果保存在vps内
 *
 */
bool loc_tool_pdoa_2_base::cal_position_pdoa(std::vector<pdoa_msg_ptr>& vps, const pdoa_param& pa)
{
    float b = 0.075;
    float d = 0.5*b;

    float p = pa.pdoa*b / (2.0*3.1415926);
    logn_info(3,"[pdoa] before adjust, p = %.4f, pdoa=%.4f", p, pa.pdoa);
    p = -2919.8*pow(p, 4.0) - 88.74*pow(p, 3.0) + 5.6182*p*p + 1.1041*p - 0.00079884;
    //p = -0.0222*pow(p, 4.0) + 0.0328*pow(p, 3.0) + 0.0729*p*p + 0.854*p + 0.0111;
    //p *= 0.01;

    float alpha = p / d;
    if(fabs(alpha) > 1.0){
        logn_info(3, "[pdoa] p>d, alpha=%.4f, p=%.4f, d=%.4f", alpha, p , d);
        return false;
    }

    //double x = (d*d + 2*pa.r*p - p*p) / (2*d);
    //double y = (pa.r - 0.5*p)*sqrt(1.0 - (p/d)*(p/d));

	double angle = acos(alpha)*180/3.14;
	logn_info(3, "[pdoa] angle=%.2f, distance=%.2f", angle, pa.r);

    double x = d / 2.0 + alpha*(pa.r - p / 2.0);
    double y = (pa.r - 0.5*p)*sqrt(1.0 - alpha*alpha);

    logn_info(3, "[pdoa] param, distance=%.4f, p=%.4f, d=%.4f, x=%.4f, y=%.4f, p/d=%.4f", pa.r, p, d, x, y, alpha);

    vps.push_back(std::make_shared<pdoa_message>(x, y, pa.a, pa.r, pa.ax, pa.ay, pa.pdoa, angle));
    vps.push_back(std::make_shared<pdoa_message>(x, -y, pa.a, pa.r, pa.ax, pa.ay, pa.pdoa, angle));

    return true;
}

// pdoa一维定位
std::vector<point> loc_tool_pdoa_1_base::calc_location(std::vector<loc_message>& loc)
{
    //logn_info(3, "[pdoa] loc_tool_pdoa_1_base::calc_location, card_id=%d, loc.size=%d", loc[0].m_card_id, loc.size());
    int32_t last_ct = -1;
    std::vector<point> vc;
    std::vector<loc_message> lm;

    for(auto rit = loc.rbegin(); rit != loc.rend(); ++rit){
        if(rit->m_sit->is_path_empty() || rit->m_num_ticks <= 0){
            continue;
        }
        if(-1 == last_ct){
            last_ct = rit->m_card_ct;
        }

        double dist_tof = rit->m_num_ticks*15.65*2.996*1e-4/rit->m_sit->m_scale;
        auto v = rit->m_sit->solving_pdoa(rit->m_ant_id, dist_tof);
        lm.insert(lm.begin(), *rit);
        vc.insert(std::end(vc), std::begin(v), std::end(v));
    }
    loc.swap(lm);
	
    return std::move(vc);
}

int loc_tool_pdoa_1_base::index() 
{
	return 6;
}

// tdoa三维定位
std::vector<point> loc_tool_tdoa_3_base::calc_location(std::vector<loc_message>& loc)
{
	return std::vector<point>();
}
int loc_tool_tdoa_3_base::index() 
{
	return 5;
}

// tdoa二维定位
std::vector<point> loc_tool_tdoa_2_base::calc_location(std::vector<loc_message>& locm)
{
    if(locm.size() < 3){
        return std::vector<point>();
    }

    std::vector<point> sol;     // 解的列表,返回值
    sol.resize(0);

    std::vector<algo_solution> _algo_sol;
    _algo_sol.resize(0);

    std::vector<double> vtk;    // 保存ki
    std::vector<double> vtd;    // 保存di
    std::vector<point> vtc;     // 保存xi，1 yi，1
    std::vector<point> vtp;     // 保存三角形的顶点坐标
    std::vector<int> vts;

    vtk.resize(0);
    vtd.resize(0);
    vtc.resize(0);
    vtp.resize(0);
    vts.resize(0);

    // 保存卡数据中第一条的分站坐标和插值时间
    point fp;
    unsigned long long fts = 0;

    double _height_offset = 0.0;

    loc_message datas[3];
    for(auto iter0 = locm.begin(); iter0 != locm.end(); ++iter0)
    {
        for(auto iter1 = iter0 + 1; iter1 != locm.end(); ++iter1)
        {
            for(auto iter2 = iter1 + 1; iter2 != locm.end(); ++iter2)
            {
                vtk.clear();
                vtd.clear();
                vtc.clear();
                vts.clear();
                vtp.clear();

                datas[0] = *(iter0);
                datas[1] = *(iter1);
                datas[2] = *(iter2);

                for(int i = 0; i < 3; ++i)
                {
                    double k = 0.0;
                    k = pow(datas[i].m_sit->x, 2) + pow(datas[i].m_sit->y, 2);
                    vtk.push_back(k);

                    if(i == 0)
                    {
                        fts = datas[i].m_num_ticks;
                        fp.x = datas[i].m_sit->x;
                        fp.y = datas[i].m_sit->y;

                        vtd.push_back(0.0);
                    }else{
                        long long diff_time = datas[i].m_num_ticks - fts;
                        double d = diff_time* DWT_TIME_UNITS * SPEED_OF_LIGHT;

                        if(d > 0){
                            vts.push_back(1);
                        }else{
                            vts.push_back(-1);
                        }

                        vtd.push_back(d);
                    }

                    vtc.push_back(point(datas[i].m_sit->x - fp.x, datas[i].m_sit->y - fp.y));
                    vtp.push_back(point(datas[i].m_sit->x, datas[i].m_sit->y));
                }

                double a[4] = {0};
                double dt = vtd[1]*vtc[2].y - vtd[2]*vtc[1].y;
                if(dt < 1E-5 || fabs(vtd[1]) < 1E-5){
                    continue;
                }

                a[0] = (vtd[2]*vtc[1].x - vtd[1]*vtc[2].x)/dt;
                a[1] = (-1)*((vtk[1] - vtk[0] - pow(vtd[1],2))*vtd[2] - (vtk[2] - vtk[0] - pow(vtd[2], 2))*vtd[1])*0.5 / dt;
                a[2] = 0.5*(vtk[1] - vtk[0] - pow(vtd[1],2) - 2*vtc[1].y*a[1]) / vtd[1];
                a[3] = -1*(vtc[1].x + vtc[1].y*a[0]) / vtd[1];

                double A = 0.0, B = 0.0, C = 0.0;
                A = pow(a[3], 2) - 1 - pow(a[0], 2);
                B = 2*(a[2]*a[3] + fp.x + a[0]*(fp.y - a[1]));
                C = pow(a[2], 2) - pow(fp.x, 2) - pow(fp.y - a[1], 2) - pow(_height_offset, 2);

                std::vector<point> _vp;
                double delta = pow(B, 2) - 4*A*C;
                if(delta > 0){
                    point p;
                    p.x = ((-1.0)*B + sqrt(delta))/(2.0*A);
                    p.y = a[0]*p.x + a[1];
                    _vp.push_back(p);

                    p.x = ((-1)*B - sqrt(delta))/(2.0*A);
                    p.y = a[0]*p.x + a[1];
                    _vp.push_back(p);
                }else{
                    continue;
                }

                int idx = -1;
                for(std::size_t i = 0; i < _vp.size(); ++i)
                {
                    bool cond[2] = {false};

                    // 对两个解进行判断，需要同时满足两个条件：
                    // 1.解到点1和点2的距离差的方向性和之前的参数相同
                    // 2.解到点1和点3的距离差的方向性和之前的参数相同
                    double d1 = sqrt(pow(vtp[1].x - _vp[i].x, 2) + pow(vtp[1].y - _vp[i].y, 2)) - sqrt(pow(vtp[0].x - _vp[i].x, 2) + pow(vtp[0].y - _vp[i].y, 2));
        
                    double d2 = sqrt(pow(vtp[2].x - _vp[i].x, 2) + pow(vtp[2].y - _vp[i].y, 2)) - sqrt(pow(vtp[0].x - _vp[i].x, 2) + pow(vtp[0].y - _vp[i].y, 2));

                    if((d1 < 0 && vts[0] == -1) || (d1 > 0 && vts[0] == 1))
                    {
                        cond[0] = true;
                    }
                    if((d2 < 0 && vts[1] == -1) || (d2 > 0 && vts[1] == 1))
                    {
                        cond[1] = true;
                    }

                    if(cond[0] && cond[1]){
                        idx = i;
                    }
                }

                // 判断求出的解是否在基站构成的三角形内
                

                if(idx != -1){
                    if(is_in_triangle(vtp, _vp[idx]))
                    {
                        //sol.push_back(_vp[idx]);
                        algo_solution _as;
                        _as.m_pos = std::make_shared<point>(_vp[idx]);
                        _as.m_triangle = std::make_shared<triangle>();

                        site_point _sp(point(datas[0].m_sit->x, datas[0].m_sit->y));
                        _sp.m_site_id = datas[0].m_sit->m_id;

                        _as.m_triangle->m_vertex.push_back(std::move(_sp));

                        site_point _sp1(point(datas[1].m_sit->x, datas[1].m_sit->y));
                        _sp1.m_site_id = datas[1].m_sit->m_id;
                        _as.m_triangle->m_vertex.push_back(std::move(_sp1));

                        site_point _sp2(point(datas[2].m_sit->x, datas[2].m_sit->y));
                        _sp2.m_site_id = datas[2].m_sit->m_id;
                        _as.m_triangle->m_vertex.push_back(std::move(_sp2));

                        _algo_sol.push_back(_as);
                    }else{
                        sol.push_back(_vp[idx]);
                    }
                }
            }
        }
    }

    // 如果有多于1个的解，
    // 需要计算该解到其对应三角形质心的距离，
    // 然后根据这个距离排序，选择距离最小的解
    if(_algo_sol.size() > 1)
    {
        double min_distance = 99999999999.0;
        point _p;
        for(auto it = _algo_sol.cbegin(); it != _algo_sol.cend(); ++it)
        {
            double d = (*it).m_triangle->get_distance(*((*it).m_pos));
            if(d < min_distance){
                min_distance = d;
                _p = *((*it).m_pos);
                sol.push_back(*((*it).m_pos));
            }    
        }
    }else if(_algo_sol.size() == 1){
        sol.push_back(*(_algo_sol[0].m_pos));
    }

	return sol;
}

int loc_tool_tdoa_2_base::index() 
{
	return 4;
}

bool loc_tool_tdoa_2_base::is_in_triangle(const std::vector<point>& vps, const point& p)
{
    double sabc = 0.0, sadb = 0.0, sbdc = 0.0, sadc = 0.0;
    sabc = get_triangle_area(vps[0], vps[1], vps[2]);
    sadb = get_triangle_area(vps[0], p, vps[1]);
    sbdc = get_triangle_area(vps[1], p, vps[2]);
    sadc = get_triangle_area(vps[0], p, vps[2]);

    double sum = sadb + sbdc + sadc;
    if((sabc - sum) > 1E-5 && (sabc - sum) < 1E-5){
        return true;
    }else{
        return false;
    }

    return false;
}

double loc_tool_tdoa_2_base::get_triangle_area(const point& p0,const point& p1, const point& p2)
{
    return abs((p1.x - p0.x)*(p2.y - p1.y) - (p1.y - p0.y)*(p2.x - p1.x))/2.0;   
}

// tdoa一维定位
std::vector<point> loc_tool_tdoa_1_base::calc_location(std::vector<loc_message>& locm)
{
	return std::vector<point>();
}
int loc_tool_tdoa_1_base::index() 
{
	return 3;
}


// tof三维定位
std::vector<point> loc_tool_tof_3_base::calc_location(std::vector<loc_message>&locm)
{
	return std::vector<point>();
}
int loc_tool_tof_3_base::index() 
{
	return 2;
}

// tof二维定位
std::vector<point> loc_tool_tof_2_base::calc_location(std::vector<loc_message>&locm)
{
	return std::vector<point>();
}
int loc_tool_tof_2_base::index() 
{
	return 1;
}

// tof一维定位
std::vector<point> loc_tool_tof_1_base::calc_location(std::vector<loc_message>&locm)
{
    int32_t last_ct = -1;
    std::vector<point> vec;
    std::vector<loc_message> lm;
    for(auto rit = locm.rbegin();rit != locm.rend();rit++)
    {
        if(rit->m_sit->is_path_empty() || rit->m_num_ticks == 0)
          continue;
        if(last_ct == -1)
          last_ct = rit->m_card_ct;
        else if(last_ct != rit->m_card_ct)
          continue;
		double dist_tof=rit->m_num_ticks*15.65*2.996*1e-4/rit->m_sit->m_scale;
        auto v = rit->m_sit->solving(rit->m_ant_id, dist_tof);
        lm.insert(lm.begin(),*rit);
        vec.insert(std::end(vec),std::begin(v),std::end(v));
    }
    locm.swap(lm);
	return std::move(vec);
}
int loc_tool_tof_1_base::index() 
{
	return 0;
}

loc_tool_main::loc_tool_main()
{
	set_tool(new loc_tool_tof_1_base());
	set_tool(new loc_tool_tof_2_base());
	set_tool(new loc_tool_tof_3_base());
	set_tool(new loc_tool_tdoa_1_base());
	set_tool(new loc_tool_tdoa_2_base());
	set_tool(new loc_tool_tdoa_3_base());
    set_tool(new loc_tool_pdoa_1_base());
    set_tool(new loc_tool_pdoa_2_base());
    set_tool(new loc_tool_pdoa_3_base());
}

loc_tool_main::~loc_tool_main()
{
	for(auto&tool:g_tool)
		delete tool;
}

void loc_tool_main::set_tool(loc_tool*tool)
{
	int index = tool->index();
	if(g_tool[index])
	{
		delete g_tool[index];
		g_tool[index]=0;
	}
	g_tool[index] = tool;
}

std::vector<point> loc_tool_main::calc_location(std::vector<loc_message>&locm)
{
	if(locm.empty())
    {
        return {};
    }
	int tool_index = locm[0].tool_index();
    /*int i = 1, len = locm.size();
	for(; i < len; ++i)
	{
		if(tool_index != locm[i].tool_index())
			break;
	}*/

    uint8_t type = tool_index>>4;
    uint8_t dim = tool_index & 0x03;

    tool_index = type*3 + dim - 1;

    log_info("[algo] loc_tool, tool_index=%d, loc_type=%d, dimension=%d", tool_index, type, dim);
    // 调用对应的算法进行定位计算
	//if(i==len)
	{
		return std::move(g_tool[tool_index]->calc_location(locm));
	}

	//包含至少两种定位方式的基站，目前只考虑两种
	/*std::vector<loc_message> locm1,locm2;
	locm1.assign(locm.begin(),locm.begin()+i);

	for(;i<len;i++)
	{
		if(tool_index!=locm[i].tool_index())
			locm2.push_back(locm[i]);
		else
			locm1.push_back(locm[i]);
	}
    bool flag = false;
    if(locm2[0].tool_index() > tool_index)
        flag = true;
    std::vector<point> rc;
    if(flag && (locm2[0].m_sit->config().best_msg_cnt<=(int)locm2.size()))
    {
		int index=locm2[0].tool_index();
		rc = std::move(g_tool[index]->calc_location(locm2));
        locm.swap(locm2);
    }
    else if(locm1[0].m_sit->config().best_msg_cnt<=(int)locm1.size())
	{
	    rc = std::move(g_tool[tool_index]->calc_location(locm1));
        locm.swap(locm1);
	}
    return std::move(rc);*/
}

loc_tool* loc_tool_main::g_tool[9]={0,0,0,0,0,0,0,0,0};