Merge pull request autowarefoundation#668 from tier4/sync-upstream

chore: sync upstream

.cspell-partial.json

@@ -0,0 +1,19 @@
+{
+  "ignorePaths": [
+    "**/common/**",
+    "**/control/**",
+    "**/docs/**",
+    "**/evaluator/**",
+    "**/launch/**",
+    "**/localization/**",
+    "**/perception/**",
+    "**/planning/**",
+    "**/sensing/**",
+    "**/simulator/**",
+    "**/system/**",
+    "**/tools/**",
+    "**/vehicles/**"
+  ],
+  "ignoreRegExpList": [],
+  "words": []
+}

.github/workflows/spell-check-partial.yaml

@@ -0,0 +1,18 @@
+name: spell-check-partial
+
+on:
+  pull_request:
+
+jobs:
+  spell-check-partial:
+    runs-on: ubuntu-latest
+    steps:
+      - name: Check out repository
+        uses: actions/checkout@v3
+
+      - name: Run spell-check
+        uses: autowarefoundation/autoware-github-actions/spell-check@v1
+        with:
+          cspell-json-url: https://raw.githubusercontent.com/tier4/autoware-spell-check-dict/main/.cspell.json
+          local-cspell-json: .cspell-partial.json
+          incremental-files-only: true

common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp

@@ -770,6 +770,28 @@ inline bool isTwistCovarianceValid(
   return false;
 }
 
+// NOTE: much faster than boost::geometry::intersects()
+inline std::optional<geometry_msgs::msg::Point> intersect(
+  const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2,
+  const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4)
+{
+  // calculate intersection point
+  const double det = (p1.x - p2.x) * (p4.y - p3.y) - (p4.x - p3.x) * (p1.y - p2.y);
+  if (det == 0.0) {
+    return std::nullopt;
+  }
+
+  const double t = ((p4.y - p3.y) * (p4.x - p2.x) + (p3.x - p4.x) * (p4.y - p2.y)) / det;
+  const double s = ((p2.y - p1.y) * (p4.x - p2.x) + (p1.x - p2.x) * (p4.y - p2.y)) / det;
+  if (t < 0 || 1 < t || s < 0 || 1 < s) {
+    return std::nullopt;
+  }
+
+  geometry_msgs::msg::Point intersect_point;
+  intersect_point.x = t * p1.x + (1.0 - t) * p2.x;
+  intersect_point.y = t * p1.y + (1.0 - t) * p2.y;
+  return intersect_point;
+}
 }  // namespace tier4_autoware_utils
 
 #endif  // TIER4_AUTOWARE_UTILS__GEOMETRY__GEOMETRY_HPP_

common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp

@@ -1689,3 +1689,108 @@ TEST(geometry, isTwistCovarianceValid)
   twist_with_covariance.covariance.at(0) = 1.0;
   EXPECT_EQ(tier4_autoware_utils::isTwistCovarianceValid(twist_with_covariance), true);
 }
