modularization/src1/decision/common/adc_decision_function/ivnearpoint_simple.cpp

#include "ivnearpoint_simple.h"


#include "adc_tools/compute_00.h"
#include "math/gnss_coordinate_convert.h"

ivnearpoint_simple::ivnearpoint_simple()
{

}

ivnearpoint_simple::~ivnearpoint_simple()
{

}


int ivnearpoint_simple::FindNearPoint(const std::vector<iv::GPSData> & gpsMapLine,double fLon, double fLat, double fHeading)
{
    iv::GPS_INS now_gps_ins;
    now_gps_ins.gps_lat = fLat;
    now_gps_ins.gps_lng = fLon;
    now_gps_ins.ins_heading_angle = fHeading;
    GaussProjCal(now_gps_ins.gps_lng,now_gps_ins.gps_lat,&now_gps_ins.gps_x,&now_gps_ins.gps_y);
    int nindex;
    int lastIndex,mindis,maxAngle;
    if(mnLastIndex <0)
    {
        nindex = iv::decition::Compute00().getFirstNearestPointIndex(now_gps_ins, gpsMapLine,
                                                       lastIndex,
                                                       mindis,
                                                       maxAngle);
        mnLastIndex = nindex;
        return nindex;
    }

    nindex = iv::decition::Compute00().getNearestPointIndex(now_gps_ins, gpsMapLine,
                                                   lastIndex,
                                                   mindis,
                                                   maxAngle);

    if(nindex < 0)
    {
        nindex = iv::decition::Compute00().getFirstNearestPointIndex(now_gps_ins, gpsMapLine,
                                                       lastIndex,
                                                       mindis,
                                                       maxAngle);
        mnLastIndex = nindex;
        return nindex;
    }

    mnLastIndex = nindex;
    return nindex;
}

void ivnearpoint_simple::ResetFind()
{
    mnLastIndex = -1;
}