1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859 |
- #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;
- }
|