API tools faq
paste
Guest User

OCPN find closest point on ENC

a guest
Nov 18th, 2017
102
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 6.25 KB | None | 0 0
raw download clone embed print report
  1. //поиск ближайшей точки на отрезке
  2. void GetClosestPoint(double Ax, double Ay, double Bx, double By, double Px, double Py, double& min_distance, MyPoint *nearest)
  3. {
  4.     double APx = Px - Ax;
  5.     double APy = Py - Ay;
  6.     double ABx = Bx - Ax;
  7.     double ABy = By - Ay;
  8.     double magAB2 = ABx*ABx + ABy*ABy;
  9.     double ABdotAP = ABx*APx + ABy*APy;
  10.     double t = ABdotAP / magAB2;
  11.     double xret, yret;
  12.  
  13.     if (t < 0)
  14.     {
  15.         xret = Ax;
  16.         yret = Ay;
  17.     }
  18.     else if (t > 1)
  19.     {
  20.         xret = Bx;
  21.         yret = By;
  22.     }
  23.     else
  24.     {
  25.         xret = Ax + ABx*t;
  26.         yret = Ay + ABy*t;
  27.     }
  28.     double distance = pow((Px - xret), 2) + pow((Py - yret), 2);
  29.     if (min_distance > distance)
  30.     {
  31.         min_distance = distance;
  32.         nearest->x = xret;
  33.         nearest->y = yret;
  34.     }
  35.  
  36.  
  37.  
  38. }
  39.  
  40.  
  41.  
  42. float clip(float n, float lower, float upper) {
  43.     return std::max(lower, std::min(n, upper));
  44. }
  45.  
  46.  
  47. bool objectTypeCheck(std::vector<wxString> &types, PI_S57Obj *top) {
  48.  
  49.     for (const wxString& type : types){
  50.        
  51.         if (strcmp(top->FeatureName, type) == 0) return true;
  52.  
  53.     }
  54.     return false;
  55.  
  56. }
  57. bool ChartS63::FindClosestObjectPoint(float lat, float lon, float select_radius,
  58.     PlugIn_ViewPort *VPoint, double *retLat, double *retLon, std::vector<wxString> &types){
  59.     MyPoint nearest{ 0, 0 };
  60.     PI_S57Obj *top;
  61.     double min_distance = 1000000000;
  62.     for (int i = 0; i < PRIO_NUM; ++i) {
  63.        
  64.  
  65.         int point_type = 3;
  66.         top = razRules[i][point_type];
  67.  
  68.  
  69.         while (top != NULL){
  70.  
  71.            
  72.             if (objectTypeCheck(types, top)){
  73.  
  74.                 if (top->pPolyTessGeo) {
  75.  
  76.                     PolyTriGroup *ppg = ((PolyTessGeo *)top->pPolyTessGeo)->Get_PolyTriGroup_head();
  77.  
  78.                     TriPrim *pTP = ppg->tri_prim_head;
  79.                     double easting, northing;
  80.                     toSM_Plugin(lat, lon, m_ref_lat, m_ref_lon, &easting, &northing);
  81.  
  82.  
  83.  
  84.                 //if (isObjectInRadius(easting, northing, top, 10)){
  85.                     MyPoint pvert_list[3];
  86.  
  87.                     while (pTP) {
  88.  
  89.                         if (ppg->data_type == DATA_TYPE_DOUBLE) {
  90.  
  91.                             double *p_vertex = pTP->p_vertex;
  92.  
  93.                             switch (pTP->type){
  94. //каждый полигон разбиваем на отрезки и находим ближайшую точку а нем
  95.                             case PTG_TRIANGLE_FAN: {
  96.                                                        for (int it = 0; it < pTP->nVert - 2; it++) {
  97.  
  98.                                                            GetClosestPoint(p_vertex[0], p_vertex[1], p_vertex[(it * 2) + 2], p_vertex[(it * 2) + 3], easting, northing, min_distance, &nearest);
  99.                                                            GetClosestPoint(p_vertex[(it * 2) + 2], p_vertex[(it * 2) + 3], p_vertex[(it * 2) + 4], p_vertex[(it * 2) + 5], easting, northing, min_distance, &nearest);
  100.                                                            GetClosestPoint(p_vertex[0], p_vertex[1], p_vertex[(it * 2) + 4], p_vertex[(it * 2) + 5], easting, northing, min_distance, &nearest);
  101.  
  102.                                                        }
  103.                                                        break;
  104.                             }
  105.                             case PTG_TRIANGLE_STRIP: {
  106.                                                          for (int it = 0; it < pTP->nVert - 2; it++) {
  107.  
  108.                                                              GetClosestPoint(p_vertex[(it * 2)], p_vertex[(it * 2) + 1], p_vertex[(it * 2) + 2], p_vertex[(it * 2) + 3], easting, northing, min_distance, &nearest);
  109.                                                              GetClosestPoint(p_vertex[(it * 2) + 2], p_vertex[(it * 2) + 3], p_vertex[(it * 2) + 4], p_vertex[(it * 2) + 5], easting, northing, min_distance, &nearest);
  110.                                                              GetClosestPoint(p_vertex[(it * 2)], p_vertex[(it * 2) + 1], p_vertex[(it * 2) + 4], p_vertex[(it * 2) + 5], easting, northing, min_distance, &nearest);
  111.  
  112.                                                          }
  113.                                                          break;
  114.                             }
  115.                             case PTG_TRIANGLES: {
  116.                                                     for (int it = 0; it < pTP->nVert; it += 3) {
  117.  
  118.                                                         GetClosestPoint(p_vertex[(it * 2)], p_vertex[(it * 2) + 1], p_vertex[(it * 2) + 2], p_vertex[(it * 2) + 3], easting, northing, min_distance, &nearest);
  119.                                                         GetClosestPoint(p_vertex[(it * 2) + 2], p_vertex[(it * 2) + 3], p_vertex[(it * 2) + 4], p_vertex[(it * 2) + 5], easting, northing, min_distance, &nearest);
  120.                                                         GetClosestPoint(p_vertex[(it * 2)], p_vertex[(it * 2) + 1], p_vertex[(it * 2) + 4], p_vertex[(it * 2) + 5], easting, northing, min_distance, &nearest);
  121.  
  122.                                                     }
  123.                                                     break;
  124.                             }
  125.                             }
  126.                         }  // double
  127.                         else{
  128.  
  129.                             float *p_vertex = (float *)pTP->p_vertex;
  130.  
  131.                             switch (pTP->type){
  132.                             case PTG_TRIANGLE_FAN: {
  133.                                                        for (int it = 0; it < pTP->nVert - 2; it++) {
  134.  
  135.                                                            GetClosestPoint(p_vertex[0], p_vertex[1], p_vertex[(it * 2) + 2], p_vertex[(it * 2) + 3], easting, northing, min_distance, &nearest);
  136.                                                            GetClosestPoint(p_vertex[(it * 2) + 2], p_vertex[(it * 2) + 3], p_vertex[(it * 2) + 4], p_vertex[(it * 2) + 5], easting, northing, min_distance, &nearest);
  137.                                                            GetClosestPoint(p_vertex[0], p_vertex[1], p_vertex[(it * 2) + 4], p_vertex[(it * 2) + 5], easting, northing, min_distance, &nearest);
  138.  
  139.                                                        }
  140.                                                        break;
  141.                             }
  142.                             case PTG_TRIANGLE_STRIP: {
  143.                                                          for (int it = 0; it < pTP->nVert - 2; it++) {
  144.  
  145.                                                              GetClosestPoint(p_vertex[(it * 2)], p_vertex[(it * 2) + 1], p_vertex[(it * 2) + 2], p_vertex[(it * 2) + 3], easting, northing, min_distance, &nearest);
  146.                                                              GetClosestPoint(p_vertex[(it * 2) + 2], p_vertex[(it * 2) + 3], p_vertex[(it * 2) + 4], p_vertex[(it * 2) + 5], easting, northing, min_distance, &nearest);
  147.                                                              GetClosestPoint(p_vertex[(it * 2)], p_vertex[(it * 2) + 1], p_vertex[(it * 2) + 4], p_vertex[(it * 2) + 5], easting, northing, min_distance, &nearest);
  148.  
  149.                                                          }
  150.                                                          break;
  151.                             }
  152.                             case PTG_TRIANGLES: {
  153.                                                     for (int it = 0; it < pTP->nVert; it += 3) {
  154.  
  155.                                                         GetClosestPoint(p_vertex[(it * 2)], p_vertex[(it * 2) + 1], p_vertex[(it * 2) + 2], p_vertex[(it * 2) + 3], easting, northing, min_distance, &nearest);
  156.                                                         GetClosestPoint(p_vertex[(it * 2) + 2], p_vertex[(it * 2) + 3], p_vertex[(it * 2) + 4], p_vertex[(it * 2) + 5], easting, northing, min_distance, &nearest);
  157.                                                         GetClosestPoint(p_vertex[(it * 2)], p_vertex[(it * 2) + 1], p_vertex[(it * 2) + 4], p_vertex[(it * 2) + 5], easting, northing, min_distance, &nearest);
  158.  
  159.                                                     }
  160.                                                     break;
  161.                             }
  162.                             }
  163.                         }// float
  164.  
  165.                         pTP = pTP->p_next;
  166.  
  167.                     }
  168.  
  169.  
  170.                     //}
  171.  
  172.                 }
  173.  
  174.  
  175.  
  176.             }
  177.             top = top->next;
  178.         }
  179.  
  180.  
  181.     }
  182.  
  183.     if (nearest.x==0) return false;
  184.     fromSM_Plugin(nearest.x, nearest.y, m_ref_lat, m_ref_lon, retLat, retLon);
  185.     return true;
  186. }
Add Comment
Please, Sign In to add comment
Public Pastes
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!