cross_product

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

Author: silanus23

nav2_util/include/nav2_util/geometry_utils.hpp

@@ -282,6 +282,35 @@ inline double distance_to_segment(
   return std::hypot(dx_proj, dy_proj);
 }
 
+/**
+ * @brief Computes the 2D cross product between the vector from start to end and the vector from start to point.
+ * The sign of this calculation's result can be used to determine which side are you on of the track.
+ *
+ * See: https://en.wikipedia.org/wiki/Cross_product
+ *
+ * @param point The point to check relative to the segment.
+ * @param start The starting pose of the segment.
+ * @param end The ending pose of the segment.
+ * @return The signed 2D cross product value.
+ */
+inline double cross_product_2d(
+  const geometry_msgs::msg::Point & point,
+  const geometry_msgs::msg::Pose & start,
+  const geometry_msgs::msg::Pose & end)
+{
+  const auto & p = point;
+  const auto & a = start.position;
+  const auto & b = end.position;
+
+  const double path_vec_x = b.x - a.x;
+  const double path_vec_y = b.y - a.y;
+
+  const double robot_vec_x = p.x - a.x;
+  const double robot_vec_y = p.y - a.y;
+
+  return (path_vec_x * robot_vec_y) - (path_vec_y * robot_vec_x);
+}
+
 }  // namespace geometry_utils
 }  // namespace nav2_util