modularization/src/fusion/fusion_radar_merge/radarmerge.cpp

#include "radarmerge.h"

#include <QDateTime>
#include <math.h>

static int g_seq = 0;

iv::radar::radarobjectarray mergefunc(std::vector<iv::radar_data> xvectorradar)
{
    iv::radar::radarobjectarray xradararray;

    xradararray.set_mstime(QDateTime::currentMSecsSinceEpoch());
    int i,j;
    for(i=0;i<xvectorradar.size();i++)
    {
        double cos_rotation,sin_rotation;
        cos_rotation = cos(xvectorradar[i].frotation);
        sin_rotation = sin(xvectorradar[i].frotation);
        if(xvectorradar[i].bUpdate)
        {
            for(j=0;j<xvectorradar[i].xradararray.obj_size();j++)
            {
                iv::radar::radarobject * pobj =  xradararray.add_obj();
                pobj->CopyFrom(xvectorradar[i].xradararray.obj(j));

                double x,y;
                x = pobj->x();y = pobj->y();
                x = pobj->x()*cos_rotation +pobj->y()*sin_rotation;
                y = pobj->y()*cos_rotation - pobj->x()*sin_rotation;
                pobj->set_x(x + xvectorradar[i].foff_x);pobj->set_y(y + xvectorradar[i].foff_y);

                x = pobj->vx();y = pobj->vy();
                x = pobj->vx()*cos_rotation +pobj->vy()*sin_rotation;
                y = pobj->vy()*cos_rotation - pobj->vx()*sin_rotation;
                pobj->set_vx(x);pobj->set_vy(y);

            }
        }
    }

    return xradararray;
}