Merge pull request #81 from UmbrellaLeaf5/trajectory_calculator

Trajectory calculator

math/littles_algorithm/adjacency_matrix.cpp

@@ -141,6 +141,7 @@ void AdjacencyMatrix::CalculateData() {
   selected_edge_ = std::make_pair(matrix_[selected_value_.first][size_],
                                   matrix_[size_][selected_value_.second]);
 }
+
 void AdjacencyMatrix::ExtendTo(std::size_t num_of_flyers) {
   for (std::size_t i = 1; i < num_of_flyers; ++i) {
     matrix_.insert(matrix_.begin() + size_, matrix_[0]);

math/optimal_way/obstacles.h

@@ -46,6 +46,8 @@ struct LinearFunction {
 struct Point : public lib::Point {
   Point() = default;
 
+  Point(const lib::Point& p) : lib::Point(p) {}
+
   Point(double xx, double yy) : lib::Point{xx, yy} {}
 
   // Вторая точка касательной

math/optimal_way/optimal_way.cpp

@@ -89,7 +89,7 @@ void OptimalWayCalculator::AddGraphTangentPoints() {
     for (auto& point : circle.GetTangentPoints()) {
       PathWayNode new_node(point, graph_.nodes.size());
       new_node.circle_ptr = std::make_shared<CircleObstacle>(circle);
-      graph_.nodes.push_back(std::make_shared<PathWayNode>(new_node));
+      graph_.AddNode(std::make_shared<PathWayNode>(new_node));
       for (std::size_t i = 0; i < graph_.nodes.size() - 1; ++i) {
         if (graph_.nodes[i]->circle_ptr &&
             ((*graph_.nodes[i]->circle_ptr) == circle)) {
@@ -105,25 +105,34 @@ void OptimalWayCalculator::AddGraphTangentPoints() {
       }
     }
 
-  for (auto& poly : polys_)
-    for (auto& point : poly.GetTangentPoints()) {
-      PathWayNode new_node(point, graph_.nodes.size());
-      new_node.poly_ptr = std::make_unique<PolygonObstacle>(poly);
-      graph_.nodes.push_back(std::make_shared<PathWayNode>(new_node));
+  for (auto& poly : polys_) {
+    std::vector<Point> vertexes = poly.GetVertexes();
+    for (std::size_t i = 0; i < vertexes.size(); ++i) {
+      PathWayNode new_node(vertexes[i], graph_.nodes.size());
+      new_node.poly_ptr = std::make_shared<PolygonObstacle>(poly);
+      graph_.AddNode(std::make_shared<PathWayNode>(new_node));
+      // Проводим ребра в графе только между соседними вершинами многоугольника
+      if (i != 0)
+        graph_.AddEdge(
+            graph_.nodes[graph_.nodes.size() - 2]->number, new_node.number,
+            DistanceBetweenPoints(graph_.nodes[graph_.nodes.size() - 2]->point,
+                                  new_node.point));
+      if (i == vertexes.size() - 1)
+        graph_.AddEdge(
+            graph_.nodes[graph_.nodes.size() - vertexes.size()]->number,
+            new_node.number,
+            DistanceBetweenPoints(
+                graph_.nodes[graph_.nodes.size() - vertexes.size()]->point,
+                new_node.point));
       for (std::size_t i = 0; i < graph_.nodes.size() - 1; ++i) {
-        if (graph_.nodes[i]->poly_ptr &&
-            ((*graph_.nodes[i]->poly_ptr) == poly)) {
-          graph_.AddEdge(graph_.nodes[i]->number, new_node.number,
-                         DistanceBetweenPointsOnPolygon(
-                             poly, graph_.nodes[i]->point, new_node.point));
-        } else if (new_node.point ==
-                   *graph_.nodes[i]->point.another_tangent_point) {
-          graph_.AddEdge(
-              graph_.nodes[i]->number, new_node.number,
-              DistanceBetweenPoints(graph_.nodes[i]->point, new_node.point));
-        }
+        if (new_node.point != *graph_.nodes[i]->point.another_tangent_point)
+          continue;
+        graph_.AddEdge(
+            graph_.nodes[i]->number, new_node.number,
+            DistanceBetweenPoints(graph_.nodes[i]->point, new_node.point));
       }
     }
+  }
 }
 
 std::size_t OptimalWayCalculator::AddGraphControlPoints(Point point) {
@@ -133,7 +142,7 @@ std::size_t OptimalWayCalculator::AddGraphControlPoints(Point point) {
       if (TangentGoesThroughOtherObstacle(tangent_point, point)) continue;
       PathWayNode new_node(tangent_point, graph_.nodes.size());
       new_node.circle_ptr = std::make_shared<CircleObstacle>(circle);
-      graph_.nodes.push_back(std::make_shared<PathWayNode>(new_node));
+      graph_.AddNode(std::make_shared<PathWayNode>(new_node));
       for (std::size_t i = 0; i < graph_.nodes.size() - 1; ++i) {
         if (*graph_.nodes[i]->circle_ptr == circle) {
           graph_.AddEdge(graph_.nodes[i]->number, new_node.number,
@@ -149,7 +158,7 @@ std::size_t OptimalWayCalculator::AddGraphControlPoints(Point point) {
       if (TangentGoesThroughOtherObstacle(tangent_point, point)) continue;
       PathWayNode new_node(tangent_point, graph_.nodes.size());
       new_node.poly_ptr = std::make_shared<PolygonObstacle>(poly);
-      graph_.nodes.push_back(std::make_shared<PathWayNode>(new_node));
+      graph_.AddNode(std::make_shared<PathWayNode>(new_node));
       for (std::size_t i = 0; i < graph_.nodes.size() - 1; ++i) {
         if (*graph_.nodes[i]->poly_ptr == poly) {
           graph_.AddEdge(graph_.nodes[i]->number, new_node.number,
@@ -166,12 +175,12 @@ void OptimalWayCalculator::FindOptimalWay(Point p1, Point p2) {
   std::size_t point1_new_nodes = AddGraphControlPoints(p1);
   std::size_t point2_new_nodes = AddGraphControlPoints(p2);
   PathWayNode new_node1(p1, graph_.nodes.size());
-  graph_.nodes.push_back(std::make_shared<PathWayNode>(new_node1));
+  graph_.AddNode(std::make_shared<PathWayNode>(new_node1));
   for (std::size_t i = normal_graph_size_; i < point1_new_nodes; ++i)
     graph_.AddEdge(graph_.nodes[i]->number, new_node1.number,
                    DistanceBetweenPoints(graph_.nodes[i]->point, p1));
   PathWayNode new_node2(p2, graph_.nodes.size());
-  graph_.nodes.push_back(std::make_shared<PathWayNode>(new_node2));
+  graph_.AddNode(std::make_shared<PathWayNode>(new_node2));
   for (std::size_t i = point1_new_nodes; i < point2_new_nodes; ++i)
     graph_.AddEdge(graph_.nodes[i]->number, new_node2.number,
                    DistanceBetweenPoints(graph_.nodes[i]->point, p2));

math/optimal_way/optimal_way.h

@@ -21,6 +21,10 @@ class OptimalWayCalculator {
     normal_graph_size_ = graph_.nodes.size();
   }
 
+  std::vector<std::shared_ptr<PathWayNode>> GetGraphNodes() {
+    return graph_.nodes;
+  }
+
   std::vector<std::size_t> GetOptimalWay(Point point1, Point point2) {
     FindOptimalWay(point1, point2);
     return optimal_way_;
@@ -36,25 +40,33 @@ class OptimalWayCalculator {
   // Граф для алгоритма Дейкстры
   PathWayGraph graph_;
 
+  // Количество вершин в графе без связей с контрольными точками
   std::size_t normal_graph_size_;
 
   // Оптимальный путь
   std::vector<std::size_t> optimal_way_;
 
+  // Длина оптимального пути
   double optimal_way_length_;
 
   /**
    * @brief Проверяет, пересекает ли общая касательная двух препятствий другое
    * препятствие
    * @param tangent: общая касательная
-   * @param circle1_index: номер препятствие 1
-   * @param circle2_index: номер препятствие 2
+   * @param obstacle1: номер препятствие 1
+   * @param obstacle2: номер препятствие 2
    * @return bool: результат проверки
    */
   template <typename T, typename U>
   bool TangentGoesThroughOtherObstacle(const LinearFunction& tangent,
                                        T& obstacle1, U& obstacle2);
 
+  /**
+   * @brief Проверяет, пересекает ли касательная препятствие
+   * @param tangent_point: точка касания
+   * @param control_point: контрольная точка
+   * @return результат проверки
+   */
   bool TangentGoesThroughOtherObstacle(const Point& tangent_point,
                                        const Point& control_point);
 

math/trajectory.cpp

@@ -1,8 +1,79 @@
 // header file:
 #include "trajectory.h"
 
+using lib::Hill;
+using lib::Segment;
+using lib::Target;
+using lib::TrappyCircle;
+using lib::TrappyLine;
+
 namespace math {
 
-TrajectoryCalculator::TrajectoryCalculator() {}
+std::vector<Segment> TrajectoryCalculator::GetTrajectoryPart(
+    std::vector<std::size_t> optimal_way,
+    const std::vector<std::shared_ptr<PathWayNode>>& nodes) {
+  std::vector<Segment> trajectory_part;
+  for (std::size_t i = 0; i < optimal_way.size() - 1; ++i) {
+    // Если точки ляжат на одной окружности, их следует соединить дугой
+    if ((nodes[optimal_way[i]]->circle_ptr) &&
+        (nodes[optimal_way[i] + 1]->circle_ptr) &&
+        (nodes[optimal_way[i]]->circle_ptr ==
+         nodes[optimal_way[i] + 1]->circle_ptr))
+      trajectory_part.push_back(Segment(
+          nodes[optimal_way[i]]->point, nodes[optimal_way[i] + 1]->point,
+          nodes[optimal_way[i]]->circle_ptr->GetCenter()));
+    else {
+      trajectory_part.push_back(Segment(nodes[optimal_way[i]]->point,
+                                        nodes[optimal_way[i] + 1]->point));
+    }
+  }
+  return trajectory_part;
+}
+
+void TrajectoryCalculator::CalculateTrajectory() {
+  // Матрица смежности контрольных точек
+  std::vector<std::vector<double>> matrix(targets_.size());
+  for (auto& elem : matrix) elem.resize(targets_.size());
+
+  // Матрица оптимальных путей между контрольными точками
+  std::vector<std::vector<std::vector<Segment>>> optimal_ways(targets_.size());
+  for (auto& elem : optimal_ways) elem.resize(targets_.size());
+
+  // Заполнение матриц
+  OptimalWayCalculator owc(circles_, polys_);
+
+  for (std::size_t i = 0; i < targets_.size() - 1; ++i) {
+    matrix[i][i] = inf;
+    for (std::size_t j = i + 1; i < targets_.size(); ++j) {
+      std::vector<std::size_t> optimal_way =
+          owc.GetOptimalWay(targets_[i], targets_[j]);
+      std::vector<std::shared_ptr<PathWayNode>> nodes = owc.GetGraphNodes();
+      std::vector<Segment> segment_way = GetTrajectoryPart(optimal_way, nodes);
+      optimal_ways[i][j] = segment_way;
+      std::reverse(segment_way.begin(), segment_way.begin());
+      optimal_ways[j][i] = segment_way;
+
+      matrix[i][j] = owc.GetOptimalWayLength();
+      matrix[j][i] = owc.GetOptimalWayLength();
+    }
+  }
+
+  for (auto& line : forbidden_lines_) {
+    matrix[line.first][line.second] = inf;
+    matrix[line.second][line.first] = inf;
+  }
+
+  // Просчет кратчайшей траектории по алгоритму Литтла
+  AdjacencyMatrix adj_matrix = AdjacencyMatrix::WithExtraRowCol(matrix);
+  TravellingSalesmansProblem tsp(adj_matrix);
+  std::vector<std::size_t> traj = tsp.GetTrajectory();
+
+  // Объединение частей траектории
+  for (std::size_t i = 0; i < traj.size() - 1; ++i) {
+    trajectory_.insert(trajectory_.end(),
+                       optimal_ways[traj[i]][traj[i + 1]].begin(),
+                       optimal_ways[traj[i]][traj[i + 1]].end());
+  }
+}
 
 }  // namespace math

math/trajectory.h

@@ -5,16 +5,78 @@
 #include "math/littles_algorithm/travelling_salesmans_problem.h"
 #include "math/optimal_way/optimal_way.h"
 
-using lib::Hill;
-using lib::Target;
-using lib::TrappyCircle;
-using lib::TrappyLine;
-
 namespace math {
 
 class TrajectoryCalculator {
  public:
-  TrajectoryCalculator();
+  /**
+   * @brief Инициализирует экземпляр класса TrajectoryCalculator
+   * @param targets: контрольные точки
+   * @param lines: запрещённые перелеты
+   * @param circles: круговые препятствия
+   * @param hills: многоугольные препятствия
+   */
+  TrajectoryCalculator(const std::vector<lib::Target>& targets,
+                       const std::vector<lib::TrappyLine>& lines,
+                       const std::vector<lib::TrappyCircle>& circles,
+                       const std::vector<lib::Hill>& hills) {
+    for (auto& target : targets) targets_.push_back(Point(target.GetPoint()));
+
+    for (auto& line : lines) {
+      std::pair<std::size_t, std::size_t> indexes;
+      std::pair<lib::Target, lib::Target> points = line.GetTargets();
+      for (std::size_t i = 0; i < targets.size(); ++i)
+        if (targets[i] == points.first)
+          indexes.first = i;
+        else if (targets[i] == points.second)
+          indexes.second = i;
+      forbidden_lines_.push_back(indexes);
+    }
+
+    for (auto& circle : circles)
+      circles_.push_back(
+          CircleObstacle(Point(circle.GetCenter()), circle.GetRadius()));
+
+    for (auto& hill : hills) {
+      std::vector<Point> vertexes;
+      for (auto& vertex : hill.GetVertices()) vertexes.push_back(Point(vertex));
+      polys_.push_back(PolygonObstacle(vertexes));
+    }
+
+    CalculateTrajectory();
+  }
+
+  /// @brief Возвращает траекторию
+  std::vector<lib::Segment> GetTrajectory() { return trajectory_; }
+
+ private:
+  // Контрольные точки
+  std::vector<Point> targets_;
+
+  // Запрещенные перелёты
+  std::vector<std::pair<std::size_t, std::size_t>> forbidden_lines_;
+
+  // Круговые препятствия
+  std::vector<CircleObstacle> circles_;
+
+  // Многоугольные препятствия
+  std::vector<PolygonObstacle> polys_;
+
+  // Траектория облета контрольных точек
+  std::vector<lib::Segment> trajectory_;
+
+  /**
+   * @brief Подсчет части траектории
+   * @param optimal_way: порядок обхода точек
+   * @param nodes: вершины графа
+   * @return часть траектории
+   */
+  std::vector<lib::Segment> GetTrajectoryPart(
+      std::vector<std::size_t> optimal_way,
+      const std::vector<std::shared_ptr<PathWayNode>>& nodes);
+
+  /// @brief Расcчитывает траекторию
+  void CalculateTrajectory();
 };
 
 }  // namespace math