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