ivnearpoint_simple.cpp 1.7 KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859
  1. #include "ivnearpoint_simple.h"
  2. #include "adc_tools/compute_00.h"
  3. #include "math/gnss_coordinate_convert.h"
  4. ivnearpoint_simple::ivnearpoint_simple()
  5. {
  6. }
  7. ivnearpoint_simple::~ivnearpoint_simple()
  8. {
  9. }
  10. int ivnearpoint_simple::FindNearPoint(const std::vector<iv::GPSData> & gpsMapLine,double fLon, double fLat, double fHeading)
  11. {
  12. iv::GPS_INS now_gps_ins;
  13. now_gps_ins.gps_lat = fLat;
  14. now_gps_ins.gps_lng = fLon;
  15. now_gps_ins.ins_heading_angle = fHeading;
  16. GaussProjCal(now_gps_ins.gps_lng,now_gps_ins.gps_lat,&now_gps_ins.gps_x,&now_gps_ins.gps_y);
  17. int nindex;
  18. int lastIndex,mindis,maxAngle;
  19. if(mnLastIndex <0)
  20. {
  21. nindex = iv::decition::Compute00().getFirstNearestPointIndex(now_gps_ins, gpsMapLine,
  22. lastIndex,
  23. mindis,
  24. maxAngle);
  25. mnLastIndex = nindex;
  26. return nindex;
  27. }
  28. nindex = iv::decition::Compute00().getNearestPointIndex(now_gps_ins, gpsMapLine,
  29. lastIndex,
  30. mindis,
  31. maxAngle);
  32. if(nindex < 0)
  33. {
  34. nindex = iv::decition::Compute00().getFirstNearestPointIndex(now_gps_ins, gpsMapLine,
  35. lastIndex,
  36. mindis,
  37. maxAngle);
  38. mnLastIndex = nindex;
  39. return nindex;
  40. }
  41. mnLastIndex = nindex;
  42. return nindex;
  43. }
  44. void ivnearpoint_simple::ResetFind()
  45. {
  46. mnLastIndex = -1;
  47. }