fork download
copy 
  1. #include <boost/geometry.hpp>
  2. #include <boost/geometry/geometries/point.hpp>
  3. #include <boost/geometry/geometries/linestring.hpp>
  4. #include <boost/geometry/index/rtree.hpp>
  5. #include <iostream>
  6. #include <vector>
  7. #include <cmath>
  8. #include <algorithm>
  9. #include <limits>
  10. #include <unordered_map>
  11. #include <unordered_set>
  12. #include <chrono>
  13.  
  14. namespace bg = boost::geometry;
  15. namespace bgi = boost::geometry::index;
  16.  
  17. typedef bg::model::point<double, 2, bg::cs::geographic<bg::degree>> point_type;
  18. typedef bg::model::linestring<point_type> linestring_type;
  19. typedef bg::model::box<point_type> box_type;
  20. typedef std::pair<box_type, size_t> rtree_value;
  21.  
  22. // Структура направляющего вектора с дополнительными метриками
  23. struct DirectionVector {
  24.     double x, y, z;
  25.     double azimuth;
  26.     point_type midpoint; // Центральная точка линии для вычисления приблизительного расстояния
  27.  
  28.     DirectionVector(const linestring_type& line) {
  29.         if (line.size() < 2) {
  30.             x = y = z = azimuth = 0;
  31.             midpoint = point_type(0, 0);
  32.             return;
  33.         }
  34.  
  35.         const point_type& A = line.front();
  36.         const point_type& B = line.back();
  37.  
  38.         // Вычисляем азимут
  39.         double lat1 = bg::get<0>(A) * M_PI / 180.0;
  40.         double lon1 = bg::get<1>(A) * M_PI / 180.0;
  41.         double lat2 = bg::get<0>(B) * M_PI / 180.0;
  42.         double lon2 = bg::get<1>(B) * M_PI / 180.0;
  43.  
  44.         double dLon = lon2 - lon1;
  45.         double y_comp = sin(dLon) * cos(lat2);
  46.         double x_comp = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(dLon);
  47.         azimuth = atan2(y_comp, x_comp) * 180.0 / M_PI;
  48.         if (azimuth < 0) azimuth += 360.0;
  49.  
  50.         // Вычисляем 3D вектор для точного расчета
  51.         double Ax, Ay, Az, Bx, By, Bz;
  52.         geo_to_ecef(bg::get<0>(A), bg::get<1>(A), Ax, Ay, Az);
  53.         geo_to_ecef(bg::get<0>(B), bg::get<1>(B), Bx, By, Bz);
  54.  
  55.         x = Bx - Ax;
  56.         y = By - Ay;
  57.         z = Bz - Az;
  58.  
  59.         // Нормализация
  60.         double len = sqrt(x*x + y*y + z*z);
  61.         x /= len;
  62.         y /= len;
  63.         z /= len;
  64.  
  65.         // Вычисляем среднюю точку линии
  66.         midpoint = point_type(
  67.             (bg::get<0>(A) + bg::get<0>(B)) / 2.0,
  68.             (bg::get<1>(A) + bg::get<1>(B)) / 2.0
  69.         );
  70.     }
  71.  
  72.     static void geo_to_ecef(double lat, double lon, double& x, double& y, double& z) {
  73.         const double R = 6378137.0; // Радиус Земли в метрах
  74.         const double lat_rad = lat * M_PI / 180.0;
  75.         const double lon_rad = lon * M_PI / 180.0;
  76.  
  77.         x = R * cos(lat_rad) * cos(lon_rad);
  78.         y = R * cos(lat_rad) * sin(lon_rad);
  79.         z = R * sin(lat_rad);
  80.     }
  81.  
  82.     // Функция для быстрой грубой оценки схожести направлений
  83.     bool is_roughly_collinear(const DirectionVector& other, double threshold = 15.0) const {
  84.         double delta_azimuth = fabs(azimuth - other.azimuth);
  85.         if (delta_azimuth > 180.0) {
  86.             delta_azimuth = 360.0 - delta_azimuth;
  87.         }
  88.         return delta_azimuth < threshold || delta_azimuth > (180.0 - threshold);
  89.     }
  90. };
  91.  
  92. // Вычисление точного угла между векторами
  93. double calculate_exact_angle(const DirectionVector& v1, const DirectionVector& v2) {
  94.     double dot_product = v1.x*v2.x + v1.y*v2.y + v1.z*v2.z;
  95.     dot_product = std::max(-1.0, std::min(1.0, dot_product));
  96.  
  97.     double angle_rad = acos(dot_product);
  98.     double angle_deg = angle_rad * 180.0 / M_PI;
  99.  
  100.     return angle_deg > 90.0 ? 180.0 - angle_deg : angle_deg;
  101. }
  102.  
  103. // Вычисление расстояния между двумя точками в метрах (формула гаверсинуса)
  104. double haversine_distance(const point_type& p1, const point_type& p2) {
  105.     const double R = 6378137.0; // Радиус Земли в метрах
  106.  
  107.     double lat1 = bg::get<0>(p1) * M_PI / 180.0;
  108.     double lon1 = bg::get<1>(p1) * M_PI / 180.0;
  109.     double lat2 = bg::get<0>(p2) * M_PI / 180.0;
  110.     double lon2 = bg::get<1>(p2) * M_PI / 180.0;
  111.  
  112.     double dLat = lat2 - lat1;
  113.     double dLon = lon2 - lon1;
  114.  
  115.     double a = sin(dLat/2) * sin(dLat/2) + 
  116.                cos(lat1) * cos(lat2) * 
  117.                sin(dLon/2) * sin(dLon/2);
  118.     double c = 2 * atan2(sqrt(a), sqrt(1-a));
  119.  
  120.     return R * c;
  121. }
  122.  
  123. // Вычисление расстояния между двумя линиями
  124. double calculate_line_distance(const linestring_type& line1, const linestring_type& line2) {
  125.     if (line1.size() < 2 || line2.size() < 2) {
  126.         return std::numeric_limits<double>::max();
  127.     }
  128.  
  129.     // Для коллинеарных линий используем среднее расстояние между соответствующими точками
  130.     double d1 = haversine_distance(line1.front(), line2.front());
  131.     double d2 = haversine_distance(line1.back(), line2.back());
  132.  
  133.     // Проверяем, не перевернута ли вторая линия относительно первой
  134.     double d3 = haversine_distance(line1.front(), line2.back());
  135.     double d4 = haversine_distance(line1.back(), line2.front());
  136.  
  137.     // Выбираем минимальное из двух возможных соответствий точек
  138.     if (d1 + d2 > d3 + d4) {
  139.         return (d3 + d4) / 2.0;
  140.     } else {
  141.         return (d1 + d2) / 2.0;
  142.     }
  143. }
  144.  
  145. // Класс для эффективного поиска коллинеарных и близких линий
  146. class CollinearityFinder {
  147. private:
  148.     // Пространственный индекс
  149.     bgi::rtree<rtree_value, bgi::quadratic<16>> spatial_index;
  150.  
  151.     // Индекс по направлению (азимуту)
  152.     std::unordered_map<int, std::vector<size_t>> direction_index;
  153.  
  154.     // Кэш направляющих векторов
  155.     std::vector<DirectionVector> directions;
  156.  
  157.     // Исходные линии
  158.     std::vector<linestring_type> lines;
  159.  
  160.     // Параметры поиска
  161.     double spatial_filter_expansion = 0.01;  // ~1 км
  162.     double collinearity_threshold = 10.0;    // в градусах
  163.     double distance_weight = 0.5;           // Вес расстояния в комбинированной оценке
  164.  
  165. public:
  166.     CollinearityFinder(const std::vector<linestring_type>& input_lines) : lines(input_lines) {
  167.         preprocess();
  168.     }
  169.  
  170.     // Установка весов для баланса между коллинеарностью и расстоянием
  171.     void set_distance_weight(double weight) {
  172.         distance_weight = std::max(0.0, std::min(1.0, weight));
  173.     }
  174.  
  175. private:
  176.     void preprocess() {
  177.         auto start_time = std::chrono::high_resolution_clock::now();
  178.  
  179.         // Предварительное вычисление направляющих векторов
  180.         directions.reserve(lines.size());
  181.  
  182.         for (const auto& line : lines) {
  183.             directions.emplace_back(line);
  184.         }
  185.  
  186.         // Строим пространственный индекс
  187.         for (size_t i = 0; i < lines.size(); ++i) {
  188.             box_type box;
  189.             bg::envelope(lines[i], box);
  190.             spatial_index.insert(std::make_pair(box, i));
  191.  
  192.             // Индексируем по азимуту (с шагом 10 градусов)
  193.             int azimuth_bin = static_cast<int>(directions[i].azimuth / 10.0);
  194.             direction_index[azimuth_bin].push_back(i);
  195.  
  196.             // Добавляем также в противоположный сектор
  197.             int opposite_bin = static_cast<int>((directions[i].azimuth + 180.0) / 10.0) % 36;
  198.             direction_index[opposite_bin].push_back(i);
  199.         }
  200.  
  201.         auto end_time = std::chrono::high_resolution_clock::now();
  202.         auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(end_time - start_time).count();
  203.         std::cout << "Preprocessing completed in " << duration << " ms" << std::endl;
  204.     }
  205.  
  206. public:
  207.     // Поиск наиболее коллинеарной и близкой линии
  208.     std::pair<size_t, std::pair<double, double>> find_most_collinear_and_close(const linestring_type& reference_line) {
  209.         auto start_time = std::chrono::high_resolution_clock::now();
  210.  
  211.         DirectionVector reference_vector(reference_line);
  212.  
  213.         // Этап 1: Пространственная фильтрация
  214.         box_type query_box;
  215.         bg::envelope(reference_line, query_box);
  216.  
  217.         // Расширяем область поиска
  218.         point_type min_corner = query_box.min_corner();
  219.         point_type max_corner = query_box.max_corner();
  220.         bg::set<0>(min_corner, bg::get<0>(min_corner) - spatial_filter_expansion);
  221.         bg::set<1>(min_corner, bg::get<1>(min_corner) - spatial_filter_expansion);
  222.         bg::set<0>(max_corner, bg::get<0>(max_corner) + spatial_filter_expansion);
  223.         bg::set<1>(max_corner, bg::get<1>(max_corner) + spatial_filter_expansion);
  224.         query_box = box_type(min_corner, max_corner);
  225.  
  226.         std::vector<rtree_value> spatial_candidates;
  227.         spatial_index.query(bgi::intersects(query_box), std::back_inserter(spatial_candidates));
  228.  
  229.         // Этап 2: Фильтрация по направлению
  230.         int azimuth_bin = static_cast<int>(reference_vector.azimuth / 10.0);
  231.  
  232.         // Собираем соседние бины азимута
  233.         std::vector<size_t> azimuth_candidates;
  234.         for (int offset = -1; offset <= 1; ++offset) {
  235.             int current_bin = (azimuth_bin + offset) % 36;
  236.             if (current_bin < 0) current_bin += 36;
  237.  
  238.             auto it = direction_index.find(current_bin);
  239.             if (it != direction_index.end()) {
  240.                 azimuth_candidates.insert(azimuth_candidates.end(), it->second.begin(), it->second.end());
  241.             }
  242.         }
  243.  
  244.         std::unordered_set<size_t> azimuth_set(azimuth_candidates.begin(), azimuth_candidates.end());
  245.  
  246.         // Этап 3: Комбинированная фильтрация
  247.         std::vector<size_t> candidates;
  248.         for (const auto& spatial_candidate : spatial_candidates) {
  249.             size_t idx = spatial_candidate.second;
  250.             if (azimuth_set.find(idx) != azimuth_set.end() && 
  251.                 reference_vector.is_roughly_collinear(directions[idx])) {
  252.                 candidates.push_back(idx);
  253.             }
  254.         }
  255.  
  256.         auto filter_time = std::chrono::high_resolution_clock::now();
  257.         auto filter_duration = std::chrono::duration_cast<std::chrono::milliseconds>(filter_time - start_time).count();
  258.         std::cout << "Filtering reduced candidates from " << lines.size() << " to " << candidates.size() 
  259.                   << " in " << filter_duration << " ms" << std::endl;
  260.  
  261.         // Этап 4: Вычисление комбинированной оценки (угол и расстояние)
  262.         double best_score = std::numeric_limits<double>::max();
  263.         size_t best_index = 0;
  264.         double best_angle = 0.0;
  265.         double best_distance = 0.0;
  266.  
  267.         if (candidates.empty()) {
  268.             std::cout << "No candidates found after filtering" << std::endl;
  269.             return {0, {std::numeric_limits<double>::max(), std::numeric_limits<double>::max()}};
  270.         }
  271.  
  272.         // Предварительное вычисление некоторых метрик для эталонной линии
  273.         double ref_line_length = 0.0;
  274.         if (reference_line.size() >= 2) {
  275.             ref_line_length = haversine_distance(reference_line.front(), reference_line.back());
  276.         }
  277.  
  278.         // Нормализуем расстояние относительно длины эталонной линии
  279.         double distance_normalize_factor = (ref_line_length > 0) ? (1.0 / ref_line_length) : 1.0;
  280.  
  281.         // Вычисляем комбинированную оценку для каждого кандидата
  282.         for (size_t idx : candidates) {
  283.             // Точный угол
  284.             double angle = calculate_exact_angle(reference_vector, directions[idx]);
  285.  
  286.             // Угол должен быть меньше порога для дальнейшего рассмотрения
  287.             if (angle > collinearity_threshold) {
  288.                 continue;
  289.             }
  290.  
  291.             // Вычисляем расстояние между линиями
  292.             double distance = calculate_line_distance(reference_line, lines[idx]);
  293.  
  294.             // Нормализуем расстояние относительно длины эталонной линии
  295.             double normalized_distance = distance * distance_normalize_factor;
  296.  
  297.             // Нормализуем угол к диапазону [0, 1], где 0 - идеальная коллинеарность
  298.             double normalized_angle = angle / 90.0;
  299.  
  300.             // Комбинированная оценка: взвешенная сумма нормализованного угла и расстояния
  301.             double score = (1.0 - distance_weight) * normalized_angle + distance_weight * normalized_distance;
  302.  
  303.             if (score < best_score) {
  304.                 best_score = score;
  305.                 best_index = idx;
  306.                 best_angle = angle;
  307.                 best_distance = distance;
  308.             }
  309.         }
  310.  
  311.         auto end_time = std::chrono::high_resolution_clock::now();
  312.         auto total_duration = std::chrono::duration_cast<std::chrono::milliseconds>(end_time - start_time).count();
  313.         std::cout << "Total search completed in " << total_duration << " ms" << std::endl;
  314.  
  315.         return {best_index, {best_angle, best_distance}};
  316.     }
  317. };
  318.  
  319. int main() {
  320.     // Эталонная линия
  321.     linestring_type reference_line;
  322.     bg::append(reference_line, point_type(45.094734, 37.565922));
  323.     bg::append(reference_line, point_type(45.094696, 37.566153));
  324.  
  325.     // Создаем тестовый набор линий
  326.     std::vector<linestring_type> lines;
  327.  
  328.     // Генерируем случайные линии
  329.     std::cout << "Generating test data..." << std::endl;
  330.     const int NUM_LINES = 10000; // Для тестирования используем 10,000 линий
  331.  
  332.     for (int i = 0; i < NUM_LINES; ++i) {
  333.         double base_lat = 45.0 + (std::rand() % 2000 - 1000) * 0.0001;
  334.         double base_lon = 37.5 + (std::rand() % 2000 - 1000) * 0.0001;
  335.  
  336.         linestring_type line;
  337.         bg::append(line, point_type(base_lat, base_lon));
  338.  
  339.         double angle = (std::rand() % 360) * M_PI / 180.0;
  340.         double length = 0.0002 + (std::rand() % 100) * 0.00001;
  341.  
  342.         bg::append(line, point_type(
  343.             base_lat + length * cos(angle),
  344.             base_lon + length * sin(angle)
  345.         ));
  346.  
  347.         lines.push_back(line);
  348.     }
  349.  
  350.     // Добавляем линию, коллинеарную и близкую к эталонной
  351.     linestring_type collinear_close_line;
  352.     bg::append(collinear_close_line, point_type(45.094730, 37.565920));
  353.     bg::append(collinear_close_line, point_type(45.094692, 37.566151));
  354.     lines.push_back(collinear_close_line);
  355.  
  356.     // Добавляем линию, коллинеарную, но далекую
  357.     linestring_type collinear_far_line;
  358.     bg::append(collinear_far_line, point_type(45.084730, 37.555920));
  359.     bg::append(collinear_far_line, point_type(45.084692, 37.556151));
  360.     lines.push_back(collinear_far_line);
  361.  
  362.     // Добавляем линию, близкую, но не коллинеарную
  363.     linestring_type close_noncollinear_line;
  364.     bg::append(close_noncollinear_line, point_type(45.094734, 37.565922));
  365.     bg::append(close_noncollinear_line, point_type(45.094834, 37.566022));
  366.     lines.push_back(close_noncollinear_line);
  367.  
  368.     std::cout << "Created " << lines.size() << " test lines" << std::endl;
  369.  
  370.     // Инициализируем поисковик
  371.     CollinearityFinder finder(lines);
  372.  
  373.     // Устанавливаем вес расстояния в 0.5 (50% важности на коллинеарность, 50% на расстояние)
  374.     finder.set_distance_weight(0.5);
  375.  
  376.     // Ищем наиболее коллинеарную и близкую линию
  377.     auto result = finder.find_most_collinear_and_close(reference_line);
  378.  
  379.     std::cout << "Best match: index=" << result.first 
  380.               << ", angle=" << result.second.first << " degrees"
  381.               << ", distance=" << result.second.second << " meters" << std::endl;
  382.  
  383.     return 0;
  384. }
  385.  
Success #stdin #stdout 0.22s 7132KB
comments (?)
stdin
copy 
Standard input is empty
stdout
copy 
Generating test data...
Created 10003 test lines
Preprocessing completed in 205 ms
Filtering reduced candidates from 10003 to 8 in 0 ms
Total search completed in 0 ms
Best match: index=10000, angle=0 degrees, distance=0.472202 meters
https://ideone.com/fqLkfD
language:
C++14 (gcc 8.3)
created:
19 hours ago
visibility:
public

Share or Embed source code

Discover > Sphere Engine API

The brand new service which powers Ideone!

Discover > IDE Widget

Widget for compiling and running the source code in a web browser!