Fix/geometry bug fixes

common/math/geometry/include/geometry/linear_algebra.hpp

@@ -50,6 +50,4 @@ geometry_msgs::msg::Vector3 operator-(
   const geometry_msgs::msg::Vector3 & v0, const geometry_msgs::msg::Vector3 & v1);
 geometry_msgs::msg::Point operator-(
   const geometry_msgs::msg::Point & v0, const geometry_msgs::msg::Point & v1);
-bool operator==(const geometry_msgs::msg::Point & v0, const geometry_msgs::msg::Point & v1);
-bool operator==(const geometry_msgs::msg::Vector3 & v0, const geometry_msgs::msg::Vector3 & v1);
 #endif  // GEOMETRY__LINEAR_ALGEBRA_HPP_

common/math/geometry/include/geometry/spline/catmull_rom_spline.hpp

@@ -49,6 +49,9 @@ class CatmullRomSpline : public CatmullRomSplineInterface
     -> double;
   auto getSquaredDistanceVector(const geometry_msgs::msg::Point & point, const double s) const
     -> geometry_msgs::msg::Vector3;
+  auto getCollisionPointsIn2D(
+    const std::vector<geometry_msgs::msg::Point> & polygon,
+    const bool search_backward = false) const -> std::set<double>;
   auto getCollisionPointIn2D(
     const geometry_msgs::msg::Point & point0, const geometry_msgs::msg::Point & point1,
     const bool search_backward = false) const -> std::optional<double>;

common/math/geometry/include/geometry/spline/hermite_curve.hpp

@@ -63,12 +63,18 @@ class HermiteCurve
     const geometry_msgs::msg::Point & point, double s, bool denormalize_s = false) const;
   geometry_msgs::msg::Vector3 getSquaredDistanceVector(
     const geometry_msgs::msg::Point & point, double s, bool denormalize_s = false) const;
+  std::set<double> getCollisionPointsIn2D(
+    const geometry_msgs::msg::Point & point0, const geometry_msgs::msg::Point & point1,
+    bool search_backward = false, bool denormalize_s = false) const;
   std::optional<double> getCollisionPointIn2D(
     const geometry_msgs::msg::Point & point0, const geometry_msgs::msg::Point & point1,
-    bool search_backward = false) const;
+    bool search_backward = false, bool denormalize_s = false) const;
+  std::set<double> getCollisionPointsIn2D(
+    const std::vector<geometry_msgs::msg::Point> & polygon, bool search_backward = false,
+    bool close_start_end = true, bool denormalize_s = false) const;
   std::optional<double> getCollisionPointIn2D(
     const std::vector<geometry_msgs::msg::Point> & polygon, bool search_backward = false,
-    bool close_start_end = true) const;
+    bool close_start_end = true, bool denormalize_s = false) const;
 
 private:
   std::pair<double, double> get2DMinMaxCurvatureValue() const;

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

