Rjd 1278/fix isIntersect2D

common/math/geometry/src/intersection/intersection.cpp

@@ -21,22 +21,44 @@ namespace geometry
 {
 bool isIntersect2D(const LineSegment & line0, const LineSegment & line1)
 {
-  double s, t;
-  s = (line0.start_point.x - line0.end_point.x) * (line1.start_point.y - line0.start_point.y) -
-      (line0.start_point.y - line0.end_point.y) * (line1.start_point.x - line0.start_point.x);
-  t = (line0.start_point.x - line0.end_point.x) * (line1.end_point.y - line0.start_point.y) -
-      (line0.start_point.y - line0.end_point.y) * (line1.end_point.x - line0.start_point.x);
-  if (s * t > 0) {
-    return false;
-  }
-  s = (line1.start_point.x - line1.end_point.x) * (line0.start_point.y - line1.start_point.y) -
-      (line1.start_point.y - line1.end_point.y) * (line0.start_point.x - line1.start_point.x);
-  t = (line1.start_point.x - line1.end_point.x) * (line0.end_point.y - line1.start_point.y) -
-      (line1.start_point.y - line1.end_point.y) * (line0.end_point.x - line1.start_point.x);
-  if (s * t > 0) {
-    return false;
+  using Point = geometry_msgs::msg::Point;
+
+  const auto p0 = line0.start_point, q0 = line0.end_point;
+  const auto p1 = line1.start_point, q1 = line1.end_point;
+
+  constexpr auto within_bounding_box =
+    [](const Point & start, const Point & end, const Point & to_check) -> bool {
+    const bool x_inside = ((to_check.x - start.x) * (end.x - to_check.x) >= 0.0);
+    const bool y_inside = ((to_check.y - start.y) * (end.y - to_check.y) >= 0.0);
+    return x_inside && y_inside;
+  };
+
+  constexpr auto orientation =
+    [](const Point & start, const Point & end, const Point & to_check) -> double {
+    return (end.y - start.y) * (to_check.x - end.x) - (end.x - start.x) * (to_check.y - end.y);
+  };
+
+  constexpr auto sign = [](const double x) -> int {
+    constexpr double tolerance = 1.0e-10;
+    if (x > +tolerance) return +1;
+    if (x < -tolerance) return -1;
+    return 0;
+  };
+
+  const int ori_p0 = sign(orientation(p1, q1, p0));
+  const int ori_q0 = sign(orientation(p1, q1, q0));
+  const int ori_p1 = sign(orientation(p0, q0, p1));
+  const int ori_q1 = sign(orientation(p0, q0, q1));
+
+  // Special case
+  // If the lines are collinear; they intersect if and only if their bounding boxes overlap
+  if (ori_p1 == 0 && ori_q1 == 0 && ori_p0 == 0 && ori_q0 == 0) {
+    return within_bounding_box(p0, q0, p1) || within_bounding_box(p0, q0, p1) ||
+           within_bounding_box(p1, q1, p0) || within_bounding_box(p1, q1, p0);
   }
-  return true;
+
+  // General case
+  return ori_p1 != ori_q1 && ori_p0 != ori_q0;
 }
 
 bool isIntersect2D(const std::vector<LineSegment> & lines)

common/math/geometry/src/polygon/line_segment.cpp

@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include <cmath>
+#include <geometry/intersection/intersection.hpp>
 #include <geometry/polygon/line_segment.hpp>
 #include <geometry/quaternion/euler_to_quaternion.hpp>
 #include <geometry/transform.hpp>
@@ -144,22 +145,7 @@ auto LineSegment::getSValue(
  */
 auto LineSegment::isIntersect2D(const LineSegment & l0) const -> bool
 {
-  double s, t;
-  s = (l0.start_point.x - l0.end_point.x) * (start_point.y - l0.start_point.y) -
-      (l0.start_point.y - l0.end_point.y) * (start_point.x - l0.start_point.x);
-  t = (l0.start_point.x - l0.end_point.x) * (end_point.y - l0.start_point.y) -
-      (l0.start_point.y - l0.end_point.y) * (end_point.x - l0.start_point.x);
-  if (s * t > 0) {
-    return false;
-  }
-  s = (start_point.x - end_point.x) * (l0.start_point.y - start_point.y) -
-      (start_point.y - end_point.y) * (l0.start_point.x - start_point.x);
-  t = (start_point.x - end_point.x) * (l0.end_point.y - start_point.y) -
-      (start_point.y - end_point.y) * (l0.end_point.x - start_point.x);
-  if (s * t > 0) {
-    return false;
-  }
-  return true;
+  return math::geometry::isIntersect2D(*this, l0);
 }
 
 /**