geometry utils

Signed-off-by: silanus23 <[email protected]>

Author: silanus23

nav2_util/include/nav2_util/geometry_utils.hpp

@@ -264,42 +264,45 @@ inline double cross_product_2d(
  * @param polygon Polygon to check if the point is inside
  * @return True if given point is inside polygon, otherwise false
  */
-template<class PointT>
-inline bool isPointInsidePolygon(
-  const double px, const double py, const std::vector<PointT> & polygon)
+inline double distanceToPoint(const geometry_msgs::msg::PoseStamped &global_pose,
+                                const geometry_msgs::msg::PoseStamped &target)
 {
-  // Adaptation of Shimrat, Moshe. "Algorithm 112: position of point relative to polygon."
-  // Communications of the ACM 5.8 (1962): 434.
-  // Implementation of ray crossings algorithm for point in polygon task solving.
-  // Y coordinate is fixed. Moving the ray on X+ axis starting from given point.
-  // Odd number of intersections with polygon boundaries means the point is inside polygon.
-  const int points_num = polygon.size();
-  int i, j;  // Polygon vertex iterators
-  bool res = false;  // Final result, initialized with already inverted value
-
-  // Starting from the edge where the last point of polygon is connected to the first
-  i = points_num - 1;
-  for (j = 0; j < points_num; j++) {
-    // Checking the edge only if given point is between edge boundaries by Y coordinates.
-    // One of the condition should contain equality in order to exclude the edges
-    // parallel to X+ ray.
-    if ((py <= polygon[i].y) == (py > polygon[j].y)) {
-      // Calculating the intersection coordinate of X+ ray
-      const double x_inter = polygon[i].x +
-        (py - polygon[i].y) *
-        (polygon[j].x - polygon[i].x) /
-        (polygon[j].y - polygon[i].y);
-      // If intersection with checked edge is greater than point x coordinate,
-      // inverting the result
-      if (x_inter > px) {
-        res = !res;
-      }
-    }
-    i = j;
-  }
-  return res;
+    const double xDist = global_pose.pose.position.x - target.pose.position.x;
+    const double yDist = global_pose.pose.position.y - target.pose.position.y;
+    return std::hypot(xDist,yDist);
 }
 
+/**
+ * @brief Find the shortest distance to a vector
+ * @param global_pose Robot's current or planned position
+ * @param start Starting point of target vector
+ * @param finish End point of target vector
+ * @return int
+ */
+inline double distanceToSegment(
+const geometry_msgs::msg::PoseStamped & point,
+const geometry_msgs::msg::PoseStamped & start,
+const geometry_msgs::msg::PoseStamped & end)
+{
+const auto & p = point.pose.position;
+const auto & a = start.pose.position;
+const auto & b = end.pose.position;
+
+const double seg_len_sq = distanceToPoint(start, end);
+if (seg_len_sq <= 1e-8) {
+    return std::sqrt(distanceToPoint(start, end));
+}
+
+const double dot = ((p.x - a.x) * (b.x - a.x)) + ((p.y - a.y) * (b.y - a.y));
+const double t = std::clamp(dot / seg_len_sq, 0.0, 1.0);
+
+const double proj_x = a.x + t * (b.x - a.x);
+const double proj_y = a.y + t * (b.y - a.y);
+
+const double dx_proj = p.x - proj_x;
+const double dy_proj = p.y - proj_y;
+return std::hypot(dx_proj,dy_proj);
+}
 }  // namespace geometry_utils
 }  // namespace nav2_util