From 388355d946c3e5a1bc3a4bf7139478bcd3e9d272 Mon Sep 17 00:00:00 2001
From: "Michael X. Grey" <mxgrey@intrinsic.ai>
Date: Fri, 24 Nov 2023 13:58:57 +0000
Subject: [PATCH] Provide function that checks if a line segment intersects a
 door

Signed-off-by: Michael X. Grey <mxgrey@intrinsic.ai>
---
 rmf_traffic/include/rmf_traffic/agv/Graph.hpp |  6 ++
 rmf_traffic/src/rmf_traffic/agv/Graph.cpp     | 71 +++++++++++++++++++
 2 files changed, 77 insertions(+)

diff --git a/rmf_traffic/include/rmf_traffic/agv/Graph.hpp b/rmf_traffic/include/rmf_traffic/agv/Graph.hpp
index 42d18500..64e8ebd5 100644
--- a/rmf_traffic/include/rmf_traffic/agv/Graph.hpp
+++ b/rmf_traffic/include/rmf_traffic/agv/Graph.hpp
@@ -89,6 +89,12 @@ class Graph
     /// Get the name of the map that this door is on.
     const std::string& map() const;
 
+    /// Check if the line formed by p0 -> p1 intersects this door.
+    bool intersects(
+      Eigen::Vector2d p0,
+      Eigen::Vector2d p1,
+      double envelope = 0.0) const;
+
     /// Constructor
     DoorProperties(
       std::string name,
diff --git a/rmf_traffic/src/rmf_traffic/agv/Graph.cpp b/rmf_traffic/src/rmf_traffic/agv/Graph.cpp
index aa5bb25e..8bce988e 100644
--- a/rmf_traffic/src/rmf_traffic/agv/Graph.cpp
+++ b/rmf_traffic/src/rmf_traffic/agv/Graph.cpp
@@ -146,6 +146,77 @@ const std::string& Graph::DoorProperties::map() const
   return _pimpl->map;
 }
 
+//==============================================================================
+namespace {
+double distance_from_point_to_segment(
+  Eigen::Vector2d q,
+  Eigen::Vector2d p0,
+  Eigen::Vector2d p1)
+{
+  const double endpoint_distance = std::min((q - p0).norm(), (q - p1).norm());
+  const auto L = (p1 - p0).norm();
+  if (L < 1e-3)
+  {
+    return endpoint_distance;
+  }
+
+  const Eigen::Vector2d n = (p1 - p0) / L;
+  const Eigen::Vector2d v = q - p0;
+  const Eigen::Vector2d q_proj = (v.dot(n)) * n;
+  const double ortho_distance = (q - q_proj).norm();
+  return std::min(ortho_distance, endpoint_distance);
+}
+} // anonymous namespace
+
+//==============================================================================
+bool Graph::DoorProperties::intersects(
+  Eigen::Vector2d p0,
+  Eigen::Vector2d p1,
+  double envelope) const
+{
+  const auto q0 = _pimpl->start;
+  const auto q1 = _pimpl->end;
+  for (const auto test : std::vector<std::function<double()>>{
+      [&]{ return distance_from_point_to_segment(p0, q0, q1); },
+      [&]{ return distance_from_point_to_segment(p1, q0, q1); },
+      [&]{ return distance_from_point_to_segment(q0, p0, p1); },
+      [&]{ return distance_from_point_to_segment(_pimpl->end, p0, p1); }
+    })
+  {
+    const double distance = test();
+    if (distance <= envelope)
+      return true;
+  }
+
+  // If none of the endpoints are within range of the other lines, then the only
+  // way for an intersection to exist is if the lines truly cross each other.
+  const double det = (p0.x() - p1.x()) * (q0.y() - q1.y())
+    - (p0.y() - p1.y()) * (q0.x() - q1.x());
+
+  if (std::abs(det) < 1e-8)
+  {
+    // The lines are essentially parallel and their endpoints aren't close
+    // enough, so there is no intersection.
+    return false;
+  }
+
+  const double t = ( (p0.x() - q0.x()) * (q0.y() - q1.y())
+    - (p0.y() - q0.y()) * (q0.x() - q1.x()) )
+    / det;
+
+  if (t < 0.0 || 1.0 < t)
+    return false;
+
+  const double u = ( (p0.x() - q0.x()) * (p0.y() - p1.y())
+    - (p0.y() - q0.y()) * (p0.x() - p1.x()) )
+    / det;
+
+  if (u < 0.0 || 1.0 < u)
+    return false;
+
+  return true;
+}
+
 //==============================================================================
 Graph::DoorProperties::DoorProperties(
   std::string name,