+
+TEST(geometry, intersect)
+{
+  using tier4_autoware_utils::createPoint;
+  using tier4_autoware_utils::intersect;
+
+  {  // Normally crossing
+    const auto p1 = createPoint(0.0, -1.0, 0.0);
+    const auto p2 = createPoint(0.0, 1.0, 0.0);
+    const auto p3 = createPoint(-1.0, 0.0, 0.0);
+    const auto p4 = createPoint(1.0, 0.0, 0.0);
+    const auto result = intersect(p1, p2, p3, p4);
+
+    EXPECT_TRUE(result);
+    EXPECT_NEAR(result->x, 0.0, epsilon);
+    EXPECT_NEAR(result->y, 0.0, epsilon);
+    EXPECT_NEAR(result->z, 0.0, epsilon);
+  }
+
+  {  // No crossing
+    const auto p1 = createPoint(0.0, -1.0, 0.0);
+    const auto p2 = createPoint(0.0, 1.0, 0.0);
+    const auto p3 = createPoint(1.0, 0.0, 0.0);
+    const auto p4 = createPoint(3.0, 0.0, 0.0);
+    const auto result = intersect(p1, p2, p3, p4);
+
+    EXPECT_FALSE(result);
+  }
+
+  {  // One segment is the point on the other's segment
+    const auto p1 = createPoint(0.0, -1.0, 0.0);
+    const auto p2 = createPoint(0.0, 1.0, 0.0);
+    const auto p3 = createPoint(0.0, 0.0, 0.0);
+    const auto p4 = createPoint(0.0, 0.0, 0.0);
+    const auto result = intersect(p1, p2, p3, p4);
+
+    EXPECT_FALSE(result);
+  }
+
+  {  // One segment is the point not on the other's segment
+    const auto p1 = createPoint(0.0, -1.0, 0.0);
+    const auto p2 = createPoint(0.0, 1.0, 0.0);
+    const auto p3 = createPoint(1.0, 0.0, 0.0);
+    const auto p4 = createPoint(1.0, 0.0, 0.0);
+    const auto result = intersect(p1, p2, p3, p4);
+
+    EXPECT_FALSE(result);
+  }
+
+  {  // Both segments are the points which are the same position
+    const auto p1 = createPoint(0.0, 0.0, 0.0);
+    const auto p2 = createPoint(0.0, 0.0, 0.0);
+    const auto p3 = createPoint(0.0, 0.0, 0.0);
+    const auto p4 = createPoint(0.0, 0.0, 0.0);
+    const auto result = intersect(p1, p2, p3, p4);
+
+    EXPECT_FALSE(result);
+  }
+
+  {  // Both segments are the points which are different position
+    const auto p1 = createPoint(0.0, 1.0, 0.0);
+    const auto p2 = createPoint(0.0, 1.0, 0.0);
+    const auto p3 = createPoint(1.0, 0.0, 0.0);
+    const auto p4 = createPoint(1.0, 0.0, 0.0);
+    const auto result = intersect(p1, p2, p3, p4);
+
+    EXPECT_FALSE(result);
+  }
+
+  {  // Segments are the same
+    const auto p1 = createPoint(0.0, -1.0, 0.0);
+    const auto p2 = createPoint(0.0, 1.0, 0.0);
+    const auto p3 = createPoint(0.0, -1.0, 0.0);
+    const auto p4 = createPoint(0.0, 1.0, 0.0);
+    const auto result = intersect(p1, p2, p3, p4);
+
+    EXPECT_FALSE(result);
+  }
+
+  {  // One's edge is on the other's segment
+    const auto p1 = createPoint(0.0, -1.0, 0.0);
+    const auto p2 = createPoint(0.0, 1.0, 0.0);
+    const auto p3 = createPoint(0.0, 0.0, 0.0);
+    const auto p4 = createPoint(1.0, 0.0, 0.0);
+    const auto result = intersect(p1, p2, p3, p4);
+
+    EXPECT_TRUE(result);
+    EXPECT_NEAR(result->x, 0.0, epsilon);
+    EXPECT_NEAR(result->y, 0.0, epsilon);
+    EXPECT_NEAR(result->z, 0.0, epsilon);
+  }
+
+  {  // One's edge is the same as the other's edge.
+    const auto p1 = createPoint(0.0, -1.0, 0.0);
+    const auto p2 = createPoint(0.0, 1.0, 0.0);
+    const auto p3 = createPoint(0.0, -1.0, 0.0);
+    const auto p4 = createPoint(2.0, -1.0, 0.0);
+    const auto result = intersect(p1, p2, p3, p4);
+
+    EXPECT_TRUE(result);
+    EXPECT_NEAR(result->x, 0.0, epsilon);
+    EXPECT_NEAR(result->y, -1.0, epsilon);
+    EXPECT_NEAR(result->z, 0.0, epsilon);
+  }
+}