@@ -41,8 +41,8 @@ bool isIntersect2D(const LineSegment & line0, const LineSegment & line1)
 
 bool isIntersect2D(const std::vector<LineSegment> & lines)
 {
-  for (size_t i = 0; i <= lines.size(); i++) {
-    for (size_t m = 0; m <= lines.size(); m++) {
+  for (size_t i = 0; i < lines.size(); ++i) {
+    for (size_t m = 0; m < lines.size(); ++m) {
       if (i != m && isIntersect2D(lines[i], lines[m])) {
         return true;
       }
@@ -74,8 +74,8 @@ std::optional<geometry_msgs::msg::Point> getIntersection2D(
 std::vector<geometry_msgs::msg::Point> getIntersection2D(const std::vector<LineSegment> & lines)
 {
   std::vector<geometry_msgs::msg::Point> ret;
-  for (size_t i = 0; i <= lines.size(); i++) {
-    for (size_t m = 0; m <= lines.size(); m++) {
+  for (size_t i = 0; i < lines.size(); ++i) {
+    for (size_t m = 0; m < lines.size(); ++m) {
       if (i != m) {
         const auto point = getIntersection2D(lines[i], lines[m]);
         if (point) {

common/math/geometry/src/linear_algebra.cpp

@@ -149,21 +149,3 @@ geometry_msgs::msg::Point operator-(
   ret.z = v0.z - v1.z;
   return ret;
 }
-
-bool operator==(const geometry_msgs::msg::Point & v0, const geometry_msgs::msg::Point & v1)
-{
-  constexpr double e = std::numeric_limits<double>::epsilon();
-  if (std::fabs(v0.x - v1.x) <= e && std::fabs(v0.y - v1.y) <= e && std::fabs(v0.z - v1.z) <= e) {
-    return true;
-  }
-  return false;
-}
-
-bool operator==(const geometry_msgs::msg::Vector3 & v0, const geometry_msgs::msg::Vector3 & v1)
-{
-  constexpr double e = std::numeric_limits<double>::epsilon();
-  if (std::fabs(v0.x - v1.x) <= e && std::fabs(v0.y - v1.y) <= e && std::fabs(v0.z - v1.z) <= e) {
-    return true;
-  }
-  return false;
-}

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

@@ -39,6 +39,13 @@ LineSegment::LineSegment(
 : start_point(start_point), end_point([&]() -> geometry_msgs::msg::Point {
     geometry_msgs::msg::Point ret;
     double vec_size = std::hypot(vec.x, vec.y);
+    if (vec_size == 0.0) {
+      THROW_SIMULATION_ERROR(
+        "Invalid vector is specified, while constructing LineSegment. ",
+        "The vector should have a non zero length to initialize the line segment correctly. ",
+        "This message is not originally intended to be displayed, if you see it, please "
+        "contact the developer of traffic_simulator.");
+    }
     ret.x = start_point.x + vec.x / vec_size * length;
     ret.y = start_point.y + vec.y / vec_size * length;
     ret.z = start_point.z + vec.z / vec_size * length;

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

@@ -19,6 +19,7 @@
 #include <boost/geometry/geometries/polygon.hpp>
 #include <geometry/polygon/polygon.hpp>
 #include <rclcpp/rclcpp.hpp>
+#include <scenario_simulator_exception/exception.hpp>
 
 namespace math
 {
@@ -51,13 +52,41 @@ std::vector<geometry_msgs::msg::Point> get2DConvexHull(
 
 double getMaxValue(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis)
 {
+  if (points.empty()) {
+    THROW_SIMULATION_ERROR(
+      "Invalid point list is specified, while getting max value on ",
+      axis == Axis::X   ? "X"
+      : axis == Axis::Y ? "Y"
+                        : "Z",
+      " axis. ",
+      "The point list in getMaxValue should have at least one point to get the max value from. "
+      "This message is not originally intended to be displayed, if you see it, please "
+      "contact the developer of traffic_simulator.");
+  }
   const auto values = filterByAxis(points, axis);
+  if (values.size() == 1) {
+    return values.front();
+  }
   return *std::max_element(values.begin(), values.end());
 }
 
 double getMinValue(const std::vector<geometry_msgs::msg::Point> & points, const Axis & axis)
 {
+  if (points.empty()) {
+    THROW_SIMULATION_ERROR(
+      "Invalid point list is specified, while getting min value on ",
+      axis == Axis::X   ? "X"
+      : axis == Axis::Y ? "Y"
+                        : "Z",
+      " axis. ",
+      "The point list in getMinValue should have at least one point to get the min value from. "
+      "This message is not originally intended to be displayed, if you see it, please "
+      "contact the developer of traffic_simulator.");
+  }
   const auto values = filterByAxis(points, axis);
+  if (values.size() == 1) {
+    return values.front();
+  }
   return *std::min_element(values.begin(), values.end());
 }
 

common/math/geometry/src/spline/catmull_rom_spline.cpp

@@ -31,6 +31,9 @@ auto CatmullRomSpline::getPolygon(
   const double width, const size_t num_points, const double z_offset)
   -> std::vector<geometry_msgs::msg::Point>
 {
+  if (num_points == 0) {
+    return {};
+  }
   std::vector<geometry_msgs::msg::Point> points;
   std::vector<geometry_msgs::msg::Point> left_bounds = getLeftBounds(width, num_points, z_offset);
   std::vector<geometry_msgs::msg::Point> right_bounds = getRightBounds(width, num_points, z_offset);
@@ -278,15 +281,9 @@ auto CatmullRomSpline::getSInSplineCurve(const size_t curve_index, const double
   THROW_SEMANTIC_ERROR("curve index does not match");  // LCOV_EXCL_LINE
 }
 
-/**
- * @brief Get collision point in 2D (x and y)
- * @param polygon points of polygons.
- * @param search_backward If true, return collision points with maximum s value. If false, return collision points with minimum s value.
- * @return std::optional<double> Denormalized s values along the frenet coordinate of the spline curve.
- */
-auto CatmullRomSpline::getCollisionPointIn2D(
+auto CatmullRomSpline::getCollisionPointsIn2D(
   const std::vector<geometry_msgs::msg::Point> & polygon, const bool search_backward) const
-  -> std::optional<double>
+  -> std::set<double>
 {
   if (polygon.size() <= 1) {
     THROW_SIMULATION_ERROR(
@@ -297,32 +294,20 @@ auto CatmullRomSpline::getCollisionPointIn2D(
   }
   /// @note If the spline has three or more control points.
   const auto get_collision_point_2d_with_curve =
-    [this](const auto & polygon, const auto search_backward) -> std::optional<double> {
-    size_t n = curves_.size();
-    if (search_backward) {
-      for (size_t i = 0; i < n; i++) {
-        auto s = curves_[n - 1 - i].getCollisionPointIn2D(polygon, search_backward);
-        if (s) {
-          return getSInSplineCurve(n - 1 - i, s.value());
-        }
-      }
-      return std::optional<double>();
-    } else {
-      for (size_t i = 0; i < n; i++) {
-        auto s = curves_[i].getCollisionPointIn2D(polygon, search_backward);
-        if (s) {
-          return std::optional<double>(getSInSplineCurve(i, s.value()));
-        }
-      }
-      return std::optional<double>();
+    [this](const auto & polygon, const auto search_backward) -> std::set<double> {
+    std::set<double> s_value_candidates;
+    for (size_t i = 0; i < curves_.size(); ++i) {
+      /// @note The polygon is assumed to be closed
+      const auto s = curves_[i].getCollisionPointsIn2D(polygon, search_backward, true, true);
+      std::for_each(s.begin(), s.end(), [&s_value_candidates, i, this](const auto & s) {
+        s_value_candidates.insert(getSInSplineCurve(i, s));
+      });
     }
-    return std::optional<double>();
+    return s_value_candidates;
   };
   /// @note If the spline has two control points. (Same as single line segment.)
-  const auto get_collision_point_2d_with_line =
-    [this](const auto & polygon, const auto search_backward) -> std::optional<double> {
-    const auto polygon_lines = getLineSegments(polygon);
-    std::vector<double> s_value_candidates = {};
+  const auto get_collision_point_2d_with_line = [this](const auto & polygon) -> std::set<double> {
+    std::set<double> s_value_candidates;
     for (const auto & line : getLineSegments(polygon)) {
       if (static_cast<int>(line_segments_.size()) != 1) {
         THROW_SIMULATION_ERROR(
@@ -332,26 +317,21 @@ auto CatmullRomSpline::getCollisionPointIn2D(
           "This message is not originally intended to be displayed, if you see it, please "
           "contact the developer of traffic_simulator.");
       }
-      if (const auto s_value = line_segments_[0].getIntersection2DSValue(line)) {
-        s_value_candidates.push_back(s_value.value());
+      if (const auto s = line_segments_[0].getIntersection2DSValue(line, true)) {
+        s_value_candidates.insert(s.value());
       }
     }
-    if (s_value_candidates.empty()) {
-      return std::optional<double>();
-    }
-    return search_backward
-             ? *std::max_element(s_value_candidates.begin(), s_value_candidates.end())
-             : *std::min_element(s_value_candidates.begin(), s_value_candidates.end());
+    return s_value_candidates;
   };
   /// @note If the spline has one control point. (Same as point.)
-  const auto get_collision_point_2d_with_point = [this](const auto & polygon) {
-    const auto polygon_lines = getLineSegments(polygon);
-    for (const auto & line : polygon_lines) {
+  const auto get_collision_point_2d_with_point = [this](const auto & polygon) -> std::set<double> {
+    std::set<double> s_value_candidates;
+    for (const auto & line : getLineSegments(polygon)) {
       if (line.isIntersect2D(control_points[0])) {
-        return std::optional<double>(0);
+        s_value_candidates.insert(0.0);
       }
     }
-    return std::optional<double>();
+    return s_value_candidates;
   };
 
   switch (control_points.size()) {
@@ -366,29 +346,49 @@ auto CatmullRomSpline::getCollisionPointIn2D(
       return get_collision_point_2d_with_point(polygon);
     /// @note In this case, spline is interpreted as line segment.
     case 2:
-      return get_collision_point_2d_with_line(polygon, search_backward);
+      return get_collision_point_2d_with_line(polygon);
     /// @note In this case, spline is interpreted as curve.
     default:
       return get_collision_point_2d_with_curve(polygon, search_backward);
   }
 }
 
+/**
+ * @brief Get collision point in 2D (x and y)
+ * @param polygon points of polygons.
+ * @param search_backward If true, return collision points with maximum s value. If false, return collision points with minimum s value.
+ * @return std::optional<double> Denormalized s values along the frenet coordinate of the spline curve.
+ */
+auto CatmullRomSpline::getCollisionPointIn2D(
+  const std::vector<geometry_msgs::msg::Point> & polygon, const bool search_backward) const
+  -> std::optional<double>
+{
+  std::set<double> s_value_candidates = getCollisionPointsIn2D(polygon, search_backward);
+  if (s_value_candidates.empty()) {
+    return std::nullopt;
+  }
+  if (search_backward) {
+    return *(s_value_candidates.rbegin());
+  }
+  return *(s_value_candidates.begin());
+}
+
 auto CatmullRomSpline::getCollisionPointIn2D(
   const geometry_msgs::msg::Point & point0, const geometry_msgs::msg::Point & point1,
   const bool search_backward) const -> std::optional<double>
 {
   size_t n = curves_.size();
   if (search_backward) {
     for (size_t i = 0; i < n; i++) {
-      auto s = curves_[n - 1 - i].getCollisionPointIn2D(point0, point1, search_backward);
+      auto s = curves_[n - 1 - i].getCollisionPointIn2D(point0, point1, search_backward, true);
       if (s) {
         return getSInSplineCurve(n - 1 - i, s.value());
       }
     }
     return std::nullopt;
   } else {
     for (size_t i = 0; i < n; i++) {
-      auto s = curves_[i].getCollisionPointIn2D(point0, point1, search_backward);
+      auto s = curves_[i].getCollisionPointIn2D(point0, point1, search_backward, true);
       if (s) {
         return getSInSplineCurve(i, s.value());
       }
@@ -567,7 +567,12 @@ auto CatmullRomSpline::getMaximum2DCurvature() const -> double
   if (maximum_2d_curvatures_.empty()) {
     THROW_SIMULATION_ERROR("maximum 2D curvature vector size is 0.");  // LCOV_EXCL_LINE
   }
-  return *std::max_element(maximum_2d_curvatures_.begin(), maximum_2d_curvatures_.end());
+  const auto [min, max] =
+    std::minmax_element(maximum_2d_curvatures_.begin(), maximum_2d_curvatures_.end());
+  if (std::fabs(*min) > std::fabs(*max)) {
+    return *min;
+  }
+  return *max;
 }
 
 auto CatmullRomSpline::getNormalVector(const double s) const -> geometry_msgs::msg::Vector3

common/math/geometry/src/spline/catmull_rom_subspline.cpp

@@ -14,6 +14,7 @@
 
 #include <geometry/spline/catmull_rom_subspline.hpp>
 #include <optional>
+#include <scenario_simulator_exception/exception.hpp>
 #include <vector>
 
 namespace math
@@ -25,17 +26,40 @@ double CatmullRomSubspline::getLength() const { return end_s_ - start_s_; }
 std::optional<double> CatmullRomSubspline::getCollisionPointIn2D(
   const std::vector<geometry_msgs::msg::Point> & polygon, const bool search_backward) const
 {
-  auto s = spline_->getCollisionPointIn2D(polygon, search_backward);
+  /// @note Make sure end is greater than start, otherwise the spline is invalid
+  if (end_s_ < start_s_) {
+    THROW_SIMULATION_ERROR(
+      "The start of the subspline is greater than the end. "
+      "The start of the subspline should always be less than the end. ",
+      "Subspline start: ", start_s_, " Subspline end: ", end_s_, " ",
+      "Something completely unexpected happened. ",
+      "This message is not originally intended to be displayed, if you see it, please "
+      "contact the developer of traffic_simulator.");
+  }
+
+  std::set<double> s_value_candidates = spline_->getCollisionPointsIn2D(polygon);
 
-  if (!s) {
+  if (s_value_candidates.empty()) {
     return std::nullopt;
   }
 
-  if (s.value() < start_s_ || end_s_ < s.value()) {
+  /// @note Iterators for the range of this subspline
+  auto begin = s_value_candidates.lower_bound(start_s_);
+  auto end = s_value_candidates.upper_bound(end_s_);
+
+  /**
+   * @note If begin == end there is no collision in the given range, or it is past the range
+   * If begin == s_value_candidates.end() all elements are less than start_s_
+   * end == s_value_candidates.end() is valid, all elements are less than end_s_
+   */
+  if (begin == end || begin == s_value_candidates.end()) {
     return std::nullopt;
   }
 
-  return s.value() - start_s_;
+  if (search_backward) {
+    return *(--end) - start_s_;  // end is past the last element, so we need the one before
+  }
+  return *begin - start_s_;
 }
 }  // namespace geometry
 }  // namespace math