add substitute method for LinearFunction

UmbrellaLeaf5 · Apr 23, 2024 · d7d257e · d7d257e
1 parent 8147623
commit d7d257e
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 8 deletions.
diff --git a/math/optimal_way/helpers_functions.cpp b/math/optimal_way/helpers_functions.cpp
@@ -262,14 +262,8 @@ bool AreThereIntersections(const PolygonObstacle& poly_obst, const Point& pnt1,
   std::vector<Point> vertexes = poly_obst.GetVertexes();
   for (std::size_t i = 0; i < vertexes.size() - 1; ++i) {
     LinearFunction v_line(vertexes[i], vertexes[i + 1]);
-    if (((line.a_coef * vertexes[i].x + line.b_coef * vertexes[i].y +
-          line.c_coef) *
-             (line.a_coef * vertexes[i + 1].x +
-              line.b_coef * vertexes[i + 1].y + line.c_coef) <
-         0) &&
-        ((v_line.a_coef * pnt1.x + v_line.b_coef * pnt1.y + v_line.c_coef) *
-             (v_line.a_coef * pnt2.x + v_line.b_coef * pnt2.y + v_line.c_coef) <
-         0))
+    if ((line.Substitute(vertexes[i]) * line.Substitute(vertexes[i + 1]) < 0) &&
+        (v_line.Substitute(pnt1) * v_line.Substitute(pnt2) < 0))
       return true;
   }
   return false;

diff --git a/math/optimal_way/obstacles.h b/math/optimal_way/obstacles.h
@@ -25,6 +25,10 @@ struct LinearFunction {
 
   double a_coef, b_coef, c_coef;
 
+  double Substitute(const lib::Point& p) {
+    return a_coef * p.x + b_coef * p.y + c_coef;
+  }
+
   bool operator==(const LinearFunction& other) {
     return (std::abs(a_coef - other.a_coef) < precision &&
             std::abs(b_coef - other.b_coef) < precision &&