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