Skip to content

Commit

Permalink
Merge pull request #78 from UmbrellaLeaf5/optimal_way_tests
Browse files Browse the repository at this point in the history 
Optimal way tests
  • Loading branch information
@Romanov-Fedor
Romanov-Fedor authored May 9, 2024
2 parents 1213417 + 6d308cf commit 1c28be9
Show file tree
Hide file tree
Showing 8 changed files with 234 additions and 82 deletions.
17 changes: 13 additions & 4 deletions math/optimal_way/helpers_functions.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -166,9 +166,13 @@ std::vector<LinearFunction> TangentsBetween(const CircleObstacle& circle1,
auto FindTangent = [&x_1, &x_0, &y_1, &y_0](double r_0, double r_1) {
double a, b, c;
if (std::abs(x_1 - x_0) > precision) {
b = ((r_1 - r_0) * (y_1 - y_0) +
sqrt(pow(x_1 - x_0, 2) *
(pow(x_1 - x_0, 2) + pow(y_1 - y_0, 2) - pow(r_1 - r_0, 2)))) /
double root = pow(x_1 - x_0, 2) *
(pow(x_1 - x_0, 2) + pow(y_1 - y_0, 2) - pow(r_1 - r_0, 2));
if (std::abs(root) < precision)
root = 0;
else
root = sqrt(root);
b = ((r_1 - r_0) * (y_1 - y_0) + root) /
(pow(x_1 - x_0, 2) + pow(y_1 - y_0, 2));

a = ((r_1 - r_0) - b * (y_1 - y_0)) / (x_1 - x_0);
Expand Down Expand Up @@ -225,7 +229,7 @@ bool AreThereIntersections(const CircleObstacle& cr_obst, const Point& point1,
(pow(radius, 2) - pow(center.x, 2) - pow(b_coef, 2) -
pow(center.y, 2) + 2 * b_coef * center.y) *
(1 + pow(slope, 2));
if (discriminant <= 0)
if (discriminant < precision)
return false;
else {
double x_1 =
Expand Down Expand Up @@ -299,6 +303,11 @@ bool AreThereIntersections(const PolygonObstacle& poly_obst,
return false;
}

bool IsPointInsideCircle(const Point& point, const CircleObstacle& circle) {
return (DistanceBetweenPoints(circle.GetCenter(), point) -
circle.GetRadius()) < 0;
}

template std::vector<LinearFunction> TangentsBetween<CircleObstacle>(
const PolygonObstacle& polygon, const CircleObstacle& obstacle);
template std::vector<LinearFunction> TangentsBetween<PolygonObstacle>(
Expand Down
8 changes: 8 additions & 0 deletions math/optimal_way/helpers_functions.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -126,4 +126,12 @@ bool AreThereIntersections(const PolygonObstacle& poly_obst, const Point& pnt1,
bool AreThereIntersections(const PolygonObstacle& poly_obst,
const LinearFunction& line);

/**
* @brief Проверяет, находится ли точка внутри окружности
* @param point: точка
* @param circle: окружность
* @return bool: результат проверки
*/
bool IsPointInsideCircle(const Point& point, const CircleObstacle& circle);

} // namespace math
1 change: 1 addition & 0 deletions math/optimal_way/obstacles.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -106,6 +106,7 @@ class CircleObstacle {
/// @brief Многоугольное препятствие
class PolygonObstacle {
public:
PolygonObstacle() = default;
/**
* @brief Инициализирует экземпляр PolygonObstacle
* @param vertexes: вершины многоугольника
Expand Down
163 changes: 91 additions & 72 deletions math/optimal_way/optimal_way.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,7 @@
namespace math {

template <typename T, typename U>
bool OptimalWayCalculator::TangentGoesTroughOtherObstacle(
bool OptimalWayCalculator::TangentGoesThroughOtherObstacle(
const LinearFunction& tangent, T& obstacle1, U& obstacle2) {
std::pair<Point, Point> tangent_points =
TangentPoints(tangent, obstacle1, obstacle2);
Expand All @@ -22,6 +22,16 @@ bool OptimalWayCalculator::TangentGoesTroughOtherObstacle(
return false;
}

bool OptimalWayCalculator::TangentGoesThroughOtherObstacle(
const Point& tangent_point, const Point& control_point) {
for (auto& circle : circles_)
if (AreThereIntersections(circle, tangent_point, control_point))
return true;
for (auto& poly : polys_)
if (AreThereIntersections(poly, tangent_point, control_point)) return true;
return false;
}

template <typename T, typename U>
void OptimalWayCalculator::AddTangent(const LinearFunction& tangent,
T& obstacle1, U& obstacle2) {
Expand All @@ -44,7 +54,7 @@ void OptimalWayCalculator::AddCommonTangents() {
TangentsBetween(circles_[i], circles_[j]);

for (std::size_t k = 0; k < tangents.size(); ++k)
if (!TangentGoesTroughOtherObstacle<CircleObstacle, CircleObstacle>(
if (!TangentGoesThroughOtherObstacle<CircleObstacle, CircleObstacle>(
tangents[k], circles_[i], circles_[j]))
AddTangent<CircleObstacle, CircleObstacle>(tangents[k], circles_[i],
circles_[j]);
Expand All @@ -56,7 +66,7 @@ void OptimalWayCalculator::AddCommonTangents() {
TangentsBetween(polys_[i], circles_[j]);

for (std::size_t k = 0; k < tangents.size(); ++k)
if (!TangentGoesTroughOtherObstacle<PolygonObstacle, CircleObstacle>(
if (!TangentGoesThroughOtherObstacle<PolygonObstacle, CircleObstacle>(
tangents[k], polys_[i], circles_[j]))
AddTangent<PolygonObstacle, CircleObstacle>(tangents[k], polys_[i],
circles_[j]);
Expand All @@ -66,7 +76,7 @@ void OptimalWayCalculator::AddCommonTangents() {
TangentsBetween(polys_[i], polys_[j]);

for (std::size_t k = 0; k < tangents.size(); ++k)
if (!TangentGoesTroughOtherObstacle<PolygonObstacle, PolygonObstacle>(
if (!TangentGoesThroughOtherObstacle<PolygonObstacle, PolygonObstacle>(
tangents[k], polys_[i], polys_[j]))
AddTangent<PolygonObstacle, PolygonObstacle>(tangents[k], polys_[i],
polys_[j]);
Expand All @@ -78,102 +88,111 @@ void OptimalWayCalculator::AddGraphTangentPoints() {
for (auto& circle : circles_)
for (auto& point : circle.GetTangentPoints()) {
PathWayNode new_node(point, graph_.nodes.size());
new_node.circle_ptr = std::make_unique<CircleObstacle>(circle);
for (auto& prev : graph_.nodes) {
if (prev->circle_ptr && ((*prev->circle_ptr) == circle)) {
graph_.AddEdge(prev->number, new_node.number,
DistanceBetweenPointsOnCircle(circle, prev->point,
new_node.point));
} else if (prev->circle_ptr &&
(new_node.point == (*prev->point.another_tangent_point))) {
graph_.AddEdge(prev->number, new_node.number,
DistanceBetweenPoints(prev->point, new_node.point));
new_node.circle_ptr = std::make_shared<CircleObstacle>(circle);
graph_.nodes.push_back(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)) {
graph_.AddEdge(graph_.nodes[i]->number, new_node.number,
DistanceBetweenPointsOnCircle(
circle, 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));
}
}
graph_.nodes.push_back(std::make_shared<PathWayNode>(new_node));
}

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);
for (auto& prev : graph_.nodes) {
if (prev->poly_ptr && ((*prev->poly_ptr) == poly)) {
graph_.AddEdge((*prev).number, new_node.number,
DistanceBetweenPointsOnPolygon(poly, prev->point,
new_node.point));
} else if (prev->poly_ptr &&
(new_node.point == (*prev->point.another_tangent_point))) {
graph_.AddEdge((*prev).number, new_node.number,
DistanceBetweenPoints(prev->point, new_node.point));
graph_.nodes.push_back(std::make_shared<PathWayNode>(new_node));
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));
}
}
graph_.nodes.push_back(std::make_shared<PathWayNode>(new_node));
}
}

void OptimalWayCalculator::AddGraphControlPoints(Point point1, Point point2) {
for (auto& point : {point1, point2}) {
PathWayNode new_node{point, graph_.nodes.size()};
for (auto& prev : graph_.nodes) {
if (prev->circle_ptr) {
std::pair<Point, Point> tangent_points =
TangentPoints((*prev->circle_ptr), point);
for (auto& tangent_point :
{tangent_points.first, tangent_points.second})
if (tangent_point == prev->point) {
bool is_valid = true;
for (auto& circle : circles_)
if (AreThereIntersections(circle, prev->point, new_node.point))
is_valid = false;
for (auto& poly : polys_)
if (AreThereIntersections(poly, prev->point, new_node.point))
is_valid = false;
if (is_valid)
graph_.AddEdge(
prev->number, new_node.number,
DistanceBetweenPoints(prev->point, new_node.point));
}
std::size_t OptimalWayCalculator::AddGraphControlPoints(Point point) {
for (auto& circle : circles_) {
std::pair<Point, Point> tangent_points = TangentPoints(circle, point);
for (auto& tangent_point : {tangent_points.first, tangent_points.second}) {
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));
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,
DistanceBetweenPointsOnCircle(circle, tangent_point,
graph_.nodes[i]->point));
}
}
if (prev->poly_ptr) {
std::pair<Point, Point> tangent_points =
TangentPoints((*prev->poly_ptr), point);
for (auto& tangent_point :
{tangent_points.first, tangent_points.second})
if (tangent_point == prev->point) {
bool is_valid = true;
for (auto& circle : circles_)
if (AreThereIntersections(circle, prev->point, new_node.point))
is_valid = false;
for (auto& poly : polys_)
if (AreThereIntersections(poly, prev->point, new_node.point))
is_valid = false;
if (is_valid)
graph_.AddEdge(
prev->number, new_node.number,
DistanceBetweenPoints(prev->point, new_node.point));
}
}
}
for (auto& poly : polys_) {
std::pair<Point, Point> tangent_points = TangentPoints(poly, point);
for (auto& tangent_point : {tangent_points.first, tangent_points.second}) {
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));
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,
DistanceBetweenPointsOnPolygon(
poly, tangent_point, graph_.nodes[i]->point));
}
}
}
graph_.nodes.push_back(std::make_shared<PathWayNode>(new_node));
}
return graph_.nodes.size();
}

std::vector<size_t> OptimalWayCalculator::FindOptimalWay(Point p1, Point p2) {
AddGraphControlPoints(p1, p2);
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));
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));
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));
if (!TangentGoesThroughOtherObstacle(p1, p2))
graph_.AddEdge(graph_.nodes.size() - 2, graph_.nodes.size() - 1,
DistanceBetweenPoints(p1, p2));
DijkstrasAlgorithm da(graph_);
return da.Get_Min_Path();
optimal_way_ = da.Get_Min_Path();
optimal_way_length_ = da.Get_Min_Len();
graph_.RemoveLastNodes(graph_.nodes.size() - normal_graph_size_);
}

template bool OptimalWayCalculator::TangentGoesTroughOtherObstacle<
template bool OptimalWayCalculator::TangentGoesThroughOtherObstacle<
CircleObstacle, CircleObstacle>(const LinearFunction& tangent,
CircleObstacle& obstacle1,
CircleObstacle& obstacle2);
template bool OptimalWayCalculator::TangentGoesTroughOtherObstacle<
template bool OptimalWayCalculator::TangentGoesThroughOtherObstacle<
PolygonObstacle, CircleObstacle>(const LinearFunction& tangent,
PolygonObstacle& obstacle1,
CircleObstacle& obstacle2);
template bool OptimalWayCalculator::TangentGoesTroughOtherObstacle<
template bool OptimalWayCalculator::TangentGoesThroughOtherObstacle<
PolygonObstacle, PolygonObstacle>(const LinearFunction& tangent,
PolygonObstacle& obstacle1,
PolygonObstacle& obstacle2);
Expand Down
21 changes: 16 additions & 5 deletions math/optimal_way/optimal_way.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,12 +18,16 @@ class OptimalWayCalculator {
: circles_{circles}, polys_{polys} {
AddCommonTangents();
AddGraphTangentPoints();
normal_graph_size_ = graph_.nodes.size();
}

std::vector<std::size_t> GetOptimalWay(Point point1, Point point2) {
return FindOptimalWay(point1, point2);
FindOptimalWay(point1, point2);
return optimal_way_;
}

double GetOptimalWayLength() { return optimal_way_length_; }

private:
std::vector<CircleObstacle> circles_;

Expand All @@ -32,9 +36,13 @@ class OptimalWayCalculator {
// Граф для алгоритма Дейкстры
PathWayGraph graph_;

std::size_t normal_graph_size_;

// Оптимальный путь
std::vector<std::size_t> optimal_way_;

double optimal_way_length_;

/**
* @brief Проверяет, пересекает ли общая касательная двух препятствий другое
* препятствие
Expand All @@ -44,8 +52,11 @@ class OptimalWayCalculator {
* @return bool: результат проверки
*/
template <typename T, typename U>
bool TangentGoesTroughOtherObstacle(const LinearFunction& tangent,
T& obstacle1, U& obstacle2);
bool TangentGoesThroughOtherObstacle(const LinearFunction& tangent,
T& obstacle1, U& obstacle2);

bool TangentGoesThroughOtherObstacle(const Point& tangent_point,
const Point& control_point);

/**
* @brief Добавляет информацию об общей касательной двух препятствий
Expand All @@ -63,10 +74,10 @@ class OptimalWayCalculator {
void AddGraphTangentPoints();

// Добавляет в граф контрольные точки
void AddGraphControlPoints(Point p1, Point p2);
std::size_t AddGraphControlPoints(Point point);

// Находит оптимальный маршрут
std::vector<size_t> FindOptimalWay(Point p1, Point p2);
void FindOptimalWay(Point p1, Point p2);
};

} // namespace math
14 changes: 14 additions & 0 deletions math/optimal_way/path_graph.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,20 @@ struct PathWayGraph {
nodes.push_back(new_node);
}

/**
* @brief Удалить последние n вершин
* @param n: количество вершин
*/
void RemoveLastNodes(std::size_t n) {
for (std::size_t i = 0; i < n; ++i) {
for (auto& other_node : nodes[nodes.size() - 1]->edges)
for (std::size_t j = 0; j < n; ++j)
if (other_node->edges[j] == nodes[nodes.size() - 1])
other_node->edges.erase(other_node->edges.begin() + j);
nodes.erase(nodes.begin() + nodes.size() - 1);
}
}

// Добавить новое ребро
void AddEdge(std::size_t node_1, std::size_t node_2, double length) {
std::shared_ptr<PathWayNode> node_ptr1, node_ptr2;
Expand Down
Loading

0 comments on commit 1c28be9

Please sign in to comment.