Skip to content

Commit

Permalink
fix function name and comments
Browse files Browse the repository at this point in the history
Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>
  • Loading branch information
maxime-clem committed Jul 4, 2024
1 parent b5298a1 commit fa2b158
Show file tree
Hide file tree
Showing 5 changed files with 13 additions and 18 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -584,7 +584,7 @@ std::optional<geometry_msgs::msg::Point> intersect(
* @brief Check if 2 convex polygons intersect using the GJK algorithm
* @details much faster than boost::geometry::intersects()
*/
bool intersect(const Polygon2d & convex_polygon1, const Polygon2d & convex_polygon2);
bool intersects(const Polygon2d & convex_polygon1, const Polygon2d & convex_polygon2);

} // namespace autoware::universe_utils

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -31,9 +31,9 @@ Point2d cross_product(const Point2d & p1, const Point2d & p2, const Point2d & p3
} // namespace
/**
* @brief Check if 2 convex polygons intersect using the GJK algorithm
* @details much faster than boost::geometry::intersects()
* @details much faster than boost::geometry::overlaps() but limited to convex polygons
*/
bool intersect(const Polygon2d & convex_polygon1, const Polygon2d & convex_polygon2);
bool intersects(const Polygon2d & convex_polygon1, const Polygon2d & convex_polygon2);
} // namespace autoware::universe_utils::gjk

#endif // AUTOWARE__UNIVERSE_UTILS__GEOMETRY__GJK_2D_HPP_
4 changes: 2 additions & 2 deletions common/autoware_universe_utils/src/geometry/geometry.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -385,9 +385,9 @@ std::optional<geometry_msgs::msg::Point> intersect(
return intersect_point;
}

bool intersect(const Polygon2d & convex_polygon1, const Polygon2d & convex_polygon2)
bool intersects(const Polygon2d & convex_polygon1, const Polygon2d & convex_polygon2)
{
return gjk::intersect(convex_polygon1, convex_polygon2);
return gjk::intersects(convex_polygon1, convex_polygon2);
}

} // namespace autoware::universe_utils
8 changes: 2 additions & 6 deletions common/autoware_universe_utils/src/geometry/gjk_2d.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,6 @@ double dot_product(const Point2d & p1, const Point2d & p2)
return p1.x() * p2.x() + p1.y() * p2.y();
}

/// @brief return the polygon polygon that is the furthest away from a direction vector
size_t furthest_vertex_idx(const Polygon2d & poly, const Point2d & direction)
{
auto furthest_distance = std::numeric_limits<double>::lowest();
Expand Down Expand Up @@ -67,11 +66,8 @@ Point2d cross_product(const Point2d & p1, const Point2d & p2, const Point2d & p3
return Point2d(-p3.y() * tmp, p3.x() * tmp);
}
} // namespace
/**
* @brief Check if 2 convex polygons intersect using the GJK algorithm
* @details much faster than boost::geometry::intersects()
*/
bool intersect(const Polygon2d & convex_polygon1, const Polygon2d & convex_polygon2)

bool intersects(const Polygon2d & convex_polygon1, const Polygon2d & convex_polygon2)
{
if (convex_polygon1.outer().empty() || convex_polygon2.outer().empty()) {
return false;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,6 @@

#include "autoware/universe_utils/geometry/boost_geometry.hpp"
#include "autoware/universe_utils/geometry/geometry.hpp"
#include "autoware/universe_utils/geometry/gjk_2d.hpp"
#include "autoware/universe_utils/geometry/random_convex_polygon.hpp"
#include "autoware/universe_utils/math/unit_conversion.hpp"
#include "autoware/universe_utils/system/stop_watch.hpp"
Expand Down Expand Up @@ -1850,7 +1849,7 @@ TEST(geometry, intersectPolygon)
poly2.outer().emplace_back(0, 1);
boost::geometry::correct(poly1);
boost::geometry::correct(poly2);
EXPECT_TRUE(autoware::universe_utils::intersect(poly1, poly2));
EXPECT_TRUE(autoware::universe_utils::intersects(poly1, poly2));
}
{ // 2 triangles with no intersection (but they share an edge)
autoware::universe_utils::Polygon2d poly1, poly2;
Expand All @@ -1862,7 +1861,7 @@ TEST(geometry, intersectPolygon)
poly2.outer().emplace_back(2, 2);
boost::geometry::correct(poly1);
boost::geometry::correct(poly2);
EXPECT_FALSE(autoware::universe_utils::intersect(poly1, poly2));
EXPECT_FALSE(autoware::universe_utils::intersects(poly1, poly2));
}
{ // 2 triangles with no intersection (but they share a point)
autoware::universe_utils::Polygon2d poly1, poly2;
Expand All @@ -1874,7 +1873,7 @@ TEST(geometry, intersectPolygon)
poly2.outer().emplace_back(2, 2);
boost::geometry::correct(poly1);
boost::geometry::correct(poly2);
EXPECT_FALSE(autoware::universe_utils::intersect(poly1, poly2));
EXPECT_FALSE(autoware::universe_utils::intersects(poly1, poly2));
}
{ // 2 triangles with no intersection and no touching
autoware::universe_utils::Polygon2d poly1, poly2;
Expand All @@ -1886,7 +1885,7 @@ TEST(geometry, intersectPolygon)
poly2.outer().emplace_back(3, 5);
boost::geometry::correct(poly1);
boost::geometry::correct(poly2);
EXPECT_FALSE(autoware::universe_utils::intersect(poly1, poly2));
EXPECT_FALSE(autoware::universe_utils::intersects(poly1, poly2));
}
{ // triangle and quadrilateral with intersection
autoware::universe_utils::Polygon2d poly1, poly2;
Expand All @@ -1899,7 +1898,7 @@ TEST(geometry, intersectPolygon)
poly2.outer().emplace_back(12, 7);
boost::geometry::correct(poly1);
boost::geometry::correct(poly2);
EXPECT_TRUE(autoware::universe_utils::intersect(poly1, poly2));
EXPECT_TRUE(autoware::universe_utils::intersects(poly1, poly2));
}
}

Expand All @@ -1922,7 +1921,7 @@ TEST(geometry, intersectPolygonRand)
const auto ground_truth = boost::geometry::intersects(polygons[i], polygons[j]);
ground_truth_ns += sw.toc();
sw.tic();
const auto gjk = autoware::universe_utils::gjk::intersect(polygons[i], polygons[j]);
const auto gjk = autoware::universe_utils::intersects(polygons[i], polygons[j]);
gjk_ns += sw.toc();
if (ground_truth != gjk) {
std::cout << "Failed for the 2 polygons: ";
Expand Down

0 comments on commit fa2b158

Please sign in to comment.